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
mt7921_tm_set(struct mt7921_dev * dev,struct mt7921_tm_cmd * req)34 mt7921_tm_set(struct mt7921_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 __mt7921_mcu_drv_pmctrl(dev);
61
62 mt76_wr(dev, MT_WF_RFCR(0), dev->mt76.rxfilter);
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_CE_CMD(TEST_CTRL), &cmd,
70 sizeof(cmd), false);
71 if (ret)
72 goto out;
73
74 if (normal) {
75 /* Switch back to the normal world */
76 phy->test.state = MT76_TM_STATE_OFF;
77 pm->enable = true;
78 }
79 out:
80 mutex_unlock(&dev->mt76.mutex);
81
82 return ret;
83 }
84
85 static int
mt7921_tm_query(struct mt7921_dev * dev,struct mt7921_tm_cmd * req,struct mt7921_tm_evt * evt_resp)86 mt7921_tm_query(struct mt7921_dev *dev, struct mt7921_tm_cmd *req,
87 struct mt7921_tm_evt *evt_resp)
88 {
89 struct mt7921_rftest_cmd cmd = {
90 .action = req->action,
91 .param0 = cpu_to_le32(req->param0),
92 .param1 = cpu_to_le32(req->param1),
93 };
94 struct mt7921_rftest_evt *evt;
95 struct sk_buff *skb;
96 int ret;
97
98 ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL),
99 &cmd, sizeof(cmd), true, &skb);
100 if (ret)
101 goto out;
102
103 evt = (struct mt7921_rftest_evt *)skb->data;
104 evt_resp->param0 = le32_to_cpu(evt->param0);
105 evt_resp->param1 = le32_to_cpu(evt->param1);
106 out:
107 dev_kfree_skb(skb);
108
109 return ret;
110 }
111
mt7921_testmode_cmd(struct ieee80211_hw * hw,struct ieee80211_vif * vif,void * data,int len)112 int mt7921_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
113 void *data, int len)
114 {
115 struct nlattr *tb[NUM_MT76_TM_ATTRS];
116 struct mt76_phy *mphy = hw->priv;
117 struct mt7921_phy *phy = mphy->priv;
118 int err;
119
120 if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
121 !(hw->conf.flags & IEEE80211_CONF_MONITOR))
122 return -ENOTCONN;
123
124 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
125 mt76_tm_policy, NULL);
126 if (err)
127 return err;
128
129 if (tb[MT76_TM_ATTR_DRV_DATA]) {
130 struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
131 int ret;
132
133 data = tb[MT76_TM_ATTR_DRV_DATA];
134 ret = nla_parse_nested_deprecated(drv_tb,
135 MT7921_TM_ATTR_MAX,
136 data, mt7921_tm_policy,
137 NULL);
138 if (ret)
139 return ret;
140
141 data = drv_tb[MT7921_TM_ATTR_SET];
142 if (data)
143 return mt7921_tm_set(phy->dev, nla_data(data));
144 }
145
146 return -EINVAL;
147 }
148
mt7921_testmode_dump(struct ieee80211_hw * hw,struct sk_buff * msg,struct netlink_callback * cb,void * data,int len)149 int mt7921_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
150 struct netlink_callback *cb, void *data, int len)
151 {
152 struct nlattr *tb[NUM_MT76_TM_ATTRS];
153 struct mt76_phy *mphy = hw->priv;
154 struct mt7921_phy *phy = mphy->priv;
155 int err;
156
157 if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
158 !(hw->conf.flags & IEEE80211_CONF_MONITOR) ||
159 !mt76_testmode_enabled(mphy))
160 return -ENOTCONN;
161
162 if (cb->args[2]++ > 0)
163 return -ENOENT;
164
165 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
166 mt76_tm_policy, NULL);
167 if (err)
168 return err;
169
170 if (tb[MT76_TM_ATTR_DRV_DATA]) {
171 struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
172 int ret;
173
174 data = tb[MT76_TM_ATTR_DRV_DATA];
175 ret = nla_parse_nested_deprecated(drv_tb,
176 MT7921_TM_ATTR_MAX,
177 data, mt7921_tm_policy,
178 NULL);
179 if (ret)
180 return ret;
181
182 data = drv_tb[MT7921_TM_ATTR_QUERY];
183 if (data) {
184 struct mt7921_tm_evt evt_resp;
185
186 err = mt7921_tm_query(phy->dev, nla_data(data),
187 &evt_resp);
188 if (err)
189 return err;
190
191 return nla_put(msg, MT7921_TM_ATTR_RSP,
192 sizeof(evt_resp), &evt_resp);
193 }
194 }
195
196 return -EINVAL;
197 }
198
