[PATCH 04/13] clk: stm32mp13: add stm32_gate management

[gabriel.fernandez at foss.st.com]

From: Gabriel Fernandez <gabriel.fernandez at foss.st.com>

Just to introduce management of a stm32 gate clock.

Change-Id: I9c823f2742e8680bc131adcb854ad50ee66e682b
Signed-off-by: Gabriel Fernandez <gabriel.fernandez at foss.st.com>
---
 drivers/clk/stm32/clk-stm32-core.c | 124 +++++++++++++++++++++++++++++
 drivers/clk/stm32/clk-stm32-core.h |  37 +++++++++
 drivers/clk/stm32/clk-stm32mp13.c  |   6 ++
 3 files changed, 167 insertions(+)

diff --git a/drivers/clk/stm32/clk-stm32-core.c b/drivers/clk/stm32/clk-stm32-core.c
index f34659625aff..91eae0386e5e 100644
--- a/drivers/clk/stm32/clk-stm32-core.c
+++ b/drivers/clk/stm32/clk-stm32-core.c
@@ -92,6 +92,109 @@ int stm32_rcc_init(struct device *dev, const struct of_device_id *match_data,
 	return 0;
 }
 
+void clk_stm32_endisable_gate(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);
+	}
+}
+
+void clk_stm32_disable_unused_gate(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);
+}
+
+int clk_stm32_is_enabled_gate(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;
+}
+
+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);
+
+	clk_stm32_endisable_gate(gate->base, gate->clock_data,
+				 gate->gate_id, enable);
+
+	spin_unlock_irqrestore(gate->lock, flags);
+}
+
+int clk_stm32_gate_enable(struct clk_hw *hw)
+{
+	clk_stm32_gate_endisable(hw, 1);
+
+	return 0;
+}
+
+void clk_stm32_gate_disable(struct clk_hw *hw)
+{
+	clk_stm32_gate_endisable(hw, 0);
+}
+
+int clk_stm32_gate_is_enabled(struct clk_hw *hw)
+{
+	struct clk_stm32_gate *gate = to_clk_stm32_gate(hw);
+
+	return clk_stm32_is_enabled_gate(gate->base, gate->clock_data,
+					 gate->gate_id);
+}
+
+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);
+
+	clk_stm32_disable_unused_gate(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,
+};
+
 u8 clk_stm32_get_parent_mux(void __iomem *base,
 			    struct clk_stm32_clock_data *data,
 			    u16 mux_id)
@@ -150,6 +253,27 @@ const struct clk_ops clk_stm32_mux_ops = {
 	.set_parent	= clk_stm32_mux_set_parent,
 };
 
+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;
+}
+
 struct clk_hw *clk_stm32_mux_register(struct device *dev,
 				      const struct stm32_rcc_match_data *data,
 				      void __iomem *base,
diff --git a/drivers/clk/stm32/clk-stm32-core.h b/drivers/clk/stm32/clk-stm32-core.h
index 7c9f503d3388..1b4a73556512 100644
--- a/drivers/clk/stm32/clk-stm32-core.h
+++ b/drivers/clk/stm32/clk-stm32-core.h
@@ -84,6 +84,16 @@ int stm32_rcc_init(struct device *dev, const struct of_device_id *match_data,
 #define DIV_NO_RDY		0xFF
 
 /* Definition of clock structure */
+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)
+
 struct clk_stm32_mux {
 	u16 mux_id;
 	struct clk_hw hw;
@@ -95,6 +105,22 @@ struct clk_stm32_mux {
 #define to_clk_stm32_mux(_hw) container_of(_hw, struct clk_stm32_mux, hw)
 
 /* Clock ops */
+void clk_stm32_endisable_gate(void __iomem *base,
+			      struct clk_stm32_clock_data *data,
+			      u16 gate_id, int enable);
+void clk_stm32_disable_unused_gate(void __iomem *base,
+				   struct clk_stm32_clock_data *data,
+				   u16 gate_id);
+int clk_stm32_is_enabled_gate(void __iomem *base,
+			      struct clk_stm32_clock_data *data,
+			      u16 gate_id);
+
+void clk_stm32_gate_endisable(struct clk_hw *hw, int enable);
+int clk_stm32_gate_enable(struct clk_hw *hw);
+void clk_stm32_gate_disable(struct clk_hw *hw);
+int clk_stm32_gate_is_enabled(struct clk_hw *hw);
+void clk_stm32_gate_disable_unused(struct clk_hw *hw);
+
 u8 clk_stm32_get_parent_mux(void __iomem *base,
 			    struct clk_stm32_clock_data *data, u16 mux_id);
 
@@ -105,9 +131,16 @@ int clk_stm32_set_parent_mux(void __iomem *base,
 u8 clk_stm32_mux_get_parent(struct clk_hw *hw);
 int clk_stm32_mux_set_parent(struct clk_hw *hw, u8 index);
 
+extern const struct clk_ops clk_stm32_gate_ops;
 extern const struct clk_ops clk_stm32_mux_ops;
 
 /* Clock registering */
+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_hw *clk_stm32_mux_register(struct device *dev,
 				      const struct stm32_rcc_match_data *data,
 				      void __iomem *base,
@@ -121,6 +154,10 @@ struct clk_hw *clk_stm32_mux_register(struct device *dev,
 	.func		= (_register),\
 }
 
+#define STM32_GATE_CFG(_binding, _clk)\
+	STM32_CLOCK_CFG(_binding, &(_clk), struct clk_stm32_gate *,\
+			&clk_stm32_gate_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 89d4e039e4e6..24c0c9ff3602 100644
--- 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),
 };
 
 u16 stm32mp13_cpt_gate[GATE_NB];
-- 
2.25.1