377 lines · c
1// SPDX-License-Identifier: ISC2/* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */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 /* must be last */14 NUM_TM_CHANGED15};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 int48mt7615_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 void98mt7615_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 void133mt7615_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 mt76_update_channel(phy->mt76);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 void152mt7615_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 void172mt7615_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 /* 2.4 GHz band */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 /* 5 GHz band */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 else219 val |= (2 << 28) | (2 << 26) | (8 << 20);220 mt7615_rf_wr(dev, i, 0x48, val);221 }222}223 224static void225mt7615_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 void242mt7615_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 int259mt7615_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 int296mt7615_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 int324mt7615_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