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 25 QDF_STATUS iot_sim_frame_update(struct wlan_objmgr_pdev *pdev, qdf_nbuf_t nbuf) 26 { 27 uint8_t type, subtype, seq = 0; 28 struct iot_sim_context *isc; 29 uint8_t *buf = qdf_nbuf_data(nbuf), *frm = NULL; 30 int fixed_param_length = 0; 31 bool is_action_frm = false; 32 uint8_t cat, cat_index; 33 int auth_seq_index = 0, offset = 0; 34 struct iot_sim_rule *piot_sim_rule = NULL; 35 qdf_size_t buf_len = 0; 36 37 type = (buf[0] & IEEE80211_FC0_TYPE_MASK) >> IEEE80211_FC0_TYPE_SHIFT; 38 subtype = (buf[0] & IEEE80211_FC0_SUBTYPE_MASK); 39 isc = wlan_objmgr_pdev_get_comp_private_obj(pdev, WLAN_IOT_SIM_COMP); 40 if (!isc) { 41 iot_sim_err("pdev IOT_SIM object is NULL!"); 42 return QDF_STATUS_SUCCESS; 43 } 44 45 if (type == IEEE80211_FC0_TYPE_MGT && 46 subtype == IEEE80211_FC0_SUBTYPE_AUTH) { 47 /* Authentication frame */ 48 auth_seq_index = IEEE80211_FRAME_BODY_OFFSET + 2; 49 seq = le16toh(*(u_int16_t *)(buf + auth_seq_index)); 50 } else if (type == IEEE80211_FC0_TYPE_MGT && 51 subtype == IEEE80211_FC0_SUBTYPE_PROBE_RESP) 52 /* Probe response frame */ 53 fixed_param_length = 12; 54 else if (type == IEEE80211_FC0_TYPE_MGT && 55 (subtype == IEEE80211_FC0_SUBTYPE_ASSOC_RESP || 56 subtype == IEEE80211_FC0_SUBTYPE_REASSOC_RESP)) 57 /* Assoc/Reassoc response frame */ 58 fixed_param_length = 6; 59 else if (type == IEEE80211_FC0_TYPE_MGT && 60 subtype == IEEE80211_FC0_SUBTYPE_ACTION) { 61 /* Action frame */ 62 frm = buf + IEEE80211_FRAME_BODY_OFFSET; 63 64 is_action_frm = true; 65 if (iot_sim_get_index_for_action_frm(frm, &cat, &cat_index)) { 66 iot_sim_err("get_index_for_action_frm failed"); 67 return QDF_STATUS_SUCCESS; 68 } 69 } 70 71 subtype >>= IEEE80211_FC0_SUBTYPE_SHIFT; 72 iot_sim_debug("iot_sim: Change content operation for frame"); 73 iot_sim_debug("type:%d subtype:%d seq:%d, is_action_frm: %u", 74 type, subtype, seq, is_action_frm); 75 76 /* Only broadcast peer is getting handled right now. 77 * Need to add support for peer based content modification 78 */ 79 qdf_spin_lock(&isc->bcast_peer.iot_sim_lock); 80 if (!isc->bcast_peer.rule_per_seq[seq]) 81 goto norule; 82 83 buf_len = qdf_nbuf_len(nbuf); 84 85 if (is_action_frm) 86 piot_sim_rule = isc->bcast_peer.rule_per_seq[seq]-> 87 rule_per_action_frm[cat][cat_index]; 88 else 89 piot_sim_rule = isc->bcast_peer.rule_per_seq[seq]-> 90 rule_per_type[type][subtype]; 91 92 if (!piot_sim_rule) 93 goto norule; 94 95 if (!piot_sim_rule->frm_content || !piot_sim_rule->len) 96 goto norule; 97 98 if (piot_sim_rule->offset == 99 IEEE80211_FRAME_BODY_OFFSET) { 100 offset = IEEE80211_FRAME_BODY_OFFSET; 101 } else if (piot_sim_rule->offset == 0) { 102 offset = 0; 103 } else if (buf[piot_sim_rule->offset] == 104 piot_sim_rule->frm_content[0]) { 105 offset = piot_sim_rule->offset; 106 } else { 107 offset = IEEE80211_FRAME_BODY_OFFSET + 108 fixed_param_length; 109 while (((offset + 1) < buf_len) && 110 (buf[offset] < piot_sim_rule->frm_content[0])) { 111 offset += buf[offset + 1] + 2; 112 } 113 } 114 115 if (offset <= buf_len) { 116 buf += offset; 117 qdf_mem_copy(buf, piot_sim_rule->frm_content, 118 piot_sim_rule->len); 119 qdf_nbuf_set_pktlen(nbuf, offset + 120 piot_sim_rule->len); 121 iot_sim_debug("iot_sim: Content updated"); 122 } else { 123 iot_sim_err("Failed to modify content"); 124 } 125 qdf_spin_unlock(&isc->bcast_peer.iot_sim_lock); 126 return QDF_STATUS_SUCCESS; 127 128 norule: 129 iot_sim_debug("Rule not set for this frame"); 130 qdf_spin_unlock(&isc->bcast_peer.iot_sim_lock); 131 return QDF_STATUS_SUCCESS; 132 } 133