1 // SPDX-License-Identifier: GPL-2.0
2 /******************************************************************************
3  *
4  *  Copyright(c) 2003 - 2004 Intel Corporation. All rights reserved.
5  *
6  *  Contact Information:
7  *  James P. Ketrenos <ipw2100-admin@linux.intel.com>
8  *  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
9  *
10  *
11  *  Few modifications for Realtek's Wi-Fi drivers by
12  *  Andrea Merello <andrea.merello@gmail.com>
13  *
14  *  A special thanks goes to Realtek for their support !
15  *
16  ******************************************************************************/
17 
18 #include <linux/compiler.h>
19 #include <linux/errno.h>
20 #include <linux/if_arp.h>
21 #include <linux/in6.h>
22 #include <linux/in.h>
23 #include <linux/ip.h>
24 #include <linux/kernel.h>
25 #include <linux/module.h>
26 #include <linux/netdevice.h>
27 #include <linux/pci.h>
28 #include <linux/proc_fs.h>
29 #include <linux/skbuff.h>
30 #include <linux/slab.h>
31 #include <linux/tcp.h>
32 #include <linux/types.h>
33 #include <linux/wireless.h>
34 #include <linux/etherdevice.h>
35 #include <linux/uaccess.h>
36 #include <linux/if_vlan.h>
37 
38 #include "ieee80211.h"
39 
40 
41 /*
42  *
43  *
44  * 802.11 Data Frame
45  *
46  *
47  * 802.11 frame_contorl for data frames - 2 bytes
48  *      ,-----------------------------------------------------------------------------------------.
49  * bits | 0  |  1  |  2  |  3  |  4  |  5  |  6  |  7  |  8  |  9  |  a  |  b  |  c  |  d  |  e   |
50  *      |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
51  * val  | 0  |  0  |  0  |  1  |  x  |  0  |  0  |  0  |  1  |  0  |  x  |  x  |  x  |  x  |  x   |
52  *      |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
53  * desc | ^-ver-^  |  ^type-^  |  ^-----subtype-----^  | to  |from |more |retry| pwr |more |wep   |
54  *      |          |           | x=0 data,x=1 data+ack | DS  | DS  |frag |     | mgm |data |      |
55  *      '-----------------------------------------------------------------------------------------'
56  *                                                    /\
57  *                                                    |
58  * 802.11 Data Frame                                  |
59  *           ,--------- 'ctrl' expands to >-----------'
60  *           |
61  *        ,--'---,-------------------------------------------------------------.
62  *  Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
63  *        |------|------|---------|---------|---------|------|---------|------|
64  *  Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  Frame  |  fcs |
65  *        |      | tion | (BSSID) |         |         | ence |  data   |      |
66  *        `--------------------------------------------------|         |------'
67  *  Total: 28 non-data bytes                                 `----.----'
68  *                                                                |
69  *         .- 'Frame data' expands to <---------------------------'
70  *         |
71  *         V
72  *        ,---------------------------------------------------.
73  *  Bytes |  1   |  1   |    1    |    3     |  2   |  0-2304 |
74  *        |------|------|---------|----------|------|---------|
75  *  Desc. | SNAP | SNAP | Control |Eth Tunnel| Type | IP      |
76  *        | DSAP | SSAP |         |          |      | Packet  |
77  *        | 0xAA | 0xAA |0x03 (UI)|0x00-00-F8|      |         |
78  *        `-----------------------------------------|         |
79  *  Total: 8 non-data bytes                         `----.----'
80  *                                                       |
81  *         .- 'IP Packet' expands, if WEP enabled, to <--'
82  *         |
83  *         V
84  *        ,-----------------------.
85  *  Bytes |  4  |   0-2296  |  4  |
86  *        |-----|-----------|-----|
87  *  Desc. | IV  | Encrypted | ICV |
88  *        |     | IP Packet |     |
89  *        `-----------------------'
90  *  Total: 8 non-data bytes
91  *
92  *
93  *  802.3 Ethernet Data Frame
94  *
95  *        ,-----------------------------------------.
96  *  Bytes |   6   |   6   |  2   |  Variable |   4  |
97  *        |-------|-------|------|-----------|------|
98  *  Desc. | Dest. | Source| Type | IP Packet |  fcs |
99  *        |  MAC  |  MAC  |      |           |      |
100  *        `-----------------------------------------'
101  *  Total: 18 non-data bytes
102  *
103  *  In the event that fragmentation is required, the incoming payload is split into
104  *  N parts of size ieee->fts.  The first fragment contains the SNAP header and the
105  *  remaining packets are just data.
106  *
107  *  If encryption is enabled, each fragment payload size is reduced by enough space
108  *  to add the prefix and postfix (IV and ICV totalling 8 bytes in the case of WEP)
109  *  So if you have 1500 bytes of payload with ieee->fts set to 500 without
110  *  encryption it will take 3 frames.  With WEP it will take 4 frames as the
111  *  payload of each frame is reduced to 492 bytes.
112  *
113  * SKB visualization
114  *
115  *  ,- skb->data
116  * |
117  * |    ETHERNET HEADER        ,-<-- PAYLOAD
118  * |                           |     14 bytes from skb->data
119  * |  2 bytes for Type --> ,T. |     (sizeof ethhdr)
120  * |                       | | |
121  * |,-Dest.--. ,--Src.---. | | |
122  * |  6 bytes| | 6 bytes | | | |
123  * v         | |         | | | |
124  * 0         | v       1 | v | v           2
125  * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
126  *     ^     | ^         | ^ |
127  *     |     | |         | | |
128  *     |     | |         | `T' <---- 2 bytes for Type
129  *     |     | |         |
130  *     |     | '---SNAP--' <-------- 6 bytes for SNAP
131  *     |     |
132  *     `-IV--' <-------------------- 4 bytes for IV (WEP)
133  *
134  *      SNAP HEADER
135  *
136  */
137 
138 static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
139 static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
140 
ieee80211_put_snap(u8 * data,u16 h_proto)141 static inline int ieee80211_put_snap(u8 *data, u16 h_proto)
142 {
143 	struct ieee80211_snap_hdr *snap;
144 	u8 *oui;
145 
146 	snap = (struct ieee80211_snap_hdr *)data;
147 	snap->dsap = 0xaa;
148 	snap->ssap = 0xaa;
149 	snap->ctrl = 0x03;
150 
151 	if (h_proto == 0x8137 || h_proto == 0x80f3)
152 		oui = P802_1H_OUI;
153 	else
154 		oui = RFC1042_OUI;
155 	snap->oui[0] = oui[0];
156 	snap->oui[1] = oui[1];
157 	snap->oui[2] = oui[2];
158 
159 	*(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
160 
161 	return SNAP_SIZE + sizeof(u16);
162 }
163 
ieee80211_encrypt_fragment(struct ieee80211_device * ieee,struct sk_buff * frag,int hdr_len)164 int ieee80211_encrypt_fragment(
165 	struct ieee80211_device *ieee,
166 	struct sk_buff *frag,
167 	int hdr_len)
168 {
169 	struct ieee80211_crypt_data *crypt = ieee->crypt[ieee->tx_keyidx];
170 	int res;
171 
172 	if (!(crypt && crypt->ops)) {
173 		printk("=========>%s(), crypt is null\n", __func__);
174 		return -1;
175 	}
176 
177 	if (ieee->tkip_countermeasures &&
178 	    crypt && crypt->ops && strcmp(crypt->ops->name, "TKIP") == 0) {
179 		if (net_ratelimit()) {
180 			struct rtl_80211_hdr_3addrqos *header;
181 
182 			header = (struct rtl_80211_hdr_3addrqos *)frag->data;
183 			netdev_dbg(ieee->dev, "TKIP countermeasures: dropped "
184 			       "TX packet to %pM\n", header->addr1);
185 		}
186 		return -1;
187 	}
188 
189 	/* To encrypt, frame format is:
190 	 * IV (4 bytes), clear payload (including SNAP), ICV (4 bytes)
191 	 */
192 
193 	// PR: FIXME: Copied from hostap. Check fragmentation/MSDU/MPDU encryption.
194 	/* Host-based IEEE 802.11 fragmentation for TX is not yet supported, so
195 	 * call both MSDU and MPDU encryption functions from here.
196 	 */
197 	atomic_inc(&crypt->refcnt);
198 	res = 0;
199 	if (crypt->ops->encrypt_msdu)
200 		res = crypt->ops->encrypt_msdu(frag, hdr_len, crypt->priv);
201 	if (res == 0 && crypt->ops->encrypt_mpdu)
202 		res = crypt->ops->encrypt_mpdu(frag, hdr_len, crypt->priv);
203 
204 	atomic_dec(&crypt->refcnt);
205 	if (res < 0) {
206 		netdev_info(ieee->dev, "Encryption failed: len=%d.\n",
207 			    frag->len);
208 		ieee->ieee_stats.tx_discards++;
209 		return -1;
210 	}
211 
212 	return 0;
213 }
214 
215 
ieee80211_txb_free(struct ieee80211_txb * txb)216 void ieee80211_txb_free(struct ieee80211_txb *txb)
217 {
218 	//int i;
219 	if (unlikely(!txb))
220 		return;
221 	kfree(txb);
222 }
223 EXPORT_SYMBOL(ieee80211_txb_free);
224 
ieee80211_alloc_txb(int nr_frags,int txb_size,gfp_t gfp_mask)225 static struct ieee80211_txb *ieee80211_alloc_txb(int nr_frags, int txb_size,
226 						 gfp_t gfp_mask)
227 {
228 	struct ieee80211_txb *txb;
229 	int i;
230 	txb = kmalloc(
231 		sizeof(struct ieee80211_txb) + (sizeof(u8 *) * nr_frags),
232 		gfp_mask);
233 	if (!txb)
234 		return NULL;
235 
236 	memset(txb, 0, sizeof(struct ieee80211_txb));
237 	txb->nr_frags = nr_frags;
238 	txb->frag_size = __cpu_to_le16(txb_size);
239 
240 	for (i = 0; i < nr_frags; i++) {
241 		txb->fragments[i] = dev_alloc_skb(txb_size);
242 		if (unlikely(!txb->fragments[i])) {
243 			i--;
244 			break;
245 		}
246 		memset(txb->fragments[i]->cb, 0, sizeof(txb->fragments[i]->cb));
247 	}
248 	if (unlikely(i != nr_frags)) {
249 		while (i >= 0)
250 			dev_kfree_skb_any(txb->fragments[i--]);
251 		kfree(txb);
252 		return NULL;
253 	}
254 	return txb;
255 }
256 
257 // Classify the to-be send data packet
258 // Need to acquire the sent queue index.
259 static int
ieee80211_classify(struct sk_buff * skb,struct ieee80211_network * network)260 ieee80211_classify(struct sk_buff *skb, struct ieee80211_network *network)
261 {
262 	struct ethhdr *eth;
263 	struct iphdr *ip;
264 	eth = (struct ethhdr *)skb->data;
265 	if (eth->h_proto != htons(ETH_P_IP))
266 		return 0;
267 
268 	ip = ip_hdr(skb);
269 	switch (ip->tos & 0xfc) {
270 	case 0x20:
271 		return 2;
272 	case 0x40:
273 		return 1;
274 	case 0x60:
275 		return 3;
276 	case 0x80:
277 		return 4;
278 	case 0xa0:
279 		return 5;
280 	case 0xc0:
281 		return 6;
282 	case 0xe0:
283 		return 7;
284 	default:
285 		return 0;
286 	}
287 }
288 
ieee80211_tx_query_agg_cap(struct ieee80211_device * ieee,struct sk_buff * skb,struct cb_desc * tcb_desc)289 static void ieee80211_tx_query_agg_cap(struct ieee80211_device *ieee,
290 				       struct sk_buff *skb, struct cb_desc *tcb_desc)
291 {
292 	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
293 	struct tx_ts_record        *pTxTs = NULL;
294 	struct rtl_80211_hdr_1addr *hdr = (struct rtl_80211_hdr_1addr *)skb->data;
295 
296 	if (!pHTInfo->bCurrentHTSupport || !pHTInfo->bEnableHT)
297 		return;
298 	if (!IsQoSDataFrame(skb->data))
299 		return;
300 
301 	if (is_multicast_ether_addr(hdr->addr1))
302 		return;
303 	//check packet and mode later
304 	if (!ieee->GetNmodeSupportBySecCfg(ieee->dev))
305 		return;
306 
307 	if (pHTInfo->bCurrentAMPDUEnable) {
308 		if (!GetTs(ieee, (struct ts_common_info **)(&pTxTs), hdr->addr1, skb->priority, TX_DIR, true)) {
309 			printk("===>can't get TS\n");
310 			return;
311 		}
312 		if (!pTxTs->tx_admitted_ba_record.valid) {
313 			TsStartAddBaProcess(ieee, pTxTs);
314 			goto FORCED_AGG_SETTING;
315 		} else if (!pTxTs->using_ba) {
316 			if (SN_LESS(pTxTs->tx_admitted_ba_record.start_seq_ctrl.field.seq_num, (pTxTs->tx_cur_seq + 1) % 4096))
317 				pTxTs->using_ba = true;
318 			else
319 				goto FORCED_AGG_SETTING;
320 		}
321 
322 		if (ieee->iw_mode == IW_MODE_INFRA) {
323 			tcb_desc->bAMPDUEnable = true;
324 			tcb_desc->ampdu_factor = pHTInfo->CurrentAMPDUFactor;
325 			tcb_desc->ampdu_density = pHTInfo->CurrentMPDUDensity;
326 		}
327 	}
328 FORCED_AGG_SETTING:
329 	switch (pHTInfo->ForcedAMPDUMode) {
330 	case HT_AGG_AUTO:
331 		break;
332 
333 	case HT_AGG_FORCE_ENABLE:
334 		tcb_desc->bAMPDUEnable = true;
335 		tcb_desc->ampdu_density = pHTInfo->ForcedMPDUDensity;
336 		tcb_desc->ampdu_factor = pHTInfo->ForcedAMPDUFactor;
337 		break;
338 
339 	case HT_AGG_FORCE_DISABLE:
340 		tcb_desc->bAMPDUEnable = false;
341 		tcb_desc->ampdu_density = 0;
342 		tcb_desc->ampdu_factor = 0;
343 		break;
344 
345 	}
346 		return;
347 }
348 
ieee80211_qurey_ShortPreambleMode(struct ieee80211_device * ieee,struct cb_desc * tcb_desc)349 static void ieee80211_qurey_ShortPreambleMode(struct ieee80211_device *ieee,
350 					      struct cb_desc *tcb_desc)
351 {
352 	tcb_desc->bUseShortPreamble = false;
353 	if (tcb_desc->data_rate == 2) {//// 1M can only use Long Preamble. 11B spec
354 		return;
355 	} else if (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_PREAMBLE) {
356 		tcb_desc->bUseShortPreamble = true;
357 	}
358 	return;
359 }
360 static void
ieee80211_query_HTCapShortGI(struct ieee80211_device * ieee,struct cb_desc * tcb_desc)361 ieee80211_query_HTCapShortGI(struct ieee80211_device *ieee, struct cb_desc *tcb_desc)
362 {
363 	PRT_HIGH_THROUGHPUT		pHTInfo = ieee->pHTInfo;
364 
365 	tcb_desc->bUseShortGI		= false;
366 
367 	if (!pHTInfo->bCurrentHTSupport || !pHTInfo->bEnableHT)
368 		return;
369 
370 	if (pHTInfo->bForcedShortGI) {
371 		tcb_desc->bUseShortGI = true;
372 		return;
373 	}
374 
375 	if (pHTInfo->bCurBW40MHz && pHTInfo->bCurShortGI40MHz)
376 		tcb_desc->bUseShortGI = true;
377 	else if (!pHTInfo->bCurBW40MHz && pHTInfo->bCurShortGI20MHz)
378 		tcb_desc->bUseShortGI = true;
379 }
380 
ieee80211_query_BandwidthMode(struct ieee80211_device * ieee,struct cb_desc * tcb_desc)381 static void ieee80211_query_BandwidthMode(struct ieee80211_device *ieee,
382 					  struct cb_desc *tcb_desc)
383 {
384 	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
385 
386 	tcb_desc->bPacketBW = false;
387 
388 	if (!pHTInfo->bCurrentHTSupport || !pHTInfo->bEnableHT)
389 		return;
390 
391 	if (tcb_desc->bMulticast || tcb_desc->bBroadcast)
392 		return;
393 
394 	if ((tcb_desc->data_rate & 0x80) == 0) // If using legacy rate, it shall use 20MHz channel.
395 		return;
396 	//BandWidthAutoSwitch is for auto switch to 20 or 40 in long distance
397 	if (pHTInfo->bCurBW40MHz && pHTInfo->bCurTxBW40MHz && !ieee->bandwidth_auto_switch.bforced_tx20Mhz)
398 		tcb_desc->bPacketBW = true;
399 	return;
400 }
401 
ieee80211_query_protectionmode(struct ieee80211_device * ieee,struct cb_desc * tcb_desc,struct sk_buff * skb)402 static void ieee80211_query_protectionmode(struct ieee80211_device *ieee,
403 					   struct cb_desc *tcb_desc,
404 					   struct sk_buff *skb)
405 {
406 	// Common Settings
407 	tcb_desc->bRTSSTBC			= false;
408 	tcb_desc->bRTSUseShortGI		= false; // Since protection frames are always sent by legacy rate, ShortGI will never be used.
409 	tcb_desc->bCTSEnable			= false; // Most of protection using RTS/CTS
410 	tcb_desc->RTSSC				= 0;		// 20MHz: Don't care;  40MHz: Duplicate.
411 	tcb_desc->bRTSBW			= false; // RTS frame bandwidth is always 20MHz
412 
413 	if (tcb_desc->bBroadcast || tcb_desc->bMulticast) //only unicast frame will use rts/cts
414 		return;
415 
416 	if (is_broadcast_ether_addr(skb->data + 16))  //check addr3 as infrastructure add3 is DA.
417 		return;
418 
419 	if (ieee->mode < IEEE_N_24G) /* b, g mode */ {
420 			// (1) RTS_Threshold is compared to the MPDU, not MSDU.
421 			// (2) If there are more than one frag in  this MSDU, only the first frag uses protection frame.
422 			//		Other fragments are protected by previous fragment.
423 			//		So we only need to check the length of first fragment.
424 		if (skb->len > ieee->rts) {
425 			tcb_desc->bRTSEnable = true;
426 			tcb_desc->rts_rate = MGN_24M;
427 		} else if (ieee->current_network.buseprotection) {
428 			// Use CTS-to-SELF in protection mode.
429 			tcb_desc->bRTSEnable = true;
430 			tcb_desc->bCTSEnable = true;
431 			tcb_desc->rts_rate = MGN_24M;
432 		}
433 		//otherwise return;
434 		return;
435 	} else { // 11n High throughput case.
436 		PRT_HIGH_THROUGHPUT pHTInfo = ieee->pHTInfo;
437 		while (true) {
438 			//check ERP protection
439 			if (ieee->current_network.buseprotection) {// CTS-to-SELF
440 				tcb_desc->bRTSEnable = true;
441 				tcb_desc->bCTSEnable = true;
442 				tcb_desc->rts_rate = MGN_24M;
443 				break;
444 			}
445 			//check HT op mode
446 			if (pHTInfo->bCurrentHTSupport && pHTInfo->bEnableHT) {
447 				u8 HTOpMode = pHTInfo->CurrentOpMode;
448 				if ((pHTInfo->bCurBW40MHz && (HTOpMode == 2 || HTOpMode == 3)) ||
449 							(!pHTInfo->bCurBW40MHz && HTOpMode == 3)) {
450 					tcb_desc->rts_rate = MGN_24M; // Rate is 24Mbps.
451 					tcb_desc->bRTSEnable = true;
452 					break;
453 				}
454 			}
455 			//check rts
456 			if (skb->len > ieee->rts) {
457 				tcb_desc->rts_rate = MGN_24M; // Rate is 24Mbps.
458 				tcb_desc->bRTSEnable = true;
459 				break;
460 			}
461 			//to do list: check MIMO power save condition.
462 			//check AMPDU aggregation for TXOP
463 			if (tcb_desc->bAMPDUEnable) {
464 				tcb_desc->rts_rate = MGN_24M; // Rate is 24Mbps.
465 				// According to 8190 design, firmware sends CF-End only if RTS/CTS is enabled. However, it degrads
466 				// throughput around 10M, so we disable of this mechanism. 2007.08.03 by Emily
467 				tcb_desc->bRTSEnable = false;
468 				break;
469 			}
470 			//check IOT action
471 			if (pHTInfo->IOTAction & HT_IOT_ACT_FORCED_CTS2SELF) {
472 				tcb_desc->bCTSEnable	= true;
473 				tcb_desc->rts_rate  =	MGN_24M;
474 				tcb_desc->bRTSEnable = true;
475 				break;
476 			}
477 			// Totally no protection case!!
478 			goto NO_PROTECTION;
479 		}
480 		}
481 	// For test , CTS replace with RTS
482 	if (0) {
483 		tcb_desc->bCTSEnable	= true;
484 		tcb_desc->rts_rate = MGN_24M;
485 		tcb_desc->bRTSEnable	= true;
486 	}
487 	if (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
488 		tcb_desc->bUseShortPreamble = true;
489 	if (ieee->mode == IW_MODE_MASTER)
490 		goto NO_PROTECTION;
491 	return;
492 NO_PROTECTION:
493 	tcb_desc->bRTSEnable	= false;
494 	tcb_desc->bCTSEnable	= false;
495 	tcb_desc->rts_rate		= 0;
496 	tcb_desc->RTSSC		= 0;
497 	tcb_desc->bRTSBW		= false;
498 }
499 
500 
ieee80211_txrate_selectmode(struct ieee80211_device * ieee,struct cb_desc * tcb_desc)501 static void ieee80211_txrate_selectmode(struct ieee80211_device *ieee,
502 					struct cb_desc *tcb_desc)
503 {
504 	if (ieee->bTxDisableRateFallBack)
505 		tcb_desc->bTxDisableRateFallBack = true;
506 
507 	if (ieee->bTxUseDriverAssingedRate)
508 		tcb_desc->bTxUseDriverAssingedRate = true;
509 	if (!tcb_desc->bTxDisableRateFallBack || !tcb_desc->bTxUseDriverAssingedRate) {
510 		if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC)
511 			tcb_desc->RATRIndex = 0;
512 	}
513 }
514 
ieee80211_query_seqnum(struct ieee80211_device * ieee,struct sk_buff * skb,u8 * dst)515 static void ieee80211_query_seqnum(struct ieee80211_device *ieee,
516 				   struct sk_buff *skb, u8 *dst)
517 {
518 	if (is_multicast_ether_addr(dst))
519 		return;
520 	if (IsQoSDataFrame(skb->data)) /* we deal qos data only */ {
521 		struct tx_ts_record *pTS = NULL;
522 		if (!GetTs(ieee, (struct ts_common_info **)(&pTS), dst, skb->priority, TX_DIR, true)) {
523 			return;
524 		}
525 		pTS->tx_cur_seq = (pTS->tx_cur_seq + 1) % 4096;
526 	}
527 }
528 
ieee80211_xmit(struct sk_buff * skb,struct net_device * dev)529 netdev_tx_t ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
530 {
531 	struct ieee80211_device *ieee = netdev_priv(dev);
532 	struct ieee80211_txb *txb = NULL;
533 	struct rtl_80211_hdr_3addrqos *frag_hdr;
534 	int i, bytes_per_frag, nr_frags, bytes_last_frag, frag_size;
535 	unsigned long flags;
536 	struct net_device_stats *stats = &ieee->stats;
537 	int ether_type = 0, encrypt;
538 	int bytes, fc, qos_ctl = 0, hdr_len;
539 	struct sk_buff *skb_frag;
540 	struct rtl_80211_hdr_3addrqos header = { /* Ensure zero initialized */
541 		.duration_id = 0,
542 		.seq_ctl = 0,
543 		.qos_ctl = 0
544 	};
545 	u8 dest[ETH_ALEN], src[ETH_ALEN];
546 	int qos_actived = ieee->current_network.qos_data.active;
547 
548 	struct ieee80211_crypt_data *crypt;
549 
550 	struct cb_desc *tcb_desc;
551 
552 	spin_lock_irqsave(&ieee->lock, flags);
553 
554 	/* If there is no driver handler to take the TXB, dont' bother
555 	 * creating it...
556 	 */
557 	if ((!ieee->hard_start_xmit && !(ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)) ||
558 	   ((!ieee->softmac_data_hard_start_xmit && (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)))) {
559 		netdev_warn(ieee->dev, "No xmit handler.\n");
560 		goto success;
561 	}
562 
563 
564 	if (likely(ieee->raw_tx == 0)) {
565 		if (unlikely(skb->len < SNAP_SIZE + sizeof(u16))) {
566 			netdev_warn(ieee->dev, "skb too small (%d).\n",
567 				    skb->len);
568 			goto success;
569 		}
570 
571 		memset(skb->cb, 0, sizeof(skb->cb));
572 		ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
573 
574 		crypt = ieee->crypt[ieee->tx_keyidx];
575 
576 		encrypt = !(ether_type == ETH_P_PAE && ieee->ieee802_1x) &&
577 			ieee->host_encrypt && crypt && crypt->ops;
578 
579 		if (!encrypt && ieee->ieee802_1x &&
580 		ieee->drop_unencrypted && ether_type != ETH_P_PAE) {
581 			stats->tx_dropped++;
582 			goto success;
583 		}
584 	#ifdef CONFIG_IEEE80211_DEBUG
585 		if (crypt && !encrypt && ether_type == ETH_P_PAE) {
586 			struct eapol *eap = (struct eapol *)(skb->data +
587 				sizeof(struct ethhdr) - SNAP_SIZE - sizeof(u16));
588 			IEEE80211_DEBUG_EAP("TX: IEEE 802.11 EAPOL frame: %s\n",
589 				eap_get_type(eap->type));
590 		}
591 	#endif
592 
593 		/* Save source and destination addresses */
594 		memcpy(&dest, skb->data, ETH_ALEN);
595 		memcpy(&src, skb->data + ETH_ALEN, ETH_ALEN);
596 
597 		/* Advance the SKB to the start of the payload */
598 		skb_pull(skb, sizeof(struct ethhdr));
599 
600 		/* Determine total amount of storage required for TXB packets */
601 		bytes = skb->len + SNAP_SIZE + sizeof(u16);
602 
603 		if (encrypt)
604 			fc = IEEE80211_FTYPE_DATA | IEEE80211_FCTL_WEP;
605 		else
606 
607 			fc = IEEE80211_FTYPE_DATA;
608 
609 		//if(ieee->current_network.QoS_Enable)
610 		if (qos_actived)
611 			fc |= IEEE80211_STYPE_QOS_DATA;
612 		else
613 			fc |= IEEE80211_STYPE_DATA;
614 
615 		if (ieee->iw_mode == IW_MODE_INFRA) {
616 			fc |= IEEE80211_FCTL_TODS;
617 			/* To DS: Addr1 = BSSID, Addr2 = SA,
618 			 * Addr3 = DA
619 			 */
620 			memcpy(&header.addr1, ieee->current_network.bssid, ETH_ALEN);
621 			memcpy(&header.addr2, &src, ETH_ALEN);
622 			memcpy(&header.addr3, &dest, ETH_ALEN);
623 		} else if (ieee->iw_mode == IW_MODE_ADHOC) {
624 			/* not From/To DS: Addr1 = DA, Addr2 = SA,
625 			 * Addr3 = BSSID
626 			 */
627 			memcpy(&header.addr1, dest, ETH_ALEN);
628 			memcpy(&header.addr2, src, ETH_ALEN);
629 			memcpy(&header.addr3, ieee->current_network.bssid, ETH_ALEN);
630 		}
631 
632 		header.frame_ctl = cpu_to_le16(fc);
633 
634 		/* Determine fragmentation size based on destination (multicast
635 		 * and broadcast are not fragmented)
636 		 */
637 		if (is_multicast_ether_addr(header.addr1)) {
638 			frag_size = MAX_FRAG_THRESHOLD;
639 			qos_ctl |= QOS_CTL_NOTCONTAIN_ACK;
640 		} else {
641 			frag_size = ieee->fts;//default:392
642 			qos_ctl = 0;
643 		}
644 
645 		//if (ieee->current_network.QoS_Enable)
646 		if (qos_actived) {
647 			hdr_len = IEEE80211_3ADDR_LEN + 2;
648 
649 			skb->priority = ieee80211_classify(skb, &ieee->current_network);
650 			qos_ctl |= skb->priority; //set in the ieee80211_classify
651 			header.qos_ctl = cpu_to_le16(qos_ctl & IEEE80211_QOS_TID);
652 		} else {
653 			hdr_len = IEEE80211_3ADDR_LEN;
654 		}
655 		/* Determine amount of payload per fragment.  Regardless of if
656 		 * this stack is providing the full 802.11 header, one will
657 		 * eventually be affixed to this fragment -- so we must account for
658 		 * it when determining the amount of payload space.
659 		 */
660 		bytes_per_frag = frag_size - hdr_len;
661 		if (ieee->config &
662 		(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
663 			bytes_per_frag -= IEEE80211_FCS_LEN;
664 
665 		/* Each fragment may need to have room for encryption pre/postfix */
666 		if (encrypt)
667 			bytes_per_frag -= crypt->ops->extra_prefix_len +
668 				crypt->ops->extra_postfix_len;
669 
670 		/* Number of fragments is the total bytes_per_frag /
671 		 * payload_per_fragment
672 		 */
673 		nr_frags = bytes / bytes_per_frag;
674 		bytes_last_frag = bytes % bytes_per_frag;
675 		if (bytes_last_frag)
676 			nr_frags++;
677 		else
678 			bytes_last_frag = bytes_per_frag;
679 
680 		/* When we allocate the TXB we allocate enough space for the reserve
681 		 * and full fragment bytes (bytes_per_frag doesn't include prefix,
682 		 * postfix, header, FCS, etc.)
683 		 */
684 		txb = ieee80211_alloc_txb(nr_frags, frag_size + ieee->tx_headroom, GFP_ATOMIC);
685 		if (unlikely(!txb)) {
686 			netdev_warn(ieee->dev, "Could not allocate TXB\n");
687 			goto failed;
688 		}
689 		txb->encrypted = encrypt;
690 		txb->payload_size = __cpu_to_le16(bytes);
691 
692 		//if (ieee->current_network.QoS_Enable)
693 		if (qos_actived)
694 			txb->queue_index = UP2AC(skb->priority);
695 		else
696 			txb->queue_index = WME_AC_BK;
697 
698 
699 
700 		for (i = 0; i < nr_frags; i++) {
701 			skb_frag = txb->fragments[i];
702 			tcb_desc = (struct cb_desc *)(skb_frag->cb + MAX_DEV_ADDR_SIZE);
703 			if (qos_actived) {
704 				skb_frag->priority = skb->priority;//UP2AC(skb->priority);
705 				tcb_desc->queue_index =  UP2AC(skb->priority);
706 			} else {
707 				skb_frag->priority = WME_AC_BK;
708 				tcb_desc->queue_index = WME_AC_BK;
709 			}
710 			skb_reserve(skb_frag, ieee->tx_headroom);
711 
712 			if (encrypt) {
713 				if (ieee->hwsec_active)
714 					tcb_desc->bHwSec = 1;
715 				else
716 					tcb_desc->bHwSec = 0;
717 				skb_reserve(skb_frag, crypt->ops->extra_prefix_len);
718 			} else {
719 				tcb_desc->bHwSec = 0;
720 			}
721 			frag_hdr = skb_put_data(skb_frag, &header, hdr_len);
722 
723 			/* If this is not the last fragment, then add the MOREFRAGS
724 			 * bit to the frame control
725 			 */
726 			if (i != nr_frags - 1) {
727 				frag_hdr->frame_ctl = cpu_to_le16(
728 					fc | IEEE80211_FCTL_MOREFRAGS);
729 				bytes = bytes_per_frag;
730 
731 			} else {
732 				/* The last fragment takes the remaining length */
733 				bytes = bytes_last_frag;
734 			}
735 			//if(ieee->current_network.QoS_Enable)
736 			if (qos_actived) {
737 				// add 1 only indicate to corresponding seq number control 2006/7/12
738 				frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[UP2AC(skb->priority) + 1] << 4 | i);
739 			} else {
740 				frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4 | i);
741 			}
742 
743 			/* Put a SNAP header on the first fragment */
744 			if (i == 0) {
745 				ieee80211_put_snap(
746 					skb_put(skb_frag, SNAP_SIZE + sizeof(u16)),
747 					ether_type);
748 				bytes -= SNAP_SIZE + sizeof(u16);
749 			}
750 
751 			skb_put_data(skb_frag, skb->data, bytes);
752 
753 			/* Advance the SKB... */
754 			skb_pull(skb, bytes);
755 
756 			/* Encryption routine will move the header forward in order
757 			 * to insert the IV between the header and the payload
758 			 */
759 			if (encrypt)
760 				ieee80211_encrypt_fragment(ieee, skb_frag, hdr_len);
761 			if (ieee->config &
762 			(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
763 				skb_put(skb_frag, 4);
764 		}
765 
766 		if (qos_actived) {
767 			if (ieee->seq_ctrl[UP2AC(skb->priority) + 1] == 0xFFF)
768 				ieee->seq_ctrl[UP2AC(skb->priority) + 1] = 0;
769 			else
770 				ieee->seq_ctrl[UP2AC(skb->priority) + 1]++;
771 		} else {
772 			if (ieee->seq_ctrl[0] == 0xFFF)
773 				ieee->seq_ctrl[0] = 0;
774 			else
775 				ieee->seq_ctrl[0]++;
776 		}
777 	} else {
778 		if (unlikely(skb->len < sizeof(struct rtl_80211_hdr_3addr))) {
779 			netdev_warn(ieee->dev, "skb too small (%d).\n",
780 				    skb->len);
781 			goto success;
782 		}
783 
784 		txb = ieee80211_alloc_txb(1, skb->len, GFP_ATOMIC);
785 		if (!txb) {
786 			netdev_warn(ieee->dev, "Could not allocate TXB\n");
787 			goto failed;
788 		}
789 
790 		txb->encrypted = 0;
791 		txb->payload_size = __cpu_to_le16(skb->len);
792 		skb_put_data(txb->fragments[0], skb->data, skb->len);
793 	}
794 
795  success:
796 //WB add to fill data tcb_desc here. only first fragment is considered, need to change, and you may remove to other place.
797 	if (txb) {
798 		tcb_desc = (struct cb_desc *)(txb->fragments[0]->cb + MAX_DEV_ADDR_SIZE);
799 		tcb_desc->bTxEnableFwCalcDur = 1;
800 		if (is_multicast_ether_addr(header.addr1))
801 			tcb_desc->bMulticast = 1;
802 		if (is_broadcast_ether_addr(header.addr1))
803 			tcb_desc->bBroadcast = 1;
804 		ieee80211_txrate_selectmode(ieee, tcb_desc);
805 		if (tcb_desc->bMulticast ||  tcb_desc->bBroadcast)
806 			tcb_desc->data_rate = ieee->basic_rate;
807 		else
808 			tcb_desc->data_rate = CURRENT_RATE(ieee->mode, ieee->rate, ieee->HTCurrentOperaRate);
809 		ieee80211_qurey_ShortPreambleMode(ieee, tcb_desc);
810 		ieee80211_tx_query_agg_cap(ieee, txb->fragments[0], tcb_desc);
811 		ieee80211_query_HTCapShortGI(ieee, tcb_desc);
812 		ieee80211_query_BandwidthMode(ieee, tcb_desc);
813 		ieee80211_query_protectionmode(ieee, tcb_desc, txb->fragments[0]);
814 		ieee80211_query_seqnum(ieee, txb->fragments[0], header.addr1);
815 	}
816 	spin_unlock_irqrestore(&ieee->lock, flags);
817 	dev_kfree_skb_any(skb);
818 	if (txb) {
819 		if (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE) {
820 			ieee80211_softmac_xmit(txb, ieee);
821 		} else {
822 			if ((*ieee->hard_start_xmit)(txb, dev) == 0) {
823 				stats->tx_packets++;
824 				stats->tx_bytes += __le16_to_cpu(txb->payload_size);
825 				return NETDEV_TX_OK;
826 			}
827 			ieee80211_txb_free(txb);
828 		}
829 	}
830 
831 	return NETDEV_TX_OK;
832 
833  failed:
834 	spin_unlock_irqrestore(&ieee->lock, flags);
835 	netif_stop_queue(dev);
836 	stats->tx_errors++;
837 	return 1;
838 
839 }
840