1 // SPDX-License-Identifier: BSD-3-Clause-Clear 2 /* 3 * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved. 4 * Copyright (c) 2021-2025 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 "hif.h" 14 #include "../testmode_i.h" 15 16 #define ATH12K_FTM_SEGHDR_CURRENT_SEQ GENMASK(3, 0) 17 #define ATH12K_FTM_SEGHDR_TOTAL_SEGMENTS GENMASK(7, 4) 18 19 static const struct nla_policy ath12k_tm_policy[ATH_TM_ATTR_MAX + 1] = { 20 [ATH_TM_ATTR_CMD] = { .type = NLA_U32 }, 21 [ATH_TM_ATTR_DATA] = { .type = NLA_BINARY, 22 .len = ATH_TM_DATA_MAX_LEN }, 23 [ATH_TM_ATTR_WMI_CMDID] = { .type = NLA_U32 }, 24 [ATH_TM_ATTR_VERSION_MAJOR] = { .type = NLA_U32 }, 25 [ATH_TM_ATTR_VERSION_MINOR] = { .type = NLA_U32 }, 26 }; 27 28 static struct ath12k *ath12k_tm_get_ar(struct ath12k_base *ab) 29 { 30 struct ath12k_pdev *pdev; 31 struct ath12k *ar; 32 int i; 33 34 for (i = 0; i < ab->num_radios; i++) { 35 pdev = &ab->pdevs[i]; 36 ar = pdev->ar; 37 38 if (ar && ar->ah->state == ATH12K_HW_STATE_TM) 39 return ar; 40 } 41 42 return NULL; 43 } 44 45 void ath12k_tm_wmi_event_unsegmented(struct ath12k_base *ab, u32 cmd_id, 46 struct sk_buff *skb) 47 { 48 struct sk_buff *nl_skb; 49 struct ath12k *ar; 50 51 ath12k_dbg(ab, ATH12K_DBG_TESTMODE, 52 "testmode event wmi cmd_id %d skb length %d\n", 53 cmd_id, skb->len); 54 55 ath12k_dbg_dump(ab, ATH12K_DBG_TESTMODE, NULL, "", skb->data, skb->len); 56 57 ar = ath12k_tm_get_ar(ab); 58 if (!ar) { 59 ath12k_warn(ab, "testmode event not handled due to invalid pdev\n"); 60 return; 61 } 62 63 spin_lock_bh(&ar->data_lock); 64 65 nl_skb = cfg80211_testmode_alloc_event_skb(ar->ah->hw->wiphy, 66 2 * nla_total_size(sizeof(u32)) + 67 nla_total_size(skb->len), 68 GFP_ATOMIC); 69 spin_unlock_bh(&ar->data_lock); 70 71 if (!nl_skb) { 72 ath12k_warn(ab, 73 "failed to allocate skb for unsegmented testmode wmi event\n"); 74 return; 75 } 76 77 if (nla_put_u32(nl_skb, ATH_TM_ATTR_CMD, ATH_TM_CMD_WMI) || 78 nla_put_u32(nl_skb, ATH_TM_ATTR_WMI_CMDID, cmd_id) || 79 nla_put(nl_skb, ATH_TM_ATTR_DATA, skb->len, skb->data)) { 80 ath12k_warn(ab, "failed to populate testmode unsegmented event\n"); 81 kfree_skb(nl_skb); 82 return; 83 } 84 85 cfg80211_testmode_event(nl_skb, GFP_ATOMIC); 86 } 87 88 void ath12k_tm_process_event(struct ath12k_base *ab, u32 cmd_id, 89 const struct ath12k_wmi_ftm_event *ftm_msg, 90 u16 length) 91 { 92 struct sk_buff *nl_skb; 93 struct ath12k *ar; 94 u32 data_pos, pdev_id; 95 u16 datalen; 96 u8 total_segments, current_seq; 97 u8 const *buf_pos; 98 99 ath12k_dbg(ab, ATH12K_DBG_TESTMODE, 100 "testmode event wmi cmd_id %d ftm event msg %pK datalen %d\n", 101 cmd_id, ftm_msg, length); 102 ath12k_dbg_dump(ab, ATH12K_DBG_TESTMODE, NULL, "", ftm_msg, length); 103 pdev_id = DP_HW2SW_MACID(le32_to_cpu(ftm_msg->seg_hdr.pdev_id)); 104 105 if (pdev_id >= ab->num_radios) { 106 ath12k_warn(ab, "testmode event not handled due to invalid pdev id\n"); 107 return; 108 } 109 110 ar = ab->pdevs[pdev_id].ar; 111 112 if (!ar) { 113 ath12k_warn(ab, "testmode event not handled due to absence of pdev\n"); 114 return; 115 } 116 117 current_seq = le32_get_bits(ftm_msg->seg_hdr.segmentinfo, 118 ATH12K_FTM_SEGHDR_CURRENT_SEQ); 119 total_segments = le32_get_bits(ftm_msg->seg_hdr.segmentinfo, 120 ATH12K_FTM_SEGHDR_TOTAL_SEGMENTS); 121 datalen = length - (sizeof(struct ath12k_wmi_ftm_seg_hdr_params)); 122 buf_pos = ftm_msg->data; 123 124 if (current_seq == 0) { 125 ab->ftm_event_obj.expected_seq = 0; 126 ab->ftm_event_obj.data_pos = 0; 127 } 128 129 data_pos = ab->ftm_event_obj.data_pos; 130 131 if ((data_pos + datalen) > ATH_FTM_EVENT_MAX_BUF_LENGTH) { 132 ath12k_warn(ab, 133 "Invalid event length date_pos[%d] datalen[%d]\n", 134 data_pos, datalen); 135 return; 136 } 137 138 memcpy(&ab->ftm_event_obj.eventdata[data_pos], buf_pos, datalen); 139 data_pos += datalen; 140 141 if (++ab->ftm_event_obj.expected_seq != total_segments) { 142 ab->ftm_event_obj.data_pos = data_pos; 143 ath12k_dbg(ab, ATH12K_DBG_TESTMODE, 144 "partial data received current_seq[%d], total_seg[%d]\n", 145 current_seq, total_segments); 146 return; 147 } 148 149 ath12k_dbg(ab, ATH12K_DBG_TESTMODE, 150 "total data length[%d] = [%d]\n", 151 data_pos, ftm_msg->seg_hdr.len); 152 153 spin_lock_bh(&ar->data_lock); 154 nl_skb = cfg80211_testmode_alloc_event_skb(ar->ah->hw->wiphy, 155 2 * nla_total_size(sizeof(u32)) + 156 nla_total_size(data_pos), 157 GFP_ATOMIC); 158 spin_unlock_bh(&ar->data_lock); 159 160 if (!nl_skb) { 161 ath12k_warn(ab, 162 "failed to allocate skb for testmode wmi event\n"); 163 return; 164 } 165 166 if (nla_put_u32(nl_skb, ATH_TM_ATTR_CMD, 167 ATH_TM_CMD_WMI_FTM) || 168 nla_put_u32(nl_skb, ATH_TM_ATTR_WMI_CMDID, cmd_id) || 169 nla_put(nl_skb, ATH_TM_ATTR_DATA, data_pos, 170 &ab->ftm_event_obj.eventdata[0])) { 171 ath12k_warn(ab, "failed to populate testmode event"); 172 kfree_skb(nl_skb); 173 return; 174 } 175 176 cfg80211_testmode_event(nl_skb, GFP_ATOMIC); 177 } 178 179 static int ath12k_tm_cmd_get_version(struct ath12k *ar, struct nlattr *tb[]) 180 { 181 struct sk_buff *skb; 182 183 ath12k_dbg(ar->ab, ATH12K_DBG_TESTMODE, 184 "testmode cmd get version_major %d version_minor %d\n", 185 ATH_TESTMODE_VERSION_MAJOR, 186 ATH_TESTMODE_VERSION_MINOR); 187 188 spin_lock_bh(&ar->data_lock); 189 skb = cfg80211_testmode_alloc_reply_skb(ar->ah->hw->wiphy, 190 2 * nla_total_size(sizeof(u32))); 191 spin_unlock_bh(&ar->data_lock); 192 193 if (!skb) 194 return -ENOMEM; 195 196 if (nla_put_u32(skb, ATH_TM_ATTR_VERSION_MAJOR, 197 ATH_TESTMODE_VERSION_MAJOR) || 198 nla_put_u32(skb, ATH_TM_ATTR_VERSION_MINOR, 199 ATH_TESTMODE_VERSION_MINOR)) { 200 kfree_skb(skb); 201 return -ENOBUFS; 202 } 203 204 return cfg80211_testmode_reply(skb); 205 } 206 207 static int ath12k_tm_cmd_process_ftm(struct ath12k *ar, struct nlattr *tb[]) 208 { 209 struct ath12k_wmi_pdev *wmi = ar->wmi; 210 struct sk_buff *skb; 211 struct ath12k_wmi_ftm_cmd *ftm_cmd; 212 int ret = 0; 213 void *buf; 214 size_t aligned_len; 215 u32 cmd_id, buf_len; 216 u16 chunk_len, total_bytes, num_segments; 217 u8 segnumber = 0, *bufpos; 218 219 ath12k_dbg(ar->ab, ATH12K_DBG_TESTMODE, "ah->state %d\n", ar->ah->state); 220 if (ar->ah->state != ATH12K_HW_STATE_TM) 221 return -ENETDOWN; 222 223 if (!tb[ATH_TM_ATTR_DATA]) 224 return -EINVAL; 225 226 buf = nla_data(tb[ATH_TM_ATTR_DATA]); 227 buf_len = nla_len(tb[ATH_TM_ATTR_DATA]); 228 cmd_id = WMI_PDEV_UTF_CMDID; 229 ath12k_dbg(ar->ab, ATH12K_DBG_TESTMODE, 230 "testmode cmd wmi cmd_id %d buf %pK buf_len %d\n", 231 cmd_id, buf, buf_len); 232 ath12k_dbg_dump(ar->ab, ATH12K_DBG_TESTMODE, NULL, "", buf, buf_len); 233 bufpos = buf; 234 total_bytes = buf_len; 235 num_segments = total_bytes / MAX_WMI_UTF_LEN; 236 237 if (buf_len - (num_segments * MAX_WMI_UTF_LEN)) 238 num_segments++; 239 240 while (buf_len) { 241 if (buf_len > MAX_WMI_UTF_LEN) 242 chunk_len = MAX_WMI_UTF_LEN; /* MAX message */ 243 else 244 chunk_len = buf_len; 245 246 skb = ath12k_wmi_alloc_skb(wmi->wmi_ab, (chunk_len + 247 sizeof(struct ath12k_wmi_ftm_cmd))); 248 249 if (!skb) 250 return -ENOMEM; 251 252 ftm_cmd = (struct ath12k_wmi_ftm_cmd *)skb->data; 253 aligned_len = chunk_len + sizeof(struct ath12k_wmi_ftm_seg_hdr_params); 254 ftm_cmd->tlv_header = ath12k_wmi_tlv_hdr(WMI_TAG_ARRAY_BYTE, aligned_len); 255 ftm_cmd->seg_hdr.len = cpu_to_le32(total_bytes); 256 ftm_cmd->seg_hdr.msgref = cpu_to_le32(ar->ftm_msgref); 257 ftm_cmd->seg_hdr.segmentinfo = 258 le32_encode_bits(num_segments, 259 ATH12K_FTM_SEGHDR_TOTAL_SEGMENTS) | 260 le32_encode_bits(segnumber, 261 ATH12K_FTM_SEGHDR_CURRENT_SEQ); 262 ftm_cmd->seg_hdr.pdev_id = cpu_to_le32(ar->pdev->pdev_id); 263 segnumber++; 264 memcpy(&ftm_cmd->data, bufpos, chunk_len); 265 ret = ath12k_wmi_cmd_send(wmi, skb, cmd_id); 266 267 if (ret) { 268 ath12k_warn(ar->ab, "ftm wmi command fail: %d\n", ret); 269 kfree_skb(skb); 270 return ret; 271 } 272 273 buf_len -= chunk_len; 274 bufpos += chunk_len; 275 } 276 277 ++ar->ftm_msgref; 278 return ret; 279 } 280 281 static int ath12k_tm_cmd_testmode_start(struct ath12k *ar, struct nlattr *tb[]) 282 { 283 if (ar->ah->state == ATH12K_HW_STATE_TM) 284 return -EALREADY; 285 286 if (ar->ah->state != ATH12K_HW_STATE_OFF) 287 return -EBUSY; 288 289 ar->ab->ftm_event_obj.eventdata = kzalloc(ATH_FTM_EVENT_MAX_BUF_LENGTH, 290 GFP_KERNEL); 291 292 if (!ar->ab->ftm_event_obj.eventdata) 293 return -ENOMEM; 294 295 ar->ah->state = ATH12K_HW_STATE_TM; 296 ar->ftm_msgref = 0; 297 return 0; 298 } 299 300 static int ath12k_tm_cmd_wmi(struct ath12k *ar, struct nlattr *tb[]) 301 { 302 struct ath12k_wmi_pdev *wmi = ar->wmi; 303 struct sk_buff *skb; 304 struct wmi_pdev_set_param_cmd *cmd; 305 int ret = 0, tag; 306 void *buf; 307 u32 cmd_id, buf_len; 308 309 if (!tb[ATH_TM_ATTR_DATA]) 310 return -EINVAL; 311 312 if (!tb[ATH_TM_ATTR_WMI_CMDID]) 313 return -EINVAL; 314 315 buf = nla_data(tb[ATH_TM_ATTR_DATA]); 316 buf_len = nla_len(tb[ATH_TM_ATTR_DATA]); 317 318 if (!buf_len) { 319 ath12k_warn(ar->ab, "No data present in testmode command\n"); 320 return -EINVAL; 321 } 322 323 cmd_id = nla_get_u32(tb[ATH_TM_ATTR_WMI_CMDID]); 324 325 cmd = buf; 326 tag = le32_get_bits(cmd->tlv_header, WMI_TLV_TAG); 327 328 if (tag == WMI_TAG_PDEV_SET_PARAM_CMD) 329 cmd->pdev_id = cpu_to_le32(ar->pdev->pdev_id); 330 331 ath12k_dbg(ar->ab, ATH12K_DBG_TESTMODE, 332 "testmode cmd wmi cmd_id %d buf length %d\n", 333 cmd_id, buf_len); 334 335 ath12k_dbg_dump(ar->ab, ATH12K_DBG_TESTMODE, NULL, "", buf, buf_len); 336 337 skb = ath12k_wmi_alloc_skb(wmi->wmi_ab, buf_len); 338 339 if (!skb) 340 return -ENOMEM; 341 342 memcpy(skb->data, buf, buf_len); 343 344 ret = ath12k_wmi_cmd_send(wmi, skb, cmd_id); 345 if (ret) { 346 dev_kfree_skb(skb); 347 ath12k_warn(ar->ab, "failed to transmit wmi command (testmode): %d\n", 348 ret); 349 } 350 351 return ret; 352 } 353 354 int ath12k_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif, 355 void *data, int len) 356 { 357 struct ath12k_hw *ah = hw->priv; 358 struct ath12k *ar = NULL; 359 struct nlattr *tb[ATH_TM_ATTR_MAX + 1]; 360 struct ath12k_base *ab; 361 struct wiphy *wiphy = hw->wiphy; 362 int ret; 363 364 lockdep_assert_held(&wiphy->mtx); 365 366 ret = nla_parse(tb, ATH_TM_ATTR_MAX, data, len, ath12k_tm_policy, 367 NULL); 368 if (ret) 369 return ret; 370 371 if (!tb[ATH_TM_ATTR_CMD]) 372 return -EINVAL; 373 374 /* TODO: have to handle ar for MLO case */ 375 if (ah->num_radio) 376 ar = ah->radio; 377 378 if (!ar) 379 return -EINVAL; 380 381 ab = ar->ab; 382 switch (nla_get_u32(tb[ATH_TM_ATTR_CMD])) { 383 case ATH_TM_CMD_WMI: 384 return ath12k_tm_cmd_wmi(ar, tb); 385 case ATH_TM_CMD_TESTMODE_START: 386 return ath12k_tm_cmd_testmode_start(ar, tb); 387 case ATH_TM_CMD_GET_VERSION: 388 return ath12k_tm_cmd_get_version(ar, tb); 389 case ATH_TM_CMD_WMI_FTM: 390 set_bit(ATH12K_FLAG_FTM_SEGMENTED, &ab->dev_flags); 391 return ath12k_tm_cmd_process_ftm(ar, tb); 392 default: 393 return -EOPNOTSUPP; 394 } 395 } 396