testmode.c

  1// SPDX-License-Identifier: BSD-3-Clause-Clear
  2
  3#include "mt7925.h"
  4#include "mcu.h"
  5
  6#define MT7925_EVT_RSP_LEN 512
  7
  8enum mt7925_testmode_attr {
  9	MT7925_TM_ATTR_UNSPEC,
 10	MT7925_TM_ATTR_SET,
 11	MT7925_TM_ATTR_QUERY,
 12	MT7925_TM_ATTR_RSP,
 13
 14	/* keep last */
 15	NUM_MT7925_TM_ATTRS,
 16	MT7925_TM_ATTR_MAX = NUM_MT7925_TM_ATTRS - 1,
 17};
 18
 19struct mt7925_tm_cmd {
 20	u8 padding[4];
 21	struct uni_cmd_testmode_ctrl c;
 22} __packed;
 23
 24struct mt7925_tm_evt {
 25	u32 param0;
 26	u32 param1;
 27} __packed;
 28
 29static const struct nla_policy mt7925_tm_policy[NUM_MT7925_TM_ATTRS] = {
 30	[MT7925_TM_ATTR_SET] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7925_tm_cmd)),
 31	[MT7925_TM_ATTR_QUERY] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7925_tm_cmd)),
 32};
 33
 34static int
 35mt7925_tm_set(struct mt792x_dev *dev, struct mt7925_tm_cmd *req)
 36{
 37	struct mt7925_rftest_cmd cmd;
 38	struct mt7925_rftest_cmd *pcmd = &cmd;
 39	bool testmode = false, normal = false;
 40	struct mt76_connac_pm *pm = &dev->pm;
 41	struct mt76_phy *phy = &dev->mphy;
 42	int ret = -ENOTCONN;
 43
 44	memset(pcmd, 0, sizeof(*pcmd));
 45	memcpy(pcmd, req, sizeof(struct mt7925_tm_cmd));
 46
 47	mutex_lock(&dev->mt76.mutex);
 48
 49	if (pcmd->ctrl.action == CMD_TEST_CTRL_ACT_SWITCH_MODE) {
 50		if (pcmd->ctrl.data.op_mode == CMD_TEST_CTRL_ACT_SWITCH_MODE_NORMAL)
 51			normal = true;
 52		else
 53			testmode = true;
 54	}
 55
 56	if (testmode) {
 57		/* Make sure testmode running on full power mode */
 58		pm->enable = false;
 59		cancel_delayed_work_sync(&pm->ps_work);
 60		cancel_work_sync(&pm->wake_work);
 61		__mt792x_mcu_drv_pmctrl(dev);
 62
 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_UNI_CMD(TESTMODE_CTRL), &cmd,
 70				sizeof(cmd), false);
 71
 72	if (ret)
 73		goto out;
 74
 75	if (normal) {
 76		/* Switch back to the normal world */
 77		phy->test.state = MT76_TM_STATE_OFF;
 78		pm->enable = true;
 79	}
 80out:
 81	mutex_unlock(&dev->mt76.mutex);
 82
 83	return ret;
 84}
 85
 86static int
 87mt7925_tm_query(struct mt792x_dev *dev, struct mt7925_tm_cmd *req,
 88		char *evt_resp)
 89{
 90	struct mt7925_rftest_cmd cmd;
 91	char *pcmd = (char *)&cmd;
 92	struct sk_buff *skb = NULL;
 93	int ret = 1;
 94
 95	memset(pcmd, 0, sizeof(*pcmd));
 96	memcpy(pcmd + 4, (char *)&req->c, sizeof(struct uni_cmd_testmode_ctrl));
 97
 98	if (*((uint16_t *)req->padding) == MCU_UNI_CMD_TESTMODE_CTRL)
 99		ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_UNI_QUERY(TESTMODE_CTRL),
100						&cmd, sizeof(cmd), true, &skb);
101	else if (*((uint16_t *)req->padding) == MCU_UNI_CMD_TESTMODE_RX_STAT)
102		ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_UNI_QUERY(TESTMODE_RX_STAT),
103						&cmd, sizeof(cmd), true, &skb);
104
105	if (ret)
106		goto out;
107
108	memcpy((char *)evt_resp, (char *)skb->data + 8, MT7925_EVT_RSP_LEN);
109
110out:
111	dev_kfree_skb(skb);
112
113	return ret;
114}
115
116int mt7925_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
117			void *data, int len)
118{
119	struct nlattr *tb[NUM_MT76_TM_ATTRS];
120	struct mt76_phy *mphy = hw->priv;
121	struct mt792x_phy *phy = mphy->priv;
122	int err;
123
124	if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
125	    !(hw->conf.flags & IEEE80211_CONF_MONITOR))
126		return -ENOTCONN;
127
128	err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
129				   mt76_tm_policy, NULL);
130	if (err)
131		return err;
132
133	if (tb[MT76_TM_ATTR_DRV_DATA]) {
134		struct nlattr *drv_tb[NUM_MT7925_TM_ATTRS], *data;
135		int ret;
136
137		data = tb[MT76_TM_ATTR_DRV_DATA];
138		ret = nla_parse_nested_deprecated(drv_tb,
139						  MT7925_TM_ATTR_MAX,
140						  data, mt7925_tm_policy,
141						  NULL);
142		if (ret)
143			return ret;
144
145		data = drv_tb[MT7925_TM_ATTR_SET];
146		if (data)
147			return mt7925_tm_set(phy->dev, nla_data(data));
148	}
149
150	return -EINVAL;
151}
152
153int mt7925_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
154			 struct netlink_callback *cb, void *data, int len)
155{
156	struct nlattr *tb[NUM_MT76_TM_ATTRS];
157	struct mt76_phy *mphy = hw->priv;
158	struct mt792x_phy *phy = mphy->priv;
159	int err;
160
161	if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
162	    !(hw->conf.flags & IEEE80211_CONF_MONITOR) ||
163	    !mt76_testmode_enabled(mphy))
164		return -ENOTCONN;
165
166	if (cb->args[2]++ > 0)
167		return -ENOENT;
168
169	err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
170				   mt76_tm_policy, NULL);
171	if (err)
172		return err;
173
174	if (tb[MT76_TM_ATTR_DRV_DATA]) {
175		struct nlattr *drv_tb[NUM_MT7925_TM_ATTRS], *data;
176		int ret;
177
178		data = tb[MT76_TM_ATTR_DRV_DATA];
179		ret = nla_parse_nested_deprecated(drv_tb,
180						  MT7925_TM_ATTR_MAX,
181						  data, mt7925_tm_policy,
182						  NULL);
183		if (ret)
184			return ret;
185
186		data = drv_tb[MT7925_TM_ATTR_QUERY];
187		if (data) {
188			char evt_resp[MT7925_EVT_RSP_LEN];
189
190			err = mt7925_tm_query(phy->dev, nla_data(data),
191					      evt_resp);
192			if (err)
193				return err;
194
195			return nla_put(msg, MT7925_TM_ATTR_RSP,
196				       sizeof(evt_resp), evt_resp);
197		}
198	}
199
200	return -EINVAL;
201}