/******************************************************************************
|
*
|
* Copyright(c) 2019 Realtek Corporation.
|
*
|
* This program is free software; you can redistribute it and/or modify it
|
* under the terms of version 2 of the GNU General Public License as
|
* published by the Free Software Foundation.
|
*
|
* This program is distributed in the hope that it will be useful, but WITHOUT
|
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
|
* more details.
|
*
|
*****************************************************************************/
|
#include <drv_types.h>
|
|
static const u8 bcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
#ifdef RTW_PHL_TEST_FPGA
|
static const u8 ap_addr[ETH_ALEN] = {0x00, 0xe0, 0x4c, 0x88, 0x52, 0xa0};
|
u8 ap_ssid[] = {'p','h','l','-','5','g'};
|
#else
|
static const u8 ap_addr[ETH_ALEN] = {0x00, 0xe0, 0x4c, 0x12, 0x34, 0x56};
|
u8 ap_ssid[] = {'f','a','k','e','a','p'};
|
#endif
|
u8 ap_ch = 1;
|
u16 ap_beacon_interval = 100; /* unit: ms */
|
u16 ap_capability_info = 0x0001;
|
u8 ap_datarate[] = {0x82, 0x84, 0x8b, 0x96, 0xc, 0x12, 0x18, 0x24};
|
|
int rtw_fakeap_tx(struct _ADAPTER *a, struct xmit_frame *frame)
|
{
|
u8 *data = frame->buf_addr + TXDESC_OFFSET;
|
u32 data_len = frame->attrib.last_txcmdsz;
|
struct sk_buff *skb;
|
u8 *buf;
|
|
|
if (GetFrameType(data) != WIFI_MGT_TYPE)
|
return _FAIL;
|
|
switch (get_frame_sub_type(data)) {
|
case WIFI_AUTH:
|
case WIFI_ASSOCREQ:
|
case WIFI_REASSOCREQ:
|
case WIFI_PROBEREQ:
|
break;
|
|
default:
|
return _FAIL;
|
}
|
|
skb = rtw_skb_alloc(data_len);
|
buf = skb_put(skb, data_len);
|
_rtw_memcpy(buf, data, data_len);
|
rtw_free_xmitbuf(&a->xmitpriv, frame->pxmitbuf);
|
rtw_free_xmitframe(&a->xmitpriv, frame);
|
|
skb_queue_tail(&adapter_to_dvobj(a)->fakeap.rxq, skb);
|
_set_workitem(&adapter_to_dvobj(a)->fakeap.work);
|
|
return _SUCCESS;
|
}
|
|
static void add_ie(struct sk_buff *skb, u8 index, u8 len, const u8 *source)
|
{
|
u8 *buf;
|
|
|
buf = skb_put(skb, 2 + len);
|
*buf = index;
|
*(buf + 1) = len;
|
if (len > 0)
|
_rtw_memcpy(buf + 2, source, len);
|
}
|
|
void rtw_fakeap_bcn_timer_hdl(void *p)
|
{
|
struct dvobj_priv *d;
|
struct sk_buff *skb;
|
struct rtw_ieee80211_hdr *wlanhdr;
|
int len;
|
u8 *buf;
|
u8 hdrlen;
|
|
|
d = (struct dvobj_priv*)p;
|
hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
|
len = hdrlen + _FIXED_IE_LENGTH_
|
+ 2 + sizeof(ap_ssid)
|
+ 2 + sizeof(ap_datarate)
|
+ 2 + 1;
|
skb = rtw_skb_alloc(len);
|
_rtw_memset(skb->data, 0, len);
|
|
buf = skb_put(skb, hdrlen);
|
wlanhdr = (struct rtw_ieee80211_hdr*)buf;
|
set_frame_sub_type(&wlanhdr->frame_ctl, WIFI_BEACON);
|
_rtw_memcpy(wlanhdr->addr1, bcast_addr, ETH_ALEN);
|
_rtw_memcpy(wlanhdr->addr2, ap_addr, ETH_ALEN);
|
_rtw_memcpy(wlanhdr->addr3, ap_addr, ETH_ALEN);
|
|
buf = skb_put(skb, _FIXED_IE_LENGTH_);
|
/* timestamp: 8 bytes */
|
/* beacon interval: 2 bytes */
|
*(u16*)(buf + 8) = ap_beacon_interval;
|
/* capability info: 2 bytes */
|
*(u16*)(buf + 10) = ap_capability_info;
|
|
/* SSID */
|
add_ie(skb, _SSID_IE_, sizeof(ap_ssid), ap_ssid);
|
|
/* supported rates... */
|
add_ie(skb, _SUPPORTEDRATES_IE_, sizeof(ap_datarate), ap_datarate);
|
|
/* DS parameter set */
|
add_ie(skb, _DSSET_IE_, 1, &ap_ch);
|
|
skb_queue_tail(&d->fakeap.rxq, skb);
|
_set_workitem(&d->fakeap.work);
|
}
|
|
static void rx_prepare(struct _ADAPTER *adapter, union recv_frame *r)
|
{
|
struct rx_pkt_attrib *a;
|
|
|
/* init recv_frame */
|
_rtw_init_listhead(&r->u.hdr.list);
|
r->u.hdr.len = 0;
|
a = &r->u.hdr.attrib;
|
_rtw_memset(a, 0, sizeof(*a));
|
|
/* fill sec related attrib, iv_len and icv_len will be filled by
|
* validate_recv_data_frame() */
|
a->crc_err = 0;
|
a->icv_err = 0;
|
a->encrypt = 0;
|
|
/* fill rx pkt attrib */
|
a->hdrlen = 0;
|
a->bw = CHANNEL_WIDTH_MAX;
|
a->pkt_len = 0;
|
a->pkt_rpt_type = NORMAL_RX;
|
a->drvinfo_sz = 0;
|
a->bdecrypted = 0;
|
a->qos = 0;
|
a->priority = 0;
|
a->amsdu = 0;
|
a->mdata = 0;
|
a->mfrag = 0;
|
a->seq_num = 0;
|
a->frag_num = 0;
|
a->data_rate = DESC_RATE6M;
|
a->ppdu_cnt = 1;
|
a->free_cnt = 0;
|
}
|
|
static union recv_frame* gen_probersp(struct _ADAPTER *a)
|
{
|
union recv_frame *r;
|
u32 len;
|
struct sk_buff *skb;
|
struct rtw_ieee80211_hdr *wlanhdr;
|
u8 *buf;
|
u8 hdrlen;
|
u8 ielen;
|
|
|
r = rtw_alloc_recvframe(&a->recvpriv.free_recv_queue);
|
if (!r) {
|
RTW_ERR("%s: alloc recvframe failed!\n", __func__);
|
return NULL;
|
}
|
rtw_init_recvframe(r, &a->recvpriv);
|
rx_prepare(a, r);
|
|
hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
|
len = RXDESC_OFFSET + hdrlen + _CAPABILITY_ + _STATUS_CODE_ + _ASOC_ID_
|
+ 2 + sizeof(ap_ssid)
|
+ 2 + sizeof(ap_datarate)
|
+ 2 + 1;
|
skb = rtw_skb_alloc(len);
|
_rtw_memset(skb->data, 0, len);
|
skb->dev = a->pnetdev;
|
skb_reserve(skb, RXDESC_OFFSET);
|
r->u.hdr.pkt = skb;
|
|
buf = skb_put(skb, hdrlen);
|
wlanhdr = (struct rtw_ieee80211_hdr*)buf;
|
set_frame_sub_type(&wlanhdr->frame_ctl, WIFI_PROBERSP);
|
_rtw_memcpy(wlanhdr->addr1, adapter_mac_addr(a), ETH_ALEN);
|
_rtw_memcpy(wlanhdr->addr2, ap_addr, ETH_ALEN);
|
_rtw_memcpy(wlanhdr->addr3, ap_addr, ETH_ALEN);
|
|
buf = skb_put(skb, _FIXED_IE_LENGTH_);
|
/* timestamp: 8 bytes */
|
/* beacon interval: 2 bytes */
|
*(u16*)(buf + 8) = ap_beacon_interval;
|
/* capability info: 2 bytes */
|
*(u16*)(buf + 10) = ap_capability_info;
|
|
/* SSID */
|
add_ie(skb, _SSID_IE_, sizeof(ap_ssid), ap_ssid);
|
|
/* supported rates... */
|
add_ie(skb, _SUPPORTEDRATES_IE_, sizeof(ap_datarate), ap_datarate);
|
|
/* DS parameter set */
|
add_ie(skb, _DSSET_IE_, 1, &ap_ch);
|
|
/* handle r->u.hdr.attrib */
|
|
/* handle recv_frame pointer */
|
r->u.hdr.len = skb->len;
|
r->u.hdr.rx_head = skb->head;
|
r->u.hdr.rx_data = skb->data;
|
r->u.hdr.rx_tail = skb_tail_pointer(skb);
|
r->u.hdr.rx_end = skb_end_pointer(skb);
|
|
return r;
|
}
|
|
static union recv_frame* gen_beacon(struct _ADAPTER *a, struct sk_buff *beacon)
|
{
|
union recv_frame *r;
|
u32 len;
|
struct sk_buff *skb;
|
struct rtw_ieee80211_hdr *wlanhdr;
|
u8 *buf;
|
u8 hdrlen;
|
|
|
r = rtw_alloc_recvframe(&a->recvpriv.free_recv_queue);
|
if (!r) {
|
RTW_ERR("%s: alloc recvframe failed!\n", __func__);
|
return NULL;
|
}
|
rtw_init_recvframe(r, &a->recvpriv);
|
rx_prepare(a, r);
|
|
hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
|
len = RXDESC_OFFSET + beacon->len;
|
skb = rtw_skb_alloc(len);
|
_rtw_memset(skb->data, 0, len);
|
skb->dev = a->pnetdev;
|
skb_reserve(skb, RXDESC_OFFSET);
|
r->u.hdr.pkt = skb;
|
|
buf = skb_put(skb, beacon->len);
|
_rtw_memcpy(buf, beacon->data, beacon->len);
|
|
/* handle r->u.hdr.attrib */
|
|
/* handle recv_frame pointer */
|
r->u.hdr.len = skb->len;
|
r->u.hdr.rx_head = skb->head;
|
r->u.hdr.rx_data = skb->data;
|
r->u.hdr.rx_tail = skb_tail_pointer(skb);
|
r->u.hdr.rx_end = skb_end_pointer(skb);
|
|
return r;
|
}
|
|
static union recv_frame *gen_auth(struct _ADAPTER *a, u8 *bssid)
|
{
|
union recv_frame *r;
|
u32 len;
|
struct sk_buff *skb;
|
struct rtw_ieee80211_hdr *wlanhdr;
|
u8 *buf;
|
u8 hdrlen;
|
|
|
r = rtw_alloc_recvframe(&a->recvpriv.free_recv_queue);
|
if (!r) {
|
RTW_ERR("%s: alloc recvframe failed!\n", __func__);
|
return NULL;
|
}
|
rtw_init_recvframe(r, &a->recvpriv);
|
rx_prepare(a, r);
|
|
hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
|
len = RXDESC_OFFSET + hdrlen
|
+ _AUTH_ALGM_NUM_ + _AUTH_SEQ_NUM_ + _STATUS_CODE_;
|
skb = rtw_skb_alloc(len);
|
_rtw_memset(skb->data, 0, len);
|
skb->dev = a->pnetdev;
|
skb_reserve(skb, RXDESC_OFFSET);
|
r->u.hdr.pkt = skb;
|
|
buf = skb_put(skb, hdrlen);
|
wlanhdr = (struct rtw_ieee80211_hdr*)buf;
|
set_frame_sub_type(&wlanhdr->frame_ctl, WIFI_AUTH);
|
_rtw_memcpy(wlanhdr->addr1, adapter_mac_addr(a), ETH_ALEN);
|
_rtw_memcpy(wlanhdr->addr2, bssid, ETH_ALEN);
|
_rtw_memcpy(wlanhdr->addr3, bssid, ETH_ALEN);
|
|
/* setting auth algo number */
|
buf = skb_put(skb, _AUTH_ALGM_NUM_);
|
*(u16*)buf = cpu_to_le16(0);;
|
|
/* setting auth seq number */
|
buf = skb_put(skb, _AUTH_SEQ_NUM_);
|
*(u16*)buf = cpu_to_le16(2);
|
|
/* setting status code... */
|
buf = skb_put(skb, _STATUS_CODE_);
|
*(u16*)buf = cpu_to_le16(0);
|
|
/* handle r->u.hdr.attrib */
|
|
/* handle recv_frame pointer */
|
r->u.hdr.len = skb->len;
|
r->u.hdr.rx_head = skb->head;
|
r->u.hdr.rx_data = skb->data;
|
r->u.hdr.rx_tail = skb_tail_pointer(skb);
|
r->u.hdr.rx_end = skb_end_pointer(skb);
|
|
return r;
|
}
|
|
static union recv_frame *gen_assocrsp(struct _ADAPTER *a, u8 *bssid)
|
{
|
union recv_frame *r;
|
u32 len;
|
struct sk_buff *skb;
|
struct rtw_ieee80211_hdr *wlanhdr;
|
u8 *buf;
|
u8 hdrlen;
|
|
|
r = rtw_alloc_recvframe(&a->recvpriv.free_recv_queue);
|
if (!r) {
|
RTW_ERR("%s: alloc recvframe failed!\n", __func__);
|
return NULL;
|
}
|
rtw_init_recvframe(r, &a->recvpriv);
|
rx_prepare(a, r);
|
|
hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
|
len = RXDESC_OFFSET + hdrlen
|
+ _CAPABILITY_ + _STATUS_CODE_ + _ASOC_ID_
|
+ 2 + sizeof(ap_datarate);
|
skb = rtw_skb_alloc(len);
|
_rtw_memset(skb->data, 0, len);
|
skb->dev = a->pnetdev;
|
skb_reserve(skb, RXDESC_OFFSET);
|
r->u.hdr.pkt = skb;
|
|
buf = skb_put(skb, hdrlen);
|
wlanhdr = (struct rtw_ieee80211_hdr*)buf;
|
set_frame_sub_type(&wlanhdr->frame_ctl, WIFI_ASSOCRSP);
|
_rtw_memcpy(wlanhdr->addr1, adapter_mac_addr(a), ETH_ALEN);
|
_rtw_memcpy(wlanhdr->addr2, bssid, ETH_ALEN);
|
_rtw_memcpy(wlanhdr->addr3, bssid, ETH_ALEN);
|
|
|
/* capability info: 2 bytes */
|
buf = skb_put(skb, _CAPABILITY_);
|
*(u16*)buf = ap_capability_info;
|
|
/* status code: 2 bytes */
|
buf = skb_put(skb, _STATUS_CODE_);
|
*(u16*)buf = 0;
|
|
/* AID: 2 bytes */
|
buf = skb_put(skb, _ASOC_ID_);
|
*(u16*)buf = 100;
|
|
/* supported rates... */
|
add_ie(skb, _SUPPORTEDRATES_IE_, sizeof(ap_datarate), ap_datarate);
|
|
/* handle r->u.hdr.attrib */
|
|
/* handle recv_frame pointer */
|
r->u.hdr.len = skb->len;
|
r->u.hdr.rx_head = skb->head;
|
r->u.hdr.rx_data = skb->data;
|
r->u.hdr.rx_tail = skb_tail_pointer(skb);
|
r->u.hdr.rx_end = skb_end_pointer(skb);
|
|
// recvframe_put(r, buflen);
|
// pre_recv_entry(r, NULL);
|
|
return r;
|
}
|
|
static union recv_frame* fakeap_dispatcher(struct _ADAPTER *a,
|
struct sk_buff *skb)
|
{
|
u8 *pframe;
|
union recv_frame *rframe = NULL;
|
|
|
pframe = skb->data;
|
RTW_INFO("A1-" MAC_FMT "\n", MAC_ARG(GetAddr1Ptr(pframe)));
|
RTW_INFO("A2-" MAC_FMT "\n", MAC_ARG(get_addr2_ptr(pframe)));
|
RTW_INFO("A3-" MAC_FMT "\n", MAC_ARG(GetAddr3Ptr(pframe)));
|
|
if (GetFrameType(pframe) != WIFI_MGT_TYPE)
|
return NULL;
|
|
switch (get_frame_sub_type(pframe)) {
|
case WIFI_AUTH:
|
{
|
u16 algo, seq;
|
|
algo = le16_to_cpu(*(u16 *)(pframe + WLAN_HDR_A3_LEN));
|
seq = le16_to_cpu(*(u16 *)(pframe + WLAN_HDR_A3_LEN + 2));
|
|
if ((algo == 0) && (seq == 1))
|
rframe = gen_auth(a, GetAddr1Ptr(pframe));
|
}
|
break;
|
|
case WIFI_ASSOCREQ:
|
case WIFI_REASSOCREQ:
|
rframe = gen_assocrsp(a, GetAddr1Ptr(pframe));
|
break;
|
|
case WIFI_PROBEREQ:
|
rframe = gen_probersp(a);
|
break;
|
|
case WIFI_BEACON:
|
rframe = gen_beacon(a, skb);
|
|
default:
|
break;
|
}
|
|
return rframe;
|
}
|
|
extern sint validate_recv_frame(struct _ADAPTER*, union recv_frame**);
|
void rtw_fakeap_work(struct work_struct *work)
|
{
|
struct dvobj_priv *d;
|
struct _ADAPTER *a;
|
struct sk_buff_head *pktq;
|
struct sk_buff *skb, *tmp;
|
u8 *ta, *ra;
|
union recv_frame *rframe;
|
int i;
|
|
|
d = container_of(work, struct dvobj_priv, fakeap.work);
|
|
pktq = &d->fakeap.rxq;
|
skb_queue_walk_safe(pktq, skb, tmp) {
|
__skb_unlink(skb, pktq);
|
|
ta = get_addr2_ptr(skb->data);
|
ra = GetAddr1Ptr(skb->data);
|
if (is_broadcast_mac_addr(ra)
|
&& (_rtw_memcmp(ta, ap_addr, ETH_ALEN)==_TRUE)
|
&& (get_frame_sub_type(skb->data) == WIFI_BEACON)) {
|
for (i = 0; i < d->iface_nums; i++) {
|
a = d->padapters[i];
|
if (a) {
|
rframe = fakeap_dispatcher(a, skb);
|
if (rframe) {
|
validate_recv_frame(a, &rframe);
|
rframe = NULL;
|
}
|
}
|
}
|
} else {
|
a = rtw_get_iface_by_macddr(dvobj_get_primary_adapter(d),
|
ta);
|
if (a)
|
rframe = fakeap_dispatcher(a, skb);
|
}
|
rtw_skb_free(skb);
|
if (rframe) {
|
validate_recv_frame(a, &rframe);
|
rframe = NULL;
|
}
|
}
|
}
|
