/*
========================================================================
Routine Description:
1. Append bit 1 to end of the message
2. Append the length of message in rightmost 64 bits
3. Transform the Hash Value to digest message
Arguments:
pSHA_CTX Pointer to SHA256_CTX_STRUC
Return Value:
digestMessage Digest message
Note:
None
========================================================================
*/
VOID RT_SHA256_End (
IN SHA256_CTX_STRUC *pSHA_CTX,
OUT UINT8 DigestMessage[])
{
UINT index;
uint64_t message_length_bits;
/* Append bit 1 to end of the message */
NdisFillMemory(pSHA_CTX->Block + pSHA_CTX->BlockLen, 1, 0x80);
/* 55 = 64 - 8 - 1: append 1 bit(1 byte) and message length (8 bytes) */
if (pSHA_CTX->BlockLen > 55)
RT_SHA256_Hash(pSHA_CTX);
/* End of if */
/* Append the length of message in rightmost 64 bits */
message_length_bits = pSHA_CTX->MessageLen*8;
message_length_bits = cpu2be64(message_length_bits);
memmove(&pSHA_CTX->Block[56], &message_length_bits, 8);
RT_SHA256_Hash(pSHA_CTX);
/* Return message digest, transform the uint32_t hash value to bytes */
for (index = 0; index < 8;index++)
pSHA_CTX->HashValue[index] = cpu2be32(pSHA_CTX->HashValue[index]);
/* End of for */
memmove(DigestMessage, pSHA_CTX->HashValue, SHA256_DIGEST_SIZE);
} /* End of RT_SHA256_End */
/*
========================================================================
Routine Description:
1. Append bit 1 to end of the message
2. Append the length of message in rightmost 64 bits
3. Transform the Hash Value to digest message
Arguments:
pMD5_CTX Pointer to MD5_CTX_STRUC
Return Value:
digestMessage Digest message
Note:
None
========================================================================
*/
VOID RT_MD5_End (
IN MD5_CTX_STRUC *pMD5_CTX,
OUT UINT8 DigestMessage[])
{
UINT index;
UINT64 message_length_bits;
/* append 1 bits to end of the message */
NdisFillMemory(pMD5_CTX->Block + pMD5_CTX->BlockLen, 1, 0x80);
/* 55 = 64 - 8 - 1: append 1 bit(1 byte) and message length (8 bytes) */
if (pMD5_CTX->BlockLen > 55)
RT_MD5_Hash(pMD5_CTX);
/* End of if */
/* Append the length of message in rightmost 64 bits */
message_length_bits = pMD5_CTX->MessageLen*8;
message_length_bits = cpu2le64(message_length_bits);
NdisMoveMemory(&pMD5_CTX->Block[56], &message_length_bits, 8);
RT_MD5_Hash(pMD5_CTX);
/* Return message digest, transform the UINT32 hash value to bytes */
for (index = 0; index < 4;index++)
pMD5_CTX->HashValue[index] = cpu2le32(pMD5_CTX->HashValue[index]);
/* End of for */
NdisMoveMemory(DigestMessage, pMD5_CTX->HashValue, MD5_DIGEST_SIZE);
} /* End of RT_MD5_End */
inline u8* _rtw_zvmalloc(u32 sz)
{
u8 *pbuf;
#ifdef PLATFORM_LINUX
pbuf = _rtw_vmalloc(sz);
if (pbuf != NULL)
memset(pbuf, 0, sz);
#endif
#ifdef PLATFORM_WINDOWS
NdisAllocateMemoryWithTag(&pbuf,sz, RT_TAG);
if (pbuf != NULL)
NdisFillMemory(pbuf, sz, 0);
#endif
return pbuf;
}
void _rtw_memset(void *pbuf, int c, u32 sz)
{
#ifdef PLATFORM_LINUX
memset(pbuf, c, sz);
#endif
#ifdef PLATFORM_WINDOWS
#if 0
NdisZeroMemory(pbuf, sz);
if (c != 0) memset(pbuf, c, sz);
#else
NdisFillMemory(pbuf, sz, c);
#endif
#endif
}
u8* _rtw_zmalloc(u32 sz)
{
u8 *pbuf = _rtw_malloc(sz);
if (pbuf != NULL) {
#ifdef PLATFORM_LINUX
memset(pbuf, 0, sz);
#endif
#ifdef PLATFORM_WINDOWS
NdisFillMemory(pbuf, sz, 0);
#endif
}
return pbuf;
}
u8* _rtw_zmalloc(u32 sz)
{
u8 *pbuf;
#ifdef PLATFORM_LINUX
// kzalloc(sz, GFP_KERNEL);
pbuf = kmalloc(sz, /*GFP_KERNEL*/GFP_ATOMIC);
if (pbuf != NULL)
memset(pbuf, 0, sz);
#endif
#ifdef PLATFORM_WINDOWS
NdisAllocateMemoryWithTag(&pbuf,sz, RT_TAG);
if (pbuf != NULL)
NdisFillMemory(pbuf, sz, 0);
#endif
return pbuf;
}
u8* _rtw_zmalloc(u32 sz)
{
#ifdef PLATFORM_FREEBSD
return malloc(sz,M_DEVBUF,M_ZERO|M_NOWAIT);
#else // PLATFORM_FREEBSD
u8 *pbuf = _rtw_malloc(sz);
if (pbuf != NULL) {
#ifdef PLATFORM_LINUX
memset(pbuf, 0, sz);
#endif
#ifdef PLATFORM_WINDOWS
NdisFillMemory(pbuf, sz, 0);
#endif
}
return pbuf;
#endif // PLATFORM_FREEBSD
}
PVOID
filterAuditAllocMem(
NDIS_HANDLE NdisHandle,
ULONG Size,
ULONG FileNumber,
ULONG LineNumber
)
{
PVOID pBuffer;
PFILTERD_ALLOCATION pAllocInfo;
if (!filterdInitDone)
{
NdisAllocateSpinLock(&(filterdMemoryLock));
filterdInitDone = TRUE;
}
//
// Integer overflow check
//
if ((Size + sizeof(FILTERD_ALLOCATION)) < Size)
{
DEBUGP(DL_VERY_LOUD+50,
("filterAuditAllocMem: Integer overflow error file %d, line %d, Size %d \n",
FileNumber, LineNumber, Size));
pBuffer = NULL;
}
else
{
pAllocInfo = NdisAllocateMemoryWithTagPriority(
NdisHandle,
Size+sizeof(FILTERD_ALLOCATION),
(ULONG)'gdTF',
LowPoolPriority
);
if (pAllocInfo == (PFILTERD_ALLOCATION)NULL)
{
DEBUGP(DL_VERY_LOUD+50,
("filterAuditAllocMem: file %d, line %d, Size %d failed!\n",
FileNumber, LineNumber, Size));
pBuffer = NULL;
}
else
{
pBuffer = (PVOID)&(pAllocInfo->UserData);
NdisFillMemory(pBuffer, Size, 0xaf);
pAllocInfo->Signature = FILTERD_MEMORY_SIGNATURE;
pAllocInfo->FileNumber = FileNumber;
pAllocInfo->LineNumber = LineNumber;
pAllocInfo->Size = Size;
pAllocInfo->OwnerHandle = NdisHandle;
pAllocInfo->Next = (PFILTERD_ALLOCATION)NULL;
NdisAcquireSpinLock(&(filterdMemoryLock));
pAllocInfo->Prev = filterdMemoryTail;
if (filterdMemoryTail == (PFILTERD_ALLOCATION)NULL)
{
//
// empty list
//
filterdMemoryHead = filterdMemoryTail = pAllocInfo;
}
else
{
filterdMemoryTail->Next = pAllocInfo;
}
filterdMemoryTail = pAllocInfo;
filterdAllocCount++;
NdisReleaseSpinLock(&(filterdMemoryLock));
}
}
DEBUGP(DL_VERY_LOUD+100,
("filterAuditAllocMem: file %c%c%c%c, line %d, %d bytes, OwerHandle %p, Memory 0x%p\n",
(CHAR)(FileNumber & 0xff),
(CHAR)((FileNumber >> 8) & 0xff),
(CHAR)((FileNumber >> 16) & 0xff),
(CHAR)((FileNumber >> 24) & 0xff),
LineNumber, Size, NdisHandle, pBuffer));
return (pBuffer);
}
static BOOLEAN NfcProcessPasswdTV(
IN PRTMP_ADAPTER pAdapter,
IN PUCHAR pPlainData,
IN INT PlainLength,
IN PWSC_CTRL pWscCtrl,
IN UCHAR bHandOver)
{
USHORT WscType, WscLen;
PUCHAR pData;
INT DH_Len = 0, idx;
pData = pPlainData;
hex_dump("NfcProcessPasswdTV - PlainData", pPlainData, PlainLength);
// Start to process WSC IEs within credential
if (bHandOver)
{
pData +=2; /* Skip length of WSC attribute */
PlainLength -= 2;
}
while (PlainLength > 4)
{
WSC_IE TLV_Recv;
memcpy((UINT8 *)&TLV_Recv, pData, 4);
WscType = be2cpu16(TLV_Recv.Type);
WscLen = be2cpu16(TLV_Recv.Length);
pData += 4;
PlainLength -= 4;
// Parse M2 WSC type and store to RegData structure
switch (WscType)
{
case WSC_ID_OOB_DEV_PWD:
hex_dump("NfcProcessPasswdTV - WSC_ID_OOB_DEV_PWD", pData, WscLen);
NdisMoveMemory(&pWscCtrl->PeerNfcPasswdHash[0], pData, NFC_DEV_PASSWD_HASH_LEN);
NdisMoveMemory(&pWscCtrl->PeerNfcPasswdID, pData+20, sizeof(pWscCtrl->PeerNfcPasswdID));
pWscCtrl->PeerNfcPasswdLen = WscLen - 2 - NFC_DEV_PASSWD_HASH_LEN;
if (pWscCtrl->PeerNfcPasswdLen > NFC_DEV_PASSWD_LEN)
{
MTWF_LOG(DBG_CAT_ALL, DBG_SUBCAT_ALL, DBG_LVL_TRACE, ("%s --> PeerNfcPasswdLen(%d) > NFC_DEV_PASSWD_LEN(%d)\n",
__FUNCTION__, pWscCtrl->PeerNfcPasswdLen, NFC_DEV_PASSWD_LEN));
pWscCtrl->PeerNfcPasswdLen = NFC_DEV_PASSWD_LEN;
}
if (bHandOver== TRUE)
{
if (pWscCtrl->PeerNfcPasswdID != DEV_PASS_ID_NFC_HANDOVER)
MTWF_LOG(DBG_CAT_ALL, DBG_SUBCAT_ALL, DBG_LVL_TRACE, ("%s --> PeerNfcPasswdID(%04x) it should 0x0007 \n",
__FUNCTION__, pWscCtrl->PeerNfcPasswdID));
}
NdisMoveMemory(&pWscCtrl->PeerNfcPasswd[0], pData+20+sizeof(pWscCtrl->PeerNfcPasswdID), pWscCtrl->PeerNfcPasswdLen);
hex_dump("PeerNfcPasswd", pWscCtrl->PeerNfcPasswd, pWscCtrl->PeerNfcPasswdLen);
if (bHandOver==FALSE)
{
/* Due to M3 & M4 use the same WscGenPSK1 function,
need copy to NfcPasswd, too */
NdisMoveMemory(&pWscCtrl->NfcPasswd[0], &pWscCtrl->PeerNfcPasswd[0], pWscCtrl->PeerNfcPasswdLen);
pWscCtrl->NfcPasswdLen = pWscCtrl->PeerNfcPasswdLen;
}
break;
case WSC_ID_UUID_E:
NdisMoveMemory(pWscCtrl->RegData.PeerInfo.Uuid, pData, WscLen);
break;
case WSC_ID_SSID:
hex_dump("NfcProcessPasswdTV - WSC_ID_SSID", pData, WscLen);
break;
case WSC_ID_VENDOR_EXT:
hex_dump("NfcProcessPasswdTV - WSC_ID_VENDOR_EXT", pData, WscLen);
break;
default:
MTWF_LOG(DBG_CAT_ALL, DBG_SUBCAT_ALL, DBG_LVL_TRACE, ("%s --> Unknown IE 0x%04x\n", __FUNCTION__, WscType));
break;
}
// Offset to net WSC Ie
pData += WscLen;
PlainLength -= WscLen;
}
MTWF_LOG(DBG_CAT_ALL, DBG_SUBCAT_ALL, DBG_LVL_TRACE, ("%s --> bHandOver=%d", __FUNCTION__, bHandOver));
if (bHandOver== FALSE)
{
DH_Len = sizeof(pWscCtrl->RegData.Pkr);
// Enrollee 192 random bytes for DH key generation
for (idx = 0; idx < 192; idx++)
pWscCtrl->RegData.EnrolleeRandom[idx] = RandomByte(pAdapter);
NdisZeroMemory(pWscCtrl->RegData.Pkr, sizeof(pWscCtrl->RegData.Pkr));
RT_DH_PublicKey_Generate (
WPS_DH_G_VALUE, sizeof(WPS_DH_G_VALUE),
WPS_DH_P_VALUE, sizeof(WPS_DH_P_VALUE),
pWscCtrl->RegData.EnrolleeRandom, sizeof(pWscCtrl->RegData.EnrolleeRandom),
pWscCtrl->RegData.Pkr, (UINT *) &DH_Len);
/* Need to prefix zero padding */
if((DH_Len != sizeof(pWscCtrl->RegData.Pkr)) &&
(DH_Len < sizeof(pWscCtrl->RegData.Pkr)))
{
UCHAR TempKey[192];
INT DiffCnt;
DiffCnt = sizeof(pWscCtrl->RegData.Pkr) - DH_Len;
NdisFillMemory(&TempKey, DiffCnt, 0);
NdisCopyMemory(&TempKey[DiffCnt], pWscCtrl->RegData.Pkr, DH_Len);
NdisCopyMemory(pWscCtrl->RegData.Pkr, TempKey, sizeof(TempKey));
DH_Len += DiffCnt;
MTWF_LOG(DBG_CAT_ALL, DBG_SUBCAT_ALL, DBG_LVL_TRACE, ("%s: Do zero padding!\n", __FUNCTION__));
}
hex_dump("Pkr", pWscCtrl->RegData.Pkr, 192);
}
return TRUE;
}
INT NfcBuildOOBDevPasswdTLV(
IN PRTMP_ADAPTER pAd,
IN PWSC_CTRL pWscCtrl,
IN UCHAR HandoverType,
OUT UCHAR *pbuf,
OUT USHORT *pBufLen)
{
INT Status = NDIS_STATUS_SUCCESS;
UCHAR *TB = NULL;;
PUCHAR pData = NULL, pSrcData = NULL;
USHORT PasswdID = 0, len;
PWSC_REG_DATA pReg = (PWSC_REG_DATA) &pWscCtrl->RegData;
INT Len = 0, templen = 0;
INT DH_Len = 0, idx;
UCHAR HashData[SHA256_DIGEST_SIZE];
INT nfc_dev_passwd_len=0;
#ifdef WSC_V2_SUPPORT
PWSC_TLV pWscTLV = &pWscCtrl->WscV2Info.ExtraTlv;
#endif /* WSC_V2_SUPPORT */
os_alloc_mem(pAd, (UCHAR **)&pData, (NFC_WSC_TLV_SIZE*sizeof(UCHAR)));
if (pData == NULL)
{
return NDIS_STATUS_RESOURCES;
}
os_alloc_mem(pAd, (UCHAR **)&TB, (128*sizeof(UCHAR)));
if (pData == NULL)
{
os_free_mem(NULL, pData);
return NDIS_STATUS_RESOURCES;
}
DH_Len = sizeof(pWscCtrl->RegData.Pke);
/*
Enrollee 192 random bytes for DH key generation
*/
for (idx = 0; idx < 192; idx++)
pWscCtrl->RegData.EnrolleeRandom[idx] = RandomByte(pAd);
NdisZeroMemory(pWscCtrl->RegData.Pke, sizeof(pWscCtrl->RegData.Pke));
RT_DH_PublicKey_Generate (
WPS_DH_G_VALUE, sizeof(WPS_DH_G_VALUE),
WPS_DH_P_VALUE, sizeof(WPS_DH_P_VALUE),
pWscCtrl->RegData.EnrolleeRandom, sizeof(pWscCtrl->RegData.EnrolleeRandom),
pWscCtrl->RegData.Pke, (UINT *) &DH_Len);
/* Need to prefix zero padding */
if((DH_Len != sizeof(pWscCtrl->RegData.Pke)) &&
(DH_Len < sizeof(pWscCtrl->RegData.Pke)))
{
UCHAR TempKey[192];
INT DiffCnt;
DiffCnt = sizeof(pWscCtrl->RegData.Pke) - DH_Len;
NdisFillMemory(&TempKey, DiffCnt, 0);
NdisCopyMemory(&TempKey[DiffCnt], pWscCtrl->RegData.Pke, DH_Len);
NdisCopyMemory(pWscCtrl->RegData.Pke, TempKey, sizeof(TempKey));
DH_Len += DiffCnt;
MTWF_LOG(DBG_CAT_ALL, DBG_SUBCAT_ALL, DBG_LVL_TRACE, ("%s: Do zero padding!\n", __FUNCTION__));
}
/* For Handover case, We may as Registrar
So keep the same public key for Registrar */
RTMPMoveMemory(pWscCtrl->RegData.Pkr, pWscCtrl->RegData.Pke, DH_Len);
hex_dump("Pkr", pWscCtrl->RegData.Pkr, DH_Len);
hex_dump("Pke", pWscCtrl->RegData.Pkr, DH_Len);
RT_SHA256(&pWscCtrl->RegData.Pke[0], 192, &HashData[0]);
RTMPMoveMemory(&pWscCtrl->NfcPasswdHash[0], &HashData[0], NFC_DEV_PASSWD_HASH_LEN);
hex_dump("NfcBuildOOBDevPasswdTLV - Public Key HashData", &HashData[0], 20);
if (HandoverType == TYPE_PASSWDHO_S || HandoverType == TYPE_PASSWDHO_R)
{
PasswdID = DEV_PASS_ID_NFC_HANDOVER;
pWscCtrl->NfcPasswdID = cpu2be16(PasswdID);
NdisZeroMemory(&pWscCtrl->NfcPasswd[0], 32);
pWscCtrl->NfcPasswdLen = NFC_DEV_PASSWD_LEN;
}
else
{
PasswdID = (RandomByte(pAd) << 8) + RandomByte(pAd);
if (PasswdID < 0x10)
PasswdID = 0x10;
pWscCtrl->NfcPasswdID = cpu2be16(PasswdID);
NfcGenRandomPasswd(pAd, pWscCtrl);
hex_dump("NfcBuildOOBDevPasswdTLV - NfcPasswd", &pWscCtrl->NfcPasswd[0], NFC_DEV_PASSWD_LEN);
}
pSrcData = pData;
NdisZeroMemory(pData, NFC_WSC_TLV_SIZE);
NdisZeroMemory(&TB[0], 128);
if (HandoverType == TYPE_PASSWDHO_R || HandoverType == TYPE_PASSWDHO_S)
{
/* Reserv for "Length of WSC attribute" */
pData += 2;
}
hex_dump("NfcBuildOOBDevPasswdTLV - 1 pSrcData", pSrcData, Len);
NdisMoveMemory(&TB[0], &HashData[0], NFC_DEV_PASSWD_HASH_LEN);
NdisMoveMemory(&TB[20], &pWscCtrl->NfcPasswdID, sizeof(pWscCtrl->NfcPasswdID));
if (HandoverType == TYPE_PASSWORD)
{
/* New SPEC Handover remove this part. */
NdisMoveMemory(&TB[22], &pWscCtrl->NfcPasswd[0], NFC_DEV_PASSWD_LEN);
nfc_dev_passwd_len = NFC_DEV_PASSWD_LEN;
}
templen = AppendWSCTLV(WSC_ID_OOB_DEV_PWD, pData, &TB[0], NFC_DEV_PASSWD_HASH_LEN+sizeof(PasswdID)+nfc_dev_passwd_len);
pData += templen;
Len += templen;
hex_dump("NfcBuildOOBDevPasswdTLV - 2 pSrcData", pSrcData, Len);
if (HandoverType == TYPE_PASSWDHO_S) /* Build for Handover Select Message */
{
templen = AppendWSCTLV(WSC_ID_SSID, pData, pAd->ApCfg.MBSSID[0].Ssid, pAd->ApCfg.MBSSID[0].SsidLen);
pData += templen;
Len += templen;
/* Optional items. RF_Band, AP_Channel and MAC_Address */
UCHAR RF_Band;
if (pAd->CommonCfg.Channel > 14)
RF_Band = 0x02; /* 5.0GHz */
else
RF_Band = 0x01; /* 2.4GHz */
templen = AppendWSCTLV(WSC_ID_RF_BAND, pData, &RF_Band, 0);
pData += templen;
Len += templen;
USHORT Channel = 0;
Channel = pAd->CommonCfg.Channel;
#ifdef RT_BIG_ENDIAN
Channel = SWAP16(Channel);
#endif /* RT_BIG_ENDIAN */
templen = AppendWSCTLV(WSC_ID_AP_CHANNEL, pData, (UINT8 *)&Channel, 0);
pData += templen;
Len += templen;
templen = AppendWSCTLV(WSC_ID_MAC_ADDR, pData, pAd->CommonCfg.Bssid, 0);
pData += templen;
Len += templen;
}
else if (HandoverType == TYPE_PASSWDHO_R) /* Build for Handover Request Message */
{
templen = AppendWSCTLV(WSC_ID_UUID_E, pData, &pWscCtrl->Wsc_Uuid_E[0], 0);
pData += templen;
Len += templen;
}
#ifdef WSC_V2_SUPPORT
if (pWscCtrl->WscV2Info.bEnableWpsV2)
{
/* Version2 */
WscGenV2Msg(pWscCtrl,
FALSE,
NULL,
0,
&pData,
&Len);
/* Extra attribute that is not defined in WSC Sepc. */
if (pWscTLV->pTlvData && pWscTLV->TlvLen)
{
templen = AppendWSCTLV(pWscTLV->TlvTag, pData, (UINT8 *)pWscTLV->pTlvData, pWscTLV->TlvLen);
pData += templen;
Len += templen;
}
}
#endif // WSC_V2_SUPPORT //
if (HandoverType == TYPE_PASSWDHO_R || HandoverType == TYPE_PASSWDHO_S)
{
/*Assign for "Length of WSC attribute" */
len = cpu2be16(Len);
memcpy(pSrcData, &len, 2);
hex_dump("NfcBuildOOBDevPasswdTLV - pSrcData", pSrcData, Len+2);
}
else
hex_dump("NfcBuildOOBDevPasswdTLV - pSrcData", pSrcData, Len);
if (pbuf && (Len < NFC_WSC_TLV_SIZE))
{
if (HandoverType == TYPE_PASSWDHO_R || HandoverType == TYPE_PASSWDHO_S)
{
NdisMoveMemory(pbuf, pSrcData, Len+2);
*pBufLen = (USHORT)Len+2;
}
else
{
NdisMoveMemory(pbuf, pSrcData, Len);
*pBufLen = (USHORT)Len;
}
hex_dump("NfcBuildOOBDevPasswdTLV", pbuf, *pBufLen);
}
else
{
MTWF_LOG(DBG_CAT_ALL, DBG_SUBCAT_ALL, DBG_LVL_TRACE, ("%s: (Len=%d)\n", __FUNCTION__, Len));
Status = NDIS_STATUS_RESOURCES;
}
os_free_mem(NULL, pSrcData);
os_free_mem(NULL, TB);
return Status;
}
