From 72a6312eda9b142ac5910e5f4b652fa8ae8d222d Mon Sep 17 00:00:00 2001
From: Roy Pledge <roy.pledge@nxp.com>
Date: Thu, 25 Oct 2018 16:55:53 -0400
Subject: [PATCH] soc: fsl: dpio: Fix order restoration API for QBMan 5.0

The mechanism for indicating to HW that a frame was dropped
when performing HW order restoration changed in QBMan 5.0 to
use a management command instead of a special enqueue command.
This patch implements that change when running on a QBMan 5.0
and above device.

Signed-off-by: Roy Pledge <roy.pledge@nxp.com>
---
 drivers/soc/fsl/dpio/dpio-service.c | 10 +++++++
 drivers/soc/fsl/dpio/qbman-portal.c | 59 ++++++++++++++++++++++++++++++++-----
 drivers/soc/fsl/dpio/qbman-portal.h |  9 ++++++
 3 files changed, 71 insertions(+), 7 deletions(-)

--- a/drivers/soc/fsl/dpio/dpio-service.c
+++ b/drivers/soc/fsl/dpio/dpio-service.c
@@ -803,10 +803,20 @@ int dpaa2_io_service_orp_seqnum_drop(str
 {
 	struct qbman_eq_desc ed;
 	struct dpaa2_fd fd;
+	unsigned long irqflags;
+	int ret;
 
 	d = service_select(d);
 	if (!d)
 		return -ENODEV;
+
+	if ((d->swp->desc->qman_version & QMAN_REV_MASK) >= QMAN_REV_5000) {
+		spin_lock_irqsave(&d->lock_mgmt_cmd, irqflags);
+		ret = qbman_orp_drop(d->swp, orpid, seqnum);
+		spin_unlock_irqrestore(&d->lock_mgmt_cmd, irqflags);
+		return ret;
+	}
+
 	qbman_eq_desc_clear(&ed);
 	qbman_eq_desc_set_orp_hole(&ed, orpid, seqnum);
 	return qbman_swp_enqueue(d->swp, &ed, &fd);
--- a/drivers/soc/fsl/dpio/qbman-portal.c
+++ b/drivers/soc/fsl/dpio/qbman-portal.c
@@ -12,19 +12,13 @@
 
 #include "qbman-portal.h"
 
-#define QMAN_REV_4000   0x04000000
-#define QMAN_REV_4100   0x04010000
-#define QMAN_REV_4101   0x04010001
-#define QMAN_REV_5000   0x05000000
-
-#define QMAN_REV_MASK   0xffff0000
-
 /* All QBMan command and result structures use this "valid bit" encoding */
 #define QB_VALID_BIT ((u32)0x80)
 
 /* QBMan portal management command codes */
 #define QBMAN_MC_ACQUIRE       0x30
 #define QBMAN_WQCHAN_CONFIGURE 0x46
+#define QBMAN_MC_ORP           0x63
 
 /* CINH register offsets */
 #define QBMAN_CINH_SWP_EQCR_PI      0x800
@@ -1246,3 +1240,54 @@ u32 qbman_bp_info_num_free_bufs(struct q
 {
 	return le32_to_cpu(a->fill);
 }
+
+struct qbman_orp_cmd_desc {
+	u8 verb;
+	u8 reserved;
+	u8 cid;
+	u8 reserved2;
+	u16 orpid;
+	u16 seqnum;
+	u8 reserved3[56];
+};
+
+struct qbman_orp_cmd_rslt {
+	u8 verb;
+	u8 rslt;
+	u8 cid;
+	u8 reserved1[61];
+};
+
+int qbman_orp_drop(struct qbman_swp *s, u16 orpid, u16 seqnum)
+{
+	struct qbman_orp_cmd_desc *p;
+	struct qbman_orp_cmd_rslt *r;
+	void *resp;
+
+	p = (struct qbman_orp_cmd_desc *)qbman_swp_mc_start(s);
+	if (!p)
+		return -EBUSY;
+
+	p->cid = 0x7;
+	p->orpid = cpu_to_le16(orpid);
+	p->seqnum = cpu_to_le16(seqnum);
+
+	resp = qbman_swp_mc_complete(s, p, QBMAN_MC_ORP);
+	if (!resp) {
+		pr_err("qbman: Drop sequence num %d orpid 0x%x failed, no response\n",
+		       seqnum, orpid);
+		return -EIO;
+	}
+	r = (struct qbman_orp_cmd_rslt *)resp;
+	/* Decode the outcome */
+	WARN_ON((r->verb & QBMAN_RESPONSE_VERB_MASK) != QBMAN_MC_ORP);
+
+	/* Determine success or failure */
+	if (r->rslt != QBMAN_MC_RSLT_OK) {
+		pr_err("Drop seqnum %d of prpid 0x%x failed, code=0x%02x\n",
+		       seqnum, orpid, r->rslt);
+		return -EIO;
+	}
+
+	return 0;
+}
--- a/drivers/soc/fsl/dpio/qbman-portal.h
+++ b/drivers/soc/fsl/dpio/qbman-portal.h
@@ -9,6 +9,13 @@
 
 #include <soc/fsl/dpaa2-fd.h>
 
+#define QMAN_REV_4000   0x04000000
+#define QMAN_REV_4100   0x04010000
+#define QMAN_REV_4101   0x04010001
+#define QMAN_REV_5000   0x05000000
+
+#define QMAN_REV_MASK   0xffff0000
+
 struct dpaa2_dq;
 struct qbman_swp;
 
@@ -178,6 +185,8 @@ void qbman_eq_desc_set_qd(struct qbman_e
 int qbman_swp_enqueue(struct qbman_swp *p, const struct qbman_eq_desc *d,
 		      const struct dpaa2_fd *fd);
 
+int qbman_orp_drop(struct qbman_swp *s, u16 orpid, u16 seqnum);
+
 void qbman_release_desc_clear(struct qbman_release_desc *d);
 void qbman_release_desc_set_bpid(struct qbman_release_desc *d, u16 bpid);
 void qbman_release_desc_set_rcdi(struct qbman_release_desc *d, int enable);
