clk: stm32mp13: add safe mux management

Some muxes need to set a the safe position when clock is off.

Signed-off-by: Gabriel Fernandez <gabriel.fernandez@foss.st.com>
Link: https://lore.kernel.org/r/20220516070600.7692-12-gabriel.fernandez@foss.st.com
Signed-off-by: Stephen Boyd <sboyd@kernel.org>

diff --git a/drivers/clk/stm32/clk-stm32-core.c b/drivers/clk/stm32/clk-stm32-core.c
index e5a22bb..45a279e 100644 (file)
--- a/drivers/clk/stm32/clk-stm32-core.c
+++ b/drivers/clk/stm32/clk-stm32-core.c
@@ -495,6 +495,54 @@ static int clk_stm32_composite_is_enabled(struct clk_hw *hw)
        return stm32_gate_is_enabled(composite->base, composite->clock_data, composite->gate_id);
 }
 
+#define MUX_SAFE_POSITION 0
+
+static int clk_stm32_has_safe_mux(struct clk_hw *hw)
+{
+       struct clk_stm32_composite *composite = to_clk_stm32_composite(hw);
+       const struct stm32_mux_cfg *mux = &composite->clock_data->muxes[composite->mux_id];
+
+       return !!(mux->flags & MUX_SAFE);
+}
+
+static void clk_stm32_set_safe_position_mux(struct clk_hw *hw)
+{
+       struct clk_stm32_composite *composite = to_clk_stm32_composite(hw);
+
+       if (!clk_stm32_composite_is_enabled(hw)) {
+               unsigned long flags = 0;
+
+               if (composite->clock_data->is_multi_mux) {
+                       struct clk_hw *other_mux_hw = NULL;
+
+                       other_mux_hw = composite->clock_data->is_multi_mux(hw);
+
+                       if (!other_mux_hw || clk_stm32_composite_is_enabled(other_mux_hw))
+                               return;
+               }
+
+               spin_lock_irqsave(composite->lock, flags);
+
+               stm32_mux_set_parent(composite->base, composite->clock_data,
+                                    composite->mux_id, MUX_SAFE_POSITION);
+
+               spin_unlock_irqrestore(composite->lock, flags);
+       }
+}
+
+static void clk_stm32_safe_restore_position_mux(struct clk_hw *hw)
+{
+       struct clk_stm32_composite *composite = to_clk_stm32_composite(hw);
+       int sel = clk_hw_get_parent_index(hw);
+       unsigned long flags = 0;
+
+       spin_lock_irqsave(composite->lock, flags);
+
+       stm32_mux_set_parent(composite->base, composite->clock_data, composite->mux_id, sel);
+
+       spin_unlock_irqrestore(composite->lock, flags);
+}
+
 static void clk_stm32_composite_gate_endisable(struct clk_hw *hw, int enable)
 {
        struct clk_stm32_composite *composite = to_clk_stm32_composite(hw);
@@ -516,6 +564,9 @@ static int clk_stm32_composite_gate_enable(struct clk_hw *hw)
 
        clk_stm32_composite_gate_endisable(hw, 1);
 
+       if (composite->mux_id != NO_STM32_MUX && clk_stm32_has_safe_mux(hw))
+               clk_stm32_safe_restore_position_mux(hw);
+
        return 0;
 }
 
@@ -527,6 +578,9 @@ static void clk_stm32_composite_gate_disable(struct clk_hw *hw)
                return;
 
        clk_stm32_composite_gate_endisable(hw, 0);
+
+       if (composite->mux_id != NO_STM32_MUX && clk_stm32_has_safe_mux(hw))
+               clk_stm32_set_safe_position_mux(hw);
 }
 
 static void clk_stm32_composite_disable_unused(struct clk_hw *hw)

diff --git a/drivers/clk/stm32/clk-stm32-core.h b/drivers/clk/stm32/clk-stm32-core.h
index dab1b65..76cffda 100644 (file)
--- a/drivers/clk/stm32/clk-stm32-core.h
+++ b/drivers/clk/stm32/clk-stm32-core.h
@@ -84,6 +84,7 @@ int stm32_rcc_init(struct device *dev, const struct of_device_id *match_data,
 
 /* MUX define */
 #define MUX_NO_RDY             0xFF
+#define MUX_SAFE               BIT(7)
 
 /* DIV define */
 #define DIV_NO_RDY             0xFF

diff --git a/drivers/clk/stm32/clk-stm32mp13.c b/drivers/clk/stm32/clk-stm32mp13.c
index 08e3fe0..1192eee 100644 (file)
--- a/drivers/clk/stm32/clk-stm32mp13.c
+++ b/drivers/clk/stm32/clk-stm32mp13.c
@@ -359,6 +359,9 @@ enum enum_mux_cfg {
 #define CFG_MUX(_id, _offset, _shift, _witdh)\
        _CFG_MUX(_id, _offset, _shift, _witdh, MUX_NO_RDY, 0)
 
+#define CFG_MUX_SAFE(_id, _offset, _shift, _witdh)\
+       _CFG_MUX(_id, _offset, _shift, _witdh, MUX_NO_RDY, MUX_SAFE)
+
 static const struct stm32_mux_cfg stm32mp13_muxes[] = {
        CFG_MUX(MUX_I2C12,      RCC_I2C12CKSELR,        0, 3),
        CFG_MUX(MUX_LPTIM45,    RCC_LPTIM45CKSELR,      0, 3),
@@ -394,10 +397,10 @@ static const struct stm32_mux_cfg stm32mp13_muxes[] = {
        CFG_MUX(MUX_UART6,      RCC_UART6CKSELR,        0, 3),
        CFG_MUX(MUX_USBO,       RCC_USBCKSELR,          4, 1),
        CFG_MUX(MUX_USBPHY,     RCC_USBCKSELR,          0, 2),
-       CFG_MUX(MUX_FMC,        RCC_FMCCKSELR,          0, 2),
-       CFG_MUX(MUX_QSPI,       RCC_QSPICKSELR,         0, 2),
-       CFG_MUX(MUX_SDMMC1,     RCC_SDMMC12CKSELR,      0, 3),
-       CFG_MUX(MUX_SDMMC2,     RCC_SDMMC12CKSELR,      3, 3),
+       CFG_MUX_SAFE(MUX_FMC,   RCC_FMCCKSELR,          0, 2),
+       CFG_MUX_SAFE(MUX_QSPI,  RCC_QSPICKSELR,         0, 2),
+       CFG_MUX_SAFE(MUX_SDMMC1, RCC_SDMMC12CKSELR,     0, 3),
+       CFG_MUX_SAFE(MUX_SDMMC2, RCC_SDMMC12CKSELR,     3, 3),
 };
 
 struct clk_stm32_securiy {