1
2
3
4#include "mt7615.h"
5#include "eeprom.h"
6#include "mcu.h"
7
8enum {
9 TM_CHANGED_TXPOWER_CTRL,
10 TM_CHANGED_TXPOWER,
11 TM_CHANGED_FREQ_OFFSET,
12
13
14 NUM_TM_CHANGED
15};
16
17
18static const u8 tm_change_map[] = {
19 [TM_CHANGED_TXPOWER_CTRL] = MT76_TM_ATTR_TX_POWER_CONTROL,
20 [TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
21 [TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
22};
23
24static const u32 reg_backup_list[] = {
25 MT_WF_PHY_RFINTF3_0(0),
26 MT_WF_PHY_RFINTF3_0(1),
27 MT_WF_PHY_RFINTF3_0(2),
28 MT_WF_PHY_RFINTF3_0(3),
29 MT_ANT_SWITCH_CON(2),
30 MT_ANT_SWITCH_CON(3),
31 MT_ANT_SWITCH_CON(4),
32 MT_ANT_SWITCH_CON(6),
33 MT_ANT_SWITCH_CON(7),
34 MT_ANT_SWITCH_CON(8),
35};
36
37static const struct {
38 u16 wf;
39 u16 reg;
40} rf_backup_list[] = {
41 { 0, 0x48 },
42 { 1, 0x48 },
43 { 2, 0x48 },
44 { 3, 0x48 },
45};
46
47static int
48mt7615_tm_set_tx_power(struct mt7615_phy *phy)
49{
50 struct mt7615_dev *dev = phy->dev;
51 struct mt76_phy *mphy = phy->mt76;
52 int i, ret, n_chains = hweight8(mphy->antenna_mask);
53 struct cfg80211_chan_def *chandef = &mphy->chandef;
54 int freq = chandef->center_freq1, len, target_chains;
55 u8 *data, *eep = (u8 *)dev->mt76.eeprom.data;
56 enum nl80211_band band = chandef->chan->band;
57 struct sk_buff *skb;
58 struct {
59 u8 center_chan;
60 u8 dbdc_idx;
61 u8 band;
62 u8 rsv;
63 } __packed req_hdr = {
64 .center_chan = ieee80211_frequency_to_channel(freq),
65 .band = band,
66 .dbdc_idx = phy != &dev->phy,
67 };
68 u8 *tx_power = NULL;
69
70 if (mphy->test.state != MT76_TM_STATE_OFF)
71 tx_power = mphy->test.tx_power;
72
73 len = MT7615_EE_MAX - MT_EE_NIC_CONF_0;
74 skb = mt76_mcu_msg_alloc(&dev->mt76, NULL, sizeof(req_hdr) + len);
75 if (!skb)
76 return -ENOMEM;
77
78 skb_put_data(skb, &req_hdr, sizeof(req_hdr));
79 data = skb_put_data(skb, eep + MT_EE_NIC_CONF_0, len);
80
81 target_chains = mt7615_ext_pa_enabled(dev, band) ? 1 : n_chains;
82 for (i = 0; i < target_chains; i++) {
83 ret = mt7615_eeprom_get_target_power_index(dev, chandef->chan, i);
84 if (ret < 0) {
85 dev_kfree_skb(skb);
86 return -EINVAL;
87 }
88
89 if (tx_power && tx_power[i])
90 data[ret - MT_EE_NIC_CONF_0] = tx_power[i];
91 }
92
93 return mt76_mcu_skb_send_msg(&dev->mt76, skb,
94 MCU_EXT_CMD(SET_TX_POWER_CTRL), false);
95}
96
97static void
98mt7615_tm_reg_backup_restore(struct mt7615_phy *phy)
99{
100 struct mt7615_dev *dev = phy->dev;
101 u32 *b = phy->test.reg_backup;
102 int n_regs = ARRAY_SIZE(reg_backup_list);
103 int n_rf_regs = ARRAY_SIZE(rf_backup_list);
104 int i;
105
106 if (phy->mt76->test.state == MT76_TM_STATE_OFF) {
107 for (i = 0; i < n_regs; i++)
108 mt76_wr(dev, reg_backup_list[i], b[i]);
109
110 for (i = 0; i < n_rf_regs; i++)
111 mt7615_rf_wr(dev, rf_backup_list[i].wf,
112 rf_backup_list[i].reg, b[n_regs + i]);
113 return;
114 }
115
116 if (b)
117 return;
118
119 b = devm_kzalloc(dev->mt76.dev, 4 * (n_regs + n_rf_regs),
120 GFP_KERNEL);
121 if (!b)
122 return;
123
124 phy->test.reg_backup = b;
125 for (i = 0; i < n_regs; i++)
126 b[i] = mt76_rr(dev, reg_backup_list[i]);
127 for (i = 0; i < n_rf_regs; i++)
128 b[n_regs + i] = mt7615_rf_rr(dev, rf_backup_list[i].wf,
129 rf_backup_list[i].reg);
130}
131
132static void
133mt7615_tm_init(struct mt7615_phy *phy)
134{
135 struct mt7615_dev *dev = phy->dev;
136 unsigned int total_flags = ~0;
137
138 if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
139 return;
140
141 mt7615_mcu_set_sku_en(phy, phy->mt76->test.state == MT76_TM_STATE_OFF);
142
143 mutex_unlock(&dev->mt76.mutex);
144 mt7615_set_channel(phy);
145 mt7615_ops.configure_filter(phy->mt76->hw, 0, &total_flags, 0);
146 mutex_lock(&dev->mt76.mutex);
147
148 mt7615_tm_reg_backup_restore(phy);
149}
150
151static void
152mt7615_tm_set_rx_enable(struct mt7615_dev *dev, bool en)
153{
154 u32 rqcr_mask = (MT_ARB_RQCR_RX_START |
155 MT_ARB_RQCR_RXV_START |
156 MT_ARB_RQCR_RXV_R_EN |
157 MT_ARB_RQCR_RXV_T_EN) *
158 (BIT(0) | BIT(MT_ARB_RQCR_BAND_SHIFT));
159
160 if (en) {
161 mt76_clear(dev, MT_ARB_SCR,
162 MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
163 mt76_set(dev, MT_ARB_RQCR, rqcr_mask);
164 } else {
165 mt76_set(dev, MT_ARB_SCR,
166 MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
167 mt76_clear(dev, MT_ARB_RQCR, rqcr_mask);
168 }
169}
170
171static void
172mt7615_tm_set_tx_antenna(struct mt7615_phy *phy, bool en)
173{
174 struct mt7615_dev *dev = phy->dev;
175 struct mt76_testmode_data *td = &phy->mt76->test;
176 u8 mask = td->tx_antenna_mask;
177 int i;
178
179 if (!mask)
180 return;
181
182 if (!en)
183 mask = phy->mt76->chainmask;
184
185 for (i = 0; i < 4; i++) {
186 mt76_rmw_field(dev, MT_WF_PHY_RFINTF3_0(i),
187 MT_WF_PHY_RFINTF3_0_ANT,
188 (mask & BIT(i)) ? 0 : 0xa);
189 }
190
191
192 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(3), MT_ANT_SWITCH_CON_MODE(0),
193 (mask & BIT(0)) ? 0x8 : 0x1b);
194 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(2),
195 (mask & BIT(1)) ? 0xe : 0x1b);
196 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(6), MT_ANT_SWITCH_CON_MODE1(0),
197 (mask & BIT(2)) ? 0x0 : 0xf);
198 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE1(2),
199 (mask & BIT(3)) ? 0x6 : 0xf);
200
201
202 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(1),
203 (mask & BIT(0)) ? 0xd : 0x1b);
204 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(2), MT_ANT_SWITCH_CON_MODE(3),
205 (mask & BIT(1)) ? 0x13 : 0x1b);
206 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE1(1),
207 (mask & BIT(2)) ? 0x5 : 0xf);
208 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(8), MT_ANT_SWITCH_CON_MODE1(3),
209 (mask & BIT(3)) ? 0xb : 0xf);
210
211 for (i = 0; i < 4; i++) {
212 u32 val;
213
214 val = mt7615_rf_rr(dev, i, 0x48);
215 val &= ~(0x3ff << 20);
216 if (mask & BIT(i))
217 val |= 3 << 20;
218 else
219 val |= (2 << 28) | (2 << 26) | (8 << 20);
220 mt7615_rf_wr(dev, i, 0x48, val);
221 }
222}
223
224static void
225mt7615_tm_set_tx_frames(struct mt7615_phy *phy, bool en)
226{
227 struct mt7615_dev *dev = phy->dev;
228 struct ieee80211_tx_info *info;
229 struct sk_buff *skb = phy->mt76->test.tx_skb;
230
231 mt7615_mcu_set_chan_info(phy, MCU_EXT_CMD(SET_RX_PATH));
232 mt7615_tm_set_tx_antenna(phy, en);
233 mt7615_tm_set_rx_enable(dev, !en);
234 if (!en || !skb)
235 return;
236
237 info = IEEE80211_SKB_CB(skb);
238 info->control.vif = phy->monitor_vif;
239}
240
241static void
242mt7615_tm_update_params(struct mt7615_phy *phy, u32 changed)
243{
244 struct mt7615_dev *dev = phy->dev;
245 struct mt76_testmode_data *td = &phy->mt76->test;
246 bool en = phy->mt76->test.state != MT76_TM_STATE_OFF;
247
248 if (changed & BIT(TM_CHANGED_TXPOWER_CTRL))
249 mt7615_mcu_set_test_param(dev, MCU_ATE_SET_TX_POWER_CONTROL,
250 en, en && td->tx_power_control);
251 if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
252 mt7615_mcu_set_test_param(dev, MCU_ATE_SET_FREQ_OFFSET,
253 en, en ? td->freq_offset : 0);
254 if (changed & BIT(TM_CHANGED_TXPOWER))
255 mt7615_tm_set_tx_power(phy);
256}
257
258static int
259mt7615_tm_set_state(struct mt76_phy *mphy, enum mt76_testmode_state state)
260{
261 struct mt7615_phy *phy = mphy->priv;
262 struct mt76_testmode_data *td = &mphy->test;
263 enum mt76_testmode_state prev_state = td->state;
264
265 mphy->test.state = state;
266
267 if (prev_state == MT76_TM_STATE_TX_FRAMES)
268 mt7615_tm_set_tx_frames(phy, false);
269 else if (state == MT76_TM_STATE_TX_FRAMES)
270 mt7615_tm_set_tx_frames(phy, true);
271
272 if (state <= MT76_TM_STATE_IDLE)
273 mt7615_tm_init(phy);
274
275 if ((state == MT76_TM_STATE_IDLE &&
276 prev_state == MT76_TM_STATE_OFF) ||
277 (state == MT76_TM_STATE_OFF &&
278 prev_state == MT76_TM_STATE_IDLE)) {
279 u32 changed = 0;
280 int i;
281
282 for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
283 u16 cur = tm_change_map[i];
284
285 if (td->param_set[cur / 32] & BIT(cur % 32))
286 changed |= BIT(i);
287 }
288
289 mt7615_tm_update_params(phy, changed);
290 }
291
292 return 0;
293}
294
295static int
296mt7615_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb,
297 enum mt76_testmode_state new_state)
298{
299 struct mt76_testmode_data *td = &mphy->test;
300 struct mt7615_phy *phy = mphy->priv;
301 u32 changed = 0;
302 int i;
303
304 BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
305
306 if (new_state == MT76_TM_STATE_OFF ||
307 td->state == MT76_TM_STATE_OFF)
308 return 0;
309
310 if (td->tx_antenna_mask & ~mphy->chainmask)
311 return -EINVAL;
312
313 for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
314 if (tb[tm_change_map[i]])
315 changed |= BIT(i);
316 }
317
318 mt7615_tm_update_params(phy, changed);
319
320 return 0;
321}
322
323static int
324mt7615_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
325{
326 struct mt7615_phy *phy = mphy->priv;
327 void *rx, *rssi;
328 int i;
329
330 rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
331 if (!rx)
332 return -ENOMEM;
333
334 if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, phy->test.last_freq_offset))
335 return -ENOMEM;
336
337 rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
338 if (!rssi)
339 return -ENOMEM;
340
341 for (i = 0; i < ARRAY_SIZE(phy->test.last_rcpi); i++)
342 if (nla_put_u8(msg, i, phy->test.last_rcpi[i]))
343 return -ENOMEM;
344
345 nla_nest_end(msg, rssi);
346
347 rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI);
348 if (!rssi)
349 return -ENOMEM;
350
351 for (i = 0; i < ARRAY_SIZE(phy->test.last_ib_rssi); i++)
352 if (nla_put_s8(msg, i, phy->test.last_ib_rssi[i]))
353 return -ENOMEM;
354
355 nla_nest_end(msg, rssi);
356
357 rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI);
358 if (!rssi)
359 return -ENOMEM;
360
361 for (i = 0; i < ARRAY_SIZE(phy->test.last_wb_rssi); i++)
362 if (nla_put_s8(msg, i, phy->test.last_wb_rssi[i]))
363 return -ENOMEM;
364
365 nla_nest_end(msg, rssi);
366
367 nla_nest_end(msg, rx);
368
369 return 0;
370}
371
372const struct mt76_testmode_ops mt7615_testmode_ops = {
373 .set_state = mt7615_tm_set_state,
374 .set_params = mt7615_tm_set_params,
375 .dump_stats = mt7615_tm_dump_stats,
376};
377
