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