[PATCH v14 03/23] bus/dpaa: fix FQD dest wq channel decoding

From: Prashant Gupta <[email protected]>

qman_shutdown_fq() decoded the FQD dest_wq field as:

	channel = dest_wq & 0x7;
	wq      = dest_wq >> 3;

but in the hardware descriptor the work queue is bits 2:0 and the
channel is bits 15:3. The old code therefore extracted a 0-7 value as
the channel and the real channel as the work queue. qman_shutdown_fq()
compares the channel against the pool-channel range to decide how to
drain the queue, and that comparison can never succeed with a 0-7
value, so pool-channel FQs were not drained correctly on shutdown.

Fixes: c47ff048b99a ("bus/dpaa: add QMAN driver core routines")
Cc: [email protected]

Signed-off-by: Prashant Gupta <[email protected]>
---
 drivers/bus/dpaa/base/qbman/qman.c | 14 ++------------
 drivers/bus/dpaa/base/qbman/qman.h | 23 ++++++++++++++++++++++-
 2 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/drivers/bus/dpaa/base/qbman/qman.c b/drivers/bus/dpaa/base/qbman/qman.c
index d289df2d33..931d487de4 100644
--- a/drivers/bus/dpaa/base/qbman/qman.c
+++ b/drivers/bus/dpaa/base/qbman/qman.c
@@ -2707,14 +2707,6 @@ int qman_delete_cgr(struct qman_cgr *cgr)
 	return ret;
 }
 
-#define GENMASK(h, l) \
-	(((~0U) >> (sizeof(unsigned int) * 8 - ((h) - (l) + 1))) << (l))
-
-/* 'fqid' is a 24-bit field in every h/w descriptor */
-#define QM_FQID_MASK    GENMASK(23, 0)
-#define qm_fqid_set(p, v) ((p)->fqid = cpu_to_be32((v) & QM_FQID_MASK))
-#define qm_fqid_get(p)    (be32_to_cpu((p)->fqid) & QM_FQID_MASK)
-
 static int
 _qm_mr_consume_and_match_verb(struct qm_portal *p, int v)
 {
@@ -2801,7 +2793,6 @@ qman_shutdown_fq(u32 fqid)
 	u32 res;
 	u8 state;
 	u32 channel, wq;
-	u16 dest_wq;
 
 	DPAA_BUS_DEBUG("In shutdown for queue = %x", fqid);
 	p = get_affine_portal();
@@ -2831,9 +2822,8 @@ qman_shutdown_fq(u32 fqid)
 	}
 
 	/* Need to store these since the MCR gets reused */
-	dest_wq = be16_to_cpu(mcr->queryfq.fqd.dest_wq);
-	channel = dest_wq & 0x7;
-	wq = dest_wq >> 3;
+	channel = qm_fqd_get_chan(&mcr->queryfq.fqd);
+	wq = qm_fqd_get_wq(&mcr->queryfq.fqd);
 
 	switch (state) {
 	case QM_MCR_NP_STATE_TEN_SCHED:
diff --git a/drivers/bus/dpaa/base/qbman/qman.h b/drivers/bus/dpaa/base/qbman/qman.h
index 43a16d1e3b..bd97689a91 100644
--- a/drivers/bus/dpaa/base/qbman/qman.h
+++ b/drivers/bus/dpaa/base/qbman/qman.h
@@ -1,12 +1,15 @@
 /* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
  *
  * Copyright 2008-2016 Freescale Semiconductor Inc.
- * Copyright 2017 NXP
+ * Copyright 2017,2026 NXP
  *
  */
 
 #include "qman_priv.h"
 
+#define GENMASK(h, l) \
+	(((~0U) >> (sizeof(u32) * 8 - ((h) - (l) + 1))) << (l))
+
 /***************************/
 /* Portal register assists */
 /***************************/
@@ -42,6 +45,14 @@
 #define QM_CL_RR0		0x3900
 #define QM_CL_RR1		0x3940
 
+#define QM_FQD_CHAN_OFF                3
+#define QM_FQD_WQ_MASK         GENMASK(2, 0)
+/* 'fqid' is a 24-bit field in every h/w descriptor */
+#define QM_FQID_MASK    GENMASK(23, 0)
+
+#define qm_fqid_set(p, v) ((p)->fqid = cpu_to_be32((v) & QM_FQID_MASK))
+#define qm_fqid_get(p)    (be32_to_cpu((p)->fqid) & QM_FQID_MASK)
+
 /* BTW, the drivers (and h/w programming model) already obtain the required
  * synchronisation for portal accesses via lwsync(), hwsync(), and
  * data-dependencies. Use of barrier()s or other order-preserving primitives
@@ -911,3 +922,13 @@ static inline void __qm_isr_write(struct qm_portal *portal, enum qm_isr_reg n,
 	__qm_out(&portal->addr, QM_REG_ISR + (n << 2), val);
 #endif
 }
+
+static inline int qm_fqd_get_chan(const struct qm_fqd *fqd)
+{
+	return be16_to_cpu(fqd->dest_wq) >> QM_FQD_CHAN_OFF;
+}
+
+static inline int qm_fqd_get_wq(const struct qm_fqd *fqd)
+{
+	return be16_to_cpu(fqd->dest_wq) & QM_FQD_WQ_MASK;
+}
-- 
2.25.1