soc: mediatek: mutex: Add mtk_mutex_set_mod support to set MOD1

Add mtk_mutex_set_mod support to set MOD1

Signed-off-by: Roy-CW.Yeh <roy-cw.yeh@mediatek.com>
Signed-off-by: Moudy Ho <moudy.ho@mediatek.com>
Reviewed-by: AngeloGioacchino Del Regno <angelogioacchino.delregno@collabora.com>
Reviewed-by: Chen-Yu Tsai <wenst@chromium.org>
Link: https://lore.kernel.org/r/20230206091109.1324-6-moudy.ho@mediatek.com
Signed-off-by: Matthias Brugger <matthias.bgg@gmail.com>

diff --git a/drivers/soc/mediatek/mtk-mutex.c b/drivers/soc/mediatek/mtk-mutex.c
index c5b1b42303ac26f388defbd9e6c6161dd07df4ab..a8d5fc3df5f0ac516c8c5c7c3edf22baeaf34559 100644 (file)
--- 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))
 
@@ -828,7 +829,7 @@ int mtk_mutex_write_mod(struct mtk_mutex *mutex,
        struct mtk_mutex_ctx *mtx = container_of(mutex, struct mtk_mutex_ctx,
                                                 mutex[mutex->id]);
        unsigned int reg;
-       unsigned int offset;
+       u32 reg_offset, id_offset = 0;
 
        WARN_ON(&mtx->mutex[mutex->id] != mutex);
 
@@ -838,16 +839,34 @@ 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);
+       /*
+        * Some SoCs may have multiple MUTEX_MOD registers as more than 32 mods
+        * are present, hence requiring multiple 32-bits registers.
+        *
+        * The mutex_table_mod fully represents that by defining the number of
+        * the mod sequentially, later used as a bit number, which can be more
+        * than 0..31.
+        *
+        * In order to retain compatibility with older SoCs, we perform R/W on
+        * the single 32 bits registers, but this requires us to translate the
+        * mutex ID bit accordingly.
+        */
+       if (mtx->data->mutex_table_mod[idx] < 32) {
+               reg_offset = DISP_REG_MUTEX_MOD(mtx->data->mutex_mod_reg,
+                                               mutex->id);
+       } else {
+               reg_offset = DISP_REG_MUTEX_MOD1(mtx->data->mutex_mod_reg,
+                                                mutex->id);
+               id_offset = 32;
+       }
 
+       reg = readl_relaxed(mtx->regs + reg_offset);
        if (clear)
-               reg &= ~BIT(mtx->data->mutex_table_mod[idx]);
+               reg &= ~BIT(mtx->data->mutex_table_mod[idx] - id_offset);
        else
-               reg |= BIT(mtx->data->mutex_table_mod[idx]);
+               reg |= BIT(mtx->data->mutex_table_mod[idx] - id_offset);
 
-       writel_relaxed(reg, mtx->regs + offset);
+       writel_relaxed(reg, mtx->regs + reg_offset);
 
        return 0;
 }