From 7b407130a0c9e3469f31af1dfd7e6ddc1ec56e4a Mon Sep 17 00:00:00 2001 From: astsam Date: Sun, 9 Apr 2017 11:42:39 +0300 Subject: [PATCH] Add frame injection --- core/rtw_mlme_ext.c | 53 +++---- core/rtw_xmit.c | 233 ++++++++++++++++++------------ hal/rtl8812a/usb/rtl8812au_xmit.c | 37 ++++- include/rtw_xmit.h | 3 +- 4 files changed, 194 insertions(+), 132 deletions(-) diff --git a/core/rtw_mlme_ext.c b/core/rtw_mlme_ext.c index 5a39673..9ff7eaf 100755 --- a/core/rtw_mlme_ext.c +++ b/core/rtw_mlme_ext.c @@ -7098,32 +7098,20 @@ void update_mgnt_tx_rate(_adapter *padapter, u8 rate) /* RTW_INFO("%s(): rate = %x\n",__FUNCTION__, rate); */ } - void update_monitor_frame_attrib(_adapter *padapter, struct pkt_attrib *pattrib) { HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); u8 wireless_mode; struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); - struct xmit_priv *pxmitpriv = &padapter->xmitpriv; - struct sta_info *psta = NULL; - struct sta_priv *pstapriv = &padapter->stapriv; - struct sta_info *pbcmc_sta = NULL; - - psta = rtw_get_stainfo(pstapriv, pattrib->ra); - pbcmc_sta = rtw_get_bcmc_stainfo(padapter); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; pattrib->hdrlen = 24; pattrib->nr_frags = 1; pattrib->priority = 7; + pattrib->inject = 0xa5; - if (pbcmc_sta) - pattrib->mac_id = pbcmc_sta->mac_id; - else { - pattrib->mac_id = 0; - RTW_INFO("mgmt use mac_id 0 will affect RA\n"); - } + pattrib->mac_id = 0; pattrib->qsel = QSLT_MGNT; - pattrib->pktlen = 0; if (pmlmeext->tx_rate == IEEE80211_CCK_RATE_1MB) @@ -7132,25 +7120,25 @@ void update_monitor_frame_attrib(_adapter *padapter, struct pkt_attrib *pattrib) wireless_mode = WIRELESS_11G; pattrib->raid = rtw_get_mgntframe_raid(padapter, wireless_mode); -#ifdef CONFIG_80211AC_VHT - if (pHalData->rf_type == RF_1T1R) - pattrib->raid = RATEID_IDX_VHT_1SS; - else if (pHalData->rf_type == RF_2T2R || pHalData->rf_type == RF_2T4R) - pattrib->raid = RATEID_IDX_VHT_2SS; - else if (pHalData->rf_type == RF_3T3R) - pattrib->raid = RATEID_IDX_VHT_3SS; - else - pattrib->raid = RATEID_IDX_BGN_40M_1SS; -#endif + #ifdef CONFIG_80211AC_VHT + if (pHalData->rf_type == RF_1T1R) + pattrib->raid = RATEID_IDX_VHT_1SS; + else if (pHalData->rf_type == RF_2T2R || pHalData->rf_type == RF_2T4R) + pattrib->raid = RATEID_IDX_VHT_2SS; + else if (pHalData->rf_type == RF_3T3R) + pattrib->raid = RATEID_IDX_VHT_3SS; + else + pattrib->raid = RATEID_IDX_BGN_40M_1SS; + #endif -#ifdef CONFIG_80211AC_VHT - pattrib->rate = MGN_VHT1SS_MCS9; -#else - pattrib->rate = MGN_MCS7; -#endif + #ifdef CONFIG_80211AC_VHT + pattrib->rate = MGN_VHT1SS_MCS9; + #else + pattrib->rate = MGN_MCS7; + #endif pattrib->encrypt = _NO_PRIVACY_; - pattrib->bswenc = _FALSE; + pattrib->bswenc = _FALSE; pattrib->qos_en = _FALSE; pattrib->ht_en = 1; @@ -7160,14 +7148,13 @@ void update_monitor_frame_attrib(_adapter *padapter, struct pkt_attrib *pattrib) pattrib->seqnum = pmlmeext->mgnt_seq; - pattrib->retry_ctrl = _TRUE; + pattrib->retry_ctrl = _FALSE; pattrib->mbssid = 0; pattrib->hw_ssn_sel = pxmitpriv->hw_ssn_seq_no; } - void update_mgntframe_attrib(_adapter *padapter, struct pkt_attrib *pattrib) { u8 wireless_mode; diff --git a/core/rtw_xmit.c b/core/rtw_xmit.c index 91bce15..b26bd2a 100644 --- a/core/rtw_xmit.c +++ b/core/rtw_xmit.c @@ -3806,6 +3806,16 @@ static void do_queue_select(_adapter *padapter, struct pkt_attrib *pattrib) #endif /* CONFIG_MCC_MODE */ } + +static inline void dump_buf(u8 *buf, u32 len) +{ + u32 i; + printk("-----------------Len %d----------------\n", len); + for(i=0; ixmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + u8 *buf = skb->data; + u32 len = skb->len; + u8 category, action; + int type = -1; + + //RTW_INFO(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev)); + if (skb) rtw_mstat_update(MSTAT_TYPE_SKB, MSTAT_ALLOC_SUCCESS, skb->truesize); @@ -3843,107 +3876,121 @@ s32 rtw_monitor_xmit_entry(struct sk_buff *skb, struct net_device *ndev) if (unlikely(skb->len < rtap_len)) goto fail; - if (rtap_len != 12) { - RTW_INFO("radiotap len (should be 14): %d\n", rtap_len); - goto fail; - } + ret = rtw_ieee80211_radiotap_iterator_init(&iterator, rtap_hdr, skb->len, NULL); + while (!ret) { + ret = rtw_ieee80211_radiotap_iterator_next(&iterator); + if (ret) + continue; + + /* see if this argument is something we can use */ + switch (iterator.this_arg_index) { + + case IEEE80211_RADIOTAP_RATE: /* u8 */ + fixed_rate = *iterator.this_arg; + break; + + case IEEE80211_RADIOTAP_TX_FLAGS: + txflags = get_unaligned_le16(iterator.this_arg); + break; + + case IEEE80211_RADIOTAP_MCS: { /* u8,u8,u8 */ + u8 mcs_have = iterator.this_arg[0]; + if (mcs_have & IEEE80211_RADIOTAP_MCS_HAVE_MCS) { + fixed_rate = iterator.this_arg[2] & 0x7f; + if(fixed_rate > 31) + fixed_rate = 0; + fixed_rate += MGN_MCS0; + } + if ((mcs_have & 4) && + (iterator.this_arg[1] & 4)) + sgi = 1; + if ((mcs_have & 1) && + (iterator.this_arg[1] & 1)) + bwidth = 1; + if ((mcs_have & 0x10) && + (iterator.this_arg[1] & 0x10)) + ldpc = 1; + if ((mcs_have & 0x20)) + stbc = (iterator.this_arg[1] >> 5) & 3; + } + break; + + case IEEE80211_RADIOTAP_VHT: { + /* u16 known, u8 flags, u8 bandwidth, u8 mcs_nss[4], u8 coding, u8 group_id, u16 partial_aid */ + u8 known = iterator.this_arg[0]; + u8 flags = iterator.this_arg[2]; + unsigned int mcs, nss; + if((known & 4) && (flags & 4)) + sgi = 1; + if((known & 1) && (flags & 1)) + stbc = 1; + if(known & 0x40) { + bwidth = iterator.this_arg[3] & 0x1f; + if(bwidth>=1 && bwidth<=3) + bwidth = 1; // 40 MHz + else if(bwidth>=4 && bwidth<=10) + bwidth = 2; // 80 MHz + else + bwidth = 0; // 20 MHz + } + if(iterator.this_arg[8] & 1) + ldpc = 1; + mcs = (iterator.this_arg[4]>>4) & 0x0f; + nss = iterator.this_arg[4] & 0x0f; + if(nss > 0) { + if(nss > 4) nss = 4; + if(mcs > 9) mcs = 9; + fixed_rate = MGN_VHT1SS_MCS0 + ((nss-1)*10 + mcs); + } + } + break; + + default: + break; + } + } /* Skip the ratio tap header */ skb_pull(skb, rtap_len); - dot11_hdr = (struct rtw_ieee80211_hdr *)skb->data; - frame_ctl = le16_to_cpu(dot11_hdr->frame_ctl); +// dot11_hdr = (struct ieee80211_hdr *)skb->data; +// frame_ctl = le16_to_cpu(dot11_hdr->frame_control); /* Check if the QoS bit is set */ - - if ((frame_ctl & RTW_IEEE80211_FCTL_FTYPE) == RTW_IEEE80211_FTYPE_DATA) { - - struct xmit_frame *pmgntframe; - struct pkt_attrib *pattrib; - unsigned char *pframe; - struct rtw_ieee80211_hdr *pwlanhdr; - struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); - struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); - u8 *buf = skb->data; - u32 len = skb->len; - u8 category, action; - int type = -1; - - pmgntframe = alloc_mgtxmitframe(pxmitpriv); - if (pmgntframe == NULL) { - rtw_udelay_os(500); - goto fail; - } - pattrib = &pmgntframe->attrib; - - update_monitor_frame_attrib(padapter, pattrib); - - pattrib->retry_ctrl = _FALSE; - - _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); - - pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; - - _rtw_memcpy(pframe, (void *)buf, len); - - pattrib->pktlen = len; - - pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; - - if (is_broadcast_mac_addr(pwlanhdr->addr3) || is_broadcast_mac_addr(pwlanhdr->addr1)) - pattrib->rate = MGN_24M; - - pmlmeext->mgnt_seq = GetSequence(pwlanhdr); - pattrib->seqnum = pmlmeext->mgnt_seq; - pmlmeext->mgnt_seq++; - - pattrib->last_txcmdsz = pattrib->pktlen; - - dump_mgntframe(padapter, pmgntframe); - - } else { - struct xmit_frame *pmgntframe; - struct pkt_attrib *pattrib; - unsigned char *pframe; - struct rtw_ieee80211_hdr *pwlanhdr; - struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); - struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); - u8 *buf = skb->data; - u32 len = skb->len; - u8 category, action; - int type = -1; - - pmgntframe = alloc_mgtxmitframe(pxmitpriv); - if (pmgntframe == NULL) - goto fail; - - pattrib = &pmgntframe->attrib; - update_mgntframe_attrib(padapter, pattrib); - pattrib->retry_ctrl = _FALSE; - - _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); - - pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; - - _rtw_memcpy(pframe, (void *)buf, len); - - pattrib->pktlen = len; - - pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; - - pmlmeext->mgnt_seq = GetSequence(pwlanhdr); - pattrib->seqnum = pmlmeext->mgnt_seq; - pmlmeext->mgnt_seq++; - - pattrib->last_txcmdsz = pattrib->pktlen; - - dump_mgntframe(padapter, pmgntframe); - + + if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL) { + rtw_udelay_os(500); + goto fail; } + pattrib = &pmgntframe->attrib; + update_monitor_frame_attrib(padapter, pattrib); + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + + _rtw_memcpy(pframe, (void*)skb->data, skb->len); + + pattrib->pktlen = skb->len; + + pattrib->rate = fixed_rate; + pattrib->sgi = sgi; + pattrib->bwmode = bwidth; // 0-20 MHz, 1-40 MHz, 2-80 MHz + pattrib->ldpc = ldpc; + pattrib->stbc = stbc; + pattrib->retry_ctrl = (txflags & 0x08)?_FALSE:_TRUE; + + + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + pmlmeext->mgnt_seq = GetSequence(pwlanhdr); + pattrib->seqnum = pmlmeext->mgnt_seq; + pmlmeext->mgnt_seq++; + + pattrib->last_txcmdsz = pattrib->pktlen; + dump_mgntframe(padapter, pmgntframe); + fail: - rtw_skb_free(skb); - return 0; } diff --git a/hal/rtl8812a/usb/rtl8812au_xmit.c b/hal/rtl8812a/usb/rtl8812au_xmit.c index dce1e9c..bc5f941 100644 --- a/hal/rtl8812a/usb/rtl8812au_xmit.c +++ b/hal/rtl8812a/usb/rtl8812au_xmit.c @@ -289,6 +289,32 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz , u8 ba SET_TX_DESC_GID_8812(ptxdesc, pattrib->txbf_g_id); SET_TX_DESC_PAID_8812(ptxdesc, pattrib->txbf_p_aid); #endif + +/* injected frame */ + if(pattrib->inject == 0xa5) { + SET_TX_DESC_RETRY_LIMIT_ENABLE_8812(ptxdesc, 1); + if (pattrib->retry_ctrl == _TRUE) { + SET_TX_DESC_DATA_RETRY_LIMIT_8812(ptxdesc, 6); + } else { + SET_TX_DESC_DATA_RETRY_LIMIT_8812(ptxdesc, 0); + } + if(pattrib->sgi == _TRUE) { + SET_TX_DESC_DATA_SHORT_8812(ptxdesc, 1); + } else { + SET_TX_DESC_DATA_SHORT_8812(ptxdesc, 0); + } + SET_TX_DESC_USE_RATE_8812(ptxdesc, 1); + SET_TX_DESC_TX_RATE_8812(ptxdesc, MRateToHwRate(pattrib->rate)); + if (pattrib->ldpc) + SET_TX_DESC_DATA_LDPC_8812(ptxdesc, 1); + SET_TX_DESC_DATA_STBC_8812(ptxdesc, pattrib->stbc & 3); + //SET_TX_DESC_GF_8812(ptxdesc, 1); // no MCS rates if sets, GreenField? + //SET_TX_DESC_LSIG_TXOP_EN_8812(ptxdesc, 1); + //SET_TX_DESC_HTC_8812(ptxdesc, 1); + //SET_TX_DESC_NO_ACM_8812(ptxdesc, 1); + SET_TX_DESC_DATA_BW_8812(ptxdesc, pattrib->bwmode); // 0 - 20 MHz, 1 - 40 MHz, 2 - 80 MHz + } + rtl8812a_cal_txdesc_chksum(ptxdesc); _dbg_dump_tx_info(padapter, pxmitframe->frame_tag, ptxdesc); return pull; @@ -410,6 +436,7 @@ static s32 rtw_dump_xframe(_adapter *padapter, struct xmit_frame *pxmitframe) rtw_count_tx_stats(padapter, pxmitframe, sz); /* RTW_INFO("rtw_write_port, w_sz=%d, sz=%d, txdesc_sz=%d, tid=%d\n", w_sz, sz, w_sz-sz, pattrib->priority); */ + RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_write_port, w_sz=%d\n", w_sz)); mem_addr += w_sz; @@ -1015,9 +1042,9 @@ s32 rtl8812au_hostap_mgnt_xmit_entry(_adapter *padapter, _pkt *pkt) ptxdesc->txdw4 |= cpu_to_le32(BIT(7)); /* Hw set sequence number */ ptxdesc->txdw3 |= cpu_to_le32((8 << 28)); /* set bit3 to 1. Suugested by TimChen. 2009.12.29. */ - - rtl8188eu_cal_txdesc_chksum(ptxdesc); - /* ----- end of fill tx desc ----- */ + //rtl8188eu_cal_txdesc_chksum(ptxdesc); + rtl8812au_cal_txdesc_chksum(ptxdesc); + // ----- end of fill tx desc ----- /* */ skb_put(pxmit_skb, len + TXDESC_SIZE); @@ -1034,8 +1061,7 @@ s32 rtl8812au_hostap_mgnt_xmit_entry(_adapter *padapter, _pkt *pkt) pipe = usb_sndbulkpipe(pdvobj->pusbdev, pHalData->Queue2EPNum[(u8)MGT_QUEUE_INX] & 0x0f); usb_fill_bulk_urb(urb, pdvobj->pusbdev, pipe, - pxmit_skb->data, pxmit_skb->len, rtl8192cu_hostap_mgnt_xmit_cb, pxmit_skb); - + pxmit_skb->data, pxmit_skb->len, rtl8812au_hostap_mgnt_xmit_cb, pxmit_skb); urb->transfer_flags |= URB_ZERO_PACKET; usb_anchor_urb(urb, &phostapdpriv->anchored); rc = usb_submit_urb(urb, GFP_ATOMIC); @@ -1056,3 +1082,4 @@ _exit: } #endif + diff --git a/include/rtw_xmit.h b/include/rtw_xmit.h index 4c35ef1..d057ed7 100644 --- a/include/rtw_xmit.h +++ b/include/rtw_xmit.h @@ -447,7 +447,8 @@ struct pkt_attrib { u16 txbf_p_aid;/*beamforming Partial_AID*/ u16 txbf_g_id;/*beamforming Group ID*/ #endif - + u8 inject; /* == a5 if injected */ + }; #endif