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