soc/fsl/qbman: Fixup qman_shutdown_fq()

When shutting down a FQ on a dedicated channel only the
SW portal associated with that channel can dequeue from it.
Make sure the correct portal is use.

Signed-off-by: Roy Pledge <roy.pledge@nxp.com>
Signed-off-by: Li Yang <leoyang.li@nxp.com>

diff --git a/drivers/soc/fsl/qbman/qman.c b/drivers/soc/fsl/qbman/qman.c
index 4a99ce5ce929f750c529fc1f0f93d1c16e12579f..bf68d86d80ee53c45159f9f5745506476ebad3ea 100644 (file)
--- a/drivers/soc/fsl/qbman/qman.c
+++ b/drivers/soc/fsl/qbman/qman.c
@@ -1018,6 +1018,20 @@ static inline void put_affine_portal(void)
        put_cpu_var(qman_affine_portal);
 }
 
+
+static inline struct qman_portal *get_portal_for_channel(u16 channel)
+{
+       int i;
+
+       for (i = 0; i < num_possible_cpus(); i++) {
+               if (affine_portals[i] &&
+                   affine_portals[i]->config->channel == channel)
+                       return affine_portals[i];
+       }
+
+       return NULL;
+}
+
 static struct workqueue_struct *qm_portal_wq;
 
 int qman_dqrr_set_ithresh(struct qman_portal *portal, u8 ithresh)
@@ -2601,7 +2615,7 @@ static int _qm_dqrr_consume_and_match(struct qm_portal *p, u32 fqid, int s,
 
 int qman_shutdown_fq(u32 fqid)
 {
-       struct qman_portal *p;
+       struct qman_portal *p, *channel_portal;
        struct device *dev;
        union qm_mc_command *mcc;
        union qm_mc_result *mcr;
@@ -2641,17 +2655,28 @@ int qman_shutdown_fq(u32 fqid)
        channel = qm_fqd_get_chan(&mcr->queryfq.fqd);
        wq = qm_fqd_get_wq(&mcr->queryfq.fqd);
 
+       if (channel < qm_channel_pool1) {
+               channel_portal = get_portal_for_channel(channel);
+               if (channel_portal == NULL) {
+                       dev_err(dev, "Can't find portal for dedicated channel 0x%x\n",
+                               channel);
+                       ret = -EIO;
+                       goto out;
+               }
+       } else
+               channel_portal = p;
+
        switch (state) {
        case QM_MCR_NP_STATE_TEN_SCHED:
        case QM_MCR_NP_STATE_TRU_SCHED:
        case QM_MCR_NP_STATE_ACTIVE:
        case QM_MCR_NP_STATE_PARKED:
                orl_empty = 0;
-               mcc = qm_mc_start(&p->p);
+               mcc = qm_mc_start(&channel_portal->p);
                qm_fqid_set(&mcc->fq, fqid);
-               qm_mc_commit(&p->p, QM_MCC_VERB_ALTER_RETIRE);
-               if (!qm_mc_result_timeout(&p->p, &mcr)) {
-                       dev_err(dev, "QUERYFQ_NP timeout\n");
+               qm_mc_commit(&channel_portal->p, QM_MCC_VERB_ALTER_RETIRE);
+               if (!qm_mc_result_timeout(&channel_portal->p, &mcr)) {
+                       dev_err(dev, "ALTER_RETIRE timeout\n");
                        ret = -ETIMEDOUT;
                        goto out;
                }
@@ -2659,6 +2684,9 @@ int qman_shutdown_fq(u32 fqid)
                            QM_MCR_VERB_ALTER_RETIRE);
                res = mcr->result; /* Make a copy as we reuse MCR below */
 
+               if (res == QM_MCR_RESULT_OK)
+                       drain_mr_fqrni(&channel_portal->p);
+
                if (res == QM_MCR_RESULT_PENDING) {
                        /*
                         * Need to wait for the FQRN in the message ring, which
@@ -2688,21 +2716,25 @@ int qman_shutdown_fq(u32 fqid)
                        }
                        /* Set the sdqcr to drain this channel */
                        if (channel < qm_channel_pool1)
-                               qm_dqrr_sdqcr_set(&p->p,
+                               qm_dqrr_sdqcr_set(&channel_portal->p,
                                                  QM_SDQCR_TYPE_ACTIVE |
                                                  QM_SDQCR_CHANNELS_DEDICATED);
                        else
-                               qm_dqrr_sdqcr_set(&p->p,
+                               qm_dqrr_sdqcr_set(&channel_portal->p,
                                                  QM_SDQCR_TYPE_ACTIVE |
                                                  QM_SDQCR_CHANNELS_POOL_CONV
                                                  (channel));
                        do {
                                /* Keep draining DQRR while checking the MR*/
-                               qm_dqrr_drain_nomatch(&p->p);
+                               qm_dqrr_drain_nomatch(&channel_portal->p);
                                /* Process message ring too */
-                               found_fqrn = qm_mr_drain(&p->p, FQRN);
+                               found_fqrn = qm_mr_drain(&channel_portal->p,
+                                                        FQRN);
                                cpu_relax();
                        } while (!found_fqrn);
+                       /* Restore SDQCR */
+                       qm_dqrr_sdqcr_set(&channel_portal->p,
+                                         channel_portal->sdqcr);
 
                }
                if (res != QM_MCR_RESULT_OK &&
@@ -2733,9 +2765,8 @@ int qman_shutdown_fq(u32 fqid)
                                 * Wait for a dequeue and process the dequeues,
                                 * making sure to empty the ring completely
                                 */
-                       } while (qm_dqrr_drain_wait(&p->p, fqid, FQ_EMPTY));
+                       } while (!qm_dqrr_drain_wait(&p->p, fqid, FQ_EMPTY));
                }
-               qm_dqrr_sdqcr_set(&p->p, 0);
 
                while (!orl_empty) {
                        /* Wait for the ORL to have been completely drained */