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 }
197