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(uint32_t pid, enum wifi_pos_cmd_ids cmd, 575 uint32_t buf_len, uint8_t *buf) 576 { 577 tAniMsgHdr *aniHdr; 578 struct sk_buff *skb = NULL; 579 struct nlmsghdr *nlh; 580 struct wlan_objmgr_psoc *psoc = wifi_pos_get_psoc(); 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 { 771 /* SKB->data contains NL msg */ 772 /* NLMSG_DATA(nlh) contains ANI msg */ 773 struct nlmsghdr *nlh; 774 tAniMsgHdr *msg_hdr; 775 size_t field_info_len; 776 777 nlh = (struct nlmsghdr *)skb->data; 778 if (!nlh) { 779 osif_err("Netlink header null"); 780 return OEM_ERR_NULL_MESSAGE_HEADER; 781 } 782 783 if (nlh->nlmsg_len < NLMSG_LENGTH(sizeof(*msg_hdr))) { 784 osif_err("nlmsg_len(%d) and msg_hdr_size(%zu) mis-match", 785 nlh->nlmsg_len, sizeof(*msg_hdr)); 786 return OEM_ERR_INVALID_MESSAGE_LENGTH; 787 } 788 789 msg_hdr = NLMSG_DATA(nlh); 790 if (!msg_hdr) { 791 osif_err("Message header null"); 792 return OEM_ERR_NULL_MESSAGE_HEADER; 793 } 794 795 if (nlh->nlmsg_len < NLMSG_LENGTH(sizeof(*msg_hdr) + msg_hdr->length)) { 796 osif_err("nlmsg_len(%d) and animsg_len(%d) mis-match", 797 nlh->nlmsg_len, msg_hdr->length); 798 return OEM_ERR_INVALID_MESSAGE_LENGTH; 799 } 800 801 req->msg_type = map_ani_msg_req_to_wifi_pos_cmd( 802 (uint32_t)msg_hdr->type); 803 req->rsp_version = WIFI_POS_RSP_V1_FLAT_MEMORY; 804 req->buf_len = msg_hdr->length; 805 req->buf = (uint8_t *)&msg_hdr[1]; 806 req->pid = nlh->nlmsg_pid; 807 req->field_info_buf = NULL; 808 809 field_info_len = nlh->nlmsg_len - 810 (NLMSG_LENGTH(sizeof(*msg_hdr) + msg_hdr->length)); 811 if (field_info_len) { 812 req->field_info_buf = (struct wifi_pos_field_info *) 813 (req->buf + req->buf_len); 814 req->field_info_buf_len = field_info_len; 815 } 816 817 return 0; 818 } 819 #endif 820 821 /** 822 * __os_if_wifi_pos_callback() - callback registered with NL service socket to 823 * process wifi pos request 824 * @skb: request message sk_buff 825 * 826 * Return: status of operation 827 */ 828 #ifdef CNSS_GENL 829 static void __os_if_wifi_pos_callback(const void *data, int data_len, 830 void *ctx, int pid) 831 { 832 uint8_t err; 833 QDF_STATUS status; 834 struct wifi_pos_req_msg req = {0}; 835 struct wlan_objmgr_psoc *psoc = wifi_pos_get_psoc(); 836 837 osif_debug("enter: pid %d", pid); 838 if (!psoc) { 839 osif_err("global psoc object not registered yet."); 840 return; 841 } 842 843 wlan_objmgr_psoc_get_ref(psoc, WLAN_WIFI_POS_OSIF_ID); 844 err = wifi_pos_parse_req(data, data_len, pid, &req); 845 if (err) { 846 os_if_wifi_pos_send_rsp(wifi_pos_get_app_pid(psoc), 847 WIFI_POS_CMD_ERROR, sizeof(err), &err); 848 status = QDF_STATUS_E_INVAL; 849 goto release_psoc_ref; 850 } 851 852 status = ucfg_wifi_pos_process_req(psoc, &req, os_if_wifi_pos_send_rsp); 853 if (QDF_IS_STATUS_ERROR(status)) 854 osif_err("ucfg_wifi_pos_process_req failed. status: %d", 855 status); 856 857 release_psoc_ref: 858 wlan_objmgr_psoc_release_ref(psoc, WLAN_WIFI_POS_OSIF_ID); 859 } 860 861 static void os_if_wifi_pos_callback(const void *data, int data_len, 862 void *ctx, int pid) 863 { 864 struct qdf_op_sync *op_sync; 865 866 if (qdf_op_protect(&op_sync)) 867 return; 868 869 __os_if_wifi_pos_callback(data, data_len, ctx, pid); 870 qdf_op_unprotect(op_sync); 871 } 872 #else 873 static int __os_if_wifi_pos_callback(struct sk_buff *skb) 874 { 875 uint8_t err; 876 QDF_STATUS status; 877 struct wifi_pos_req_msg req = {0}; 878 struct wlan_objmgr_psoc *psoc = wifi_pos_get_psoc(); 879 880 osif_debug("enter"); 881 if (!psoc) { 882 osif_err("global psoc object not registered yet."); 883 return -EINVAL; 884 } 885 886 wlan_objmgr_psoc_get_ref(psoc, WLAN_WIFI_POS_OSIF_ID); 887 err = wifi_pos_parse_req(skb, &req); 888 if (err) { 889 os_if_wifi_pos_send_rsp(wifi_pos_get_app_pid(psoc), 890 WIFI_POS_CMD_ERROR, sizeof(err), &err); 891 status = QDF_STATUS_E_INVAL; 892 goto release_psoc_ref; 893 } 894 895 status = ucfg_wifi_pos_process_req(psoc, &req, os_if_wifi_pos_send_rsp); 896 if (QDF_IS_STATUS_ERROR(status)) 897 osif_err("ucfg_wifi_pos_process_req failed. status: %d", 898 status); 899 900 release_psoc_ref: 901 wlan_objmgr_psoc_release_ref(psoc, WLAN_WIFI_POS_OSIF_ID); 902 903 return qdf_status_to_os_return(status); 904 } 905 906 static int os_if_wifi_pos_callback(struct sk_buff *skb) 907 { 908 struct qdf_op_sync *op_sync; 909 int err; 910 911 if (qdf_op_protect(&op_sync)) 912 return -EINVAL; 913 914 err = __os_if_wifi_pos_callback(skb); 915 qdf_op_unprotect(op_sync); 916 917 return err; 918 } 919 #endif 920 921 #ifdef CNSS_GENL 922 int os_if_wifi_pos_register_nl(void) 923 { 924 int ret = register_cld_cmd_cb(WLAN_NL_MSG_OEM, 925 os_if_wifi_pos_callback, NULL); 926 if (ret) 927 osif_err("register_cld_cmd_cb failed"); 928 929 return ret; 930 } 931 #else 932 int os_if_wifi_pos_register_nl(void) 933 { 934 return nl_srv_register(WLAN_NL_MSG_OEM, os_if_wifi_pos_callback); 935 } 936 #endif /* CNSS_GENL */ 937 qdf_export_symbol(os_if_wifi_pos_register_nl); 938 939 #ifdef CNSS_GENL 940 int os_if_wifi_pos_deregister_nl(void) 941 { 942 int ret = deregister_cld_cmd_cb(WLAN_NL_MSG_OEM); 943 if (ret) 944 osif_err("deregister_cld_cmd_cb failed"); 945 946 return ret; 947 } 948 #else 949 int os_if_wifi_pos_deregister_nl(void) 950 { 951 return 0; 952 } 953 #endif /* CNSS_GENL */ 954 955 void os_if_wifi_pos_send_peer_status(struct qdf_mac_addr *peer_mac, 956 uint8_t peer_status, 957 uint8_t peer_timing_meas_cap, 958 uint8_t session_id, 959 struct wifi_pos_ch_info *chan_info, 960 enum QDF_OPMODE dev_mode) 961 { 962 struct wlan_objmgr_psoc *psoc = wifi_pos_get_psoc(); 963 struct wifi_pos_peer_status_info *peer_info; 964 965 if (!psoc) { 966 osif_err("global wifi_pos psoc object not registered"); 967 return; 968 } 969 970 if (!wifi_pos_is_app_registered(psoc) || 971 wifi_pos_get_app_pid(psoc) == 0) { 972 osif_debug("app is not registered or pid is invalid"); 973 return; 974 } 975 976 peer_info = qdf_mem_malloc(sizeof(*peer_info)); 977 if (!peer_info) 978 return; 979 980 qdf_mem_copy(peer_info->peer_mac_addr, peer_mac->bytes, 981 sizeof(peer_mac->bytes)); 982 peer_info->peer_status = peer_status; 983 peer_info->vdev_id = session_id; 984 peer_info->peer_capability = peer_timing_meas_cap; 985 peer_info->reserved0 = 0; 986 /* Set 0th bit of reserved0 for STA mode */ 987 if (QDF_STA_MODE == dev_mode) 988 peer_info->reserved0 |= 0x01; 989 990 if (chan_info) { 991 peer_info->peer_chan_info.chan_id = chan_info->chan_id; 992 peer_info->peer_chan_info.reserved0 = 0; 993 peer_info->peer_chan_info.mhz = chan_info->mhz; 994 peer_info->peer_chan_info.band_center_freq1 = 995 chan_info->band_center_freq1; 996 peer_info->peer_chan_info.band_center_freq2 = 997 chan_info->band_center_freq2; 998 peer_info->peer_chan_info.info = chan_info->info; 999 peer_info->peer_chan_info.reg_info_1 = chan_info->reg_info_1; 1000 peer_info->peer_chan_info.reg_info_2 = chan_info->reg_info_2; 1001 } 1002 1003 os_if_wifi_pos_send_rsp(wifi_pos_get_app_pid(psoc), 1004 WIFI_POS_PEER_STATUS_IND, 1005 sizeof(*peer_info), (uint8_t *)peer_info); 1006 qdf_mem_free(peer_info); 1007 } 1008 1009 int os_if_wifi_pos_populate_caps(struct wlan_objmgr_psoc *psoc, 1010 struct wifi_pos_driver_caps *caps) 1011 { 1012 if (!psoc || !caps) { 1013 osif_err("psoc or caps buffer is null"); 1014 return -EINVAL; 1015 } 1016 1017 return qdf_status_to_os_return(wifi_pos_populate_caps(psoc, caps)); 1018 } 1019