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