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  */
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 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 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 
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 
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 
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