mt76: mt7915: implement testmode rx support

Support testmode rx and display rx statistic by parsing RXV packet
type, which is currently only enabled in testmode.

Reviewed-by: Ryder Lee <ryder.lee@mediatek.com>
Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
Signed-off-by: Felix Fietkau <nbd@nbd.name>

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/dma.c b/drivers/net/wireless/mediatek/mt76/mt7915/dma.c
index cfa12c4c671fa9488f1aff1c4cd58e9382c78f23..33c42ecef2a4c9e5b84a8cdd19b704e59e973cae 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7915/dma.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/dma.c
@@ -61,6 +61,11 @@ void mt7915_queue_rx_skb(struct mt76_dev *mdev, enum mt76_rxq_id q,
        case PKT_TYPE_RX_EVENT:
                mt7915_mcu_rx_event(dev, skb);
                break;
+#ifdef CONFIG_NL80211_TESTMODE
+       case PKT_TYPE_TXRXV:
+               mt7915_mac_fill_rx_vector(dev, skb);
+               break;
+#endif
        case PKT_TYPE_NORMAL:
                if (!mt7915_mac_fill_rx(dev, skb)) {
                        mt76_rx(&dev->mt76, q, skb);

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mac.c b/drivers/net/wireless/mediatek/mt76/mt7915/mac.c
index 923d87be79e056acd3d894f45751c33814131bc3..74299887b9936619923c562483cbe155c5bc80f4 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mac.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mac.c
@@ -562,6 +562,44 @@ int mt7915_mac_fill_rx(struct mt7915_dev *dev, struct sk_buff *skb)
        return 0;
 }
 
+#ifdef CONFIG_NL80211_TESTMODE
+void mt7915_mac_fill_rx_vector(struct mt7915_dev *dev, struct sk_buff *skb)
+{
+       __le32 *rxd = (__le32 *)skb->data;
+       __le32 *rxv = rxd + 4;
+       u32 rcpi, ib_rssi, wb_rssi, v20, v21;
+       s32 foe;
+       u8 snr;
+       int i;
+
+       rcpi = le32_to_cpu(rxv[6]);
+       ib_rssi = le32_to_cpu(rxv[7]);
+       wb_rssi = le32_to_cpu(rxv[8]) >> 5;
+
+       for (i = 0; i < 4; i++, rcpi >>= 8, ib_rssi >>= 8, wb_rssi >>= 9) {
+               if (i == 3)
+                       wb_rssi = le32_to_cpu(rxv[9]);
+
+               dev->test.last_rcpi[i] = rcpi & 0xff;
+               dev->test.last_ib_rssi[i] = ib_rssi & 0xff;
+               dev->test.last_wb_rssi[i] = wb_rssi & 0xff;
+       }
+
+       v20 = le32_to_cpu(rxv[20]);
+       v21 = le32_to_cpu(rxv[21]);
+
+       foe = FIELD_GET(MT_CRXV_FOE_LO, v20) |
+             (FIELD_GET(MT_CRXV_FOE_HI, v21) << MT_CRXV_FOE_SHIFT);
+
+       snr = FIELD_GET(MT_CRXV_SNR, v20) - 16;
+
+       dev->test.last_freq_offset = foe;
+       dev->test.last_snr = snr;
+
+       dev_kfree_skb(skb);
+}
+#endif
+
 static void
 mt7915_mac_write_txwi_tm(struct mt7915_dev *dev, struct mt76_phy *mphy,
                         __le32 *txwi, struct sk_buff *skb)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mac.h b/drivers/net/wireless/mediatek/mt76/mt7915/mac.h
index 0921b6f10a9927613056c5c3380d4406e7eb3464..d420392b952dec2d07f5c94522cf7eceb04e7416 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mac.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mac.h
@@ -128,6 +128,11 @@ enum rx_pkt_type {
 #define MT_CRXV_HE_BEAM_CHNG           BIT(13)
 #define MT_CRXV_HE_DOPPLER             BIT(16)
 
+#define MT_CRXV_SNR            GENMASK(18, 13)
+#define MT_CRXV_FOE_LO         GENMASK(31, 19)
+#define MT_CRXV_FOE_HI         GENMASK(6, 0)
+#define MT_CRXV_FOE_SHIFT      13
+
 enum tx_header_format {
        MT_HDR_FORMAT_802_3,
        MT_HDR_FORMAT_CMD,

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h b/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
index 957528510f8341f6f16507458d34fb945f1f8ce1..9e47905021fb056f7b7a67428586e6837e4f4e14 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
@@ -171,6 +171,12 @@ struct mt7915_dev {
        struct {
                u32 *reg_backup;
 
+               s32 last_freq_offset;
+               u8 last_rcpi[4];
+               s8 last_ib_rssi[4];
+               s8 last_wb_rssi[4];
+               u8 last_snr;
+
                u8 spe_idx;
        } test;
 #endif
@@ -442,6 +448,7 @@ void mt7915_mac_write_txwi(struct mt7915_dev *dev, __le32 *txwi,
                           struct ieee80211_key_conf *key, bool beacon);
 void mt7915_mac_set_timing(struct mt7915_phy *phy);
 int mt7915_mac_fill_rx(struct mt7915_dev *dev, struct sk_buff *skb);
+void mt7915_mac_fill_rx_vector(struct mt7915_dev *dev, struct sk_buff *skb);
 void mt7915_mac_tx_free(struct mt7915_dev *dev, struct sk_buff *skb);
 int mt7915_mac_sta_add(struct mt76_dev *mdev, struct ieee80211_vif *vif,
                       struct ieee80211_sta *sta);

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c b/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c
index 0b90ecbe0012228873f43dc37a7f685a3a31eb21..c2dd19d95a45b6893314f4b3eac6ffadad019985 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c
@@ -31,6 +31,8 @@ static const struct reg_band reg_backup_list[] = {
        REG_BAND(TMAC_ICR0),
        REG_BAND_IDX(ARB_DRNGR0, 0),
        REG_BAND_IDX(ARB_DRNGR0, 1),
+       REG_BAND(WF_RFCR),
+       REG_BAND(WF_RFCR1),
 };
 
 static int
@@ -111,6 +113,10 @@ mt7915_tm_reg_backup_restore(struct mt7915_dev *dev, struct mt7915_phy *phy)
 
        mt76_wr(dev, MT_TMAC_TFCR0(ext_phy), 0);
        mt76_clear(dev, MT_TMAC_TCR0(ext_phy), MT_TMAC_TCR0_TBTT_STOP_CTRL);
+
+       /* config rx filter for testmode rx */
+       mt76_wr(dev, MT_WF_RFCR(ext_phy), 0xcf70a);
+       mt76_wr(dev, MT_WF_RFCR1(ext_phy), 0);
 }
 
 static void
@@ -156,6 +162,20 @@ mt7915_tm_set_tx_frames(struct mt7915_dev *dev, bool en)
        info->control.vif = dev->phy.monitor_vif;
 }
 
+static void
+mt7915_tm_set_rx_frames(struct mt7915_dev *dev, bool en)
+{
+       if (en) {
+               mutex_unlock(&dev->mt76.mutex);
+               mt7915_set_channel(&dev->phy);
+               mutex_lock(&dev->mt76.mutex);
+
+               mt7915_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
+       }
+
+       mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_RX_RXV, en);
+}
+
 static int
 mt7915_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
 {
@@ -169,12 +189,69 @@ mt7915_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
                mt7915_tm_set_tx_frames(dev, false);
        else if (state == MT76_TM_STATE_TX_FRAMES)
                mt7915_tm_set_tx_frames(dev, true);
+       else if (prev_state == MT76_TM_STATE_RX_FRAMES)
+               mt7915_tm_set_rx_frames(dev, false);
+       else if (state == MT76_TM_STATE_RX_FRAMES)
+               mt7915_tm_set_rx_frames(dev, true);
        else if (prev_state == MT76_TM_STATE_OFF || state == MT76_TM_STATE_OFF)
                mt7915_tm_init(dev);
 
        return 0;
 }
 
+static int
+mt7915_tm_dump_stats(struct mt76_dev *mdev, struct sk_buff *msg)
+{
+       struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
+       void *rx, *rssi;
+       int i;
+
+       rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
+       if (!rx)
+               return -ENOMEM;
+
+       if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, dev->test.last_freq_offset))
+               return -ENOMEM;
+
+       rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
+       if (!rssi)
+               return -ENOMEM;
+
+       for (i = 0; i < ARRAY_SIZE(dev->test.last_rcpi); i++)
+               if (nla_put_u8(msg, i, dev->test.last_rcpi[i]))
+                       return -ENOMEM;
+
+       nla_nest_end(msg, rssi);
+
+       rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI);
+       if (!rssi)
+               return -ENOMEM;
+
+       for (i = 0; i < ARRAY_SIZE(dev->test.last_ib_rssi); i++)
+               if (nla_put_s8(msg, i, dev->test.last_ib_rssi[i]))
+                       return -ENOMEM;
+
+       nla_nest_end(msg, rssi);
+
+       rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI);
+       if (!rssi)
+               return -ENOMEM;
+
+       for (i = 0; i < ARRAY_SIZE(dev->test.last_wb_rssi); i++)
+               if (nla_put_s8(msg, i, dev->test.last_wb_rssi[i]))
+                       return -ENOMEM;
+
+       nla_nest_end(msg, rssi);
+
+       if (nla_put_u8(msg, MT76_TM_RX_ATTR_SNR, dev->test.last_snr))
+               return -ENOMEM;
+
+       nla_nest_end(msg, rx);
+
+       return 0;
+}
+
 const struct mt76_testmode_ops mt7915_testmode_ops = {
        .set_state = mt7915_tm_set_state,
+       .dump_stats = mt7915_tm_dump_stats,
 };