1 /* 2 * Copyright (c) 2012-2021 The Linux Foundation. All rights reserved. 3 * 4 * Permission to use, copy, modify, and/or distribute this software for 5 * any purpose with or without fee is hereby granted, provided that the 6 * above copyright notice and this permission notice appear in all 7 * copies. 8 * 9 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL 10 * WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED 11 * WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE 12 * AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL 13 * DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR 14 * PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER 15 * TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR 16 * PERFORMANCE OF THIS SOFTWARE. 17 */ 18 19 /** 20 * DOC: wlan_hdd_wifi_pos.c 21 * This file defines the important functions pertinent to wifi positioning 22 * component's os_if layer. 23 */ 24 25 #include "qdf_platform.h" 26 #include "qdf_module.h" 27 #include "wlan_nlink_srv.h" 28 #include "wlan_ptt_sock_svc.h" 29 #include "wlan_nlink_common.h" 30 #include "os_if_wifi_pos.h" 31 #include "wifi_pos_api.h" 32 #include "wlan_cfg80211.h" 33 #include "wlan_objmgr_psoc_obj.h" 34 #ifdef CNSS_GENL 35 #ifdef CONFIG_CNSS_OUT_OF_TREE 36 #include "cnss_nl.h" 37 #else 38 #include <net/cnss_nl.h> 39 #endif 40 #include "linux/genetlink.h" 41 #include "wifi_pos_utils_pub.h" 42 #endif 43 44 #ifdef CNSS_GENL 45 #define WLAN_CLD80211_MAX_SIZE SKB_WITH_OVERHEAD(8192UL) 46 47 #define CLD80211_ATTR_CMD 4 48 #define CLD80211_ATTR_CMD_TAG_DATA 5 49 #define CLD80211_ATTR_MAX 5 50 51 static const uint32_t 52 cap_resp_sub_attr_len[CLD80211_SUB_ATTR_CAPS_MAX + 1] = { 53 [CLD80211_SUB_ATTR_CAPS_OEM_TARGET_SIGNATURE] = 54 OEM_TARGET_SIGNATURE_LEN, 55 [CLD80211_SUB_ATTR_CAPS_OEM_TARGET_TYPE] = sizeof(uint32_t), 56 [CLD80211_SUB_ATTR_CAPS_OEM_FW_VERSION] = sizeof(uint32_t), 57 [CLD80211_SUB_ATTR_CAPS_DRIVER_VERSION_MAJOR] = sizeof(uint8_t), 58 [CLD80211_SUB_ATTR_CAPS_DRIVER_VERSION_MINOR] = sizeof(uint8_t), 59 [CLD80211_SUB_ATTR_CAPS_DRIVER_VERSION_PATCH] = sizeof(uint8_t), 60 [CLD80211_SUB_ATTR_CAPS_DRIVER_VERSION_BUILD] = sizeof(uint8_t), 61 [CLD80211_SUB_ATTR_CAPS_ALLOWED_DWELL_TIME_MIN] = sizeof(uint16_t), 62 [CLD80211_SUB_ATTR_CAPS_ALLOWED_DWELL_TIME_MAX] = sizeof(uint16_t), 63 [CLD80211_SUB_ATTR_CAPS_CURRENT_DWELL_TIME_MIN] = sizeof(uint16_t), 64 [CLD80211_SUB_ATTR_CAPS_CURRENT_DWELL_TIME_MAX] = sizeof(uint16_t), 65 [CLD80211_SUB_ATTR_CAPS_SUPPORTED_BANDS] = sizeof(uint16_t), 66 [CLD80211_SUB_ATTR_CAPS_USER_DEFINED_CAPS] = 67 sizeof(struct wifi_pos_user_defined_caps), 68 }; 69 70 static const uint32_t 71 peer_status_sub_attr_len[CLD80211_SUB_ATTR_PEER_MAX + 1] = { 72 [CLD80211_SUB_ATTR_PEER_MAC_ADDR] = ETH_ALEN, 73 [CLD80211_SUB_ATTR_PEER_STATUS] = sizeof(uint8_t), 74 [CLD80211_SUB_ATTR_PEER_VDEV_ID] = sizeof(uint8_t), 75 [CLD80211_SUB_ATTR_PEER_CAPABILITY] = sizeof(uint32_t), 76 [CLD80211_SUB_ATTR_PEER_RESERVED] = sizeof(uint32_t), 77 [CLD80211_SUB_ATTR_PEER_CHAN_INFO] = 78 sizeof(struct wifi_pos_ch_info_rsp), 79 }; 80 81 static const uint32_t 82 ch_resp_sub_attr_len[CLD80211_SUB_ATTR_CH_MAX + 1] = { 83 [CLD80211_SUB_ATTR_CHANNEL_NUM_CHAN] = sizeof(uint32_t), 84 [CLD80211_SUB_ATTR_CH_LIST] = sizeof(uint32_t), 85 [CLD80211_SUB_ATTR_CH_CHAN_ID] = sizeof(uint32_t), 86 [CLD80211_SUB_ATTR_CH_MHZ] = sizeof(uint32_t), 87 [CLD80211_SUB_ATTR_CH_BAND_CF_1] = sizeof(uint32_t), 88 [CLD80211_SUB_ATTR_CH_BAND_CF_2] = sizeof(uint32_t), 89 [CLD80211_SUB_ATTR_CH_INFO] = sizeof(uint32_t), 90 [CLD80211_SUB_ATTR_CH_REG_INFO_1] = sizeof(uint32_t), 91 [CLD80211_SUB_ATTR_CH_REG_INFO_2] = sizeof(uint32_t), 92 }; 93 #endif 94 95 static int map_wifi_pos_cmd_to_ani_msg_rsp( 96 enum wifi_pos_cmd_ids cmd) 97 { 98 switch (cmd) { 99 case WIFI_POS_CMD_REGISTRATION: 100 return ANI_MSG_APP_REG_RSP; 101 case WIFI_POS_CMD_SET_CAPS: 102 return ANI_MSG_SET_OEM_CAP_RSP; 103 case WIFI_POS_CMD_GET_CAPS: 104 return ANI_MSG_GET_OEM_CAP_RSP; 105 case WIFI_POS_CMD_GET_CH_INFO: 106 return ANI_MSG_CHANNEL_INFO_RSP; 107 case WIFI_POS_CMD_OEM_DATA: 108 return ANI_MSG_OEM_DATA_RSP; 109 case WIFI_POS_CMD_ERROR: 110 return ANI_MSG_OEM_ERROR; 111 case WIFI_POS_PEER_STATUS_IND: 112 return ANI_MSG_PEER_STATUS_IND; 113 default: 114 osif_err("response message is invalid :%d", cmd); 115 return -EINVAL; 116 } 117 } 118 119 static enum wifi_pos_cmd_ids 120 map_ani_msg_req_to_wifi_pos_cmd(uint32_t cmd) 121 { 122 switch (cmd) { 123 case ANI_MSG_APP_REG_REQ: 124 return WIFI_POS_CMD_REGISTRATION; 125 case ANI_MSG_SET_OEM_CAP_REQ: 126 return WIFI_POS_CMD_SET_CAPS; 127 case ANI_MSG_GET_OEM_CAP_REQ: 128 return WIFI_POS_CMD_GET_CAPS; 129 case ANI_MSG_CHANNEL_INFO_REQ: 130 return WIFI_POS_CMD_GET_CH_INFO; 131 case ANI_MSG_OEM_DATA_REQ: 132 return WIFI_POS_CMD_OEM_DATA; 133 default: 134 osif_err("ani req is invalid :%d", cmd); 135 return WIFI_POS_CMD_INVALID; 136 } 137 } 138 139 #ifdef CNSS_GENL 140 static enum wifi_pos_cmd_ids 141 map_cld_vendor_sub_cmd_to_wifi_pos_cmd( 142 enum cld80211_vendor_sub_cmds cmd) 143 { 144 switch (cmd) { 145 case CLD80211_VENDOR_SUB_CMD_REGISTRATION: 146 return WIFI_POS_CMD_REGISTRATION; 147 case CLD80211_VENDOR_SUB_CMD_SET_CAPS: 148 return WIFI_POS_CMD_SET_CAPS; 149 case CLD80211_VENDOR_SUB_CMD_GET_CAPS: 150 return WIFI_POS_CMD_GET_CAPS; 151 case CLD80211_VENDOR_SUB_CMD_GET_CH_INFO: 152 return WIFI_POS_CMD_GET_CH_INFO; 153 case CLD80211_VENDOR_SUB_CMD_OEM_DATA: 154 return WIFI_POS_CMD_OEM_DATA; 155 default: 156 osif_err("cld vendor subcmd is invalid :%d", cmd); 157 return WIFI_POS_CMD_INVALID; 158 } 159 } 160 161 static enum cld80211_vendor_sub_cmds 162 map_wifi_pos_cmd_to_cld_vendor_sub_cmd( 163 enum wifi_pos_cmd_ids cmd) 164 { 165 switch (cmd) { 166 case WIFI_POS_CMD_REGISTRATION: 167 return CLD80211_VENDOR_SUB_CMD_REGISTRATION; 168 case WIFI_POS_CMD_SET_CAPS: 169 return CLD80211_VENDOR_SUB_CMD_SET_CAPS; 170 case WIFI_POS_CMD_GET_CAPS: 171 return CLD80211_VENDOR_SUB_CMD_GET_CAPS; 172 case WIFI_POS_CMD_GET_CH_INFO: 173 return CLD80211_VENDOR_SUB_CMD_GET_CH_INFO; 174 case WIFI_POS_CMD_OEM_DATA: 175 return CLD80211_VENDOR_SUB_CMD_OEM_DATA; 176 case WIFI_POS_CMD_ERROR: 177 return ANI_MSG_OEM_ERROR; 178 case WIFI_POS_PEER_STATUS_IND: 179 return ANI_MSG_PEER_STATUS_IND; 180 default: 181 osif_err("response message is invalid :%d", cmd); 182 return CLD80211_VENDOR_SUB_CMD_INVALID; 183 } 184 } 185 186 static void os_if_wifi_pos_send_peer_nl_status(uint32_t pid, uint8_t *buf) 187 { 188 void *hdr; 189 int flags = GFP_KERNEL; 190 struct sk_buff *msg = NULL; 191 struct nlattr *nest1, *nest2, *nest3; 192 struct wifi_pos_peer_status_info *peer_info; 193 struct wifi_pos_ch_info_rsp *chan_info; 194 195 msg = cld80211_oem_rsp_alloc_skb(pid, &hdr, &nest1, &flags); 196 if (!msg) { 197 osif_err("alloc_skb failed"); 198 return; 199 } 200 201 peer_info = (struct wifi_pos_peer_status_info *)buf; 202 chan_info = &peer_info->peer_chan_info; 203 204 nla_put_u32(msg, CLD80211_ATTR_CMD, 205 CLD80211_VENDOR_SUB_CMD_PEER_STATUS_IND); 206 nest2 = nla_nest_start(msg, CLD80211_ATTR_CMD_TAG_DATA); 207 if (!nest2) { 208 osif_err("nla_nest_start failed"); 209 dev_kfree_skb(msg); 210 return; 211 } 212 213 nla_put(msg, CLD80211_SUB_ATTR_PEER_MAC_ADDR, 214 ETH_ALEN, peer_info->peer_mac_addr); 215 nla_put_u8(msg, CLD80211_SUB_ATTR_PEER_STATUS, 216 peer_info->peer_status); 217 nla_put_u8(msg, CLD80211_SUB_ATTR_PEER_VDEV_ID, 218 peer_info->vdev_id); 219 nla_put_u32(msg, CLD80211_SUB_ATTR_PEER_CAPABILITY, 220 peer_info->peer_capability); 221 nla_put_u32(msg, CLD80211_SUB_ATTR_PEER_RESERVED, 222 peer_info->reserved0); 223 nest3 = nla_nest_start(msg, CLD80211_SUB_ATTR_PEER_CHAN_INFO); 224 if (!nest3) { 225 osif_err("nla_nest_start failed"); 226 dev_kfree_skb(msg); 227 return; 228 } 229 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_CHAN_ID, 230 chan_info->chan_id); 231 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_MHZ, chan_info->mhz); 232 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_BAND_CF_1, 233 chan_info->band_center_freq1); 234 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_BAND_CF_2, 235 chan_info->band_center_freq2); 236 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_INFO, chan_info->info); 237 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_REG_INFO_1, 238 chan_info->reg_info_1); 239 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_REG_INFO_2, 240 chan_info->reg_info_2); 241 242 nla_nest_end(msg, nest3); 243 nla_nest_end(msg, nest2); 244 245 osif_debug("sending oem rsp: type: %d to pid (%d)", 246 CLD80211_VENDOR_SUB_CMD_PEER_STATUS_IND, pid); 247 248 cld80211_oem_send_reply(msg, hdr, nest1, flags); 249 } 250 251 static void os_if_send_cap_nl_resp(uint32_t pid, uint8_t *buf) 252 { 253 void *hdr; 254 int flags = GFP_KERNEL; 255 struct sk_buff *msg = NULL; 256 struct nlattr *nest1, *nest2; 257 struct wifi_pos_oem_get_cap_rsp *cap_rsp; 258 259 msg = cld80211_oem_rsp_alloc_skb(pid, &hdr, &nest1, &flags); 260 if (!msg) { 261 osif_err("alloc_skb failed"); 262 return; 263 } 264 265 nla_put_u32(msg, CLD80211_ATTR_CMD, 266 map_wifi_pos_cmd_to_cld_vendor_sub_cmd(WIFI_POS_CMD_GET_CAPS)); 267 268 cap_rsp = (struct wifi_pos_oem_get_cap_rsp *)(buf); 269 nest2 = nla_nest_start(msg, CLD80211_ATTR_CMD_TAG_DATA); 270 271 if (!nest2) { 272 osif_err("nla_nest_start failed"); 273 dev_kfree_skb(msg); 274 return; 275 } 276 277 nla_put(msg, CLD80211_SUB_ATTR_CAPS_OEM_TARGET_SIGNATURE, 278 OEM_TARGET_SIGNATURE_LEN, OEM_TARGET_SIGNATURE); 279 nla_put_u32(msg, CLD80211_SUB_ATTR_CAPS_OEM_TARGET_TYPE, 280 cap_rsp->driver_cap.oem_target_type); 281 nla_put_u32(msg, CLD80211_SUB_ATTR_CAPS_OEM_FW_VERSION, 282 cap_rsp->driver_cap.oem_fw_version); 283 nla_put_u8(msg, CLD80211_SUB_ATTR_CAPS_DRIVER_VERSION_MAJOR, 284 cap_rsp->driver_cap.driver_version.major); 285 nla_put_u8(msg, CLD80211_SUB_ATTR_CAPS_DRIVER_VERSION_MINOR, 286 cap_rsp->driver_cap.driver_version.minor); 287 nla_put_u8(msg, CLD80211_SUB_ATTR_CAPS_DRIVER_VERSION_PATCH, 288 cap_rsp->driver_cap.driver_version.patch); 289 nla_put_u8(msg, CLD80211_SUB_ATTR_CAPS_DRIVER_VERSION_BUILD, 290 cap_rsp->driver_cap.driver_version.build); 291 nla_put_u16(msg, CLD80211_SUB_ATTR_CAPS_ALLOWED_DWELL_TIME_MIN, 292 cap_rsp->driver_cap.allowed_dwell_time_min); 293 nla_put_u16(msg, CLD80211_SUB_ATTR_CAPS_ALLOWED_DWELL_TIME_MAX, 294 cap_rsp->driver_cap.allowed_dwell_time_max); 295 nla_put_u16(msg, CLD80211_SUB_ATTR_CAPS_CURRENT_DWELL_TIME_MIN, 296 cap_rsp->driver_cap.curr_dwell_time_min); 297 nla_put_u16(msg, CLD80211_SUB_ATTR_CAPS_CURRENT_DWELL_TIME_MAX, 298 cap_rsp->driver_cap.curr_dwell_time_max); 299 nla_put_u16(msg, CLD80211_SUB_ATTR_CAPS_SUPPORTED_BANDS, 300 cap_rsp->driver_cap.supported_bands); 301 nla_put(msg, CLD80211_SUB_ATTR_CAPS_USER_DEFINED_CAPS, 302 sizeof(struct wifi_pos_user_defined_caps), 303 &cap_rsp->user_defined_cap); 304 nla_nest_end(msg, nest2); 305 306 osif_debug("sending oem rsp: type: %d to pid (%d)", 307 CLD80211_VENDOR_SUB_CMD_GET_CAPS, pid); 308 309 cld80211_oem_send_reply(msg, hdr, nest1, flags); 310 } 311 312 static void 313 os_if_get_chan_nl_resp_len(uint32_t *chan_info, uint32_t *attr_headers) 314 { 315 uint32_t i; 316 struct nlattr more_data; 317 struct nlattr attr_tag_data; 318 struct nlattr cld80211_subattr_ch_list; 319 struct nlattr chan_iter; 320 321 *attr_headers = NLA_ALIGN(sizeof(attr_tag_data)); 322 *attr_headers += NLA_ALIGN(sizeof(more_data)); 323 *attr_headers += nla_total_size( 324 ch_resp_sub_attr_len[CLD80211_SUB_ATTR_CHANNEL_NUM_CHAN]); 325 *attr_headers += sizeof(cld80211_subattr_ch_list); 326 327 *chan_info = NLA_ALIGN(sizeof(chan_iter)); 328 i = CLD80211_SUB_ATTR_CH_LIST; 329 for (; i <= CLD80211_SUB_ATTR_CH_MAX; i++) 330 *chan_info += nla_total_size(ch_resp_sub_attr_len[i]); 331 } 332 333 static uint8_t os_if_get_max_chan_nl_resp(uint8_t chan_num) 334 { 335 struct nlattr vendor_data; 336 struct nlattr attr_cmd; 337 uint32_t chan_info = 0, attr_headers = 0; 338 uint32_t chan_info_msg_len, chan_allow = 0; 339 340 os_if_get_chan_nl_resp_len(&chan_info, &attr_headers); 341 attr_headers += NLA_ALIGN(sizeof(vendor_data)); 342 attr_headers += NLA_ALIGN(sizeof(attr_cmd)); 343 344 chan_info_msg_len = WLAN_CLD80211_MAX_SIZE; 345 chan_info_msg_len -= WIFIPOS_RESERVE_BYTES; 346 chan_info_msg_len -= attr_headers; 347 348 chan_allow = chan_info_msg_len / chan_info; 349 350 if (chan_num > chan_allow) 351 return chan_allow; 352 else 353 return chan_num; 354 } 355 356 static int 357 os_if_create_ch_nl_resp(uint32_t pid, uint8_t *buf, uint16_t num_chan, 358 bool is_frag) 359 { 360 void *hdr; 361 int i; 362 int flags = GFP_KERNEL; 363 struct sk_buff *msg = NULL; 364 struct nlattr *nest1, *nest2; 365 struct nlattr *nest3, *nest4; 366 struct wifi_pos_ch_info_rsp *channel_rsp; 367 368 channel_rsp = (struct wifi_pos_ch_info_rsp *)buf; 369 370 msg = cld80211_oem_rsp_alloc_skb(pid, &hdr, &nest1, &flags); 371 if (!msg) { 372 osif_err("alloc_skb failed"); 373 return -EPERM; 374 } 375 376 nla_put_u32(msg, CLD80211_ATTR_CMD, 377 CLD80211_VENDOR_SUB_CMD_GET_CH_INFO); 378 379 nest2 = nla_nest_start(msg, CLD80211_ATTR_CMD_TAG_DATA); 380 if (!nest2) 381 goto fail; 382 383 if (is_frag) 384 nla_put_flag(msg, CLD80211_SUB_ATTR_CH_MORE_DATA); 385 386 nla_put_u32(msg, CLD80211_SUB_ATTR_CHANNEL_NUM_CHAN, num_chan); 387 388 nest3 = nla_nest_start(msg, CLD80211_SUB_ATTR_CH_LIST); 389 if (!nest3) 390 goto fail; 391 for (i = 0; i < num_chan; i++) { 392 nest4 = nla_nest_start(msg, i); 393 if (!nest4) 394 goto fail; 395 396 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_CHAN_ID, 397 channel_rsp->chan_id); 398 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_MHZ, channel_rsp->mhz); 399 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_BAND_CF_1, 400 channel_rsp->band_center_freq1); 401 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_BAND_CF_2, 402 channel_rsp->band_center_freq2); 403 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_INFO, channel_rsp->info); 404 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_REG_INFO_1, 405 channel_rsp->reg_info_1); 406 nla_put_u32(msg, CLD80211_SUB_ATTR_CH_REG_INFO_2, 407 channel_rsp->reg_info_2); 408 nla_nest_end(msg, nest4); 409 channel_rsp++; 410 } 411 412 nla_nest_end(msg, nest3); 413 nla_nest_end(msg, nest2); 414 415 osif_debug("sending oem rsp: type: %d to pid (%d)", 416 CLD80211_VENDOR_SUB_CMD_GET_CH_INFO, pid); 417 418 cld80211_oem_send_reply(msg, hdr, nest1, flags); 419 return 0; 420 421 fail: 422 osif_err("failed to fill CHAN_RESP attributes"); 423 dev_kfree_skb(msg); 424 return -EPERM; 425 } 426 427 static void os_if_send_chan_nl_resp(uint32_t pid, uint8_t *buf) 428 { 429 int err; 430 uint8_t check_chans = 0; 431 uint8_t *chnk_ptr, chan_allow = 0; 432 bool resp_frag = false; 433 434 check_chans = buf[0]; 435 chnk_ptr = &buf[1]; 436 437 do { 438 chan_allow = os_if_get_max_chan_nl_resp(check_chans); 439 440 if (check_chans > chan_allow) 441 resp_frag = true; 442 else 443 resp_frag = false; 444 check_chans -= chan_allow; 445 446 err = os_if_create_ch_nl_resp(pid, chnk_ptr, 447 chan_allow, resp_frag); 448 if (err) { 449 osif_err("failed to alloc memory for ch_nl_resp"); 450 return; 451 } 452 chnk_ptr += (sizeof(struct wifi_pos_ch_info_rsp) * 453 chan_allow); 454 } while (resp_frag); 455 } 456 457 static int 458 os_if_create_oemdata_resp(uint32_t pid, uint8_t *buf, bool frag_resp, 459 uint32_t chnk_len) 460 { 461 void *hdr; 462 int flags = GFP_KERNEL; 463 struct sk_buff *msg = NULL; 464 struct nlattr *nest1, *nest2; 465 466 msg = cld80211_oem_rsp_alloc_skb(pid, &hdr, &nest1, &flags); 467 if (!msg) { 468 osif_err("alloc_skb failed"); 469 return -EPERM; 470 } 471 472 nla_put_u32(msg, CLD80211_ATTR_CMD, CLD80211_VENDOR_SUB_CMD_OEM_DATA); 473 474 nest2 = nla_nest_start(msg, CLD80211_ATTR_CMD_TAG_DATA); 475 if (!nest2) 476 goto fail; 477 478 if (frag_resp) 479 nla_put_flag(msg, CLD80211_SUB_ATTR_OEM_MORE_DATA); 480 481 nla_put(msg, CLD80211_SUB_ATTR_BINARY_DATA, chnk_len, buf); 482 483 nla_nest_end(msg, nest2); 484 osif_debug("sending oem rsp: type: %d to pid (%d)", 485 CLD80211_VENDOR_SUB_CMD_OEM_DATA, pid); 486 cld80211_oem_send_reply(msg, hdr, nest1, flags); 487 return 0; 488 489 fail: 490 osif_err("failed to fill CHAN_RESP attributes"); 491 dev_kfree_skb(msg); 492 return -EPERM; 493 } 494 495 static void 496 os_if_send_oem_data_nl_resp(uint32_t pid, uint8_t *buf, 497 uint32_t buf_len) 498 { 499 int err; 500 uint32_t attr_len; 501 uint32_t chnk_len, remain_len; 502 uint8_t *chnk_ptr; 503 bool frag_resp = false; 504 505 struct nlattr vendor_data; 506 struct nlattr attr_cmd; 507 struct nlattr attr_tag_data; 508 struct nlattr cld80211_subattr_bindata; 509 struct nlattr more_data; 510 511 attr_len = WIFIPOS_RESERVE_BYTES; 512 attr_len += NLMSG_ALIGN(sizeof(vendor_data)); 513 attr_len += NLMSG_ALIGN(sizeof(attr_cmd)); 514 attr_len += NLMSG_ALIGN(sizeof(attr_tag_data)); 515 attr_len += NLMSG_ALIGN(sizeof(more_data)); 516 517 chnk_ptr = buf; 518 chnk_len = buf_len; 519 remain_len = buf_len; 520 do { 521 if (attr_len + nla_total_size(chnk_len) > 522 WLAN_CLD80211_MAX_SIZE) { 523 frag_resp = true; 524 525 chnk_len = WLAN_CLD80211_MAX_SIZE - (attr_len + 526 sizeof(cld80211_subattr_bindata)); 527 } else { 528 frag_resp = false; 529 } 530 531 remain_len -= chnk_len; 532 533 err = os_if_create_oemdata_resp(pid, chnk_ptr, 534 frag_resp, chnk_len); 535 if (err) { 536 osif_err("failed to alloc memory for oem_nl_resp"); 537 return; 538 } 539 chnk_ptr += chnk_len; 540 chnk_len = remain_len; 541 } while (frag_resp); 542 } 543 544 static void os_if_send_nl_resp(uint32_t pid, uint8_t *buf, 545 enum wifi_pos_cmd_ids cmd, uint32_t len) 546 { 547 switch (cmd) { 548 case WIFI_POS_CMD_GET_CAPS: 549 os_if_send_cap_nl_resp(pid, buf); 550 break; 551 case WIFI_POS_CMD_GET_CH_INFO: 552 os_if_send_chan_nl_resp(pid, buf); 553 break; 554 case WIFI_POS_CMD_OEM_DATA: 555 os_if_send_oem_data_nl_resp(pid, buf, len); 556 break; 557 case WIFI_POS_PEER_STATUS_IND: 558 os_if_wifi_pos_send_peer_nl_status(pid, buf); 559 break; 560 default: 561 osif_err("response message is invalid :%d", cmd); 562 } 563 } 564 #else 565 static void os_if_send_nl_resp(uint32_t pid, uint8_t *buf, 566 enum wifi_pos_cmd_ids cmd, uint32_t len) 567 { 568 } 569 #endif 570 571 /** 572 * os_if_wifi_pos_send_rsp() - send oem registration response 573 * 574 * This function sends oem message to registered application process 575 * 576 * Return: none 577 */ 578 static void os_if_wifi_pos_send_rsp(struct wlan_objmgr_psoc *psoc, uint32_t pid, 579 enum wifi_pos_cmd_ids cmd, uint32_t buf_len, 580 uint8_t *buf) 581 { 582 tAniMsgHdr *aniHdr; 583 struct sk_buff *skb = NULL; 584 struct nlmsghdr *nlh; 585 586 /* OEM msg is always to a specific process and cannot be a broadcast */ 587 if (pid == 0) { 588 osif_err("invalid dest pid"); 589 return; 590 } 591 592 if (ucfg_wifi_pos_is_nl_rsp(psoc)) { 593 os_if_send_nl_resp(pid, buf, cmd, buf_len); 594 } else { 595 skb = alloc_skb(NLMSG_SPACE(sizeof(tAniMsgHdr) + buf_len), 596 GFP_ATOMIC); 597 if (!skb) { 598 osif_alert("alloc_skb failed"); 599 return; 600 } 601 602 nlh = (struct nlmsghdr *)skb->data; 603 nlh->nlmsg_pid = 0; /* from kernel */ 604 nlh->nlmsg_flags = 0; 605 nlh->nlmsg_seq = 0; 606 nlh->nlmsg_type = WLAN_NL_MSG_OEM; 607 nlh->nlmsg_len = NLMSG_LENGTH(sizeof(tAniMsgHdr) + buf_len); 608 609 aniHdr = NLMSG_DATA(nlh); 610 aniHdr->type = map_wifi_pos_cmd_to_ani_msg_rsp(cmd); 611 qdf_mem_copy(&aniHdr[1], buf, buf_len); 612 aniHdr->length = buf_len; 613 614 skb_put(skb, NLMSG_SPACE(sizeof(tAniMsgHdr) + buf_len)); 615 osif_debug("sending oem rsp: type: %d len(%d) to pid (%d)", 616 aniHdr->type, buf_len, pid); 617 nl_srv_ucast_oem(skb, pid, MSG_DONTWAIT); 618 } 619 } 620 621 #ifdef CNSS_GENL 622 static int 623 wifi_pos_parse_nla_oemdata_req(uint32_t len, uint8_t *buf, 624 struct wifi_pos_req_msg *req) 625 { 626 struct nlattr *tb_oem_data[CLD80211_SUB_ATTR_MSG_OEM_DATA_REQ_MAX + 1]; 627 628 if (wlan_cfg80211_nla_parse(tb_oem_data, 629 CLD80211_SUB_ATTR_MSG_OEM_DATA_REQ_MAX, 630 (struct nlattr *)buf, len, NULL)) { 631 osif_err("invalid data in request"); 632 return OEM_ERR_INVALID_MESSAGE_TYPE; 633 } 634 635 if (!tb_oem_data[CLD80211_SUB_ATTR_MSG_OEM_DATA_FW]) { 636 osif_err("CLD80211_SUB_ATTR_MSG_OEM_DATA_FW not present"); 637 return OEM_ERR_INVALID_MESSAGE_TYPE; 638 } 639 req->buf_len = nla_len( 640 tb_oem_data[CLD80211_SUB_ATTR_MSG_OEM_DATA_FW]); 641 req->buf = nla_data( 642 tb_oem_data[CLD80211_SUB_ATTR_MSG_OEM_DATA_FW]); 643 644 return 0; 645 } 646 647 static int wifi_pos_parse_nla_req(const void *data, int len, int pid, 648 struct wifi_pos_req_msg *req) 649 { 650 uint8_t *msg; 651 struct nlattr *tb[CLD80211_ATTR_MAX + 1]; 652 uint32_t msg_len; 653 654 if (wlan_cfg80211_nla_parse(tb, CLD80211_ATTR_MAX, data, len, NULL)) { 655 osif_err("invalid data in request"); 656 return OEM_ERR_INVALID_MESSAGE_TYPE; 657 } 658 659 req->pid = pid; 660 req->msg_type = map_cld_vendor_sub_cmd_to_wifi_pos_cmd( 661 nla_get_u32(tb[CLD80211_ATTR_CMD])); 662 req->rsp_version = WIFI_POS_RSP_V2_NL; 663 664 if (tb[CLD80211_ATTR_CMD_TAG_DATA]) { 665 msg_len = nla_len(tb[CLD80211_ATTR_CMD_TAG_DATA]); 666 msg = nla_data(tb[CLD80211_ATTR_CMD_TAG_DATA]); 667 668 if (req->msg_type == WIFI_POS_CMD_OEM_DATA) { 669 if (wifi_pos_parse_nla_oemdata_req(msg_len, msg, req)) { 670 osif_err("parsing oemdata req failed"); 671 return OEM_ERR_INVALID_MESSAGE_LENGTH; 672 } 673 } else { 674 req->buf_len = msg_len; 675 req->buf = msg; 676 } 677 } 678 if (tb[CLD80211_ATTR_META_DATA]) 679 osif_err("meta data dropped. Apps can use CLD80211_ATTR_CMD_TAG_DATA sub attrs"); 680 681 return 0; 682 } 683 684 static int wifi_pos_parse_ani_req(const void *data, int len, int pid, 685 struct wifi_pos_req_msg *req) 686 { 687 tAniMsgHdr *msg_hdr; 688 struct nlattr *tb[CLD80211_ATTR_MAX + 1]; 689 uint32_t msg_len, id, nl_field_info_size, expected_field_info_size; 690 struct wifi_pos_field_info *field_info; 691 692 if (wlan_cfg80211_nla_parse(tb, CLD80211_ATTR_MAX, data, len, NULL)) { 693 osif_err("invalid data in request"); 694 return OEM_ERR_INVALID_MESSAGE_TYPE; 695 } 696 697 msg_len = nla_len(tb[CLD80211_ATTR_DATA]); 698 if (msg_len < sizeof(*msg_hdr)) { 699 osif_err("Insufficient length for msg_hdr: %u", msg_len); 700 return OEM_ERR_INVALID_MESSAGE_LENGTH; 701 } 702 703 msg_hdr = nla_data(tb[CLD80211_ATTR_DATA]); 704 req->msg_type = map_ani_msg_req_to_wifi_pos_cmd( 705 (uint32_t)msg_hdr->type); 706 req->rsp_version = WIFI_POS_RSP_V1_FLAT_MEMORY; 707 708 if (msg_len < sizeof(*msg_hdr) + msg_hdr->length) { 709 osif_err("Insufficient length for msg_hdr buffer: %u", 710 msg_len); 711 return OEM_ERR_INVALID_MESSAGE_LENGTH; 712 } 713 714 req->buf_len = msg_hdr->length; 715 req->buf = (uint8_t *)&msg_hdr[1]; 716 req->pid = pid; 717 718 id = CLD80211_ATTR_META_DATA; 719 if (!tb[id]) 720 return 0; 721 722 nl_field_info_size = nla_len(tb[id]); 723 if (nl_field_info_size < sizeof(*field_info)) { 724 osif_err("Insufficient length for field_info_buf: %u", 725 nl_field_info_size); 726 return OEM_ERR_INVALID_MESSAGE_LENGTH; 727 } 728 729 field_info = nla_data(tb[id]); 730 if (!field_info->count) { 731 osif_debug("field_info->count is zero, ignoring META_DATA"); 732 return 0; 733 } 734 735 if ((field_info->count - 1) > 736 ((UINT_MAX - sizeof(*field_info)) / 737 sizeof(struct wifi_pos_field))) { 738 return OEM_ERR_INVALID_MESSAGE_LENGTH; 739 } 740 741 expected_field_info_size = sizeof(*field_info) + 742 (field_info->count - 1) * sizeof(struct wifi_pos_field); 743 744 if (nl_field_info_size < expected_field_info_size) { 745 osif_err("Insufficient len for total no.of %u fields", 746 field_info->count); 747 return OEM_ERR_INVALID_MESSAGE_LENGTH; 748 } 749 750 req->field_info_buf = field_info; 751 req->field_info_buf_len = nl_field_info_size; 752 753 return 0; 754 } 755 756 757 static int wifi_pos_parse_req(const void *data, int len, int pid, 758 struct wifi_pos_req_msg *req) 759 { 760 int status = 0; 761 struct nlattr *tb[CLD80211_ATTR_MAX + 1]; 762 763 if (wlan_cfg80211_nla_parse(tb, CLD80211_ATTR_MAX, data, len, NULL)) { 764 osif_err("invalid data in request"); 765 return OEM_ERR_INVALID_MESSAGE_TYPE; 766 } 767 768 if (tb[CLD80211_ATTR_DATA]) { 769 status = wifi_pos_parse_ani_req(data, len, pid, req); 770 } else if (tb[CLD80211_ATTR_CMD]) { 771 status = wifi_pos_parse_nla_req(data, len, pid, req); 772 } else { 773 osif_err("Valid CLD80211 ATTR not present"); 774 return OEM_ERR_INVALID_MESSAGE_TYPE; 775 } 776 return status; 777 } 778 #else 779 static int wifi_pos_parse_req(struct sk_buff *skb, struct wifi_pos_req_msg *req, 780 struct wlan_objmgr_psoc **psoc) 781 { 782 /* SKB->data contains NL msg */ 783 /* NLMSG_DATA(nlh) contains ANI msg */ 784 struct nlmsghdr *nlh; 785 tAniMsgHdr *msg_hdr; 786 size_t field_info_len; 787 int interface_len; 788 char *interface = NULL; 789 uint8_t pdev_id = 0; 790 uint32_t tgt_pdev_id = 0; 791 uint8_t i; 792 uint32_t offset; 793 QDF_STATUS status; 794 795 nlh = (struct nlmsghdr *)skb->data; 796 if (!nlh) { 797 osif_err("Netlink header null"); 798 return OEM_ERR_NULL_MESSAGE_HEADER; 799 } 800 801 if (nlh->nlmsg_len < NLMSG_LENGTH(sizeof(*msg_hdr))) { 802 osif_err("nlmsg_len(%d) and msg_hdr_size(%zu) mis-match", 803 nlh->nlmsg_len, sizeof(*msg_hdr)); 804 return OEM_ERR_INVALID_MESSAGE_LENGTH; 805 } 806 807 msg_hdr = NLMSG_DATA(nlh); 808 if (!msg_hdr) { 809 osif_err("Message header null"); 810 return OEM_ERR_NULL_MESSAGE_HEADER; 811 } 812 813 if (nlh->nlmsg_len < NLMSG_LENGTH(sizeof(*msg_hdr) + msg_hdr->length)) { 814 osif_err("nlmsg_len(%d) and animsg_len(%d) mis-match", 815 nlh->nlmsg_len, msg_hdr->length); 816 return OEM_ERR_INVALID_MESSAGE_LENGTH; 817 } 818 819 req->msg_type = map_ani_msg_req_to_wifi_pos_cmd( 820 (uint32_t)msg_hdr->type); 821 req->rsp_version = WIFI_POS_RSP_V1_FLAT_MEMORY; 822 req->buf_len = msg_hdr->length; 823 req->buf = (uint8_t *)&msg_hdr[1]; 824 req->pid = nlh->nlmsg_pid; 825 req->field_info_buf = NULL; 826 req->field_info_buf_len = 0; 827 828 field_info_len = nlh->nlmsg_len - 829 (NLMSG_LENGTH(sizeof(*msg_hdr) + msg_hdr->length + 830 sizeof(struct wifi_pos_interface))); 831 if (field_info_len) { 832 req->field_info_buf = 833 (struct wifi_pos_field_info *)(req->buf + req->buf_len); 834 req->field_info_buf_len = sizeof(struct wifi_pos_field_info); 835 } 836 837 interface_len = nlh->nlmsg_len - 838 (NLMSG_LENGTH(sizeof(*msg_hdr) + msg_hdr->length + 839 req->field_info_buf_len)); 840 841 if (interface_len) { 842 interface = (char *)(req->buf + req->buf_len + 843 req->field_info_buf_len); 844 req->interface.length = *interface; 845 846 if (req->interface.length > sizeof(req->interface.dev_name)) { 847 osif_err("interface length exceeds array length"); 848 return OEM_ERR_INVALID_MESSAGE_LENGTH; 849 } 850 851 qdf_mem_copy(req->interface.dev_name, 852 (interface + sizeof(uint8_t)), 853 req->interface.length); 854 855 status = ucfg_wifi_psoc_get_pdev_id_by_dev_name( 856 req->interface.dev_name, &pdev_id, psoc); 857 if (QDF_IS_STATUS_ERROR(status)) { 858 osif_err("failed to get pdev_id and psoc"); 859 return OEM_ERR_NULL_CONTEXT; 860 } 861 862 status = wifi_pos_convert_host_pdev_id_to_target( 863 *psoc, pdev_id, &tgt_pdev_id); 864 if (QDF_IS_STATUS_ERROR(status)) { 865 osif_err("failed to get target pdev_id"); 866 return OEM_ERR_NULL_CONTEXT; 867 } 868 869 for (i = 0; 870 (req->field_info_buf && (i < req->field_info_buf->count)); 871 i++) { 872 if (req->field_info_buf->fields[i].id == 873 META_DATA_PDEV) { 874 offset = req->field_info_buf->fields[i].offset; 875 *((uint32_t *)&req->buf[offset]) = tgt_pdev_id; 876 } 877 } 878 } 879 880 return 0; 881 } 882 #endif 883 884 /** 885 * __os_if_wifi_pos_callback() - callback registered with NL service socket to 886 * process wifi pos request 887 * @skb: request message sk_buff 888 * 889 * Return: status of operation 890 */ 891 #ifdef CNSS_GENL 892 static void __os_if_wifi_pos_callback(const void *data, int data_len, 893 void *ctx, int pid) 894 { 895 uint8_t err; 896 QDF_STATUS status; 897 struct wifi_pos_req_msg req = {0}; 898 struct wlan_objmgr_psoc *psoc = wifi_pos_get_psoc(); 899 900 osif_debug("enter: pid %d", pid); 901 if (!psoc) { 902 osif_err("global psoc object not registered yet."); 903 return; 904 } 905 906 wlan_objmgr_psoc_get_ref(psoc, WLAN_WIFI_POS_OSIF_ID); 907 err = wifi_pos_parse_req(data, data_len, pid, &req); 908 if (err) { 909 os_if_wifi_pos_send_rsp(psoc, wifi_pos_get_app_pid(psoc), 910 WIFI_POS_CMD_ERROR, sizeof(err), &err); 911 status = QDF_STATUS_E_INVAL; 912 goto release_psoc_ref; 913 } 914 915 status = ucfg_wifi_pos_process_req(psoc, &req, os_if_wifi_pos_send_rsp); 916 if (QDF_IS_STATUS_ERROR(status)) 917 osif_err("ucfg_wifi_pos_process_req failed. status: %d", 918 status); 919 920 release_psoc_ref: 921 wlan_objmgr_psoc_release_ref(psoc, WLAN_WIFI_POS_OSIF_ID); 922 } 923 924 static void os_if_wifi_pos_callback(const void *data, int data_len, 925 void *ctx, int pid) 926 { 927 struct qdf_op_sync *op_sync; 928 929 if (qdf_op_protect(&op_sync)) 930 return; 931 932 __os_if_wifi_pos_callback(data, data_len, ctx, pid); 933 qdf_op_unprotect(op_sync); 934 } 935 #else 936 static int __os_if_wifi_pos_callback(struct sk_buff *skb) 937 { 938 uint8_t err; 939 QDF_STATUS status; 940 struct wifi_pos_req_msg req = {0}; 941 struct wlan_objmgr_psoc *psoc = NULL; 942 943 osif_debug("enter"); 944 945 err = wifi_pos_parse_req(skb, &req, &psoc); 946 if (err) { 947 osif_err("wifi_pos_parse_req failed"); 948 return -EINVAL; 949 } 950 951 wlan_objmgr_psoc_get_ref(psoc, WLAN_WIFI_POS_OSIF_ID); 952 953 if (err) { 954 os_if_wifi_pos_send_rsp(psoc, wifi_pos_get_app_pid(psoc), 955 WIFI_POS_CMD_ERROR, sizeof(err), &err); 956 status = QDF_STATUS_E_INVAL; 957 goto release_psoc_ref; 958 } 959 960 status = ucfg_wifi_pos_process_req(psoc, &req, os_if_wifi_pos_send_rsp); 961 if (QDF_IS_STATUS_ERROR(status)) 962 osif_err("ucfg_wifi_pos_process_req failed. status: %d", 963 status); 964 965 release_psoc_ref: 966 wlan_objmgr_psoc_release_ref(psoc, WLAN_WIFI_POS_OSIF_ID); 967 968 return qdf_status_to_os_return(status); 969 } 970 971 static int os_if_wifi_pos_callback(struct sk_buff *skb) 972 { 973 struct qdf_op_sync *op_sync; 974 int err; 975 976 if (qdf_op_protect(&op_sync)) 977 return -EINVAL; 978 979 err = __os_if_wifi_pos_callback(skb); 980 qdf_op_unprotect(op_sync); 981 982 return err; 983 } 984 #endif 985 986 #ifdef CNSS_GENL 987 int os_if_wifi_pos_register_nl(void) 988 { 989 int ret = register_cld_cmd_cb(WLAN_NL_MSG_OEM, 990 os_if_wifi_pos_callback, NULL); 991 if (ret) 992 osif_err("register_cld_cmd_cb failed"); 993 994 return ret; 995 } 996 #else 997 int os_if_wifi_pos_register_nl(void) 998 { 999 return nl_srv_register(WLAN_NL_MSG_OEM, os_if_wifi_pos_callback); 1000 } 1001 #endif /* CNSS_GENL */ 1002 qdf_export_symbol(os_if_wifi_pos_register_nl); 1003 1004 #ifdef CNSS_GENL 1005 int os_if_wifi_pos_deregister_nl(void) 1006 { 1007 int ret = deregister_cld_cmd_cb(WLAN_NL_MSG_OEM); 1008 if (ret) 1009 osif_err("deregister_cld_cmd_cb failed"); 1010 1011 return ret; 1012 } 1013 #else 1014 int os_if_wifi_pos_deregister_nl(void) 1015 { 1016 return 0; 1017 } 1018 #endif /* CNSS_GENL */ 1019 1020 void os_if_wifi_pos_send_peer_status(struct qdf_mac_addr *peer_mac, 1021 uint8_t peer_status, 1022 uint8_t peer_timing_meas_cap, 1023 uint8_t session_id, 1024 struct wifi_pos_ch_info *chan_info, 1025 enum QDF_OPMODE dev_mode) 1026 { 1027 struct wlan_objmgr_psoc *psoc = wifi_pos_get_psoc(); 1028 struct wifi_pos_peer_status_info *peer_info; 1029 1030 if (!psoc) { 1031 osif_err("global wifi_pos psoc object not registered"); 1032 return; 1033 } 1034 1035 if (!wifi_pos_is_app_registered(psoc) || 1036 wifi_pos_get_app_pid(psoc) == 0) { 1037 osif_debug("app is not registered or pid is invalid"); 1038 return; 1039 } 1040 1041 peer_info = qdf_mem_malloc(sizeof(*peer_info)); 1042 if (!peer_info) 1043 return; 1044 1045 qdf_mem_copy(peer_info->peer_mac_addr, peer_mac->bytes, 1046 sizeof(peer_mac->bytes)); 1047 peer_info->peer_status = peer_status; 1048 peer_info->vdev_id = session_id; 1049 peer_info->peer_capability = peer_timing_meas_cap; 1050 peer_info->reserved0 = 0; 1051 /* Set 0th bit of reserved0 for STA mode */ 1052 if (QDF_STA_MODE == dev_mode) 1053 peer_info->reserved0 |= 0x01; 1054 1055 if (chan_info) { 1056 peer_info->peer_chan_info.chan_id = chan_info->chan_id; 1057 peer_info->peer_chan_info.reserved0 = 0; 1058 peer_info->peer_chan_info.mhz = chan_info->mhz; 1059 peer_info->peer_chan_info.band_center_freq1 = 1060 chan_info->band_center_freq1; 1061 peer_info->peer_chan_info.band_center_freq2 = 1062 chan_info->band_center_freq2; 1063 peer_info->peer_chan_info.info = chan_info->info; 1064 peer_info->peer_chan_info.reg_info_1 = chan_info->reg_info_1; 1065 peer_info->peer_chan_info.reg_info_2 = chan_info->reg_info_2; 1066 } 1067 1068 os_if_wifi_pos_send_rsp(psoc, wifi_pos_get_app_pid(psoc), 1069 WIFI_POS_PEER_STATUS_IND, 1070 sizeof(*peer_info), (uint8_t *)peer_info); 1071 qdf_mem_free(peer_info); 1072 } 1073 1074 int os_if_wifi_pos_populate_caps(struct wlan_objmgr_psoc *psoc, 1075 struct wifi_pos_driver_caps *caps) 1076 { 1077 if (!psoc || !caps) { 1078 osif_err("psoc or caps buffer is null"); 1079 return -EINVAL; 1080 } 1081 1082 return qdf_status_to_os_return(wifi_pos_populate_caps(psoc, caps)); 1083 } 1084