wlan_wmm.c

marvell wifi driver GSPI-8385-LINUX-OMAP1510-5.0.10.p0-144-src.rar

			HostCmd_DS_COMMAND *cmd, u16 action, void *InfoBuf)
{
	cmd->Command = wlan_cpu_to_le16(HostCmd_CMD_802_11_WMM_PRIO_PKT_AVAIL);
	cmd->Size = wlan_cpu_to_le16(
			sizeof(HostCmd_DS_802_11_WMM_PRIO_PKT_AVAIL)+S_DS_GEN);

	cmd->params.priopktavail.PacketAC = priv->adapter->
						priopktavail.PacketAC;

	return 0;
}

inline int sendWMMStatusChangeCmd(wlan_private *priv)
{
	return PrepareAndSendCommand(priv, HostCmd_CMD_802_11_WMM_GET_STATUS,
			0, HostCmd_OPTION_USE_INT,
			0, HostCmd_PENDING_ON_NONE, NULL);
}

int wmm_lists_empty(wlan_private *priv)
{
	int i;

	for (i = 0; i < MAX_AC_QUEUES; i++)
		if (!list_empty((struct list_head*)
					&priv->adapter->wmm.TxSkbQ[i]))
			return 0;

	return 1;
}

void wmm_cleanup_queues(wlan_private *priv)
{
	int 		i;
	struct sk_buff	*delNode, *Q;

	for (i = 0; i < MAX_AC_QUEUES; i++) {
		Q = &priv->adapter->wmm.TxSkbQ[i];
		
		while (!list_empty((struct list_head *) Q)) {
			delNode = Q->next;
			list_del((struct list_head *)delNode);
			kfree_skb(delNode);
		}
	}
}

#define IPTOS_OFFSET 5
void wmm_map_and_add_skb(wlan_private *priv, struct sk_buff *skb)
{
	wlan_adapter 	*Adapter = priv->adapter;
	u8		tos, ac0, ac;
	struct ethhdr	*eth = (struct ethhdr *)skb->data;

	switch (eth->h_proto)
	{
	case __constant_htons(ETH_P_IP):
		PRINTK("packet type ETH_P_IP: %04x, tos=%#x prio=%#x\n",eth->h_proto,skb->nh.iph->tos,skb->priority);
		tos = IPTOS_PREC(skb->nh.iph->tos) >> IPTOS_OFFSET;
		break;
	case __constant_htons(ETH_P_ARP):
		PRINTK("ARP packet %04x\n",eth->h_proto);
	default:
		tos = 0;
		break;
	}

	ac0 = wmm_tos2ac[tos];
	ac = wmm_ac_downgrade[ac0];
	
	skb->priority = wmm_tos2priority[tos];
	PRINTK("wmm_map: tos=%#x, ac0=%#x ac=%#x, prio=%#x\n",tos,ac0,ac,skb->priority);

	/* Access control of the current packet not the Lowest */
	if(ac > AC_PRIO_BE)
		Adapter->wmm.fw_notify = 1;

	list_add_tail((struct list_head *) skb,
			(struct list_head *) &Adapter->wmm.TxSkbQ[ac]);
}

static void wmm_pop_highest_prio_skb(wlan_private *priv)
{
	int i;
	wlan_adapter *Adapter = priv->adapter;
	u8 ac;

	for (i = 0; i < MAX_AC_QUEUES; i++) {
		ac = Adapter->CurBssParams.wmm_queue_prio[i];
		if(!list_empty((struct list_head*) 
					&Adapter->wmm.TxSkbQ[ac])) {
			Adapter->CurrentTxSkb = Adapter->wmm.TxSkbQ[ac].next;
			list_del((struct list_head *) Adapter->
						wmm.TxSkbQ[ac].next);
			break;
		}
	}
}

static void wmm_send_prio_pkt_avail(wlan_private *priv)
{
#if 0	/* WMM_PRIO_PKT_AVAIL command not supported for now */
	int i;

	for (i = 0; i < MAX_AC_QUEUES; i++) {
		ac = Adapter->CurBssParams.wmm_queue_prio[i];
		if(!list_empty((struct list_head*)
					&priv->adapter->wmm.TxSkbQ[ac]))
			break;

	if(i >= MAX_AC_QUEUES)		/* No High prio packets available */
		return;

	priv->adapter->priopktavail.PacketAC = ac;

	PrepareAndSendCommand(priv,
		HostCmd_CMD_802_11_WMM_PRIO_PKT_AVAIL,
		0, HostCmd_OPTION_USE_INT,
		0, HostCmd_PENDING_ON_NONE, 
		NULL);
#endif
}

void wmm_setup_queue_priorities(wlan_private *priv)
{
	wlan_adapter *Adapter = priv->adapter;
#ifdef WMM_AIFS
	pWMM_PARA_IE pIe = (pWMM_PARA_IE)Adapter->BSSIDList[Adapter->ulCurrentBSSIDIndex].Wmm_IE;

	HEXDUMP("Wmm_IE in association response", (u8 *)pIe,
		Adapter->BSSIDList[Adapter->ulCurrentBSSIDIndex].Wmm_ie_len);
	if (pIe->OuiSubtype == WMM_OUISUBTYPE_PARA) {
		u16 cwmax, cwmin, avg_back_off;
		u16 tmp[4];
		int i, j, n = 0;
		PRINTK("WMM Parameter IE: version=%d, QoSInfo Parameter Set Count=%d, Reserved=%#x\n",
	    		pIe->Version,
	    		pIe->QoSInfo.ParaSetCount,
	    		pIe->Reserved);
		PRINTK("BE AC_Parameters: ACI=%d, ACM=%d, AIFSN=%d\n",
	    		pIe->AC_Paras_BE.ACI_AIFSN.ACI,
	    		pIe->AC_Paras_BE.ACI_AIFSN.ACM,
	    		pIe->AC_Paras_BE.ACI_AIFSN.AIFSN);
		PRINTK("                  ECWmin=%d, ECWmax=%d, Txop_Limit=%d\n",
	    		pIe->AC_Paras_BE.ECW.ECW_Min,
	    		pIe->AC_Paras_BE.ECW.ECW_Max,
               		wlan_le16_to_cpu(pIe->AC_Paras_BE.Txop_Limit)<<WMM_TXOP_LIMIT_UNITS_SHIFT);
		cwmax = (1 << pIe->AC_Paras_BE.ECW.ECW_Max) - 1;
		cwmin = (1 << pIe->AC_Paras_BE.ECW.ECW_Min) - 1;
		avg_back_off = (cwmin >> 1) + pIe->AC_Paras_BE.ACI_AIFSN.AIFSN;
		PRINTK("CWmax=%d CWmin=%d Avg Back-off=%d\n",cwmax,cwmin,avg_back_off);
		Adapter->CurBssParams.wmm_queue_prio[n] = AC_PRIO_BE;
		tmp[n++] = avg_back_off;
		PRINTK("BK AC_Parameters: ACI=%d, ACM=%d, AIFSN=%d\n",
	    		pIe->AC_Paras_BK.ACI_AIFSN.ACI,
	    		pIe->AC_Paras_BK.ACI_AIFSN.ACM,
	    		pIe->AC_Paras_BK.ACI_AIFSN.AIFSN);
		PRINTK("                  ECWmin=%d, ECWmax=%d, Txop_Limit=%d\n",
	    		pIe->AC_Paras_BK.ECW.ECW_Min,
	    		pIe->AC_Paras_BK.ECW.ECW_Max,
               		wlan_le16_to_cpu(pIe->AC_Paras_BK.Txop_Limit)<<WMM_TXOP_LIMIT_UNITS_SHIFT);
		cwmax = (1 << pIe->AC_Paras_BK.ECW.ECW_Max) - 1;
		cwmin = (1 << pIe->AC_Paras_BK.ECW.ECW_Min) - 1;
		avg_back_off = (cwmin >> 1) + pIe->AC_Paras_BK.ACI_AIFSN.AIFSN;
		PRINTK("CWmax=%d CWmin=%d Avg Back-off=%d\n",cwmax,cwmin,avg_back_off);
		Adapter->CurBssParams.wmm_queue_prio[n] = AC_PRIO_BK;
		tmp[n++] = avg_back_off;
		PRINTK("VI AC_Parameters: ACI=%d, ACM=%d, AIFSN=%d\n",
	    		pIe->AC_Paras_VI.ACI_AIFSN.ACI,
	    		pIe->AC_Paras_VI.ACI_AIFSN.ACM,
	    		pIe->AC_Paras_VI.ACI_AIFSN.AIFSN);
		PRINTK("                  ECWmin=%d, ECWmax=%d, Txop_Limit=%d\n",
	    		pIe->AC_Paras_VI.ECW.ECW_Min,
	    		pIe->AC_Paras_VI.ECW.ECW_Max,
               		wlan_le16_to_cpu(pIe->AC_Paras_VI.Txop_Limit)<<WMM_TXOP_LIMIT_UNITS_SHIFT);
		cwmax = (1 << pIe->AC_Paras_VI.ECW.ECW_Max) - 1;
		cwmin = (1 << pIe->AC_Paras_VI.ECW.ECW_Min) - 1;
		avg_back_off = (cwmin >> 1) + pIe->AC_Paras_VI.ACI_AIFSN.AIFSN;
		PRINTK("CWmax=%d CWmin=%d Avg Back-off=%d\n",cwmax,cwmin,avg_back_off);
		Adapter->CurBssParams.wmm_queue_prio[n] = AC_PRIO_VI;
		tmp[n++] = avg_back_off;
		PRINTK("VO AC_Parameters: ACI=%d, ACM=%d, AIFSN=%d\n",
	    		pIe->AC_Paras_VO.ACI_AIFSN.ACI,
	    		pIe->AC_Paras_VO.ACI_AIFSN.ACM,
	    		pIe->AC_Paras_VO.ACI_AIFSN.AIFSN);
		PRINTK("                  ECWmin=%d, ECWmax=%d, Txop_Limit=%d\n",
	    		pIe->AC_Paras_VO.ECW.ECW_Min,
	    		pIe->AC_Paras_VO.ECW.ECW_Max,
               		wlan_le16_to_cpu(pIe->AC_Paras_VO.Txop_Limit)<<WMM_TXOP_LIMIT_UNITS_SHIFT);
		cwmax = (1 << pIe->AC_Paras_VO.ECW.ECW_Max) - 1;
		cwmin = (1 << pIe->AC_Paras_VO.ECW.ECW_Min) - 1;
		avg_back_off = (cwmin >> 1) + pIe->AC_Paras_VO.ACI_AIFSN.AIFSN;
		PRINTK("CWmax=%d CWmin=%d Avg Back-off=%d\n",cwmax,cwmin,avg_back_off);
		Adapter->CurBssParams.wmm_queue_prio[n] = AC_PRIO_VO;
		tmp[n++] = avg_back_off;
		HEXDUMP("avg_back_off  ", (u8 *)tmp, sizeof(tmp));
		HEXDUMP("wmm_queue_prio", Adapter->CurBssParams.wmm_queue_prio,
			sizeof(Adapter->CurBssParams.wmm_queue_prio));
		/* bubble sort */
		for (i=0; i < n; i++) {
		    for (j=1; j < n - i; j++) {
			if (tmp[j-1] > tmp[j]) {
				SWAP_U16(tmp[j-1],tmp[j]);
				SWAP_U8(Adapter->CurBssParams.wmm_queue_prio[j-1],
					Adapter->CurBssParams.wmm_queue_prio[j]);
			}
			else if (tmp[j-1] == tmp[j]) {
				if (Adapter->CurBssParams.wmm_queue_prio[j-1] <
					Adapter->CurBssParams.wmm_queue_prio[j]) {
					SWAP_U8(Adapter->CurBssParams.wmm_queue_prio[j-1],
						Adapter->CurBssParams.wmm_queue_prio[j]);
				}
			}
		    }
		}
		HEXDUMP("avg_back_off  ", (u8 *)tmp, sizeof(tmp));
		HEXDUMP("wmm_queue_prio", Adapter->CurBssParams.wmm_queue_prio,
			sizeof(Adapter->CurBssParams.wmm_queue_prio));
	}
	else if (((pWMM_PARA_IE)pIe)->OuiSubtype == WMM_OUISUBTYPE_IE)
#endif /* WMM_AIFS */
	{
		/* default queue priorities: VO->VI->BE->BK */
		Adapter->CurBssParams.wmm_queue_prio[0] = AC_PRIO_VO;
		Adapter->CurBssParams.wmm_queue_prio[1] = AC_PRIO_VI;
		Adapter->CurBssParams.wmm_queue_prio[2] = AC_PRIO_BE;
		Adapter->CurBssParams.wmm_queue_prio[3] = AC_PRIO_BK;
	}
}

void wmm_setup_ac_downgrade(wlan_private *priv)
{
	wlan_adapter *Adapter = priv->adapter;
	u8 ac0, ac;
	int i, j;

	PRINTK("wmm.acstatus = %#x\n", Adapter->wmm.acstatus);

	for (i = 0; i < MAX_AC_QUEUES; i++) {	/* default settings without ACM */
		wmm_ac_downgrade[i] = (u8)i;
	}
	HEXDUMP("wmm_ac_downgrade default", wmm_ac_downgrade, sizeof(wmm_ac_downgrade));

	for (i = 0; i < MAX_AC_QUEUES - 1; i++) {	/* skip the last one */
		ac0 = Adapter->CurBssParams.wmm_queue_prio[i];	/* original AC */
		ac = Adapter->CurBssParams.wmm_queue_prio[i+1];	/* downgraded AC */
		if (Adapter->wmm.acstatus & (1<<ac0)) {
			for (j = 0; j < MAX_AC_QUEUES; j++) {
				if (ac0 == wmm_ac_downgrade[j]) {
					PRINTK("wmm_setup_ac_downgrade: i=%d j=%d ac0=%d ac=%d\n",i,j,ac0,ac);
					wmm_ac_downgrade[j] = ac;
				}
			}
			PRINTK("wmm_setup_ac_downgrade: AC %#x has been downgraded to %#x\n",ac0,ac);
		}
	}

	HEXDUMP("wmm_ac_downgrade", wmm_ac_downgrade, sizeof(wmm_ac_downgrade));

	ac0 = Adapter->CurBssParams.wmm_queue_prio[MAX_AC_QUEUES - 1];
	if (Adapter->wmm.acstatus & (1<<ac0)) {
		printk("wmm_setup_ac_downgrade: ignored ACM for the AC %#x as it has the lowest priority\n",ac0);
	}
}

void wmm_process_tx(wlan_private * priv)
{
	wlan_adapter   *Adapter = priv->adapter;
	u32		flags;

	OS_INTERRUPT_SAVE_AREA;	/* Needed for Threadx; Dummy for Linux */

	ENTER();
    
#ifdef PS_REQUIRED
	if ((Adapter->PSState == PS_STATE_SLEEP)
#ifdef PS_PRESLEEP
		|| (Adapter->PSState == PS_STATE_PRE_SLEEP)
#endif
	) {
		PRINTK1("In PS State %d"
			" - Not sending the packet\n", Adapter->PSState);
		goto done;
	}
#endif

	spin_lock_irqsave(&Adapter->CurrentTxLock, flags);

	if(priv->wlan_dev.dnld_sent) {

		if(priv->adapter->wmm.fw_notify) {
			wmm_send_prio_pkt_avail(priv);
			priv->adapter->wmm.fw_notify = 0;
		}

		spin_unlock_irqrestore(&Adapter->CurrentTxLock, flags);
		
		goto done;
	}

	wmm_pop_highest_prio_skb(priv);
	spin_unlock_irqrestore(&Adapter->CurrentTxLock, flags);

	wlan_process_tx(priv);
		
	
done:
	LEAVE();
}