]>
Commit | Line | Data |
---|---|---|
9bb7e0f2 JB |
1 | /* SPDX-License-Identifier: GPL-2.0 */ |
2 | /* | |
73807523 | 3 | * Copyright (C) 2018 - 2021 Intel Corporation |
9bb7e0f2 JB |
4 | */ |
5 | #ifndef __PMSR_H | |
6 | #define __PMSR_H | |
7 | #include <net/cfg80211.h> | |
8 | #include "core.h" | |
9 | #include "nl80211.h" | |
10 | #include "rdev-ops.h" | |
11 | ||
12 | static int pmsr_parse_ftm(struct cfg80211_registered_device *rdev, | |
13 | struct nlattr *ftmreq, | |
14 | struct cfg80211_pmsr_request_peer *out, | |
15 | struct genl_info *info) | |
16 | { | |
17 | const struct cfg80211_pmsr_capabilities *capa = rdev->wiphy.pmsr_capa; | |
18 | struct nlattr *tb[NL80211_PMSR_FTM_REQ_ATTR_MAX + 1]; | |
19 | u32 preamble = NL80211_PREAMBLE_DMG; /* only optional in DMG */ | |
20 | ||
21 | /* validate existing data */ | |
22 | if (!(rdev->wiphy.pmsr_capa->ftm.bandwidths & BIT(out->chandef.width))) { | |
23 | NL_SET_ERR_MSG(info->extack, "FTM: unsupported bandwidth"); | |
24 | return -EINVAL; | |
25 | } | |
26 | ||
27 | /* no validation needed - was already done via nested policy */ | |
8cb08174 JB |
28 | nla_parse_nested_deprecated(tb, NL80211_PMSR_FTM_REQ_ATTR_MAX, ftmreq, |
29 | NULL, NULL); | |
9bb7e0f2 JB |
30 | |
31 | if (tb[NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE]) | |
32 | preamble = nla_get_u32(tb[NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE]); | |
33 | ||
34 | /* set up values - struct is 0-initialized */ | |
35 | out->ftm.requested = true; | |
36 | ||
37 | switch (out->chandef.chan->band) { | |
38 | case NL80211_BAND_60GHZ: | |
39 | /* optional */ | |
40 | break; | |
41 | default: | |
42 | if (!tb[NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE]) { | |
43 | NL_SET_ERR_MSG(info->extack, | |
44 | "FTM: must specify preamble"); | |
45 | return -EINVAL; | |
46 | } | |
47 | } | |
48 | ||
49 | if (!(capa->ftm.preambles & BIT(preamble))) { | |
50 | NL_SET_ERR_MSG_ATTR(info->extack, | |
51 | tb[NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE], | |
52 | "FTM: invalid preamble"); | |
53 | return -EINVAL; | |
54 | } | |
55 | ||
56 | out->ftm.preamble = preamble; | |
57 | ||
58 | out->ftm.burst_period = 0; | |
59 | if (tb[NL80211_PMSR_FTM_REQ_ATTR_BURST_PERIOD]) | |
60 | out->ftm.burst_period = | |
61 | nla_get_u32(tb[NL80211_PMSR_FTM_REQ_ATTR_BURST_PERIOD]); | |
62 | ||
63 | out->ftm.asap = !!tb[NL80211_PMSR_FTM_REQ_ATTR_ASAP]; | |
64 | if (out->ftm.asap && !capa->ftm.asap) { | |
65 | NL_SET_ERR_MSG_ATTR(info->extack, | |
66 | tb[NL80211_PMSR_FTM_REQ_ATTR_ASAP], | |
67 | "FTM: ASAP mode not supported"); | |
68 | return -EINVAL; | |
69 | } | |
70 | ||
71 | if (!out->ftm.asap && !capa->ftm.non_asap) { | |
72 | NL_SET_ERR_MSG(info->extack, | |
73 | "FTM: non-ASAP mode not supported"); | |
74 | return -EINVAL; | |
75 | } | |
76 | ||
77 | out->ftm.num_bursts_exp = 0; | |
78 | if (tb[NL80211_PMSR_FTM_REQ_ATTR_NUM_BURSTS_EXP]) | |
79 | out->ftm.num_bursts_exp = | |
80 | nla_get_u32(tb[NL80211_PMSR_FTM_REQ_ATTR_NUM_BURSTS_EXP]); | |
81 | ||
82 | if (capa->ftm.max_bursts_exponent >= 0 && | |
83 | out->ftm.num_bursts_exp > capa->ftm.max_bursts_exponent) { | |
84 | NL_SET_ERR_MSG_ATTR(info->extack, | |
85 | tb[NL80211_PMSR_FTM_REQ_ATTR_NUM_BURSTS_EXP], | |
86 | "FTM: max NUM_BURSTS_EXP must be set lower than the device limit"); | |
87 | return -EINVAL; | |
88 | } | |
89 | ||
90 | out->ftm.burst_duration = 15; | |
91 | if (tb[NL80211_PMSR_FTM_REQ_ATTR_BURST_DURATION]) | |
92 | out->ftm.burst_duration = | |
93 | nla_get_u32(tb[NL80211_PMSR_FTM_REQ_ATTR_BURST_DURATION]); | |
94 | ||
95 | out->ftm.ftms_per_burst = 0; | |
96 | if (tb[NL80211_PMSR_FTM_REQ_ATTR_FTMS_PER_BURST]) | |
97 | out->ftm.ftms_per_burst = | |
98 | nla_get_u32(tb[NL80211_PMSR_FTM_REQ_ATTR_FTMS_PER_BURST]); | |
99 | ||
100 | if (capa->ftm.max_ftms_per_burst && | |
101 | (out->ftm.ftms_per_burst > capa->ftm.max_ftms_per_burst || | |
102 | out->ftm.ftms_per_burst == 0)) { | |
103 | NL_SET_ERR_MSG_ATTR(info->extack, | |
104 | tb[NL80211_PMSR_FTM_REQ_ATTR_FTMS_PER_BURST], | |
105 | "FTM: FTMs per burst must be set lower than the device limit but non-zero"); | |
106 | return -EINVAL; | |
107 | } | |
108 | ||
109 | out->ftm.ftmr_retries = 3; | |
110 | if (tb[NL80211_PMSR_FTM_REQ_ATTR_NUM_FTMR_RETRIES]) | |
111 | out->ftm.ftmr_retries = | |
112 | nla_get_u32(tb[NL80211_PMSR_FTM_REQ_ATTR_NUM_FTMR_RETRIES]); | |
113 | ||
114 | out->ftm.request_lci = !!tb[NL80211_PMSR_FTM_REQ_ATTR_REQUEST_LCI]; | |
115 | if (out->ftm.request_lci && !capa->ftm.request_lci) { | |
116 | NL_SET_ERR_MSG_ATTR(info->extack, | |
117 | tb[NL80211_PMSR_FTM_REQ_ATTR_REQUEST_LCI], | |
118 | "FTM: LCI request not supported"); | |
119 | } | |
120 | ||
121 | out->ftm.request_civicloc = | |
122 | !!tb[NL80211_PMSR_FTM_REQ_ATTR_REQUEST_CIVICLOC]; | |
123 | if (out->ftm.request_civicloc && !capa->ftm.request_civicloc) { | |
124 | NL_SET_ERR_MSG_ATTR(info->extack, | |
125 | tb[NL80211_PMSR_FTM_REQ_ATTR_REQUEST_CIVICLOC], | |
126 | "FTM: civic location request not supported"); | |
127 | } | |
128 | ||
efb5520d AS |
129 | out->ftm.trigger_based = |
130 | !!tb[NL80211_PMSR_FTM_REQ_ATTR_TRIGGER_BASED]; | |
131 | if (out->ftm.trigger_based && !capa->ftm.trigger_based) { | |
132 | NL_SET_ERR_MSG_ATTR(info->extack, | |
133 | tb[NL80211_PMSR_FTM_REQ_ATTR_TRIGGER_BASED], | |
134 | "FTM: trigger based ranging is not supported"); | |
135 | return -EINVAL; | |
136 | } | |
137 | ||
138 | out->ftm.non_trigger_based = | |
139 | !!tb[NL80211_PMSR_FTM_REQ_ATTR_NON_TRIGGER_BASED]; | |
140 | if (out->ftm.non_trigger_based && !capa->ftm.non_trigger_based) { | |
141 | NL_SET_ERR_MSG_ATTR(info->extack, | |
142 | tb[NL80211_PMSR_FTM_REQ_ATTR_NON_TRIGGER_BASED], | |
143 | "FTM: trigger based ranging is not supported"); | |
144 | return -EINVAL; | |
145 | } | |
146 | ||
147 | if (out->ftm.trigger_based && out->ftm.non_trigger_based) { | |
148 | NL_SET_ERR_MSG(info->extack, | |
149 | "FTM: can't set both trigger based and non trigger based"); | |
150 | return -EINVAL; | |
151 | } | |
152 | ||
153 | if ((out->ftm.trigger_based || out->ftm.non_trigger_based) && | |
154 | out->ftm.preamble != NL80211_PREAMBLE_HE) { | |
155 | NL_SET_ERR_MSG_ATTR(info->extack, | |
156 | tb[NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE], | |
157 | "FTM: non EDCA based ranging must use HE preamble"); | |
158 | return -EINVAL; | |
159 | } | |
160 | ||
73807523 AS |
161 | out->ftm.lmr_feedback = |
162 | !!tb[NL80211_PMSR_FTM_REQ_ATTR_LMR_FEEDBACK]; | |
163 | if (!out->ftm.trigger_based && !out->ftm.non_trigger_based && | |
164 | out->ftm.lmr_feedback) { | |
165 | NL_SET_ERR_MSG_ATTR(info->extack, | |
166 | tb[NL80211_PMSR_FTM_REQ_ATTR_LMR_FEEDBACK], | |
167 | "FTM: LMR feedback set for EDCA based ranging"); | |
168 | return -EINVAL; | |
169 | } | |
170 | ||
dd3e4fc7 AS |
171 | if (tb[NL80211_PMSR_FTM_REQ_ATTR_BSS_COLOR]) { |
172 | if (!out->ftm.non_trigger_based && !out->ftm.trigger_based) { | |
173 | NL_SET_ERR_MSG_ATTR(info->extack, | |
174 | tb[NL80211_PMSR_FTM_REQ_ATTR_BSS_COLOR], | |
175 | "FTM: BSS color set for EDCA based ranging"); | |
176 | return -EINVAL; | |
177 | } | |
178 | ||
179 | out->ftm.bss_color = | |
180 | nla_get_u8(tb[NL80211_PMSR_FTM_REQ_ATTR_BSS_COLOR]); | |
181 | } | |
182 | ||
9bb7e0f2 JB |
183 | return 0; |
184 | } | |
185 | ||
186 | static int pmsr_parse_peer(struct cfg80211_registered_device *rdev, | |
187 | struct nlattr *peer, | |
188 | struct cfg80211_pmsr_request_peer *out, | |
189 | struct genl_info *info) | |
190 | { | |
191 | struct nlattr *tb[NL80211_PMSR_PEER_ATTR_MAX + 1]; | |
192 | struct nlattr *req[NL80211_PMSR_REQ_ATTR_MAX + 1]; | |
193 | struct nlattr *treq; | |
194 | int err, rem; | |
195 | ||
196 | /* no validation needed - was already done via nested policy */ | |
8cb08174 JB |
197 | nla_parse_nested_deprecated(tb, NL80211_PMSR_PEER_ATTR_MAX, peer, |
198 | NULL, NULL); | |
9bb7e0f2 JB |
199 | |
200 | if (!tb[NL80211_PMSR_PEER_ATTR_ADDR] || | |
201 | !tb[NL80211_PMSR_PEER_ATTR_CHAN] || | |
202 | !tb[NL80211_PMSR_PEER_ATTR_REQ]) { | |
203 | NL_SET_ERR_MSG_ATTR(info->extack, peer, | |
204 | "insufficient peer data"); | |
205 | return -EINVAL; | |
206 | } | |
207 | ||
208 | memcpy(out->addr, nla_data(tb[NL80211_PMSR_PEER_ATTR_ADDR]), ETH_ALEN); | |
209 | ||
210 | /* reuse info->attrs */ | |
211 | memset(info->attrs, 0, sizeof(*info->attrs) * (NL80211_ATTR_MAX + 1)); | |
8cb08174 JB |
212 | err = nla_parse_nested_deprecated(info->attrs, NL80211_ATTR_MAX, |
213 | tb[NL80211_PMSR_PEER_ATTR_CHAN], | |
d15da2a2 | 214 | NULL, info->extack); |
9bb7e0f2 JB |
215 | if (err) |
216 | return err; | |
217 | ||
218 | err = nl80211_parse_chandef(rdev, info, &out->chandef); | |
219 | if (err) | |
220 | return err; | |
221 | ||
222 | /* no validation needed - was already done via nested policy */ | |
8cb08174 JB |
223 | nla_parse_nested_deprecated(req, NL80211_PMSR_REQ_ATTR_MAX, |
224 | tb[NL80211_PMSR_PEER_ATTR_REQ], NULL, | |
225 | NULL); | |
9bb7e0f2 JB |
226 | |
227 | if (!req[NL80211_PMSR_REQ_ATTR_DATA]) { | |
228 | NL_SET_ERR_MSG_ATTR(info->extack, | |
229 | tb[NL80211_PMSR_PEER_ATTR_REQ], | |
230 | "missing request type/data"); | |
231 | return -EINVAL; | |
232 | } | |
233 | ||
234 | if (req[NL80211_PMSR_REQ_ATTR_GET_AP_TSF]) | |
235 | out->report_ap_tsf = true; | |
236 | ||
237 | if (out->report_ap_tsf && !rdev->wiphy.pmsr_capa->report_ap_tsf) { | |
238 | NL_SET_ERR_MSG_ATTR(info->extack, | |
239 | req[NL80211_PMSR_REQ_ATTR_GET_AP_TSF], | |
240 | "reporting AP TSF is not supported"); | |
241 | return -EINVAL; | |
242 | } | |
243 | ||
244 | nla_for_each_nested(treq, req[NL80211_PMSR_REQ_ATTR_DATA], rem) { | |
245 | switch (nla_type(treq)) { | |
246 | case NL80211_PMSR_TYPE_FTM: | |
247 | err = pmsr_parse_ftm(rdev, treq, out, info); | |
248 | break; | |
249 | default: | |
250 | NL_SET_ERR_MSG_ATTR(info->extack, treq, | |
251 | "unsupported measurement type"); | |
252 | err = -EINVAL; | |
253 | } | |
254 | } | |
255 | ||
256 | if (err) | |
257 | return err; | |
258 | ||
259 | return 0; | |
260 | } | |
261 | ||
262 | int nl80211_pmsr_start(struct sk_buff *skb, struct genl_info *info) | |
263 | { | |
264 | struct nlattr *reqattr = info->attrs[NL80211_ATTR_PEER_MEASUREMENTS]; | |
265 | struct cfg80211_registered_device *rdev = info->user_ptr[0]; | |
266 | struct wireless_dev *wdev = info->user_ptr[1]; | |
267 | struct cfg80211_pmsr_request *req; | |
268 | struct nlattr *peers, *peer; | |
269 | int count, rem, err, idx; | |
270 | ||
271 | if (!rdev->wiphy.pmsr_capa) | |
272 | return -EOPNOTSUPP; | |
273 | ||
274 | if (!reqattr) | |
275 | return -EINVAL; | |
276 | ||
277 | peers = nla_find(nla_data(reqattr), nla_len(reqattr), | |
278 | NL80211_PMSR_ATTR_PEERS); | |
279 | if (!peers) | |
280 | return -EINVAL; | |
281 | ||
282 | count = 0; | |
283 | nla_for_each_nested(peer, peers, rem) { | |
284 | count++; | |
285 | ||
286 | if (count > rdev->wiphy.pmsr_capa->max_peers) { | |
287 | NL_SET_ERR_MSG_ATTR(info->extack, peer, | |
288 | "Too many peers used"); | |
289 | return -EINVAL; | |
290 | } | |
291 | } | |
292 | ||
293 | req = kzalloc(struct_size(req, peers, count), GFP_KERNEL); | |
294 | if (!req) | |
295 | return -ENOMEM; | |
296 | ||
297 | if (info->attrs[NL80211_ATTR_TIMEOUT]) | |
298 | req->timeout = nla_get_u32(info->attrs[NL80211_ATTR_TIMEOUT]); | |
299 | ||
300 | if (info->attrs[NL80211_ATTR_MAC]) { | |
301 | if (!rdev->wiphy.pmsr_capa->randomize_mac_addr) { | |
302 | NL_SET_ERR_MSG_ATTR(info->extack, | |
303 | info->attrs[NL80211_ATTR_MAC], | |
304 | "device cannot randomize MAC address"); | |
305 | err = -EINVAL; | |
306 | goto out_err; | |
307 | } | |
308 | ||
309 | err = nl80211_parse_random_mac(info->attrs, req->mac_addr, | |
310 | req->mac_addr_mask); | |
311 | if (err) | |
312 | goto out_err; | |
313 | } else { | |
0acd9928 | 314 | memcpy(req->mac_addr, wdev_address(wdev), ETH_ALEN); |
76763741 | 315 | eth_broadcast_addr(req->mac_addr_mask); |
9bb7e0f2 JB |
316 | } |
317 | ||
318 | idx = 0; | |
319 | nla_for_each_nested(peer, peers, rem) { | |
320 | /* NB: this reuses info->attrs, but we no longer need it */ | |
321 | err = pmsr_parse_peer(rdev, peer, &req->peers[idx], info); | |
322 | if (err) | |
323 | goto out_err; | |
324 | idx++; | |
325 | } | |
326 | ||
327 | req->n_peers = count; | |
328 | req->cookie = cfg80211_assign_cookie(rdev); | |
ff1bab1b | 329 | req->nl_portid = info->snd_portid; |
9bb7e0f2 JB |
330 | |
331 | err = rdev_start_pmsr(rdev, wdev, req); | |
332 | if (err) | |
333 | goto out_err; | |
334 | ||
335 | list_add_tail(&req->list, &wdev->pmsr_list); | |
336 | ||
337 | nl_set_extack_cookie_u64(info->extack, req->cookie); | |
338 | return 0; | |
339 | out_err: | |
340 | kfree(req); | |
341 | return err; | |
342 | } | |
343 | ||
344 | void cfg80211_pmsr_complete(struct wireless_dev *wdev, | |
345 | struct cfg80211_pmsr_request *req, | |
346 | gfp_t gfp) | |
347 | { | |
348 | struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); | |
0288e5e1 | 349 | struct cfg80211_pmsr_request *tmp, *prev, *to_free = NULL; |
9bb7e0f2 JB |
350 | struct sk_buff *msg; |
351 | void *hdr; | |
352 | ||
353 | trace_cfg80211_pmsr_complete(wdev->wiphy, wdev, req->cookie); | |
354 | ||
355 | msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp); | |
356 | if (!msg) | |
357 | goto free_request; | |
358 | ||
359 | hdr = nl80211hdr_put(msg, 0, 0, 0, | |
360 | NL80211_CMD_PEER_MEASUREMENT_COMPLETE); | |
361 | if (!hdr) | |
362 | goto free_msg; | |
363 | ||
364 | if (nla_put_u32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx) || | |
365 | nla_put_u64_64bit(msg, NL80211_ATTR_WDEV, wdev_id(wdev), | |
366 | NL80211_ATTR_PAD)) | |
367 | goto free_msg; | |
368 | ||
369 | if (nla_put_u64_64bit(msg, NL80211_ATTR_COOKIE, req->cookie, | |
370 | NL80211_ATTR_PAD)) | |
371 | goto free_msg; | |
372 | ||
373 | genlmsg_end(msg, hdr); | |
374 | genlmsg_unicast(wiphy_net(wdev->wiphy), msg, req->nl_portid); | |
375 | goto free_request; | |
376 | free_msg: | |
377 | nlmsg_free(msg); | |
378 | free_request: | |
379 | spin_lock_bh(&wdev->pmsr_lock); | |
0288e5e1 AS |
380 | /* |
381 | * cfg80211_pmsr_process_abort() may have already moved this request | |
382 | * to the free list, and will free it later. In this case, don't free | |
383 | * it here. | |
384 | */ | |
385 | list_for_each_entry_safe(tmp, prev, &wdev->pmsr_list, list) { | |
386 | if (tmp == req) { | |
387 | list_del(&req->list); | |
388 | to_free = req; | |
389 | break; | |
390 | } | |
391 | } | |
9bb7e0f2 | 392 | spin_unlock_bh(&wdev->pmsr_lock); |
0288e5e1 | 393 | kfree(to_free); |
9bb7e0f2 JB |
394 | } |
395 | EXPORT_SYMBOL_GPL(cfg80211_pmsr_complete); | |
396 | ||
397 | static int nl80211_pmsr_send_ftm_res(struct sk_buff *msg, | |
398 | struct cfg80211_pmsr_result *res) | |
399 | { | |
400 | if (res->status == NL80211_PMSR_STATUS_FAILURE) { | |
401 | if (nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_FAIL_REASON, | |
402 | res->ftm.failure_reason)) | |
403 | goto error; | |
404 | ||
405 | if (res->ftm.failure_reason == | |
406 | NL80211_PMSR_FTM_FAILURE_PEER_BUSY && | |
407 | res->ftm.busy_retry_time && | |
408 | nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_BUSY_RETRY_TIME, | |
409 | res->ftm.busy_retry_time)) | |
410 | goto error; | |
411 | ||
412 | return 0; | |
413 | } | |
414 | ||
415 | #define PUT(tp, attr, val) \ | |
416 | do { \ | |
417 | if (nla_put_##tp(msg, \ | |
418 | NL80211_PMSR_FTM_RESP_ATTR_##attr, \ | |
419 | res->ftm.val)) \ | |
420 | goto error; \ | |
421 | } while (0) | |
422 | ||
423 | #define PUTOPT(tp, attr, val) \ | |
424 | do { \ | |
425 | if (res->ftm.val##_valid) \ | |
426 | PUT(tp, attr, val); \ | |
427 | } while (0) | |
428 | ||
429 | #define PUT_U64(attr, val) \ | |
430 | do { \ | |
431 | if (nla_put_u64_64bit(msg, \ | |
432 | NL80211_PMSR_FTM_RESP_ATTR_##attr,\ | |
433 | res->ftm.val, \ | |
434 | NL80211_PMSR_FTM_RESP_ATTR_PAD)) \ | |
435 | goto error; \ | |
436 | } while (0) | |
437 | ||
438 | #define PUTOPT_U64(attr, val) \ | |
439 | do { \ | |
440 | if (res->ftm.val##_valid) \ | |
441 | PUT_U64(attr, val); \ | |
442 | } while (0) | |
443 | ||
444 | if (res->ftm.burst_index >= 0) | |
445 | PUT(u32, BURST_INDEX, burst_index); | |
446 | PUTOPT(u32, NUM_FTMR_ATTEMPTS, num_ftmr_attempts); | |
447 | PUTOPT(u32, NUM_FTMR_SUCCESSES, num_ftmr_successes); | |
448 | PUT(u8, NUM_BURSTS_EXP, num_bursts_exp); | |
449 | PUT(u8, BURST_DURATION, burst_duration); | |
450 | PUT(u8, FTMS_PER_BURST, ftms_per_burst); | |
451 | PUTOPT(s32, RSSI_AVG, rssi_avg); | |
452 | PUTOPT(s32, RSSI_SPREAD, rssi_spread); | |
453 | if (res->ftm.tx_rate_valid && | |
454 | !nl80211_put_sta_rate(msg, &res->ftm.tx_rate, | |
455 | NL80211_PMSR_FTM_RESP_ATTR_TX_RATE)) | |
456 | goto error; | |
457 | if (res->ftm.rx_rate_valid && | |
458 | !nl80211_put_sta_rate(msg, &res->ftm.rx_rate, | |
459 | NL80211_PMSR_FTM_RESP_ATTR_RX_RATE)) | |
460 | goto error; | |
461 | PUTOPT_U64(RTT_AVG, rtt_avg); | |
462 | PUTOPT_U64(RTT_VARIANCE, rtt_variance); | |
463 | PUTOPT_U64(RTT_SPREAD, rtt_spread); | |
464 | PUTOPT_U64(DIST_AVG, dist_avg); | |
465 | PUTOPT_U64(DIST_VARIANCE, dist_variance); | |
466 | PUTOPT_U64(DIST_SPREAD, dist_spread); | |
467 | if (res->ftm.lci && res->ftm.lci_len && | |
468 | nla_put(msg, NL80211_PMSR_FTM_RESP_ATTR_LCI, | |
469 | res->ftm.lci_len, res->ftm.lci)) | |
470 | goto error; | |
471 | if (res->ftm.civicloc && res->ftm.civicloc_len && | |
472 | nla_put(msg, NL80211_PMSR_FTM_RESP_ATTR_CIVICLOC, | |
473 | res->ftm.civicloc_len, res->ftm.civicloc)) | |
474 | goto error; | |
475 | #undef PUT | |
476 | #undef PUTOPT | |
477 | #undef PUT_U64 | |
478 | #undef PUTOPT_U64 | |
479 | ||
480 | return 0; | |
481 | error: | |
482 | return -ENOSPC; | |
483 | } | |
484 | ||
485 | static int nl80211_pmsr_send_result(struct sk_buff *msg, | |
486 | struct cfg80211_pmsr_result *res) | |
487 | { | |
488 | struct nlattr *pmsr, *peers, *peer, *resp, *data, *typedata; | |
489 | ||
ae0be8de | 490 | pmsr = nla_nest_start_noflag(msg, NL80211_ATTR_PEER_MEASUREMENTS); |
9bb7e0f2 JB |
491 | if (!pmsr) |
492 | goto error; | |
493 | ||
ae0be8de | 494 | peers = nla_nest_start_noflag(msg, NL80211_PMSR_ATTR_PEERS); |
9bb7e0f2 JB |
495 | if (!peers) |
496 | goto error; | |
497 | ||
ae0be8de | 498 | peer = nla_nest_start_noflag(msg, 1); |
9bb7e0f2 JB |
499 | if (!peer) |
500 | goto error; | |
501 | ||
502 | if (nla_put(msg, NL80211_PMSR_PEER_ATTR_ADDR, ETH_ALEN, res->addr)) | |
503 | goto error; | |
504 | ||
ae0be8de | 505 | resp = nla_nest_start_noflag(msg, NL80211_PMSR_PEER_ATTR_RESP); |
9bb7e0f2 JB |
506 | if (!resp) |
507 | goto error; | |
508 | ||
509 | if (nla_put_u32(msg, NL80211_PMSR_RESP_ATTR_STATUS, res->status) || | |
510 | nla_put_u64_64bit(msg, NL80211_PMSR_RESP_ATTR_HOST_TIME, | |
511 | res->host_time, NL80211_PMSR_RESP_ATTR_PAD)) | |
512 | goto error; | |
513 | ||
514 | if (res->ap_tsf_valid && | |
515 | nla_put_u64_64bit(msg, NL80211_PMSR_RESP_ATTR_AP_TSF, | |
b6584202 | 516 | res->ap_tsf, NL80211_PMSR_RESP_ATTR_PAD)) |
9bb7e0f2 JB |
517 | goto error; |
518 | ||
519 | if (res->final && nla_put_flag(msg, NL80211_PMSR_RESP_ATTR_FINAL)) | |
520 | goto error; | |
521 | ||
ae0be8de | 522 | data = nla_nest_start_noflag(msg, NL80211_PMSR_RESP_ATTR_DATA); |
9bb7e0f2 JB |
523 | if (!data) |
524 | goto error; | |
525 | ||
ae0be8de | 526 | typedata = nla_nest_start_noflag(msg, res->type); |
9bb7e0f2 JB |
527 | if (!typedata) |
528 | goto error; | |
529 | ||
530 | switch (res->type) { | |
531 | case NL80211_PMSR_TYPE_FTM: | |
532 | if (nl80211_pmsr_send_ftm_res(msg, res)) | |
533 | goto error; | |
534 | break; | |
535 | default: | |
536 | WARN_ON(1); | |
537 | } | |
538 | ||
539 | nla_nest_end(msg, typedata); | |
540 | nla_nest_end(msg, data); | |
541 | nla_nest_end(msg, resp); | |
542 | nla_nest_end(msg, peer); | |
543 | nla_nest_end(msg, peers); | |
544 | nla_nest_end(msg, pmsr); | |
545 | ||
546 | return 0; | |
547 | error: | |
548 | return -ENOSPC; | |
549 | } | |
550 | ||
551 | void cfg80211_pmsr_report(struct wireless_dev *wdev, | |
552 | struct cfg80211_pmsr_request *req, | |
553 | struct cfg80211_pmsr_result *result, | |
554 | gfp_t gfp) | |
555 | { | |
556 | struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); | |
557 | struct sk_buff *msg; | |
558 | void *hdr; | |
559 | int err; | |
560 | ||
561 | trace_cfg80211_pmsr_report(wdev->wiphy, wdev, req->cookie, | |
562 | result->addr); | |
563 | ||
564 | /* | |
565 | * Currently, only variable items are LCI and civic location, | |
566 | * both of which are reasonably short so we don't need to | |
567 | * worry about them here for the allocation. | |
568 | */ | |
569 | msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp); | |
570 | if (!msg) | |
571 | return; | |
572 | ||
573 | hdr = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_PEER_MEASUREMENT_RESULT); | |
574 | if (!hdr) | |
575 | goto free; | |
576 | ||
577 | if (nla_put_u32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx) || | |
578 | nla_put_u64_64bit(msg, NL80211_ATTR_WDEV, wdev_id(wdev), | |
579 | NL80211_ATTR_PAD)) | |
580 | goto free; | |
581 | ||
582 | if (nla_put_u64_64bit(msg, NL80211_ATTR_COOKIE, req->cookie, | |
583 | NL80211_ATTR_PAD)) | |
584 | goto free; | |
585 | ||
586 | err = nl80211_pmsr_send_result(msg, result); | |
587 | if (err) { | |
588 | pr_err_ratelimited("peer measurement result: message didn't fit!"); | |
589 | goto free; | |
590 | } | |
591 | ||
592 | genlmsg_end(msg, hdr); | |
593 | genlmsg_unicast(wiphy_net(wdev->wiphy), msg, req->nl_portid); | |
594 | return; | |
595 | free: | |
596 | nlmsg_free(msg); | |
597 | } | |
598 | EXPORT_SYMBOL_GPL(cfg80211_pmsr_report); | |
599 | ||
73350424 | 600 | static void cfg80211_pmsr_process_abort(struct wireless_dev *wdev) |
9bb7e0f2 | 601 | { |
9bb7e0f2 JB |
602 | struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); |
603 | struct cfg80211_pmsr_request *req, *tmp; | |
604 | LIST_HEAD(free_list); | |
605 | ||
73350424 JB |
606 | lockdep_assert_held(&wdev->mtx); |
607 | ||
9bb7e0f2 JB |
608 | spin_lock_bh(&wdev->pmsr_lock); |
609 | list_for_each_entry_safe(req, tmp, &wdev->pmsr_list, list) { | |
610 | if (req->nl_portid) | |
611 | continue; | |
612 | list_move_tail(&req->list, &free_list); | |
613 | } | |
614 | spin_unlock_bh(&wdev->pmsr_lock); | |
615 | ||
616 | list_for_each_entry_safe(req, tmp, &free_list, list) { | |
9bb7e0f2 | 617 | rdev_abort_pmsr(rdev, wdev, req); |
9bb7e0f2 JB |
618 | |
619 | kfree(req); | |
620 | } | |
621 | } | |
622 | ||
73350424 JB |
623 | void cfg80211_pmsr_free_wk(struct work_struct *work) |
624 | { | |
625 | struct wireless_dev *wdev = container_of(work, struct wireless_dev, | |
626 | pmsr_free_wk); | |
627 | ||
628 | wdev_lock(wdev); | |
629 | cfg80211_pmsr_process_abort(wdev); | |
630 | wdev_unlock(wdev); | |
631 | } | |
632 | ||
9bb7e0f2 JB |
633 | void cfg80211_pmsr_wdev_down(struct wireless_dev *wdev) |
634 | { | |
635 | struct cfg80211_pmsr_request *req; | |
636 | bool found = false; | |
637 | ||
638 | spin_lock_bh(&wdev->pmsr_lock); | |
639 | list_for_each_entry(req, &wdev->pmsr_list, list) { | |
640 | found = true; | |
641 | req->nl_portid = 0; | |
642 | } | |
643 | spin_unlock_bh(&wdev->pmsr_lock); | |
644 | ||
645 | if (found) | |
73350424 JB |
646 | cfg80211_pmsr_process_abort(wdev); |
647 | ||
9bb7e0f2 JB |
648 | WARN_ON(!list_empty(&wdev->pmsr_list)); |
649 | } | |
650 | ||
651 | void cfg80211_release_pmsr(struct wireless_dev *wdev, u32 portid) | |
652 | { | |
653 | struct cfg80211_pmsr_request *req; | |
654 | ||
655 | spin_lock_bh(&wdev->pmsr_lock); | |
656 | list_for_each_entry(req, &wdev->pmsr_list, list) { | |
657 | if (req->nl_portid == portid) { | |
658 | req->nl_portid = 0; | |
659 | schedule_work(&wdev->pmsr_free_wk); | |
660 | } | |
661 | } | |
662 | spin_unlock_bh(&wdev->pmsr_lock); | |
663 | } | |
664 | ||
665 | #endif /* __PMSR_H */ |