diff --git a/drivers/net/wireless/ralink/rt2x00/rt2800lib.c b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
index d11c7b2..4b85ef3 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
@@ -7740,6 +7740,39 @@ static char rt2800_lp_tx_filter_bw_cal(struct rt2x00_dev *rt2x00dev)
 	return cal_val;
 }
 
+#define RF_SAVE_NUM 24
+#define BBP_SAVE_NUM 3
+static const int rf_save_ids[RF_SAVE_NUM] = {0, 1, 3, 4, 5, 6, 7, 8,
+	17, 18, 19, 20, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 58, 59};
+
+static void rt2800_phy_save(struct rt2x00_dev *rt2x00dev,
+			    u8 *const bbp_regs, u8 *const rf_regs)
+{
+	int i;
+
+	rt2800_bbp_read(rt2x00dev, 23, &bbp_regs[0]);
+
+	rt2800_bbp_dcoc_read(rt2x00dev, 0, &bbp_regs[1]);
+	rt2800_bbp_dcoc_read(rt2x00dev, 2, &bbp_regs[2]);
+
+	for (i = 0; i < RF_SAVE_NUM; i++)
+		rt2800_rfcsr_read_bank(rt2x00dev, 5, rf_save_ids[i], &rf_regs[i]);
+}
+
+static void rt2800_phy_restore(struct rt2x00_dev *rt2x00dev,
+			       const u8 *const bbp_regs, const u8 *const rf_regs)
+{
+	int i;
+
+	for (i = 0; i < RF_SAVE_NUM; i++)
+		rt2800_rfcsr_write_bank(rt2x00dev, 5, rf_save_ids[i], rf_regs[i]);
+
+	rt2800_bbp_write(rt2x00dev, 23, bbp_regs[0]);
+
+	rt2800_bbp_dcoc_write(rt2x00dev, 0, bbp_regs[1]);
+	rt2800_bbp_dcoc_write(rt2x00dev, 2, bbp_regs[2]);
+}
+
 static void rt2800_bw_filter_calibration(struct rt2x00_dev *rt2x00dev,
 					 bool btxcal)
 {
@@ -7751,52 +7784,15 @@ static void rt2800_bw_filter_calibration(struct rt2x00_dev *rt2x00dev,
 	int loop = 0, is_ht40, cnt;
 	u8 bbp_val, rf_val;
 	char cal_r32_init, cal_r32_val, cal_diff;
-	u8 saverfb5r00, saverfb5r01, saverfb5r03, saverfb5r04, saverfb5r05;
-	u8 saverfb5r06, saverfb5r07;
-	u8 saverfb5r08, saverfb5r17, saverfb5r18, saverfb5r19, saverfb5r20;
-	u8 saverfb5r37, saverfb5r38, saverfb5r39, saverfb5r40, saverfb5r41;
-	u8 saverfb5r42, saverfb5r43, saverfb5r44, saverfb5r45, saverfb5r46;
-	u8 saverfb5r58, saverfb5r59;
-	u8 savebbp159r0, savebbp159r2, savebbpr23;
 	u32 MAC_RF_CONTROL0, MAC_RF_BYPASS0;
+	u8 bbp_regs[BBP_SAVE_NUM];
+	u8 rf_regs[RF_SAVE_NUM];
 
 	/* Save MAC registers */
 	rt2800_register_read(rt2x00dev, RF_CONTROL0, &MAC_RF_CONTROL0);
 	rt2800_register_read(rt2x00dev, RF_BYPASS0, &MAC_RF_BYPASS0);
 
-	/* save BBP registers */
-	rt2800_bbp_read(rt2x00dev, 23, &savebbpr23);
-
-	rt2800_bbp_dcoc_read(rt2x00dev, 0, &savebbp159r0);
-	rt2800_bbp_dcoc_read(rt2x00dev, 2, &savebbp159r2);
-
-	/* Save RF registers */
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 0, &saverfb5r00);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 1, &saverfb5r01);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 3, &saverfb5r03);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 4, &saverfb5r04);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 5, &saverfb5r05);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 6, &saverfb5r06);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 7, &saverfb5r07);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 8, &saverfb5r08);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 17, &saverfb5r17);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 18, &saverfb5r18);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 19, &saverfb5r19);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 20, &saverfb5r20);
-
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 37, &saverfb5r37);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 38, &saverfb5r38);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 39, &saverfb5r39);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 40, &saverfb5r40);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 41, &saverfb5r41);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 42, &saverfb5r42);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 43, &saverfb5r43);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 44, &saverfb5r44);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 45, &saverfb5r45);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 46, &saverfb5r46);
-
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 58, &saverfb5r58);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 59, &saverfb5r59);
+	rt2800_phy_save(rt2x00dev, bbp_regs, rf_regs);
 
 	rt2800_rfcsr_read_bank(rt2x00dev, 5, 0, &rf_val);
 	rf_val |= 0x3;
@@ -7948,37 +7944,7 @@ static void rt2800_bw_filter_calibration(struct rt2x00_dev *rt2x00dev,
 		loop++;
 	} while (loop <= 1);
 
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 0, saverfb5r00);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 1, saverfb5r01);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 3, saverfb5r03);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 4, saverfb5r04);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 5, saverfb5r05);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 6, saverfb5r06);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 7, saverfb5r07);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 8, saverfb5r08);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 17, saverfb5r17);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 18, saverfb5r18);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 19, saverfb5r19);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 20, saverfb5r20);
-
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 37, saverfb5r37);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 38, saverfb5r38);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 39, saverfb5r39);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 40, saverfb5r40);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 41, saverfb5r41);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 42, saverfb5r42);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 43, saverfb5r43);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 44, saverfb5r44);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 45, saverfb5r45);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 46, saverfb5r46);
-
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 58, saverfb5r58);
-	rt2800_rfcsr_write_bank(rt2x00dev, 5, 59, saverfb5r59);
-
-	rt2800_bbp_write(rt2x00dev, 23, savebbpr23);
-
-	rt2800_bbp_dcoc_write(rt2x00dev, 0, savebbp159r0);
-	rt2800_bbp_dcoc_write(rt2x00dev, 2, savebbp159r2);
+	rt2800_phy_restore(rt2x00dev, bbp_regs, rf_regs);
 
 	rt2800_bbp_read(rt2x00dev, 4, &bbp_val);
 	rt2x00_set_field8(&bbp_val, BBP4_BANDWIDTH,
