[PATCH v2 1/3] scsi: ufs: core: Introduce mcq ops to config cqid

[Po-Wen Kao <powen.kao@mediatek.com>]

From: Peter Wang <peter.wang@mediatek.com>

MCQ sq/cq mapping is not just one for one, could many for one.
This patch allow host driver to change the mapping, assign cqid
for each hw queue.

Signed-off-by: Po-Wen Kao <powen.kao@mediatek.com>
Signed-off-by: Peter Wang <peter.wang@mediatek.com>
---
 drivers/ufs/core/ufs-mcq.c     |  2 +-
 drivers/ufs/core/ufshcd-priv.h |  8 ++++++++
 drivers/ufs/core/ufshcd.c      | 11 +++++++++++
 include/ufs/ufshcd.h           |  3 +++
 4 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/drivers/ufs/core/ufs-mcq.c b/drivers/ufs/core/ufs-mcq.c
index 51b3c6ae781d..1ba9c395c6b0 100644
--- a/drivers/ufs/core/ufs-mcq.c
+++ b/drivers/ufs/core/ufs-mcq.c
@@ -368,7 +368,7 @@ void ufshcd_mcq_make_queues_operational(struct ufs_hba *hba)
 		 * Submission Queue Attribute
 		 */
 		ufsmcq_writel(hba, (1 << QUEUE_EN_OFFSET) | qsize |
-			      (i << QUEUE_ID_OFFSET),
+			      (hwq->cqid << QUEUE_ID_OFFSET),
 			      MCQ_CFG_n(REG_SQATTR, i));
 	}
 }
diff --git a/drivers/ufs/core/ufshcd-priv.h b/drivers/ufs/core/ufshcd-priv.h
index d53b93c21a0c..2de068b96c71 100644
--- a/drivers/ufs/core/ufshcd-priv.h
+++ b/drivers/ufs/core/ufshcd-priv.h
@@ -287,6 +287,14 @@ static inline int ufshcd_mcq_vops_config_esi(struct ufs_hba *hba)
 	return -EOPNOTSUPP;
 }
 
+static inline int ufshcd_mcq_vops_config_cqid(struct ufs_hba *hba)
+{
+	if (hba->vops && hba->vops->config_cqid)
+		return hba->vops->config_cqid(hba);
+
+	return -EOPNOTSUPP;
+}
+
 extern const struct ufs_pm_lvl_states ufs_pm_lvl_states[];
 
 /**
diff --git a/drivers/ufs/core/ufshcd.c b/drivers/ufs/core/ufshcd.c
index 4ec8dacb447c..fad9ff4469b0 100644
--- a/drivers/ufs/core/ufshcd.c
+++ b/drivers/ufs/core/ufshcd.c
@@ -8488,11 +8488,22 @@ static int ufshcd_alloc_mcq(struct ufs_hba *hba)
 static void ufshcd_config_mcq(struct ufs_hba *hba)
 {
 	int ret;
+	struct ufs_hw_queue *hwq;
+	int i;
 
 	ret = ufshcd_mcq_vops_config_esi(hba);
 	dev_info(hba->dev, "ESI %sconfigured\n", ret ? "is not " : "");
 
 	ufshcd_enable_intr(hba, UFSHCD_ENABLE_MCQ_INTRS);
+
+	ret = ufshcd_mcq_vops_config_cqid(hba);
+	if (ret) {
+		for (i = 0; i < hba->nr_hw_queues; i++) {
+			hwq = &hba->uhq[i];
+			hwq->cqid = i;
+		}
+	}
+
 	ufshcd_mcq_make_queues_operational(hba);
 	ufshcd_mcq_config_mac(hba, hba->nutrs);
 
diff --git a/include/ufs/ufshcd.h b/include/ufs/ufshcd.h
index df1d04f7a542..d5b16f968d7f 100644
--- a/include/ufs/ufshcd.h
+++ b/include/ufs/ufshcd.h
@@ -307,6 +307,7 @@ struct ufs_pwr_mode_info {
  * @op_runtime_config: called to config Operation and runtime regs Pointers
  * @get_outstanding_cqs: called to get outstanding completion queues
  * @config_esi: called to config Event Specific Interrupt
+ * @config_cqid: called to config cqid for each sq
  */
 struct ufs_hba_variant_ops {
 	const char *name;
@@ -352,6 +353,7 @@ struct ufs_hba_variant_ops {
 	int	(*get_outstanding_cqs)(struct ufs_hba *hba,
 				       unsigned long *ocqs);
 	int	(*config_esi)(struct ufs_hba *hba);
+	int	(*config_cqid)(struct ufs_hba *hba);
 };
 
 /* clock gating state  */
@@ -1100,6 +1102,7 @@ struct ufs_hw_queue {
 	dma_addr_t cqe_dma_addr;
 	u32 max_entries;
 	u32 id;
+	u32 cqid;
 	u32 sq_tail_slot;
 	spinlock_t sq_lock;
 	u32 cq_tail_slot;
-- 
2.18.0