197 lines · c
1// SPDX-License-Identifier: ISC2 3#include "mt7921.h"4#include "mcu.h"5 6enum 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 17struct mt7921_tm_cmd {18 u8 action;19 u32 param0;20 u32 param1;21};22 23struct mt7921_tm_evt {24 u32 param0;25 u32 param1;26};27 28static 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 33static int34mt7921_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 else52 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 }78out:79 mutex_unlock(&dev->mt76.mutex);80 81 return ret;82}83 84static int85mt7921_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);105out:106 dev_kfree_skb(skb);107 108 return ret;109}110 111int 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 148int 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}197