mt76: testmode: add support to set MAC

Add support to set SA/DA/BSSID in testmode.
During mass production stage, some test scripts or test equipments need
to set fixed addresses to detect or parse if a test is passed.
Also, MAC setting is necessary for some tx/rx tests with rx filter,
to make sure rx site only receives expected packets.

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/mt76.h b/drivers/net/wireless/mediatek/mt76/mt76.h
index e2da720..e7ced94 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt76.h
+++ b/drivers/net/wireless/mediatek/mt76/mt76.h
@@ -599,6 +599,8 @@ struct mt76_testmode_data {
        u8 tx_power[4];
        u8 tx_power_control;
 
+       u8 addr[3][ETH_ALEN];
+
        u32 tx_pending;
        u32 tx_queued;
        u16 tx_queued_limit;

diff --git a/drivers/net/wireless/mediatek/mt76/testmode.c b/drivers/net/wireless/mediatek/mt76/testmode.c
index 66afc2b..1a01ad7 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/testmode.c
+++ b/drivers/net/wireless/mediatek/mt76/testmode.c
@@ -126,9 +126,9 @@ int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
 
        hdr = __skb_put_zero(head, head_len);
        hdr->frame_control = cpu_to_le16(fc);
-       memcpy(hdr->addr1, phy->macaddr, sizeof(phy->macaddr));
-       memcpy(hdr->addr2, phy->macaddr, sizeof(phy->macaddr));
-       memcpy(hdr->addr3, phy->macaddr, sizeof(phy->macaddr));
+       memcpy(hdr->addr1, td->addr[0], ETH_ALEN);
+       memcpy(hdr->addr2, td->addr[1], ETH_ALEN);
+       memcpy(hdr->addr3, td->addr[2], ETH_ALEN);
        skb_set_queue_mapping(head, IEEE80211_AC_BE);
 
        info = IEEE80211_SKB_CB(head);
@@ -318,6 +318,10 @@ mt76_testmode_init_defaults(struct mt76_phy *phy)
        td->tx_count = 1;
        td->tx_rate_mode = MT76_TM_TX_MODE_OFDM;
        td->tx_rate_nss = 1;
+
+       memcpy(td->addr[0], phy->macaddr, ETH_ALEN);
+       memcpy(td->addr[1], phy->macaddr, ETH_ALEN);
+       memcpy(td->addr[2], phy->macaddr, ETH_ALEN);
 }
 
 static int
@@ -493,6 +497,20 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
                }
        }
 
+       if (tb[MT76_TM_ATTR_MAC_ADDRS]) {
+               struct nlattr *cur;
+               int idx = 0;
+               int rem;
+
+               nla_for_each_nested(cur, tb[MT76_TM_ATTR_MAC_ADDRS], rem) {
+                       if (nla_len(cur) != ETH_ALEN || idx >= 3)
+                               goto out;
+
+                       memcpy(td->addr[idx], nla_data(cur), ETH_ALEN);
+                       idx++;
+               }
+       }
+
        if (dev->test_ops->set_params) {
                err = dev->test_ops->set_params(phy, tb, state);
                if (err)
@@ -635,6 +653,18 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
                nla_nest_end(msg, a);
        }
 
+       if (mt76_testmode_param_present(td, MT76_TM_ATTR_MAC_ADDRS)) {
+               a = nla_nest_start(msg, MT76_TM_ATTR_MAC_ADDRS);
+               if (!a)
+                       goto out;
+
+               for (i = 0; i < 3; i++)
+                       if (nla_put(msg, i, ETH_ALEN, td->addr[i]))
+                               goto out;
+
+               nla_nest_end(msg, a);
+       }
+
        err = 0;
 
 out:

diff --git a/drivers/net/wireless/mediatek/mt76/testmode.h b/drivers/net/wireless/mediatek/mt76/testmode.h
index d1f9c03..5e2792d 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/testmode.h
+++ b/drivers/net/wireless/mediatek/mt76/testmode.h
@@ -45,6 +45,8 @@
  * @MT76_TM_ATTR_TX_TIME: packet transmission time, in unit of us (u32)
  *
  * @MT76_TM_ATTR_DRV_DATA: driver specific netlink attrs (nested)
+ *
+ * @MT76_TM_ATTR_MAC_ADDRS: array of nested MAC addresses (nested)
  */
 enum mt76_testmode_attr {
        MT76_TM_ATTR_UNSPEC,
@@ -81,6 +83,8 @@ enum mt76_testmode_attr {
 
        MT76_TM_ATTR_DRV_DATA,
 
+       MT76_TM_ATTR_MAC_ADDRS,
+
        /* keep last */
        NUM_MT76_TM_ATTRS,
        MT76_TM_ATTR_MAX = NUM_MT76_TM_ATTRS - 1,