1 // SPDX-License-Identifier: BSD-3-Clause-Clear 2 /* 3 * Copyright (c) 2018-2019 The Linux Foundation. All rights reserved. 4 * Copyright (c) 2023 Qualcomm Innovation Center, Inc. All rights reserved. 5 */ 6 7 #include "testmode.h" 8 #include <net/netlink.h> 9 #include "debug.h" 10 #include "wmi.h" 11 #include "hw.h" 12 #include "core.h" 13 #include "testmode_i.h" 14 15 #define ATH11K_FTM_SEGHDR_CURRENT_SEQ GENMASK(3, 0) 16 #define ATH11K_FTM_SEGHDR_TOTAL_SEGMENTS GENMASK(7, 4) 17 18 static const struct nla_policy ath11k_tm_policy[ATH11K_TM_ATTR_MAX + 1] = { 19 [ATH11K_TM_ATTR_CMD] = { .type = NLA_U32 }, 20 [ATH11K_TM_ATTR_DATA] = { .type = NLA_BINARY, 21 .len = ATH11K_TM_DATA_MAX_LEN }, 22 [ATH11K_TM_ATTR_WMI_CMDID] = { .type = NLA_U32 }, 23 [ATH11K_TM_ATTR_VERSION_MAJOR] = { .type = NLA_U32 }, 24 [ATH11K_TM_ATTR_VERSION_MINOR] = { .type = NLA_U32 }, 25 }; 26 27 static struct ath11k *ath11k_tm_get_ar(struct ath11k_base *ab) 28 { 29 struct ath11k_pdev *pdev; 30 struct ath11k *ar = NULL; 31 int i; 32 33 for (i = 0; i < ab->num_radios; i++) { 34 pdev = &ab->pdevs[i]; 35 ar = pdev->ar; 36 37 if (ar && ar->state == ATH11K_STATE_FTM) 38 break; 39 } 40 41 return ar; 42 } 43 44 /* This function handles unsegmented events. Data in various events are aggregated 45 * in application layer, this event is unsegmented from host perspective. 46 */ 47 static void ath11k_tm_wmi_event_unsegmented(struct ath11k_base *ab, u32 cmd_id, 48 struct sk_buff *skb) 49 { 50 struct sk_buff *nl_skb; 51 struct ath11k *ar; 52 53 ath11k_dbg(ab, ATH11K_DBG_TESTMODE, 54 "event wmi cmd_id %d skb length %d\n", 55 cmd_id, skb->len); 56 ath11k_dbg_dump(ab, ATH11K_DBG_TESTMODE, NULL, "", skb->data, skb->len); 57 58 ar = ath11k_tm_get_ar(ab); 59 if (!ar) { 60 ath11k_warn(ab, "testmode event not handled due to invalid pdev\n"); 61 return; 62 } 63 64 spin_lock_bh(&ar->data_lock); 65 66 nl_skb = cfg80211_testmode_alloc_event_skb(ar->hw->wiphy, 67 2 * nla_total_size(sizeof(u32)) + 68 nla_total_size(skb->len), 69 GFP_ATOMIC); 70 if (!nl_skb) { 71 ath11k_warn(ab, 72 "failed to allocate skb for unsegmented testmode wmi event\n"); 73 goto out; 74 } 75 76 if (nla_put_u32(nl_skb, ATH11K_TM_ATTR_CMD, ATH11K_TM_CMD_WMI) || 77 nla_put_u32(nl_skb, ATH11K_TM_ATTR_WMI_CMDID, cmd_id) || 78 nla_put(nl_skb, ATH11K_TM_ATTR_DATA, skb->len, skb->data)) { 79 ath11k_warn(ab, "failed to populate testmode unsegmented event\n"); 80 kfree_skb(nl_skb); 81 goto out; 82 } 83 84 cfg80211_testmode_event(nl_skb, GFP_ATOMIC); 85 spin_unlock_bh(&ar->data_lock); 86 return; 87 88 out: 89 spin_unlock_bh(&ar->data_lock); 90 ath11k_warn(ab, "Failed to send testmode event to higher layers\n"); 91 } 92 93 /* This function handles segmented events. Data of various events received 94 * from firmware is aggregated and sent to application layer 95 */ 96 static int ath11k_tm_process_event(struct ath11k_base *ab, u32 cmd_id, 97 const struct wmi_ftm_event_msg *ftm_msg, 98 u16 length) 99 { 100 struct sk_buff *nl_skb; 101 int ret = 0; 102 struct ath11k *ar; 103 u8 const *buf_pos; 104 u16 datalen; 105 u8 total_segments, current_seq; 106 u32 data_pos; 107 u32 pdev_id; 108 109 ath11k_dbg(ab, ATH11K_DBG_TESTMODE, 110 "event wmi cmd_id %d ftm event msg %pK datalen %d\n", 111 cmd_id, ftm_msg, length); 112 ath11k_dbg_dump(ab, ATH11K_DBG_TESTMODE, NULL, "", ftm_msg, length); 113 pdev_id = DP_HW2SW_MACID(ftm_msg->seg_hdr.pdev_id); 114 115 if (pdev_id >= ab->num_radios) { 116 ath11k_warn(ab, "testmode event not handled due to invalid pdev id: %d\n", 117 pdev_id); 118 return -EINVAL; 119 } 120 121 ar = ab->pdevs[pdev_id].ar; 122 if (!ar) { 123 ath11k_warn(ab, "testmode event not handled due to absence of pdev\n"); 124 return -ENODEV; 125 } 126 127 current_seq = FIELD_GET(ATH11K_FTM_SEGHDR_CURRENT_SEQ, 128 ftm_msg->seg_hdr.segmentinfo); 129 total_segments = FIELD_GET(ATH11K_FTM_SEGHDR_TOTAL_SEGMENTS, 130 ftm_msg->seg_hdr.segmentinfo); 131 datalen = length - (sizeof(struct wmi_ftm_seg_hdr)); 132 buf_pos = ftm_msg->data; 133 134 spin_lock_bh(&ar->data_lock); 135 136 if (current_seq == 0) { 137 ab->testmode.expected_seq = 0; 138 ab->testmode.data_pos = 0; 139 } 140 141 data_pos = ab->testmode.data_pos; 142 143 if ((data_pos + datalen) > ATH11K_FTM_EVENT_MAX_BUF_LENGTH) { 144 ath11k_warn(ab, "Invalid ftm event length at %d: %d\n", 145 data_pos, datalen); 146 ret = -EINVAL; 147 goto out; 148 } 149 150 memcpy(&ab->testmode.eventdata[data_pos], buf_pos, datalen); 151 data_pos += datalen; 152 153 if (++ab->testmode.expected_seq != total_segments) { 154 ab->testmode.data_pos = data_pos; 155 ath11k_dbg(ab, ATH11K_DBG_TESTMODE, 156 "partial data received current_seq %d total_seg %d\n", 157 current_seq, total_segments); 158 goto out; 159 } 160 161 ath11k_dbg(ab, ATH11K_DBG_TESTMODE, 162 "total data length pos %d len %d\n", 163 data_pos, ftm_msg->seg_hdr.len); 164 nl_skb = cfg80211_testmode_alloc_event_skb(ar->hw->wiphy, 165 2 * nla_total_size(sizeof(u32)) + 166 nla_total_size(data_pos), 167 GFP_ATOMIC); 168 if (!nl_skb) { 169 ath11k_warn(ab, 170 "failed to allocate skb for segmented testmode wmi event\n"); 171 ret = -ENOMEM; 172 goto out; 173 } 174 175 if (nla_put_u32(nl_skb, ATH11K_TM_ATTR_CMD, 176 ATH11K_TM_CMD_WMI_FTM) || 177 nla_put_u32(nl_skb, ATH11K_TM_ATTR_WMI_CMDID, cmd_id) || 178 nla_put(nl_skb, ATH11K_TM_ATTR_DATA, data_pos, 179 &ab->testmode.eventdata[0])) { 180 ath11k_warn(ab, "failed to populate segmented testmode event"); 181 kfree_skb(nl_skb); 182 ret = -ENOBUFS; 183 goto out; 184 } 185 186 cfg80211_testmode_event(nl_skb, GFP_ATOMIC); 187 188 out: 189 spin_unlock_bh(&ar->data_lock); 190 return ret; 191 } 192 193 static void ath11k_tm_wmi_event_segmented(struct ath11k_base *ab, u32 cmd_id, 194 struct sk_buff *skb) 195 { 196 const void **tb; 197 const struct wmi_ftm_event_msg *ev; 198 u16 length; 199 int ret; 200 201 tb = ath11k_wmi_tlv_parse_alloc(ab, skb->data, skb->len, GFP_ATOMIC); 202 if (IS_ERR(tb)) { 203 ret = PTR_ERR(tb); 204 ath11k_warn(ab, "failed to parse ftm event tlv: %d\n", ret); 205 return; 206 } 207 208 ev = tb[WMI_TAG_ARRAY_BYTE]; 209 if (!ev) { 210 ath11k_warn(ab, "failed to fetch ftm msg\n"); 211 kfree(tb); 212 return; 213 } 214 215 length = skb->len - TLV_HDR_SIZE; 216 ret = ath11k_tm_process_event(ab, cmd_id, ev, length); 217 if (ret) 218 ath11k_warn(ab, "Failed to process ftm event\n"); 219 220 kfree(tb); 221 } 222 223 void ath11k_tm_wmi_event(struct ath11k_base *ab, u32 cmd_id, struct sk_buff *skb) 224 { 225 if (test_bit(ATH11K_FLAG_FTM_SEGMENTED, &ab->dev_flags)) 226 ath11k_tm_wmi_event_segmented(ab, cmd_id, skb); 227 else 228 ath11k_tm_wmi_event_unsegmented(ab, cmd_id, skb); 229 } 230 231 static int ath11k_tm_cmd_get_version(struct ath11k *ar, struct nlattr *tb[]) 232 { 233 struct sk_buff *skb; 234 int ret; 235 236 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE, 237 "cmd get version_major %d version_minor %d\n", 238 ATH11K_TESTMODE_VERSION_MAJOR, 239 ATH11K_TESTMODE_VERSION_MINOR); 240 241 skb = cfg80211_testmode_alloc_reply_skb(ar->hw->wiphy, 242 nla_total_size(sizeof(u32))); 243 if (!skb) 244 return -ENOMEM; 245 246 ret = nla_put_u32(skb, ATH11K_TM_ATTR_VERSION_MAJOR, 247 ATH11K_TESTMODE_VERSION_MAJOR); 248 if (ret) { 249 kfree_skb(skb); 250 return ret; 251 } 252 253 ret = nla_put_u32(skb, ATH11K_TM_ATTR_VERSION_MINOR, 254 ATH11K_TESTMODE_VERSION_MINOR); 255 if (ret) { 256 kfree_skb(skb); 257 return ret; 258 } 259 260 return cfg80211_testmode_reply(skb); 261 } 262 263 static int ath11k_tm_cmd_testmode_start(struct ath11k *ar, struct nlattr *tb[]) 264 { 265 int ret; 266 267 mutex_lock(&ar->conf_mutex); 268 269 if (ar->state == ATH11K_STATE_FTM) { 270 ret = -EALREADY; 271 goto err; 272 } 273 274 /* start utf only when the driver is not in use */ 275 if (ar->state != ATH11K_STATE_OFF) { 276 ret = -EBUSY; 277 goto err; 278 } 279 280 ar->ab->testmode.eventdata = kzalloc(ATH11K_FTM_EVENT_MAX_BUF_LENGTH, 281 GFP_KERNEL); 282 if (!ar->ab->testmode.eventdata) { 283 ret = -ENOMEM; 284 goto err; 285 } 286 287 ar->state = ATH11K_STATE_FTM; 288 ar->ftm_msgref = 0; 289 290 mutex_unlock(&ar->conf_mutex); 291 292 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE, "cmd start\n"); 293 return 0; 294 295 err: 296 mutex_unlock(&ar->conf_mutex); 297 return ret; 298 } 299 300 static int ath11k_tm_cmd_wmi(struct ath11k *ar, struct nlattr *tb[], 301 struct ieee80211_vif *vif) 302 { 303 struct ath11k_pdev_wmi *wmi = ar->wmi; 304 struct sk_buff *skb; 305 struct ath11k_vif *arvif; 306 u32 cmd_id, buf_len; 307 int ret, tag; 308 void *buf; 309 u32 *ptr; 310 311 mutex_lock(&ar->conf_mutex); 312 313 if (!tb[ATH11K_TM_ATTR_DATA]) { 314 ret = -EINVAL; 315 goto out; 316 } 317 318 if (!tb[ATH11K_TM_ATTR_WMI_CMDID]) { 319 ret = -EINVAL; 320 goto out; 321 } 322 323 buf = nla_data(tb[ATH11K_TM_ATTR_DATA]); 324 buf_len = nla_len(tb[ATH11K_TM_ATTR_DATA]); 325 if (!buf_len) { 326 ath11k_warn(ar->ab, "No data present in testmode wmi command\n"); 327 ret = -EINVAL; 328 goto out; 329 } 330 331 cmd_id = nla_get_u32(tb[ATH11K_TM_ATTR_WMI_CMDID]); 332 333 /* Make sure that the buffer length is long enough to 334 * hold TLV and pdev/vdev id. 335 */ 336 if (buf_len < sizeof(struct wmi_tlv) + sizeof(u32)) { 337 ret = -EINVAL; 338 goto out; 339 } 340 341 ptr = buf; 342 tag = FIELD_GET(WMI_TLV_TAG, *ptr); 343 344 /* pdev/vdev id start after TLV header */ 345 ptr++; 346 347 if (tag == WMI_TAG_PDEV_SET_PARAM_CMD) 348 *ptr = ar->pdev->pdev_id; 349 350 if (ar->ab->fw_mode != ATH11K_FIRMWARE_MODE_FTM && 351 (tag == WMI_TAG_VDEV_SET_PARAM_CMD || tag == WMI_TAG_UNIT_TEST_CMD)) { 352 if (vif) { 353 arvif = (struct ath11k_vif *)vif->drv_priv; 354 *ptr = arvif->vdev_id; 355 } else { 356 ret = -EINVAL; 357 goto out; 358 } 359 } 360 361 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE, 362 "cmd wmi cmd_id %d buf length %d\n", 363 cmd_id, buf_len); 364 365 ath11k_dbg_dump(ar->ab, ATH11K_DBG_TESTMODE, NULL, "", buf, buf_len); 366 367 skb = ath11k_wmi_alloc_skb(wmi->wmi_ab, buf_len); 368 if (!skb) { 369 ret = -ENOMEM; 370 goto out; 371 } 372 373 memcpy(skb->data, buf, buf_len); 374 375 ret = ath11k_wmi_cmd_send(wmi, skb, cmd_id); 376 if (ret) { 377 dev_kfree_skb(skb); 378 ath11k_warn(ar->ab, "failed to transmit wmi command (testmode): %d\n", 379 ret); 380 goto out; 381 } 382 383 ret = 0; 384 385 out: 386 mutex_unlock(&ar->conf_mutex); 387 return ret; 388 } 389 390 static int ath11k_tm_cmd_wmi_ftm(struct ath11k *ar, struct nlattr *tb[]) 391 { 392 struct ath11k_pdev_wmi *wmi = ar->wmi; 393 struct ath11k_base *ab = ar->ab; 394 struct sk_buff *skb; 395 u32 cmd_id, buf_len, hdr_info; 396 int ret; 397 void *buf; 398 u8 segnumber = 0, seginfo; 399 u16 chunk_len, total_bytes, num_segments; 400 u8 *bufpos; 401 struct wmi_ftm_cmd *ftm_cmd; 402 403 set_bit(ATH11K_FLAG_FTM_SEGMENTED, &ab->dev_flags); 404 405 mutex_lock(&ar->conf_mutex); 406 407 if (ar->state != ATH11K_STATE_FTM) { 408 ret = -ENETDOWN; 409 goto out; 410 } 411 412 if (!tb[ATH11K_TM_ATTR_DATA]) { 413 ret = -EINVAL; 414 goto out; 415 } 416 417 buf = nla_data(tb[ATH11K_TM_ATTR_DATA]); 418 buf_len = nla_len(tb[ATH11K_TM_ATTR_DATA]); 419 cmd_id = WMI_PDEV_UTF_CMDID; 420 421 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE, 422 "cmd wmi ftm cmd_id %d buffer length %d\n", 423 cmd_id, buf_len); 424 ath11k_dbg_dump(ar->ab, ATH11K_DBG_TESTMODE, NULL, "", buf, buf_len); 425 426 bufpos = buf; 427 total_bytes = buf_len; 428 num_segments = total_bytes / MAX_WMI_UTF_LEN; 429 430 if (buf_len - (num_segments * MAX_WMI_UTF_LEN)) 431 num_segments++; 432 433 while (buf_len) { 434 chunk_len = min_t(u16, buf_len, MAX_WMI_UTF_LEN); 435 436 skb = ath11k_wmi_alloc_skb(wmi->wmi_ab, (chunk_len + 437 sizeof(struct wmi_ftm_cmd))); 438 if (!skb) { 439 ret = -ENOMEM; 440 goto out; 441 } 442 443 ftm_cmd = (struct wmi_ftm_cmd *)skb->data; 444 hdr_info = FIELD_PREP(WMI_TLV_TAG, WMI_TAG_ARRAY_BYTE) | 445 FIELD_PREP(WMI_TLV_LEN, (chunk_len + 446 sizeof(struct wmi_ftm_seg_hdr))); 447 ftm_cmd->tlv_header = hdr_info; 448 ftm_cmd->seg_hdr.len = total_bytes; 449 ftm_cmd->seg_hdr.msgref = ar->ftm_msgref; 450 seginfo = FIELD_PREP(ATH11K_FTM_SEGHDR_TOTAL_SEGMENTS, num_segments) | 451 FIELD_PREP(ATH11K_FTM_SEGHDR_CURRENT_SEQ, segnumber); 452 ftm_cmd->seg_hdr.segmentinfo = seginfo; 453 segnumber++; 454 455 memcpy(&ftm_cmd->data, bufpos, chunk_len); 456 457 ret = ath11k_wmi_cmd_send(wmi, skb, cmd_id); 458 if (ret) { 459 ath11k_warn(ar->ab, "failed to send wmi ftm command: %d\n", ret); 460 goto out; 461 } 462 463 buf_len -= chunk_len; 464 bufpos += chunk_len; 465 } 466 467 ar->ftm_msgref++; 468 ret = 0; 469 470 out: 471 mutex_unlock(&ar->conf_mutex); 472 return ret; 473 } 474 475 int ath11k_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif, 476 void *data, int len) 477 { 478 struct ath11k *ar = hw->priv; 479 struct nlattr *tb[ATH11K_TM_ATTR_MAX + 1]; 480 int ret; 481 482 ret = nla_parse(tb, ATH11K_TM_ATTR_MAX, data, len, ath11k_tm_policy, 483 NULL); 484 if (ret) 485 return ret; 486 487 if (!tb[ATH11K_TM_ATTR_CMD]) 488 return -EINVAL; 489 490 switch (nla_get_u32(tb[ATH11K_TM_ATTR_CMD])) { 491 case ATH11K_TM_CMD_GET_VERSION: 492 return ath11k_tm_cmd_get_version(ar, tb); 493 case ATH11K_TM_CMD_WMI: 494 return ath11k_tm_cmd_wmi(ar, tb, vif); 495 case ATH11K_TM_CMD_TESTMODE_START: 496 return ath11k_tm_cmd_testmode_start(ar, tb); 497 case ATH11K_TM_CMD_WMI_FTM: 498 return ath11k_tm_cmd_wmi_ftm(ar, tb); 499 default: 500 return -EOPNOTSUPP; 501 } 502 } 503