503 lines · c
1// SPDX-License-Identifier: BSD-3-Clause-Clear2/*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 18static 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 27static 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 aggregated45 * in application layer, this event is unsegmented from host perspective.46 */47static 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 88out: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 received94 * from firmware is aggregated and sent to application layer95 */96static 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 188out:189 spin_unlock_bh(&ar->data_lock);190 return ret;191}192 193static 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, 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 223void 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 else228 ath11k_tm_wmi_event_unsegmented(ab, cmd_id, skb);229}230 231static 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 263static 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 295err:296 mutex_unlock(&ar->conf_mutex);297 return ret;298}299 300static 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 to334 * 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 = ath11k_vif_to_arvif(vif);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 385out:386 mutex_unlock(&ar->conf_mutex);387 return ret;388}389 390static 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 470out:471 mutex_unlock(&ar->conf_mutex);472 return ret;473}474 475int 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