From 327d7144b0bdba5703b6e8f773e9c3ab0ffdffe3 Mon Sep 17 00:00:00 2001 From: Sergei Makarenkov Date: Fri, 20 Jan 2017 11:31:04 +0300 Subject: [PATCH] Parse radiotap headers for proper frame injection --- core/rtw_mlme_ext.c | 57 +++++++++ core/rtw_xmit.c | 188 ++++++++++++++++++++++++++++++ hal/rtl8812a/usb/rtl8812au_xmit.c | 32 ++++- include/rtw_xmit.h | 1 + 4 files changed, 275 insertions(+), 3 deletions(-) diff --git a/core/rtw_mlme_ext.c b/core/rtw_mlme_ext.c index 7ddc312..37bdfbf 100644 --- a/core/rtw_mlme_ext.c +++ b/core/rtw_mlme_ext.c @@ -7308,6 +7308,63 @@ void update_mgnt_tx_rate(_adapter *padapter, u8 rate) //DBG_871X("%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; + + pattrib->hdrlen = 24; + pattrib->nr_frags = 1; + pattrib->priority = 7; + pattrib->inject = 0xa5; + + pattrib->mac_id = 0; + pattrib->qsel = QSLT_MGNT; + pattrib->pktlen = 0; + + if (pmlmeext->tx_rate == IEEE80211_CCK_RATE_1MB) + wireless_mode = WIRELESS_11B; + else + 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 + pattrib->rate = MGN_VHT1SS_MCS9; + #else + pattrib->rate = MGN_MCS7; + #endif + + pattrib->encrypt = _NO_PRIVACY_; + pattrib->bswenc = _FALSE; + + pattrib->qos_en = _FALSE; + pattrib->ht_en = 1; + pattrib->bwmode = CHANNEL_WIDTH_20; + pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + pattrib->sgi = _FALSE; + + pattrib->seqnum = pmlmeext->mgnt_seq; + + 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 a741c9e..fa6bef3 100644 --- a/core/rtw_xmit.c +++ b/core/rtw_xmit.c @@ -3753,6 +3753,194 @@ static void do_queue_select(_adapter *padapter, struct pkt_attrib *pattrib) pattrib->qsel = qsel; } +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; + + //DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev)); + + if (skb) + rtw_mstat_update(MSTAT_TYPE_SKB, MSTAT_ALLOC_SUCCESS, skb->truesize); + + if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header))) + goto fail; + + rtap_hdr = (struct ieee80211_radiotap_header *)skb->data; + if (unlikely(rtap_hdr->it_version)) + goto fail; + + rtap_len = ieee80211_get_radiotap_len(skb->data); + if (unlikely(skb->len < 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 ieee80211_hdr *)skb->data; +// frame_ctl = le16_to_cpu(dot11_hdr->frame_control); + /* Check if the QoS bit is set */ + + 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; + + //printk("**** rt mcs %x rate %x raid %d sgi %d bwidth %d ldpc %d stbc %d txflags %x\n", fixed_rate, pattrib->rate, pattrib->raid, sgi, bwidth, ldpc, stbc, txflags); + 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; +} + /* * The main transmit(tx) entry * diff --git a/hal/rtl8812a/usb/rtl8812au_xmit.c b/hal/rtl8812a/usb/rtl8812au_xmit.c index d1b4fb8..34b8e40 100755 --- a/hal/rtl8812a/usb/rtl8812au_xmit.c +++ b/hal/rtl8812a/usb/rtl8812au_xmit.c @@ -303,6 +303,31 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz ,u8 bag SET_TX_DESC_GID_8812(ptxdesc, pattrib->txbf_g_id); SET_TX_DESC_PAID_8812(ptxdesc, pattrib->txbf_p_aid); +/* 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; @@ -445,7 +470,7 @@ static s32 rtw_dump_xframe(_adapter *padapter, struct xmit_frame *pxmitframe) rtw_count_tx_stats(padapter, pxmitframe, sz); RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_write_port, w_sz=%d\n", w_sz)); - //DBG_8192C("rtw_write_port, w_sz=%d, sz=%d, txdesc_sz=%d, tid=%d\n", w_sz, sz, w_sz-sz, pattrib->priority); + //DBG_8192C("rtw_write_port, w_sz=%d, sz=%d, txdesc_sz=%d, tid=%d, nr_frags=%d %x\n", w_sz, sz, w_sz-sz, pattrib->priority, pattrib->nr_frags, pxmitbuf ->pbuf[0]); mem_addr += w_sz; @@ -1078,7 +1103,8 @@ s32 rtl8812au_hostap_mgnt_xmit_entry(_adapter *padapter, _pkt *pkt) ptxdesc->txdw3 |= cpu_to_le32((8 <<28)); //set bit3 to 1. Suugested by TimChen. 2009.12.29. - rtl8188eu_cal_txdesc_chksum(ptxdesc); + //rtl8188eu_cal_txdesc_chksum(ptxdesc); + rtl8812au_cal_txdesc_chksum(ptxdesc); // ----- end of fill tx desc ----- // @@ -1096,7 +1122,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); diff --git a/include/rtw_xmit.h b/include/rtw_xmit.h index 1429361..54800b4 100644 --- a/include/rtw_xmit.h +++ b/include/rtw_xmit.h @@ -407,6 +407,7 @@ struct pkt_attrib u8 icmp_pkt; u16 txbf_p_aid;/*beamforming Partial_AID*/ u16 txbf_g_id;/*beamforming Group ID*/ + u8 inject; /* == a5 if injected */ }; #endif