[PATCH v2 8/8] media: mediatek: mdp3: Add programming flow for unsupported subsys ID hardware

[Jason-JH.Lin <jason-jh.lin@mediatek.com>]

To support hardware without subsys IDs on new SoCs, add a programming
flow that checks whether the subsys ID is valid. If the subsys ID is
invalid, the flow will call 2 alternative CMDQ APIs:
cmdq_pkt_assign() and cmdq_pkt_write_s_mask_value() to achieve the
same functionality.

Signed-off-by: Jason-JH.Lin <jason-jh.lin@mediatek.com>
---
 .../platform/mediatek/mdp3/mtk-mdp3-cmdq.c    | 26 ++++++++++--
 .../platform/mediatek/mdp3/mtk-mdp3-comp.h    | 41 +++++++++++++++----
 2 files changed, 55 insertions(+), 12 deletions(-)

diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c
index df0ab338ef41..58beeffd3fdf 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c
@@ -320,9 +320,18 @@ static int mdp_path_config_subfrm(struct mdp_cmdq_cmd *cmd,
 		return ret;
 	/* Enable mux settings */
 	for (index = 0; index < ctrl->num_sets; index++) {
+		struct cmdq_client *cl = (struct cmdq_client *)cmd->pkt.cl;
+
 		set = &ctrl->sets[index];
-		cmdq_pkt_write_mask(&cmd->pkt, set->subsys_id, set->reg,
-				    set->value, 0xFFFFFFFF);
+		if (cmdq_subsys_is_valid(cl->chan, set->subsys_id)) {
+			cmdq_pkt_write_mask(&cmd->pkt, set->subsys_id, set->reg,
+					    set->value, 0xFFFFFFFF);
+		} else {
+			/* only MMIO access, no need to check mminfro_offset */
+			cmdq_pkt_assign(&cmd->pkt, 0, CMDQ_ADDR_HIGH(set->reg));
+			cmdq_pkt_write_s_mask_value(&cmd->pkt, 0, CMDQ_ADDR_LOW(set->reg),
+						    set->value, 0xFFFFFFFF);
+		}
 	}
 	/* Config sub-frame information */
 	for (index = (num_comp - 1); index >= 0; index--) {
@@ -376,9 +385,18 @@ static int mdp_path_config_subfrm(struct mdp_cmdq_cmd *cmd,
 	}
 	/* Disable mux settings */
 	for (index = 0; index < ctrl->num_sets; index++) {
+		struct cmdq_client *cl = (struct cmdq_client *)cmd->pkt.cl;
+
 		set = &ctrl->sets[index];
-		cmdq_pkt_write_mask(&cmd->pkt, set->subsys_id, set->reg,
-				    0, 0xFFFFFFFF);
+		if (cmdq_subsys_is_valid(cl->chan, set->subsys_id)) {
+			cmdq_pkt_write_mask(&cmd->pkt, set->subsys_id, set->reg,
+					    0, 0xFFFFFFFF);
+		} else {
+			/* only MMIO access, no need to check mminfro_offset */
+			cmdq_pkt_assign(&cmd->pkt, 0, CMDQ_ADDR_HIGH(set->reg));
+			cmdq_pkt_write_s_mask_value(&cmd->pkt, 0, CMDQ_ADDR_LOW(set->reg),
+						    0, 0xFFFFFFFF);
+		}
 	}
 
 	return 0;
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h
index 3e5d2da1c807..f6c041934779 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h
@@ -9,9 +9,25 @@
 
 #include "mtk-mdp3-cmdq.h"
 
-#define MM_REG_WRITE_MASK(cmd, id, base, ofst, val, mask, ...)	\
-	cmdq_pkt_write_mask(&((cmd)->pkt), id,			\
-		(base) + (ofst), (val), (mask), ##__VA_ARGS__)
+#define MM_REG_WRITE_MASK(cmd, id, base, ofst, val, mask, ...)				\
+do {											\
+	typeof(cmd) (_cmd) = (cmd);							\
+	typeof(id) (_id) = (id);							\
+	typeof(base) (_base) = (base);							\
+	typeof(ofst) (_ofst) = (ofst);							\
+	typeof(val) (_val) = (val);							\
+	typeof(mask) (_mask) = (mask);							\
+	if (cmdq_subsys_is_valid(((struct cmdq_client *)_cmd->pkt.cl)->chan, _id)) {	\
+		cmdq_pkt_write_mask(&_cmd->pkt, _id, _base + _ofst, _val,		\
+				    _mask, ##__VA_ARGS__);				\
+	} else {									\
+		/* only MMIO access, no need to check mminfro_offset */			\
+		cmdq_pkt_assign(&_cmd->pkt, 0, CMDQ_ADDR_HIGH(_base));			\
+		cmdq_pkt_write_s_mask_value(&_cmd->pkt, 0,				\
+					    CMDQ_ADDR_LOW(_base + _ofst), _val,		\
+					    _mask, ##__VA_ARGS__);			\
+	}										\
+} while (0)
 
 #define MM_REG_WRITE(cmd, id, base, ofst, val, mask, ...)	\
 do {								\
@@ -49,11 +65,20 @@ do {								\
 	cmdq_pkt_set_event(&((c)->pkt), (e));			\
 } while (0)
 
-#define MM_REG_POLL_MASK(cmd, id, base, ofst, val, _mask, ...)	\
-do {								\
-	typeof(_mask) (_m) = (_mask);				\
-	cmdq_pkt_poll_mask(&((cmd)->pkt), id,			\
-		(base) + (ofst), (val), (_m), ##__VA_ARGS__);	\
+#define MM_REG_POLL_MASK(cmd, id, base, ofst, val, _mask, ...)				\
+do {											\
+	typeof(cmd) (_cmd) = (cmd);							\
+	typeof(id) (_id) = (id);							\
+	typeof(base) (_base) = (base);							\
+	typeof(ofst) (_ofst) = (ofst);							\
+	typeof(val) (_val) = (val);							\
+	typeof(_mask) (_m) = (_mask);							\
+	if (cmdq_subsys_is_valid(((struct cmdq_client *)_cmd->pkt.cl)->chan, _id))	\
+		cmdq_pkt_poll_mask(&_cmd->pkt, _id, _base + _ofst, _val,		\
+				   _m, ##__VA_ARGS__);					\
+	else										\
+		cmdq_pkt_poll_addr(&_cmd->pkt, _base + _ofst, _val,			\
+				   _m, ##__VA_ARGS__);					\
 } while (0)
 
 #define MM_REG_POLL(cmd, id, base, ofst, val, mask, ...)	\
-- 
2.43.0