?? rtmp_tkip.c
字號:
{
b = (a >> 1) | 0x8000;
}
else
{
b = (a >> 1) & 0x7fff;
}
b = b % 65536;
return b;
}
/*
========================================================================
Routine Description:
Key Mixing function.
Arguments:
pAdapter Pointer to our adapter
pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
pTA Pointer to transmitter address
nl Least significant 16 bits of IV16
nh Most significant 32 bits of IV32
Return Value:
None
Note:
Calculates an RC4 key.
========================================================================
*/
VOID RTMPTkipMixTKey(
IN PTKIP_KEY_INFO pTkip,
IN PUCHAR pTKey,
IN PUCHAR pTA)
{
UINT tsc0;
UINT tsc1;
UINT tsc2;
UINT ppk0;
UINT ppk1;
UINT ppk2;
UINT ppk3;
UINT ppk4;
UINT ppk5;
UINT p1k[5];
int i;
int j;
tsc0 = (UINT)((pTkip->IV32 >> 16) % 65536); /* msb */
tsc1 = (UINT)(pTkip->IV32 % 65536);
tsc2 = (UINT)(pTkip->IV16 % 65536); /* lsb */
/* Phase 1, step 1 */
p1k[0] = tsc1;
p1k[1] = tsc0;
p1k[2] = (unsigned int)(pTA[0] + (pTA[1]*256));
p1k[3] = (unsigned int)(pTA[2] + (pTA[3]*256));
p1k[4] = (unsigned int)(pTA[4] + (pTA[5]*256));
/* Phase 1, step 2 */
for (i=0; i<8; i++)
{
j = 2*(i & 1);
p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*pTKey[1+j]) + pTKey[j])) % 65536 )) % 65536;
p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*pTKey[5+j]) + pTKey[4+j])) % 65536 )) % 65536;
p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*pTKey[9+j]) + pTKey[8+j])) % 65536 )) % 65536;
p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*pTKey[13+j]) + pTKey[12+j])) % 65536 )) % 65536;
p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*pTKey[1+j]) + pTKey[j]))) % 65536 )) % 65536;
p1k[4] = (p1k[4] + i) % 65536;
}
/* Phase 2, Step 1 */
ppk0 = p1k[0];
ppk1 = p1k[1];
ppk2 = p1k[2];
ppk3 = p1k[3];
ppk4 = p1k[4];
ppk5 = (p1k[4] + tsc2) % 65536;
/* Phase2, Step 2 */
ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*pTKey[1]) + pTKey[0])) % 65536);
ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*pTKey[3]) + pTKey[2])) % 65536);
ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*pTKey[5]) + pTKey[4])) % 65536);
ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*pTKey[7]) + pTKey[6])) % 65536);
ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*pTKey[9]) + pTKey[8])) % 65536);
ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*pTKey[11]) + pTKey[10])) % 65536);
ppk0 = ppk0 + rotr1(ppk5 ^ ((256*pTKey[13]) + pTKey[12]));
ppk1 = ppk1 + rotr1(ppk0 ^ ((256*pTKey[15]) + pTKey[14]));
ppk2 = ppk2 + rotr1(ppk1);
ppk3 = ppk3 + rotr1(ppk2);
ppk4 = ppk4 + rotr1(ppk3);
ppk5 = ppk5 + rotr1(ppk4);
/* Phase 2, Step 3 */
pTkip->RC4KEY[0] = (tsc2 >> 8) % 256;
pTkip->RC4KEY[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
pTkip->RC4KEY[2] = tsc2 % 256;
pTkip->RC4KEY[3] = ((ppk5 ^ ((256*pTKey[1]) + pTKey[0])) >> 1) % 256;
pTkip->RC4KEY[4] = ppk0 % 256;
pTkip->RC4KEY[5] = (ppk0 >> 8) % 256;
pTkip->RC4KEY[6] = ppk1 % 256;
pTkip->RC4KEY[7] = (ppk1 >> 8) % 256;
pTkip->RC4KEY[8] = ppk2 % 256;
pTkip->RC4KEY[9] = (ppk2 >> 8) % 256;
pTkip->RC4KEY[10] = ppk3 % 256;
pTkip->RC4KEY[11] = (ppk3 >> 8) % 256;
pTkip->RC4KEY[12] = ppk4 % 256;
pTkip->RC4KEY[13] = (ppk4 >> 8) % 256;
pTkip->RC4KEY[14] = ppk5 % 256;
pTkip->RC4KEY[15] = (ppk5 >> 8) % 256;
}
VOID RTMPTkipMixKey(
UCHAR *key,
UCHAR *ta,
ULONG pnl, /* Least significant 16 bits of PN */
ULONG pnh, /* Most significant 32 bits of PN */
UCHAR *rc4key,
UINT *p1k)
{
UINT tsc0;
UINT tsc1;
UINT tsc2;
UINT ppk0;
UINT ppk1;
UINT ppk2;
UINT ppk3;
UINT ppk4;
UINT ppk5;
INT i;
INT j;
tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
tsc1 = (unsigned int)(pnh % 65536);
tsc2 = (unsigned int)(pnl % 65536); /* lsb */
/* Phase 1, step 1 */
p1k[0] = tsc1;
p1k[1] = tsc0;
p1k[2] = (UINT)(ta[0] + (ta[1]*256));
p1k[3] = (UINT)(ta[2] + (ta[3]*256));
p1k[4] = (UINT)(ta[4] + (ta[5]*256));
/* Phase 1, step 2 */
for (i=0; i<8; i++)
{
j = 2*(i & 1);
p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
p1k[4] = (p1k[4] + i) % 65536;
}
/* Phase 2, Step 1 */
ppk0 = p1k[0];
ppk1 = p1k[1];
ppk2 = p1k[2];
ppk3 = p1k[3];
ppk4 = p1k[4];
ppk5 = (p1k[4] + tsc2) % 65536;
/* Phase2, Step 2 */
ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
ppk2 = ppk2 + rotr1(ppk1);
ppk3 = ppk3 + rotr1(ppk2);
ppk4 = ppk4 + rotr1(ppk3);
ppk5 = ppk5 + rotr1(ppk4);
/* Phase 2, Step 3 */
/* Phase 2, Step 3 */
tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
tsc1 = (unsigned int)(pnh % 65536);
tsc2 = (unsigned int)(pnl % 65536); /* lsb */
rc4key[0] = (tsc2 >> 8) % 256;
rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
rc4key[2] = tsc2 % 256;
rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
rc4key[4] = ppk0 % 256;
rc4key[5] = (ppk0 >> 8) % 256;
rc4key[6] = ppk1 % 256;
rc4key[7] = (ppk1 >> 8) % 256;
rc4key[8] = ppk2 % 256;
rc4key[9] = (ppk2 >> 8) % 256;
rc4key[10] = ppk3 % 256;
rc4key[11] = (ppk3 >> 8) % 256;
rc4key[12] = ppk4 % 256;
rc4key[13] = (ppk4 >> 8) % 256;
rc4key[14] = ppk5 % 256;
rc4key[15] = (ppk5 >> 8) % 256;
}
//
// TRUE: Success!
// FALSE: Decrypt Error!
//
BOOLEAN RTMPSoftDecryptTKIP(
IN PRT2570ADAPTER pAdapter,
IN PUCHAR pData,
IN ULONG DataByteCnt,
IN PWPA_KEY pWpaKey)
{
PHEADER_802_11 pHeader;
UCHAR KeyID;
UINT HeaderLen;
UCHAR fc0;
UCHAR fc1;
USHORT fc;
UINT frame_type;
UINT frame_subtype;
UINT from_ds;
UINT to_ds;
INT a4_exists;
INT qc_exists;
USHORT duration;
USHORT seq_control;
USHORT qos_control;
UCHAR TA[MAC_ADDR_LEN];
UCHAR DA[MAC_ADDR_LEN];
UCHAR SA[MAC_ADDR_LEN];
UCHAR RC4Key[16];
UINT p1k[5]; //for mix_key;
ULONG pnl;/* Least significant 16 bits of PN */
ULONG pnh;/* Most significant 32 bits of PN */
UINT num_blocks;
UINT payload_remainder;
ARCFOURCONTEXT ArcFourContext;
ULONG crc32 = 0;
ULONG trailfcs = 0;
UCHAR MIC[8];
UCHAR TrailMIC[8];
fc0 = *pData;
fc1 = *(pData + 1);
fc = *((PUSHORT)pData);
frame_type = ((fc0 >> 2) & 0x03);
frame_subtype = ((fc0 >> 4) & 0x0f);
from_ds = (fc1 & 0x2) >> 1;
to_ds = (fc1 & 0x1);
a4_exists = (from_ds & to_ds);
qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
(frame_subtype == 0x09) || /* Likely to change. */
(frame_subtype == 0x0a) ||
(frame_subtype == 0x0b)
);
HeaderLen = 24;
if (a4_exists)
HeaderLen += 6;
KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
KeyID = KeyID >> 6;
if (pWpaKey[KeyID].KeyLen == 0)
{
DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID);
return FALSE;
}
duration = *((PUSHORT)(pData+2));
seq_control = *((PUSHORT)(pData+22));
if (qc_exists)
{
if (a4_exists)
{
qos_control = *((PUSHORT)(pData+30));
}
else
{
qos_control = *((PUSHORT)(pData+24));
}
}
if (to_ds == 0 && from_ds == 1)
{
NdisMoveMemory(&DA, pData+4, MAC_ADDR_LEN);
NdisMoveMemory(&SA, pData+16, MAC_ADDR_LEN);
NdisMoveMemory(&TA, pData+10, MAC_ADDR_LEN); //BSSID
}
else if (to_ds == 0 && from_ds == 0 )
{
NdisMoveMemory(&TA, pData+10, MAC_ADDR_LEN);
NdisMoveMemory(&DA, pData+4, MAC_ADDR_LEN);
NdisMoveMemory(&SA, pData+10, MAC_ADDR_LEN);
}
else if (to_ds == 1 && from_ds == 0)
{
NdisMoveMemory(&SA, pData+10, MAC_ADDR_LEN);
NdisMoveMemory(&TA, pData+10, MAC_ADDR_LEN);
NdisMoveMemory(&DA, pData+16, MAC_ADDR_LEN);
}
else if (to_ds == 1 && from_ds == 1)
{
NdisMoveMemory(&TA, pData+10, MAC_ADDR_LEN);
NdisMoveMemory(&DA, pData+16, MAC_ADDR_LEN);
NdisMoveMemory(&SA, pData+22, MAC_ADDR_LEN);
}
num_blocks = (DataByteCnt - 16) / 16;
payload_remainder = (DataByteCnt - 16) % 16;
pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
pnh = *((PULONG)(pData + HeaderLen + 4));
RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4); //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
crc32 ^= 0xffffffff; /* complement */
if(crc32 != trailfcs)
{
DBGPRINT_RAW(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"); //ICV error.
return (FALSE);
}
NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
RTMPInitMICEngine(pAdapter, pWpaKey[KeyID].Key, DA, SA, pWpaKey[KeyID].RxMic);
RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
RTMPTkipGetMIC(&pAdapter->PrivateInfo.Tx);
NdisMoveMemory(MIC, pAdapter->PrivateInfo.Tx.MIC, 8);
if (!NdisEqualMemory(MIC, TrailMIC, 8))
{
DBGPRINT_RAW(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"); //MIC error.
RTMPReportMicError(pAdapter, &pWpaKey[KeyID]);
return (FALSE);
}
pHeader = (PHEADER_802_11)pData;
pHeader->Controlhead.Frame.Wep = 0; //None WEP
DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
return TRUE;
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -