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