1 // SPDX-License-Identifier: ISC
2
3 #include "mt7925.h"
4 #include "mcu.h"
5
6 #define MT7925_EVT_RSP_LEN 512
7
8 enum 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
19 struct mt7925_tm_cmd {
20 u8 padding[4];
21 struct uni_cmd_testmode_ctrl c;
22 } __packed;
23
24 struct mt7925_tm_evt {
25 u32 param0;
26 u32 param1;
27 } __packed;
28
29 static 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
34 static int
mt7925_tm_set(struct mt792x_dev * dev,struct mt7925_tm_cmd * req)35 mt7925_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 }
80 out:
81 mutex_unlock(&dev->mt76.mutex);
82
83 return ret;
84 }
85
86 static int
mt7925_tm_query(struct mt792x_dev * dev,struct mt7925_tm_cmd * req,char * evt_resp)87 mt7925_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
110 out:
111 dev_kfree_skb(skb);
112
113 return ret;
114 }
115
mt7925_testmode_cmd(struct ieee80211_hw * hw,struct ieee80211_vif * vif,void * data,int len)116 int 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
mt7925_testmode_dump(struct ieee80211_hw * hw,struct sk_buff * msg,struct netlink_callback * cb,void * data,int len)153 int 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 }
202