[PATCH v1 5/6] soc: mediatek: mutex: Add mtk_mutex_set_mod support to set MOD1

[Moudy Ho]

From: "Roy-CW.Yeh" <roy-cw.yeh at mediatek.com>

Add mtk_mutex_set_mod support to set MOD1

Signed-off-by: Roy-CW.Yeh <roy-cw.yeh at mediatek.com>
---
 drivers/soc/mediatek/mtk-mutex.c | 27 +++++++++++++++++++--------
 1 file changed, 19 insertions(+), 8 deletions(-)

diff --git a/drivers/soc/mediatek/mtk-mutex.c b/drivers/soc/mediatek/mtk-mutex.c
index c1a33d52038e..5dcbd61fe42c 100644
--- a/drivers/soc/mediatek/mtk-mutex.c
+++ b/drivers/soc/mediatek/mtk-mutex.c
@@ -23,6 +23,7 @@
 #define DISP_REG_MUTEX(n)			(0x24 + 0x20 * (n))
 #define DISP_REG_MUTEX_RST(n)			(0x28 + 0x20 * (n))
 #define DISP_REG_MUTEX_MOD(mutex_mod_reg, n)	(mutex_mod_reg + 0x20 * (n))
+#define DISP_REG_MUTEX_MOD1(mutex_mod_reg, n)	((mutex_mod_reg) + 0x20 * (n) + 0x4)
 #define DISP_REG_MUTEX_SOF(mutex_sof_reg, n)	(mutex_sof_reg + 0x20 * (n))
 #define DISP_REG_MUTEX_MOD2(n)			(0x34 + 0x20 * (n))
 
@@ -750,14 +751,24 @@ int mtk_mutex_write_mod(struct mtk_mutex *mutex,
 		return -EINVAL;
 	}
 
-	offset = DISP_REG_MUTEX_MOD(mtx->data->mutex_mod_reg,
-				    mutex->id);
-	reg = readl_relaxed(mtx->regs + offset);
-
-	if (clear)
-		reg &= ~BIT(mtx->data->mutex_table_mod[idx]);
-	else
-		reg |= BIT(mtx->data->mutex_table_mod[idx]);
+	if (mtx->data->mutex_table_mod[idx] < 32) {
+		offset = DISP_REG_MUTEX_MOD(mtx->data->mutex_mod_reg,
+					    mutex->id);
+		reg = readl_relaxed(mtx->regs + offset);
+		if (clear)
+			reg &= ~BIT(mtx->data->mutex_table_mod[idx]);
+		else
+			reg |= BIT(mtx->data->mutex_table_mod[idx]);
+
+	} else {
+		offset = DISP_REG_MUTEX_MOD1(mtx->data->mutex_mod_reg,
+					     mutex->id);
+		reg = readl_relaxed(mtx->regs + offset);
+		if (clear)
+			reg &= ~BIT(mtx->data->mutex_table_mod[idx] - 32);
+		else
+			reg |= BIT(mtx->data->mutex_table_mod[idx] - 32);
+	}
 
 	writel_relaxed(reg, mtx->regs + offset);
 
-- 
2.18.0