wilc1000: added queue support for WMM

Added multiple queues[BK,BE,VI,VO] to handle different priority data
packets. Before adding a packet to the queue, checked its priority from
the header, and then add to the suitable queue. The limit for each queue
is maintained separately. Also while passing the packets to the firmware
via VMM take care to select data packets based on priority and available
space.

Signed-off-by: Ajay Singh <ajay.kathat@microchip.com>
Signed-off-by: Kalle Valo <kvalo@codeaurora.org>
Link: https://lore.kernel.org/r/20201125114059.10006-6-ajay.kathat@microchip.com

1 files changed, 257 insertions, 58 deletions

diff --git a/drivers/net/wireless/microchip/wilc1000/wlan.c b/drivers/net/wireless/microchip/wilc1000/wlan.c
index 0ff4de28f622..993ea7c03429 100644
--- a/drivers/net/wireless/microchip/wilc1000/wlan.c
+++ b/drivers/net/wireless/microchip/wilc1000/wlan.c
@@ -6,6 +6,7 @@
 
 #include <linux/if_ether.h>
 #include <linux/ip.h>
+#include <net/dsfield.h>
 #include "cfg80211.h"
 #include "wlan_cfg.h"
 
@@ -28,33 +29,34 @@ static inline void release_bus(struct wilc *wilc, enum bus_release release)
 	mutex_unlock(&wilc->hif_cs);
 }
 
-static void wilc_wlan_txq_remove(struct wilc *wilc, struct txq_entry_t *tqe)
+static void wilc_wlan_txq_remove(struct wilc *wilc, u8 q_num,
+				 struct txq_entry_t *tqe)
 {
 	list_del(&tqe->list);
 	wilc->txq_entries -= 1;
+	wilc->txq[q_num].count--;
 }
 
 static struct txq_entry_t *
-wilc_wlan_txq_remove_from_head(struct net_device *dev)
+wilc_wlan_txq_remove_from_head(struct wilc *wilc, u8 q_num)
 {
 	struct txq_entry_t *tqe = NULL;
 	unsigned long flags;
-	struct wilc_vif *vif = netdev_priv(dev);
-	struct wilc *wilc = vif->wilc;
 
 	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
-	if (!list_empty(&wilc->txq_head.list)) {
-		tqe = list_first_entry(&wilc->txq_head.list, struct txq_entry_t,
-				       list);
+	if (!list_empty(&wilc->txq[q_num].txq_head.list)) {
+		tqe = list_first_entry(&wilc->txq[q_num].txq_head.list,
+				       struct txq_entry_t, list);
 		list_del(&tqe->list);
 		wilc->txq_entries -= 1;
+		wilc->txq[q_num].count--;
 	}
 	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
 	return tqe;
 }
 
-static void wilc_wlan_txq_add_to_tail(struct net_device *dev,
+static void wilc_wlan_txq_add_to_tail(struct net_device *dev, u8 q_num,
 				      struct txq_entry_t *tqe)
 {
 	unsigned long flags;
@@ -63,15 +65,16 @@ static void wilc_wlan_txq_add_to_tail(struct net_device *dev,
 
 	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
-	list_add_tail(&tqe->list, &wilc->txq_head.list);
+	list_add_tail(&tqe->list, &wilc->txq[q_num].txq_head.list);
 	wilc->txq_entries += 1;
+	wilc->txq[q_num].count++;
 
 	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
 
 	complete(&wilc->txq_event);
 }
 
-static void wilc_wlan_txq_add_to_head(struct wilc_vif *vif,
+static void wilc_wlan_txq_add_to_head(struct wilc_vif *vif, u8 q_num,
 				      struct txq_entry_t *tqe)
 {
 	unsigned long flags;
@@ -81,8 +84,9 @@ static void wilc_wlan_txq_add_to_head(struct wilc_vif *vif,
 
 	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
-	list_add(&tqe->list, &wilc->txq_head.list);
+	list_add(&tqe->list, &wilc->txq[q_num].txq_head.list);
 	wilc->txq_entries += 1;
+	wilc->txq[q_num].count++;
 
 	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
 	mutex_unlock(&wilc->txq_add_to_head_cs);
@@ -212,7 +216,7 @@ static void wilc_wlan_txq_filter_dup_tcp_ack(struct net_device *dev)
 
 			tqe = f->pending_acks[i].txqe;
 			if (tqe) {
-				wilc_wlan_txq_remove(wilc, tqe);
+				wilc_wlan_txq_remove(wilc, tqe->q_num, tqe);
 				tqe->status = 1;
 				if (tqe->tx_complete_func)
 					tqe->tx_complete_func(tqe->priv,
@@ -268,10 +272,138 @@ static int wilc_wlan_txq_add_cfg_pkt(struct wilc_vif *vif, u8 *buffer,
 	tqe->buffer_size = buffer_size;
 	tqe->tx_complete_func = NULL;
 	tqe->priv = NULL;
+	tqe->q_num = AC_VO_Q;
 	tqe->ack_idx = NOT_TCP_ACK;
 	tqe->vif = vif;
 
-	wilc_wlan_txq_add_to_head(vif, tqe);
+	wilc_wlan_txq_add_to_head(vif, AC_VO_Q, tqe);
+
+	return 1;
+}
+
+static bool is_ac_q_limit(struct wilc *wl, u8 q_num)
+{
+	u8 factors[NQUEUES] = {1, 1, 1, 1};
+	u16 i;
+	unsigned long flags;
+	struct wilc_tx_queue_status *q = &wl->tx_q_limit;
+	u8 end_index;
+	u8 q_limit;
+	bool ret = false;
+
+	spin_lock_irqsave(&wl->txq_spinlock, flags);
+	if (!q->initialized) {
+		for (i = 0; i < AC_BUFFER_SIZE; i++)
+			q->buffer[i] = i % NQUEUES;
+
+		for (i = 0; i < NQUEUES; i++) {
+			q->cnt[i] = AC_BUFFER_SIZE * factors[i] / NQUEUES;
+			q->sum += q->cnt[i];
+		}
+		q->end_index = AC_BUFFER_SIZE - 1;
+		q->initialized = 1;
+	}
+
+	end_index = q->end_index;
+	q->cnt[q->buffer[end_index]] -= factors[q->buffer[end_index]];
+	q->cnt[q_num] += factors[q_num];
+	q->sum += (factors[q_num] - factors[q->buffer[end_index]]);
+
+	q->buffer[end_index] = q_num;
+	if (end_index > 0)
+		q->end_index--;
+	else
+		q->end_index = AC_BUFFER_SIZE - 1;
+
+	if (!q->sum)
+		q_limit = 1;
+	else
+		q_limit = (q->cnt[q_num] * FLOW_CONTROL_UPPER_THRESHOLD / q->sum) + 1;
+
+	if (wl->txq[q_num].count <= q_limit)
+		ret = true;
+
+	spin_unlock_irqrestore(&wl->txq_spinlock, flags);
+
+	return ret;
+}
+
+static inline u8 ac_classify(struct wilc *wilc, struct sk_buff *skb)
+{
+	u8 q_num = AC_BE_Q;
+	u8 dscp;
+
+	switch (skb->protocol) {
+	case htons(ETH_P_IP):
+		dscp = ipv4_get_dsfield(ip_hdr(skb)) & 0xfc;
+		break;
+	case htons(ETH_P_IPV6):
+		dscp = ipv6_get_dsfield(ipv6_hdr(skb)) & 0xfc;
+		break;
+	default:
+		return q_num;
+	}
+
+	switch (dscp) {
+	case 0x08:
+	case 0x20:
+	case 0x40:
+		q_num = AC_BK_Q;
+		break;
+	case 0x80:
+	case 0xA0:
+	case 0x28:
+		q_num = AC_VI_Q;
+		break;
+	case 0xC0:
+	case 0xD0:
+	case 0xE0:
+	case 0x88:
+	case 0xB8:
+		q_num = AC_VO_Q;
+		break;
+	}
+
+	return q_num;
+}
+
+static inline int ac_balance(struct wilc *wl, u8 *ratio)
+{
+	u8 i, max_count = 0;
+
+	if (!ratio)
+		return -EINVAL;
+
+	for (i = 0; i < NQUEUES; i++)
+		if (wl->txq[i].fw.count > max_count)
+			max_count = wl->txq[i].fw.count;
+
+	for (i = 0; i < NQUEUES; i++)
+		ratio[i] = max_count - wl->txq[i].fw.count;
+
+	return 0;
+}
+
+static inline void ac_update_fw_ac_pkt_info(struct wilc *wl, u32 reg)
+{
+	wl->txq[AC_BK_Q].fw.count = FIELD_GET(BK_AC_COUNT_FIELD, reg);
+	wl->txq[AC_BE_Q].fw.count = FIELD_GET(BE_AC_COUNT_FIELD, reg);
+	wl->txq[AC_VI_Q].fw.count = FIELD_GET(VI_AC_COUNT_FIELD, reg);
+	wl->txq[AC_VO_Q].fw.count = FIELD_GET(VO_AC_COUNT_FIELD, reg);
+
+	wl->txq[AC_BK_Q].fw.acm = FIELD_GET(BK_AC_ACM_STAT_FIELD, reg);
+	wl->txq[AC_BE_Q].fw.acm = FIELD_GET(BE_AC_ACM_STAT_FIELD, reg);
+	wl->txq[AC_VI_Q].fw.acm = FIELD_GET(VI_AC_ACM_STAT_FIELD, reg);
+	wl->txq[AC_VO_Q].fw.acm = FIELD_GET(VO_AC_ACM_STAT_FIELD, reg);
+}
+
+static inline u8 ac_change(struct wilc *wilc, u8 *ac)
+{
+	do {
+		if (wilc->txq[*ac].fw.acm == 0)
+			return 0;
+		(*ac)++;
+	} while (*ac < NQUEUES);
 
 	return 1;
 }
@@ -283,6 +415,7 @@ int wilc_wlan_txq_add_net_pkt(struct net_device *dev, void *priv, u8 *buffer,
 	struct txq_entry_t *tqe;
 	struct wilc_vif *vif = netdev_priv(dev);
 	struct wilc *wilc;
+	u8 q_num;
 
 	wilc = vif->wilc;
 
@@ -304,10 +437,24 @@ int wilc_wlan_txq_add_net_pkt(struct net_device *dev, void *priv, u8 *buffer,
 	tqe->priv = priv;
 	tqe->vif = vif;
 
-	tqe->ack_idx = NOT_TCP_ACK;
-	if (vif->ack_filter.enabled)
-		tcp_process(dev, tqe);
-	wilc_wlan_txq_add_to_tail(dev, tqe);
+	q_num = ac_classify(wilc, priv);
+	tqe->q_num = q_num;
+	if (ac_change(wilc, &q_num)) {
+		tx_complete_fn(priv, 0);
+		kfree(tqe);
+		return 0;
+	}
+
+	if (is_ac_q_limit(wilc, q_num)) {
+		tqe->ack_idx = NOT_TCP_ACK;
+		if (vif->ack_filter.enabled)
+			tcp_process(dev, tqe);
+		wilc_wlan_txq_add_to_tail(dev, q_num, tqe);
+	} else {
+		tx_complete_fn(priv, 0);
+		kfree(tqe);
+	}
+
 	return wilc->txq_entries;
 }
 
@@ -337,22 +484,23 @@ int wilc_wlan_txq_add_mgmt_pkt(struct net_device *dev, void *priv, u8 *buffer,
 	tqe->buffer_size = buffer_size;
 	tqe->tx_complete_func = tx_complete_fn;
 	tqe->priv = priv;
+	tqe->q_num = AC_BE_Q;
 	tqe->ack_idx = NOT_TCP_ACK;
 	tqe->vif = vif;
-	wilc_wlan_txq_add_to_tail(dev, tqe);
+	wilc_wlan_txq_add_to_tail(dev, AC_VO_Q, tqe);
 	return 1;
 }
 
-static struct txq_entry_t *wilc_wlan_txq_get_first(struct wilc *wilc)
+static struct txq_entry_t *wilc_wlan_txq_get_first(struct wilc *wilc, u8 q_num)
 {
 	struct txq_entry_t *tqe = NULL;
 	unsigned long flags;
 
 	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
-	if (!list_empty(&wilc->txq_head.list))
-		tqe = list_first_entry(&wilc->txq_head.list, struct txq_entry_t,
-				       list);
+	if (!list_empty(&wilc->txq[q_num].txq_head.list))
+		tqe = list_first_entry(&wilc->txq[q_num].txq_head.list,
+				       struct txq_entry_t, list);
 
 	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
 
@@ -360,13 +508,14 @@ static struct txq_entry_t *wilc_wlan_txq_get_first(struct wilc *wilc)
 }
 
 static struct txq_entry_t *wilc_wlan_txq_get_next(struct wilc *wilc,
-						  struct txq_entry_t *tqe)
+						  struct txq_entry_t *tqe,
+						  u8 q_num)
 {
 	unsigned long flags;
 
 	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
-	if (!list_is_last(&tqe->list, &wilc->txq_head.list))
+	if (!list_is_last(&tqe->list, &wilc->txq[q_num].txq_head.list))
 		tqe = list_next_entry(tqe, list);
 	else
 		tqe = NULL;
@@ -489,54 +638,92 @@ EXPORT_SYMBOL_GPL(host_sleep_notify);
 int wilc_wlan_handle_txq(struct wilc *wilc, u32 *txq_count)
 {
 	int i, entries = 0;
+	u8 k, ac;
 	u32 sum;
 	u32 reg;
+	u8 ac_desired_ratio[NQUEUES] = {0, 0, 0, 0};
+	u8 ac_preserve_ratio[NQUEUES] = {1, 1, 1, 1};
+	u8 *num_pkts_to_add;
+	u8 vmm_entries_ac[WILC_VMM_TBL_SIZE];
 	u32 offset = 0;
+	bool max_size_over = 0, ac_exist = 0;
 	int vmm_sz = 0;
-	struct txq_entry_t *tqe;
+	struct txq_entry_t *tqe_q[NQUEUES];
 	int ret = 0;
 	int counter;
 	int timeout;
 	u32 vmm_table[WILC_VMM_TBL_SIZE];
+	u8 ac_pkt_num_to_chip[NQUEUES] = {0, 0, 0, 0};
 	const struct wilc_hif_func *func;
+	int srcu_idx;
 	u8 *txb = wilc->tx_buffer;
-	struct net_device *dev;
 	struct wilc_vif *vif;
 
 	if (wilc->quit)
 		goto out_update_cnt;
 
+	if (ac_balance(wilc, ac_desired_ratio))
+		return -EINVAL;
+
 	mutex_lock(&wilc->txq_add_to_head_cs);
-	tqe = wilc_wlan_txq_get_first(wilc);
-	if (!tqe)
-		goto out_unlock;
-	dev = tqe->vif->ndev;
-	wilc_wlan_txq_filter_dup_tcp_ack(dev);
+
+	srcu_idx = srcu_read_lock(&wilc->srcu);
+	list_for_each_entry_rcu(vif, &wilc->vif_list, list)
+		wilc_wlan_txq_filter_dup_tcp_ack(vif->ndev);
+	srcu_read_unlock(&wilc->srcu, srcu_idx);
+
+	for (ac = 0; ac < NQUEUES; ac++)
+		tqe_q[ac] = wilc_wlan_txq_get_first(wilc, ac);
+
 	i = 0;
 	sum = 0;
-	while (tqe && (i < (WILC_VMM_TBL_SIZE - 1))) {
-		if (tqe->type == WILC_CFG_PKT)
-			vmm_sz = ETH_CONFIG_PKT_HDR_OFFSET;
-		else if (tqe->type == WILC_NET_PKT)
-			vmm_sz = ETH_ETHERNET_HDR_OFFSET;
-		else
-			vmm_sz = HOST_HDR_OFFSET;
-
-		vmm_sz += tqe->buffer_size;
-		vmm_sz = ALIGN(vmm_sz, 4);
-
-		if ((sum + vmm_sz) > WILC_TX_BUFF_SIZE)
-			break;
+	max_size_over = 0;
+	num_pkts_to_add = ac_desired_ratio;
+	do {
+		ac_exist = 0;
+		for (ac = 0; (ac < NQUEUES) && (!max_size_over); ac++) {
+			if (!tqe_q[ac])
+				continue;
+
+			vif = tqe_q[ac]->vif;
+			ac_exist = 1;
+			for (k = 0; (k < num_pkts_to_add[ac]) &&
+			     (!max_size_over) && tqe_q[ac]; k++) {
+				if (i >= (WILC_VMM_TBL_SIZE - 1)) {
+					max_size_over = 1;
+					break;
+				}
 
-		vmm_table[i] = vmm_sz / 4;
-		if (tqe->type == WILC_CFG_PKT)
-			vmm_table[i] |= BIT(10);
-		cpu_to_le32s(&vmm_table[i]);
+				if (tqe_q[ac]->type == WILC_CFG_PKT)
+					vmm_sz = ETH_CONFIG_PKT_HDR_OFFSET;
+				else if (tqe_q[ac]->type == WILC_NET_PKT)
+					vmm_sz = ETH_ETHERNET_HDR_OFFSET;
+				else
+					vmm_sz = HOST_HDR_OFFSET;
 
-		i++;
-		sum += vmm_sz;
-		tqe = wilc_wlan_txq_get_next(wilc, tqe);
-	}
+				vmm_sz += tqe_q[ac]->buffer_size;
+				vmm_sz = ALIGN(vmm_sz, 4);
+
+				if ((sum + vmm_sz) > WILC_TX_BUFF_SIZE) {
+					max_size_over = 1;
+					break;
+				}
+				vmm_table[i] = vmm_sz / 4;
+				if (tqe_q[ac]->type == WILC_CFG_PKT)
+					vmm_table[i] |= BIT(10);
+
+				cpu_to_le32s(&vmm_table[i]);
+				vmm_entries_ac[i] = ac;
+
+				i++;
+				sum += vmm_sz;
+				tqe_q[ac] = wilc_wlan_txq_get_next(wilc,
+								   tqe_q[ac],
+								   ac);
+			}
+		}
+		num_pkts_to_add = ac_preserve_ratio;
+	} while (!max_size_over && ac_exist);
 
 	if (i == 0)
 		goto out_unlock;
@@ -550,8 +737,10 @@ int wilc_wlan_handle_txq(struct wilc *wilc, u32 *txq_count)
 		if (ret)
 			break;
 
-		if ((reg & 0x1) == 0)
+		if ((reg & 0x1) == 0) {
+			ac_update_fw_ac_pkt_info(wilc, reg);
 			break;
+		}
 
 		counter++;
 		if (counter > 200) {
@@ -620,11 +809,13 @@ int wilc_wlan_handle_txq(struct wilc *wilc, u32 *txq_count)
 	offset = 0;
 	i = 0;
 	do {
+		struct txq_entry_t *tqe;
 		u32 header, buffer_offset;
 		char *bssid;
 		u8 mgmt_ptk = 0;
 
-		tqe = wilc_wlan_txq_remove_from_head(dev);
+		tqe = wilc_wlan_txq_remove_from_head(wilc, vmm_entries_ac[i]);
+		ac_pkt_num_to_chip[vmm_entries_ac[i]]++;
 		if (!tqe)
 			break;
 
@@ -649,8 +840,11 @@ int wilc_wlan_handle_txq(struct wilc *wilc, u32 *txq_count)
 		if (tqe->type == WILC_CFG_PKT) {
 			buffer_offset = ETH_CONFIG_PKT_HDR_OFFSET;
 		} else if (tqe->type == WILC_NET_PKT) {
+			int prio = tqe->q_num;
+
 			bssid = tqe->vif->bssid;
 			buffer_offset = ETH_ETHERNET_HDR_OFFSET;
+			memcpy(&txb[offset + 4], &prio, sizeof(prio));
 			memcpy(&txb[offset + 8], bssid, 6);
 		} else {
 			buffer_offset = HOST_HDR_OFFSET;
@@ -668,6 +862,8 @@ int wilc_wlan_handle_txq(struct wilc *wilc, u32 *txq_count)
 			vif->ack_filter.pending_acks[tqe->ack_idx].txqe = NULL;
 		kfree(tqe);
 	} while (--entries);
+	for (i = 0; i < NQUEUES; i++)
+		wilc->txq[i].fw.count += ac_pkt_num_to_chip[i];
 
 	acquire_bus(wilc, WILC_BUS_ACQUIRE_AND_WAKEUP);
 
@@ -966,14 +1162,17 @@ void wilc_wlan_cleanup(struct net_device *dev)
 {
 	struct txq_entry_t *tqe;
 	struct rxq_entry_t *rqe;
+	u8 ac;
 	struct wilc_vif *vif = netdev_priv(dev);
 	struct wilc *wilc = vif->wilc;
 
 	wilc->quit = 1;
-	while ((tqe = wilc_wlan_txq_remove_from_head(dev))) {
-		if (tqe->tx_complete_func)
-			tqe->tx_complete_func(tqe->priv, 0);
-		kfree(tqe);
+	for (ac = 0; ac < NQUEUES; ac++) {
+		while ((tqe = wilc_wlan_txq_remove_from_head(wilc, ac))) {
+			if (tqe->tx_complete_func)
+				tqe->tx_complete_func(tqe->priv, 0);
+			kfree(tqe);
+		}
 	}
 
 	while ((rqe = wilc_wlan_rxq_remove(wilc)))