From 51e44e49161b7638f375f438868c4437d92f6c4d Mon Sep 17 00:00:00 2001
From: Carter Chen <carter.chen@infineon.com>
Date: Mon, 10 Apr 2023 02:26:53 -0500
Subject: [PATCH 153/301] non-upstream: sdio t-put tuning

verifying on SDIO In-Band interrupt only as of present

add module parameter "sdio_rxf_thread=1" to enable it.

1.) Handling rx data frame of SDIO intf in a kthread.
2.) fixing a potential null pointer dereference in sdio_isr
    which dues to a data structure is not ready.

Fixes: CYW55560A1-1343

Signed-off-by: Carter Chen <carter.chen@infineon.com>
---
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h
index 8c3613ff2..6c7e63cd4 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h
@@ -291,8 +291,8 @@ static inline void brcmf_bus_remove(struct brcmf_bus *bus)
  */

 /* Receive frame for delivery to OS.  Callee disposes of rxp. */
-void brcmf_rx_frame(struct device *dev, struct sk_buff *rxp, bool handle_event,
-		    bool inirq);
+struct sk_buff *brcmf_rx_frame(struct device *dev, struct sk_buff *rxp, bool handle_event,
+			       bool inirq);
 /* Receive async event packet from firmware. Callee disposes of rxp. */
 void brcmf_rx_event(struct device *dev, struct sk_buff *rxp);

diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c
index 25cac7f08..1065951cb 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c
@@ -100,6 +100,10 @@ static int brcmf_sdio_in_isr;
 module_param_named(sdio_in_isr, brcmf_sdio_in_isr, int, 0400);
 MODULE_PARM_DESC(sdio_in_isr, "Handle SDIO DPC in ISR");

+static int brcmf_sdio_rxf_in_kthread;
+module_param_named(sdio_rxf_thread, brcmf_sdio_rxf_in_kthread, int, 0400);
+MODULE_PARM_DESC(sdio_rxf_thread, "SDIO RX Frame in Kthread");
+
 static struct brcmfmac_platform_data *brcmfmac_pdata;
 struct brcmf_mp_global_t brcmf_mp_global;

@@ -619,6 +623,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->sdio_rxf_in_kthread_enabled = !!brcmf_sdio_rxf_in_kthread;

 	if (bus_type == BRCMF_BUSTYPE_SDIO)
 		settings->bus.sdio.txglomsz = brcmf_sdiod_txglomsz;
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h
index 559e0193f..7e36d0b3b 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h
@@ -60,6 +60,7 @@ struct brcmf_mp_device {
 	bool		fw_ap_select;
 	bool		disable_6ghz;
 	bool		sdio_in_isr;
+	bool		sdio_rxf_in_kthread_enabled;
 	struct brcmfmac_pd_cc *country_codes;
 	const char	*board_type;
 	unsigned char	mac[ETH_ALEN];
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
index 4e2f5e0e9..e83cd3b36 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
@@ -508,8 +508,8 @@ static int brcmf_rx_hdrpull(struct brcmf_pub *drvr, struct sk_buff *skb,
 	return 0;
 }

-void brcmf_rx_frame(struct device *dev, struct sk_buff *skb, bool handle_event,
-		    bool inirq)
+struct sk_buff *brcmf_rx_frame(struct device *dev, struct sk_buff *skb, bool handle_event,
+			       bool inirq)
 {
 	struct brcmf_if *ifp;
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
@@ -518,7 +518,7 @@ void brcmf_rx_frame(struct device *dev, struct sk_buff *skb, bool handle_event,
 	brcmf_dbg(DATA, "Enter: %s: rxp=%p\n", dev_name(dev), skb);

 	if (brcmf_rx_hdrpull(drvr, skb, &ifp))
-		return;
+		return NULL;

 	if (brcmf_proto_is_reorder_skb(skb)) {
 		brcmf_proto_rxreorder(ifp, skb);
@@ -530,8 +530,14 @@ void brcmf_rx_frame(struct device *dev, struct sk_buff *skb, bool handle_event,
 			brcmf_fweh_process_skb(ifp->drvr, skb,
 					       BCMILCP_SUBTYPE_VENDOR_LONG, gfp);
 		}
-		brcmf_netif_rx(ifp, skb);
+
+		/* if sdio_rxf_in_kthread, enqueue it and process it later. */
+		if (brcmf_feat_is_sdio_rxf_in_kthread(drvr))
+			return skb;
+		else
+			brcmf_netif_rx(ifp, skb);
 	}
+	return NULL;
 }

 void brcmf_rx_event(struct device *dev, struct sk_buff *skb)
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
index 9dc51639b..d2e18f67d 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
@@ -392,8 +392,7 @@ bool brcmf_feat_is_6ghz_enabled(struct brcmf_if *ifp)
 	return (!ifp->drvr->settings->disable_6ghz);
 }

-bool brcmf_feat_sdio_in_isr(struct brcmf_if *ifp)
+bool brcmf_feat_is_sdio_rxf_in_kthread(struct brcmf_pub *drvr)
 {
-	return ifp->drvr->settings->sdio_in_isr;
+	return drvr->settings->sdio_rxf_in_kthread_enabled;
 }
-
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
index 1d2be55e5..3e669de02 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
@@ -142,12 +142,10 @@ bool brcmf_feat_is_quirk_enabled(struct brcmf_if *ifp,
 bool brcmf_feat_is_6ghz_enabled(struct brcmf_if *ifp);

 /**
- * brcmf_feat_sdio_in_isr() - handle SDIO DPC handler in ISR
+ * brcmf_feat_is_sdio_rxf_in_kthread() - handle SDIO Rx frame in kthread.
  *
- * @ifp: interface instance.
- *
- * Return: true if handle SDIO DPC in ISR is enabled; otherwise false.
+ * @drvr: driver instance.
  */
-bool brcmf_feat_sdio_in_isr(struct brcmf_if *ifp);
+bool brcmf_feat_is_sdio_rxf_in_kthread(struct brcmf_pub *drvr);

 #endif /* _BRCMF_FEATURE_H */
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c
index 3542ee5bb..d63b6fabe 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c
@@ -346,6 +346,9 @@ static int brcmf_sdio_f2_ready(struct brcmf_sdio *bus);
 static int brcmf_ulp_event_notify(struct brcmf_if *ifp,
 				  const struct brcmf_event_msg *evtmsg,
 				  void *data);
+static void
+brcmf_sched_rxf(struct brcmf_sdio *bus, struct sk_buff *skb);
+

 #ifdef DEBUG
 /* Device console log buffer state */
@@ -418,6 +421,11 @@ struct brcmf_sdio_hdrinfo {
 	u16 tail_pad;
 };

+struct task_ctl {
+	struct	task_struct *p_task;
+	struct	semaphore sema;
+};
+
 /*
  * hold counter variables
  */
@@ -545,6 +553,12 @@ struct brcmf_sdio {
 	u16 head_align;		/* buffer pointer alignment */
 	u16 sgentry_align;	/* scatter-gather buffer alignment */
 	struct mutex sdsem;
+	#define MAXSKBPEND 1024
+	struct sk_buff *skbbuf[MAXSKBPEND];
+	u32 store_idx;
+	u32 sent_idx;
+	struct task_ctl	thr_rxf_ctl;
+	spinlock_t	rxf_lock; /* lock for rxf idx protection */
 };

 /* clkstate */
@@ -1537,16 +1551,76 @@ static void brcmf_sdio_hdpack(struct brcmf_sdio *bus, u8 *header,
 	trace_brcmf_sdpcm_hdr(SDPCM_TX + !!(bus->txglom), header);
 }

+static inline int brcmf_rxf_enqueue(struct brcmf_sdio *bus, struct sk_buff *skb)
+{
+	u32 store_idx;
+	u32 sent_idx;
+
+	if (!skb) {
+		brcmf_err("NULL skb!!!\n");
+		return -EINVAL;
+	}
+
+	spin_lock_bh(&bus->rxf_lock);
+	store_idx = bus->store_idx;
+	sent_idx = bus->sent_idx;
+	if (bus->skbbuf[store_idx]) {
+		/* Make sure the previous packets are processed */
+		spin_unlock_bh(&bus->rxf_lock);
+		brcmf_err("pktbuf not consumed %p, store idx %d sent idx %d\n",
+			  skb, store_idx, sent_idx);
+		msleep(1000);
+		return -EINVAL;
+	}
+	brcmf_dbg(DATA, "Store SKB %p. idx %d -> %d\n",
+		  skb, store_idx, (store_idx + 1) & (MAXSKBPEND - 1));
+	bus->skbbuf[store_idx] = skb;
+	bus->store_idx = (store_idx + 1) & (MAXSKBPEND - 1);
+	spin_unlock_bh(&bus->rxf_lock);
+
+	return 0;
+}
+
+static struct sk_buff *brcmf_rxf_dequeue(struct brcmf_sdio *bus)
+{
+	u32 store_idx;
+	u32 sent_idx;
+	struct sk_buff *skb;
+
+	spin_lock_bh(&bus->rxf_lock);
+
+	store_idx = bus->store_idx;
+	sent_idx = bus->sent_idx;
+	skb = bus->skbbuf[sent_idx];
+
+	if (!skb) {
+		spin_unlock_bh(&bus->rxf_lock);
+		brcmf_err("Dequeued packet is NULL, store idx %d sent idx %d\n",
+			  store_idx, sent_idx);
+		return NULL;
+	}
+
+	bus->skbbuf[sent_idx] = NULL;
+	bus->sent_idx = (sent_idx + 1) & (MAXSKBPEND - 1);
+
+	brcmf_dbg(DATA, "dequeue (%p), sent idx %d\n",
+		  skb, sent_idx);
+
+	spin_unlock_bh(&bus->rxf_lock);
+
+	return skb;
+}
+
 static u8 brcmf_sdio_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 {
 	u16 dlen, totlen;
 	u8 *dptr, num = 0;
 	u16 sublen;
 	struct sk_buff *pfirst, *pnext;
-
+	struct sk_buff *skb_head = NULL, *skb_prev = NULL, *skb_to_rxfq = NULL;
+	struct brcmf_bus *bus_if = dev_get_drvdata(bus->sdiodev->dev);
 	int errcode;
 	u8 doff;
-
 	struct brcmf_sdio_hdrinfo rd_new;

 	/* If packets, issue read(s) and send up packet chain */
@@ -1740,16 +1814,31 @@ static u8 brcmf_sdio_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 					   pfirst->len, pfirst->next,
 					   pfirst->prev);
 			skb_unlink(pfirst, &bus->glom);
-			if (brcmf_sdio_fromevntchan(&dptr[SDPCM_HWHDR_LEN]))
+			if (brcmf_sdio_fromevntchan(&dptr[SDPCM_HWHDR_LEN])) {
 				brcmf_rx_event(bus->sdiodev->dev, pfirst);
-			else
-				brcmf_rx_frame(bus->sdiodev->dev, pfirst,
-					       false, false);
+				skb_to_rxfq = NULL;
+			} else {
+				skb_to_rxfq = brcmf_rx_frame(bus->sdiodev->dev, pfirst,
+							     false, false);
+			}
+
+			if (brcmf_feat_is_sdio_rxf_in_kthread(bus_if->drvr) && skb_to_rxfq) {
+				if (!skb_head)
+					skb_head = skb_to_rxfq;
+				else
+					skb_prev->next = skb_to_rxfq;
+
+				skb_prev = skb_to_rxfq;
+			}
 			bus->sdcnt.rxglompkts++;
 		}

 		bus->sdcnt.rxglomframes++;
 	}
+
+	if (brcmf_feat_is_sdio_rxf_in_kthread(bus_if->drvr) && skb_head)
+		brcmf_sched_rxf(bus, skb_head);
+
 	return num;

 frame_error_handle:
@@ -1900,6 +1989,8 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
 	uint rxcount = 0;	/* Total frames read */
 	struct brcmf_sdio_hdrinfo *rd = &bus->cur_read, rd_new;
 	u8 head_read = 0;
+	struct sk_buff *skb_to_rxfq = NULL, *skb_head = NULL, *skb_prev = NULL;
+	struct brcmf_bus *bus_if = dev_get_drvdata(bus->sdiodev->dev);

 	brcmf_dbg(SDIO, "Enter\n");

@@ -2082,13 +2173,25 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
 		__skb_trim(pkt, rd->len);
 		skb_pull(pkt, rd->dat_offset);

-		if (pkt->len == 0)
+		if (pkt->len == 0) {
 			brcmu_pkt_buf_free_skb(pkt);
-		else if (rd->channel == SDPCM_EVENT_CHANNEL)
+			skb_to_rxfq = NULL;
+		} else if (rd->channel == SDPCM_EVENT_CHANNEL) {
 			brcmf_rx_event(bus->sdiodev->dev, pkt);
-		else
-			brcmf_rx_frame(bus->sdiodev->dev, pkt,
-				       false, false);
+			skb_to_rxfq = NULL;
+		} else {
+			skb_to_rxfq = brcmf_rx_frame(bus->sdiodev->dev, pkt,
+						     false, false);
+		}
+
+		if (brcmf_feat_is_sdio_rxf_in_kthread(bus_if->drvr) && skb_to_rxfq) {
+			if (!skb_head)
+				skb_head = skb_to_rxfq;
+			else
+				skb_prev->next = skb_to_rxfq;
+
+			skb_prev = skb_to_rxfq;
+		}

 		/* prepare the descriptor for the next read */
 		rd->len = rd->len_nxtfrm << 4;
@@ -2097,6 +2200,9 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
 		rd->channel = SDPCM_EVENT_CHANNEL;
 	}

+	if (brcmf_feat_is_sdio_rxf_in_kthread(bus_if->drvr) && skb_head)
+		brcmf_sched_rxf(bus, skb_head);
+
 	rxcount = maxframes - rxleft;
 	/* Message if we hit the limit */
 	if (!rxleft)
@@ -2528,6 +2634,12 @@ static void brcmf_sdio_bus_stop(struct device *dev)
 		bus->watchdog_tsk = NULL;
 	}

+	if (bus->thr_rxf_ctl.p_task) {
+		send_sig(SIGTERM, bus->thr_rxf_ctl.p_task, 1);
+		kthread_stop(bus->thr_rxf_ctl.p_task);
+		bus->thr_rxf_ctl.p_task = NULL;
+	}
+
 	if (sdiodev->state != BRCMF_SDIOD_NOMEDIUM) {
 		sdio_claim_host(sdiodev->func1);

@@ -3880,9 +3992,6 @@ void brcmf_sdio_trigger_dpc(struct brcmf_sdio *bus)

 void brcmf_sdio_isr(struct brcmf_sdio *bus, bool in_isr)
 {
-	struct brcmf_bus *bus_if = dev_get_drvdata(bus->sdiodev->dev);
-	struct brcmf_if *ifp = bus_if->drvr->iflist[0];
-
 	brcmf_dbg(TRACE, "Enter\n");

 	if (!bus) {
@@ -3903,7 +4012,7 @@ void brcmf_sdio_isr(struct brcmf_sdio *bus, bool in_isr)
 	if (!bus->intr)
 		brcmf_err("isr w/o interrupt configured!\n");

-	if (brcmf_feat_sdio_in_isr(ifp)) {
+	if (bus->sdiodev->settings->sdio_in_isr) {
 		if (!mutex_trylock(&bus->sdsem)) {
 			bus->dpc_triggered = true;
 			queue_work(bus->brcmf_wq, &bus->datawork);
@@ -4014,10 +4123,8 @@ static void brcmf_sdio_dataworker(struct work_struct *work)
 {
 	struct brcmf_sdio *bus = container_of(work, struct brcmf_sdio,
 					      datawork);
-	struct brcmf_bus *bus_if = dev_get_drvdata(bus->sdiodev->dev);
-	struct brcmf_if *ifp = bus_if->drvr->iflist[0];

-	if (brcmf_feat_sdio_in_isr(ifp)) {
+	if (bus->sdiodev->settings->sdio_in_isr) {
 		if (mutex_trylock(&bus->sdsem)) {
 			bus->dpc_running = true;

@@ -4375,6 +4482,84 @@ brcmf_sdio_probe_attach(struct brcmf_sdio *bus)
 	return false;
 }

+static void
+brcmf_sched_rxf(struct brcmf_sdio *bus, struct sk_buff *skb)
+{
+	brcmf_dbg(SDIO, "Enter\n");
+	do {
+		if (!brcmf_rxf_enqueue(bus, skb)) {
+			break;
+		} else {
+			brcmf_err("brcmf_rxf_enqueue failed\n");
+			goto done;
+		}
+	} while (1);
+
+	if (bus->thr_rxf_ctl.p_task)
+		up(&bus->thr_rxf_ctl.sema);
+
+done:
+	return;
+}
+
+static int
+brcmf_sdio_rxf_thread(void *data)
+{
+	struct brcmf_sdio *bus = (struct brcmf_sdio *)data;
+	struct sched_param param;
+
+	allow_signal(SIGTERM);
+	/* This thread doesn't need any user-level access,
+	 * so get rid of all our resources
+	 */
+	memset(&param, 0, sizeof(struct sched_param));
+	param.sched_priority = 1;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(5, 9, 0))
+	if (param.sched_priority >= MAX_RT_PRIO / 2)
+		/* If the priority is MAX_RT_PRIO/2 or higher,
+		 * it is considered as high priority.
+		 * sched_priority of FIFO task dosen't
+		 * exceed MAX_RT_PRIO/2.
+		 */
+		sched_set_fifo(current);
+	else
+		/* For when you don't much care about FIFO,
+		 * but want to be above SCHED_NORMAL.
+		 */
+		sched_set_fifo_low(current);
+#else
+	sched_setscheduler(current, SCHED_FIFO, &param);
+#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(5, 9, 0) */
+
+
+	while (1) {
+		if (kthread_should_stop())
+			break;
+
+		if (down_interruptible(&bus->thr_rxf_ctl.sema) == 0) {
+			struct sk_buff *skb = NULL;
+
+			smp_mb();/* ensure skb null */
+			skb = brcmf_rxf_dequeue(bus);
+			if (!skb) {
+				brcmf_err("nothing is dequeued, thread terminate\n");
+				break;
+			}
+
+			while (skb) {
+				struct sk_buff *skbnext = skb->next;
+
+				skb->next = NULL;
+				netif_rx(skb);
+				skb = skbnext;
+			}
+		} else {
+			break;
+		}
+	}
+	return 0;
+}
+
 static int
 brcmf_sdio_watchdog_thread(void *data)
 {
@@ -4801,6 +4986,20 @@ struct brcmf_sdio *brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev)
 	/* Initialize thread based operation and lock */
 	mutex_init(&bus->sdsem);

+	/* too early to have drvr */
+	if (sdiodev->settings->sdio_rxf_in_kthread_enabled) {
+		memset(&bus->skbbuf[0], 0, sizeof(void *) * MAXSKBPEND);
+		sema_init(&bus->thr_rxf_ctl.sema, 0);
+		spin_lock_init(&bus->rxf_lock);
+		bus->thr_rxf_ctl.p_task = kthread_run(brcmf_sdio_rxf_thread,
+						      bus, "brcmf_rxf/%s",
+						      dev_name(&sdiodev->func1->dev));
+		if (IS_ERR(bus->thr_rxf_ctl.p_task)) {
+			brcmf_err("brcmf_sdio_rxf_thread failed to start\n");
+			bus->thr_rxf_ctl.p_task = NULL;
+		}
+	}
+
 	/* Set up the watchdog timer */
 	timer_setup(&bus->timer, brcmf_sdio_watchdog, 0);
 	/* Initialize watchdog thread */
@@ -4879,6 +5078,12 @@ void brcmf_sdio_remove(struct brcmf_sdio *bus)
 			bus->watchdog_tsk = NULL;
 		}

+		if (bus->thr_rxf_ctl.p_task) {
+			send_sig(SIGTERM, bus->thr_rxf_ctl.p_task, 1);
+			kthread_stop(bus->thr_rxf_ctl.p_task);
+			bus->thr_rxf_ctl.p_task = NULL;
+		}
+
 		/* De-register interrupt handler */
 		brcmf_sdiod_intr_unregister(bus->sdiodev);

