Commit | Line | Data |
---|---|---|
9bb7e0f2 JB |
1 | /* SPDX-License-Identifier: GPL-2.0 */ |
2 | /* | |
73807523 | 3 | * Copyright (C) 2018 - 2021 Intel Corporation |
9bb7e0f2 | 4 | */ |
9bb7e0f2 JB |
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 */ | |
8cb08174 JB |
26 | nla_parse_nested_deprecated(tb, NL80211_PMSR_FTM_REQ_ATTR_MAX, ftmreq, |
27 | NULL, NULL); | |
9bb7e0f2 JB |
28 | |
29 | if (tb[NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE]) | |
30 | preamble = nla_get_u32(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(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(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(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(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(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 | ||
efb5520d AS |
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 | ||
73807523 AS |
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 | ||
dd3e4fc7 AS |
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(tb[NL80211_PMSR_FTM_REQ_ATTR_BSS_COLOR]); | |
179 | } | |
180 | ||
9bb7e0f2 JB |
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 */ | |
8cb08174 JB |
195 | nla_parse_nested_deprecated(tb, NL80211_PMSR_PEER_ATTR_MAX, peer, |
196 | NULL, NULL); | |
9bb7e0f2 JB |
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)); | |
8cb08174 JB |
210 | err = nla_parse_nested_deprecated(info->attrs, NL80211_ATTR_MAX, |
211 | tb[NL80211_PMSR_PEER_ATTR_CHAN], | |
d15da2a2 | 212 | NULL, info->extack); |
9bb7e0f2 JB |
213 | if (err) |
214 | return err; | |
215 | ||
216 | err = nl80211_parse_chandef(rdev, info, &out->chandef); | |
217 | if (err) | |
218 | return err; | |
219 | ||
220 | /* no validation needed - was already done via nested policy */ | |
8cb08174 JB |
221 | nla_parse_nested_deprecated(req, NL80211_PMSR_REQ_ATTR_MAX, |
222 | tb[NL80211_PMSR_PEER_ATTR_REQ], NULL, | |
223 | NULL); | |
9bb7e0f2 JB |
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(treq)) { | |
244 | case NL80211_PMSR_TYPE_FTM: | |
245 | err = pmsr_parse_ftm(rdev, 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(nla_data(reqattr), nla_len(reqattr), | |
276 | 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 | ||
295 | if (info->attrs[NL80211_ATTR_TIMEOUT]) | |
296 | req->timeout = nla_get_u32(info->attrs[NL80211_ATTR_TIMEOUT]); | |
297 | ||
298 | if (info->attrs[NL80211_ATTR_MAC]) { | |
299 | if (!rdev->wiphy.pmsr_capa->randomize_mac_addr) { | |
300 | NL_SET_ERR_MSG_ATTR(info->extack, | |
301 | info->attrs[NL80211_ATTR_MAC], | |
302 | "device cannot randomize MAC address"); | |
303 | err = -EINVAL; | |
304 | goto out_err; | |
305 | } | |
306 | ||
307 | err = nl80211_parse_random_mac(info->attrs, req->mac_addr, | |
308 | req->mac_addr_mask); | |
309 | if (err) | |
310 | goto out_err; | |
311 | } else { | |
0acd9928 | 312 | memcpy(req->mac_addr, wdev_address(wdev), ETH_ALEN); |
76763741 | 313 | eth_broadcast_addr(req->mac_addr_mask); |
9bb7e0f2 JB |
314 | } |
315 | ||
316 | idx = 0; | |
317 | nla_for_each_nested(peer, peers, rem) { | |
318 | /* NB: this reuses info->attrs, but we no longer need it */ | |
319 | err = pmsr_parse_peer(rdev, peer, &req->peers[idx], info); | |
320 | if (err) | |
321 | goto out_err; | |
322 | idx++; | |
323 | } | |
324 | ||
325 | req->n_peers = count; | |
326 | req->cookie = cfg80211_assign_cookie(rdev); | |
ff1bab1b | 327 | req->nl_portid = info->snd_portid; |
9bb7e0f2 JB |
328 | |
329 | err = rdev_start_pmsr(rdev, wdev, req); | |
330 | if (err) | |
331 | goto out_err; | |
332 | ||
333 | list_add_tail(&req->list, &wdev->pmsr_list); | |
334 | ||
335 | nl_set_extack_cookie_u64(info->extack, req->cookie); | |
336 | return 0; | |
337 | out_err: | |
338 | kfree(req); | |
339 | return err; | |
340 | } | |
341 | ||
342 | void cfg80211_pmsr_complete(struct wireless_dev *wdev, | |
343 | struct cfg80211_pmsr_request *req, | |
344 | gfp_t gfp) | |
345 | { | |
346 | struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); | |
0288e5e1 | 347 | struct cfg80211_pmsr_request *tmp, *prev, *to_free = NULL; |
9bb7e0f2 JB |
348 | struct sk_buff *msg; |
349 | void *hdr; | |
350 | ||
351 | trace_cfg80211_pmsr_complete(wdev->wiphy, wdev, req->cookie); | |
352 | ||
353 | msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp); | |
354 | if (!msg) | |
355 | goto free_request; | |
356 | ||
357 | hdr = nl80211hdr_put(msg, 0, 0, 0, | |
358 | NL80211_CMD_PEER_MEASUREMENT_COMPLETE); | |
359 | if (!hdr) | |
360 | goto free_msg; | |
361 | ||
362 | if (nla_put_u32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx) || | |
363 | nla_put_u64_64bit(msg, NL80211_ATTR_WDEV, wdev_id(wdev), | |
364 | NL80211_ATTR_PAD)) | |
365 | goto free_msg; | |
366 | ||
367 | if (nla_put_u64_64bit(msg, NL80211_ATTR_COOKIE, req->cookie, | |
368 | NL80211_ATTR_PAD)) | |
369 | goto free_msg; | |
370 | ||
371 | genlmsg_end(msg, hdr); | |
372 | genlmsg_unicast(wiphy_net(wdev->wiphy), msg, req->nl_portid); | |
373 | goto free_request; | |
374 | free_msg: | |
375 | nlmsg_free(msg); | |
376 | free_request: | |
377 | spin_lock_bh(&wdev->pmsr_lock); | |
0288e5e1 AS |
378 | /* |
379 | * cfg80211_pmsr_process_abort() may have already moved this request | |
380 | * to the free list, and will free it later. In this case, don't free | |
381 | * it here. | |
382 | */ | |
383 | list_for_each_entry_safe(tmp, prev, &wdev->pmsr_list, list) { | |
384 | if (tmp == req) { | |
385 | list_del(&req->list); | |
386 | to_free = req; | |
387 | break; | |
388 | } | |
389 | } | |
9bb7e0f2 | 390 | spin_unlock_bh(&wdev->pmsr_lock); |
0288e5e1 | 391 | kfree(to_free); |
9bb7e0f2 JB |
392 | } |
393 | EXPORT_SYMBOL_GPL(cfg80211_pmsr_complete); | |
394 | ||
395 | static int nl80211_pmsr_send_ftm_res(struct sk_buff *msg, | |
396 | struct cfg80211_pmsr_result *res) | |
397 | { | |
398 | if (res->status == NL80211_PMSR_STATUS_FAILURE) { | |
399 | if (nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_FAIL_REASON, | |
400 | res->ftm.failure_reason)) | |
401 | goto error; | |
402 | ||
403 | if (res->ftm.failure_reason == | |
404 | NL80211_PMSR_FTM_FAILURE_PEER_BUSY && | |
405 | res->ftm.busy_retry_time && | |
406 | nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_BUSY_RETRY_TIME, | |
407 | res->ftm.busy_retry_time)) | |
408 | goto error; | |
409 | ||
410 | return 0; | |
411 | } | |
412 | ||
413 | #define PUT(tp, attr, val) \ | |
414 | do { \ | |
415 | if (nla_put_##tp(msg, \ | |
416 | NL80211_PMSR_FTM_RESP_ATTR_##attr, \ | |
417 | res->ftm.val)) \ | |
418 | goto error; \ | |
419 | } while (0) | |
420 | ||
421 | #define PUTOPT(tp, attr, val) \ | |
422 | do { \ | |
423 | if (res->ftm.val##_valid) \ | |
424 | PUT(tp, attr, val); \ | |
425 | } while (0) | |
426 | ||
427 | #define PUT_U64(attr, val) \ | |
428 | do { \ | |
429 | if (nla_put_u64_64bit(msg, \ | |
430 | NL80211_PMSR_FTM_RESP_ATTR_##attr,\ | |
431 | res->ftm.val, \ | |
432 | NL80211_PMSR_FTM_RESP_ATTR_PAD)) \ | |
433 | goto error; \ | |
434 | } while (0) | |
435 | ||
436 | #define PUTOPT_U64(attr, val) \ | |
437 | do { \ | |
438 | if (res->ftm.val##_valid) \ | |
439 | PUT_U64(attr, val); \ | |
440 | } while (0) | |
441 | ||
442 | if (res->ftm.burst_index >= 0) | |
443 | PUT(u32, BURST_INDEX, burst_index); | |
444 | PUTOPT(u32, NUM_FTMR_ATTEMPTS, num_ftmr_attempts); | |
445 | PUTOPT(u32, NUM_FTMR_SUCCESSES, num_ftmr_successes); | |
446 | PUT(u8, NUM_BURSTS_EXP, num_bursts_exp); | |
447 | PUT(u8, BURST_DURATION, burst_duration); | |
448 | PUT(u8, FTMS_PER_BURST, ftms_per_burst); | |
449 | PUTOPT(s32, RSSI_AVG, rssi_avg); | |
450 | PUTOPT(s32, RSSI_SPREAD, rssi_spread); | |
451 | if (res->ftm.tx_rate_valid && | |
452 | !nl80211_put_sta_rate(msg, &res->ftm.tx_rate, | |
453 | NL80211_PMSR_FTM_RESP_ATTR_TX_RATE)) | |
454 | goto error; | |
455 | if (res->ftm.rx_rate_valid && | |
456 | !nl80211_put_sta_rate(msg, &res->ftm.rx_rate, | |
457 | NL80211_PMSR_FTM_RESP_ATTR_RX_RATE)) | |
458 | goto error; | |
459 | PUTOPT_U64(RTT_AVG, rtt_avg); | |
460 | PUTOPT_U64(RTT_VARIANCE, rtt_variance); | |
461 | PUTOPT_U64(RTT_SPREAD, rtt_spread); | |
462 | PUTOPT_U64(DIST_AVG, dist_avg); | |
463 | PUTOPT_U64(DIST_VARIANCE, dist_variance); | |
464 | PUTOPT_U64(DIST_SPREAD, dist_spread); | |
465 | if (res->ftm.lci && res->ftm.lci_len && | |
466 | nla_put(msg, NL80211_PMSR_FTM_RESP_ATTR_LCI, | |
467 | res->ftm.lci_len, res->ftm.lci)) | |
468 | goto error; | |
469 | if (res->ftm.civicloc && res->ftm.civicloc_len && | |
470 | nla_put(msg, NL80211_PMSR_FTM_RESP_ATTR_CIVICLOC, | |
471 | res->ftm.civicloc_len, res->ftm.civicloc)) | |
472 | goto error; | |
473 | #undef PUT | |
474 | #undef PUTOPT | |
475 | #undef PUT_U64 | |
476 | #undef PUTOPT_U64 | |
477 | ||
478 | return 0; | |
479 | error: | |
480 | return -ENOSPC; | |
481 | } | |
482 | ||
483 | static int nl80211_pmsr_send_result(struct sk_buff *msg, | |
484 | struct cfg80211_pmsr_result *res) | |
485 | { | |
486 | struct nlattr *pmsr, *peers, *peer, *resp, *data, *typedata; | |
487 | ||
ae0be8de | 488 | pmsr = nla_nest_start_noflag(msg, NL80211_ATTR_PEER_MEASUREMENTS); |
9bb7e0f2 JB |
489 | if (!pmsr) |
490 | goto error; | |
491 | ||
ae0be8de | 492 | peers = nla_nest_start_noflag(msg, NL80211_PMSR_ATTR_PEERS); |
9bb7e0f2 JB |
493 | if (!peers) |
494 | goto error; | |
495 | ||
ae0be8de | 496 | peer = nla_nest_start_noflag(msg, 1); |
9bb7e0f2 JB |
497 | if (!peer) |
498 | goto error; | |
499 | ||
500 | if (nla_put(msg, NL80211_PMSR_PEER_ATTR_ADDR, ETH_ALEN, res->addr)) | |
501 | goto error; | |
502 | ||
ae0be8de | 503 | resp = nla_nest_start_noflag(msg, NL80211_PMSR_PEER_ATTR_RESP); |
9bb7e0f2 JB |
504 | if (!resp) |
505 | goto error; | |
506 | ||
507 | if (nla_put_u32(msg, NL80211_PMSR_RESP_ATTR_STATUS, res->status) || | |
508 | nla_put_u64_64bit(msg, NL80211_PMSR_RESP_ATTR_HOST_TIME, | |
509 | res->host_time, NL80211_PMSR_RESP_ATTR_PAD)) | |
510 | goto error; | |
511 | ||
512 | if (res->ap_tsf_valid && | |
513 | nla_put_u64_64bit(msg, NL80211_PMSR_RESP_ATTR_AP_TSF, | |
b6584202 | 514 | res->ap_tsf, NL80211_PMSR_RESP_ATTR_PAD)) |
9bb7e0f2 JB |
515 | goto error; |
516 | ||
517 | if (res->final && nla_put_flag(msg, NL80211_PMSR_RESP_ATTR_FINAL)) | |
518 | goto error; | |
519 | ||
ae0be8de | 520 | data = nla_nest_start_noflag(msg, NL80211_PMSR_RESP_ATTR_DATA); |
9bb7e0f2 JB |
521 | if (!data) |
522 | goto error; | |
523 | ||
ae0be8de | 524 | typedata = nla_nest_start_noflag(msg, res->type); |
9bb7e0f2 JB |
525 | if (!typedata) |
526 | goto error; | |
527 | ||
528 | switch (res->type) { | |
529 | case NL80211_PMSR_TYPE_FTM: | |
530 | if (nl80211_pmsr_send_ftm_res(msg, res)) | |
531 | goto error; | |
532 | break; | |
533 | default: | |
534 | WARN_ON(1); | |
535 | } | |
536 | ||
537 | nla_nest_end(msg, typedata); | |
538 | nla_nest_end(msg, data); | |
539 | nla_nest_end(msg, resp); | |
540 | nla_nest_end(msg, peer); | |
541 | nla_nest_end(msg, peers); | |
542 | nla_nest_end(msg, pmsr); | |
543 | ||
544 | return 0; | |
545 | error: | |
546 | return -ENOSPC; | |
547 | } | |
548 | ||
549 | void cfg80211_pmsr_report(struct wireless_dev *wdev, | |
550 | struct cfg80211_pmsr_request *req, | |
551 | struct cfg80211_pmsr_result *result, | |
552 | gfp_t gfp) | |
553 | { | |
554 | struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); | |
555 | struct sk_buff *msg; | |
556 | void *hdr; | |
557 | int err; | |
558 | ||
559 | trace_cfg80211_pmsr_report(wdev->wiphy, wdev, req->cookie, | |
560 | result->addr); | |
561 | ||
562 | /* | |
563 | * Currently, only variable items are LCI and civic location, | |
564 | * both of which are reasonably short so we don't need to | |
565 | * worry about them here for the allocation. | |
566 | */ | |
567 | msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp); | |
568 | if (!msg) | |
569 | return; | |
570 | ||
571 | hdr = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_PEER_MEASUREMENT_RESULT); | |
572 | if (!hdr) | |
573 | goto free; | |
574 | ||
575 | if (nla_put_u32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx) || | |
576 | nla_put_u64_64bit(msg, NL80211_ATTR_WDEV, wdev_id(wdev), | |
577 | NL80211_ATTR_PAD)) | |
578 | goto free; | |
579 | ||
580 | if (nla_put_u64_64bit(msg, NL80211_ATTR_COOKIE, req->cookie, | |
581 | NL80211_ATTR_PAD)) | |
582 | goto free; | |
583 | ||
584 | err = nl80211_pmsr_send_result(msg, result); | |
585 | if (err) { | |
586 | pr_err_ratelimited("peer measurement result: message didn't fit!"); | |
587 | goto free; | |
588 | } | |
589 | ||
590 | genlmsg_end(msg, hdr); | |
591 | genlmsg_unicast(wiphy_net(wdev->wiphy), msg, req->nl_portid); | |
592 | return; | |
593 | free: | |
594 | nlmsg_free(msg); | |
595 | } | |
596 | EXPORT_SYMBOL_GPL(cfg80211_pmsr_report); | |
597 | ||
73350424 | 598 | static void cfg80211_pmsr_process_abort(struct wireless_dev *wdev) |
9bb7e0f2 | 599 | { |
9bb7e0f2 JB |
600 | struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); |
601 | struct cfg80211_pmsr_request *req, *tmp; | |
602 | LIST_HEAD(free_list); | |
603 | ||
73350424 JB |
604 | lockdep_assert_held(&wdev->mtx); |
605 | ||
9bb7e0f2 JB |
606 | spin_lock_bh(&wdev->pmsr_lock); |
607 | list_for_each_entry_safe(req, tmp, &wdev->pmsr_list, list) { | |
608 | if (req->nl_portid) | |
609 | continue; | |
610 | list_move_tail(&req->list, &free_list); | |
611 | } | |
612 | spin_unlock_bh(&wdev->pmsr_lock); | |
613 | ||
614 | list_for_each_entry_safe(req, tmp, &free_list, list) { | |
9bb7e0f2 | 615 | rdev_abort_pmsr(rdev, wdev, req); |
9bb7e0f2 JB |
616 | |
617 | kfree(req); | |
618 | } | |
619 | } | |
620 | ||
73350424 JB |
621 | void cfg80211_pmsr_free_wk(struct work_struct *work) |
622 | { | |
623 | struct wireless_dev *wdev = container_of(work, struct wireless_dev, | |
624 | pmsr_free_wk); | |
625 | ||
626 | wdev_lock(wdev); | |
627 | cfg80211_pmsr_process_abort(wdev); | |
628 | wdev_unlock(wdev); | |
629 | } | |
630 | ||
9bb7e0f2 JB |
631 | void cfg80211_pmsr_wdev_down(struct wireless_dev *wdev) |
632 | { | |
633 | struct cfg80211_pmsr_request *req; | |
634 | bool found = false; | |
635 | ||
636 | spin_lock_bh(&wdev->pmsr_lock); | |
637 | list_for_each_entry(req, &wdev->pmsr_list, list) { | |
638 | found = true; | |
639 | req->nl_portid = 0; | |
640 | } | |
641 | spin_unlock_bh(&wdev->pmsr_lock); | |
642 | ||
643 | if (found) | |
73350424 JB |
644 | cfg80211_pmsr_process_abort(wdev); |
645 | ||
9bb7e0f2 JB |
646 | WARN_ON(!list_empty(&wdev->pmsr_list)); |
647 | } | |
648 | ||
649 | void cfg80211_release_pmsr(struct wireless_dev *wdev, u32 portid) | |
650 | { | |
651 | struct cfg80211_pmsr_request *req; | |
652 | ||
653 | spin_lock_bh(&wdev->pmsr_lock); | |
654 | list_for_each_entry(req, &wdev->pmsr_list, list) { | |
655 | if (req->nl_portid == portid) { | |
656 | req->nl_portid = 0; | |
657 | schedule_work(&wdev->pmsr_free_wk); | |
658 | } | |
659 | } | |
660 | spin_unlock_bh(&wdev->pmsr_lock); | |
661 | } |