testmode.c

  1// SPDX-License-Identifier: BSD-3-Clause-Clear
  2
  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 int
 34mt7921_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	}
 78out:
 79	mutex_unlock(&dev->mt76.mutex);
 80
 81	return ret;
 82}
 83
 84static int
 85mt7921_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}