rtmp_ckipmic.c

ralink 2870 usb无线网卡 最新驱动

    rcon = rcon_table[round];

    xor_32(&key[0], sbox_key, &key[0]);
    key[0] = key[0] ^ rcon;

    xor_32(&key[4], &key[0], &key[4]);
    xor_32(&key[8], &key[4], &key[8]);
    xor_32(&key[12], &key[8], &key[12]);
}

VOID byte_sub(
    IN  PUCHAR  in,
    OUT PUCHAR  out)
{
    INT i;

    for (i=0; i< 16; i++)
    {
        out[i] = RTMPCkipSbox(in[i]);
    }
}

VOID shift_row(
    IN  PUCHAR  in,
    OUT PUCHAR  out)
{
    out[0] =  in[0];
    out[1] =  in[5];
    out[2] =  in[10];
    out[3] =  in[15];
    out[4] =  in[4];
    out[5] =  in[9];
    out[6] =  in[14];
    out[7] =  in[3];
    out[8] =  in[8];
    out[9] =  in[13];
    out[10] = in[2];
    out[11] = in[7];
    out[12] = in[12];
    out[13] = in[1];
    out[14] = in[6];
    out[15] = in[11];
}

VOID mix_column(
    IN  PUCHAR  in,
    OUT PUCHAR  out)
{
    INT         i;
    UCHAR       add1b[4];
    UCHAR       add1bf7[4];
    UCHAR       rotl[4];
    UCHAR       swap_halfs[4];
    UCHAR       andf7[4];
    UCHAR       rotr[4];
    UCHAR       temp[4];
    UCHAR       tempb[4];

    for (i=0 ; i<4; i++)
    {
        if ((in[i] & 0x80)== 0x80)
            add1b[i] = 0x1b;
        else
            add1b[i] = 0x00;
    }

    swap_halfs[0] = in[2];    /* Swap halfs */
    swap_halfs[1] = in[3];
    swap_halfs[2] = in[0];
    swap_halfs[3] = in[1];

    rotl[0] = in[3];        /* Rotate left 8 bits */
    rotl[1] = in[0];
    rotl[2] = in[1];
    rotl[3] = in[2];

    andf7[0] = in[0] & 0x7f;
    andf7[1] = in[1] & 0x7f;
    andf7[2] = in[2] & 0x7f;
    andf7[3] = in[3] & 0x7f;

    for (i = 3; i>0; i--)    /* logical shift left 1 bit */
    {
        andf7[i] = andf7[i] << 1;
        if ((andf7[i-1] & 0x80) == 0x80)
        {
            andf7[i] = (andf7[i] | 0x01);
        }
    }
    andf7[0] = andf7[0] << 1;
    andf7[0] = andf7[0] & 0xfe;

    xor_32(add1b, andf7, add1bf7);

    xor_32(in, add1bf7, rotr);

    temp[0] = rotr[0];         /* Rotate right 8 bits */
    rotr[0] = rotr[1];
    rotr[1] = rotr[2];
    rotr[2] = rotr[3];
    rotr[3] = temp[0];

    xor_32(add1bf7, rotr, temp);
    xor_32(swap_halfs, rotl,tempb);
    xor_32(temp, tempb, out);
}

VOID RTMPAesEncrypt(
    IN  PUCHAR  key,
    IN  PUCHAR  data,
    IN  PUCHAR  ciphertext)
{
    INT             round;
    INT             i;
    UCHAR           intermediatea[16];
    UCHAR           intermediateb[16];
    UCHAR           round_key[16];

    for(i=0; i<16; i++) round_key[i] = key[i];

    for (round = 0; round < 11; round++)
    {
        if (round == 0)
        {
            xor_128(round_key, data, ciphertext);
            next_key(round_key, round); 
        }
        else if (round == 10)
        {
            byte_sub(ciphertext, intermediatea);
            shift_row(intermediatea, intermediateb);
            xor_128(intermediateb, round_key, ciphertext);
        }
        else    /* 1 - 9 */
        {
            byte_sub(ciphertext, intermediatea);
            shift_row(intermediatea, intermediateb);
            mix_column(&intermediateb[0], &intermediatea[0]);
            mix_column(&intermediateb[4], &intermediatea[4]);
            mix_column(&intermediateb[8], &intermediatea[8]);
            mix_column(&intermediateb[12], &intermediatea[12]);
            xor_128(intermediatea, round_key, ciphertext);
            next_key(round_key, round);
        }
    }

}

/* calculate the mic */
VOID RTMPMicFinal(
    IN  PMIC_CONTEXT    pContext,
    OUT UCHAR           digest[4])
{
    INT             byte_position;
    ULONG           val;
    ULONGLONG       sum, utmp;
    LONGLONG        stmp;

    /* deal with partial 32-bit word left over from last update */
    if ( (byte_position = (pContext->position & 3)) != 0) {
        /* have a partial word in part to deal with -- zero unused bytes */
        do {
            pContext->part[byte_position++] = 0;
            pContext->position++;
        } while (byte_position < 4);
        val = GETBIG32(&pContext->part[0]);
        MIC_ACCUM(val);
    }

    /* reduce the accumulated u64 to a 32-bit MIC */
    sum = pContext->accum;
    stmp = (sum  & 0xffffffffL) - ((sum >> 32)  * 15);
    utmp = (stmp & 0xffffffffL) - ((stmp >> 32) * 15);
    sum = utmp & 0xffffffffL;
    if (utmp > 0x10000000fL)
        sum -= 15;

    val = (ULONG)sum;
    digest[0] = (UCHAR)((val>>24) & 0xFF);
    digest[1] = (UCHAR) ((val>>16) & 0xFF);
    digest[2] = (UCHAR) ((val>>8) & 0xFF);
    digest[3] = (UCHAR)((val>>0) & 0xFF);
}

VOID RTMPCkipInsertCMIC(
    IN  PRTMP_ADAPTER   pAd,
    OUT PUCHAR          pMIC,
    IN  PUCHAR          p80211hdr,
    IN  PNDIS_PACKET    pPacket,
    IN  PCIPHER_KEY     pKey,
    IN  PUCHAR          mic_snap)
{
	PACKET_INFO		PacketInfo;
	PUCHAR			pSrcBufVA;
	ULONG			SrcBufLen;
    PUCHAR          pDA, pSA, pProto;
    UCHAR           bigethlen[2];
	UCHAR			ckip_ck[16];
    MIC_CONTEXT     mic_ctx;
    USHORT          payloadlen;
	UCHAR			i;

	if (pKey == NULL)
	{
		DBGPRINT_ERR(("RTMPCkipInsertCMIC, Before to form the CKIP key (CK), pKey can't be NULL\n"));
		return;
	}

    switch (*(p80211hdr+1) & 3)
    {
        case 0: /* FromDs=0, ToDs=0 */
            pDA = p80211hdr+4;
            pSA = p80211hdr+10;
            break;
        case 1: /* FromDs=0, ToDs=1 */
            pDA = p80211hdr+16;
            pSA = p80211hdr+10;
            break;
        case 2: /* FromDs=1, ToDs=0 */
            pDA = p80211hdr+4;
            pSA = p80211hdr+16;
            break;
        case 3: /* FromDs=1, ToDs=1 */
            pDA = p80211hdr+16;
            pSA = p80211hdr+24;
            break;
    }

	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);

    if (SrcBufLen < LENGTH_802_3)
        return;

    pProto = pSrcBufVA + 12;
    payloadlen = PacketInfo.TotalPacketLength - LENGTH_802_3 + 18; // CKIP_LLC(8)+CMIC(4)+TxSEQ(4)+PROTO(2)=18
    
    bigethlen[0] = (unsigned char)(payloadlen >> 8);
    bigethlen[1] = (unsigned char)payloadlen;

	//
	// Encryption Key expansion to form the CKIP Key (CKIP_CK).
	//
	if (pKey->KeyLen < 16)
	{
		for(i = 0; i < (16 / pKey->KeyLen); i++)
		{
			NdisMoveMemory(ckip_ck + i * pKey->KeyLen, 
							pKey->Key, 
							pKey->KeyLen);			
		}
		NdisMoveMemory(ckip_ck + i * pKey->KeyLen,
						pKey->Key,
						16 - (i * pKey->KeyLen));
	}
	else
	{
		NdisMoveMemory(ckip_ck, pKey->Key, pKey->KeyLen);
	}	
    RTMPCkipMicInit(&mic_ctx, ckip_ck);
    RTMPMicUpdate(&mic_ctx, pDA, MAC_ADDR_LEN);            // MIC <-- DA
    RTMPMicUpdate(&mic_ctx, pSA, MAC_ADDR_LEN);            // MIC <-- SA
    RTMPMicUpdate(&mic_ctx, bigethlen, 2);                 // MIC <-- payload length starting from CKIP SNAP
    RTMPMicUpdate(&mic_ctx, mic_snap, 8);                  // MIC <-- snap header 
    RTMPMicUpdate(&mic_ctx, pAd->StaCfg.TxSEQ, 4);   // MIC <-- TxSEQ
    RTMPMicUpdate(&mic_ctx, pProto, 2);                    // MIC <-- Protocol

    pSrcBufVA += LENGTH_802_3;
    SrcBufLen -= LENGTH_802_3;

    // Mic <-- original payload. loop until all payload processed
    do
    {
        if (SrcBufLen > 0)
            RTMPMicUpdate(&mic_ctx, pSrcBufVA, SrcBufLen); 

		NdisGetNextBuffer(PacketInfo.pFirstBuffer, &PacketInfo.pFirstBuffer);
        if (PacketInfo.pFirstBuffer)
        {
            NDIS_QUERY_BUFFER(PacketInfo.pFirstBuffer, &pSrcBufVA, &SrcBufLen);
        }
        else
            break;
    } while (TRUE);
    
    RTMPMicFinal(&mic_ctx, pMIC);                          // update MIC
}