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

drivers/net/wireless/microchip/wilc1000/cfg80211.c

@@ -1709,7 +1709,7 @@ int wilc_cfg80211_init(struct wilc **wilc, struct device *dev, int io_type,
 {
 	struct wilc *wl;
 	struct wilc_vif *vif;
-	int ret;
+	int ret, i;
 
 	wl = wilc_create_wiphy(dev);
 	if (!wl)
@@ -1725,7 +1725,10 @@ int wilc_cfg80211_init(struct wilc **wilc, struct device *dev, int io_type,
 	wl->io_type = io_type;
 	wl->hif_func = ops;
 	wl->chip_ps_state = WILC_CHIP_WAKEDUP;
-	INIT_LIST_HEAD(&wl->txq_head.list);
+
+	for (i = 0; i < NQUEUES; i++)
+		INIT_LIST_HEAD(&wl->txq[i].txq_head.list);
+
 	INIT_LIST_HEAD(&wl->rxq_head.list);
 	INIT_LIST_HEAD(&wl->vif_list);
 

drivers/net/wireless/microchip/wilc1000/netdev.h

@@ -197,6 +197,14 @@ struct wilc_vif {
 	struct cfg80211_bss *bss;
 };
 
+struct wilc_tx_queue_status {
+	u8 buffer[AC_BUFFER_SIZE];
+	u16 end_index;
+	u16 cnt[NQUEUES];
+	u16 sum;
+	bool initialized;
+};
+
 struct wilc {
 	struct wiphy *wiphy;
 	const struct wilc_hif_func *hif_func;
@@ -245,9 +253,10 @@ struct wilc {
 	u32 rx_buffer_offset;
 	u8 *tx_buffer;
 
-	struct txq_entry_t txq_head;
+	struct txq_handle txq[NQUEUES];
 	int txq_entries;
 
+	struct wilc_tx_queue_status tx_q_limit;
 	struct rxq_entry_t rxq_head;
 
 	const struct firmware *firmware;

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;
 
+	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, 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)
+	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;
+				}
+
+				if (tqe_q[ac]->type == WILC_CFG_PKT)
 					vmm_sz = ETH_CONFIG_PKT_HDR_OFFSET;
-		else if (tqe->type == WILC_NET_PKT)
+				else if (tqe_q[ac]->type == WILC_NET_PKT)
 					vmm_sz = ETH_ETHERNET_HDR_OFFSET;
 				else
 					vmm_sz = HOST_HDR_OFFSET;
 
-		vmm_sz += tqe->buffer_size;
+				vmm_sz += tqe_q[ac]->buffer_size;
 				vmm_sz = ALIGN(vmm_sz, 4);
 
-		if ((sum + vmm_sz) > WILC_TX_BUFF_SIZE)
+				if ((sum + vmm_sz) > WILC_TX_BUFF_SIZE) {
+					max_size_over = 1;
 					break;
-
+				}
 				vmm_table[i] = vmm_sz / 4;
-		if (tqe->type == WILC_CFG_PKT)
+				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 = wilc_wlan_txq_get_next(wilc, tqe);
+				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,15 +1162,18 @@ 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))) {
+	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)))
 		kfree(rqe);

drivers/net/wireless/microchip/wilc1000/wlan.h

@@ -207,6 +207,18 @@
 
 #define MODALIAS		"WILC_SPI"
 
+#define NQUEUES			4
+#define AC_BUFFER_SIZE		1000
+
+#define VO_AC_COUNT_FIELD		GENMASK(31, 25)
+#define VO_AC_ACM_STAT_FIELD		BIT(24)
+#define VI_AC_COUNT_FIELD		GENMASK(23, 17)
+#define VI_AC_ACM_STAT_FIELD		BIT(16)
+#define BE_AC_COUNT_FIELD		GENMASK(15, 9)
+#define BE_AC_ACM_STAT_FIELD		BIT(8)
+#define BK_AC_COUNT_FIELD		GENMASK(7, 3)
+#define BK_AC_ACM_STAT_FIELD		BIT(1)
+
 #define WILC_PKT_HDR_CONFIG_FIELD	BIT(31)
 #define WILC_PKT_HDR_OFFSET_FIELD	GENMASK(30, 22)
 #define WILC_PKT_HDR_TOTAL_LEN_FIELD	GENMASK(21, 11)
@@ -295,10 +307,17 @@
  *      Tx/Rx Queue Structure
  *
  ********************************************/
+enum ip_pkt_priority {
+	AC_VO_Q = 0,
+	AC_VI_Q = 1,
+	AC_BE_Q = 2,
+	AC_BK_Q = 3
+};
 
 struct txq_entry_t {
 	struct list_head list;
 	int type;
+	u8 q_num;
 	int ack_idx;
 	u8 *buffer;
 	int buffer_size;
@@ -308,6 +327,17 @@ struct txq_entry_t {
 	void (*tx_complete_func)(void *priv, int status);
 };
 
+struct txq_fw_recv_queue_stat {
+	u8 acm;
+	u8 count;
+};
+
+struct txq_handle {
+	struct txq_entry_t txq_head;
+	u16 count;
+	struct txq_fw_recv_queue_stat fw;
+};
+
 struct rxq_entry_t {
 	struct list_head list;
 	u8 *buffer;