qman: Add CI-stashing support for QMan rev3.0 or later

Signed-off-by: Geoff Thorpe <Geoff.Thorpe@freescale.com>
Signed-off-by: Haiying Wang <Haiying.Wang@freescale.com>
Change-Id: Icda6a8547559ab45ceea7160cde566ca022e92e8
Reviewed-on: http://git.am.freescale.net:8181/3698
Reviewed-by: Schmitt Richard-B43082 <B43082@freescale.com>
Tested-by: Schmitt Richard-B43082 <B43082@freescale.com>

3 files changed, 58 insertions, 11 deletions

diff --git a/drivers/staging/fsl_qbman/fsl_usdpaa.c b/drivers/staging/fsl_qbman/fsl_usdpaa.c
index bce680f..0167b7b 100644
--- a/drivers/staging/fsl_qbman/fsl_usdpaa.c
+++ b/drivers/staging/fsl_qbman/fsl_usdpaa.c
@@ -326,7 +326,8 @@ static int init_qm_portal(struct qm_portal_config *config,
 	qm_dqrr_cdc_consume_n(portal, 0xffff);
 
 	/* Initialize the EQCR */
-	if (qm_eqcr_init(portal, qm_eqcr_pvb, qm_eqcr_cce)) {
+	if (qm_eqcr_init(portal, qm_eqcr_pvb, 
+			portal->eqcr.use_eqcr_ci_stashing ? 3 : 0, 1)) {
 		pr_err("Qman EQCR initialisation failed\n");
 		return 1;
 	}
diff --git a/drivers/staging/fsl_qbman/qman_high.c b/drivers/staging/fsl_qbman/qman_high.c
index 9f969bd..eaa2e39 100644
--- a/drivers/staging/fsl_qbman/qman_high.c
+++ b/drivers/staging/fsl_qbman/qman_high.c
@@ -361,9 +361,6 @@ loop:
 	goto loop;
 }
 
-
-
-
 struct qman_portal *qman_create_portal(
 			struct qman_portal *portal,
 			const struct qm_portal_config *config,
@@ -382,12 +379,20 @@ struct qman_portal *qman_create_portal(
 
 	__p = &portal->p;
 
+	portal->p.eqcr.use_eqcr_ci_stashing = ((qman_ip_rev >= QMAN_REV30) ?
+								1 : 0);
+
 	/* prep the low-level portal struct with the mapped addresses from the
 	 * config, everything that follows depends on it and "config" is more
 	 * for (de)reference... */
 	__p->addr.addr_ce = config->addr_virt[DPA_PORTAL_CE];
 	__p->addr.addr_ci = config->addr_virt[DPA_PORTAL_CI];
-	if (qm_eqcr_init(__p, qm_eqcr_pvb)) {
+	/*
+	 * If CI-stashing is used, the current defaults use a threshold of 3,
+	 * and stash with high-than-DQRR priority.
+	 */
+	if (qm_eqcr_init(__p, qm_eqcr_pvb,
+			portal->p.eqcr.use_eqcr_ci_stashing ? 3 : 0, 1)) {
 		pr_err("Qman EQCR initialisation failed\n");
 		goto fail_eqcr;
 	}
@@ -1986,10 +1991,23 @@ static inline struct qm_eqcr_entry *try_eq_start(struct qman_portal **p,
 		(*p)->eqci_owned = fq;
 	}
 #endif
-	avail = qm_eqcr_get_avail(&(*p)->p);
-	if (avail < 2)
-		update_eqcr_ci(*p, avail);
-	eq = qm_eqcr_start(&(*p)->p);
+	if ((*p)->p.eqcr.use_eqcr_ci_stashing) {
+		/*
+		 * The stashing case is easy, only update if we need to in
+		 * order to try and liberate ring entries.
+		 */
+		eq = qm_eqcr_start_stash(&(*p)->p);
+	} else {
+		/*
+		 * The non-stashing case is harder, need to prefetch ahead of
+		 * time.
+		 */
+		avail = qm_eqcr_get_avail(&(*p)->p);
+		if (avail < 2)
+			update_eqcr_ci(*p, avail);
+		eq = qm_eqcr_start_no_stash(&(*p)->p);
+	}
+
 	if (unlikely(!eq)) {
 #ifdef CONFIG_FSL_DPA_CAN_WAIT_SYNC
 		if (unlikely((flags & QMAN_ENQUEUE_FLAG_WAIT) &&
diff --git a/drivers/staging/fsl_qbman/qman_low.h b/drivers/staging/fsl_qbman/qman_low.h
index a610394..715c3a8 100644
--- a/drivers/staging/fsl_qbman/qman_low.h
+++ b/drivers/staging/fsl_qbman/qman_low.h
@@ -166,6 +166,7 @@ enum qm_mr_cmode {		/* matches QCSP_CFG::MM */
 struct qm_eqcr {
 	struct qm_eqcr_entry *ring, *cursor;
 	u8 ci, available, ithresh, vbit;
+	u32 use_eqcr_ci_stashing;
 #ifdef CONFIG_FSL_DPA_CHECKING
 	u32 busy;
 	enum qm_eqcr_pmode pmode;
@@ -277,7 +278,9 @@ static inline void EQCR_INC(struct qm_eqcr *eqcr)
 }
 
 static inline int qm_eqcr_init(struct qm_portal *portal,
-				enum qm_eqcr_pmode pmode)
+				enum qm_eqcr_pmode pmode,
+				unsigned int eq_stash_thresh,
+				int eq_stash_prio)
 {
 	/* This use of 'register', as well as all other occurances, is because
 	 * it has been observed to generate much faster code with gcc than is
@@ -301,6 +304,8 @@ static inline int qm_eqcr_init(struct qm_portal *portal,
 	eqcr->pmode = pmode;
 #endif
 	cfg = (qm_in(CFG) & 0x00ffffff) |
+		(eq_stash_thresh << 28) | /* QCSP_CFG: EST */
+		(eq_stash_prio << 26)	| /* QCSP_CFG: EP */
 		((pmode & 0x3) << 24);	/* QCSP_CFG::EPM */
 	qm_out(CFG, cfg);
 	return 0;
@@ -321,7 +326,8 @@ static inline void qm_eqcr_finish(struct qm_portal *portal)
 		pr_crit("EQCR destroyed unquiesced\n");
 }
 
-static inline struct qm_eqcr_entry *qm_eqcr_start(struct qm_portal *portal)
+static inline struct qm_eqcr_entry *qm_eqcr_start_no_stash(struct qm_portal
+								 *portal)
 {
 	register struct qm_eqcr *eqcr = &portal->eqcr;
 	DPA_ASSERT(!eqcr->busy);
@@ -336,6 +342,28 @@ static inline struct qm_eqcr_entry *qm_eqcr_start(struct qm_portal *portal)
 	return eqcr->cursor;
 }
 
+static inline struct qm_eqcr_entry *qm_eqcr_start_stash(struct qm_portal
+								*portal)
+{
+	register struct qm_eqcr *eqcr = &portal->eqcr;
+	u8 diff, old_ci;
+
+	DPA_ASSERT(!eqcr->busy);
+	if (!eqcr->available) {
+		old_ci = eqcr->ci;
+		eqcr->ci = qm_cl_in(EQCR_CI) & (QM_EQCR_SIZE - 1);
+		diff = qm_cyc_diff(QM_EQCR_SIZE, old_ci, eqcr->ci);
+		eqcr->available += diff;
+		if (!diff)
+			return NULL;
+	}
+#ifdef CONFIG_FSL_DPA_CHECKING
+	eqcr->busy = 1;
+#endif
+	dcbz_64(eqcr->cursor);
+	return eqcr->cursor;
+}
+
 static inline void qm_eqcr_abort(struct qm_portal *portal)
 {
 	__maybe_unused register struct qm_eqcr *eqcr = &portal->eqcr;