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