1 // SPDX-License-Identifier: ISC
2 /* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */
3
4 #include "mt7615.h"
5 #include "eeprom.h"
6 #include "mcu.h"
7
8 enum {
9 TM_CHANGED_TXPOWER_CTRL,
10 TM_CHANGED_TXPOWER,
11 TM_CHANGED_FREQ_OFFSET,
12
13 /* must be last */
14 NUM_TM_CHANGED
15 };
16
17
18 static 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
24 static 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
37 static 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
47 static int
mt7615_tm_set_tx_power(struct mt7615_phy * phy)48 mt7615_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 (dev->mt76.test.state != MT76_TM_STATE_OFF)
71 tx_power = dev->mt76.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
97 static void
mt7615_tm_reg_backup_restore(struct mt7615_dev * dev)98 mt7615_tm_reg_backup_restore(struct mt7615_dev *dev)
99 {
100 u32 *b = dev->test.reg_backup;
101 int n_regs = ARRAY_SIZE(reg_backup_list);
102 int n_rf_regs = ARRAY_SIZE(rf_backup_list);
103 int i;
104
105 if (dev->mt76.test.state == MT76_TM_STATE_OFF) {
106 for (i = 0; i < n_regs; i++)
107 mt76_wr(dev, reg_backup_list[i], b[i]);
108
109 for (i = 0; i < n_rf_regs; i++)
110 mt7615_rf_wr(dev, rf_backup_list[i].wf,
111 rf_backup_list[i].reg, b[n_regs + i]);
112 return;
113 }
114
115 if (b)
116 return;
117
118 b = devm_kzalloc(dev->mt76.dev, 4 * (n_regs + n_rf_regs),
119 GFP_KERNEL);
120 if (!b)
121 return;
122
123 dev->test.reg_backup = b;
124 for (i = 0; i < n_regs; i++)
125 b[i] = mt76_rr(dev, reg_backup_list[i]);
126 for (i = 0; i < n_rf_regs; i++)
127 b[n_regs + i] = mt7615_rf_rr(dev, rf_backup_list[i].wf,
128 rf_backup_list[i].reg);
129 }
130
131
132 static void
mt7615_tm_init_phy(struct mt7615_dev * dev,struct mt7615_phy * phy)133 mt7615_tm_init_phy(struct mt7615_dev *dev, struct mt7615_phy *phy)
134 {
135 unsigned int total_flags = ~0;
136
137 if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
138 return;
139
140 mutex_unlock(&dev->mt76.mutex);
141 mt7615_set_channel(phy);
142 mt7615_ops.configure_filter(phy->mt76->hw, 0, &total_flags, 0);
143 mutex_lock(&dev->mt76.mutex);
144
145 mt7615_tm_reg_backup_restore(dev);
146 }
147
148 static void
mt7615_tm_init(struct mt7615_dev * dev)149 mt7615_tm_init(struct mt7615_dev *dev)
150 {
151 mt7615_tm_init_phy(dev, &dev->phy);
152
153 if (dev->mt76.phy2)
154 mt7615_tm_init_phy(dev, dev->mt76.phy2->priv);
155 }
156
157 static void
mt7615_tm_set_rx_enable(struct mt7615_dev * dev,bool en)158 mt7615_tm_set_rx_enable(struct mt7615_dev *dev, bool en)
159 {
160 u32 rqcr_mask = (MT_ARB_RQCR_RX_START |
161 MT_ARB_RQCR_RXV_START |
162 MT_ARB_RQCR_RXV_R_EN |
163 MT_ARB_RQCR_RXV_T_EN) *
164 (BIT(0) | BIT(MT_ARB_RQCR_BAND_SHIFT));
165
166 if (en) {
167 mt76_clear(dev, MT_ARB_SCR,
168 MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
169 mt76_set(dev, MT_ARB_RQCR, rqcr_mask);
170 } else {
171 mt76_set(dev, MT_ARB_SCR,
172 MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
173 mt76_clear(dev, MT_ARB_RQCR, rqcr_mask);
174 }
175 }
176
177 static void
mt7615_tm_set_tx_antenna(struct mt7615_dev * dev,bool en)178 mt7615_tm_set_tx_antenna(struct mt7615_dev *dev, bool en)
179 {
180 struct mt76_testmode_data *td = &dev->mt76.test;
181 u8 mask = td->tx_antenna_mask;
182 int i;
183
184 if (!mask)
185 return;
186
187 if (!en)
188 mask = dev->phy.chainmask;
189
190 for (i = 0; i < 4; i++) {
191 mt76_rmw_field(dev, MT_WF_PHY_RFINTF3_0(i),
192 MT_WF_PHY_RFINTF3_0_ANT,
193 (td->tx_antenna_mask & BIT(i)) ? 0 : 0xa);
194
195 }
196
197 /* 2.4 GHz band */
198 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(3), MT_ANT_SWITCH_CON_MODE(0),
199 (td->tx_antenna_mask & BIT(0)) ? 0x8 : 0x1b);
200 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(2),
201 (td->tx_antenna_mask & BIT(1)) ? 0xe : 0x1b);
202 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(6), MT_ANT_SWITCH_CON_MODE1(0),
203 (td->tx_antenna_mask & BIT(2)) ? 0x0 : 0xf);
204 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE1(2),
205 (td->tx_antenna_mask & BIT(3)) ? 0x6 : 0xf);
206
207 /* 5 GHz band */
208 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(1),
209 (td->tx_antenna_mask & BIT(0)) ? 0xd : 0x1b);
210 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(2), MT_ANT_SWITCH_CON_MODE(3),
211 (td->tx_antenna_mask & BIT(1)) ? 0x13 : 0x1b);
212 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE1(1),
213 (td->tx_antenna_mask & BIT(2)) ? 0x5 : 0xf);
214 mt76_rmw_field(dev, MT_ANT_SWITCH_CON(8), MT_ANT_SWITCH_CON_MODE1(3),
215 (td->tx_antenna_mask & BIT(3)) ? 0xb : 0xf);
216
217 for (i = 0; i < 4; i++) {
218 u32 val;
219
220 val = mt7615_rf_rr(dev, i, 0x48);
221 val &= ~(0x3ff << 20);
222 if (td->tx_antenna_mask & BIT(i))
223 val |= 3 << 20;
224 else
225 val |= (2 << 28) | (2 << 26) | (8 << 20);
226 mt7615_rf_wr(dev, i, 0x48, val);
227 }
228 }
229
230 static void
mt7615_tm_set_tx_frames(struct mt7615_dev * dev,bool en)231 mt7615_tm_set_tx_frames(struct mt7615_dev *dev, bool en)
232 {
233 struct ieee80211_tx_info *info;
234 struct sk_buff *skb = dev->mt76.test.tx_skb;
235
236 mt7615_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
237 mt7615_tm_set_tx_antenna(dev, en);
238 mt7615_tm_set_rx_enable(dev, !en);
239 if (!en || !skb)
240 return;
241
242 info = IEEE80211_SKB_CB(skb);
243 info->control.vif = dev->phy.monitor_vif;
244 }
245
246 static void
mt7615_tm_update_params(struct mt7615_dev * dev,u32 changed)247 mt7615_tm_update_params(struct mt7615_dev *dev, u32 changed)
248 {
249 struct mt76_testmode_data *td = &dev->mt76.test;
250 bool en = dev->mt76.test.state != MT76_TM_STATE_OFF;
251
252 if (changed & BIT(TM_CHANGED_TXPOWER_CTRL))
253 mt7615_mcu_set_test_param(dev, MCU_ATE_SET_TX_POWER_CONTROL,
254 en, en && td->tx_power_control);
255 if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
256 mt7615_mcu_set_test_param(dev, MCU_ATE_SET_FREQ_OFFSET,
257 en, en ? td->freq_offset : 0);
258 if (changed & BIT(TM_CHANGED_TXPOWER))
259 mt7615_tm_set_tx_power(&dev->phy);
260 }
261
262 static int
mt7615_tm_set_state(struct mt76_dev * mdev,enum mt76_testmode_state state)263 mt7615_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
264 {
265 struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
266 struct mt76_testmode_data *td = &mdev->test;
267 enum mt76_testmode_state prev_state = td->state;
268
269 mdev->test.state = state;
270
271 if (prev_state == MT76_TM_STATE_TX_FRAMES)
272 mt7615_tm_set_tx_frames(dev, false);
273 else if (state == MT76_TM_STATE_TX_FRAMES)
274 mt7615_tm_set_tx_frames(dev, true);
275
276 if (state <= MT76_TM_STATE_IDLE)
277 mt7615_tm_init(dev);
278
279 if ((state == MT76_TM_STATE_IDLE &&
280 prev_state == MT76_TM_STATE_OFF) ||
281 (state == MT76_TM_STATE_OFF &&
282 prev_state == MT76_TM_STATE_IDLE)) {
283 u32 changed = 0;
284 int i;
285
286 for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
287 u16 cur = tm_change_map[i];
288
289 if (td->param_set[cur / 32] & BIT(cur % 32))
290 changed |= BIT(i);
291 }
292
293 mt7615_tm_update_params(dev, changed);
294 }
295
296 return 0;
297 }
298
299 static int
mt7615_tm_set_params(struct mt76_dev * mdev,struct nlattr ** tb,enum mt76_testmode_state new_state)300 mt7615_tm_set_params(struct mt76_dev *mdev, struct nlattr **tb,
301 enum mt76_testmode_state new_state)
302 {
303 struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
304 struct mt76_testmode_data *td = &dev->mt76.test;
305 u32 changed = 0;
306 int i;
307
308 BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
309
310 if (new_state == MT76_TM_STATE_OFF ||
311 td->state == MT76_TM_STATE_OFF)
312 return 0;
313
314 if (td->tx_antenna_mask & ~dev->phy.chainmask)
315 return -EINVAL;
316
317 for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
318 if (tb[tm_change_map[i]])
319 changed |= BIT(i);
320 }
321
322 mt7615_tm_update_params(dev, changed);
323
324 return 0;
325 }
326
327 static int
mt7615_tm_dump_stats(struct mt76_dev * mdev,struct sk_buff * msg)328 mt7615_tm_dump_stats(struct mt76_dev *mdev, struct sk_buff *msg)
329 {
330 struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
331 void *rx, *rssi;
332 int i;
333
334 rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
335 if (!rx)
336 return -ENOMEM;
337
338 if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, dev->test.last_freq_offset) ||
339 nla_put_s32(msg, MT76_TM_RX_ATTR_IB_RSSI, dev->test.last_ib_rssi) ||
340 nla_put_s32(msg, MT76_TM_RX_ATTR_WB_RSSI, dev->test.last_wb_rssi))
341 return -ENOMEM;
342
343 rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
344 if (!rssi)
345 return -ENOMEM;
346
347 for (i = 0; i < ARRAY_SIZE(dev->test.last_rcpi); i++)
348 if (nla_put_u8(msg, i, dev->test.last_rcpi[i]))
349 return -ENOMEM;
350
351 nla_nest_end(msg, rssi);
352
353 nla_nest_end(msg, rx);
354
355 return 0;
356 }
357
358 const struct mt76_testmode_ops mt7615_testmode_ops = {
359 .set_state = mt7615_tm_set_state,
360 .set_params = mt7615_tm_set_params,
361 .dump_stats = mt7615_tm_dump_stats,
362 };
363