drivers/net/wireless/ath/ath11k/testmode.c

   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 = 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
 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 }