drivers/net/wireless/mediatek/mt76-6e-usb/mt7921/testmode.c

   1 // SPDX-License-Identifier: ISC
   2
   3 #include "mt7921.h"
   4 #include "mcu.h"
   5
   6 enum mt7921_testmode_attr {
   7         MT7921_TM_ATTR_UNSPEC,
   8         MT7921_TM_ATTR_SET,
   9         MT7921_TM_ATTR_QUERY,
  10         MT7921_TM_ATTR_RSP,
  11
  12         /* keep last */
  13         NUM_MT7921_TM_ATTRS,
  14         MT7921_TM_ATTR_MAX = NUM_MT7921_TM_ATTRS - 1,
  15 };
  16
  17 struct mt7921_tm_cmd {
  18         u8 action;
  19         u32 param0;
  20         u32 param1;
  21 };
  22
  23 struct mt7921_tm_evt {
  24         u32 param0;
  25         u32 param1;
  26 };
  27
  28 static const struct nla_policy mt7921_tm_policy[NUM_MT7921_TM_ATTRS] = {
  29         [MT7921_TM_ATTR_SET] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7921_tm_cmd)),
  30         [MT7921_TM_ATTR_QUERY] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7921_tm_cmd)),
  31 };
  32
  33 static int
  34 mt7921_tm_set(struct mt7921_dev *dev, struct mt7921_tm_cmd *req)
  35 {
  36         struct mt7921_rftest_cmd cmd = {
  37                 .action = req->action,
  38                 .param0 = cpu_to_le32(req->param0),
  39                 .param1 = cpu_to_le32(req->param1),
  40         };
  41         bool testmode = false, normal = false;
  42         struct mt76_connac_pm *pm = &dev->pm;
  43         struct mt76_phy *phy = &dev->mphy;
  44         int ret = -ENOTCONN;
  45
  46         mutex_lock(&dev->mt76.mutex);
  47
  48         if (req->action == TM_SWITCH_MODE) {
  49                 if (req->param0 == MT7921_TM_NORMAL)
  50                         normal = true;
  51                 else
  52                         testmode = true;
  53         }
  54
  55         if (testmode) {
  56                 /* Make sure testmode running on full power mode */
  57                 pm->enable = false;
  58                 cancel_delayed_work_sync(&pm->ps_work);
  59                 cancel_work_sync(&pm->wake_work);
  60                 __mt7921_mcu_drv_pmctrl(dev);
  61
  62                 mt76_wr(dev, MT_WF_RFCR(0), dev->mt76.rxfilter);
  63                 phy->test.state = MT76_TM_STATE_ON;
  64         }
  65
  66         if (!mt76_testmode_enabled(phy))
  67                 goto out;
  68
  69         ret = mt76_mcu_send_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL), &cmd,
  70                                 sizeof(cmd), false);
  71         if (ret)
  72                 goto out;
  73
  74         if (normal) {
  75                 /* Switch back to the normal world */
  76                 phy->test.state = MT76_TM_STATE_OFF;
  77                 pm->enable = true;
  78         }
  79 out:
  80         mutex_unlock(&dev->mt76.mutex);
  81
  82         return ret;
  83 }
  84
  85 static int
  86 mt7921_tm_query(struct mt7921_dev *dev, struct mt7921_tm_cmd *req,
  87                 struct mt7921_tm_evt *evt_resp)
  88 {
  89         struct mt7921_rftest_cmd cmd = {
  90                 .action = req->action,
  91                 .param0 = cpu_to_le32(req->param0),
  92                 .param1 = cpu_to_le32(req->param1),
  93         };
  94         struct mt7921_rftest_evt *evt;
  95         struct sk_buff *skb;
  96         int ret;
  97
  98         ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL),
  99                                         &cmd, sizeof(cmd), true, &skb);
 100         if (ret)
 101                 goto out;
 102
 103         evt = (struct mt7921_rftest_evt *)skb->data;
 104         evt_resp->param0 = le32_to_cpu(evt->param0);
 105         evt_resp->param1 = le32_to_cpu(evt->param1);
 106 out:
 107         dev_kfree_skb(skb);
 108
 109         return ret;
 110 }
 111
 112 int mt7921_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 113                         void *data, int len)
 114 {
 115         struct nlattr *tb[NUM_MT76_TM_ATTRS];
 116         struct mt76_phy *mphy = hw->priv;
 117         struct mt7921_phy *phy = mphy->priv;
 118         int err;
 119
 120         if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
 121             !(hw->conf.flags & IEEE80211_CONF_MONITOR))
 122                 return -ENOTCONN;
 123
 124         err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
 125                                    mt76_tm_policy, NULL);
 126         if (err)
 127                 return err;
 128
 129         if (tb[MT76_TM_ATTR_DRV_DATA]) {
 130                 struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
 131                 int ret;
 132
 133                 data = tb[MT76_TM_ATTR_DRV_DATA];
 134                 ret = nla_parse_nested_deprecated(drv_tb,
 135                                                   MT7921_TM_ATTR_MAX,
 136                                                   data, mt7921_tm_policy,
 137                                                   NULL);
 138                 if (ret)
 139                         return ret;
 140
 141                 data = drv_tb[MT7921_TM_ATTR_SET];
 142                 if (data)
 143                         return mt7921_tm_set(phy->dev, nla_data(data));
 144         }
 145
 146         return -EINVAL;
 147 }
 148
 149 int mt7921_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
 150                          struct netlink_callback *cb, void *data, int len)
 151 {
 152         struct nlattr *tb[NUM_MT76_TM_ATTRS];
 153         struct mt76_phy *mphy = hw->priv;
 154         struct mt7921_phy *phy = mphy->priv;
 155         int err;
 156
 157         if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
 158             !(hw->conf.flags & IEEE80211_CONF_MONITOR) ||
 159             !mt76_testmode_enabled(mphy))
 160                 return -ENOTCONN;
 161
 162         if (cb->args[2]++ > 0)
 163                 return -ENOENT;
 164
 165         err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
 166                                    mt76_tm_policy, NULL);
 167         if (err)
 168                 return err;
 169
 170         if (tb[MT76_TM_ATTR_DRV_DATA]) {
 171                 struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
 172                 int ret;
 173
 174                 data = tb[MT76_TM_ATTR_DRV_DATA];
 175                 ret = nla_parse_nested_deprecated(drv_tb,
 176                                                   MT7921_TM_ATTR_MAX,
 177                                                   data, mt7921_tm_policy,
 178                                                   NULL);
 179                 if (ret)
 180                         return ret;
 181
 182                 data = drv_tb[MT7921_TM_ATTR_QUERY];
 183                 if (data) {
 184                         struct mt7921_tm_evt evt_resp;
 185
 186                         err = mt7921_tm_query(phy->dev, nla_data(data),
 187                                               &evt_resp);
 188                         if (err)
 189                                 return err;
 190
 191                         return nla_put(msg, MT7921_TM_ATTR_RSP,
 192                                        sizeof(evt_resp), &evt_resp);
 193                 }
 194         }
 195
 196         return -EINVAL;
 197 }