mt76/mt7615/testmode.c

0001 // SPDX-License-Identifier: ISC
0002 /* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */
0003
0004 #include "mt7615.h"
0005 #include "eeprom.h"
0006 #include "mcu.h"
0007
0008 enum {
0009     TM_CHANGED_TXPOWER_CTRL,
0010     TM_CHANGED_TXPOWER,
0011     TM_CHANGED_FREQ_OFFSET,
0012
0013     /* must be last */
0014     NUM_TM_CHANGED
0015 };
0016
0017
0018 static const u8 tm_change_map[] = {
0019     [TM_CHANGED_TXPOWER_CTRL] = MT76_TM_ATTR_TX_POWER_CONTROL,
0020     [TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
0021     [TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
0022 };
0023
0024 static const u32 reg_backup_list[] = {
0025     MT_WF_PHY_RFINTF3_0(0),
0026     MT_WF_PHY_RFINTF3_0(1),
0027     MT_WF_PHY_RFINTF3_0(2),
0028     MT_WF_PHY_RFINTF3_0(3),
0029     MT_ANT_SWITCH_CON(2),
0030     MT_ANT_SWITCH_CON(3),
0031     MT_ANT_SWITCH_CON(4),
0032     MT_ANT_SWITCH_CON(6),
0033     MT_ANT_SWITCH_CON(7),
0034     MT_ANT_SWITCH_CON(8),
0035 };
0036
0037 static const struct {
0038     u16 wf;
0039     u16 reg;
0040 } rf_backup_list[] = {
0041     { 0, 0x48 },
0042     { 1, 0x48 },
0043     { 2, 0x48 },
0044     { 3, 0x48 },
0045 };
0046
0047 static int
0048 mt7615_tm_set_tx_power(struct mt7615_phy *phy)
0049 {
0050     struct mt7615_dev *dev = phy->dev;
0051     struct mt76_phy *mphy = phy->mt76;
0052     int i, ret, n_chains = hweight8(mphy->antenna_mask);
0053     struct cfg80211_chan_def *chandef = &mphy->chandef;
0054     int freq = chandef->center_freq1, len, target_chains;
0055     u8 *data, *eep = (u8 *)dev->mt76.eeprom.data;
0056     enum nl80211_band band = chandef->chan->band;
0057     struct sk_buff *skb;
0058     struct {
0059         u8 center_chan;
0060         u8 dbdc_idx;
0061         u8 band;
0062         u8 rsv;
0063     } __packed req_hdr = {
0064         .center_chan = ieee80211_frequency_to_channel(freq),
0065         .band = band,
0066         .dbdc_idx = phy != &dev->phy,
0067     };
0068     u8 *tx_power = NULL;
0069
0070     if (mphy->test.state != MT76_TM_STATE_OFF)
0071         tx_power = mphy->test.tx_power;
0072
0073     len = MT7615_EE_MAX - MT_EE_NIC_CONF_0;
0074     skb = mt76_mcu_msg_alloc(&dev->mt76, NULL, sizeof(req_hdr) + len);
0075     if (!skb)
0076         return -ENOMEM;
0077
0078     skb_put_data(skb, &req_hdr, sizeof(req_hdr));
0079     data = skb_put_data(skb, eep + MT_EE_NIC_CONF_0, len);
0080
0081     target_chains = mt7615_ext_pa_enabled(dev, band) ? 1 : n_chains;
0082     for (i = 0; i < target_chains; i++) {
0083         ret = mt7615_eeprom_get_target_power_index(dev, chandef->chan, i);
0084         if (ret < 0) {
0085             dev_kfree_skb(skb);
0086             return -EINVAL;
0087         }
0088
0089         if (tx_power && tx_power[i])
0090             data[ret - MT_EE_NIC_CONF_0] = tx_power[i];
0091     }
0092
0093     return mt76_mcu_skb_send_msg(&dev->mt76, skb,
0094                      MCU_EXT_CMD(SET_TX_POWER_CTRL), false);
0095 }
0096
0097 static void
0098 mt7615_tm_reg_backup_restore(struct mt7615_phy *phy)
0099 {
0100     struct mt7615_dev *dev = phy->dev;
0101     u32 *b = phy->test.reg_backup;
0102     int n_regs = ARRAY_SIZE(reg_backup_list);
0103     int n_rf_regs = ARRAY_SIZE(rf_backup_list);
0104     int i;
0105
0106     if (phy->mt76->test.state == MT76_TM_STATE_OFF) {
0107         for (i = 0; i < n_regs; i++)
0108             mt76_wr(dev, reg_backup_list[i], b[i]);
0109
0110         for (i = 0; i < n_rf_regs; i++)
0111             mt7615_rf_wr(dev, rf_backup_list[i].wf,
0112                      rf_backup_list[i].reg, b[n_regs + i]);
0113         return;
0114     }
0115
0116     if (b)
0117         return;
0118
0119     b = devm_kzalloc(dev->mt76.dev, 4 * (n_regs + n_rf_regs),
0120              GFP_KERNEL);
0121     if (!b)
0122         return;
0123
0124     phy->test.reg_backup = b;
0125     for (i = 0; i < n_regs; i++)
0126         b[i] = mt76_rr(dev, reg_backup_list[i]);
0127     for (i = 0; i < n_rf_regs; i++)
0128         b[n_regs + i] = mt7615_rf_rr(dev, rf_backup_list[i].wf,
0129                          rf_backup_list[i].reg);
0130 }
0131
0132 static void
0133 mt7615_tm_init(struct mt7615_phy *phy)
0134 {
0135     struct mt7615_dev *dev = phy->dev;
0136     unsigned int total_flags = ~0;
0137
0138     if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
0139         return;
0140
0141     mt7615_mcu_set_sku_en(phy, phy->mt76->test.state == MT76_TM_STATE_OFF);
0142
0143     mutex_unlock(&dev->mt76.mutex);
0144     mt7615_set_channel(phy);
0145     mt7615_ops.configure_filter(phy->mt76->hw, 0, &total_flags, 0);
0146     mutex_lock(&dev->mt76.mutex);
0147
0148     mt7615_tm_reg_backup_restore(phy);
0149 }
0150
0151 static void
0152 mt7615_tm_set_rx_enable(struct mt7615_dev *dev, bool en)
0153 {
0154     u32 rqcr_mask = (MT_ARB_RQCR_RX_START |
0155              MT_ARB_RQCR_RXV_START |
0156              MT_ARB_RQCR_RXV_R_EN |
0157              MT_ARB_RQCR_RXV_T_EN) *
0158             (BIT(0) | BIT(MT_ARB_RQCR_BAND_SHIFT));
0159
0160     if (en) {
0161         mt76_clear(dev, MT_ARB_SCR,
0162                MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
0163         mt76_set(dev, MT_ARB_RQCR, rqcr_mask);
0164     } else {
0165         mt76_set(dev, MT_ARB_SCR,
0166              MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
0167         mt76_clear(dev, MT_ARB_RQCR, rqcr_mask);
0168     }
0169 }
0170
0171 static void
0172 mt7615_tm_set_tx_antenna(struct mt7615_phy *phy, bool en)
0173 {
0174     struct mt7615_dev *dev = phy->dev;
0175     struct mt76_testmode_data *td = &phy->mt76->test;
0176     u8 mask = td->tx_antenna_mask;
0177     int i;
0178
0179     if (!mask)
0180         return;
0181
0182     if (!en)
0183         mask = phy->mt76->chainmask;
0184
0185     for (i = 0; i < 4; i++) {
0186         mt76_rmw_field(dev, MT_WF_PHY_RFINTF3_0(i),
0187                    MT_WF_PHY_RFINTF3_0_ANT,
0188                    (mask & BIT(i)) ? 0 : 0xa);
0189     }
0190
0191     /* 2.4 GHz band */
0192     mt76_rmw_field(dev, MT_ANT_SWITCH_CON(3), MT_ANT_SWITCH_CON_MODE(0),
0193                (mask & BIT(0)) ? 0x8 : 0x1b);
0194     mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(2),
0195                (mask & BIT(1)) ? 0xe : 0x1b);
0196     mt76_rmw_field(dev, MT_ANT_SWITCH_CON(6), MT_ANT_SWITCH_CON_MODE1(0),
0197                (mask & BIT(2)) ? 0x0 : 0xf);
0198     mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE1(2),
0199                (mask & BIT(3)) ? 0x6 : 0xf);
0200
0201     /* 5 GHz band */
0202     mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(1),
0203                (mask & BIT(0)) ? 0xd : 0x1b);
0204     mt76_rmw_field(dev, MT_ANT_SWITCH_CON(2), MT_ANT_SWITCH_CON_MODE(3),
0205                (mask & BIT(1)) ? 0x13 : 0x1b);
0206     mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE1(1),
0207                (mask & BIT(2)) ? 0x5 : 0xf);
0208     mt76_rmw_field(dev, MT_ANT_SWITCH_CON(8), MT_ANT_SWITCH_CON_MODE1(3),
0209                (mask & BIT(3)) ? 0xb : 0xf);
0210
0211     for (i = 0; i < 4; i++) {
0212         u32 val;
0213
0214         val = mt7615_rf_rr(dev, i, 0x48);
0215         val &= ~(0x3ff << 20);
0216         if (mask & BIT(i))
0217             val |= 3 << 20;
0218         else
0219             val |= (2 << 28) | (2 << 26) | (8 << 20);
0220         mt7615_rf_wr(dev, i, 0x48, val);
0221     }
0222 }
0223
0224 static void
0225 mt7615_tm_set_tx_frames(struct mt7615_phy *phy, bool en)
0226 {
0227     struct mt7615_dev *dev = phy->dev;
0228     struct ieee80211_tx_info *info;
0229     struct sk_buff *skb = phy->mt76->test.tx_skb;
0230
0231     mt7615_mcu_set_chan_info(phy, MCU_EXT_CMD(SET_RX_PATH));
0232     mt7615_tm_set_tx_antenna(phy, en);
0233     mt7615_tm_set_rx_enable(dev, !en);
0234     if (!en || !skb)
0235         return;
0236
0237     info = IEEE80211_SKB_CB(skb);
0238     info->control.vif = phy->monitor_vif;
0239 }
0240
0241 static void
0242 mt7615_tm_update_params(struct mt7615_phy *phy, u32 changed)
0243 {
0244     struct mt7615_dev *dev = phy->dev;
0245     struct mt76_testmode_data *td = &phy->mt76->test;
0246     bool en = phy->mt76->test.state != MT76_TM_STATE_OFF;
0247
0248     if (changed & BIT(TM_CHANGED_TXPOWER_CTRL))
0249         mt7615_mcu_set_test_param(dev, MCU_ATE_SET_TX_POWER_CONTROL,
0250                       en, en && td->tx_power_control);
0251     if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
0252         mt7615_mcu_set_test_param(dev, MCU_ATE_SET_FREQ_OFFSET,
0253                       en, en ? td->freq_offset : 0);
0254     if (changed & BIT(TM_CHANGED_TXPOWER))
0255         mt7615_tm_set_tx_power(phy);
0256 }
0257
0258 static int
0259 mt7615_tm_set_state(struct mt76_phy *mphy, enum mt76_testmode_state state)
0260 {
0261     struct mt7615_phy *phy = mphy->priv;
0262     struct mt76_testmode_data *td = &mphy->test;
0263     enum mt76_testmode_state prev_state = td->state;
0264
0265     mphy->test.state = state;
0266
0267     if (prev_state == MT76_TM_STATE_TX_FRAMES)
0268         mt7615_tm_set_tx_frames(phy, false);
0269     else if (state == MT76_TM_STATE_TX_FRAMES)
0270         mt7615_tm_set_tx_frames(phy, true);
0271
0272     if (state <= MT76_TM_STATE_IDLE)
0273         mt7615_tm_init(phy);
0274
0275     if ((state == MT76_TM_STATE_IDLE &&
0276          prev_state == MT76_TM_STATE_OFF) ||
0277         (state == MT76_TM_STATE_OFF &&
0278          prev_state == MT76_TM_STATE_IDLE)) {
0279         u32 changed = 0;
0280         int i;
0281
0282         for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
0283             u16 cur = tm_change_map[i];
0284
0285             if (td->param_set[cur / 32] & BIT(cur % 32))
0286                 changed |= BIT(i);
0287         }
0288
0289         mt7615_tm_update_params(phy, changed);
0290     }
0291
0292     return 0;
0293 }
0294
0295 static int
0296 mt7615_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb,
0297              enum mt76_testmode_state new_state)
0298 {
0299     struct mt76_testmode_data *td = &mphy->test;
0300     struct mt7615_phy *phy = mphy->priv;
0301     u32 changed = 0;
0302     int i;
0303
0304     BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
0305
0306     if (new_state == MT76_TM_STATE_OFF ||
0307         td->state == MT76_TM_STATE_OFF)
0308         return 0;
0309
0310     if (td->tx_antenna_mask & ~mphy->chainmask)
0311         return -EINVAL;
0312
0313     for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
0314         if (tb[tm_change_map[i]])
0315             changed |= BIT(i);
0316     }
0317
0318     mt7615_tm_update_params(phy, changed);
0319
0320     return 0;
0321 }
0322
0323 static int
0324 mt7615_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
0325 {
0326     struct mt7615_phy *phy = mphy->priv;
0327     void *rx, *rssi;
0328     int i;
0329
0330     rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
0331     if (!rx)
0332         return -ENOMEM;
0333
0334     if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, phy->test.last_freq_offset))
0335         return -ENOMEM;
0336
0337     rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
0338     if (!rssi)
0339         return -ENOMEM;
0340
0341     for (i = 0; i < ARRAY_SIZE(phy->test.last_rcpi); i++)
0342         if (nla_put_u8(msg, i, phy->test.last_rcpi[i]))
0343             return -ENOMEM;
0344
0345     nla_nest_end(msg, rssi);
0346
0347     rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI);
0348     if (!rssi)
0349         return -ENOMEM;
0350
0351     for (i = 0; i < ARRAY_SIZE(phy->test.last_ib_rssi); i++)
0352         if (nla_put_s8(msg, i, phy->test.last_ib_rssi[i]))
0353             return -ENOMEM;
0354
0355     nla_nest_end(msg, rssi);
0356
0357     rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI);
0358     if (!rssi)
0359         return -ENOMEM;
0360
0361     for (i = 0; i < ARRAY_SIZE(phy->test.last_wb_rssi); i++)
0362         if (nla_put_s8(msg, i, phy->test.last_wb_rssi[i]))
0363             return -ENOMEM;
0364
0365     nla_nest_end(msg, rssi);
0366
0367     nla_nest_end(msg, rx);
0368
0369     return 0;
0370 }
0371
0372 const struct mt76_testmode_ops mt7615_testmode_ops = {
0373     .set_state = mt7615_tm_set_state,
0374     .set_params = mt7615_tm_set_params,
0375     .dump_stats = mt7615_tm_dump_stats,
0376 };