6 files changed, 85 insertions, 5 deletions

diff --git a/drivers/soc/fsl/dpio/dpio-cmd.h b/drivers/soc/fsl/dpio/dpio-cmd.h
index 5814d2f395a4..e13fd3ac1939 100644
--- a/drivers/soc/fsl/dpio/dpio-cmd.h
+++ b/drivers/soc/fsl/dpio/dpio-cmd.h
@@ -26,6 +26,7 @@
 #define DPIO_CMDID_DISABLE				DPIO_CMD(0x003)
 #define DPIO_CMDID_GET_ATTR				DPIO_CMD(0x004)
 #define DPIO_CMDID_RESET				DPIO_CMD(0x005)
+#define DPIO_CMDID_SET_STASHING_DEST			DPIO_CMD(0x120)
 
 struct dpio_cmd_open {
 	__le32 dpio_id;
@@ -47,4 +48,8 @@ struct dpio_rsp_get_attr {
 	__le32 qbman_version;
 };
 
+struct dpio_stashing_dest {
+	u8 sdest;
+};
+
 #endif /* _FSL_DPIO_CMD_H */
diff --git a/drivers/soc/fsl/dpio/dpio-driver.c b/drivers/soc/fsl/dpio/dpio-driver.c
index 2d4af32a0dec..c0cdc8946031 100644
--- a/drivers/soc/fsl/dpio/dpio-driver.c
+++ b/drivers/soc/fsl/dpio/dpio-driver.c
@@ -14,6 +14,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/delay.h>
 #include <linux/io.h>
+#include <linux/sys_soc.h>
 
 #include <linux/fsl/mc.h>
 #include <soc/fsl/dpaa2-io.h>
@@ -32,6 +33,46 @@ struct dpio_priv {
 
 static cpumask_var_t cpus_unused_mask;
 
+static const struct soc_device_attribute ls1088a_soc[] = {
+	{.family = "QorIQ LS1088A"},
+	{ /* sentinel */ }
+};
+
+static const struct soc_device_attribute ls2080a_soc[] = {
+	{.family = "QorIQ LS2080A"},
+	{ /* sentinel */ }
+};
+
+static const struct soc_device_attribute ls2088a_soc[] = {
+	{.family = "QorIQ LS2088A"},
+	{ /* sentinel */ }
+};
+
+static const struct soc_device_attribute lx2160a_soc[] = {
+	{.family = "QorIQ LX2160A"},
+	{ /* sentinel */ }
+};
+
+static int dpaa2_dpio_get_cluster_sdest(struct fsl_mc_device *dpio_dev, int cpu)
+{
+	int cluster_base, cluster_size;
+
+	if (soc_device_match(ls1088a_soc)) {
+		cluster_base = 2;
+		cluster_size = 4;
+	} else if (soc_device_match(ls2080a_soc) ||
+		   soc_device_match(ls2088a_soc) ||
+		   soc_device_match(lx2160a_soc)) {
+		cluster_base = 0;
+		cluster_size = 2;
+	} else {
+		dev_err(&dpio_dev->dev, "unknown SoC version\n");
+		return -1;
+	}
+
+	return cluster_base + cpu / cluster_size;
+}
+
 static irqreturn_t dpio_irq_handler(int irq_num, void *arg)
 {
 	struct device *dev = (struct device *)arg;
@@ -89,6 +130,7 @@ static int dpaa2_dpio_probe(struct fsl_mc_device *dpio_dev)
 	int err = -ENOMEM;
 	struct device *dev = &dpio_dev->dev;
 	int possible_next_cpu;
+	int sdest;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
@@ -145,6 +187,16 @@ static int dpaa2_dpio_probe(struct fsl_mc_device *dpio_dev)
 	desc.cpu = possible_next_cpu;
 	cpumask_clear_cpu(possible_next_cpu, cpus_unused_mask);
 
+	sdest = dpaa2_dpio_get_cluster_sdest(dpio_dev, desc.cpu);
+	if (sdest >= 0) {
+		err = dpio_set_stashing_destination(dpio_dev->mc_io, 0,
+						    dpio_dev->mc_handle,
+						    sdest);
+		if (err)
+			dev_err(dev, "dpio_set_stashing_destination failed for cpu%d\n",
+				desc.cpu);
+	}
+
 	/*
 	 * Set the CENA regs to be the cache inhibited area of the portal to
 	 * avoid coherency issues if a user migrates to another core.
@@ -220,12 +272,12 @@ static int dpaa2_dpio_remove(struct fsl_mc_device *dpio_dev)
 
 	dev = &dpio_dev->dev;
 	priv = dev_get_drvdata(dev);
+	cpu = dpaa2_io_get_cpu(priv->io);
 
 	dpaa2_io_down(priv->io);
 
 	dpio_teardown_irqs(dpio_dev);
 
-	cpu = dpaa2_io_get_cpu(priv->io);
 	cpumask_set_cpu(cpu, cpus_unused_mask);
 
 	err = dpio_open(dpio_dev->mc_io, 0, dpio_dev->obj_desc.id,
diff --git a/drivers/soc/fsl/dpio/dpio-service.c b/drivers/soc/fsl/dpio/dpio-service.c
index bc801934602a..b9539ef2c3cd 100644
--- a/drivers/soc/fsl/dpio/dpio-service.c
+++ b/drivers/soc/fsl/dpio/dpio-service.c
@@ -473,7 +473,7 @@ EXPORT_SYMBOL_GPL(dpaa2_io_service_enqueue_qd);
  * Return 0 for success, and negative error code for failure.
  */
 int dpaa2_io_service_release(struct dpaa2_io *d,
-			     u32 bpid,
+			     u16 bpid,
 			     const u64 *buffers,
 			     unsigned int num_buffers)
 {
@@ -502,7 +502,7 @@ EXPORT_SYMBOL_GPL(dpaa2_io_service_release);
  * Eg. if the buffer pool is empty, this will return zero.
  */
 int dpaa2_io_service_acquire(struct dpaa2_io *d,
-			     u32 bpid,
+			     u16 bpid,
 			     u64 *buffers,
 			     unsigned int num_buffers)
 {
@@ -630,6 +630,7 @@ struct dpaa2_dq *dpaa2_io_store_next(struct dpaa2_io_store *s, int *is_last)
 		if (!(dpaa2_dq_flags(ret) & DPAA2_DQ_STAT_VALIDFRAME))
 			ret = NULL;
 	} else {
+		prefetch(&s->vaddr[s->idx]);
 		*is_last = 0;
 	}
 
diff --git a/drivers/soc/fsl/dpio/dpio.c b/drivers/soc/fsl/dpio/dpio.c
index 521bc6946317..af74c597a675 100644
--- a/drivers/soc/fsl/dpio/dpio.c
+++ b/drivers/soc/fsl/dpio/dpio.c
@@ -166,6 +166,22 @@ int dpio_get_attributes(struct fsl_mc_io *mc_io,
 	return 0;
 }
 
+int dpio_set_stashing_destination(struct fsl_mc_io *mc_io,
+				  u32 cmd_flags,
+				  u16 token,
+				  u8 sdest)
+{
+	struct fsl_mc_command cmd = { 0 };
+	struct dpio_stashing_dest *dpio_cmd;
+
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_SET_STASHING_DEST,
+					  cmd_flags, token);
+	dpio_cmd = (struct dpio_stashing_dest *)cmd.params;
+	dpio_cmd->sdest = sdest;
+
+	return mc_send_command(mc_io, &cmd);
+}
+
 /**
  * dpio_get_api_version - Get Data Path I/O API version
  * @mc_io:	Pointer to MC portal's DPIO object
diff --git a/drivers/soc/fsl/dpio/dpio.h b/drivers/soc/fsl/dpio/dpio.h
index b2ac4ba4fb8e..da06f7258098 100644
--- a/drivers/soc/fsl/dpio/dpio.h
+++ b/drivers/soc/fsl/dpio/dpio.h
@@ -75,6 +75,11 @@ int dpio_get_attributes(struct fsl_mc_io	*mc_io,
 			u16		token,
 			struct dpio_attr	*attr);
 
+int dpio_set_stashing_destination(struct fsl_mc_io *mc_io,
+				  u32 cmd_flags,
+				  u16 token,
+				  u8 dest);
+
 int dpio_get_api_version(struct fsl_mc_io *mc_io,
 			 u32 cmd_flags,
 			 u16 *major_ver,
diff --git a/drivers/soc/fsl/dpio/qbman-portal.c b/drivers/soc/fsl/dpio/qbman-portal.c
index 0bddb85c0ae5..d02013556a1b 100644
--- a/drivers/soc/fsl/dpio/qbman-portal.c
+++ b/drivers/soc/fsl/dpio/qbman-portal.c
@@ -169,9 +169,9 @@ struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d)
 				3, /* RPM: Valid bit mode, RCR in array mode */
 				2, /* DCM: Discrete consumption ack mode */
 				3, /* EPM: Valid bit mode, EQCR in array mode */
-				0, /* mem stashing drop enable == FALSE */
+				1, /* mem stashing drop enable == TRUE */
 				1, /* mem stashing priority == TRUE */
-				0, /* mem stashing enable == FALSE */
+				1, /* mem stashing enable == TRUE */
 				1, /* dequeue stashing priority == TRUE */
 				0, /* dequeue stashing enable == FALSE */
 				0); /* EQCR_CI stashing priority == FALSE */
@@ -180,6 +180,7 @@ struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d)
 	reg = qbman_read_register(p, QBMAN_CINH_SWP_CFG);
 	if (!reg) {
 		pr_err("qbman: the portal is not enabled!\n");
+		kfree(p);
 		return NULL;
 	}