static int rtllib_classify(struct sk_buff *skb, u8 bIsAmsdu)
{
struct ethhdr *eth;
struct iphdr *ip;
eth = (struct ethhdr *)skb->data;
if (eth->h_proto != htons(ETH_P_IP))
return 0;
RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA, skb->data, skb->len);
ip = ip_hdr(skb);
switch (ip->tos & 0xfc) {
case 0x20:
return 2;
case 0x40:
return 1;
case 0x60:
return 3;
case 0x80:
return 4;
case 0xa0:
return 5;
case 0xc0:
return 6;
case 0xe0:
return 7;
default:
return 0;
}
}
int rtllib_rx_DELBA(struct rtllib_device *ieee, struct sk_buff *skb)
{
struct rtllib_hdr_3addr *delba = NULL;
union delba_param_set *pDelBaParamSet = NULL;
u16 *pReasonCode = NULL;
u8 *dst = NULL;
if (skb->len < sizeof(struct rtllib_hdr_3addr) + 6) {
RTLLIB_DEBUG(RTLLIB_DL_ERR, " Invalid skb len in DELBA(%d /"
" %d)\n", (int)skb->len,
(int)(sizeof(struct rtllib_hdr_3addr) + 6));
return -1;
}
if (ieee->current_network.qos_data.active == 0 ||
ieee->pHTInfo->bCurrentHTSupport == false) {
RTLLIB_DEBUG(RTLLIB_DL_ERR, "received DELBA while QOS or HT "
"is not supported(%d, %d)\n",
ieee->current_network. qos_data.active,
ieee->pHTInfo->bCurrentHTSupport);
return -1;
}
RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA|RTLLIB_DL_BA, skb->data, skb->len);
delba = (struct rtllib_hdr_3addr *)skb->data;
dst = (u8 *)(&delba->addr2[0]);
delba += sizeof(struct rtllib_hdr_3addr);
pDelBaParamSet = (union delba_param_set *)(delba+2);
pReasonCode = (u16 *)(delba+4);
if (pDelBaParamSet->field.Initiator == 1) {
struct rx_ts_record *pRxTs;
if (!GetTs(ieee, (struct ts_common_info **)&pRxTs, dst,
(u8)pDelBaParamSet->field.TID, RX_DIR, false)) {
RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't get TS for RXTS in "
"%s().dst: %pM TID:%d\n", __func__, dst,
(u8)pDelBaParamSet->field.TID);
return -1;
}
RxTsDeleteBA(ieee, pRxTs);
} else {
struct tx_ts_record *pTxTs;
if (!GetTs(ieee, (struct ts_common_info **)&pTxTs, dst,
(u8)pDelBaParamSet->field.TID, TX_DIR, false)) {
RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't get TS for TXTS in "
"%s()\n", __func__);
return -1;
}
pTxTs->bUsingBa = false;
pTxTs->bAddBaReqInProgress = false;
pTxTs->bAddBaReqDelayed = false;
del_timer_sync(&pTxTs->TsAddBaTimer);
TxTsDeleteBA(ieee, pTxTs);
}
return 0;
}
static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
struct ba_record *pBA,
enum tr_select TxRxSelect, u16 ReasonCode)
{
union delba_param_set DelbaParamSet;
struct sk_buff *skb = NULL;
struct rtllib_hdr_3addr *Delba = NULL;
u8 *tag = NULL;
u16 tmp = 0;
u16 len = 6 + ieee->tx_headroom;
if (net_ratelimit())
RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA,
"========>%s(), Reason"
"Code(%d) sentd to: %pM\n", __func__,
ReasonCode, dst);
memset(&DelbaParamSet, 0, 2);
DelbaParamSet.field.Initiator = (TxRxSelect == TX_DIR) ? 1 : 0;
DelbaParamSet.field.TID = pBA->BaParamSet.field.TID;
skb = dev_alloc_skb(len + sizeof(struct rtllib_hdr_3addr));
if (skb == NULL) {
RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't alloc skb for ADDBA_REQ\n");
return NULL;
}
skb_reserve(skb, ieee->tx_headroom);
Delba = (struct rtllib_hdr_3addr *) skb_put(skb,
sizeof(struct rtllib_hdr_3addr));
memcpy(Delba->addr1, dst, ETH_ALEN);
memcpy(Delba->addr2, ieee->dev->dev_addr, ETH_ALEN);
memcpy(Delba->addr3, ieee->current_network.bssid, ETH_ALEN);
Delba->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
tag = (u8 *)skb_put(skb, 6);
*tag ++= ACT_CAT_BA;
*tag ++= ACT_DELBA;
tmp = DelbaParamSet.shortData;
memcpy(tag, (u8 *)&tmp, 2);
tag += 2;
tmp = ReasonCode;
memcpy(tag, (u8 *)&tmp, 2);
tag += 2;
RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA|RTLLIB_DL_BA, skb->data, skb->len);
if (net_ratelimit())
RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA, "<=====%s()\n",
__func__);
return skb;
}
// Classify the to-be send data packet
// Need to acquire the sent queue index.
int
rtllib_classify(struct sk_buff *skb, u8 bIsAmsdu)
{
struct ethhdr *eth;
struct iphdr *ip;
eth = (struct ethhdr *)skb->data;
if (eth->h_proto != htons(ETH_P_IP))
return 0;
#ifdef ENABLE_AMSDU
if(bIsAmsdu)
ip = (struct iphdr*)(skb->data + sizeof(struct ether_header) + AMSDU_SUBHEADER_LEN + SNAP_SIZE + sizeof(u16));
else
ip = (struct iphdr*)(skb->data + sizeof(struct ether_header));
#else
RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA, skb->data, skb->len);
#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22))
ip = ip_hdr(skb);
#else
ip = (struct iphdr*)(skb->data + sizeof(struct ether_header));
#endif
#endif
switch (ip->tos & 0xfc) {
case 0x20:
return 2;
case 0x40:
return 1;
case 0x60:
return 3;
case 0x80:
return 4;
case 0xa0:
return 5;
case 0xc0:
return 6;
case 0xe0:
return 7;
default:
return 0;
}
}
static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
struct ba_record *pBA,
u16 StatusCode, u8 type)
{
struct sk_buff *skb = NULL;
struct rtllib_hdr_3addr *BAReq = NULL;
u8 *tag = NULL;
u16 tmp = 0;
u16 len = ieee->tx_headroom + 9;
RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA, "========>%s(), frame(%d)"
" sentd to: %pM, ieee->dev:%p\n", __func__,
type, Dst, ieee->dev);
if (pBA == NULL) {
RTLLIB_DEBUG(RTLLIB_DL_ERR, "pBA is NULL\n");
return NULL;
}
skb = dev_alloc_skb(len + sizeof(struct rtllib_hdr_3addr));
if (skb == NULL) {
RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't alloc skb for ADDBA_REQ\n");
return NULL;
}
memset(skb->data, 0, sizeof(struct rtllib_hdr_3addr));
skb_reserve(skb, ieee->tx_headroom);
BAReq = (struct rtllib_hdr_3addr *)skb_put(skb,
sizeof(struct rtllib_hdr_3addr));
memcpy(BAReq->addr1, Dst, ETH_ALEN);
memcpy(BAReq->addr2, ieee->dev->dev_addr, ETH_ALEN);
memcpy(BAReq->addr3, ieee->current_network.bssid, ETH_ALEN);
BAReq->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
tag = (u8 *)skb_put(skb, 9);
*tag ++= ACT_CAT_BA;
*tag ++= type;
*tag ++= pBA->DialogToken;
if (ACT_ADDBARSP == type) {
RT_TRACE(COMP_DBG, "====>to send ADDBARSP\n");
tmp = StatusCode;
memcpy(tag, (u8 *)&tmp, 2);
tag += 2;
}
tmp = pBA->BaParamSet.shortData;
memcpy(tag, (u8 *)&tmp, 2);
tag += 2;
tmp = pBA->BaTimeoutValue;
memcpy(tag, (u8 *)&tmp, 2);
tag += 2;
if (ACT_ADDBAREQ == type) {
memcpy(tag, (u8 *)&(pBA->BaStartSeqCtrl), 2);
tag += 2;
}
RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA|RTLLIB_DL_BA, skb->data, skb->len);
return skb;
}
int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
{
struct rtllib_hdr_3addr *req = NULL;
u16 rc = 0;
u8 *dst = NULL, *pDialogToken = NULL, *tag = NULL;
struct ba_record *pBA = NULL;
union ba_param_set *pBaParamSet = NULL;
u16 *pBaTimeoutVal = NULL;
union sequence_control *pBaStartSeqCtrl = NULL;
struct rx_ts_record *pTS = NULL;
if (skb->len < sizeof(struct rtllib_hdr_3addr) + 9) {
RTLLIB_DEBUG(RTLLIB_DL_ERR, " Invalid skb len in BAREQ(%d / "
"%d)\n", (int)skb->len,
(int)(sizeof(struct rtllib_hdr_3addr) + 9));
return -1;
}
RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA|RTLLIB_DL_BA, skb->data, skb->len);
req = (struct rtllib_hdr_3addr *) skb->data;
tag = (u8 *)req;
dst = (u8 *)(&req->addr2[0]);
tag += sizeof(struct rtllib_hdr_3addr);
pDialogToken = tag + 2;
pBaParamSet = (union ba_param_set *)(tag + 3);
pBaTimeoutVal = (u16 *)(tag + 5);
pBaStartSeqCtrl = (union sequence_control *)(req + 7);
RT_TRACE(COMP_DBG, "====>rx ADDBAREQ from : %pM\n", dst);
if (ieee->current_network.qos_data.active == 0 ||
(ieee->pHTInfo->bCurrentHTSupport == false) ||
(ieee->pHTInfo->IOTAction & HT_IOT_ACT_REJECT_ADDBA_REQ)) {
rc = ADDBA_STATUS_REFUSED;
RTLLIB_DEBUG(RTLLIB_DL_ERR, "Failed to reply on ADDBA_REQ as "
"some capability is not ready(%d, %d)\n",
ieee->current_network.qos_data.active,
ieee->pHTInfo->bCurrentHTSupport);
goto OnADDBAReq_Fail;
}
if (!GetTs(ieee, (struct ts_common_info **)(&pTS), dst,
(u8)(pBaParamSet->field.TID), RX_DIR, true)) {
rc = ADDBA_STATUS_REFUSED;
RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't get TS in %s()\n", __func__);
goto OnADDBAReq_Fail;
}
pBA = &pTS->RxAdmittedBARecord;
if (pBaParamSet->field.BAPolicy == BA_POLICY_DELAYED) {
rc = ADDBA_STATUS_INVALID_PARAM;
RTLLIB_DEBUG(RTLLIB_DL_ERR, "BA Policy is not correct in "
"%s()\n", __func__);
goto OnADDBAReq_Fail;
}
rtllib_FlushRxTsPendingPkts(ieee, pTS);
DeActivateBAEntry(ieee, pBA);
pBA->DialogToken = *pDialogToken;
pBA->BaParamSet = *pBaParamSet;
pBA->BaTimeoutValue = *pBaTimeoutVal;
pBA->BaStartSeqCtrl = *pBaStartSeqCtrl;
if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev) ||
(ieee->pHTInfo->IOTAction & HT_IOT_ACT_ALLOW_PEER_AGG_ONE_PKT))
pBA->BaParamSet.field.BufferSize = 1;
else
pBA->BaParamSet.field.BufferSize = 32;
ActivateBAEntry(ieee, pBA, 0);
rtllib_send_ADDBARsp(ieee, dst, pBA, ADDBA_STATUS_SUCCESS);
return 0;
OnADDBAReq_Fail:
{
struct ba_record BA;
BA.BaParamSet = *pBaParamSet;
BA.BaTimeoutValue = *pBaTimeoutVal;
BA.DialogToken = *pDialogToken;
BA.BaParamSet.field.BAPolicy = BA_POLICY_IMMEDIATE;
rtllib_send_ADDBARsp(ieee, dst, &BA, rc);
return 0;
}
}
