clk: stm32mp13: add stm32_gate management

Just to introduce management of a stm32 gate clock.

Signed-off-by: Gabriel Fernandez <gabriel.fernandez@foss.st.com>
Link: https://lore.kernel.org/r/20220516070600.7692-5-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 9869909..ca2605d 100644 (file)
--- a/drivers/clk/stm32/clk-stm32-core.c
+++ b/drivers/clk/stm32/clk-stm32-core.c
@@ -124,6 +124,57 @@ static int stm32_mux_set_parent(void __iomem *base,
        return 0;
 }
 
+static void stm32_gate_endisable(void __iomem *base,
+                                struct clk_stm32_clock_data *data,
+                                u16 gate_id, int enable)
+{
+       const struct stm32_gate_cfg *gate = &data->gates[gate_id];
+       void __iomem *addr = base + gate->offset;
+
+       if (enable) {
+               if (data->gate_cpt[gate_id]++ > 0)
+                       return;
+
+               if (gate->set_clr != 0)
+                       writel(BIT(gate->bit_idx), addr);
+               else
+                       writel(readl(addr) | BIT(gate->bit_idx), addr);
+       } else {
+               if (--data->gate_cpt[gate_id] > 0)
+                       return;
+
+               if (gate->set_clr != 0)
+                       writel(BIT(gate->bit_idx), addr + gate->set_clr);
+               else
+                       writel(readl(addr) & ~BIT(gate->bit_idx), addr);
+       }
+}
+
+static void stm32_gate_disable_unused(void __iomem *base,
+                                     struct clk_stm32_clock_data *data,
+                                     u16 gate_id)
+{
+       const struct stm32_gate_cfg *gate = &data->gates[gate_id];
+       void __iomem *addr = base + gate->offset;
+
+       if (data->gate_cpt[gate_id] > 0)
+               return;
+
+       if (gate->set_clr != 0)
+               writel(BIT(gate->bit_idx), addr + gate->set_clr);
+       else
+               writel(readl(addr) & ~BIT(gate->bit_idx), addr);
+}
+
+static int stm32_gate_is_enabled(void __iomem *base,
+                                struct clk_stm32_clock_data *data,
+                                u16 gate_id)
+{
+       const struct stm32_gate_cfg *gate = &data->gates[gate_id];
+
+       return (readl(base + gate->offset) & BIT(gate->bit_idx)) != 0;
+}
+
 static u8 clk_stm32_mux_get_parent(struct clk_hw *hw)
 {
        struct clk_stm32_mux *mux = to_clk_stm32_mux(hw);
@@ -150,6 +201,56 @@ const struct clk_ops clk_stm32_mux_ops = {
        .set_parent     = clk_stm32_mux_set_parent,
 };
 
+static void clk_stm32_gate_endisable(struct clk_hw *hw, int enable)
+{
+       struct clk_stm32_gate *gate = to_clk_stm32_gate(hw);
+       unsigned long flags = 0;
+
+       spin_lock_irqsave(gate->lock, flags);
+
+       stm32_gate_endisable(gate->base, gate->clock_data, gate->gate_id, enable);
+
+       spin_unlock_irqrestore(gate->lock, flags);
+}
+
+static int clk_stm32_gate_enable(struct clk_hw *hw)
+{
+       clk_stm32_gate_endisable(hw, 1);
+
+       return 0;
+}
+
+static void clk_stm32_gate_disable(struct clk_hw *hw)
+{
+       clk_stm32_gate_endisable(hw, 0);
+}
+
+static int clk_stm32_gate_is_enabled(struct clk_hw *hw)
+{
+       struct clk_stm32_gate *gate = to_clk_stm32_gate(hw);
+
+       return stm32_gate_is_enabled(gate->base, gate->clock_data, gate->gate_id);
+}
+
+static void clk_stm32_gate_disable_unused(struct clk_hw *hw)
+{
+       struct clk_stm32_gate *gate = to_clk_stm32_gate(hw);
+       unsigned long flags = 0;
+
+       spin_lock_irqsave(gate->lock, flags);
+
+       stm32_gate_disable_unused(gate->base, gate->clock_data, gate->gate_id);
+
+       spin_unlock_irqrestore(gate->lock, flags);
+}
+
+const struct clk_ops clk_stm32_gate_ops = {
+       .enable         = clk_stm32_gate_enable,
+       .disable        = clk_stm32_gate_disable,
+       .is_enabled     = clk_stm32_gate_is_enabled,
+       .disable_unused = clk_stm32_gate_disable_unused,
+};
+
 struct clk_hw *clk_stm32_mux_register(struct device *dev,
                                      const struct stm32_rcc_match_data *data,
                                      void __iomem *base,
@@ -170,3 +271,24 @@ struct clk_hw *clk_stm32_mux_register(struct device *dev,
 
        return hw;
 }
+
+struct clk_hw *clk_stm32_gate_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_gate *gate = cfg->clock_cfg;
+       struct clk_hw *hw = &gate->hw;
+       int err;
+
+       gate->base = base;
+       gate->lock = lock;
+       gate->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 8563e6e..f958ef6 100644 (file)
--- a/drivers/clk/stm32/clk-stm32-core.h
+++ b/drivers/clk/stm32/clk-stm32-core.h
@@ -94,8 +94,19 @@ struct clk_stm32_mux {
 
 #define to_clk_stm32_mux(_hw) container_of(_hw, struct clk_stm32_mux, hw)
 
+struct clk_stm32_gate {
+       u16 gate_id;
+       struct clk_hw hw;
+       void __iomem *base;
+       struct clk_stm32_clock_data *clock_data;
+       spinlock_t *lock; /* spin lock */
+};
+
+#define to_clk_stm32_gate(_hw) container_of(_hw, struct clk_stm32_gate, hw)
+
 /* Clock operators */
 extern const struct clk_ops clk_stm32_mux_ops;
+extern const struct clk_ops clk_stm32_gate_ops;
 
 /* Clock registering */
 struct clk_hw *clk_stm32_mux_register(struct device *dev,
@@ -104,6 +115,12 @@ struct clk_hw *clk_stm32_mux_register(struct device *dev,
                                      spinlock_t *lock,
                                      const struct clock_config *cfg);
 
+struct clk_hw *clk_stm32_gate_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),\
@@ -114,3 +131,7 @@ struct clk_hw *clk_stm32_mux_register(struct device *dev,
 #define STM32_MUX_CFG(_binding, _clk)\
        STM32_CLOCK_CFG(_binding, &(_clk), struct clk_stm32_mux *,\
                        &clk_stm32_mux_register)
+
+#define STM32_GATE_CFG(_binding, _clk)\
+       STM32_CLOCK_CFG(_binding, &(_clk), struct clk_stm32_gate *,\
+                       &clk_stm32_gate_register)

diff --git a/drivers/clk/stm32/clk-stm32mp13.c b/drivers/clk/stm32/clk-stm32mp13.c
index 40568a6..55326d4 100644 (file)
--- a/drivers/clk/stm32/clk-stm32mp13.c
+++ b/drivers/clk/stm32/clk-stm32mp13.c
@@ -410,8 +410,14 @@ static struct clk_stm32_mux ck_ker_eth1 = {
                                       CLK_OPS_PARENT_ENABLE | CLK_SET_RATE_NO_REPARENT),
 };
 
+static struct clk_stm32_gate eth1ck_k = {
+       .gate_id = GATE_ETH1CK,
+       .hw.init = CLK_HW_INIT_HW("eth1ck_k", &ck_ker_eth1.hw, &clk_stm32_gate_ops, 0),
+};
+
 static const struct clock_config stm32mp13_clock_cfg[] = {
        STM32_MUX_CFG(NO_ID, ck_ker_eth1),
+       STM32_GATE_CFG(ETH1CK_K, eth1ck_k),
 };
 
 static u16 stm32mp13_cpt_gate[GATE_NB];