From ec3ad06bb0d1337fed719db05263328906a5d156 Mon Sep 17 00:00:00 2001
From: Ethan Kim <kims@cypress.com>
Date: Wed, 22 Mar 2023 18:20:00 -0500
Subject: [PATCH 156/301] brcmfmac: Added DSCP to WMM UP mapping

Signed-off-by: Ethan Kim <kims@cypress.com>
---
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c
index 1065951cb..b9cd230b6 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c
@@ -81,6 +81,10 @@ static int brcmf_max_pm;
 module_param_named(max_pm, brcmf_max_pm, int, 0);
 MODULE_PARM_DESC(max_pm, "Use max power management mode by default");
 
+int brcmf_pkt_prio_enable;
+module_param_named(pkt_prio, brcmf_pkt_prio_enable, int, 0);
+MODULE_PARM_DESC(pkt_prio, "Support for update the packet priority");
+
 #ifdef DEBUG
 /* always succeed brcmf_bus_started() */
 static int brcmf_ignore_probe_fail;
@@ -623,6 +627,7 @@ struct brcmf_mp_device *brcmf_get_module_param(struct device *dev,
 	settings->fw_ap_select = !!brcmf_fw_ap_select;
 	settings->disable_6ghz = !!brcmf_disable_6ghz;
 	settings->sdio_in_isr = !!brcmf_sdio_in_isr;
+	settings->pkt_prio = !!brcmf_pkt_prio_enable;
 	settings->sdio_rxf_in_kthread_enabled = !!brcmf_sdio_rxf_in_kthread;
 
 	if (bus_type == BRCMF_BUSTYPE_SDIO)
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h
index 7e36d0b3b..8a6657ab0 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h
@@ -46,6 +46,7 @@ extern struct brcmf_mp_global_t brcmf_mp_global;
  * @sdio_in_isr: Handle SDIO DPC in ISR.
  * @country_codes: If available, pointer to struct for translating country codes
  * @bus: Bus specific platform data. Only SDIO at the mmoment.
+ * @pkt_prio: Support customer dscp to WMM up mapping.
  */
 struct brcmf_mp_device {
 	bool		p2p_enable;
@@ -70,6 +71,7 @@ struct brcmf_mp_device {
 	union {
 		struct brcmfmac_sdio_pd sdio;
 	} bus;
+	bool		pkt_prio;
 };
 
 void brcmf_c_set_joinpref_default(struct brcmf_if *ifp);
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
index b7b26bb8f..2204902df 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
@@ -16,6 +16,7 @@
 #include <net/ipv6.h>
 #include <brcmu_utils.h>
 #include <brcmu_wifi.h>
+#include <defs.h>
 
 #include "core.h"
 #include "bus.h"
@@ -73,6 +74,15 @@ struct wlc_d11rxhdr {
 
 #define BRCMF_STA_NULL ((struct brcmf_sta *)NULL)
 
+/* dscp exception format {dscp hex, up}  */
+struct cfg80211_dscp_exception dscp_excpt[] = {
+{DSCP_EF, 6}, {DSCP_CS4, 5}, {DSCP_AF41, 5}, {DSCP_CS3, 4} };
+
+/* dscp range : up[0 ~ 7] */
+struct cfg80211_dscp_range dscp_range[8] = {
+{0, 7}, {8, 15}, {16, 23}, {24, 31},
+{32, 39}, {40, 47}, {48, 55}, {56, 63} };
+
 char *brcmf_ifname(struct brcmf_if *ifp)
 {
 	if (!ifp)
@@ -367,9 +377,13 @@ static netdev_tx_t brcmf_netdev_start_xmit(struct sk_buff *skb,
 	if (eh->h_proto == htons(ETH_P_PAE))
 		atomic_inc(&ifp->pend_8021x_cnt);
 
+	/* Look into dscp to WMM UP mapping with cfg80211_qos_map */
+	if (drvr->settings->pkt_prio) {
+		skb->priority = cfg80211_classify8021d(skb, drvr->qos_map);
 	/* determine the priority */
-	if ((skb->priority == 0) || (skb->priority > 7))
+	} else if ((skb->priority == 0) || (skb->priority > 7)) {
 		skb->priority = cfg80211_classify8021d(skb, NULL);
+	}
 
 	/* set pacing shift for packet aggregation */
 	sk_pacing_shift_update(skb->sk, 8);
@@ -1217,6 +1231,7 @@ static int brcmf_bus_started(struct brcmf_pub *drvr, struct cfg80211_ops *ops)
 	struct brcmf_bus *bus_if = drvr->bus_if;
 	struct brcmf_if *ifp;
 	struct brcmf_if *p2p_ifp;
+	int i, num;
 
 	brcmf_dbg(TRACE, "\n");
 
@@ -1256,6 +1271,22 @@ static int brcmf_bus_started(struct brcmf_pub *drvr, struct cfg80211_ops *ops)
 		goto fail;
 	}
 
+	/* update custom DSCP to PRIO mapping */
+	if (drvr->settings->pkt_prio) {
+		drvr->qos_map = kzalloc(sizeof(struct cfg80211_qos_map), GFP_KERNEL);
+		if (!drvr->qos_map) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+		num = sizeof(dscp_excpt) / (sizeof(struct cfg80211_dscp_exception));
+		drvr->qos_map->num_des = num;
+		for (i = 0; i < num; i++) {
+			drvr->qos_map->dscp_exception[i].dscp = dscp_excpt[i].dscp;
+			drvr->qos_map->dscp_exception[i].up = dscp_excpt[i].up;
+		}
+		memcpy(drvr->qos_map->up, dscp_range, sizeof(dscp_range[8]));
+	}
+
 	ret = brcmf_net_attach(ifp, false);
 
 	if ((!ret) && (drvr->settings->p2p_enable)) {
@@ -1470,6 +1501,11 @@ void brcmf_detach(struct device *dev)
 	}
 	brcmf_bus_stop(drvr->bus_if);
 
+	if (drvr->settings->pkt_prio) {
+		kfree(drvr->qos_map);
+		drvr->qos_map = NULL;
+	}
+
 	brcmf_fweh_detach(drvr);
 	brcmf_proto_detach(drvr);
 
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.h
index 155c233cd..13223e211 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.h
@@ -146,6 +146,7 @@ struct brcmf_pub {
 	u8 sta_mac_idx;
 	const struct brcmf_fwvid_ops *vops;
 	void *vdata;
+	struct cfg80211_qos_map *qos_map;
 };
 
 /* forward declarations */
diff --git a/drivers/net/wireless/broadcom/brcm80211/include/defs.h b/drivers/net/wireless/broadcom/brcm80211/include/defs.h
index 9e7e6116e..fb25bdf54 100644
--- a/drivers/net/wireless/broadcom/brcm80211/include/defs.h
+++ b/drivers/net/wireless/broadcom/brcm80211/include/defs.h
@@ -35,6 +35,44 @@
 #define	MAXPRIO			7
 #define NUMPRIO			(MAXPRIO + 1)
 
+/* DSCP type definitions for RFC4594 */
+/* DF: Standard (RFC2474) */
+#define DSCP_DF		0x00u
+/* AF1x: High-Throughput Data (RFC2597) */
+#define DSCP_AF11	0x0Au
+#define DSCP_AF12	0x0Cu
+#define DSCP_AF13	0x0Eu
+/* CS1: Low-Priority Data (RFC3662) */
+#define DSCP_CS1	0x08u
+/* AF2x: Low-Latency Data (RFC2597) */
+#define DSCP_AF21	0x12u
+#define DSCP_AF22	0x14u
+#define DSCP_AF23	0x16u
+/* CS2: OAM (RFC2474) */
+#define DSCP_CS2	0x10u
+/* AF3x: Multimedia Streaming (RFC2597) */
+#define DSCP_AF31	0x1Au
+#define DSCP_AF32	0x1Cu
+#define DSCP_AF33	0x1Eu
+/* CS3: Broadcast Video (RFC2474) */
+#define DSCP_CS3	0x18u
+/* AF4x: Multimedia Conferencing (RFC2597) */
+#define DSCP_AF41	0x22u
+#define DSCP_AF42	0x24u
+#define DSCP_AF43	0x26u
+/* CS4: Real-Time Interactive (RFC2474) */
+#define DSCP_CS4	0x20u
+/* CS5: Signaling (RFC2474) */
+#define DSCP_CS5	0x28u
+/* VA: VOCIE-ADMIT (RFC5865) */
+#define DSCP_VA		0x2Cu
+/* EF: Telephony (RFC3246) */
+#define DSCP_EF		0x2Eu
+/* CS6: Network Control (RFC2474) */
+#define DSCP_CS6	0x30u
+/* CS7: Network Control (RFC2474) */
+#define DSCP_CS7	0x38u
+
 #define WL_NUMRATES		16	/* max # of rates in a rateset */
 
 #define BRCM_CNTRY_BUF_SZ	4	/* Country string is 3 bytes + NUL */
