wt_stmgt.c

mini-PCI driver for LHWT chipsets

#include "wtprecomp.h"

int WtWlan_is_mac_address_zero(mac_t * mac)
{
	if ((mac->vala == 0) && (mac->valb == 0)) {
		return 1;
	}
	return 0;
}

void WtWlan_clear_mac_address(mac_t * m)
{
	m->vala = 0;
	m->valb = 0;
}


int WtWlan_is_mac_address_equal(UINT8 * one, UINT8 * two)
{
	if ( memcmp(one, two, WLAN_ADDR_LEN))
		return 0; /* no match */
	else
		return 1; /* matched */
}


void WtWlan_copy_mac_address(UINT8 *to, const UINT8 * const from)
{
	memcpy(to, from, ETH_ALEN);
}


UINT8 WtWlan_is_mac_address_group(mac_t * mac)
{
	return mac->vala & 1;
}


UINT8 WtWlan_is_mac_address_directed(mac_t * mac)
{
	if (mac->vala & 1) {
		return 0;
	}
	return 1;
}


void WtWlan_set_mac_address_broadcast(UINT8 *address)
{
	memset(address, 0xff, ETH_ALEN);
}


int WtWlan_is_mac_address_broadcast(const UINT8 * const address)
{
	static const unsigned char bcast[ETH_ALEN] =
		{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
	return !memcmp(address, bcast, ETH_ALEN);
}

int WtWlan_is_mac_address_multicast(mac_t * mac)
{
	if (mac->vala & 1) {
		if ((mac->vala == 0xffffffff) && (mac->valb == 0xffff))
			return 0;
		else
			return 1;
	}
	return 0;
}

void WtWlan_log_mac_address(unsigned int level, UINT8 * mac)
{
	printk("%02X.%02X.%02X.%02X.%02X.%02X",
	       mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
}



//-------------------------------------privacy---------------------------------------------
//mic check 


void block_function(
                    unsigned long int l,
                    unsigned long int r,
                    unsigned long int *l_out,
                    unsigned long int *r_out
                    );

void michael(IN  UCHAR *key,
                      IN  UCHAR *message,
                      IN  int num_blocks,
                      IN  UCHAR *mic       );

UCHAR getMicPriority( IN PWTWLAN_TXBUF pWtTxBuf );
BOOL MICCheck(IN PWT_ADAPTER Adapter, IN PWTWLAN_RXBUF pWtRxBuf);
BOOL AddMic(IN PWT_ADAPTER Adapter, IN PWTWLAN_TXBUF pWtTxBuf);

//________________________________________________________________

unsigned long int rotr(int bits, unsigned long int a)
{
    unsigned long int c,d,e,f,g;
	
    c = (0x0001 << bits)-1;
    d = ~c;

    e = (a & d) >> bits;
    f = (a & c) << (32 - bits);

    g = e | f;

    return (g & 0xffffffff );

}

unsigned long int rotl(int bits, unsigned long int a)
{
    unsigned long int c,d,e,f,g;
	
    c = (0x0001 << (32-bits))-1;
    d = ~c;

    e = (a & c) << bits;
    f = (a & d) >> (32 - bits);

    g = e | f;

    return (g & 0xffffffff );

}

unsigned long int xswap(unsigned long int in)
{
    unsigned long int a,b,c,d,out;

    a = in & 0xff;
    b = (in >> 8) & 0xff;
    c = (in >> 16) & 0xff;
    d = (in >> 24) & 0xff;

    out = 0;
    out |= c << 24;
    out |= d << 16;
    out |= a << 8;
    out |= b;

    return out;
}
void block_function(
                    unsigned long int l,
                    unsigned long int r,
                    unsigned long int *l_out,
                    unsigned long int *r_out
                    )

{

    r = r ^ rotl(17,l);
    l = (l + r) & 0xffffffff;
    r = r ^ xswap(l);
    l = (l + r) & 0xffffffff;
    r = r ^ rotl(3,l);
    l = (l + r) & 0xffffffff;
    r = r ^ rotr(2,l);
    l = (l + r) & 0xffffffff;

    *l_out = l;
    *r_out = r;
}

void michael(IN  UCHAR *key,
                      IN  UCHAR *message,
                      IN  int num_blocks,
                      IN  UCHAR *mic       )
{
    unsigned long int l,r;
    unsigned long int l_out, r_out;
    unsigned long int k0,k1;
    unsigned long int m;
    unsigned char a,b,c,d;

//    int num_blocks;
    int block;

//    num_blocks = message_length / 4;

    k0 = key[0] + (key[1] * 256) + (key[2] * 256*256) + (key[3] * 256*256*256);
    k1 = key[4] + (key[5] * 256) + (key[6] * 256*256) + (key[7] * 256*256*256);
    
    l = k0;
    r = k1;

    for (block = 0; block < num_blocks; block++)
    {
        m = 0;
        a = message[block*4];
        b = message[1+(block*4)];
        c = message[2+(block*4)];
        d = message[3+(block*4)];

        m = a + (b * 256) + (c * 256*256) + (d * 256*256*256);
        l = l ^ m;
        block_function(l,r,&l_out,&r_out);
        l = l_out;
        r = r_out;
    }

    mic[0] = (UCHAR)(l & 0xff);
    mic[1] = (UCHAR)((l >> 8) & 0xff);
    mic[2] = (UCHAR)((l >> 16) & 0xff);
    mic[3] = (UCHAR)((l >> 24) & 0xff);
    mic[4] = (UCHAR)(r & 0xff);
    mic[5] = (UCHAR)((r >> 8) & 0xff);
    mic[6] = (UCHAR)((r >> 16) & 0xff);
    mic[7] = (UCHAR)((r >> 24) & 0xff);
}

BOOL MICCheck(IN PWT_ADAPTER Adapter, IN PWTWLAN_RXBUF pWtRxBuf)
/*check the MIC*/
{
	  p80211_hdr_t *w_hdr;
        UCHAR mic[8];
        UCHAR *premic;
		
        int message_length;
        UCHAR *message;
        UCHAR *daddr;
        UCHAR *saddr;
        UCHAR priority;
        UCHAR *key;
        PWTWLAN_KEY wlankey;

        int length;
        int num_blocks;
        int remainder;

        UCHAR header_bak[16];
		
        int i;

       memset(mic,0,8);
	   
       message_length = pWtRxBuf->RXHwBuf.pHwRxBuf->RxStatus.RecvBodyLen;

/* prepare da, sa, priority*/
/*Get the SA of pWtRxBuf */
	w_hdr = 	(p80211_hdr_t*)&pWtRxBuf->RXHwBuf.pHwRxBuf->dot11Head;  //__________zxx?
                
	if ((WLAN_GET_FC_TODS(pWtRxBuf->FrmCtl) == 0) && (WLAN_GET_FC_FROMDS(pWtRxBuf->FrmCtl) == 0)) {
             	daddr = w_hdr->a3.a1;
             	saddr = w_hdr->a3.a2;
 	} else if ((WLAN_GET_FC_TODS(pWtRxBuf->FrmCtl) == 0) && (WLAN_GET_FC_FROMDS(pWtRxBuf->FrmCtl) == 1)) {
             	daddr = w_hdr->a3.a1;
                	saddr = w_hdr->a3.a3;
  	} else if ((WLAN_GET_FC_TODS(pWtRxBuf->FrmCtl) == 1) && (WLAN_GET_FC_FROMDS(pWtRxBuf->FrmCtl) == 0)) {
                	daddr = w_hdr->a3.a3;
                	saddr = w_hdr->a3.a2;
    	} else {
                	daddr = w_hdr->a4.a3;
                	saddr = w_hdr->a4.a4;
 	}

/* prepare the priority*/
        priority = 0x00;          //____________________________zxx? 

/* prepare the message, num_blocks*/
        length = message_length + 16 + 5; /* da + sa + priority + 000 + 5a + 0000= 21 */
        num_blocks = length / 4;
        remainder = length % 4;
	
        #ifdef WIN_PLATFORM
        NdisMoveMemory(header_bak, pWtRxBuf->RXHwBuf.pHwRxBuf->dot11Head+MAX_HW_HEAD_LEN-16, 16);
	#else
	memmove(header_bak, pWtRxBuf->RXHwBuf.pHwRxBuf->dot11Head+MAX_HW_HEAD_LEN-16, 16);
	#endif
	
        message = pWtRxBuf->RXHwBuf.pHwRxBuf->dot11Head+MAX_HW_HEAD_LEN-16;

 

        for (i=0; i<6; i++)                 /* Insert addresses */
        {
            message[i] = daddr[i];
            message[i+6] = saddr[i];
        }
        message[12] = priority;    /* Insert priority */
        message[13] = 0x00;
        message[14] = 0x00;
        message[15] = 0x00;

        message[message_length+16] = 0x5a;   /* Insert padding */
        message[message_length+17] = 0x00;
        message[message_length+18] = 0x00;
        message[message_length+19] = 0x00;
        message[message_length+20] = 0x00;
        message[message_length+21] = 0x00;
        message[message_length+22] = 0x00;
        message[message_length+23] = 0x00;

        if (remainder != 0) num_blocks++;

/* prepare the KEY*/
       wlankey =  WtWlanRxGetDecryptKey(Adapter, pWtRxBuf->RXHwBuf.pHwRxBuf );
       if(wlankey == NULL)
	   	return FALSE;
	   
       key = wlankey->KeyTkip.TkipMic;

//PWTWLAN_KEY WtWlanTxGetEncryptKey( IN PWT_ADAPTER Adapter, IN PWTWLAN_HWTXBUF pHwTxBuf );

		
/* recompute the MIC*/
        michael(key, message, num_blocks, mic);

	#ifdef WIN_PLATFORM
        NdisMoveMemory(pWtRxBuf->RXHwBuf.pHwRxBuf->dot11Head+MAX_HW_HEAD_LEN-16,header_bak , 16);
	#else
	memmove(pWtRxBuf->RXHwBuf.pHwRxBuf->dot11Head+MAX_HW_HEAD_LEN-16,header_bak , 16);
	#endif

/*compare the MIC*/	
        premic = pWtRxBuf->RXHwBuf.pHwRxBuf->Buffer 
                    + pWtRxBuf->RXHwBuf.pHwRxBuf->RxStatus.RecvBodyLen - 8;
        if(*mic == *premic )
			return TRUE;
        else
			return FALSE;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////
UCHAR getMicPriority( IN PWTWLAN_TXBUF pWtTxBuf )
{
#if 0
        UCHAR priority= AC_BE;

	priority = (IEEE8021PPRIORITY) NDIS_PER_PACKET_INFO_FROM_PACKET(pWtTxBuf->NdisPacket, Ieee8021pPriority);

	return priority;
#endif
        return 0; 

}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL AddMic(IN PWT_ADAPTER Adapter, IN PWTWLAN_TXBUF pWtTxBuf)
/*
      if Mic is added successful, ruturn true;
      else return false;
*/	
{
	  p80211_hdr_t *w_hdr;
        UCHAR mic[8];
        //UCHAR *premic; //who used it? wl
		
        int message_length;
        UCHAR *message;
        UCHAR *daddr;
        UCHAR *saddr;
        UCHAR priority;
        UCHAR *key;
        PWTWLAN_KEY wlankey;

        int length;
        int num_blocks;
        int remainder;
		
        UCHAR header_bak[16];
	  UCHAR  *pDest;   
        int i;

//       mic=NULL;
       memset(mic,0,8);
       message_length = ieee2host16( pWtTxBuf->TXHwBuf.pHwTxBuf->TxAUXBuf.DataLen );

/* prepare da, sa, priority*/ 
/*Get the SA of pWtRxBuf */
	w_hdr = 	(p80211_hdr_t*)&pWtTxBuf->TXHwBuf.pHwTxBuf->dot11Head;  

  	if ((WLAN_GET_FC_TODS(ieee2host16(w_hdr->a3.fc)) == 0) && (WLAN_GET_FC_FROMDS(ieee2host16(w_hdr->a3.fc)) == 0)) {
             	daddr = w_hdr->a3.a1;
             	saddr = w_hdr->a3.a2;
 	} else if ((WLAN_GET_FC_TODS(ieee2host16(w_hdr->a3.fc)) == 0) && (WLAN_GET_FC_FROMDS(ieee2host16(w_hdr->a3.fc)) == 1)) {
             	daddr = w_hdr->a3.a1;
                	saddr = w_hdr->a3.a3;
  	} else if ((WLAN_GET_FC_TODS(ieee2host16(w_hdr->a3.fc)) == 1) && (WLAN_GET_FC_FROMDS(ieee2host16(w_hdr->a3.fc)) == 0)) {
                	daddr = w_hdr->a3.a3;
                	saddr = w_hdr->a3.a2;
    	} else {
                	daddr = w_hdr->a4.a3;
                	saddr = w_hdr->a4.a4;
 	}
/* prepare the priority */
        priority = getMicPriority(pWtTxBuf);          //____________________________zxx? 

/* prepare the message, num_blocks*/
        length = message_length + 16 + 5; /* da + sa + priority + 000 + 5a + 0000= 21 */
        num_blocks = length / 4;
        remainder = length % 4;

        //NdisMoveMemory(header_bak, pWtTxBuf->TXHwBuf.pHwTxBuf->dot11Head+MAX_HW_HEAD_LEN-16, 16);
	memmove(header_bak, pWtTxBuf->TXHwBuf.pHwTxBuf->dot11Head+MAX_HW_HEAD_LEN-16, 16);
	
        message = pWtTxBuf->TXHwBuf.pHwTxBuf->dot11Head+MAX_HW_HEAD_LEN-16;
		
 

        for (i=0; i<6; i++)                 /* Insert addresses */
        {
            message[i] = daddr[i];
            message[i+6] = saddr[i];
        }
        message[12] = priority;    /* Insert priority */
        message[13] = 0x00;
        message[14] = 0x00;
        message[15] = 0x00;

        message[message_length+16] = 0x5a;   /* Insert padding */
        message[message_length+17] = 0x00;
        message[message_length+18] = 0x00;
        message[message_length+19] = 0x00;
        message[message_length+20] = 0x00;
        message[message_length+21] = 0x00;
        message[message_length+22] = 0x00;
        message[message_length+23] = 0x00;

        if (remainder != 0) num_blocks++;

/* prepare the KEY*/
       wlankey =  WtWlanTxGetEncryptKey(Adapter, pWtTxBuf->TXHwBuf.pHwTxBuf );
       if(wlankey == NULL)
	   	return FALSE;
	   
       key = wlankey->KeyTkip.TkipMic;
		
/* recompute the MIC*/
        michael(key, message, num_blocks, mic);

/* add the MIC to WtTxBuf*/
      pDest = pWtTxBuf->TXHwBuf.pHwTxBuf->Buffer + ieee2host16( pWtTxBuf->TXHwBuf.pHwTxBuf->TxAUXBuf.DataLen );
        #ifdef WIN_PLATFORM
	NdisMoveMemory(pDest, mic, 8);
	#else
	memmove(pDest, mic, 8);
	#endif
       pWtTxBuf->TXHwBuf.pHwTxBuf->TxAUXBuf.DataLen = host2ieee16( ieee2host16( pWtTxBuf->TXHwBuf.pHwTxBuf->TxAUXBuf.DataLen ) + 8) ;
       #ifdef WIN_PLATFORM
        NdisMoveMemory(pWtTxBuf->TXHwBuf.pHwTxBuf->dot11Head+MAX_HW_HEAD_LEN-16, header_bak , 16);
	#else
	memmove(pWtTxBuf->TXHwBuf.pHwTxBuf->dot11Head+MAX_HW_HEAD_LEN-16, header_bak , 16);
	#endif

	return TRUE;	  
	
}

//-------------------------------------privacy end---------------------------------------------

//__________________________________________QoS______________________
PWTWLAN_TXBUF  Backoff_Pre(IN PWT_ADAPTER Adapter)
{
	PWTWLAN_TXBUF pWtTxBuf = NULL;

	//PQUEUE_ENTRY    pEntry;

      int cw=7;//_____________________________________?
//   	static int cnt;
      //static int slotCnt;
//      int index;
//      static Duration  dSlot  =  aSlotTime ;
//      Duration AIFS[AC_MAX];

      int AIFSN[AC_MAX];
      static int RemainTime[AC_MAX];
      int MaxRemainTime;