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 "../../core/iot_sim_cmn_api_i.h" 20 #include <wlan_objmgr_pdev_obj.h> 21 #include <wlan_objmgr_vdev_obj.h> 22 23 #define IEEE80211_FRAME_BODY_OFFSET 0x18 24 #define IEEE80211_TSF_LEN (8) 25 26 /* 27 * iot_sim_apply_content_change_rule - function to apply content change rule 28 * packet from upper stack will be modified 29 * as per the user content. 30 * @piot_sim_rule: iot_sim rule structure 31 * @nbuf: skb coming from upper stack 32 * @fixed_param_length: length of fixed parameters in frame body 33 * 34 * Return: QDF_STATUS_SUCCESS on successful content update or otherwise 35 * QDF_STATUS_E_NOSUPPORT, no content change rule found for this frame 36 */ 37 QDF_STATUS 38 iot_sim_update_beacon_template_struct(qdf_nbuf_t nbuf, 39 struct beacon_tmpl_params *param) 40 { 41 struct ie_header *ie = NULL; 42 uint16_t offset = 0, ie_len = 0; 43 struct ieee80211_ath_channelswitch_ie *csa = NULL; 44 struct ieee80211_extendedchannelswitch_ie *ecsa = NULL; 45 struct extn_ie_header *extn_ie = NULL; 46 47 if (!param) 48 return QDF_STATUS_E_NULL_VALUE; 49 50 /** 51 * Skip fixed field 52 */ 53 offset += IEEE80211_TSF_LEN; /* TSF field */ 54 offset += 2; /* Beacon interval */ 55 offset += 2; /* Capability Information */ 56 57 ie_len = wbuf_get_pktlen(nbuf) - 58 sizeof(struct ieee80211_frame) - offset; 59 ie = (struct ie_header *)((uint8_t *)qdf_nbuf_data(nbuf) + 60 sizeof(struct ieee80211_frame) + offset); 61 62 while (ie_len >= sizeof(struct ie_header)) { 63 ie_len -= sizeof(struct ie_header); 64 if (!ie->ie_len) { 65 ie += 1; 66 continue; 67 } 68 69 if (ie_len < ie->ie_len) { 70 iot_sim_err("Incomplete corrupted IE:%x", ie->ie_id); 71 return QDF_STATUS_E_INVAL; 72 } 73 74 switch (ie->ie_id) { 75 case WLAN_ELEMID_TIM: 76 if (ie->ie_len < WLAN_TIM_IE_MIN_LENGTH) { 77 iot_sim_err("Invalid TIM IE Length"); 78 goto err; 79 } 80 param->tim_ie_offset = ((uint8_t *)ie - 81 (uint8_t *)qdf_nbuf_data(nbuf)); 82 break; 83 case WLAN_ELEMID_CHANSWITCHANN: 84 if (ie->ie_len != WLAN_CSA_IE_MAX_LEN) { 85 iot_sim_err("Invalid CSA IE Length"); 86 goto err; 87 } 88 csa = 89 (struct ieee80211_ath_channelswitch_ie *)ie; 90 param->csa_switch_count_offset = 91 (((uint8_t *)&csa->tbttcount) - 92 (uint8_t *)qdf_nbuf_data(nbuf)); 93 break; 94 case WLAN_ELEMID_EXTCHANSWITCHANN: 95 if (ie->ie_len != WLAN_XCSA_IE_MAX_LEN) { 96 iot_sim_err("Invalid ECSA IE Length"); 97 goto err; 98 } 99 ecsa = 100 (struct ieee80211_extendedchannelswitch_ie *)ie; 101 param->ext_csa_switch_count_offset = 102 (((uint8_t *)&ecsa->tbttcount) - 103 (uint8_t *)qdf_nbuf_data(nbuf)); 104 break; 105 case WLAN_ELEMID_EXTN_ELEM: 106 extn_ie = (struct extn_ie_header *)ie; 107 switch (extn_ie->ie_extn_id) { 108 case WLAN_EXTN_ELEMID_ESP: 109 param->esp_ie_offset = 110 ((uint8_t *)ie - 111 (uint8_t *)qdf_nbuf_data(nbuf)); 112 break; 113 case WLAN_EXTN_ELEMID_MUEDCA: 114 param->mu_edca_ie_offset = 115 ((uint8_t *)ie - 116 (uint8_t *)qdf_nbuf_data(nbuf)); 117 break; 118 default: 119 break; 120 } 121 break; 122 case WLAN_ELEMID_MULTIPLE_BSSID: 123 offset = ((uint8_t *)ie - 124 (uint8_t *)qdf_nbuf_data(nbuf)); 125 param->mbssid_ie_offset = offset; 126 break; 127 default: 128 break; 129 } 130 /* Consume info element */ 131 ie_len -= ie->ie_len; 132 /* Go to next IE */ 133 ie = (struct ie_header *)((uint8_t *)ie + 134 sizeof(struct ie_header) + 135 ie->ie_len); 136 } 137 param->tmpl_len = wbuf_get_pktlen(nbuf); 138 param->tmpl_len_aligned = roundup(param->tmpl_len, 139 sizeof(uint32_t)); 140 param->frm = (uint8_t *)qdf_nbuf_data(nbuf); 141 return QDF_STATUS_SUCCESS; 142 err: 143 return QDF_STATUS_E_INVAL; 144 } 145 146 QDF_STATUS 147 iot_sim_apply_content_change_rule(struct wlan_objmgr_pdev *pdev, 148 struct iot_sim_rule *piot_sim_rule, 149 qdf_nbuf_t nbuf, 150 int fixed_param_length, 151 struct beacon_tmpl_params *param) 152 { 153 uint8_t *buf = NULL; 154 qdf_size_t buf_len = 0; 155 int offset = 0; 156 QDF_STATUS status = QDF_STATUS_SUCCESS; 157 158 if (!piot_sim_rule->frm_content || !piot_sim_rule->len) 159 return QDF_STATUS_E_NOSUPPORT; 160 161 buf_len = qdf_nbuf_len(nbuf); 162 buf = qdf_nbuf_data(nbuf); 163 164 if (piot_sim_rule->offset == 165 IEEE80211_FRAME_BODY_OFFSET) { 166 offset = IEEE80211_FRAME_BODY_OFFSET; 167 } else if (piot_sim_rule->offset == 0) { 168 offset = 0; 169 } else if (buf[piot_sim_rule->offset] == 170 piot_sim_rule->frm_content[0]) { 171 offset = piot_sim_rule->offset; 172 } else { 173 offset = IEEE80211_FRAME_BODY_OFFSET + 174 fixed_param_length; 175 while (((offset + 1) < buf_len) && 176 (buf[offset] < piot_sim_rule->frm_content[0])) { 177 offset += buf[offset + 1] + 2; 178 } 179 } 180 181 if (offset <= buf_len) { 182 buf += offset; 183 qdf_mem_copy(buf, piot_sim_rule->frm_content, 184 piot_sim_rule->len); 185 qdf_nbuf_set_pktlen(nbuf, offset + 186 piot_sim_rule->len); 187 iot_sim_debug("iot_sim: Content updated"); 188 } else { 189 iot_sim_err("Failed to modify content"); 190 } 191 192 if (IEEE80211_IS_BEACON((struct ieee80211_frame *)qdf_nbuf_data(nbuf))) 193 status = iot_sim_update_beacon_template_struct(nbuf, param); 194 if (QDF_IS_STATUS_ERROR(status)) 195 iot_sim_err("Failed to update beacon param"); 196 197 return QDF_STATUS_SUCCESS; 198 } 199 200 /* 201 * iot_sim_apply_drop_rule - function to apply drop rule. If set buffer will be 202 * freed here and proper return value will be sent to 203 * tgt layer. 204 * 205 * @piot_sim_rule: iot_sim rule structure 206 * @nbuf: skb coming from upper stack 207 * 208 * Return: QDF_STATUS_SUCCESS on successful drop 209 * QDF_STATUS_E_NULL_VALUE, when drop rule is applied 210 */ 211 QDF_STATUS 212 iot_sim_apply_drop_rule(struct iot_sim_rule *piot_sim_rule, 213 qdf_nbuf_t nbuf) 214 { 215 if (!piot_sim_rule->drop) 216 return QDF_STATUS_E_NOSUPPORT; 217 218 if (nbuf) 219 qdf_nbuf_free(nbuf); 220 221 iot_sim_debug("iot_sim: Drop rule applied"); 222 return QDF_STATUS_SUCCESS; 223 } 224 225 /* 226 * iot_sim_frame_update - Function to parse input packet coming from upper 227 * stack in Tx direction and to tgt layer in Rx 228 * direction. This function will also check if rule 229 * for that frame type/subtype is set or not and call 230 * specific operation functions. 231 * 232 * @pdev: pdev object 233 * @nbuf: input packet 234 * @param: beacon template cmd parameter 235 * @tx: tx or not 236 * 237 * Return: QDF_STATUS_SUCCESS in general 238 * QDF_STATUS_E_NOSUPPORT, no content change rule found for this frame 239 */ 240 QDF_STATUS iot_sim_frame_update(struct wlan_objmgr_pdev *pdev, qdf_nbuf_t nbuf, 241 struct beacon_tmpl_params *param, 242 bool tx) 243 { 244 uint8_t type, subtype, seq = 0; 245 struct iot_sim_context *isc; 246 uint8_t *buf = qdf_nbuf_data(nbuf), *frm = NULL; 247 int fixed_param_len = 0; 248 bool is_action_frm = false; 249 uint8_t cat, cat_index; 250 int auth_seq_index = 0; 251 struct iot_sim_rule *piot_sim_rule = NULL; 252 QDF_STATUS status = QDF_STATUS_SUCCESS; 253 254 isc = wlan_objmgr_pdev_get_comp_private_obj(pdev, WLAN_IOT_SIM_COMP); 255 if (!isc) { 256 iot_sim_err("pdev IOT_SIM object is NULL!"); 257 return QDF_STATUS_SUCCESS; 258 } 259 260 type = (buf[0] & IEEE80211_FC0_TYPE_MASK) >> IEEE80211_FC0_TYPE_SHIFT; 261 subtype = (buf[0] & IEEE80211_FC0_SUBTYPE_MASK); 262 263 if (type == IEEE80211_FC0_TYPE_MGT && 264 subtype == IEEE80211_FC0_SUBTYPE_AUTH) { 265 /* Authentication frame */ 266 auth_seq_index = IEEE80211_FRAME_BODY_OFFSET + 2; 267 seq = le16toh(*(u_int16_t *)(buf + auth_seq_index)); 268 } else if (type == IEEE80211_FC0_TYPE_MGT && 269 (subtype == IEEE80211_FC0_SUBTYPE_PROBE_RESP || 270 subtype == IEEE80211_FC0_SUBTYPE_BEACON)) 271 /* Probe response frame */ 272 fixed_param_len = 12; 273 else if (type == IEEE80211_FC0_TYPE_MGT && 274 (subtype == IEEE80211_FC0_SUBTYPE_ASSOC_RESP || 275 subtype == IEEE80211_FC0_SUBTYPE_REASSOC_RESP)) 276 /* Assoc/Reassoc response frame */ 277 fixed_param_len = 6; 278 else if (type == IEEE80211_FC0_TYPE_MGT && 279 subtype == IEEE80211_FC0_SUBTYPE_ACTION) { 280 /* Action frame */ 281 frm = buf + IEEE80211_FRAME_BODY_OFFSET; 282 283 is_action_frm = true; 284 if (iot_sim_get_index_for_action_frm(frm, &cat, &cat_index)) { 285 iot_sim_err("get_index_for_action_frm failed"); 286 return QDF_STATUS_SUCCESS; 287 } 288 } 289 290 subtype >>= IEEE80211_FC0_SUBTYPE_SHIFT; 291 iot_sim_debug("iot_sim: type:%d subtype:%d seq:%d, action:%u dir:%s", 292 type, subtype, seq, is_action_frm, 293 tx ? "TX" : "RX"); 294 295 /** 296 * Only broadcast peer is getting handled right now. 297 * Need to add support for peer based content modification 298 */ 299 qdf_spin_lock(&isc->bcast_peer.iot_sim_lock); 300 301 if (!isc->bcast_peer.rule_per_seq[seq]) 302 goto norule; 303 304 if (is_action_frm) 305 piot_sim_rule = isc->bcast_peer.rule_per_seq[seq]-> 306 rule_per_action_frm[cat][cat_index]; 307 else 308 piot_sim_rule = isc->bcast_peer.rule_per_seq[seq]-> 309 rule_per_type[type][subtype]; 310 311 if (!piot_sim_rule) 312 goto norule; 313 314 if (tx) { 315 if (IEEE80211_IS_BEACON((struct ieee80211_frame *) 316 qdf_nbuf_data(nbuf))) { 317 if (isc->bcn_buf) 318 qdf_nbuf_free(isc->bcn_buf); 319 isc->bcn_buf = qdf_nbuf_copy(nbuf); 320 status = 321 iot_sim_apply_content_change_rule(pdev, 322 piot_sim_rule, 323 isc->bcn_buf, 324 fixed_param_len, 325 param); 326 } else { 327 status = 328 iot_sim_apply_content_change_rule(pdev, 329 piot_sim_rule, 330 nbuf, 331 fixed_param_len, 332 param); 333 } 334 335 if (status == QDF_STATUS_E_NOSUPPORT) 336 goto norule; 337 } else { 338 status = iot_sim_apply_drop_rule(piot_sim_rule, nbuf); 339 if (QDF_IS_STATUS_SUCCESS(status)) 340 status = QDF_STATUS_E_NULL_VALUE; 341 } 342 343 qdf_spin_unlock(&isc->bcast_peer.iot_sim_lock); 344 return status; 345 346 norule: 347 iot_sim_debug("Rule not set for this frame"); 348 qdf_spin_unlock(&isc->bcast_peer.iot_sim_lock); 349 return QDF_STATUS_SUCCESS; 350 } 351