wmm.c

marvell8385 GSPI开发驱动

#include "precomp.h"
#include "pkfuncs.h"

static UCHAR  wmm_tos2ac[16][8] = {
	{	// 0 0 0 0	all enabled
		AC_PRIO_BE,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BE,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VO,
		AC_PRIO_VO
	},
	{	// 0 0 0 1	AC_PRIO_BE disabled
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VO,
		AC_PRIO_VO
	},
	{	// 0 0 1 0	AC_PRIO_BK should NOT be disabled
		AC_PRIO_BE,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BE,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VO,
		AC_PRIO_VO
	},
	{	// 0 0 1 1	AC_PRIO_BE disabled
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VO,
		AC_PRIO_VO
	},
	{	// 0 1 0 0	AC_PRIO_VI disabled
		AC_PRIO_BE,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_VO,
		AC_PRIO_VO
	},
	{	// 0 1 0 1	AC_PRIO_VI & BE	disabled
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_VO,
		AC_PRIO_VO
	},
	{	// 0 1 1 0	AC_PRIO_VI disabled
		AC_PRIO_BE,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_VO,
		AC_PRIO_VO
	},
	{	// 0 1 1 1	AC_PRIO_VI & BE	disabled
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_VO,
		AC_PRIO_VO
	},
	{	// 1 0 0 0	AC_PRIO_VO disabled
		AC_PRIO_BE,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BE,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VI
	},
	{	// 1 0 0 1	AC_PRIO_VO & BE disabled
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VI
	},
	{	// 1 0 1 0	AC_PRIO_VO disabled
		AC_PRIO_BE,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BE,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VI
	},
	{	// 1 0 1 1	AC_PRIO_VO & BE disabled
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VI,
		AC_PRIO_VI
	},
	{	// 1 1 0 0	AC_PRIO_VO & VI disabled
		AC_PRIO_BE,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_BE
	},
	{	// 1 1 0 1	AC_PRIO_VO & VI & BE disabled
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK
	},
	{	// 1 1 1 0	AC_PRIO_VO & VI disabled
		AC_PRIO_BE,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_BE,
		AC_PRIO_BE
	},
	{	// 1 1 1 1	AC_PRIO_VO & VI & BE disabled
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK,
		AC_PRIO_BK
	}
};

static UCHAR wmm_tos2priority[8] = {
/*	Priority   DSCP   DSCP   DSCP	WMM
		    P2     P1     P0 	AC    */
	0x00,	/*  0	   0	  0	AC_BE */
	0x04,	/*  0	   0	  1	AC_BK */
	0x02,	/*  0	   1	  0	AC_BK */
	0x06,	/*  0	   1	  1	AC_BE */
	0x01,	/*  1	   0	  0	AC_VI */
	0x05,	/*  1	   0	  1	AC_VI */
	0x03,	/*  1	   1	  0	AC_VO */
	0x07	/*  1	   1	  1	AC_VO */
};


int wlan_wmm_enable_ioctl(PMRVDRV_ADAPTER Adapter, PVOID InformationBuffer)
{
    NDIS_STATUS     Status;
    PNDIS_PACKET    pPacket;
    POID_MRVL_DS_WMM_STATE pWmmState;

    pWmmState = (POID_MRVL_DS_WMM_STATE)InformationBuffer;

//    DBGPRINT(DBG_WMM, ("wlan_wmm_enable_ioctl state %x \n", pWmmState->State));
//    DBGPRINT(DBG_WMM, ("wmm state size %d \n", sizeof(POID_MRVL_DS_WMM_STATE)));

    switch (pWmmState->State)
    {
	case CMD_DISABLED: /* disable */
	     if (Adapter->MediaConnectStatus == NdisMediaStateConnected)
		return FALSE; 

	     Adapter->WmmDesc.required = 0;

	     if (!Adapter->WmmDesc.enabled)
 		  return FALSE; 
	     else
		  Adapter->WmmDesc.enabled = 0;

	     if (Adapter->SentPacket) 
	     {
		 Status = NDIS_STATUS_FAILURE;
		 pPacket = Adapter->SentPacket;
  		 NDIS_SET_PACKET_STATUS(pPacket, Status);
		 NdisMSendComplete(
			Adapter->MrvDrvAdapterHdl,
			pPacket,
			Status);
		 Adapter->SentPacket = NULL;
	     }

		/* Release all skb's in all the queues */
	     wmm_cleanup_queues(Adapter);
		
	     Adapter->CurrentPacketFilter &= ~HostCmd_ACT_MAC_WMM_ENABLE;

	     SetMacPacketFilter(Adapter, Adapter->CurrentPacketFilter);
	break;

	case CMD_ENABLED: /* enable */
	     if (Adapter->MediaConnectStatus == NdisMediaStateConnected)
	     {
//                DBGPRINT(DBG_WMM, ("The media is already connected \n"));
		return FALSE; 
             } 
	     Adapter->WmmDesc.required = 1;
//             DBGPRINT(DBG_WMM, ("Enable Firmware WMM \n"));

		break;
	}

	return TRUE;
}


int wmm_lists_empty(PMRVDRV_ADAPTER Adapter)
{
	int i;

	for (i = AC_PRIO_VO; i >= AC_PRIO_BE; --i)
		if (!IsQEmpty(&Adapter->WmmDesc.TxPktQ[i]))
			return 0;

	return 1;
}

void wmm_cleanup_queues(PMRVDRV_ADAPTER Adapter)
{
	int 		i;
	PQ_KEEPER       Q;
	PNDIS_PACKET	pPacket;
	NDIS_STATUS     Status;

        Status = NDIS_STATUS_FAILURE;
	for (i = AC_PRIO_BE; i <= AC_PRIO_VO; i++)		        ;

	{
	   Q = &Adapter->WmmDesc.TxPktQ[i];
	   while (!IsQEmpty(Q))
	   {
		pPacket = (PNDIS_PACKET)PopFirstQNode(Q);
  		NDIS_SET_PACKET_STATUS(pPacket, Status);
		NdisMSendComplete(
		        Adapter->MrvDrvAdapterHdl,
			pPacket,
			Status);
	   } // end of while loop
	} // end of for loop
}

UCHAR wmm_get_tos(PNDIS_PACKET pPacket)
{
     UCHAR tos = 0, temp;
     PNDIS_BUFFER pBuffer, pNextBuffer;
     UINT BufferCount, TotalPktLen;
     UINT Length1, Length2;
     PVOID pVirtualAddr1=NULL, pVirtualAddr2=NULL;
     USHORT type;
     PUCHAR ptr;
     
     NdisQueryPacket(pPacket, NULL, &BufferCount, &pBuffer, &TotalPktLen);    
     
     
     if(TotalPktLen >= MIN_IP_PKT_LEN)
     {
     	  NdisQueryBuffer(pBuffer, pVirtualAddr1, &Length1);
          ptr = (PUCHAR)pVirtualAddr1;
     	  if(Length1 < 16)
     	  { 
     	      NdisGetNextBuffer(pBuffer, &pNextBuffer);
     	      NdisQueryBuffer(pNextBuffer, pVirtualAddr2, &Length2);
              type = ptr[12] << 8;
              switch(Length1)
              {
              	  case 13:
              	       ptr = (PUCHAR)pVirtualAddr2;
              	       type |= ptr[0];
              	       temp = ptr[2];
              	  break;
              	  case 14:
              	       type |= ptr[13];
              	       ptr = (PUCHAR)pVirtualAddr2;
                       temp = ptr[1];              	       
              	  break;
              	  case 15:
              	       type |= ptr[13];
              	       ptr = (PUCHAR)pVirtualAddr2;
                       temp = ptr[0];              	       
              	  break;
              	
              }	
     	  }
     	  else
     	  {
               type |= ptr[13];
               temp = ptr[15];     	  	
     	  }	    
          
         
         if(type = 0x0800)  // IP packet
            tos = temp;
     }

     return tos;	
	
}	

void wmm_map_and_add_pkt(PMRVDRV_ADAPTER Adapter, PNDIS_PACKET packet)
{
	UCHAR	tos, ac;

        tos = wmm_get_tos(packet);
	ac = wmm_tos2ac[Adapter->WmmDesc.acstatus][tos];

	/* Access control of the current packet not the Lowest */
	if(ac > AC_PRIO_BE)
		Adapter->WmmDesc.fw_notify = 1;
	InsertQNodeAtTail(&Adapter->WmmDesc.TxPktQ[ac], packet);
}

void wmm_pop_highest_prio_skb(PMRVDRV_ADAPTER Adapter)
{
	int i;
	PQ_KEEPER       Q;

	for(i = AC_PRIO_VO; i >= AC_PRIO_BE; --i)
	{
	   Q = &Adapter->WmmDesc.TxPktQ[i];	
	    if(!IsQEmpty(Q))
	   {
		Adapter->CurrentTxPkt = (PNDIS_PACKET)PopFirstQNode(Q);
		break;
	   }
	}
}