@@ -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,