1 // SPDX-License-Identifier: BSD-3-Clause-Clear
2 /*
3 * Copyright (c) 2018-2019 The Linux Foundation. All rights reserved.
4 */
5
6 #include "testmode.h"
7 #include <net/netlink.h>
8 #include "debug.h"
9 #include "wmi.h"
10 #include "hw.h"
11 #include "core.h"
12 #include "testmode_i.h"
13
14 static const struct nla_policy ath11k_tm_policy[ATH11K_TM_ATTR_MAX + 1] = {
15 [ATH11K_TM_ATTR_CMD] = { .type = NLA_U32 },
16 [ATH11K_TM_ATTR_DATA] = { .type = NLA_BINARY,
17 .len = ATH11K_TM_DATA_MAX_LEN },
18 [ATH11K_TM_ATTR_WMI_CMDID] = { .type = NLA_U32 },
19 [ATH11K_TM_ATTR_VERSION_MAJOR] = { .type = NLA_U32 },
20 [ATH11K_TM_ATTR_VERSION_MINOR] = { .type = NLA_U32 },
21 };
22
23 /* Returns true if callee consumes the skb and the skb should be discarded.
24 * Returns false if skb is not used. Does not sleep.
25 */
ath11k_tm_event_wmi(struct ath11k * ar,u32 cmd_id,struct sk_buff * skb)26 bool ath11k_tm_event_wmi(struct ath11k *ar, u32 cmd_id, struct sk_buff *skb)
27 {
28 struct sk_buff *nl_skb;
29 bool consumed;
30 int ret;
31
32 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE,
33 "testmode event wmi cmd_id %d skb %pK skb->len %d\n",
34 cmd_id, skb, skb->len);
35
36 ath11k_dbg_dump(ar->ab, ATH11K_DBG_TESTMODE, NULL, "", skb->data, skb->len);
37
38 spin_lock_bh(&ar->data_lock);
39
40 consumed = true;
41
42 nl_skb = cfg80211_testmode_alloc_event_skb(ar->hw->wiphy,
43 2 * sizeof(u32) + skb->len,
44 GFP_ATOMIC);
45 if (!nl_skb) {
46 ath11k_warn(ar->ab,
47 "failed to allocate skb for testmode wmi event\n");
48 goto out;
49 }
50
51 ret = nla_put_u32(nl_skb, ATH11K_TM_ATTR_CMD, ATH11K_TM_CMD_WMI);
52 if (ret) {
53 ath11k_warn(ar->ab,
54 "failed to to put testmode wmi event cmd attribute: %d\n",
55 ret);
56 kfree_skb(nl_skb);
57 goto out;
58 }
59
60 ret = nla_put_u32(nl_skb, ATH11K_TM_ATTR_WMI_CMDID, cmd_id);
61 if (ret) {
62 ath11k_warn(ar->ab,
63 "failed to to put testmode wmi even cmd_id: %d\n",
64 ret);
65 kfree_skb(nl_skb);
66 goto out;
67 }
68
69 ret = nla_put(nl_skb, ATH11K_TM_ATTR_DATA, skb->len, skb->data);
70 if (ret) {
71 ath11k_warn(ar->ab,
72 "failed to copy skb to testmode wmi event: %d\n",
73 ret);
74 kfree_skb(nl_skb);
75 goto out;
76 }
77
78 cfg80211_testmode_event(nl_skb, GFP_ATOMIC);
79
80 out:
81 spin_unlock_bh(&ar->data_lock);
82
83 return consumed;
84 }
85
ath11k_tm_cmd_get_version(struct ath11k * ar,struct nlattr * tb[])86 static int ath11k_tm_cmd_get_version(struct ath11k *ar, struct nlattr *tb[])
87 {
88 struct sk_buff *skb;
89 int ret;
90
91 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE,
92 "testmode cmd get version_major %d version_minor %d\n",
93 ATH11K_TESTMODE_VERSION_MAJOR,
94 ATH11K_TESTMODE_VERSION_MINOR);
95
96 skb = cfg80211_testmode_alloc_reply_skb(ar->hw->wiphy,
97 nla_total_size(sizeof(u32)));
98 if (!skb)
99 return -ENOMEM;
100
101 ret = nla_put_u32(skb, ATH11K_TM_ATTR_VERSION_MAJOR,
102 ATH11K_TESTMODE_VERSION_MAJOR);
103 if (ret) {
104 kfree_skb(skb);
105 return ret;
106 }
107
108 ret = nla_put_u32(skb, ATH11K_TM_ATTR_VERSION_MINOR,
109 ATH11K_TESTMODE_VERSION_MINOR);
110 if (ret) {
111 kfree_skb(skb);
112 return ret;
113 }
114
115 return cfg80211_testmode_reply(skb);
116 }
117
ath11k_tm_cmd_wmi(struct ath11k * ar,struct nlattr * tb[])118 static int ath11k_tm_cmd_wmi(struct ath11k *ar, struct nlattr *tb[])
119 {
120 struct ath11k_pdev_wmi *wmi = ar->wmi;
121 struct sk_buff *skb;
122 u32 cmd_id, buf_len;
123 int ret;
124 void *buf;
125
126 mutex_lock(&ar->conf_mutex);
127
128 if (ar->state != ATH11K_STATE_ON) {
129 ret = -ENETDOWN;
130 goto out;
131 }
132
133 if (!tb[ATH11K_TM_ATTR_DATA]) {
134 ret = -EINVAL;
135 goto out;
136 }
137
138 if (!tb[ATH11K_TM_ATTR_WMI_CMDID]) {
139 ret = -EINVAL;
140 goto out;
141 }
142
143 buf = nla_data(tb[ATH11K_TM_ATTR_DATA]);
144 buf_len = nla_len(tb[ATH11K_TM_ATTR_DATA]);
145 cmd_id = nla_get_u32(tb[ATH11K_TM_ATTR_WMI_CMDID]);
146
147 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE,
148 "testmode cmd wmi cmd_id %d buf %pK buf_len %d\n",
149 cmd_id, buf, buf_len);
150
151 ath11k_dbg_dump(ar->ab, ATH11K_DBG_TESTMODE, NULL, "", buf, buf_len);
152
153 skb = ath11k_wmi_alloc_skb(wmi->wmi_ab, buf_len);
154 if (!skb) {
155 ret = -ENOMEM;
156 goto out;
157 }
158
159 memcpy(skb->data, buf, buf_len);
160
161 ret = ath11k_wmi_cmd_send(wmi, skb, cmd_id);
162 if (ret) {
163 dev_kfree_skb(skb);
164 ath11k_warn(ar->ab, "failed to transmit wmi command (testmode): %d\n",
165 ret);
166 goto out;
167 }
168
169 ret = 0;
170
171 out:
172 mutex_unlock(&ar->conf_mutex);
173 return ret;
174 }
175
ath11k_tm_cmd(struct ieee80211_hw * hw,struct ieee80211_vif * vif,void * data,int len)176 int ath11k_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
177 void *data, int len)
178 {
179 struct ath11k *ar = hw->priv;
180 struct nlattr *tb[ATH11K_TM_ATTR_MAX + 1];
181 int ret;
182
183 ret = nla_parse(tb, ATH11K_TM_ATTR_MAX, data, len, ath11k_tm_policy,
184 NULL);
185 if (ret)
186 return ret;
187
188 if (!tb[ATH11K_TM_ATTR_CMD])
189 return -EINVAL;
190
191 switch (nla_get_u32(tb[ATH11K_TM_ATTR_CMD])) {
192 case ATH11K_TM_CMD_GET_VERSION:
193 return ath11k_tm_cmd_get_version(ar, tb);
194 case ATH11K_TM_CMD_WMI:
195 return ath11k_tm_cmd_wmi(ar, tb);
196 default:
197 return -EOPNOTSUPP;
198 }
199 }
200