clk: stm32mp13: add stm32_mux clock management

Just to introduce management of a stm32 mux clock.

Signed-off-by: Gabriel Fernandez <gabriel.fernandez@foss.st.com>
Link: https://lore.kernel.org/r/20220516070600.7692-4-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 0fab4a5..9869909 100644 (file)
--- a/drivers/clk/stm32/clk-stm32-core.c
+++ b/drivers/clk/stm32/clk-stm32-core.c
@@ -91,3 +91,82 @@ int stm32_rcc_init(struct device *dev, const struct of_device_id *match_data,
 
        return 0;
 }
+
+static u8 stm32_mux_get_parent(void __iomem *base,
+                              struct clk_stm32_clock_data *data,
+                              u16 mux_id)
+{
+       const struct stm32_mux_cfg *mux = &data->muxes[mux_id];
+       u32 mask = BIT(mux->width) - 1;
+       u32 val;
+
+       val = readl(base + mux->offset) >> mux->shift;
+       val &= mask;
+
+       return val;
+}
+
+static int stm32_mux_set_parent(void __iomem *base,
+                               struct clk_stm32_clock_data *data,
+                               u16 mux_id, u8 index)
+{
+       const struct stm32_mux_cfg *mux = &data->muxes[mux_id];
+
+       u32 mask = BIT(mux->width) - 1;
+       u32 reg = readl(base + mux->offset);
+       u32 val = index << mux->shift;
+
+       reg &= ~(mask << mux->shift);
+       reg |= val;
+
+       writel(reg, base + mux->offset);
+
+       return 0;
+}
+
+static u8 clk_stm32_mux_get_parent(struct clk_hw *hw)
+{
+       struct clk_stm32_mux *mux = to_clk_stm32_mux(hw);
+
+       return stm32_mux_get_parent(mux->base, mux->clock_data, mux->mux_id);
+}
+
+static int clk_stm32_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+       struct clk_stm32_mux *mux = to_clk_stm32_mux(hw);
+       unsigned long flags = 0;
+
+       spin_lock_irqsave(mux->lock, flags);
+
+       stm32_mux_set_parent(mux->base, mux->clock_data, mux->mux_id, index);
+
+       spin_unlock_irqrestore(mux->lock, flags);
+
+       return 0;
+}
+
+const struct clk_ops clk_stm32_mux_ops = {
+       .get_parent     = clk_stm32_mux_get_parent,
+       .set_parent     = clk_stm32_mux_set_parent,
+};
+
+struct clk_hw *clk_stm32_mux_register(struct device *dev,
+                                     const struct stm32_rcc_match_data *data,
+                                     void __iomem *base,
+                                     spinlock_t *lock,
+                                     const struct clock_config *cfg)
+{
+       struct clk_stm32_mux *mux = cfg->clock_cfg;
+       struct clk_hw *hw = &mux->hw;
+       int err;
+
+       mux->base = base;
+       mux->lock = lock;
+       mux->clock_data = data->clock_data;
+
+       err = clk_hw_register(dev, hw);
+       if (err)
+               return ERR_PTR(err);
+
+       return hw;
+}

diff --git a/drivers/clk/stm32/clk-stm32-core.h b/drivers/clk/stm32/clk-stm32-core.h
index 519723a..8563e6e 100644 (file)
--- a/drivers/clk/stm32/clk-stm32-core.h
+++ b/drivers/clk/stm32/clk-stm32-core.h
@@ -83,10 +83,34 @@ int stm32_rcc_init(struct device *dev, const struct of_device_id *match_data,
 /* DIV define */
 #define DIV_NO_RDY             0xFF
 
+/* Definition of clock structure */
+struct clk_stm32_mux {
+       u16 mux_id;
+       struct clk_hw hw;
+       void __iomem *base;
+       struct clk_stm32_clock_data *clock_data;
+       spinlock_t *lock; /* spin lock */
+};
+
+#define to_clk_stm32_mux(_hw) container_of(_hw, struct clk_stm32_mux, hw)
+
+/* Clock operators */
+extern const struct clk_ops clk_stm32_mux_ops;
+
 /* Clock registering */
+struct clk_hw *clk_stm32_mux_register(struct device *dev,
+                                     const struct stm32_rcc_match_data *data,
+                                     void __iomem *base,
+                                     spinlock_t *lock,
+                                     const struct clock_config *cfg);
+
 #define STM32_CLOCK_CFG(_binding, _clk, _struct, _register)\
 {\
        .id             = (_binding),\
        .clock_cfg      = (_struct) {_clk},\
        .func           = (_register),\
 }
+
+#define STM32_MUX_CFG(_binding, _clk)\
+       STM32_CLOCK_CFG(_binding, &(_clk), struct clk_stm32_mux *,\
+                       &clk_stm32_mux_register)

diff --git a/drivers/clk/stm32/clk-stm32mp13.c b/drivers/clk/stm32/clk-stm32mp13.c
index a2a6bbb..40568a6 100644 (file)
--- a/drivers/clk/stm32/clk-stm32mp13.c
+++ b/drivers/clk/stm32/clk-stm32mp13.c
@@ -400,7 +400,18 @@ static const struct stm32_mux_cfg stm32mp13_muxes[] = {
        CFG_MUX(MUX_SDMMC2,     RCC_SDMMC12CKSELR,      3, 3),
 };
 
+static const char * const eth12_src[] = {
+       "pll4_p", "pll3_q"
+};
+
+static struct clk_stm32_mux ck_ker_eth1 = {
+       .mux_id = MUX_ETH1,
+       .hw.init = CLK_HW_INIT_PARENTS("ck_ker_eth1", eth12_src, &clk_stm32_mux_ops,
+                                      CLK_OPS_PARENT_ENABLE | CLK_SET_RATE_NO_REPARENT),
+};
+
 static const struct clock_config stm32mp13_clock_cfg[] = {
+       STM32_MUX_CFG(NO_ID, ck_ker_eth1),
 };
 
 static u16 stm32mp13_cpt_gate[GATE_NB];