int read_reg(RTMP_ADAPTER *ad, UINT32 base, UINT16 offset, UINT32 *value)
{
NTSTATUS ret;
UINT8 req = 0;
UINT32 io_value;
if (base == 0x40)
req = 0x47;
else if (base == 0x41)
req = 0x7;
else
DBGPRINT(RT_DEBUG_ERROR, ("[%s] Unknown base %x\n", __FUNCTION__, base));
ret = RTUSB_VendorRequest(ad,
(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
DEVICE_VENDOR_REQUEST_IN,
req,
0,
offset,
&io_value,
4);
*value = le2cpu32(io_value);
if (ret)
*value = 0xffffffff;
return ret;
}
/*
========================================================================
Routine Description: Read 32-bit MAC register
Arguments:
Return Value:
IRQL =
Note:
========================================================================
*/
NTSTATUS RTUSBReadMACRegister(
IN PRTMP_ADAPTER pAd,
IN USHORT Offset,
OUT PUINT32 pValue)
{
NTSTATUS Status;
UINT32 localVal;
Status = RTUSB_VendorRequest(
pAd,
(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
DEVICE_VENDOR_REQUEST_IN,
0x7,
0,
Offset,
&localVal,
4);
*pValue = le2cpu32(localVal);
if (Status < 0)
*pValue = 0xffffffff;
return Status;
}
/*
==========================================================================
Description:
IRQL = PASSIVE_LEVEL
==========================================================================
*/
USHORT TDLS_TPKMsg1Process(
IN PRTMP_ADAPTER pAd,
IN PRT_802_11_TDLS pTDLS,
IN PUCHAR pRsnIe,
IN UCHAR RsnLen,
IN PUCHAR pFTIe,
IN UCHAR FTLen,
IN PUCHAR pTIIe,
IN UCHAR TILen)
{
USHORT StatusCode = MLME_SUCCESS;
UCHAR CipherTmp[64] = {0};
UCHAR CipherTmpLen = 0;
FT_FTIE *ft = NULL;
// Validate RsnIE
//
if (RsnLen == 0) // RSN not exist
return MLME_INVALID_INFORMATION_ELEMENT;
if (pRsnIe[2] < 1) // Smaller version
return MLME_NOT_SUPPORT_RSN_VERSION;
CipherTmpLen = CipherSuiteTDLSLen;
if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
NdisMoveMemory(CipherTmp, CipherSuiteTDLSWpa2PskTkip, CipherTmpLen);
else
NdisMoveMemory(CipherTmp, CipherSuiteTDLSWpa2PskAes, CipherTmpLen);
if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
CipherTmp[19] = TDLS_AKM_SUITE_1X;
if ( RTMPEqualMemory(&pRsnIe[16], &CipherTmp[16], 4) == 0) // Invalid TDLS AKM
return MLME_INVALID_AKMP;
if ( RTMPEqualMemory(&pRsnIe[20], &CipherTmp[20], 2) == 0) // Invalid RSN capability
return MLME_INVALID_RSN_CAPABILITIES;
if ((RsnLen != 22) || (RTMPEqualMemory(pRsnIe, CipherTmp, RsnLen) == 0)) // Invalid Pairwise Cipher
return REASON_UCIPHER_NOT_VALID;
// Validate FTIE
//
ft = (PFT_FTIE)(pFTIe + 2); // point to the element of IE
if ((FTLen != (sizeof(FT_FTIE) + 2)) || RTMPEqualMemory(&ft->MICCtr, TdlsZeroSsid, 2) == 0 ||
(RTMPEqualMemory(ft->MIC, TdlsZeroSsid, 16) == 0) || (RTMPEqualMemory(ft->ANonce, TdlsZeroSsid, 32) == 0))
return REASON_FT_INVALID_FTIE;
// Validate TIIE
//
if ((TILen != 7) || (pTIIe[2] != 2) || ( le2cpu32(*((PULONG) (pTIIe + 3))) < TDLS_KEY_TIMEOUT))
return TDLS_STATUS_CODE_UNACCEPTABLE_LIFETIME;
return StatusCode;
}
/*
========================================================================
Routine Description: Read 32-bit MAC register
Arguments:
Return Value:
IRQL =
Note:
========================================================================
*/
u32 mt76u_reg_read(struct rtmp_adapter *pAd, USHORT Offset)
{
int status;
u32 val;
status = mt76u_vendor_request(pAd,
DEVICE_VENDOR_REQUEST_IN,
MT_VEND_MULTI_READ,
0, Offset,
&val, 4);
if (status != 0)
val = 0xffffffff;
return le2cpu32(val);
}
// For MT7662 and newer
void usb_cfg_read_v3(RTMP_ADAPTER *ad, u32 *value)
{
int ret;
u32 io_value;
ret = RTUSB_VendorRequest(ad,
(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
DEVICE_VENDOR_REQUEST_IN,
0x47,
0,
U3DMA_WLCFG,
&io_value,
4);
*value = le2cpu32(io_value);
if (ret)
*value = 0xffffffff;
}
/*
==========================================================================
Description:
IRQL = PASSIVE_LEVEL
==========================================================================
*/
USHORT TDLS_TPKMsg3Process(
IN PRTMP_ADAPTER pAd,
IN PRT_802_11_TDLS pTDLS,
IN PUCHAR pRsnIe,
IN UCHAR RsnLen,
IN PUCHAR pFTIe,
IN UCHAR FTLen,
IN PUCHAR pTIIe,
IN UCHAR TILen)
{
USHORT StatusCode = MLME_SUCCESS;
UCHAR CipherTmp[64] = {0};
UCHAR CipherTmpLen = 0;
FT_FTIE *ft = NULL;
UCHAR oldMic[16];
UCHAR LinkIdentifier[20];
// Validate RsnIE
//
if (RsnLen == 0) // RSN not exist
return MLME_INVALID_INFORMATION_ELEMENT;
if (pRsnIe[2] < 1) // Smaller version
return MLME_NOT_SUPPORT_RSN_VERSION;
CipherTmpLen = CipherSuiteTDLSLen;
if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
NdisMoveMemory(CipherTmp, CipherSuiteTDLSWpa2PskTkip, CipherTmpLen);
else
NdisMoveMemory(CipherTmp, CipherSuiteTDLSWpa2PskAes, CipherTmpLen);
if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
CipherTmp[19] = TDLS_AKM_SUITE_1X;
if ( RTMPEqualMemory(&pRsnIe[16], &CipherTmp[16], 4) == 0) // Invalid TDLS AKM
return MLME_INVALID_AKMP;
if ( RTMPEqualMemory(&pRsnIe[20], &CipherTmp[20], 2) == 0) // Invalid RSN capability
return MLME_INVALID_RSN_CAPABILITIES;
if ((RsnLen != 22) || (RTMPEqualMemory(pRsnIe, CipherTmp, RsnLen) == 0)) // Invalid Pairwise Cipher
return REASON_UCIPHER_NOT_VALID;
// Validate FTIE
//
ft = (PFT_FTIE)(pFTIe + 2); // point to the element of IE
if ((FTLen != (sizeof(FT_FTIE) + 2)) || RTMPEqualMemory(&ft->MICCtr, TdlsZeroSsid, 2) == 0 ||
(RTMPEqualMemory(ft->SNonce, pTDLS->SNonce, 32) == 0) || (RTMPEqualMemory(ft->ANonce, pTDLS->ANonce, 32) == 0))
return REASON_FT_INVALID_FTIE;
// Validate TIIE
//
if ((TILen != 7) || (pTIIe[2] != 2) || ( le2cpu32(*((PULONG) (pTIIe + 3))) < pTDLS->KeyLifetime))
return TDLS_STATUS_CODE_UNACCEPTABLE_LIFETIME;
// Validate the MIC field of FTIE
//
// point to the element of IE
ft = (PFT_FTIE)(pFTIe + 2);
// backup MIC fromm the peer TDLS
NdisMoveMemory(oldMic, ft->MIC, 16);
// set MIC field to zero before MIC calculation
NdisZeroMemory(ft->MIC, 16);
// Construct LinkIdentifier (IE + Length + BSSID + Initiator MAC + Responder MAC)
NdisZeroMemory(LinkIdentifier, 20);
LinkIdentifier[0] = IE_TDLS_LINK_IDENTIFIER;
LinkIdentifier[1] = 18;
NdisMoveMemory(&LinkIdentifier[2], pAd->CommonCfg.Bssid, 6);
NdisMoveMemory(&LinkIdentifier[8], pTDLS->MacAddr, 6);
NdisMoveMemory(&LinkIdentifier[14], pAd->CurrentAddress, 6);
////////////////////////////////////////////////////////////////////////
// The MIC field of FTIE shall be calculated on the concatenation, in the following order, of
// 1. MAC_I (6 bytes)
// 2. MAC_R (6 bytes)
// 3. Transaction Sequence = 3 (1 byte)
// 4. Link Identifier (20 bytes)
// 5. RSN IE without the IE header (20 bytes)
// 6. Timeout Interval IE (7 bytes)
// 7. FTIE without the IE header, with the MIC field of FTIE set to zero (82 bytes)
{
UCHAR content[512];
ULONG c_len = 0;
ULONG tmp_len = 0;
UCHAR Seq = 3;
UCHAR mic[16];
UINT tmp_aes_len = 0;
NdisZeroMemory(mic, sizeof(mic));
/* make a header frame for calculating MIC. */
MakeOutgoingFrame(content, &tmp_len,
MAC_ADDR_LEN, pTDLS->MacAddr,
MAC_ADDR_LEN, pAd->CurrentAddress,
1, &Seq,
END_OF_ARGS);
c_len += tmp_len;
/* concatenate Link Identifier */
MakeOutgoingFrame(content + c_len, &tmp_len,
20, LinkIdentifier,
END_OF_ARGS);
c_len += tmp_len;
/* concatenate RSNIE */
MakeOutgoingFrame(content + c_len, &tmp_len,
20, pRsnIe + 2,
END_OF_ARGS);
c_len += tmp_len;
/* concatenate Timeout Interval IE */
MakeOutgoingFrame(content + c_len, &tmp_len,
7, pTIIe,
END_OF_ARGS);
c_len += tmp_len;
/* concatenate FTIE */
MakeOutgoingFrame(content + c_len, &tmp_len,
sizeof(FT_FTIE), (PUCHAR)ft,
END_OF_ARGS);
c_len += tmp_len;
/* Calculate MIC */
//AES_128_CMAC(pTDLS->TPK, content, c_len, mic);
/* Compute AES-128-CMAC over the concatenation */
tmp_aes_len = AES_KEY128_LENGTH;
AES_CMAC(content, c_len, pTDLS->TPK, 16, mic, &tmp_aes_len);
NdisMoveMemory(ft->MIC, mic, 16);
}
////////////////////////////////////////////////////////////////////////
if (RTMPEqualMemory(oldMic, ft->MIC, 16) == 0)
{
DBGPRINT(RT_DEBUG_ERROR,("TDLS_TPKMsg3Process() MIC Error!!! \n"));
return MLME_REQUEST_DECLINED;
}
return StatusCode;
}
VOID RRM_BeaconReportHandler(
IN PRTMP_ADAPTER pAd,
IN PRRM_BEACON_REP_INFO pBcnRepInfo,
IN LONG Length)
{
CHAR Rssi;
USHORT LenVIE = 0;
NDIS_802_11_VARIABLE_IEs *pVIE = NULL;
UCHAR VarIE[MAX_VIE_LEN];
ULONG Idx = BSS_NOT_FOUND;
LONG RemainLen = Length;
PRRM_BEACON_REP_INFO pBcnRep;
PUINT8 ptr;
RRM_BEACON_REP_INFO_FIELD BcnReqInfoField;
UINT32 Ptsf;
BCN_IE_LIST *ie_list = NULL;
os_alloc_mem(NULL, (UCHAR **)&ie_list, sizeof(BCN_IE_LIST));
if (ie_list == NULL) {
DBGPRINT(RT_DEBUG_ERROR, ("%s(): Alloc ie_list failed!\n", __FUNCTION__));
return;
}
NdisZeroMemory(ie_list, sizeof(BCN_IE_LIST));
ptr = (PUINT8)pBcnRepInfo;
pBcnRep = (PRRM_BEACON_REP_INFO)ptr;
Ptsf = le2cpu32(pBcnRep->ParentTSF);
BcnReqInfoField.word = pBcnRep->RepFrameInfo;
DBGPRINT(RT_DEBUG_TRACE, ("%s:: ReqClass=%d, Channel=%d\n",
__FUNCTION__, pBcnRep->RegulatoryClass, pBcnRep->ChNumber));
DBGPRINT(RT_DEBUG_TRACE, ("Bssid=%02x:%02x:%02x:%02x:%02x:%02x\n",
PRINT_MAC(pBcnRep->Bssid)));
Rssi = pBcnRep->RCPI + pAd->BbpRssiToDbmDelta;
RemainLen -= sizeof(RRM_BEACON_REP_INFO);
ptr += sizeof(RRM_BEACON_REP_INFO);
/* check option sub element IE. */
while (RemainLen > 0)
{
PRRM_SUBFRAME_INFO pRrmSubFrame;
pRrmSubFrame = (PRRM_SUBFRAME_INFO)ptr;
switch(pRrmSubFrame->SubId)
{
case 1:
if (BcnReqInfoField.field.ReportFrameType == 0)
{
/* Init Variable IE structure */
pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
pVIE->Length = 0;
PeerBeaconAndProbeRspSanity(pAd,
pRrmSubFrame->Oct,
pRrmSubFrame->Length,
pBcnRep->ChNumber,
ie_list,
&LenVIE,
pVIE);
}
break;
case 221:
DBGPRINT(RT_DEBUG_TRACE, ("%s:: SubIe: ID=%x, Len=%d\n",
__FUNCTION__, pRrmSubFrame->SubId, pRrmSubFrame->Length));
break;
}
RemainLen -= (pRrmSubFrame->Length + 2);
ptr += (pRrmSubFrame->Length + 2);
/* avoid infinite loop. */
if (pRrmSubFrame->Length == 0)
break;
}
if (NdisEqualMemory(pBcnRep->Bssid, ie_list->Bssid, MAC_ADDR_LEN) == FALSE)
{
DBGPRINT(RT_DEBUG_WARN, ("%s():BcnReq->BSSID not equal ie_list->Bssid!\n", __FUNCTION__));
}
#ifdef AP_SCAN_SUPPORT
Idx = BssTableSetEntry(pAd, &pAd->ScanTab, ie_list, Rssi, LenVIE, pVIE);
if (Idx != BSS_NOT_FOUND)
{
BSS_ENTRY *pBssEntry = &pAd->ScanTab.BssEntry[Idx];
NdisMoveMemory(pBssEntry->PTSF, (PUCHAR)&Ptsf, 4);
pBssEntry->RegulatoryClass = pBcnRep->RegulatoryClass;
pBssEntry->CondensedPhyType = BcnReqInfoField.field.CondensePhyType;
pBssEntry->RSNI = pBcnRep->RSNI;
}
#endif /* AP_SCAN_SUPPORT */
return;
}
