[PATCH v2 4/7] scsi: ufs: core: Add hwq print for debug

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

Introduce hwq printing function for debug purpose.
- ufshcd_mcq_print_hwqs()

Signed-off-by: Po-Wen Kao <powen.kao@mediatek.com>
---
 drivers/ufs/core/ufs-mcq.c     | 32 +++++++++++++++++++++++++++++++-
 drivers/ufs/core/ufshcd-priv.h |  9 +++++++++
 drivers/ufs/core/ufshcd.c      | 18 +++++++++++-------
 include/ufs/ufshci.h           | 12 ++++++++++++
 4 files changed, 63 insertions(+), 8 deletions(-)

diff --git a/drivers/ufs/core/ufs-mcq.c b/drivers/ufs/core/ufs-mcq.c
index 5d5bc0bc6e88..d1ff3ccd2085 100644
--- a/drivers/ufs/core/ufs-mcq.c
+++ b/drivers/ufs/core/ufs-mcq.c
@@ -23,7 +23,6 @@
 
 #define MAX_DEV_CMD_ENTRIES	2
 #define MCQ_CFG_MAC_MASK	GENMASK(16, 8)
-#define MCQ_QCFG_SIZE		0x40
 #define MCQ_ENTRY_SIZE_IN_DWORD	8
 #define CQE_UCD_BA GENMASK_ULL(63, 7)
 
@@ -75,6 +74,13 @@ module_param_cb(poll_queues, &poll_queue_count_ops, &poll_queues, 0644);
 MODULE_PARM_DESC(poll_queues,
 		 "Number of poll queues used for r/w. Default value is 1");
 
+static const int mcq_opr_size[] = {
+	MCQ_SQD_SIZE,
+	MCQ_SQIS_SIZE,
+	MCQ_CQD_SIZE,
+	MCQ_CQIS_SIZE,
+};
+
 /**
  * ufshcd_mcq_config_mac - Set the #Max Activ Cmds.
  * @hba: per adapter instance
@@ -237,6 +243,30 @@ static void __iomem *mcq_opr_base(struct ufs_hba *hba,
 	return opr->base + opr->stride * i;
 }
 
+void ufshcd_mcq_print_hwqs(struct ufs_hba *hba, unsigned long bitmap)
+{
+	int id, i;
+	char prefix[15];
+
+	if (!is_mcq_enabled(hba))
+		return;
+
+	for_each_set_bit(id, &bitmap, hba->nr_hw_queues) {
+		snprintf(prefix, sizeof(prefix), "q%d SQCFG: ", id);
+		ufshcd_hex_dump(prefix,
+			hba->mcq_base + MCQ_QCFG_SIZE * id, MCQ_QCFG_SQ_SIZE);
+
+		snprintf(prefix, sizeof(prefix), "q%d CQCFG: ", id);
+		ufshcd_hex_dump(prefix,
+			hba->mcq_base + MCQ_QCFG_SIZE * id + MCQ_QCFG_SQ_SIZE, MCQ_QCFG_CQ_SIZE);
+
+		for (i = 0; i < OPR_MAX ; i++) {
+			snprintf(prefix, sizeof(prefix), "q%d OPR%d: ", id, i);
+			ufshcd_hex_dump(prefix, mcq_opr_base(hba, i, id), mcq_opr_size[i]);
+		}
+	}
+}
+
 u32 ufshcd_mcq_read_cqis(struct ufs_hba *hba, int i)
 {
 	return readl(mcq_opr_base(hba, OPR_CQIS, i) + REG_CQIS);
diff --git a/drivers/ufs/core/ufshcd-priv.h b/drivers/ufs/core/ufshcd-priv.h
index 529f8507a5e4..13534a9a6d0a 100644
--- a/drivers/ufs/core/ufshcd-priv.h
+++ b/drivers/ufs/core/ufshcd-priv.h
@@ -6,6 +6,14 @@
 #include <linux/pm_runtime.h>
 #include <ufs/ufshcd.h>
 
+#define ufshcd_hex_dump(prefix_str, buf, len) do {                       \
+	size_t __len = (len);                                            \
+	print_hex_dump(KERN_ERR, prefix_str,                             \
+		       __len > 4 ? DUMP_PREFIX_OFFSET : DUMP_PREFIX_NONE,\
+		       16, 4, buf, __len, false);                        \
+} while (0)
+
+
 static inline bool ufshcd_is_user_access_allowed(struct ufs_hba *hba)
 {
 	return !hba->shutting_down;
@@ -65,6 +73,7 @@ void ufshcd_compl_one_cqe(struct ufs_hba *hba, int task_tag,
 			  struct cq_entry *cqe);
 int ufshcd_mcq_init(struct ufs_hba *hba);
 int ufshcd_mcq_decide_queue_depth(struct ufs_hba *hba);
+void ufshcd_mcq_print_hwqs(struct ufs_hba *hba, unsigned long bitmap);
 int ufshcd_mcq_memory_alloc(struct ufs_hba *hba);
 void ufshcd_mcq_make_queues_operational(struct ufs_hba *hba);
 void ufshcd_mcq_config_mac(struct ufs_hba *hba, u32 max_active_cmds);
diff --git a/drivers/ufs/core/ufshcd.c b/drivers/ufs/core/ufshcd.c
index 81c9f07ebfc8..a15a5a78160c 100644
--- a/drivers/ufs/core/ufshcd.c
+++ b/drivers/ufs/core/ufshcd.c
@@ -135,13 +135,6 @@ MODULE_PARM_DESC(use_mcq_mode, "Control MCQ mode for controllers starting from U
 		_ret;                                                   \
 	})
 
-#define ufshcd_hex_dump(prefix_str, buf, len) do {                       \
-	size_t __len = (len);                                            \
-	print_hex_dump(KERN_ERR, prefix_str,                             \
-		       __len > 4 ? DUMP_PREFIX_OFFSET : DUMP_PREFIX_NONE,\
-		       16, 4, buf, __len, false);                        \
-} while (0)
-
 int ufshcd_dump_regs(struct ufs_hba *hba, size_t offset, size_t len,
 		     const char *prefix)
 {
@@ -536,6 +529,8 @@ static
 void ufshcd_print_trs(struct ufs_hba *hba, unsigned long bitmap, bool pr_prdt)
 {
 	const struct ufshcd_lrb *lrbp;
+	struct ufs_hw_queue *hwq;
+	struct scsi_cmnd *cmd;
 	int prdt_length;
 	int tag;
 
@@ -574,7 +569,16 @@ void ufshcd_print_trs(struct ufs_hba *hba, unsigned long bitmap, bool pr_prdt)
 		if (pr_prdt)
 			ufshcd_hex_dump("UPIU PRDT: ", lrbp->ucd_prdt_ptr,
 				ufshcd_sg_entry_size(hba) * prdt_length);
+
+		if (is_mcq_enabled(hba)) {
+			cmd = lrbp->cmd;
+			if (!cmd)
+				return;
+			hwq = ufshcd_mcq_req_to_hwq(hba, scsi_cmd_to_rq(cmd));
+			ufshcd_mcq_print_hwqs(hba, 1 << hwq->id);
+		}
 	}
+
 }
 
 static void ufshcd_print_tmrs(struct ufs_hba *hba, unsigned long bitmap)
diff --git a/include/ufs/ufshci.h b/include/ufs/ufshci.h
index 11424bb03814..027a2e884f89 100644
--- a/include/ufs/ufshci.h
+++ b/include/ufs/ufshci.h
@@ -185,6 +185,18 @@ static inline u32 ufshci_version(u32 major, u32 minor)
 				CRYPTO_ENGINE_FATAL_ERROR |\
 				UIC_LINK_LOST)
 
+/* MCQ size */
+#define MCQ_QCFG_SIZE			0x40
+#define MCQ_QCFG_SQ_SIZE		0x20
+#define MCQ_QCFG_CQ_SIZE		0x20
+
+enum {
+	MCQ_SQD_SIZE		= 0x14,
+	MCQ_SQIS_SIZE		= 0x08,
+	MCQ_CQD_SIZE		= 0x08,
+	MCQ_CQIS_SIZE		= 0x0C,
+};
+
 /* HCS - Host Controller Status 30h */
 #define DEVICE_PRESENT				0x1
 #define UTP_TRANSFER_REQ_LIST_READY		0x2
-- 
2.18.0