From c095bbc29eeae597c47c90d41fab472be9e14ca6 Mon Sep 17 00:00:00 2001
From: Camelia Groza <camelia.groza@nxp.com>
Date: Wed, 30 May 2018 15:26:47 +0300
Subject: [PATCH] sdk_dpaa: ceetm: WQ/CEETM mode switchover cleanup

Wait for the WQ TX FQs to empty before switching from WQ to CEETM mode.

Signed-off-by: Camelia Groza <camelia.groza@nxp.com>
---
 .../ethernet/freescale/sdk_dpaa/dpaa_eth_ceetm.c   | 26 ++++++++++++++++++++++
 1 file changed, 26 insertions(+)

--- a/drivers/net/ethernet/freescale/sdk_dpaa/dpaa_eth_ceetm.c
+++ b/drivers/net/ethernet/freescale/sdk_dpaa/dpaa_eth_ceetm.c
@@ -65,6 +65,31 @@ static void get_dcp_and_sp(struct net_de
 	}
 }
 
+/* Wait for the DPAA Eth driver WQ TX FQs to empty */
+static void dpaa_drain_fqs(struct net_device *dev)
+{
+	const struct dpa_priv_s *priv = netdev_priv(dev);
+	struct qm_mcr_queryfq_np np;
+	struct qman_fq *fq;
+	int ret, i;
+
+	for (i = 0; i < DPAA_ETH_TX_QUEUES; i ++) {
+		fq = priv->egress_fqs[i];
+		while (true) {
+			ret = qman_query_fq_np(fq, &np);
+			if (unlikely(ret)) {
+				pr_err(KBUILD_BASENAME
+				       " : %s : unable to query FQ %x: %d\n",
+				       __func__, fq->fqid, ret);
+				break;
+			}
+
+			if (np.frm_cnt == 0)
+				break;
+		}
+	}
+}
+
 /* Enqueue Rejection Notification callback */
 static void ceetm_ern(struct qman_portal *portal, struct qman_fq *fq,
 		      const struct qm_mr_entry *msg)
@@ -1103,6 +1128,7 @@ static int ceetm_init(struct Qdisc *sch,
 	switch (priv->type) {
 	case CEETM_ROOT:
 		netif_tx_stop_all_queues(dev);
+		dpaa_drain_fqs(dev);
 		ret = ceetm_init_root(sch, priv, qopt, extack);
 		netif_tx_wake_all_queues(dev);
 		break;
