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