drivers/net/wireless/mediatek/mt76/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 mt792x_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                 __mt792x_mcu_drv_pmctrl(dev);
  61
  62                 phy->test.state = MT76_TM_STATE_ON;
  63         }
  64
  65         if (!mt76_testmode_enabled(phy))
  66                 goto out;
  67
  68         ret = mt76_mcu_send_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL), &cmd,
  69                                 sizeof(cmd), false);
  70         if (ret)
  71                 goto out;
  72
  73         if (normal) {
  74                 /* Switch back to the normal world */
  75                 phy->test.state = MT76_TM_STATE_OFF;
  76                 pm->enable = true;
  77         }
  78 out:
  79         mutex_unlock(&dev->mt76.mutex);
  80
  81         return ret;
  82 }
  83
  84 static int
  85 mt7921_tm_query(struct mt792x_dev *dev, struct mt7921_tm_cmd *req,
  86                 struct mt7921_tm_evt *evt_resp)
  87 {
  88         struct mt7921_rftest_cmd cmd = {
  89                 .action = req->action,
  90                 .param0 = cpu_to_le32(req->param0),
  91                 .param1 = cpu_to_le32(req->param1),
  92         };
  93         struct mt7921_rftest_evt *evt;
  94         struct sk_buff *skb;
  95         int ret;
  96
  97         ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL),
  98                                         &cmd, sizeof(cmd), true, &skb);
  99         if (ret)
 100                 goto out;
 101
 102         evt = (struct mt7921_rftest_evt *)skb->data;
 103         evt_resp->param0 = le32_to_cpu(evt->param0);
 104         evt_resp->param1 = le32_to_cpu(evt->param1);
 105 out:
 106         dev_kfree_skb(skb);
 107
 108         return ret;
 109 }
 110
 111 int mt7921_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 112                         void *data, int len)
 113 {
 114         struct nlattr *tb[NUM_MT76_TM_ATTRS];
 115         struct mt76_phy *mphy = hw->priv;
 116         struct mt792x_phy *phy = mphy->priv;
 117         int err;
 118
 119         if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
 120             !(hw->conf.flags & IEEE80211_CONF_MONITOR))
 121                 return -ENOTCONN;
 122
 123         err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
 124                                    mt76_tm_policy, NULL);
 125         if (err)
 126                 return err;
 127
 128         if (tb[MT76_TM_ATTR_DRV_DATA]) {
 129                 struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
 130                 int ret;
 131
 132                 data = tb[MT76_TM_ATTR_DRV_DATA];
 133                 ret = nla_parse_nested_deprecated(drv_tb,
 134                                                   MT7921_TM_ATTR_MAX,
 135                                                   data, mt7921_tm_policy,
 136                                                   NULL);
 137                 if (ret)
 138                         return ret;
 139
 140                 data = drv_tb[MT7921_TM_ATTR_SET];
 141                 if (data)
 142                         return mt7921_tm_set(phy->dev, nla_data(data));
 143         }
 144
 145         return -EINVAL;
 146 }
 147
 148 int mt7921_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
 149                          struct netlink_callback *cb, void *data, int len)
 150 {
 151         struct nlattr *tb[NUM_MT76_TM_ATTRS];
 152         struct mt76_phy *mphy = hw->priv;
 153         struct mt792x_phy *phy = mphy->priv;
 154         int err;
 155
 156         if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
 157             !(hw->conf.flags & IEEE80211_CONF_MONITOR) ||
 158             !mt76_testmode_enabled(mphy))
 159                 return -ENOTCONN;
 160
 161         if (cb->args[2]++ > 0)
 162                 return -ENOENT;
 163
 164         err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
 165                                    mt76_tm_policy, NULL);
 166         if (err)
 167                 return err;
 168
 169         if (tb[MT76_TM_ATTR_DRV_DATA]) {
 170                 struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
 171                 int ret;
 172
 173                 data = tb[MT76_TM_ATTR_DRV_DATA];
 174                 ret = nla_parse_nested_deprecated(drv_tb,
 175                                                   MT7921_TM_ATTR_MAX,
 176                                                   data, mt7921_tm_policy,
 177                                                   NULL);
 178                 if (ret)
 179                         return ret;
 180
 181                 data = drv_tb[MT7921_TM_ATTR_QUERY];
 182                 if (data) {
 183                         struct mt7921_tm_evt evt_resp;
 184
 185                         err = mt7921_tm_query(phy->dev, nla_data(data),
 186                                               &evt_resp);
 187                         if (err)
 188                                 return err;
 189
 190                         return nla_put(msg, MT7921_TM_ATTR_RSP,
 191                                        sizeof(evt_resp), &evt_resp);
 192                 }
 193         }
 194
 195         return -EINVAL;
 196 }