1 --- a/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
2 +++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
3 @@ -8469,6 +8469,155 @@ static void rt2800_rf_self_txdc_cal(stru
4 rt2x00_info(rt2x00dev, "RF Tx self calibration end\n");
7 +static int rt2800_calcrcalibrationcode(struct rt2x00_dev *rt2x00dev, int d1, int d2)
10 + calcode = ((d2 - d1) * 1000) / 43;
11 + if ((calcode%10) >= 5)
13 + calcode = (calcode / 10);
18 +static void rt2800_r_calibration(struct rt2x00_dev *rt2x00dev)
21 + u8 saverfb0r1, saverfb0r34, saverfb0r35;
22 + u8 saverfb5r4, saverfb5r17, saverfb5r18;
23 + u8 saverfb5r19, saverfb5r20;
24 + u8 savebbpr22, savebbpr47, savebbpr49;
28 + char d1 = 0, d2 = 0;
30 + u32 MAC_RF_BYPASS0, MAC_RF_CONTROL0, MAC_PWR_PIN_CFG;
31 + u32 maccfg, macstatus;
34 + saverfb0r1 = rt2800_rfcsr_read_bank(rt2x00dev, 0, 1);
35 + saverfb0r34 = rt2800_rfcsr_read_bank(rt2x00dev, 0, 34);
36 + saverfb0r35 = rt2800_rfcsr_read_bank(rt2x00dev, 0, 35);
37 + saverfb5r4 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 4);
38 + saverfb5r17 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 17);
39 + saverfb5r18 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 18);
40 + saverfb5r19 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 19);
41 + saverfb5r20 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 20);
43 + savebbpr22 = rt2800_bbp_read(rt2x00dev, 22);
44 + savebbpr47 = rt2800_bbp_read(rt2x00dev, 47);
45 + savebbpr49 = rt2800_bbp_read(rt2x00dev, 49);
47 + savemacsysctrl = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
48 + MAC_RF_BYPASS0 = rt2800_register_read(rt2x00dev, RF_BYPASS0);
49 + MAC_RF_CONTROL0 = rt2800_register_read(rt2x00dev, RF_CONTROL0);
50 + MAC_PWR_PIN_CFG = rt2800_register_read(rt2x00dev, PWR_PIN_CFG);
52 + maccfg = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
54 + rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, maccfg);
56 + for (i = 0; i < 10000; i++) {
57 + macstatus = rt2800_register_read(rt2x00dev, MAC_STATUS_CFG);
58 + if (macstatus & 0x1)
65 + rt2x00_warn(rt2x00dev, "Wait MAC Tx Status to MAX !!!\n");
67 + maccfg = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
69 + rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, maccfg);
71 + for (i = 0; i < 10000; i++) {
72 + macstatus = rt2800_register_read(rt2x00dev, MAC_STATUS_CFG);
73 + if (macstatus & 0x2)
80 + rt2x00_warn(rt2x00dev, "Wait MAC Rx Status to MAX !!!\n");
82 + rfvalue = (MAC_RF_BYPASS0 | 0x3004);
83 + rt2800_register_write(rt2x00dev, RF_BYPASS0, rfvalue);
84 + rfvalue = (MAC_RF_CONTROL0 | (~0x3002));
85 + rt2800_register_write(rt2x00dev, RF_CONTROL0, rfvalue);
87 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 4, 0x27);
88 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 17, 0x80);
89 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 18, 0x83);
90 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 19, 0x00);
91 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 20, 0x20);
93 + rt2800_rfcsr_write_bank(rt2x00dev, 0, 1, 0x00);
94 + rt2800_rfcsr_write_bank(rt2x00dev, 0, 34, 0x13);
95 + rt2800_rfcsr_write_bank(rt2x00dev, 0, 35, 0x00);
97 + rt2800_register_write(rt2x00dev, PWR_PIN_CFG, 0x1);
99 + rt2800_bbp_write(rt2x00dev, 47, 0x04);
100 + rt2800_bbp_write(rt2x00dev, 22, 0x80);
102 + bytevalue = rt2800_bbp_read(rt2x00dev, 49);
103 + if (bytevalue > 128)
104 + d1 = bytevalue - 256;
106 + d1 = (char)bytevalue;
107 + rt2800_bbp_write(rt2x00dev, 22, 0x0);
108 + rt2800_rfcsr_write_bank(rt2x00dev, 0, 35, 0x01);
110 + rt2800_bbp_write(rt2x00dev, 22, 0x80);
112 + bytevalue = rt2800_bbp_read(rt2x00dev, 49);
113 + if (bytevalue > 128)
114 + d2 = bytevalue - 256;
116 + d2 = (char)bytevalue;
117 + rt2800_bbp_write(rt2x00dev, 22, 0x0);
119 + rcalcode = rt2800_calcrcalibrationcode(rt2x00dev, d1, d2);
121 + r_cal_code = 256 + rcalcode;
123 + r_cal_code = (u8)rcalcode;
125 + rt2800_rfcsr_write_bank(rt2x00dev, 0, 7, r_cal_code);
127 + rt2800_bbp_write(rt2x00dev, 22, 0x0);
129 + bytevalue = rt2800_bbp_read(rt2x00dev, 21);
131 + rt2800_bbp_write(rt2x00dev, 21, bytevalue);
132 + bytevalue = rt2800_bbp_read(rt2x00dev, 21);
133 + bytevalue &= (~0x1);
134 + rt2800_bbp_write(rt2x00dev, 21, bytevalue);
136 + rt2800_rfcsr_write_bank(rt2x00dev, 0, 1, saverfb0r1);
137 + rt2800_rfcsr_write_bank(rt2x00dev, 0, 34, saverfb0r34);
138 + rt2800_rfcsr_write_bank(rt2x00dev, 0, 35, saverfb0r35);
139 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 4, saverfb5r4);
140 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 17, saverfb5r17);
141 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 18, saverfb5r18);
142 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 19, saverfb5r19);
143 + rt2800_rfcsr_write_bank(rt2x00dev, 5, 20, saverfb5r20);
145 + rt2800_bbp_write(rt2x00dev, 22, savebbpr22);
146 + rt2800_bbp_write(rt2x00dev, 47, savebbpr47);
147 + rt2800_bbp_write(rt2x00dev, 49, savebbpr49);
149 + rt2800_register_write(rt2x00dev, RF_BYPASS0, MAC_RF_BYPASS0);
150 + rt2800_register_write(rt2x00dev, RF_CONTROL0, MAC_RF_CONTROL0);
152 + rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, savemacsysctrl);
153 + rt2800_register_write(rt2x00dev, PWR_PIN_CFG, MAC_PWR_PIN_CFG);
156 static void rt2800_bbp_core_soft_reset(struct rt2x00_dev *rt2x00dev,
157 bool set_bw, bool is_ht40)
159 @@ -9076,6 +9225,7 @@ static void rt2800_init_rfcsr_6352(struc
160 rt2800_rfcsr_write_dccal(rt2x00dev, 5, 0x00);
161 rt2800_rfcsr_write_dccal(rt2x00dev, 17, 0x7C);
163 + rt2800_r_calibration(rt2x00dev);
164 rt2800_rf_self_txdc_cal(rt2x00dev);
165 rt2800_bw_filter_calibration(rt2x00dev, true);
166 rt2800_bw_filter_calibration(rt2x00dev, false);