void MeshBeaconTest()
{
uint8_t testRxBeacon[14] = {0x7f, 0x28, 0x9a, 0x8c, 0x32, 0x18, 0x8a, 0x3e, 0x01, 0x02, 0xa6, 0xc2, 0x3b, 0xef};
TMeshBeaconSecNwk meshBeacon;
memcpy(&meshBeacon, testRxBeacon, 14);
uint8_t encryption_key[16];
uint8_t temp_mac[16];
uint8_t mac[16];
uint8_t smnb[4] = {'s', 'm', 'n', 'b'};
uint8_t netID[16];
MeshEncryptionKeyGet(encryption_key);
MeshNetIDGet(netID);
uint16_t ivi = MeshIVindexGet();
uint16_t CIVI = 0;
uint8_t kr = meshBeacon.kr_nid[0]&0x80;
CIVI = LE_EXTRN2WORD((uint8_t*)&meshBeacon.civi);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "RX KR= %d", 1, kr);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "RX CIVI= 0x%x", 1, CIVI);
uint8_t kr_nid_civi[19];
memcpy(kr_nid_civi, &kr, 1);
memcpy(kr_nid_civi+1, netID, 16);
memcpy(kr_nid_civi+1+16, &CIVI, 2);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "kr_nid_civi = ", 0);
MeshShowKey(kr_nid_civi, 19);
AES_CMAC(encryption_key, kr_nid_civi, 19, temp_mac);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "Temp Mac = ", 0);
MeshShowKey(temp_mac, 16);//pointer to cmac, offset 10
AES_CMAC(temp_mac, smnb, 4, mac);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "Received Beacon Mac = ", 0);
MeshShowKey((uint8_t*)&meshBeacon+ 10, 4);//pointer to cmac, offset 10
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "Calc Beacon Mac = ", 0);
MeshShowKey(mac, 16);
if(memcmp((uint8_t*)&meshBeacon+ 10, mac+12, 4) == 0)
{
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "MAC MATCH", 0);
}
else
{
DBG_BUFFER(MODULE_APP, LEVEL_ERROR, "MAC dis-match", 0);
}
}
void MeshGenerateIVzero(void)
{
uint8_t smit[4] = {'s', 'm', 'i', 't'};
uint8_t smiz[4] = {'s', 'm', 'i', 'z'};
uint8_t mac[16];
AES_CMAC(netKey, smit, 4, mac);
AES_CMAC(mac, smiz, 4, IVzero);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "IVzero = ", 0);
MeshShowKey(IVzero, 16);
}
void MeshGenerateEncryptionKey(void)
{
uint8_t smit[4] = {'s', 'm', 'n', 't'};
uint8_t smiz[4] = {'s', 'm', 'n', 'k'};
uint8_t mac[16];
AES_CMAC(netKey, smit, 4, mac);
AES_CMAC(mac, smiz, 4, EncryptionKey);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "EncryptionKey = ", 0);
MeshShowKey(EncryptionKey, 16);
}
/** This function has to be called once after netKey or IVindex is set! */
void MeshGenerateNetworkIV(void)
{
uint8_t smiv[4] = {'s', 'm', 'i', 'v'};
uint8_t iv_zero_index[18];
uint8_t mac[16];
memcpy(iv_zero_index, IVzero, 16);
BE_WORD2EXTRN(iv_zero_index + 16, IVindex);
AES_CMAC(netKey, iv_zero_index, 18, mac);
AES_CMAC(mac, smiv, 4, networkIV);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "NetworkIV = ", 0);
MeshShowKey(networkIV, 16);
}
int kirk_forge(u8* inbuff, int insize)
{
KIRK_CMD1_HEADER* header = (KIRK_CMD1_HEADER*)inbuff;
AES_ctx cmac_key;
u8 cmac_header_hash[16];
u8 cmac_data_hash[16];
int chk_size;
if(is_kirk_initialized == 0) return KIRK_NOT_INITIALIZED;
if(!(header->mode == KIRK_MODE_CMD1 || header->mode == KIRK_MODE_CMD2 || header->mode == KIRK_MODE_CMD3)) return KIRK_INVALID_MODE;
if(header->data_size == 0) return KIRK_DATA_SIZE_ZERO;
if(header->mode == KIRK_MODE_CMD1){
header_keys keys; //0-15 AES key, 16-31 CMAC key
AES_cbc_decrypt(&aes_kirk1, inbuff, (u8*)&keys, 32); //decrypt AES & CMAC key to temp buffer
AES_set_key(&cmac_key, keys.CMAC, 128);
AES_CMAC(&cmac_key, inbuff+0x60, 0x30, cmac_header_hash);
if(memcmp(cmac_header_hash, header->CMAC_header_hash, 16) != 0) {
return KIRK_HEADER_HASH_INVALID;
}
//Make sure data is 16 aligned
chk_size = header->data_size;
if(chk_size % 16) chk_size += 16 - (chk_size % 16);
AES_CMAC(&cmac_key, inbuff+0x60, 0x30 + chk_size + header->data_offset, cmac_data_hash);
if(memcmp(cmac_data_hash, header->CMAC_data_hash, 16) != 0) {
//printf("data hash invalid, correcting...\n");
} else {
printf("data hash is already valid!\n");
return 100;
}
// Forge collision for data hash
memcpy(cmac_data_hash,header->CMAC_data_hash,0x10);
AES_CMAC_forge(&cmac_key, inbuff+0x60, 0x30+ chk_size + header->data_offset, cmac_data_hash);
//printf("Last row in bad file should be :\n"); for(i=0;i<0x10;i++) printf("%02x", cmac_data_hash[i]);
//printf("\n\n");
return KIRK_OPERATION_SUCCESS;
}
return KIRK_SIG_CHECK_INVALID; //Checks for cmd 2 & 3 not included right now
}
int kirk_CMD0(void* outbuff, void* inbuff, int size, int generate_trash)
{
if(is_kirk_initialized == 0) return KIRK_NOT_INITIALIZED;
KIRK_CMD1_HEADER* header = (KIRK_CMD1_HEADER*)outbuff;
memcpy(outbuff, inbuff, size);
if(header->mode != KIRK_MODE_CMD1) return KIRK_INVALID_MODE;
header_keys *keys = (header_keys *)outbuff; //0-15 AES key, 16-31 CMAC key
//FILL PREDATA WITH RANDOM DATA
if(generate_trash) kirk_CMD14(outbuff+sizeof(KIRK_CMD1_HEADER), header->data_offset);
//Make sure data is 16 aligned
int chk_size = header->data_size;
if(chk_size % 16) chk_size += 16 - (chk_size % 16);
//ENCRYPT DATA
AES_ctx k1;
AES_set_key(&k1, keys->AES, 128);
AES_cbc_encrypt(&k1, inbuff+sizeof(KIRK_CMD1_HEADER)+header->data_offset, outbuff+sizeof(KIRK_CMD1_HEADER)+header->data_offset, chk_size);
//CMAC HASHES
AES_ctx cmac_key;
AES_set_key(&cmac_key, keys->CMAC, 128);
u8 cmac_header_hash[16];
u8 cmac_data_hash[16];
AES_CMAC(&cmac_key, outbuff+0x60, 0x30, cmac_header_hash);
AES_CMAC(&cmac_key, outbuff+0x60, 0x30 + chk_size + header->data_offset, cmac_data_hash);
memcpy(header->CMAC_header_hash, cmac_header_hash, 16);
memcpy(header->CMAC_data_hash, cmac_data_hash, 16);
//ENCRYPT KEYS
AES_cbc_encrypt(&aes_kirk1, inbuff, outbuff, 16*2);
return KIRK_OPERATION_SUCCESS;
}
int kirk_CMD10(void* inbuff, int insize)
{
if(is_kirk_initialized == 0) return KIRK_NOT_INITIALIZED;
KIRK_CMD1_HEADER* header = (KIRK_CMD1_HEADER*)inbuff;
if(!(header->mode == KIRK_MODE_CMD1 || header->mode == KIRK_MODE_CMD2 || header->mode == KIRK_MODE_CMD3)) return KIRK_INVALID_MODE;
if(header->data_size == 0) return KIRK_DATA_SIZE_ZERO;
if(header->mode == KIRK_MODE_CMD1)
{
header_keys keys; //0-15 AES key, 16-31 CMAC key
AES_cbc_decrypt(&aes_kirk1, inbuff, (u8*)&keys, 32); //decrypt AES & CMAC key to temp buffer
AES_ctx cmac_key;
AES_set_key(&cmac_key, keys.CMAC, 128);
u8 cmac_header_hash[16];
u8 cmac_data_hash[16];
AES_CMAC(&cmac_key, inbuff+0x60, 0x30, cmac_header_hash);
//Make sure data is 16 aligned
int chk_size = header->data_size;
if(chk_size % 16) chk_size += 16 - (chk_size % 16);
AES_CMAC(&cmac_key, inbuff+0x60, 0x30 + chk_size + header->data_offset, cmac_data_hash);
if(memcmp(cmac_header_hash, header->CMAC_header_hash, 16) != 0)
{
printf("header hash invalid\n");
return KIRK_HEADER_HASH_INVALID;
}
if(memcmp(cmac_data_hash, header->CMAC_data_hash, 16) != 0)
{
printf("data hash invalid\n");
return KIRK_DATA_HASH_INVALID;
}
return KIRK_OPERATION_SUCCESS;
}
return KIRK_SIG_CHECK_INVALID; //Checks for cmd 2 & 3 not included right now
}
BOOLEAN PMF_CalculateBIPMIC(
IN PRTMP_ADAPTER pAd,
IN PUCHAR pAadHdr,
IN PUCHAR pFrameBuf,
IN UINT32 FrameLen,
IN PUCHAR pKey,
OUT PUCHAR pBipMic)
{
UCHAR *m_buf;
UINT32 total_len;
UCHAR cmac_output[16];
UINT mlen = AES_KEY128_LENGTH;
/* Allocate memory for MIC calculation */
os_alloc_mem(NULL, (PUCHAR *)&m_buf, MGMT_DMA_BUFFER_SIZE);
if (m_buf == NULL)
{
DBGPRINT(RT_DEBUG_ERROR, ("%s : out of resource.\n", __FUNCTION__));
return FALSE;
}
/* Initialize the buffer */
NdisZeroMemory(m_buf, MGMT_DMA_BUFFER_SIZE);
/* Construct the concatenation */
NdisMoveMemory(m_buf, pAadHdr, LEN_PMF_BIP_AAD_HDR);
total_len = LEN_PMF_BIP_AAD_HDR;
/* Append the Mgmt frame into the concatenation */
NdisMoveMemory(&m_buf[total_len], pFrameBuf, FrameLen);
total_len += FrameLen;
/* Compute AES-128-CMAC over the concatenation */
AES_CMAC(m_buf, total_len, pKey, 16, cmac_output, &mlen);
/* Truncate the first 64-bits */
NdisMoveMemory(pBipMic, cmac_output, LEN_PMF_BIP_MIC);
os_free_mem(NULL, m_buf);
return TRUE;
}
void test_cmac(void)
{
SMP_TRACE_EVENT ("test_cmac ");
UINT8 M[64] = {
0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96,
0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a,
0xae, 0x2d, 0x8a, 0x57, 0x1e, 0x03, 0xac, 0x9c,
0x9e, 0xb7, 0x6f, 0xac, 0x45, 0xaf, 0x8e, 0x51,
0x30, 0xc8, 0x1c, 0x46, 0xa3, 0x5c, 0xe4, 0x11,
0xe5, 0xfb, 0xc1, 0x19, 0x1a, 0x0a, 0x52, 0xef,
0xf6, 0x9f, 0x24, 0x45, 0xdf, 0x4f, 0x9b, 0x17,
0xad, 0x2b, 0x41, 0x7b, 0xe6, 0x6c, 0x37, 0x10
};
UINT8 key[16] = {
0x3c, 0x4f, 0xcf, 0x09, 0x88, 0x15, 0xf7, 0xab,
0xa6, 0xd2, 0xae, 0x28, 0x16, 0x15, 0x7e, 0x2b
};
UINT8 i =0, tmp;
UINT16 len;
len = 64;
for (i = 0; i < len/2; i ++)
{
tmp = M[i];
M[i] = M[len -1 - i];
M[len -1 - i] = tmp;
}
memset(&cmac_cb, 0, sizeof(tCMAC_CB));
SMP_TRACE_WARNING("\n Example 1: len = %d\n", len);
AES_CMAC(key, M, len, 128, test_cmac_cback, 0);
}
/*
==========================================================================
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;
}
/*
==========================================================================
Description:
IRQL = PASSIVE_LEVEL
==========================================================================
*/
VOID
TDLS_BuildTeardown(
IN PRTMP_ADAPTER pAd,
OUT PUCHAR pFrameBuf,
OUT PULONG pFrameLen,
IN PRT_802_11_TDLS pTDLS,
IN UINT16 ReasonCode)
{
/* fill action code */
TDLS_InsertActField(pAd, (pFrameBuf + *pFrameLen), pFrameLen, CATEGORY_TDLS, TDLS_ACTION_CODE_TEARDOWN);
/* fill reason code */
TDLS_InsertReasonCode(pAd, (pFrameBuf + *pFrameLen), pFrameLen, ReasonCode);
/* fill link identifier */
if (pTDLS->bInitiator)
TDLS_InsertLinkIdentifierIE(pAd, (pFrameBuf + *pFrameLen), pFrameLen, pTDLS->MacAddr, pAd->CurrentAddress);
else
TDLS_InsertLinkIdentifierIE(pAd, (pFrameBuf + *pFrameLen), pFrameLen, pAd->CurrentAddress, pTDLS->MacAddr);
// FTIE includes if RSNA Enabled
if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) || (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)))
{
UCHAR FTIe[128];
FT_FTIE *ft = NULL;
UCHAR content[256];
ULONG c_len = 0;
ULONG tmp_len = 0;
UCHAR seq = 4;
UCHAR mic[16];
UCHAR LinkIdentifier[20];
UINT tmp_aes_len = 0;
NdisZeroMemory(LinkIdentifier, 20);
LinkIdentifier[0] = IE_TDLS_LINK_IDENTIFIER;
LinkIdentifier[1] = 18;
NdisMoveMemory(&LinkIdentifier[2], pAd->CommonCfg.Bssid, 6);
if (pTDLS->bInitiator)
{
NdisMoveMemory(&LinkIdentifier[8], pTDLS->MacAddr, 6);
NdisMoveMemory(&LinkIdentifier[14], pAd->CurrentAddress, 6);
}
else
{
NdisMoveMemory(&LinkIdentifier[8], pAd->CurrentAddress, 6);
NdisMoveMemory(&LinkIdentifier[14], pTDLS->MacAddr, 6);
}
//NdisMoveMemory(&LinkIdentifier[8], pTDLS->MacAddr, 6);
//NdisMoveMemory(&LinkIdentifier[14], pAd->CurrentAddress, 6);
// FTIE (7.3.2.48)
// The contents of FTIE in the TDLS Teardown frame shall be the same as that included
// in the TPK Handshake Message3 with the exception of the MIC field.
// Construct FTIE (IE + Length + MIC Control + MIC + ANonce + SNonce)
// point to the element of IE
NdisZeroMemory(FTIe, sizeof(FTIe));
FTIe[0] = IE_FT_FTIE;
FTIe[1] = sizeof(FT_FTIE);
ft = (PFT_FTIE)&FTIe[2];
NdisMoveMemory(ft->ANonce, pTDLS->ANonce, 32);
NdisMoveMemory(ft->SNonce, pTDLS->SNonce, 32);
////////////////////////////////////////////////////////////////////////
// The MIC field of FTIE shall be calculated on the concatenation, in the following order, of
// 1. Link Identifier (20 bytes)
// 2. Reason Code (2 bytes)
// 3. Dialog token (1 byte)
// 4. Transaction Sequence = 4 (1 byte)
// 5. FTIE with the MIC field of FTIE set to zero (84 bytes)
/* concatenate Link Identifier, Reason Code, Dialog token, Transaction Sequence */
MakeOutgoingFrame(content, &tmp_len,
sizeof(LinkIdentifier), LinkIdentifier,
2, &ReasonCode,
1, &pTDLS->Token,
1, &seq,
END_OF_ARGS);
c_len += tmp_len;
/* concatenate FTIE */
MakeOutgoingFrame(content + c_len, &tmp_len,
FTIe[1] + 2, FTIe,
END_OF_ARGS);
c_len += tmp_len;
/* Calculate MIC */
NdisZeroMemory(mic, sizeof(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);
/* Fill Mic to ft struct */
NdisMoveMemory(ft->MIC, mic, 16);
////////////////////////////////////////////////////////////////////////
// Insert FT_IE to outgoing frame
TDLS_InsertFTIE(
pAd,
(pFrameBuf + *pFrameLen),
pFrameLen,
FTIe[1],
ft->MICCtr,
ft->MIC,
ft->ANonce,
ft->SNonce);
}
}
/*
==========================================================================
Description:
IRQL = PASSIVE_LEVEL
==========================================================================
*/
VOID
TDLS_BuildSetupConfirm(
IN PRTMP_ADAPTER pAd,
OUT PUCHAR pFrameBuf,
OUT PULONG pFrameLen,
IN PRT_802_11_TDLS pTDLS,
IN UCHAR RsnLen,
IN PUCHAR pRsnIe,
IN UCHAR FTLen,
IN PUCHAR pFTIe,
IN UCHAR TILen,
IN PUCHAR pTIIe,
IN UINT16 StatusCode)
{
/* fill action code */
TDLS_InsertActField(pAd, (pFrameBuf + *pFrameLen), pFrameLen, CATEGORY_TDLS, TDLS_ACTION_CODE_SETUP_CONFIRM);
/* fill status code */
TDLS_InsertStatusCode(pAd, (pFrameBuf + *pFrameLen), pFrameLen, StatusCode);
/* fill Dialog Token */
TDLS_InsertDialogToken(pAd, (pFrameBuf + *pFrameLen), pFrameLen, pTDLS->Token);
/* fill link identifier */
TDLS_InsertLinkIdentifierIE(pAd, (pFrameBuf + *pFrameLen), pFrameLen, pAd->CurrentAddress, pTDLS->MacAddr);
// fill Qos Capability
TDLS_InsertEDCAParameterSetIE(pAd, (pFrameBuf + *pFrameLen), pFrameLen, pTDLS);
#ifdef DOT11_N_SUPPORT
// fill HT Capability
TDLS_InsertHtCapIE(pAd, (pFrameBuf + *pFrameLen), pFrameLen);
#endif // DOT11_N_SUPPORT //
// TPK Handshake if RSNA Enabled
// Pack TPK Message 3 here!
if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) || (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)))
{
FT_FTIE *ft;
ULONG tmp;
// RSNIE (7.3.2.25)
// Insert RSN_IE of the Peer TDLS to outgoing frame
MakeOutgoingFrame((pFrameBuf + *pFrameLen), &tmp,
RsnLen, pRsnIe,
END_OF_ARGS);
*pFrameLen = *pFrameLen + tmp;
// FTIE (7.3.2.48)
// Construct FTIE (IE + Length + MIC Control + MIC + ANonce + SNonce)
// point to the element of IE
ft = (FT_FTIE *)(pFTIe + 2);
// set MIC field to zero before MIC calculation
NdisZeroMemory(ft->MIC, 16);
////////////////////////////////////////////////////////////////////////
// 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 = 2 (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];
UCHAR LinkIdentifier[20];
UINT tmp_aes_len = 0;
NdisZeroMemory(LinkIdentifier, 20);
LinkIdentifier[0] = IE_TDLS_LINK_IDENTIFIER;
LinkIdentifier[1] = 18;
NdisMoveMemory(&LinkIdentifier[2], pAd->CommonCfg.Bssid, 6);
NdisMoveMemory(&LinkIdentifier[8], pAd->CurrentAddress, 6);
NdisMoveMemory(&LinkIdentifier[14], pTDLS->MacAddr, 6);
NdisZeroMemory(mic, sizeof(mic));
/* make a header frame for calculating MIC. */
MakeOutgoingFrame(content, &tmp_len,
MAC_ADDR_LEN, pAd->CurrentAddress,
MAC_ADDR_LEN, pTDLS->MacAddr,
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);
// Fill Mic to ft struct
NdisMoveMemory(ft->MIC, mic, 16);
}
////////////////////////////////////////////////////////////////////////
// Insert FT_IE to outgoing frame
TDLS_InsertFTIE(
pAd,
(pFrameBuf + *pFrameLen),
pFrameLen,
sizeof(FT_FTIE),
ft->MICCtr,
ft->MIC,
ft->ANonce,
ft->SNonce);
// Timeout Interval (7.3.2.49)
// Insert TI_IE to outgoing frame
TDLS_InsertTimeoutIntervalIE(
pAd,
(pFrameBuf + *pFrameLen),
pFrameLen,
2, /* key lifetime interval */
pTDLS->KeyLifetime);
}
}
void MeshBeaconReceive(uint8_t* pbuffer, uint8_t offset, uint8_t len)
{
TMeshBeacon*pmesh_beacon_msg = (TMeshBeacon *)(pbuffer + offset);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "MeshBeaconReceive: service_uuid(0x%x), beacon_type(%d)", 2,
pmesh_beacon_msg->header.service_uuid,
pmesh_beacon_msg->beacon_type);
//for test only
// return;
switch(pmesh_beacon_msg->beacon_type)
{
case BEACON_TYPE_UNPROVISIONED_NODE:
{
TMeshBeaconUnprovisionedNode *pMeshBeaconUnprovisionedNode = (TMeshBeaconUnprovisionedNode *)&pmesh_beacon_msg->p.UnprovisionedNode;
(void)pMeshBeaconUnprovisionedNode;
break;
}
case BEACON_TYPE_SECNWK:
{
TMeshBeaconSecNwk *pMeshBeaconSecNwk = (TMeshBeaconSecNwk *)&pmesh_beacon_msg->p.sec_nwk;
// uint8_t testRxBeacon[14] = {0x7f, 0x28, 0x9a, 0x8c, 0x32, 0x18, 0x8a, 0x3e, 0x01, 0x02, 0xa6, 0xc2, 0x3b, 0xef};
uint8_t encryption_key[16];
uint8_t temp_mac[16];
uint8_t mac[16];
uint8_t smnb[4] = {'s', 'm', 'n', 'b'};
uint8_t netID[16];
MeshEncryptionKeyGet(encryption_key);
MeshNetIDGet(netID);
uint16_t ivi = MeshIVindexGet();
uint16_t CIVI = 0;
uint8_t kr = (pMeshBeaconSecNwk->kr_nid[0]&0x80)?1:0;
CIVI = LE_EXTRN2WORD((uint8_t*)&pMeshBeaconSecNwk->civi);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "RX KR= %d", 1, kr);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "RX CIVI= 0x%x", 1, CIVI);
uint8_t kr_nid_civi[19];
memcpy(kr_nid_civi, &kr, 1);
memcpy(kr_nid_civi+1, netID, 16);
memcpy(kr_nid_civi+1+16, &CIVI, 2);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "kr_nid_civi = ", 0);
MeshShowKey(kr_nid_civi, 19);
AES_CMAC(encryption_key, kr_nid_civi, 19, temp_mac);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "Temp Mac = ", 0);
MeshShowKey(temp_mac, 16);//pointer to cmac, offset 10
AES_CMAC(temp_mac, smnb, 4, mac);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "Received Beacon Mac = ", 0);
MeshShowKey((uint8_t*)pMeshBeaconSecNwk+ 10, 4);//pointer to cmac, offset 10
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "Calc Beacon Mac = ", 0);
MeshShowKey(mac, 16);
if(memcmp((uint8_t*)pMeshBeaconSecNwk+ 10, mac+12, 4) == 0)
{
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "MAC MATCH", 0);
}
else
{
DBG_BUFFER(MODULE_APP, LEVEL_ERROR, "MAC dis-match", 0);
}
break;
}
default:
break;
}
}
void MeshBeaconSecSend(uint8_t kr)
{
uint8_t encryption_key[16];
uint8_t temp_mac[16];
uint8_t mac[16];
uint8_t smnb[4] = {'s', 'm', 'n', 'b'};
uint8_t netID[16];
MeshEncryptionKeyGet(encryption_key);
MeshNetIDGet(netID);
uint16_t CIVI = MeshIVindexGet();
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "CIVI = 0x%x", 1, CIVI);
uint8_t kr_nid_civi[19];
memcpy(kr_nid_civi, &kr, 1);
memcpy(kr_nid_civi+1, netID, 16);
BE_WORD2EXTRN(kr_nid_civi+1+16, CIVI);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "kr_nid_civi = ", 0);
MeshShowKey(kr_nid_civi, 19);
AES_CMAC(encryption_key, kr_nid_civi, 19, temp_mac);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "Temp Mac = ", 0);
MeshShowKey(temp_mac, 16);//pointer to cmac, offset 10
AES_CMAC(temp_mac, smnb, 4, mac);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "Calc Beacon Mac = ", 0);
MeshShowKey(mac, 16);
uint8_t * pbuffer;
TMeshBeacon *pMeshBeacon;
pbuffer = MeshDataRamPoolBufferGet();
if (pbuffer == NULL)
{
return ;
}
pMeshBeacon = (TMeshBeacon *) (pbuffer + 2);
pMeshBeacon->header.service_uuid = MESH_SERVICE_UUID;
pMeshBeacon->beacon_type = BEACON_TYPE_SECNWK;
netID[8] &=0x7f;
if(kr)
{
netID[8] |= 0x80;
}
else
{
netID[8] &=0x7F;
}
memcpy(pMeshBeacon->p.sec_nwk.kr_nid, &netID[8], 8);
BE_WORD2EXTRN((uint8_t*)&pMeshBeacon->p.sec_nwk.civi, CIVI);
pMeshBeacon->p.sec_nwk.cmac = LE_EXTRN2DWORD(&mac[12]);
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "TX Beacon Data = ", 0);
MeshShowKey((uint8_t *)pMeshBeacon, 17);//pointer to cmac, offset 10
DBG_BUFFER(MODULE_APP, LEVEL_INFO, "MeshBeaconSecSend", 0);
MeshBearerSend(MESH_AD_TYPE_MESH_BEACON, pbuffer, 2, 17);
return;
}