]>
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 | ||
9bb7e0f2 JB |
171 | return 0; |
172 | } | |
173 | ||
174 | static int pmsr_parse_peer(struct cfg80211_registered_device *rdev, | |
175 | struct nlattr *peer, | |
176 | struct cfg80211_pmsr_request_peer *out, | |
177 | struct genl_info *info) | |
178 | { | |
179 | struct nlattr *tb[NL80211_PMSR_PEER_ATTR_MAX + 1]; | |
180 | struct nlattr *req[NL80211_PMSR_REQ_ATTR_MAX + 1]; | |
181 | struct nlattr *treq; | |
182 | int err, rem; | |
183 | ||
184 | /* no validation needed - was already done via nested policy */ | |
8cb08174 JB |
185 | nla_parse_nested_deprecated(tb, NL80211_PMSR_PEER_ATTR_MAX, peer, |
186 | NULL, NULL); | |
9bb7e0f2 JB |
187 | |
188 | if (!tb[NL80211_PMSR_PEER_ATTR_ADDR] || | |
189 | !tb[NL80211_PMSR_PEER_ATTR_CHAN] || | |
190 | !tb[NL80211_PMSR_PEER_ATTR_REQ]) { | |
191 | NL_SET_ERR_MSG_ATTR(info->extack, peer, | |
192 | "insufficient peer data"); | |
193 | return -EINVAL; | |
194 | } | |
195 | ||
196 | memcpy(out->addr, nla_data(tb[NL80211_PMSR_PEER_ATTR_ADDR]), ETH_ALEN); | |
197 | ||
198 | /* reuse info->attrs */ | |
199 | memset(info->attrs, 0, sizeof(*info->attrs) * (NL80211_ATTR_MAX + 1)); | |
8cb08174 JB |
200 | err = nla_parse_nested_deprecated(info->attrs, NL80211_ATTR_MAX, |
201 | tb[NL80211_PMSR_PEER_ATTR_CHAN], | |
d15da2a2 | 202 | NULL, info->extack); |
9bb7e0f2 JB |
203 | if (err) |
204 | return err; | |
205 | ||
206 | err = nl80211_parse_chandef(rdev, info, &out->chandef); | |
207 | if (err) | |
208 | return err; | |
209 | ||
210 | /* no validation needed - was already done via nested policy */ | |
8cb08174 JB |
211 | nla_parse_nested_deprecated(req, NL80211_PMSR_REQ_ATTR_MAX, |
212 | tb[NL80211_PMSR_PEER_ATTR_REQ], NULL, | |
213 | NULL); | |
9bb7e0f2 JB |
214 | |
215 | if (!req[NL80211_PMSR_REQ_ATTR_DATA]) { | |
216 | NL_SET_ERR_MSG_ATTR(info->extack, | |
217 | tb[NL80211_PMSR_PEER_ATTR_REQ], | |
218 | "missing request type/data"); | |
219 | return -EINVAL; | |
220 | } | |
221 | ||
222 | if (req[NL80211_PMSR_REQ_ATTR_GET_AP_TSF]) | |
223 | out->report_ap_tsf = true; | |
224 | ||
225 | if (out->report_ap_tsf && !rdev->wiphy.pmsr_capa->report_ap_tsf) { | |
226 | NL_SET_ERR_MSG_ATTR(info->extack, | |
227 | req[NL80211_PMSR_REQ_ATTR_GET_AP_TSF], | |
228 | "reporting AP TSF is not supported"); | |
229 | return -EINVAL; | |
230 | } | |
231 | ||
232 | nla_for_each_nested(treq, req[NL80211_PMSR_REQ_ATTR_DATA], rem) { | |
233 | switch (nla_type(treq)) { | |
234 | case NL80211_PMSR_TYPE_FTM: | |
235 | err = pmsr_parse_ftm(rdev, treq, out, info); | |
236 | break; | |
237 | default: | |
238 | NL_SET_ERR_MSG_ATTR(info->extack, treq, | |
239 | "unsupported measurement type"); | |
240 | err = -EINVAL; | |
241 | } | |
242 | } | |
243 | ||
244 | if (err) | |
245 | return err; | |
246 | ||
247 | return 0; | |
248 | } | |
249 | ||
250 | int nl80211_pmsr_start(struct sk_buff *skb, struct genl_info *info) | |
251 | { | |
252 | struct nlattr *reqattr = info->attrs[NL80211_ATTR_PEER_MEASUREMENTS]; | |
253 | struct cfg80211_registered_device *rdev = info->user_ptr[0]; | |
254 | struct wireless_dev *wdev = info->user_ptr[1]; | |
255 | struct cfg80211_pmsr_request *req; | |
256 | struct nlattr *peers, *peer; | |
257 | int count, rem, err, idx; | |
258 | ||
259 | if (!rdev->wiphy.pmsr_capa) | |
260 | return -EOPNOTSUPP; | |
261 | ||
262 | if (!reqattr) | |
263 | return -EINVAL; | |
264 | ||
265 | peers = nla_find(nla_data(reqattr), nla_len(reqattr), | |
266 | NL80211_PMSR_ATTR_PEERS); | |
267 | if (!peers) | |
268 | return -EINVAL; | |
269 | ||
270 | count = 0; | |
271 | nla_for_each_nested(peer, peers, rem) { | |
272 | count++; | |
273 | ||
274 | if (count > rdev->wiphy.pmsr_capa->max_peers) { | |
275 | NL_SET_ERR_MSG_ATTR(info->extack, peer, | |
276 | "Too many peers used"); | |
277 | return -EINVAL; | |
278 | } | |
279 | } | |
280 | ||
281 | req = kzalloc(struct_size(req, peers, count), GFP_KERNEL); | |
282 | if (!req) | |
283 | return -ENOMEM; | |
284 | ||
285 | if (info->attrs[NL80211_ATTR_TIMEOUT]) | |
286 | req->timeout = nla_get_u32(info->attrs[NL80211_ATTR_TIMEOUT]); | |
287 | ||
288 | if (info->attrs[NL80211_ATTR_MAC]) { | |
289 | if (!rdev->wiphy.pmsr_capa->randomize_mac_addr) { | |
290 | NL_SET_ERR_MSG_ATTR(info->extack, | |
291 | info->attrs[NL80211_ATTR_MAC], | |
292 | "device cannot randomize MAC address"); | |
293 | err = -EINVAL; | |
294 | goto out_err; | |
295 | } | |
296 | ||
297 | err = nl80211_parse_random_mac(info->attrs, req->mac_addr, | |
298 | req->mac_addr_mask); | |
299 | if (err) | |
300 | goto out_err; | |
301 | } else { | |
0acd9928 | 302 | memcpy(req->mac_addr, wdev_address(wdev), ETH_ALEN); |
76763741 | 303 | eth_broadcast_addr(req->mac_addr_mask); |
9bb7e0f2 JB |
304 | } |
305 | ||
306 | idx = 0; | |
307 | nla_for_each_nested(peer, peers, rem) { | |
308 | /* NB: this reuses info->attrs, but we no longer need it */ | |
309 | err = pmsr_parse_peer(rdev, peer, &req->peers[idx], info); | |
310 | if (err) | |
311 | goto out_err; | |
312 | idx++; | |
313 | } | |
314 | ||
315 | req->n_peers = count; | |
316 | req->cookie = cfg80211_assign_cookie(rdev); | |
ff1bab1b | 317 | req->nl_portid = info->snd_portid; |
9bb7e0f2 JB |
318 | |
319 | err = rdev_start_pmsr(rdev, wdev, req); | |
320 | if (err) | |
321 | goto out_err; | |
322 | ||
323 | list_add_tail(&req->list, &wdev->pmsr_list); | |
324 | ||
325 | nl_set_extack_cookie_u64(info->extack, req->cookie); | |
326 | return 0; | |
327 | out_err: | |
328 | kfree(req); | |
329 | return err; | |
330 | } | |
331 | ||
332 | void cfg80211_pmsr_complete(struct wireless_dev *wdev, | |
333 | struct cfg80211_pmsr_request *req, | |
334 | gfp_t gfp) | |
335 | { | |
336 | struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); | |
337 | struct sk_buff *msg; | |
338 | void *hdr; | |
339 | ||
340 | trace_cfg80211_pmsr_complete(wdev->wiphy, wdev, req->cookie); | |
341 | ||
342 | msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp); | |
343 | if (!msg) | |
344 | goto free_request; | |
345 | ||
346 | hdr = nl80211hdr_put(msg, 0, 0, 0, | |
347 | NL80211_CMD_PEER_MEASUREMENT_COMPLETE); | |
348 | if (!hdr) | |
349 | goto free_msg; | |
350 | ||
351 | if (nla_put_u32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx) || | |
352 | nla_put_u64_64bit(msg, NL80211_ATTR_WDEV, wdev_id(wdev), | |
353 | NL80211_ATTR_PAD)) | |
354 | goto free_msg; | |
355 | ||
356 | if (nla_put_u64_64bit(msg, NL80211_ATTR_COOKIE, req->cookie, | |
357 | NL80211_ATTR_PAD)) | |
358 | goto free_msg; | |
359 | ||
360 | genlmsg_end(msg, hdr); | |
361 | genlmsg_unicast(wiphy_net(wdev->wiphy), msg, req->nl_portid); | |
362 | goto free_request; | |
363 | free_msg: | |
364 | nlmsg_free(msg); | |
365 | free_request: | |
366 | spin_lock_bh(&wdev->pmsr_lock); | |
367 | list_del(&req->list); | |
368 | spin_unlock_bh(&wdev->pmsr_lock); | |
369 | kfree(req); | |
370 | } | |
371 | EXPORT_SYMBOL_GPL(cfg80211_pmsr_complete); | |
372 | ||
373 | static int nl80211_pmsr_send_ftm_res(struct sk_buff *msg, | |
374 | struct cfg80211_pmsr_result *res) | |
375 | { | |
376 | if (res->status == NL80211_PMSR_STATUS_FAILURE) { | |
377 | if (nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_FAIL_REASON, | |
378 | res->ftm.failure_reason)) | |
379 | goto error; | |
380 | ||
381 | if (res->ftm.failure_reason == | |
382 | NL80211_PMSR_FTM_FAILURE_PEER_BUSY && | |
383 | res->ftm.busy_retry_time && | |
384 | nla_put_u32(msg, NL80211_PMSR_FTM_RESP_ATTR_BUSY_RETRY_TIME, | |
385 | res->ftm.busy_retry_time)) | |
386 | goto error; | |
387 | ||
388 | return 0; | |
389 | } | |
390 | ||
391 | #define PUT(tp, attr, val) \ | |
392 | do { \ | |
393 | if (nla_put_##tp(msg, \ | |
394 | NL80211_PMSR_FTM_RESP_ATTR_##attr, \ | |
395 | res->ftm.val)) \ | |
396 | goto error; \ | |
397 | } while (0) | |
398 | ||
399 | #define PUTOPT(tp, attr, val) \ | |
400 | do { \ | |
401 | if (res->ftm.val##_valid) \ | |
402 | PUT(tp, attr, val); \ | |
403 | } while (0) | |
404 | ||
405 | #define PUT_U64(attr, val) \ | |
406 | do { \ | |
407 | if (nla_put_u64_64bit(msg, \ | |
408 | NL80211_PMSR_FTM_RESP_ATTR_##attr,\ | |
409 | res->ftm.val, \ | |
410 | NL80211_PMSR_FTM_RESP_ATTR_PAD)) \ | |
411 | goto error; \ | |
412 | } while (0) | |
413 | ||
414 | #define PUTOPT_U64(attr, val) \ | |
415 | do { \ | |
416 | if (res->ftm.val##_valid) \ | |
417 | PUT_U64(attr, val); \ | |
418 | } while (0) | |
419 | ||
420 | if (res->ftm.burst_index >= 0) | |
421 | PUT(u32, BURST_INDEX, burst_index); | |
422 | PUTOPT(u32, NUM_FTMR_ATTEMPTS, num_ftmr_attempts); | |
423 | PUTOPT(u32, NUM_FTMR_SUCCESSES, num_ftmr_successes); | |
424 | PUT(u8, NUM_BURSTS_EXP, num_bursts_exp); | |
425 | PUT(u8, BURST_DURATION, burst_duration); | |
426 | PUT(u8, FTMS_PER_BURST, ftms_per_burst); | |
427 | PUTOPT(s32, RSSI_AVG, rssi_avg); | |
428 | PUTOPT(s32, RSSI_SPREAD, rssi_spread); | |
429 | if (res->ftm.tx_rate_valid && | |
430 | !nl80211_put_sta_rate(msg, &res->ftm.tx_rate, | |
431 | NL80211_PMSR_FTM_RESP_ATTR_TX_RATE)) | |
432 | goto error; | |
433 | if (res->ftm.rx_rate_valid && | |
434 | !nl80211_put_sta_rate(msg, &res->ftm.rx_rate, | |
435 | NL80211_PMSR_FTM_RESP_ATTR_RX_RATE)) | |
436 | goto error; | |
437 | PUTOPT_U64(RTT_AVG, rtt_avg); | |
438 | PUTOPT_U64(RTT_VARIANCE, rtt_variance); | |
439 | PUTOPT_U64(RTT_SPREAD, rtt_spread); | |
440 | PUTOPT_U64(DIST_AVG, dist_avg); | |
441 | PUTOPT_U64(DIST_VARIANCE, dist_variance); | |
442 | PUTOPT_U64(DIST_SPREAD, dist_spread); | |
443 | if (res->ftm.lci && res->ftm.lci_len && | |
444 | nla_put(msg, NL80211_PMSR_FTM_RESP_ATTR_LCI, | |
445 | res->ftm.lci_len, res->ftm.lci)) | |
446 | goto error; | |
447 | if (res->ftm.civicloc && res->ftm.civicloc_len && | |
448 | nla_put(msg, NL80211_PMSR_FTM_RESP_ATTR_CIVICLOC, | |
449 | res->ftm.civicloc_len, res->ftm.civicloc)) | |
450 | goto error; | |
451 | #undef PUT | |
452 | #undef PUTOPT | |
453 | #undef PUT_U64 | |
454 | #undef PUTOPT_U64 | |
455 | ||
456 | return 0; | |
457 | error: | |
458 | return -ENOSPC; | |
459 | } | |
460 | ||
461 | static int nl80211_pmsr_send_result(struct sk_buff *msg, | |
462 | struct cfg80211_pmsr_result *res) | |
463 | { | |
464 | struct nlattr *pmsr, *peers, *peer, *resp, *data, *typedata; | |
465 | ||
ae0be8de | 466 | pmsr = nla_nest_start_noflag(msg, NL80211_ATTR_PEER_MEASUREMENTS); |
9bb7e0f2 JB |
467 | if (!pmsr) |
468 | goto error; | |
469 | ||
ae0be8de | 470 | peers = nla_nest_start_noflag(msg, NL80211_PMSR_ATTR_PEERS); |
9bb7e0f2 JB |
471 | if (!peers) |
472 | goto error; | |
473 | ||
ae0be8de | 474 | peer = nla_nest_start_noflag(msg, 1); |
9bb7e0f2 JB |
475 | if (!peer) |
476 | goto error; | |
477 | ||
478 | if (nla_put(msg, NL80211_PMSR_PEER_ATTR_ADDR, ETH_ALEN, res->addr)) | |
479 | goto error; | |
480 | ||
ae0be8de | 481 | resp = nla_nest_start_noflag(msg, NL80211_PMSR_PEER_ATTR_RESP); |
9bb7e0f2 JB |
482 | if (!resp) |
483 | goto error; | |
484 | ||
485 | if (nla_put_u32(msg, NL80211_PMSR_RESP_ATTR_STATUS, res->status) || | |
486 | nla_put_u64_64bit(msg, NL80211_PMSR_RESP_ATTR_HOST_TIME, | |
487 | res->host_time, NL80211_PMSR_RESP_ATTR_PAD)) | |
488 | goto error; | |
489 | ||
490 | if (res->ap_tsf_valid && | |
491 | nla_put_u64_64bit(msg, NL80211_PMSR_RESP_ATTR_AP_TSF, | |
b6584202 | 492 | res->ap_tsf, NL80211_PMSR_RESP_ATTR_PAD)) |
9bb7e0f2 JB |
493 | goto error; |
494 | ||
495 | if (res->final && nla_put_flag(msg, NL80211_PMSR_RESP_ATTR_FINAL)) | |
496 | goto error; | |
497 | ||
ae0be8de | 498 | data = nla_nest_start_noflag(msg, NL80211_PMSR_RESP_ATTR_DATA); |
9bb7e0f2 JB |
499 | if (!data) |
500 | goto error; | |
501 | ||
ae0be8de | 502 | typedata = nla_nest_start_noflag(msg, res->type); |
9bb7e0f2 JB |
503 | if (!typedata) |
504 | goto error; | |
505 | ||
506 | switch (res->type) { | |
507 | case NL80211_PMSR_TYPE_FTM: | |
508 | if (nl80211_pmsr_send_ftm_res(msg, res)) | |
509 | goto error; | |
510 | break; | |
511 | default: | |
512 | WARN_ON(1); | |
513 | } | |
514 | ||
515 | nla_nest_end(msg, typedata); | |
516 | nla_nest_end(msg, data); | |
517 | nla_nest_end(msg, resp); | |
518 | nla_nest_end(msg, peer); | |
519 | nla_nest_end(msg, peers); | |
520 | nla_nest_end(msg, pmsr); | |
521 | ||
522 | return 0; | |
523 | error: | |
524 | return -ENOSPC; | |
525 | } | |
526 | ||
527 | void cfg80211_pmsr_report(struct wireless_dev *wdev, | |
528 | struct cfg80211_pmsr_request *req, | |
529 | struct cfg80211_pmsr_result *result, | |
530 | gfp_t gfp) | |
531 | { | |
532 | struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); | |
533 | struct sk_buff *msg; | |
534 | void *hdr; | |
535 | int err; | |
536 | ||
537 | trace_cfg80211_pmsr_report(wdev->wiphy, wdev, req->cookie, | |
538 | result->addr); | |
539 | ||
540 | /* | |
541 | * Currently, only variable items are LCI and civic location, | |
542 | * both of which are reasonably short so we don't need to | |
543 | * worry about them here for the allocation. | |
544 | */ | |
545 | msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp); | |
546 | if (!msg) | |
547 | return; | |
548 | ||
549 | hdr = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_PEER_MEASUREMENT_RESULT); | |
550 | if (!hdr) | |
551 | goto free; | |
552 | ||
553 | if (nla_put_u32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx) || | |
554 | nla_put_u64_64bit(msg, NL80211_ATTR_WDEV, wdev_id(wdev), | |
555 | NL80211_ATTR_PAD)) | |
556 | goto free; | |
557 | ||
558 | if (nla_put_u64_64bit(msg, NL80211_ATTR_COOKIE, req->cookie, | |
559 | NL80211_ATTR_PAD)) | |
560 | goto free; | |
561 | ||
562 | err = nl80211_pmsr_send_result(msg, result); | |
563 | if (err) { | |
564 | pr_err_ratelimited("peer measurement result: message didn't fit!"); | |
565 | goto free; | |
566 | } | |
567 | ||
568 | genlmsg_end(msg, hdr); | |
569 | genlmsg_unicast(wiphy_net(wdev->wiphy), msg, req->nl_portid); | |
570 | return; | |
571 | free: | |
572 | nlmsg_free(msg); | |
573 | } | |
574 | EXPORT_SYMBOL_GPL(cfg80211_pmsr_report); | |
575 | ||
73350424 | 576 | static void cfg80211_pmsr_process_abort(struct wireless_dev *wdev) |
9bb7e0f2 | 577 | { |
9bb7e0f2 JB |
578 | struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); |
579 | struct cfg80211_pmsr_request *req, *tmp; | |
580 | LIST_HEAD(free_list); | |
581 | ||
73350424 JB |
582 | lockdep_assert_held(&wdev->mtx); |
583 | ||
9bb7e0f2 JB |
584 | spin_lock_bh(&wdev->pmsr_lock); |
585 | list_for_each_entry_safe(req, tmp, &wdev->pmsr_list, list) { | |
586 | if (req->nl_portid) | |
587 | continue; | |
588 | list_move_tail(&req->list, &free_list); | |
589 | } | |
590 | spin_unlock_bh(&wdev->pmsr_lock); | |
591 | ||
592 | list_for_each_entry_safe(req, tmp, &free_list, list) { | |
9bb7e0f2 | 593 | rdev_abort_pmsr(rdev, wdev, req); |
9bb7e0f2 JB |
594 | |
595 | kfree(req); | |
596 | } | |
597 | } | |
598 | ||
73350424 JB |
599 | void cfg80211_pmsr_free_wk(struct work_struct *work) |
600 | { | |
601 | struct wireless_dev *wdev = container_of(work, struct wireless_dev, | |
602 | pmsr_free_wk); | |
603 | ||
604 | wdev_lock(wdev); | |
605 | cfg80211_pmsr_process_abort(wdev); | |
606 | wdev_unlock(wdev); | |
607 | } | |
608 | ||
9bb7e0f2 JB |
609 | void cfg80211_pmsr_wdev_down(struct wireless_dev *wdev) |
610 | { | |
611 | struct cfg80211_pmsr_request *req; | |
612 | bool found = false; | |
613 | ||
614 | spin_lock_bh(&wdev->pmsr_lock); | |
615 | list_for_each_entry(req, &wdev->pmsr_list, list) { | |
616 | found = true; | |
617 | req->nl_portid = 0; | |
618 | } | |
619 | spin_unlock_bh(&wdev->pmsr_lock); | |
620 | ||
621 | if (found) | |
73350424 JB |
622 | cfg80211_pmsr_process_abort(wdev); |
623 | ||
9bb7e0f2 JB |
624 | WARN_ON(!list_empty(&wdev->pmsr_list)); |
625 | } | |
626 | ||
627 | void cfg80211_release_pmsr(struct wireless_dev *wdev, u32 portid) | |
628 | { | |
629 | struct cfg80211_pmsr_request *req; | |
630 | ||
631 | spin_lock_bh(&wdev->pmsr_lock); | |
632 | list_for_each_entry(req, &wdev->pmsr_list, list) { | |
633 | if (req->nl_portid == portid) { | |
634 | req->nl_portid = 0; | |
635 | schedule_work(&wdev->pmsr_free_wk); | |
636 | } | |
637 | } | |
638 | spin_unlock_bh(&wdev->pmsr_lock); | |
639 | } | |
640 | ||
641 | #endif /* __PMSR_H */ |