4 files changed, 48 insertions, 13 deletions
diff --git a/drivers/net/fsl-mc/dpio/qbman_portal.c b/drivers/net/fsl-mc/dpio/qbman_portal.c
index 4b64c8a..86dc13d 100644
--- a/drivers/net/fsl-mc/dpio/qbman_portal.c
+++ b/drivers/net/fsl-mc/dpio/qbman_portal.c
@@ -25,7 +25,7 @@
 #define QBMAN_CENA_SWP_VDQCR   0x780
 
 /* Reverse mapping of QBMAN_CENA_SWP_DQRR() */
-#define QBMAN_IDX_FROM_DQRR(p) (((unsigned long)p & 0xff) >> 6)
+#define QBMAN_IDX_FROM_DQRR(p) (((unsigned long)p & 0x1ff) >> 6)
 
 /*******************************/
 /* Pre-defined attribute codes */
@@ -65,6 +65,7 @@ struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d)
 {
 	int ret;
 	struct qbman_swp *p = malloc(sizeof(struct qbman_swp));
+	u32 major = 0, minor = 0;
 
 	if (!p)
 		return NULL;
@@ -80,8 +81,20 @@ struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d)
 	atomic_set(&p->vdq.busy, 1);
 	p->vdq.valid_bit = QB_VALID_BIT;
 	p->dqrr.next_idx = 0;
+
+	qbman_version(&major, &minor);
+	if (!major) {
+		printf("invalid qbman version\n");
+		return NULL;
+	}
+
+	if (major >= 4 && minor >= 1)
+		p->dqrr.dqrr_size = QBMAN_VER_4_1_DQRR_SIZE;
+	else
+		p->dqrr.dqrr_size = QBMAN_VER_4_0_DQRR_SIZE;
+
 	p->dqrr.valid_bit = QB_VALID_BIT;
-	ret = qbman_swp_sys_init(&p->sys, d);
+	ret = qbman_swp_sys_init(&p->sys, d, p->dqrr.dqrr_size);
 	if (ret) {
 		free(p);
 		printf("qbman_swp_sys_init() failed %d\n", ret);
@@ -380,7 +393,7 @@ const struct ldpaa_dq *qbman_swp_dqrr_next(struct qbman_swp *s)
 	/* There's something there. Move "next_idx" attention to the next ring
 	 * entry (and prefetch it) before returning what we found. */
 	s->dqrr.next_idx++;
-	s->dqrr.next_idx &= QBMAN_DQRR_SIZE - 1; /* Wrap around at 4 */
+	s->dqrr.next_idx &= s->dqrr.dqrr_size - 1;/* Wrap around at dqrr_size */
 	/* TODO: it's possible to do all this without conditionals, optimise it
 	 * later. */
 	if (!s->dqrr.next_idx)
diff --git a/drivers/net/fsl-mc/dpio/qbman_portal.h b/drivers/net/fsl-mc/dpio/qbman_portal.h
index 86e2c3a..97a47aa 100644
--- a/drivers/net/fsl-mc/dpio/qbman_portal.h
+++ b/drivers/net/fsl-mc/dpio/qbman_portal.h
@@ -14,8 +14,8 @@
 /* Management command result codes */
 #define QBMAN_MC_RSLT_OK      0xf0
 
-/* TBD: as of QBMan 4.1, DQRR will be 8 rather than 4! */
-#define QBMAN_DQRR_SIZE 4
+#define QBMAN_VER_4_0_DQRR_SIZE 4
+#define QBMAN_VER_4_1_DQRR_SIZE 8
 
 
 /* --------------------- */
@@ -71,6 +71,7 @@ struct qbman_swp {
 	struct {
 		uint32_t next_idx;
 		uint32_t valid_bit;
+		uint8_t dqrr_size;
 	} dqrr;
 };
 
diff --git a/drivers/net/fsl-mc/dpio/qbman_private.h b/drivers/net/fsl-mc/dpio/qbman_private.h
index f1f16b8..73bbae3 100644
--- a/drivers/net/fsl-mc/dpio/qbman_private.h
+++ b/drivers/net/fsl-mc/dpio/qbman_private.h
@@ -11,6 +11,7 @@
 #include <linux/types.h>
 #include <asm/atomic.h>
 #include <malloc.h>
+#include <asm/arch/soc.h>
 #include <fsl-mc/fsl_qbman_base.h>
 
 #define QBMAN_CHECKING
@@ -166,4 +167,22 @@ static inline void dcbz(void *ptr)
 
 #define lwsync()
 
+void qbman_version(u32 *major, u32 *minor)
+{
+	u32 svr_dev_id;
+
+	/*
+	 * LS2080A SoC and its personalities has qbman cotroller version 4.0
+	 * New SoCs like LS2088A, LS1088A has qbman conroller version 4.1
+	 */
+	svr_dev_id = get_svr() >> 16;
+	if (svr_dev_id == SVR_DEV_LS2080A) {
+		*major = 4;
+		*minor = 0;
+	} else {
+		*major = 4;
+		*minor = 1;
+	}
+}
+
 #include "qbman_sys.h"
diff --git a/drivers/net/fsl-mc/dpio/qbman_sys.h b/drivers/net/fsl-mc/dpio/qbman_sys.h
index 7a537fb..72d74c5 100644
--- a/drivers/net/fsl-mc/dpio/qbman_sys.h
+++ b/drivers/net/fsl-mc/dpio/qbman_sys.h
@@ -239,16 +239,18 @@ static inline uint32_t qbman_set_swp_cfg(uint8_t max_fill, uint8_t wn,
 {
 	uint32_t reg;
 
-	reg = e32_uint8_t(20, 3, max_fill) | e32_uint8_t(16, 3, est) |
-		e32_uint8_t(12, 2, rpm) | e32_uint8_t(10, 2, dcm) |
-		e32_uint8_t(8, 2, epm) | e32_int(5, 1, sd) |
-		e32_int(4, 1, sp) | e32_int(3, 1, se) | e32_int(2, 1, dp) |
-		e32_int(1, 1, de) | e32_int(0, 1, ep) |	e32_uint8_t(14, 1, wn);
+	reg = e32_uint8_t(20, (uint32_t)(3 + (max_fill >> 3)), max_fill) |
+		e32_uint8_t(16, 3, est) | e32_uint8_t(12, 2, rpm) |
+		e32_uint8_t(10, 2, dcm) | e32_uint8_t(8, 2, epm) |
+		e32_int(5, 1, sd) | e32_int(4, 1, sp) | e32_int(3, 1, se) |
+		e32_int(2, 1, dp) | e32_int(1, 1, de) | e32_int(0, 1, ep) |
+		e32_uint8_t(14, 1, wn);
 	return reg;
 }
 
 static inline int qbman_swp_sys_init(struct qbman_swp_sys *s,
-				     const struct qbman_swp_desc *d)
+				     const struct qbman_swp_desc *d,
+				     uint8_t dqrr_size)
 {
 	uint32_t reg;
 
@@ -270,9 +272,9 @@ static inline int qbman_swp_sys_init(struct qbman_swp_sys *s,
 	BUG_ON(reg);
 #endif
 #ifdef QBMAN_CINH_ONLY
-	reg = qbman_set_swp_cfg(4, 1, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
+	reg = qbman_set_swp_cfg(dqrr_size, 1, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
 #else
-	reg = qbman_set_swp_cfg(4, 0, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
+	reg = qbman_set_swp_cfg(dqrr_size, 0, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
 #endif
 	qbman_cinh_write(s, QBMAN_CINH_SWP_CFG, reg);
 	reg = qbman_cinh_read(s, QBMAN_CINH_SWP_CFG);