From fe96207594a5dfabb68013d873a8c10887c91dce Mon Sep 17 00:00:00 2001 From: Raphael <68374617+raphaelscholle@users.noreply.github.com> Date: Tue, 2 Jan 2024 00:01:40 +0100 Subject: [PATCH] Update rtw_recv.c --- core/rtw_recv.c | 388 +++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 386 insertions(+), 2 deletions(-) diff --git a/core/rtw_recv.c b/core/rtw_recv.c index ab261e8..19d423d 100644 --- a/core/rtw_recv.c +++ b/core/rtw_recv.c @@ -3892,8 +3892,9 @@ int recv_frame_monitor(_adapter *padapter, union recv_frame *rframe, struct rtw_ if (ndev->type == ARPHRD_IEEE80211_RADIOTAP) { /* fill radiotap header */ - if (rtw_fill_radiotap_hdr(padapter, rframe->u.hdr.adapter_link, - &rframe->u.hdr.attrib, rx_req, (u8 *)pskb) == _FAIL) { + // if (rtw_fill_radiotap_hdr(padapter, rframe->u.hdr.adapter_link, + // &rframe->u.hdr.attrib, rx_req, (u8 *)pskb) == _FAIL) { + if (rtl8812au_fill_radiotap_hdr(padapter, rframe) == _FAIL) { ret = _FAIL; goto exit; } @@ -4240,7 +4241,390 @@ int mp_recv_frame(_adapter *padapter, union recv_frame *rframe) } #endif +static sint rtl8812au_fill_radiotap_hdr(_adapter *padapter, union recv_frame *precvframe) +{ +#define CHAN2FREQ(a) ((a < 14) ? (2407+5*a) : (5000+5*a)) + +#if 0 + #define RTW_RX_RADIOTAP_PRESENT (\ + (1 << IEEE80211_RADIOTAP_TSFT) | \ + (1 << IEEE80211_RADIOTAP_FLAGS) | \ + (1 << IEEE80211_RADIOTAP_RATE) | \ + (1 << IEEE80211_RADIOTAP_CHANNEL) | \ + (0 << IEEE80211_RADIOTAP_FHSS) | \ + (1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL) | \ + (1 << IEEE80211_RADIOTAP_DBM_ANTNOISE) | \ + (0 << IEEE80211_RADIOTAP_LOCK_QUALITY) | \ + (0 << IEEE80211_RADIOTAP_TX_ATTENUATION) | \ + (0 << IEEE80211_RADIOTAP_DB_TX_ATTENUATION) | \ + (0 << IEEE80211_RADIOTAP_DBM_TX_POWER) | \ + (1 << IEEE80211_RADIOTAP_ANTENNA) | \ + (1 << IEEE80211_RADIOTAP_DB_ANTSIGNAL) | \ + (0 << IEEE80211_RADIOTAP_DB_ANTNOISE) | \ + (0 << IEEE80211_RADIOTAP_RX_FLAGS) | \ + (0 << IEEE80211_RADIOTAP_TX_FLAGS) | \ + (0 << IEEE80211_RADIOTAP_RTS_RETRIES) | \ + (0 << IEEE80211_RADIOTAP_DATA_RETRIES) | \ + (0 << IEEE80211_RADIOTAP_MCS) | \ + (0 << IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE)| \ + (0 << IEEE80211_RADIOTAP_VENDOR_NAMESPACE) | \ + (0 << IEEE80211_RADIOTAP_EXT) | \ + 0) + + /* (0 << IEEE80211_RADIOTAP_AMPDU_STATUS) | \ */ + /* (0 << IEEE80211_RADIOTAP_VHT) | \ */ +#endif + +#ifndef IEEE80211_RADIOTAP_RX_FLAGS +#define IEEE80211_RADIOTAP_RX_FLAGS 14 +#endif + +#ifndef IEEE80211_RADIOTAP_MCS +#define IEEE80211_RADIOTAP_MCS 19 +#endif +#ifndef IEEE80211_RADIOTAP_VHT +#define IEEE80211_RADIOTAP_VHT 21 +#endif + +#ifndef IEEE80211_RADIOTAP_F_BADFCS +#define IEEE80211_RADIOTAP_F_BADFCS 0x40 /* bad FCS */ +#endif + + sint ret = _SUCCESS; + _adapter *adapter = precvframe->u.hdr.adapter; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib; + + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + + u16 tmp_16bit = 0; + + static u8 data_rate[] = { + 2, 4, 11, 22, /* CCK */ + 12, 18, 24, 36, 48, 72, 93, 108, /* OFDM */ + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, /* HT MCS index */ + 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, /* VHT Nss 1 */ + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, /* VHT Nss 2 */ + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, /* VHT Nss 3 */ + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, /* VHT Nss 4 */ + }; + + _pkt *pskb = NULL; + + struct ieee80211_radiotap_header *rtap_hdr = NULL; + u8 *ptr = NULL; +#ifdef CONFIG_RADIOTAP_WITH_RXDESC + u8 hdr_buf[128] = {0}; +#else + u8 hdr_buf[64] = {0}; +#endif + u16 rt_len = 8; + u32 tmp_32bit; + int i; + + /* create header */ + rtap_hdr = (struct ieee80211_radiotap_header *)&hdr_buf[0]; + rtap_hdr->it_version = PKTHDR_RADIOTAP_VERSION; + + if(pHalData->NumTotalRFPath>0 && pattrib->physt) { + rtap_hdr->it_present |= (1<NumTotalRFPath>1) { + tmp_32bit = (1<NumTotalRFPath-1; i++) { + memcpy(&hdr_buf[rt_len], &tmp_32bit, 4); + rt_len += 4; + } + } + tmp_32bit = (1<it_present |= (1 << IEEE80211_RADIOTAP_VENDOR_NAMESPACE); +#endif + } + + /* tsft */ + /*if (pattrib->tsfl) { + u64 tmp_64bit; + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_TSFT); + tmp_64bit = cpu_to_le64(pattrib->tsfl); + memcpy(&hdr_buf[rt_len], &tmp_64bit, 8); + rt_len += 8; + }*/ + + /* flags */ + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_FLAGS); + if (0) + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_CFP; + + if (0) + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_SHORTPRE; + + if ((pattrib->encrypt == 1) || (pattrib->encrypt == 5)) + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_WEP; + + if (pattrib->mfrag) + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_FRAG; + + /* always append FCS */ + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_FCS; + + + if (0) + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_DATAPAD; + + if (pattrib->crc_err) + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_BADFCS; + + if (pattrib->sgi) { + /* Currently unspecified but used */ + hdr_buf[rt_len] |= 0x80; + } + rt_len += 1; + + /* rate */ + if (pattrib->data_rate < 12) { + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_RATE); + if (pattrib->data_rate < 4) { + /* CCK */ + hdr_buf[rt_len] = data_rate[pattrib->data_rate]; + } else { + /* OFDM */ + hdr_buf[rt_len] = data_rate[pattrib->data_rate]; + } + } + rt_len += 1; /* force padding 1 byte for aligned */ + + /* channel */ + tmp_16bit = 0; + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_CHANNEL); + tmp_16bit = CHAN2FREQ(rtw_get_oper_ch(padapter)); + /*tmp_16bit = CHAN2FREQ(pHalData->current_channel);*/ + memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; + + /* channel flags */ + tmp_16bit = 0; + if (pHalData->current_band_type == 0) + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_2GHZ); + else + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_5GHZ); + + if (pattrib->data_rate < 12) { + if (pattrib->data_rate < 4) { + /* CCK */ + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_CCK); + } else { + /* OFDM */ + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_OFDM); + } + } else + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_DYN); + memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; + + if(pattrib->physt) { + /* dBm Antenna Signal */ + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL); + hdr_buf[rt_len] = pattrib->phy_info.recv_signal_power; + rt_len += 1; + +#if 0 + /* dBm Antenna Noise */ + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_DBM_ANTNOISE); + hdr_buf[rt_len] = 0; + rt_len += 1; +#endif + + rt_len++; // alignment + } + + /* Signal Quality */ + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_LOCK_QUALITY); + tmp_16bit = cpu_to_le16(pattrib->phy_info.signal_quality); + memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; +#if 0 + /* Antenna */ + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_ANTENNA); + hdr_buf[rt_len] = pHalData->rf_type; + rt_len += 1; + + rt_len++; // alignment +#endif + /* RX flags */ + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_RX_FLAGS); +#if 0 + tmp_16bit = cpu_to_le16(0); + memcpy(ptr, &tmp_16bit, 1); +#endif + rt_len += 2; + + /* MCS information */ + if (pattrib->data_rate >= 12 && pattrib->data_rate < 44) { + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_MCS); + /* known, flag */ + hdr_buf[rt_len] |= BIT1; /* MCS index known */ + + /* bandwidth */ + hdr_buf[rt_len] |= BIT0; + hdr_buf[rt_len + 1] |= (pattrib->bw & 0x03); + + /* guard interval */ + hdr_buf[rt_len] |= BIT2; + hdr_buf[rt_len + 1] |= (pattrib->sgi & 0x01) << 2; + + /* STBC */ + hdr_buf[rt_len] |= BIT5; + hdr_buf[rt_len + 1] |= (pattrib->stbc & 0x03) << 5; + + rt_len += 2; + + /* MCS rate index */ + hdr_buf[rt_len] = data_rate[pattrib->data_rate]; + rt_len += 1; + } + + /* VHT */ + if (pattrib->data_rate >= 44 && pattrib->data_rate < 84) { + rtap_hdr->it_present |= (1 << IEEE80211_RADIOTAP_VHT); + + /* known 16 bit, flag 8 bit */ + tmp_16bit = 0; + + /* Bandwidth */ + tmp_16bit |= BIT6; + + /* Group ID */ + tmp_16bit |= BIT7; + + /* Partial AID */ + tmp_16bit |= BIT8; + + /* STBC */ + tmp_16bit |= BIT0; + hdr_buf[rt_len + 2] |= (pattrib->stbc & 0x01); + + /* Guard interval */ + tmp_16bit |= BIT2; + hdr_buf[rt_len + 2] |= (pattrib->sgi & 0x01) << 2; + + /* LDPC extra OFDM symbol */ + tmp_16bit |= BIT4; + hdr_buf[rt_len + 2] |= (pattrib->ldpc & 0x01) << 4; + + memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 3; + + /* bandwidth */ + if (pattrib->bw == 0) + hdr_buf[rt_len] |= 0; + else if (pattrib->bw == 1) + hdr_buf[rt_len] |= 1; + else if (pattrib->bw == 2) + hdr_buf[rt_len] |= 4; + else if (pattrib->bw == 3) + hdr_buf[rt_len] |= 11; + rt_len += 1; + + /* mcs_nss */ + if (pattrib->data_rate >= 44 && pattrib->data_rate < 54) { + hdr_buf[rt_len] |= 1; + hdr_buf[rt_len] |= data_rate[pattrib->data_rate] << 4; + } else if (pattrib->data_rate >= 54 && pattrib->data_rate < 64) { + hdr_buf[rt_len + 1] |= 2; + hdr_buf[rt_len + 1] |= data_rate[pattrib->data_rate] << 4; + } else if (pattrib->data_rate >= 64 && pattrib->data_rate < 74) { + hdr_buf[rt_len + 2] |= 3; + hdr_buf[rt_len + 2] |= data_rate[pattrib->data_rate] << 4; + } else if (pattrib->data_rate >= 74 && pattrib->data_rate < 84) { + hdr_buf[rt_len + 3] |= 4; + hdr_buf[rt_len + 3] |= data_rate[pattrib->data_rate] << 4; + } + rt_len += 4; + + /* coding */ + hdr_buf[rt_len] = 0; + rt_len += 1; + + /* group_id */ + hdr_buf[rt_len] = 0; + rt_len += 1; + + /* partial_aid */ + tmp_16bit = 0; + memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; + } + + if (pattrib->physt) { + for(i=0; iNumTotalRFPath; i++) { + hdr_buf[rt_len] = pattrib->phy_info.rx_pwr[i]; + rt_len ++; + hdr_buf[rt_len] = i; + rt_len ++; + } + } + +#ifdef CONFIG_RADIOTAP_WITH_RXDESC + rt_len += rt_len&1; + hdr_buf[rt_len++] = 0xde; + hdr_buf[rt_len++] = 0xab; + hdr_buf[rt_len++] = 0xbe; + hdr_buf[rt_len++] = 0xaf; + hdr_buf[rt_len++] = 24; + hdr_buf[rt_len++] = 0; + _rtw_memcpy(hdr_buf + rt_len, pattrib->rxdesc, RXDESC_SIZE); + rt_len += RXDESC_SIZE; +#endif + + /* push to skb */ + /* read skb information from recv frame */ + pskb = precvframe->u.hdr.pkt; + pskb->len = precvframe->u.hdr.len; + pskb->data = precvframe->u.hdr.rx_data; + skb_set_tail_pointer(pskb, precvframe->u.hdr.len); + + if (skb_headroom(pskb) < rt_len) { + pskb = skb_realloc_headroom(pskb, rt_len); + if(pskb == NULL) { + RTW_INFO("%s:%d %s headroom is too small.\n", __FILE__, __LINE__, __func__); + ret = _FAIL; + return ret; + } + precvframe->u.hdr.pkt = pskb; + } + + ptr = skb_push(pskb, rt_len); + if (ptr) { + rtap_hdr->it_len = cpu_to_le16(rt_len); + _rtw_memcpy(ptr, rtap_hdr, rt_len); + } else { + ret = _FAIL; + return ret; + } + out: + /* write skb information to recv frame */ + skb_reset_mac_header(pskb); + precvframe->u.hdr.len = pskb->len; + precvframe->u.hdr.rx_data = pskb->data; + precvframe->u.hdr.rx_head = pskb->head; + precvframe->u.hdr.rx_tail = skb_tail_pointer(pskb); + precvframe->u.hdr.rx_end = skb_end_pointer(pskb); + + return ret; +} int recv_func_prehandle(_adapter *padapter, union recv_frame *rframe) { int ret = _SUCCESS;