[PATCH v2 05/13] clk: stm32mp13: add stm32 divider clock

[<gabriel.fernandez@foss.st.com>]

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

Just to introduce management of a stm32 divider clock

Signed-off-by: Gabriel Fernandez <gabriel.fernandez@foss.st.com>
---
 drivers/clk/stm32/clk-stm32-core.c | 161 +++++++++++++++++++++++++++++
 drivers/clk/stm32/clk-stm32-core.h |  36 +++++++
 drivers/clk/stm32/clk-stm32mp13.c  |   7 ++
 3 files changed, 204 insertions(+)

diff --git a/drivers/clk/stm32/clk-stm32-core.c b/drivers/clk/stm32/clk-stm32-core.c
index 91eae0386e5e..57d039168779 100644
--- a/drivers/clk/stm32/clk-stm32-core.c
+++ b/drivers/clk/stm32/clk-stm32-core.c
@@ -195,6 +195,146 @@ const struct clk_ops clk_stm32_gate_ops = {
 	.disable_unused	= clk_stm32_gate_disable_unused,
 };
 
+static unsigned int _get_table_div(const struct clk_div_table *table,
+				   unsigned int val)
+{
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++)
+		if (clkt->val == val)
+			return clkt->div;
+	return 0;
+}
+
+static unsigned int _get_div(const struct clk_div_table *table,
+			     unsigned int val, unsigned long flags, u8 width)
+{
+	if (flags & CLK_DIVIDER_ONE_BASED)
+		return val;
+	if (flags & CLK_DIVIDER_POWER_OF_TWO)
+		return 1 << val;
+	if (table)
+		return _get_table_div(table, val);
+	return val + 1;
+}
+
+unsigned long clk_stm32_get_rate_divider(void __iomem *base,
+					 struct clk_stm32_clock_data *data,
+					 u16 div_id, unsigned long parent_rate)
+{
+	const struct stm32_div_cfg *divider = &data->dividers[div_id];
+	unsigned int val;
+	unsigned int div;
+
+	val =  readl(base + divider->offset) >> divider->shift;
+	val &= clk_div_mask(divider->width);
+	div = _get_div(divider->table, val, divider->flags, divider->width);
+
+	if (!div) {
+		WARN(!(divider->flags & CLK_DIVIDER_ALLOW_ZERO),
+		     "%d: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
+		     div_id);
+		return parent_rate;
+	}
+
+	return DIV_ROUND_UP_ULL((u64)parent_rate, div);
+}
+
+int clk_stm32_set_rate_divider(void __iomem *base,
+			       struct clk_stm32_clock_data *data,
+			       u16 div_id,
+			       unsigned long rate,
+			       unsigned long parent_rate)
+{
+	const struct stm32_div_cfg *divider = &data->dividers[div_id];
+	int value;
+	u32 val;
+
+	value = divider_get_val(rate, parent_rate, divider->table,
+				divider->width, divider->flags);
+	if (value < 0)
+		return value;
+
+	if (divider->flags & CLK_DIVIDER_HIWORD_MASK) {
+		val = clk_div_mask(divider->width) << (divider->shift + 16);
+	} else {
+		val = readl(base + divider->offset);
+		val &= ~(clk_div_mask(divider->width) << divider->shift);
+	}
+
+	val |= (u32)value << divider->shift;
+
+	writel(val, base + divider->offset);
+
+	return 0;
+}
+
+int clk_stm32_divider_set_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long parent_rate)
+{
+	struct clk_stm32_div *div = to_clk_stm32_divider(hw);
+	unsigned long flags = 0;
+	int ret;
+
+	if (div->div_id == NO_STM32_DIV)
+		return rate;
+
+	spin_lock_irqsave(div->lock, flags);
+
+	ret = clk_stm32_set_rate_divider(div->base, div->clock_data, div->div_id,
+					 rate, parent_rate);
+
+	spin_unlock_irqrestore(div->lock, flags);
+
+	return ret;
+}
+
+long clk_stm32_divider_round_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long *prate)
+{
+	struct clk_stm32_div *div = to_clk_stm32_divider(hw);
+	const struct stm32_div_cfg *divider;
+
+	if (div->div_id == NO_STM32_DIV)
+		return rate;
+
+	divider = &div->clock_data->dividers[div->div_id];
+
+	/* if read only, just return current value */
+	if (divider->flags & CLK_DIVIDER_READ_ONLY) {
+		u32 val;
+
+		val =  readl(div->base + divider->offset) >> divider->shift;
+		val &= clk_div_mask(divider->width);
+
+		return divider_ro_round_rate(hw, rate, prate, divider->table,
+				divider->width, divider->flags,
+				val);
+	}
+
+	return divider_round_rate_parent(hw, clk_hw_get_parent(hw),
+					 rate, prate, divider->table,
+					 divider->width, divider->flags);
+}
+
+unsigned long clk_stm32_divider_recalc_rate(struct clk_hw *hw,
+					    unsigned long parent_rate)
+{
+	struct clk_stm32_div *div = to_clk_stm32_divider(hw);
+
+	if (div->div_id == NO_STM32_DIV)
+		return parent_rate;
+
+	return clk_stm32_get_rate_divider(div->base, div->clock_data,
+					  div->div_id, parent_rate);
+}
+
+const struct clk_ops clk_stm32_divider_ops = {
+	.recalc_rate	= clk_stm32_divider_recalc_rate,
+	.round_rate	= clk_stm32_divider_round_rate,
+	.set_rate	= clk_stm32_divider_set_rate,
+};
+
 u8 clk_stm32_get_parent_mux(void __iomem *base,
 			    struct clk_stm32_clock_data *data,
 			    u16 mux_id)
@@ -274,6 +414,27 @@ struct clk_hw *clk_stm32_gate_register(struct device *dev,
 	return hw;
 }
 
+struct clk_hw *clk_stm32_div_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_div *div = cfg->clock_cfg;
+	struct clk_hw *hw = &div->hw;
+	int err;
+
+	div->base = base;
+	div->lock = lock;
+	div->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 1b4a73556512..833a3d4a064f 100644
--- a/drivers/clk/stm32/clk-stm32-core.h
+++ b/drivers/clk/stm32/clk-stm32-core.h
@@ -94,6 +94,16 @@ struct clk_stm32_gate {
 
 #define to_clk_stm32_gate(_hw) container_of(_hw, struct clk_stm32_gate, hw)
 
+struct clk_stm32_div {
+	u16 div_id;
+	struct clk_hw hw;
+	void __iomem *base;
+	struct clk_stm32_clock_data *clock_data;
+	spinlock_t *lock; /* spin lock */
+};
+
+#define to_clk_stm32_divider(_hw) container_of(_hw, struct clk_stm32_div, hw)
+
 struct clk_stm32_mux {
 	u16 mux_id;
 	struct clk_hw hw;
@@ -121,6 +131,21 @@ 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);
 
+int clk_stm32_set_rate_divider(void __iomem *base,
+			       struct clk_stm32_clock_data *data,
+			       u16 div_id, unsigned long rate,
+			       unsigned long parent_rate);
+unsigned long clk_stm32_get_rate_divider(void __iomem *base,
+					 struct clk_stm32_clock_data *data,
+					 u16 div_id, unsigned long parent_rate);
+
+int clk_stm32_divider_set_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long parent_rate);
+long clk_stm32_divider_round_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long *prate);
+unsigned long clk_stm32_divider_recalc_rate(struct clk_hw *hw,
+					    unsigned long parent_rate);
+
 u8 clk_stm32_get_parent_mux(void __iomem *base,
 			    struct clk_stm32_clock_data *data, u16 mux_id);
 
@@ -132,6 +157,7 @@ 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_divider_ops;
 extern const struct clk_ops clk_stm32_mux_ops;
 
 /* Clock registering */
@@ -141,6 +167,12 @@ struct clk_hw *clk_stm32_gate_register(struct device *dev,
 				       spinlock_t *lock,
 				       const struct clock_config *cfg);
 
+struct clk_hw *clk_stm32_div_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,
@@ -158,6 +190,10 @@ struct clk_hw *clk_stm32_mux_register(struct device *dev,
 	STM32_CLOCK_CFG(_binding, &(_clk), struct clk_stm32_gate *,\
 			&clk_stm32_gate_register)
 
+#define STM32_DIV_CFG(_binding, _clk)\
+	STM32_CLOCK_CFG(_binding, &(_clk), struct clk_stm32_div *,\
+			&clk_stm32_div_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 24c0c9ff3602..66ebfe810b08 100644
--- a/drivers/clk/stm32/clk-stm32mp13.c
+++ b/drivers/clk/stm32/clk-stm32mp13.c
@@ -415,9 +415,16 @@ static struct clk_stm32_gate eth1ck_k = {
 	.hw.init = CLK_HW_INIT_HW("eth1ck_k", &ck_ker_eth1.hw, &clk_stm32_gate_ops, 0),
 };
 
+static struct clk_stm32_div eth1ptp_k = {
+	.div_id = DIV_ETH1PTP,
+	.hw.init = CLK_HW_INIT_HW("eth1ptp_k", &ck_ker_eth1.hw, &clk_stm32_divider_ops,
+				  CLK_SET_RATE_NO_REPARENT),
+};
+
 static const struct clock_config stm32mp13_clock_cfg[] = {
 	STM32_MUX_CFG(NO_ID, ck_ker_eth1),
 	STM32_GATE_CFG(ETH1CK_K, eth1ck_k),
+	STM32_DIV_CFG(ETH1PTP_K, eth1ptp_k),
 };
 
 u16 stm32mp13_cpt_gate[GATE_NB];
-- 
2.25.1