1 files changed, 338 insertions, 48 deletions

diff --git a/drivers/net/wireless/rtl8723as/core/rtw_xmit.c b/drivers/net/wireless/rtl8723as/core/rtw_xmit.c
index 0ba3bce864fb..73043534f56c 100755
--- a/drivers/net/wireless/rtl8723as/core/rtw_xmit.c
+++ b/drivers/net/wireless/rtl8723as/core/rtw_xmit.c
@@ -1960,6 +1960,244 @@ _func_exit_;
 	return res;
 }
 
+#ifdef CONFIG_IEEE80211W
+//broadcast or multicast management pkt use BIP, unicast management pkt use CCMP encryption
+s32 rtw_mgmt_xmitframe_coalesce(_adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe)
+{
+	struct pkt_file pktfile;
+	s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
+	SIZE_PTR addr;
+	u8 *pframe, *mem_start = NULL, *tmp_buf=NULL;
+	u8 hw_hdr_offset, subtype ;
+	struct sta_info		*psta = NULL;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	u8 *pbuf_start;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+	s32 res = _FAIL;
+	u8 *BIP_AAD;
+	u8 *MGMT_body=NULL;
+	
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	u8 MME[_MME_IE_LENGTH_];
+	
+	_irqL irqL;
+	u32	ori_len;
+	mem_start = pframe = (u8 *)(pxmitframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+	tmp_buf = BIP_AAD;
+	
+_func_enter_;
+	ori_len = BIP_AAD_SIZE+pattrib->pktlen;
+	tmp_buf = BIP_AAD = rtw_zmalloc(ori_len);
+	subtype = GetFrameSubType(pframe); //bit(7)~bit(2)
+	
+	if(BIP_AAD == NULL)
+		return _FAIL;
+			
+	_enter_critical_bh(&padapter->security_key_mutex, &irqL);
+	
+	//only support station mode
+	if(!check_fwstate(pmlmepriv, WIFI_STATION_STATE) || !check_fwstate(pmlmepriv, _FW_LINKED))
+		goto xmitframe_coalesce_success;
+	
+	//IGTK key is not install, it may not support 802.11w
+	if(padapter->securitypriv.binstallBIPkey != _TRUE)
+	{
+		DBG_871X("no instll BIP key\n");
+		goto xmitframe_coalesce_success;
+	}
+	//station mode doesn't need TX BIP, just ready the code
+	if(bmcst)
+	{
+		int frame_body_len;
+		u8 mic[16];
+		
+		_rtw_memset(MME, 0, 18);
+				
+		//other types doesn't need the BIP
+		if(GetFrameSubType(pframe) != WIFI_DEAUTH && GetFrameSubType(pframe) != WIFI_DISASSOC)
+			goto xmitframe_coalesce_fail;
+		
+		MGMT_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+		pframe += pattrib->pktlen;
+		
+		//octent 0 and 1 is key index ,BIP keyid is 4 or 5, LSB only need octent 0
+		MME[0]=padapter->securitypriv.dot11wBIPKeyid;
+		//copy packet number
+		_rtw_memcpy(&MME[2], &pmlmeext->mgnt_80211w_IPN, 6);
+		//increase the packet number
+		pmlmeext->mgnt_80211w_IPN++;
+		
+		//add MME IE with MIC all zero, MME string doesn't include element id and length
+		pframe = rtw_set_ie(pframe, _MME_IE_ , 16 , MME, &(pattrib->pktlen));
+		pattrib->last_txcmdsz = pattrib->pktlen;
+		// total frame length - header length
+		frame_body_len = pattrib->pktlen - sizeof(struct rtw_ieee80211_hdr_3addr);
+		
+		//conscruct AAD, copy frame control field
+		_rtw_memcpy(BIP_AAD, &pwlanhdr->frame_ctl, 2);
+		ClearRetry(BIP_AAD);
+		ClearPwrMgt(BIP_AAD);
+		ClearMData(BIP_AAD);
+		//conscruct AAD, copy address 1 to address 3
+		_rtw_memcpy(BIP_AAD+2, pwlanhdr->addr1, 18);
+		//copy management fram body
+		_rtw_memcpy(BIP_AAD+BIP_AAD_SIZE, MGMT_body, frame_body_len);
+		/*//dump total packet include MME with zero MIC
+		{
+			int i;
+			printk("Total packet: ");
+			for(i=0; i < BIP_AAD_SIZE+frame_body_len; i++)
+				printk(" %02x ", BIP_AAD[i]);
+			printk("\n");
+		}*/
+		//calculate mic
+		if(omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey
+			, BIP_AAD, BIP_AAD_SIZE+frame_body_len, mic))
+			goto xmitframe_coalesce_fail;
+		
+		/*//dump calculated mic result
+		{
+			int i;
+			printk("Calculated mic result: ");
+			for(i=0; i<16; i++)
+				printk(" %02x ", mic[i]);
+			printk("\n");
+		}*/
+		//copy right BIP mic value, total is 128bits, we use the 0~63 bits
+		_rtw_memcpy(pframe-8, mic, 8);
+		/*/dump all packet after mic ok
+		{
+			int pp;
+			printk("pattrib->pktlen = %d \n", pattrib->pktlen);
+			for(pp=0;pp< pattrib->pktlen; pp++)
+				printk(" %02x ", mem_start[pp]);
+			printk("\n");
+		}*/
+	}
+	else //unicast mgmt frame TX
+	{
+		//start to encrypt mgmt frame
+		if(subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC ||
+			subtype == WIFI_REASSOCREQ || subtype == WIFI_ACTION)
+		{
+			if (pattrib->psta)
+				psta = pattrib->psta;
+			else
+			{
+				psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+			}
+		
+			if(psta==NULL)
+		    {
+				
+				DBG_871X("%s, psta==NUL\n", __func__);
+				goto xmitframe_coalesce_fail;
+			}
+		
+			if(!(psta->state & _FW_LINKED) || pxmitframe->buf_addr==NULL)
+			{
+				DBG_871X("%s, not _FW_LINKED or addr null\n", __func__);
+				goto xmitframe_coalesce_fail;
+			}
+			
+			//DBG_871X("%s, action frame category=%d \n", __func__, pframe[WLAN_HDR_A3_LEN]);
+			//according 802.11-2012 standard, these five types are not robust types
+			if(subtype == WIFI_ACTION && 
+			(pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_PUBLIC ||
+			pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_HT ||
+			pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_UNPROTECTED_WNM ||
+			pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_SELF_PROTECTED  ||
+			pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_P2P))
+				goto xmitframe_coalesce_fail;
+			/*//before encrypt dump the management packet content
+			{
+				int i;
+				printk("Management pkt: ");
+				for(i=0; i<pattrib->pktlen; i++)
+				printk(" %02x ", pframe[i]);
+				printk("=======\n");
+			}*/
+			
+			//bakeup original management packet
+			_rtw_memcpy(tmp_buf, pframe, pattrib->pktlen);
+			//move to data portion
+			pframe += pattrib->hdrlen;
+			
+			//802.11w unicast management packet must be _AES_
+			pattrib->iv_len = 8;
+			//it's MIC of AES
+			pattrib->icv_len = 8;
+			
+			switch(pattrib->encrypt)
+			{
+				case _AES_:
+						//set AES IV header
+						AES_IV(pattrib->iv, psta->dot11wtxpn, 0);
+					break;
+				default:
+					goto xmitframe_coalesce_fail;
+			}
+			//insert iv header into management frame
+			_rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len);
+			pframe += pattrib->iv_len;
+			//copy mgmt data portion after CCMP header
+			_rtw_memcpy(pframe, tmp_buf+pattrib->hdrlen, pattrib->pktlen-pattrib->hdrlen);
+			//move pframe to end of mgmt pkt
+			pframe += pattrib->pktlen-pattrib->hdrlen;
+			//add 8 bytes CCMP IV header to length
+			pattrib->pktlen += pattrib->iv_len;
+			/*//dump management packet include AES IV header
+			{
+				int i;
+				printk("Management pkt + IV: ");
+				//for(i=0; i<pattrib->pktlen; i++)
+				//printk(" %02x ", mem_start[i]);
+				printk("@@@@@@@@@@@@@\n");
+			}*/
+			
+			if ((pattrib->icv_len >0 )&& (pattrib->bswenc)) {
+				_rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len); 
+				pframe += pattrib->icv_len;
+			}
+			//add 8 bytes MIC
+			pattrib->pktlen += pattrib->icv_len;
+			//set final tx command size
+			pattrib->last_txcmdsz = pattrib->pktlen;
+	
+			//set protected bit must be beofre SW encrypt
+			SetPrivacy(mem_start);
+			/*//dump management packet include AES header
+			{
+				int i;
+				printk("prepare to enc Management pkt + IV: ");
+				for(i=0; i<pattrib->pktlen; i++)
+				printk(" %02x ", mem_start[i]);
+				printk("@@@@@@@@@@@@@\n");
+			}*/
+			//software encrypt
+			xmitframe_swencrypt(padapter, pxmitframe);
+		}
+	}
+
+xmitframe_coalesce_success:
+	_exit_critical_bh(&padapter->security_key_mutex, &irqL);
+	rtw_mfree(BIP_AAD, ori_len);
+_func_exit_;
+	return _SUCCESS;
+	
+xmitframe_coalesce_fail:
+	_exit_critical_bh(&padapter->security_key_mutex, &irqL);
+	rtw_mfree(BIP_AAD, ori_len);
+_func_exit_;
+
+	return _FAIL;
+}
+#endif //CONFIG_IEEE80211W
+
 /* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
  * IEEE LLC/SNAP header contains 8 octets
  * First 3 octets comprise the LLC portion
@@ -2996,14 +3234,14 @@ int rtw_br_client_tx(_adapter *padapter, struct sk_buff **pskb)
 						*((unsigned short *)(skb->data+MACADDRLEN*2+2)) = vlan_hdr;
 					}
 
-					newskb = skb_copy(skb, GFP_ATOMIC);
+					newskb = rtw_skb_copy(skb);
 					if (newskb == NULL) {
 						//priv->ext_stats.tx_drops++;
-						DEBUG_ERR("TX DROP: skb_copy fail!\n");
+						DEBUG_ERR("TX DROP: rtw_skb_copy fail!\n");
 						//goto stop_proc;
 						return -1;
 					}
-					dev_kfree_skb_any(skb);
+					rtw_skb_free(skb);
 
 					*pskb = skb = newskb;
 					if (is_vlan_tag) {
@@ -3606,10 +3844,12 @@ void wakeup_sta_to_xmit(_adapter *padapter, struct sta_info *psta)
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	
+
 	//_enter_critical_bh(&psta->sleep_q.lock, &irqL);
 	_enter_critical_bh(&pxmitpriv->lock, &irqL);
 
-
 	xmitframe_phead = get_list_head(&psta->sleep_q);
 	xmitframe_plist = get_next(xmitframe_phead);
 
@@ -3678,52 +3918,12 @@ void wakeup_sta_to_xmit(_adapter *padapter, struct sta_info *psta)
 		
 	}	
 	
-	if(psta->sleepq_len==0)
-	{
-#ifdef CONFIG_TDLS
-		if( psta->tdls_sta_state & TDLS_LINKED_STATE )
-		{
-			if(psta->state&WIFI_SLEEP_STATE)
-				psta->state ^= WIFI_SLEEP_STATE;
-
-			//_exit_critical_bh(&psta->sleep_q.lock, &irqL);
-			_exit_critical_bh(&pxmitpriv->lock, &irqL);
-			return;
-		}
-#endif //CONFIG_TDLS
-		pstapriv->tim_bitmap &= ~BIT(psta->aid);
-
-		//DBG_871X("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap);
-		//upate BCN for TIM IE
-		//update_BCNTIM(padapter);
-		update_mask = BIT(0);
-
-		if(psta->state&WIFI_SLEEP_STATE)
-			psta->state ^= WIFI_SLEEP_STATE;
-
-		if(psta->state & WIFI_STA_ALIVE_CHK_STATE)
-		{
-			psta->expire_to = pstapriv->expire_to;
-			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
-		}
-
-		pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
-	}
-	
-	//_exit_critical_bh(&psta->sleep_q.lock, &irqL);
-	_exit_critical_bh(&pxmitpriv->lock, &irqL);
-
-
 	//for BC/MC Frames
-	psta_bmc = rtw_get_bcmc_stainfo(padapter);
 	if(!psta_bmc)
-		return;
+		goto _exit;
 
 	if((pstapriv->sta_dz_bitmap&0xfffe) == 0x0)//no any sta in ps mode
 	{
-		//_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
-		_enter_critical_bh(&pxmitpriv->lock, &irqL);
-
 		xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
 		xmitframe_plist = get_next(xmitframe_phead);
 
@@ -3767,11 +3967,42 @@ void wakeup_sta_to_xmit(_adapter *padapter, struct sta_info *psta)
 			update_mask |= BIT(1);
 		}		
 	
-		//_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);	
-		_exit_critical_bh(&pxmitpriv->lock, &irqL);
-
 	}	
 
+	if(psta->sleepq_len==0)
+	{
+#ifdef CONFIG_TDLS
+		if( psta->tdls_sta_state & TDLS_LINKED_STATE )
+		{
+			if(psta->state&WIFI_SLEEP_STATE)
+				psta->state ^= WIFI_SLEEP_STATE;
+
+			goto _exit;
+		}
+#endif //CONFIG_TDLS
+		pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+		//DBG_871X("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap);
+		//upate BCN for TIM IE
+		//update_BCNTIM(padapter);
+		update_mask = BIT(0);
+
+		if(psta->state&WIFI_SLEEP_STATE)
+			psta->state ^= WIFI_SLEEP_STATE;
+
+		if(psta->state & WIFI_STA_ALIVE_CHK_STATE)
+		{
+			psta->expire_to = pstapriv->expire_to;
+			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+		}
+
+		pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	}
+
+_exit:
+
+	//_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);	
+	_exit_critical_bh(&pxmitpriv->lock, &irqL);
 	
 	if(update_mask)
 	{
@@ -4150,8 +4381,66 @@ int rtw_ack_tx_polling(struct xmit_priv *pxmitpriv, u32 timeout_ms)
 }
 #endif
 
+#ifdef CONFIG_DETECT_C2H_BY_POLLING
+s32 c2h_evt_hdl(_adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter);
+#endif
+
 int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
 {
+#ifdef CONFIG_DETECT_C2H_BY_POLLING
+	_adapter *adapter = container_of(pxmitpriv, _adapter, xmitpriv);
+	c2h_id_filter ccx_id_filter = rtw_hal_c2h_id_filter_ccx(adapter);
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+	u8 check_c2hcmd;
+	u8 check_ccx;
+	int ret = _FAIL;
+
+	pack_tx_ops->submit_time = rtw_get_current_time();
+	pack_tx_ops->timeout_ms = timeout_ms;
+	pack_tx_ops->status = RTW_SCTX_SUBMITTED;
+
+	do {
+		rtw_msleep_os(10);
+		//check_c2hcmd = rtw_read8(adapter, 0x1AF);
+		//check_ccx = rtw_read8(adapter, 0x1A0);
+		rtw_hal_get_hwreg(adapter, HW_VAR_C2HEVT_CLEAR, (u8 *)(&check_c2hcmd));
+		rtw_hal_get_hwreg(adapter, HW_VAR_C2HEVT_MSG_NORMAL, (u8 *)(&check_ccx));
+
+		
+		if (check_c2hcmd != 0)
+		{
+			if (check_c2hcmd != 0xFF)
+			{
+				c2h_evt_clear(adapter);	
+			}
+			else if (ccx_id_filter(check_ccx & 0x0F) == _TRUE)
+			{
+				c2h_evt_hdl(adapter, NULL, ccx_id_filter);
+				if (pack_tx_ops->status != RTW_SCTX_SUBMITTED)
+					break;
+
+				if (adapter->bDriverStopped) {
+					pack_tx_ops->status = RTW_SCTX_DONE_DRV_STOP;
+					break;
+				}
+				if (adapter->bSurpriseRemoved) {
+					pack_tx_ops->status = RTW_SCTX_DONE_DEV_REMOVE;
+					break;
+				}
+			}
+		}			
+	} while (rtw_get_passing_time_ms(pack_tx_ops->submit_time) < timeout_ms);
+
+	if (pack_tx_ops->status == RTW_SCTX_SUBMITTED) {
+		pack_tx_ops->status = RTW_SCTX_DONE_TIMEOUT;
+		DBG_871X("%s timeout\n", __func__);
+	}
+
+	if (pack_tx_ops->status == RTW_SCTX_DONE_SUCCESS)
+		ret = _SUCCESS;
+
+	return ret;
+#else
 #ifdef CONFIG_XMIT_ACK_POLLING
 	return rtw_ack_tx_polling(pxmitpriv, timeout_ms);
 #else
@@ -4163,6 +4452,7 @@ int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
 
 	return rtw_sctx_wait(pack_tx_ops);
 #endif
+#endif
 }
 
 void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status)