diff --git a/drivers/cam_icp/hfi.c b/drivers/cam_icp/hfi.c
index 9b9ea4bfc7..1d1d44b6a2 100644
--- a/drivers/cam_icp/hfi.c
+++ b/drivers/cam_icp/hfi.c
@@ -61,47 +61,76 @@ static void __iomem *hfi_iface_addr(struct hfi_info *hfi)
 	return IS_ERR_OR_NULL(ret) ? NULL : ret;
 }
 
+static void hfi_queue_dump(uint32_t *dwords, int count)
+{
+	int i;
+	int rows;
+	int remaining;
+
+	rows = count / 4;
+	remaining = count % 4;
+
+	for (i = 0; i < rows; i++, dwords += 4)
+		CAM_DBG(CAM_HFI,
+			"word[%04d]: 0x%08x 0x%08x 0x%08x 0x%08x",
+			i * 4, dwords[0], dwords[1], dwords[2], dwords[3]);
+
+	if (remaining == 1)
+		CAM_DBG(CAM_HFI, "word[%04d]: 0x%08x", rows * 4, dwords[0]);
+	else if (remaining == 2)
+		CAM_DBG(CAM_HFI, "word[%04d]: 0x%08x 0x%08x",
+			rows * 4, dwords[0], dwords[1]);
+	else if (remaining == 3)
+		CAM_DBG(CAM_HFI, "word[%04d]: 0x%08x 0x%08x 0x%08x",
+			rows * 4, dwords[0], dwords[1], dwords[2]);
+}
+
 void cam_hfi_queue_dump(void)
 {
+	struct hfi_mem_info *hfi_mem = &g_hfi->map;
 	struct hfi_qtbl *qtbl;
-	struct hfi_qtbl_hdr *qtbl_hdr;
-	struct hfi_q_hdr *cmd_q_hdr, *msg_q_hdr;
-	struct hfi_mem_info *hfi_mem = NULL;
-	uint32_t *read_q, *read_ptr;
-	int i;
+	struct hfi_q_hdr *q_hdr;
+	uint32_t *dwords;
+	int num_dwords;
 
-	hfi_mem = &g_hfi->map;
 	if (!hfi_mem) {
-		CAM_ERR(CAM_HFI, "Unable to dump queues hfi memory is NULL");
+		CAM_ERR(CAM_HFI, "hfi mem info NULL... unable to dump queues");
 		return;
 	}
 
 	qtbl = (struct hfi_qtbl *)hfi_mem->qtbl.kva;
-	qtbl_hdr = &qtbl->q_tbl_hdr;
 	CAM_DBG(CAM_HFI,
-		"qtbl: version = %x size = %u num q = %u qhdr_size = %u",
-		qtbl_hdr->qtbl_version, qtbl_hdr->qtbl_size,
-		qtbl_hdr->qtbl_num_q, qtbl_hdr->qtbl_qhdr_size);
+		"qtbl header: version=0x%08x tbl_size=%u numq=%u qhdr_size=%u",
+		qtbl->q_tbl_hdr.qtbl_version,
+		qtbl->q_tbl_hdr.qtbl_size,
+		qtbl->q_tbl_hdr.qtbl_num_q,
+		qtbl->q_tbl_hdr.qtbl_qhdr_size);
 
-	cmd_q_hdr = &qtbl->q_hdr[Q_CMD];
-	CAM_DBG(CAM_HFI, "cmd: size = %u r_idx = %u w_idx = %u addr = %x",
-		cmd_q_hdr->qhdr_q_size, cmd_q_hdr->qhdr_read_idx,
-		cmd_q_hdr->qhdr_write_idx, hfi_mem->cmd_q.iova);
-	read_q = (uint32_t *)g_hfi->map.cmd_q.kva;
-	read_ptr = (uint32_t *)(read_q + 0);
-	CAM_DBG(CAM_HFI, "CMD Q START");
-	for (i = 0; i < ICP_CMD_Q_SIZE_IN_BYTES >> BYTE_WORD_SHIFT; i++)
-		CAM_DBG(CAM_HFI, "Word: %d Data: 0x%08x ", i, read_ptr[i]);
+	q_hdr = &qtbl->q_hdr[Q_CMD];
+	CAM_DBG(CAM_HFI,
+		"cmd_q: addr=0x%08x size=%u read_idx=%u write_idx=%u",
+		hfi_mem->cmd_q.iova,
+		q_hdr->qhdr_q_size,
+		q_hdr->qhdr_read_idx,
+		q_hdr->qhdr_write_idx);
 
-	msg_q_hdr = &qtbl->q_hdr[Q_MSG];
-	CAM_DBG(CAM_HFI, "msg: size = %u r_idx = %u w_idx = %u addr = %x",
-		msg_q_hdr->qhdr_q_size, msg_q_hdr->qhdr_read_idx,
-		msg_q_hdr->qhdr_write_idx, hfi_mem->msg_q.iova);
-	read_q = (uint32_t *)g_hfi->map.msg_q.kva;
-	read_ptr = (uint32_t *)(read_q + 0);
-	CAM_DBG(CAM_HFI, "MSG Q START");
-	for (i = 0; i < ICP_MSG_Q_SIZE_IN_BYTES >> BYTE_WORD_SHIFT; i++)
-		CAM_DBG(CAM_HFI, "Word: %d Data: 0x%08x ", i, read_ptr[i]);
+	dwords = (uint32_t *)hfi_mem->cmd_q.kva;
+	num_dwords = ICP_CMD_Q_SIZE_IN_BYTES >> BYTE_WORD_SHIFT;
+
+	hfi_queue_dump(dwords, num_dwords);
+
+	q_hdr = &qtbl->q_hdr[Q_MSG];
+	CAM_DBG(CAM_HFI,
+		"msg_q: addr=0x%08x size=%u read_idx=%u write_idx=%u",
+		hfi_mem->msg_q.iova,
+		q_hdr->qhdr_q_size,
+		q_hdr->qhdr_read_idx,
+		q_hdr->qhdr_write_idx);
+
+	dwords = (uint32_t *)hfi_mem->msg_q.kva;
+	num_dwords = ICP_MSG_Q_SIZE_IN_BYTES >> BYTE_WORD_SHIFT;
+
+	hfi_queue_dump(dwords, num_dwords);
 }
 
 int hfi_write_cmd(void *cmd_ptr)