📄 zdhci.c
字号:
#ifndef _ZDHCI_C_
#define _ZDHCI_C_
#include "zd80211.h"
#include "zdhci.h"
#include "zdequates.h"
#include "zd1205.h"
#include "zddebug.h"
#define MAX_CHANNEL_ALLOW 13
BOOLEAN zd_PseudoIbssConnect(void);
static U8 zd_Snap_Header[6] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
static U8 zd_SnapBridgeTunnel[6] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8};
static U8 zd_Snap_Apple_Type[] = {0xAA,0xAA,0x03,0x08,0x00,0x07,0x80,0x9b};
static U8 zd_Snap_Apple_AARP[] = {0xAA,0xAA,0x03,0x00,0x00,0x00,0x80,0xf3};
extern struct net_device *g_dev;
zd_80211Obj_t *pdot11Obj = 0;
extern const U16 dot11A_Channel[];
extern const U16 dot11A_Channel_Amount;
extern void ChangeMacMode(u8 MAC_Mode,u8 Channel);
extern U32 mTimeBeforeAdhocRoaming;
U16 CurrScanCH = 1;
BOOLEAN zd_SendPkt(U8 *pEthHdr, U8 *pBody, U32 bodyLen, void *buf, U8 bEapol, void *pHash)
{
struct zd1205_private *macp = (struct zd1205_private *)g_dev->priv;
Signal_t *signal;
#if ZDCONF_LP_SUPPORT == 1
U8 EthHdr[14];
#endif
FrmDesc_t *pfrmDesc;
Frame_t *frame;
U8 vapId = 0;
//FPRINT("zd_SendPkt");
if (mPsStaCnt){
if (zd_CheckTotalQueCnt() > TXQ_THRESHOLD){
//FPRINT("Drop Tx packet");
return FALSE;
}
}
signal = allocSignal();
if (!signal){
FPRINT("zd_SendPkt out of signal");
FPRINT_V("freeSignalCount", freeSignalCount);
return FALSE;
}
#if ZDCONF_LP_SUPPORT == 1
memcpy(signal->EthHdr,pEthHdr,14);
#endif
pfrmDesc = allocFdesc();
if (!pfrmDesc){
freeSignal(signal);
FPRINT("zd_SendPkt out of description");
FPRINT_V("freeFdescCount", freeFdescCount);
return FALSE;
}
frame = pfrmDesc->mpdu;
/* FrameControl(2) Duration/ID(2) A1(6) A2(6) A3(6) Seq(2) A4/LLCHdr(6) LLCHdr(6) */
if (mBssType == AP_BSS){
memcpy((char *)&(frame->header[4]), (char *)&(pEthHdr[0]), 6); /* Set DA to A1 */
memcpy((char *)&(frame->header[16]), (char *)&(pEthHdr[6]), 6); /* Set SA to A3 */
frame->header[1] = FROM_DS_BIT;
}
else if (mBssType == INFRASTRUCTURE_BSS){
memcpy((char *)&(frame->header[4]), (char *)&mBssId, 6); /* Set BSSID to A1 */
memcpy((char *)&(frame->header[16]), (char *)&(pEthHdr[0]), 6); /* Set DA to A3 */
frame->header[1] = TO_DS_BIT;
if (mPwrState && macp->bAssoc){
frame->header[1] |= PW_SAVE_BIT;
}
else
frame->header[1] &= ~PW_SAVE_BIT;
}
else if ((mBssType == INDEPENDENT_BSS) || (mBssType == PSEUDO_IBSS)){
memcpy((char *)&(frame->header[4]), (char *)&(pEthHdr[0]), 6); /* Set DA to A1 */
memcpy((char *)&(frame->header[16]), (char *)&mBssId, 6); /* Set Bssid to A3 */
frame->header[1] = 0;
}
frame->bodyLen = bodyLen;
frame->body = pBody;
signal->buf = buf;
signal->vapId = vapId;
pfrmDesc->ConfigSet &= ~INTRA_BSS_SET;
frame->HdrLen = MAC_HDR_LNG;
frame->header[0] = ST_DATA;
setAddr2(frame, &dot11MacAddress);
if (bEapol){
pfrmDesc->ConfigSet |= EAPOL_FRAME_SET;
}
else
pfrmDesc->ConfigSet &= ~EAPOL_FRAME_SET;
signal->bDataFrm = 1;
//pfrmDesc->bDataFrm = 1;
pfrmDesc->pHash = (Hash_t *)pHash;
if (pHash == NULL && !(pEthHdr[0]&BIT_0) )
printk(KERN_ERR "===== ==== ===pHash is NULL in zd_SendPkt\n");
mkFragment(signal, pfrmDesc, pEthHdr); //10 us
#if 0
//force free for debug only
zd1205_dump_data("header", (u8 *)&frame->header[0], frame->HdrLen);
zd1205_dump_data("body", (u8 *)frame->body, frame->bodyLen);
freeSignal(signal);
freeFdesc(pfrmDesc);
return TRUE;
#endif
#if 1
if (SendPkt(signal, pfrmDesc, TRUE)) //4727
return FALSE;
else
return TRUE;
#else
SendPkt(signal, pfrmDesc, TRUE);
return TRUE;
#endif
}
#define LP_FORWARD 0
#define BC_FORWARD 1
#define BSS_FORWARD 2
void zd_WirelessForward(U8 *pHdr, U8 *pBody, U32 len, void *buf, U8 mode, void *pHash, U8 *pEthHdr)
{
Signal_t *signal;
FrmDesc_t *pfrmDesc;
Frame_t *frame;
U8 vapId = 0;
//FPRINT("zd_WirelessForward");
if (mPsStaCnt){
if (zd_CheckTotalQueCnt() > TXQ_THRESHOLD){
//FPRINT("Drop Intra-BSS packet");
pdot11Obj->ReleaseBuffer(buf);
return;
}
}
signal = allocSignal();
if (!signal){
FPRINT("zd_WirelessForward out of signal");
FPRINT_V("freeSignalCount", freeSignalCount);
pdot11Obj->ReleaseBuffer(buf);
return;
}
pfrmDesc = allocFdesc();
if (!pfrmDesc){
freeSignal(signal);
FPRINT("zd_WirelessForward out of description");
FPRINT_V("freeFdescCount", freeFdescCount);
pdot11Obj->ReleaseBuffer(buf);
return;
}
frame = pfrmDesc->mpdu;
/* FrameControl(2) Duration/ID(2) A1(6) A2(6) A3(6) Seq(2) A4/LLCHdr(6) LLCHdr(6) */
memcpy((char *)&(frame->header[4]), (char *)&(pHdr[16]), 6); /* Set DA to A1 */
memcpy((char *)&(frame->header[16]), (char *)&(pHdr[10]), 6); /* Set SA to A3 */
#if ZDCONF_LP_SUPPORT == 1
memcpy(signal->EthHdr, (char *)&(frame->header[4]),6);
memcpy(signal->EthHdr+ETH_ALEN, (char *)&(frame->header[16]),6);
#endif
frame->bodyLen = len;
frame->body = pBody;
signal->buf = buf;
signal->vapId = vapId;
if (mode == LP_FORWARD){
memcpy((char *)&(frame->header[4]), (char *)&(pHdr[10]), 6); /* Set DA to A1 */
memcpy((char *)&(frame->header[16]), (char *)&dot11MacAddress, 6); /* Set SA to A3 */
frame->body[6] = 0x38;
frame->body[7] = 0x39;
}
pfrmDesc->ConfigSet |= INTRA_BSS_SET;
pfrmDesc->ConfigSet &= ~EAPOL_FRAME_SET;
signal->bDataFrm = 1;
//pfrmDesc->bDataFrm = 1;
frame->HdrLen = MAC_HDR_LNG;
frame->header[0] = ST_DATA;
frame->header[1] = FROM_DS_BIT;
setAddr2(frame, &dot11MacAddress);
pfrmDesc->pHash = (Hash_t *)pHash;
mkFragment(signal, pfrmDesc, pEthHdr);
SendPkt(signal, pfrmDesc, FALSE);
return;
}
void zd_SendDeauthFrame(U8 *sta, U8 ReasonCode)
{
Signal_t *signal;
printk(KERN_ERR "SendDeauthFrame with ReasonCode=%u\n",ReasonCode);
if ((mBssType == INDEPENDENT_BSS) || (mBssType == PSEUDO_IBSS))
return;
if ((signal = allocSignal()) == NULL)
return;
signal->id = SIG_DEAUTH_REQ;
signal->block = BLOCK_AUTH_REQ;
signal->vapId = 0;
memcpy(&signal->frmInfo.Sta, sta, 6);
signal->frmInfo.rCode = ReasonCode;
sigEnque(pMgtQ, (signal));
return;
}
void zd_SendClass2ErrorFrame(MacAddr_t *sta, U8 vapId)
{
Signal_t *signal;
//FPRINT("zd_sendClass2ErrorFrame");
if ((mBssType == INDEPENDENT_BSS) || (mBssType == PSEUDO_IBSS))
return;
if ((signal = allocSignal()) == NULL)
return;
signal->id = SIG_DEAUTH_REQ;
signal->block = BLOCK_AUTH_REQ;
signal->vapId = vapId;
memcpy(&signal->frmInfo.Sta, sta, 6);
signal->frmInfo.rCode = RC_CLASS2_ERROR;
sigEnque(pMgtQ, (signal));
return;
}
void zd_SendClass3ErrorFrame(MacAddr_t *sta, U8 vapId)
{
Signal_t *signal;
//FPRINT("zd_SendClass3ErrorFrame");
if ((mBssType == INDEPENDENT_BSS) || (mBssType == PSEUDO_IBSS))
return;
if ((signal = allocSignal()) == NULL)
return;
signal->id = SIG_DIASSOC_REQ;
signal->block = BLOCK_ASOC;
signal->vapId = vapId;
memcpy(&signal->frmInfo.Sta, sta, 6);
signal->frmInfo.rCode = RC_CLASS3_ERROR;
sigEnque(pMgtQ, (signal));
return;
}
#define MIC_HEADER_LEN 16
BOOLEAN zd_CheckMic(U8 *pHdr, U8 *pBody, U32 bodyLen, Hash_t *pHash, U8 *pEthHdr)
{
MICvar *pRxMicKey;
U8 PkInstalled = 0;
U8 *pByte;
U8 CalMic[8];
int i = 0;
U8 *pIV = pHdr + 24;
/* Always return TRUE, 4D06 */
//return TRUE;
//if (!pHash)
// return FALSE;
if (pIV[3] & EIV_BIT)
{
if (pHdr[4] & 1) // Use group key
{
if (mGkInstalled)
pRxMicKey = &mBcMicKey;
else
return FALSE;
}
else // Use Unicast key
{
if (!pHash)
return FALSE;
if ((PkInstalled=pHash->pkInstalled))
pRxMicKey = &pHash->RxMicKey;
else
return FALSE;
}
//zd1205_dump_data("IV = ", pIV, 8);
//zd1205_dump_data("MIC K0= ", (U8 *)&pRxMicKey->K0, 4);
//zd1205_dump_data("MIC K1= ", (U8 *)&pRxMicKey->K1, 4);
//pRxMicKey = &pHash->RxMicKey;
//PkInstalled = pHash->pkInstalled;
if (1){
U32 Len = bodyLen - MIC_LNG;
#if 0
void *reg = pdot11Obj->reg;
U32 BolckLen = 0;
U32 tmpValue = 0;
U32 BlockNum = 2;
U32 MicLow, MicHigh;
U32 MicStatus = 0;
U32 HwMicStatus = 0;
register int j = 0;
U32 RxMicWrBackAddr = (U32)pEthHdr + MIC_HEADER_LEN;
U32 HwMicHighPhys = RxMicWrBackAddr + 4;
U32 HwMicStatusPhys = HwMicHighPhys + 4;
//FPRINT("************* RX MIC ****************");
//reser HW MIC status
memset(RxMicWrBackAddr, 0x11, 12);
//wait last MIC finish, then start this one
MicStatus = pdot11Obj->GetReg(reg, ZD_MIC_STATUS);
while (MicStatus & MIC_BUSY){
pdot11Obj->DelayUs(1);
MicStatus = pdot11Obj->GetReg(reg, ZD_MIC_STATUS);
j++;
if (j>1000){
bMicFinish = FALSE;
//FPRINT("Rx MIC can't start !!!");
//FPRINT_V("MicStatus", MicStatus);
zdCnt.RxMicNoStart++;
break;
}
}
//set mic key
pdot11Obj->SetReg(reg, ZD_MIC_KEY_LOW, pRxMicKey->K0);
pdot11Obj->SetReg(reg, ZD_MIC_KEY_HIGH, pRxMicKey->K1);
//set 802.3 header
pdot11Obj->SetReg(reg, ZD_MIC_START_ADDR0, (U32)pEthHdr);
pdot11Obj->SetReg(reg, ZD_MIC_BLOCK0_LEN, MIC_HEADER_LEN);
//set mac body
pdot11Obj->SetReg(reg, ZD_MIC_START_ADDR1, (U32)pBody);
pdot11Obj->SetReg(reg, ZD_MIC_BLOCK1_LEN, Len);
//set write back address
pdot11Obj->SetReg(reg, ZD_MIC_WRITE_BACK_ADDRS, (U32)RxMicWrBackAddr);
BolckLen = MIC_HEADER_LEN + Len;
tmpValue = (BlockNum | (BolckLen << 16));
pdot11Obj->SetReg(reg, ZD_MIC_TOTAL_BLOCK_NUM, tmpValue);
// busy waiting MIC finish
j= 0;
HwMicStatus = zd_le32_to_cpu(*(U32 *)(HwMicStatusPhys));
while (HwMicStatus != HW_MIC_FINISH){
pdot11Obj->DelayUs(1);
HwMicStatus = zd_le32_to_cpu(*(U32 *)(HwMicStatusPhys));
j++;
if (j>1000){
bMicFinish = FALSE;
//FPRINT("Rx MIC not finish !!!");
//FPRINT_V("HwMicStatus", HwMicStatus);
zdCnt.RxMicNoFinish++;
break;
}
}
MicLow = zd_le32_to_cpu (*(U32 *)RxMicWrBackAddr);
MicHigh = zd_le32_to_cpu (*(U32 *)(HwMicHighPhys));
pByte = pBody + Len; //point to MIC start
CalMic[0] = (U8) MicLow;
CalMic[1] = (U8) (MicLow >> 8);
CalMic[2] = (U8) (MicLow >> 16);
CalMic[3] = (U8) (MicLow >> 24);
CalMic[4] = (U8) MicHigh;
CalMic[5] = (U8) (MicHigh >> 8);
CalMic[6] = (U8) (MicHigh >> 16);
CalMic[7] = (U8) (MicHigh >> 24);
#else
//Software MIC Calculation, HW MIC failed
MICclear(pRxMicKey);
//pByte = pEthHdr;
if (mBssType == INFRASTRUCTURE_BSS ||
mBssType == INDEPENDENT_BSS)
pByte = &pHdr[4]; // DA = Addr1
else //if (mBssType == AP_BSS)
pByte = &pHdr[16]; //DA = Addr3
for (i=0; i<6; i++){
MICappendByte(*pByte++, pRxMicKey);
}
if (mBssType == AP_BSS ||
mBssType == INDEPENDENT_BSS)
pByte = &pHdr[10]; // SA=Addr2
else // if (mBssType == INFRASTRUCTURE_BSS)
pByte = &pHdr[16]; // SA=Addr3
for (i=0; i<6; i++){
MICappendByte(*pByte++, pRxMicKey);
}
MICappendByte(0,pRxMicKey);//priority
MICappendByte(0,pRxMicKey);//3 zeros
MICappendByte(0,pRxMicKey);
MICappendByte(0,pRxMicKey);
pByte = pBody;
for (i=0; i<Len; i++){
MICappendByte(*pByte++, pRxMicKey);
}
MICgetMIC(CalMic, pRxMicKey); // Append MIC (8 byte)
#endif
//FPRINT_V("Calcu HW MIC", RxCompLogBuf[RxComplogPktCnt][10]-RxCompLogBuf[RxComplogPktCnt][9]);
// now pBye point to MIC area
if (pdot11Obj->MIC_CNT && memcmp(CalMic, pByte, MIC_LNG) != 0){
zd1205_dump_data("pHdr = ", pHdr, 32);
//FPRINT_V("Body Addr", (U32)pBody);
zd1205_dump_data("pBody = ", pBody, bodyLen+16);
zd1205_dump_data("CalMic = ", CalMic, 8);
zd1205_dump_data("ReceMic = ", pByte, 8);
printk(KERN_ERR "SW MIC Check fail\n");
hostap_michael_mic_failure((struct zd1205_private *)g_dev->priv, (struct hostap_ieee80211_hdr *)pHdr, pIV[3] & KEYID_MASK);
//pdot11Obj->MicFailure(&pEthHdr[6]);
return FALSE;
}
else{
//FPRINT("***** MIC success *****");
//printk(KERN_ERR "SW MIC check OK\n");
return TRUE;
}
}
}
return FALSE;
}
void zd_ReceivePkt(U8 *pHdr, U32 hdrLen, U8 *pBody, U32 bodyLen, void *buf, U8 *pEthHdr, rxInfo_t *pRxInfo, const BOOLEAN LP)
{
U8 *x;
Signal_t *pRxSignal;
FrmDesc_t *pRxFdesc;
Frame_t *pRxFrame;
MacAddr_t *pDa, *pSa;
StationState sas;
PsMode psm = PSMODE_STA_ACTIVE;
U32 dataLen = 0xFFFFFFFF;
U8 *pData = NULL;
U8 mode;
void *bcBuf;
U8 *pBcData;
Hash_t *pHash;
U8 vapId = 0;
U8 rate = pRxInfo->rate;
U8 bDataFrm = pRxInfo->bDataFrm;
U8 SaIndex = pRxInfo->SaIndex;
U8 signalStrength = pRxInfo->signalStrength;
U8 signalQuality = pRxInfo->signalQuality;
U8 bSwCheckMIC = pRxInfo->bSwCheckMIC;
U32 LP_MAP = 0;
#if ZDCONF_LP_SUPPORT == 1
// ####### FOR A-MSDU ########
U16 wCnt;
struct sk_buff *skb = NULL;
Signal_t *signal;
FrmDesc_t *pfrmDesc;
Frame_t *frame;
U16 dataOff = 0;
U16 idx, pktLen,idx2;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -