1 /* 2 * Copyright (c) 2020, The Linux Foundation. All rights reserved. 3 * 4 * Permission to use, copy, modify, and/or distribute this software for any 5 * purpose with or without fee is hereby granted, provided that the above 6 * copyright notice and this permission notice appear in all copies. 7 * 8 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 */ 16 17 #include <wlan_iot_sim_utils_api.h> 18 #include <qdf_module.h> 19 #include <qdf_delayed_work.h> 20 #include "../../core/iot_sim_cmn_api_i.h" 21 #include <wlan_objmgr_pdev_obj.h> 22 #include <wlan_objmgr_vdev_obj.h> 23 24 #define IEEE80211_FRAME_BODY_OFFSET 0x18 25 #define IEEE80211_TSF_LEN (8) 26 27 /* 28 * iot_sim_apply_content_change_rule - function to apply content change rule 29 * packet from upper stack will be modified 30 * as per the user content. 31 * @piot_sim_rule: iot_sim rule structure 32 * @nbuf: skb coming from upper stack 33 * @fixed_param_length: length of fixed parameters in frame body 34 * 35 * Return: QDF_STATUS_SUCCESS on successful content update or otherwise 36 * QDF_STATUS_E_NOSUPPORT, no content change rule found for this frame 37 */ 38 QDF_STATUS 39 iot_sim_update_beacon_template_struct(qdf_nbuf_t nbuf, 40 struct beacon_tmpl_params *param) 41 { 42 struct ie_header *ie = NULL; 43 uint16_t offset = 0, ie_len = 0; 44 struct ieee80211_ath_channelswitch_ie *csa = NULL; 45 struct ieee80211_extendedchannelswitch_ie *ecsa = NULL; 46 struct extn_ie_header *extn_ie = NULL; 47 48 if (!param) 49 return QDF_STATUS_E_NULL_VALUE; 50 51 /** 52 * Skip fixed field 53 */ 54 offset += IEEE80211_TSF_LEN; /* TSF field */ 55 offset += 2; /* Beacon interval */ 56 offset += 2; /* Capability Information */ 57 58 ie_len = wbuf_get_pktlen(nbuf) - 59 sizeof(struct ieee80211_frame) - offset; 60 ie = (struct ie_header *)((uint8_t *)qdf_nbuf_data(nbuf) + 61 sizeof(struct ieee80211_frame) + offset); 62 63 while (ie_len >= sizeof(struct ie_header)) { 64 ie_len -= sizeof(struct ie_header); 65 if (!ie->ie_len) { 66 ie += 1; 67 continue; 68 } 69 70 if (ie_len < ie->ie_len) { 71 iot_sim_err("Incomplete corrupted IE:%x", ie->ie_id); 72 return QDF_STATUS_E_INVAL; 73 } 74 75 switch (ie->ie_id) { 76 case WLAN_ELEMID_TIM: 77 if (ie->ie_len < WLAN_TIM_IE_MIN_LENGTH) { 78 iot_sim_err("Invalid TIM IE Length"); 79 goto err; 80 } 81 param->tim_ie_offset = ((uint8_t *)ie - 82 (uint8_t *)qdf_nbuf_data(nbuf)); 83 break; 84 case WLAN_ELEMID_CHANSWITCHANN: 85 if (ie->ie_len != WLAN_CSA_IE_MAX_LEN) { 86 iot_sim_err("Invalid CSA IE Length"); 87 goto err; 88 } 89 csa = 90 (struct ieee80211_ath_channelswitch_ie *)ie; 91 param->csa_switch_count_offset = 92 (((uint8_t *)&csa->tbttcount) - 93 (uint8_t *)qdf_nbuf_data(nbuf)); 94 break; 95 case WLAN_ELEMID_EXTCHANSWITCHANN: 96 if (ie->ie_len != WLAN_XCSA_IE_MAX_LEN) { 97 iot_sim_err("Invalid ECSA IE Length"); 98 goto err; 99 } 100 ecsa = 101 (struct ieee80211_extendedchannelswitch_ie *)ie; 102 param->ext_csa_switch_count_offset = 103 (((uint8_t *)&ecsa->tbttcount) - 104 (uint8_t *)qdf_nbuf_data(nbuf)); 105 break; 106 case WLAN_ELEMID_EXTN_ELEM: 107 extn_ie = (struct extn_ie_header *)ie; 108 switch (extn_ie->ie_extn_id) { 109 case WLAN_EXTN_ELEMID_ESP: 110 param->esp_ie_offset = 111 ((uint8_t *)ie - 112 (uint8_t *)qdf_nbuf_data(nbuf)); 113 break; 114 case WLAN_EXTN_ELEMID_MUEDCA: 115 param->mu_edca_ie_offset = 116 ((uint8_t *)ie - 117 (uint8_t *)qdf_nbuf_data(nbuf)); 118 break; 119 default: 120 break; 121 } 122 break; 123 case WLAN_ELEMID_MULTIPLE_BSSID: 124 offset = ((uint8_t *)ie - 125 (uint8_t *)qdf_nbuf_data(nbuf)); 126 param->mbssid_ie_offset = offset; 127 break; 128 default: 129 break; 130 } 131 /* Consume info element */ 132 ie_len -= ie->ie_len; 133 /* Go to next IE */ 134 ie = (struct ie_header *)((uint8_t *)ie + 135 sizeof(struct ie_header) + 136 ie->ie_len); 137 } 138 param->tmpl_len = wbuf_get_pktlen(nbuf); 139 param->tmpl_len_aligned = roundup(param->tmpl_len, 140 sizeof(uint32_t)); 141 param->frm = (uint8_t *)qdf_nbuf_data(nbuf); 142 return QDF_STATUS_SUCCESS; 143 err: 144 return QDF_STATUS_E_INVAL; 145 } 146 147 QDF_STATUS 148 iot_sim_apply_content_change_rule(struct wlan_objmgr_pdev *pdev, 149 struct iot_sim_rule *piot_sim_rule, 150 qdf_nbuf_t nbuf, 151 int fixed_param_length, 152 struct beacon_tmpl_params *param) 153 { 154 uint8_t *buf = NULL; 155 qdf_size_t buf_len = 0; 156 int offset = 0; 157 QDF_STATUS status = QDF_STATUS_SUCCESS; 158 159 if (!piot_sim_rule->frm_content || !piot_sim_rule->len) 160 return QDF_STATUS_E_NOSUPPORT; 161 162 buf_len = qdf_nbuf_len(nbuf); 163 buf = qdf_nbuf_data(nbuf); 164 165 if (piot_sim_rule->offset == 166 IEEE80211_FRAME_BODY_OFFSET) { 167 offset = IEEE80211_FRAME_BODY_OFFSET; 168 } else if (piot_sim_rule->offset == 0) { 169 offset = 0; 170 } else if (buf[piot_sim_rule->offset] == 171 piot_sim_rule->frm_content[0]) { 172 offset = piot_sim_rule->offset; 173 } else { 174 offset = IEEE80211_FRAME_BODY_OFFSET + 175 fixed_param_length; 176 while (((offset + 1) < buf_len) && 177 (buf[offset] < piot_sim_rule->frm_content[0])) { 178 offset += buf[offset + 1] + 2; 179 } 180 } 181 182 if (offset <= buf_len) { 183 buf += offset; 184 qdf_mem_copy(buf, piot_sim_rule->frm_content, 185 piot_sim_rule->len); 186 qdf_nbuf_set_pktlen(nbuf, offset + 187 piot_sim_rule->len); 188 iot_sim_debug("iot_sim: Content updated"); 189 } else { 190 iot_sim_err("Failed to modify content"); 191 } 192 193 if (IEEE80211_IS_BEACON((struct ieee80211_frame *)qdf_nbuf_data(nbuf))) 194 status = iot_sim_update_beacon_template_struct(nbuf, param); 195 if (QDF_IS_STATUS_ERROR(status)) 196 iot_sim_err("Failed to update beacon param"); 197 198 return QDF_STATUS_SUCCESS; 199 } 200 201 /* 202 * iot_sim_apply_delay_drop_rule - function to apply delay or drop rule. 203 * If drop rule is set, buffer will be freed 204 * here and proper return value will be sent to 205 * tgt layer. In case of delay rule, delayed 206 * workqueue will be scheduled for rx frame 207 * processing 208 * 209 * @piot_sim_rule: iot_sim rule structure 210 * @nbuf: skb coming from upper stack 211 * 212 * Return: QDF_STATUS_SUCCESS on successful drop 213 * QDF_STATUS_E_NULL_VALUE, when drop rule is applied 214 */ 215 QDF_STATUS 216 iot_sim_apply_delay_drop_rule(struct iot_sim_rule *piot_sim_rule, 217 qdf_nbuf_t nbuf, 218 struct mgmt_rx_event_params *param) 219 { 220 struct mgmt_rx_event_params *rx_param; 221 222 if (!piot_sim_rule->drop && 223 !piot_sim_rule->delay_dur) 224 return QDF_STATUS_E_NOSUPPORT; 225 226 if (piot_sim_rule->drop && nbuf) { 227 qdf_nbuf_free(nbuf); 228 iot_sim_debug("iot_sim: Drop rule applied"); 229 } else if (piot_sim_rule->delay_dur) { 230 if (nbuf == piot_sim_rule->sec_buf) { 231 iot_sim_debug("iot_sim: rx frame process after delay"); 232 return QDF_STATUS_E_NOSUPPORT; 233 } 234 235 if (piot_sim_rule->nbuf_list[0]) { 236 if (!qdf_delayed_work_stop(piot_sim_rule-> 237 dwork)) { 238 piot_sim_rule->nbuf_list[1] = nbuf; 239 return QDF_STATUS_SUCCESS; 240 } 241 242 qdf_nbuf_free(piot_sim_rule->nbuf_list[0]); 243 qdf_mem_free(piot_sim_rule->rx_param->rx_params); 244 qdf_mem_free(piot_sim_rule->rx_param); 245 } 246 247 rx_param = qdf_mem_malloc(sizeof(struct mgmt_rx_event_params)); 248 if (!rx_param) { 249 iot_sim_err("rx_param alloc failed"); 250 return QDF_STATUS_E_NOSUPPORT; 251 } 252 253 qdf_mem_copy(rx_param, param, 254 sizeof(struct mgmt_rx_event_params)); 255 rx_param->rx_params = qdf_mem_malloc(RX_STATUS_SIZE); 256 if (!rx_param->rx_params) { 257 iot_sim_err("rx_param->rx_params alloc failed"); 258 qdf_mem_free(rx_param); 259 return QDF_STATUS_E_NOSUPPORT; 260 } 261 262 qdf_mem_copy(rx_param->rx_params, 263 param->rx_params, RX_STATUS_SIZE); 264 piot_sim_rule->rx_param = rx_param; 265 piot_sim_rule->nbuf_list[0] = nbuf; 266 if (!qdf_delayed_work_start(piot_sim_rule->dwork, 267 piot_sim_rule->delay_dur)) { 268 iot_sim_err("delayed_work_start failed"); 269 qdf_mem_free(rx_param->rx_params); 270 qdf_mem_free(rx_param); 271 return QDF_STATUS_E_NOSUPPORT; 272 } 273 274 iot_sim_err("iot_sim: Delay rule applied"); 275 } 276 277 return QDF_STATUS_SUCCESS; 278 } 279 280 /* 281 * iot_sim_frame_update - Function to parse input packet coming from upper 282 * stack in Tx direction and to tgt layer in Rx 283 * direction. This function will also check if rule 284 * for that frame type/subtype is set or not and call 285 * specific operation functions. 286 * 287 * @pdev: pdev object 288 * @nbuf: input packet 289 * @param: beacon template cmd parameter 290 * @tx: tx or not 291 * @rx_param: mgmt_rx_event_params 292 * 293 * Return: QDF_STATUS_SUCCESS in general 294 * QDF_STATUS_E_NOSUPPORT, no content change rule found for this frame 295 */ 296 QDF_STATUS iot_sim_frame_update(struct wlan_objmgr_pdev *pdev, qdf_nbuf_t nbuf, 297 struct beacon_tmpl_params *param, 298 bool tx, struct mgmt_rx_event_params *rx_param) 299 { 300 uint8_t type, subtype, seq = 0; 301 struct iot_sim_context *isc; 302 uint8_t *buf = qdf_nbuf_data(nbuf), *frm = NULL; 303 int fixed_param_len = 0; 304 bool is_action_frm = false; 305 uint8_t cat, cat_index; 306 int auth_seq_index = 0; 307 struct iot_sim_rule *piot_sim_rule = NULL; 308 QDF_STATUS status = QDF_STATUS_SUCCESS; 309 struct iot_sim_rule_per_peer *peer_rule; 310 struct ieee80211_frame *wh = (struct ieee80211_frame *)buf; 311 struct qdf_mac_addr *mac_addr; 312 313 isc = wlan_objmgr_pdev_get_comp_private_obj(pdev, WLAN_IOT_SIM_COMP); 314 if (!isc) { 315 iot_sim_err("pdev IOT_SIM object is NULL!"); 316 return QDF_STATUS_SUCCESS; 317 } 318 319 type = (buf[0] & IEEE80211_FC0_TYPE_MASK) >> IEEE80211_FC0_TYPE_SHIFT; 320 subtype = (buf[0] & IEEE80211_FC0_SUBTYPE_MASK); 321 322 if (type == IEEE80211_FC0_TYPE_MGT && 323 subtype == IEEE80211_FC0_SUBTYPE_AUTH) { 324 /* Authentication frame */ 325 auth_seq_index = IEEE80211_FRAME_BODY_OFFSET + 2; 326 seq = le16toh(*(u_int16_t *)(buf + auth_seq_index)); 327 } else if (type == IEEE80211_FC0_TYPE_MGT && 328 (subtype == IEEE80211_FC0_SUBTYPE_PROBE_RESP || 329 subtype == IEEE80211_FC0_SUBTYPE_BEACON)) 330 /* Probe response frame */ 331 fixed_param_len = 12; 332 else if (type == IEEE80211_FC0_TYPE_MGT && 333 (subtype == IEEE80211_FC0_SUBTYPE_ASSOC_RESP || 334 subtype == IEEE80211_FC0_SUBTYPE_REASSOC_RESP)) 335 /* Assoc/Reassoc response frame */ 336 fixed_param_len = 6; 337 else if (type == IEEE80211_FC0_TYPE_MGT && 338 subtype == IEEE80211_FC0_SUBTYPE_ACTION) { 339 /* Action frame */ 340 frm = buf + IEEE80211_FRAME_BODY_OFFSET; 341 342 is_action_frm = true; 343 if (iot_sim_get_index_for_action_frm(frm, &cat, 344 &cat_index, !tx)) { 345 iot_sim_err("get_index_for_action_frm failed"); 346 return QDF_STATUS_SUCCESS; 347 } 348 } 349 350 subtype >>= IEEE80211_FC0_SUBTYPE_SHIFT; 351 iot_sim_debug("iot_sim: type:%d subtype:%d seq:%d, action:%u dir:%s", 352 type, subtype, seq, is_action_frm, 353 tx ? "TX" : "RX"); 354 355 if (tx) 356 mac_addr = (struct qdf_mac_addr *)wh->i_addr1; 357 else 358 mac_addr = (struct qdf_mac_addr *)wh->i_addr2; 359 360 peer_rule = iot_sim_find_peer_from_mac(isc, mac_addr); 361 if (!peer_rule) 362 peer_rule = &isc->bcast_peer; 363 364 if (!peer_rule) 365 goto no_peer_rule; 366 367 qdf_spin_lock_bh(&isc->iot_sim_lock); 368 369 if (!peer_rule->rule_per_seq[seq]) 370 goto norule; 371 372 if (is_action_frm) 373 piot_sim_rule = peer_rule->rule_per_seq[seq]-> 374 rule_per_action_frm[cat][cat_index]; 375 else 376 piot_sim_rule = peer_rule->rule_per_seq[seq]-> 377 rule_per_type[type][subtype]; 378 379 if (!piot_sim_rule) 380 goto norule; 381 382 if (tx) { 383 if (IEEE80211_IS_BEACON((struct ieee80211_frame *) 384 qdf_nbuf_data(nbuf))) { 385 if (isc->bcn_buf) 386 qdf_nbuf_free(isc->bcn_buf); 387 isc->bcn_buf = qdf_nbuf_copy(nbuf); 388 status = 389 iot_sim_apply_content_change_rule(pdev, 390 piot_sim_rule, 391 isc->bcn_buf, 392 fixed_param_len, 393 param); 394 } else { 395 status = 396 iot_sim_apply_content_change_rule(pdev, 397 piot_sim_rule, 398 nbuf, 399 fixed_param_len, 400 param); 401 } 402 403 if (status == QDF_STATUS_E_NOSUPPORT) 404 goto norule; 405 } else { 406 status = iot_sim_apply_delay_drop_rule(piot_sim_rule, 407 nbuf, rx_param); 408 if (QDF_IS_STATUS_SUCCESS(status)) 409 status = QDF_STATUS_E_NULL_VALUE; 410 else 411 status = QDF_STATUS_SUCCESS; 412 } 413 414 qdf_spin_unlock_bh(&isc->iot_sim_lock); 415 return status; 416 417 norule: 418 iot_sim_debug("Rule not set for this frame"); 419 qdf_spin_unlock_bh(&isc->iot_sim_lock); 420 return QDF_STATUS_SUCCESS; 421 no_peer_rule: 422 iot_sim_debug("Rule not set for this peer"); 423 return QDF_STATUS_SUCCESS; 424 } 425