diff options
| -rw-r--r-- | drivers/clk/stm32/clk-stm32-core.c | 159 | ||||
| -rw-r--r-- | drivers/clk/stm32/clk-stm32-core.h | 21 | ||||
| -rw-r--r-- | drivers/clk/stm32/clk-stm32mp13.c | 7 |
3 files changed, 187 insertions, 0 deletions
diff --git a/drivers/clk/stm32/clk-stm32-core.c b/drivers/clk/stm32/clk-stm32-core.c index ca2605da873d..da38eca2b8c5 100644 --- a/drivers/clk/stm32/clk-stm32-core.c +++ b/drivers/clk/stm32/clk-stm32-core.c @@ -175,6 +175,80 @@ static int stm32_gate_is_enabled(void __iomem *base, return (readl(base + gate->offset) & BIT(gate->bit_idx)) != 0; } +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; +} + +static unsigned long stm32_divider_get_rate(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); +} + +static int stm32_divider_set_rate(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; +} + static u8 clk_stm32_mux_get_parent(struct clk_hw *hw) { struct clk_stm32_mux *mux = to_clk_stm32_mux(hw); @@ -251,6 +325,70 @@ const struct clk_ops clk_stm32_gate_ops = { .disable_unused = clk_stm32_gate_disable_unused, }; +static 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 = stm32_divider_set_rate(div->base, div->clock_data, div->div_id, rate, parent_rate); + + spin_unlock_irqrestore(div->lock, flags); + + return ret; +} + +static 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); +} + +static 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 stm32_divider_get_rate(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, +}; + struct clk_hw *clk_stm32_mux_register(struct device *dev, const struct stm32_rcc_match_data *data, void __iomem *base, @@ -292,3 +430,24 @@ 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; +} diff --git a/drivers/clk/stm32/clk-stm32-core.h b/drivers/clk/stm32/clk-stm32-core.h index f958ef610f72..c2de58a22aa8 100644 --- a/drivers/clk/stm32/clk-stm32-core.h +++ b/drivers/clk/stm32/clk-stm32-core.h @@ -104,9 +104,20 @@ 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) + /* Clock operators */ extern const struct clk_ops clk_stm32_mux_ops; extern const struct clk_ops clk_stm32_gate_ops; +extern const struct clk_ops clk_stm32_divider_ops; /* Clock registering */ struct clk_hw *clk_stm32_mux_register(struct device *dev, @@ -121,6 +132,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); + #define STM32_CLOCK_CFG(_binding, _clk, _struct, _register)\ {\ .id = (_binding),\ @@ -135,3 +152,7 @@ struct clk_hw *clk_stm32_gate_register(struct device *dev, #define STM32_GATE_CFG(_binding, _clk)\ 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) diff --git a/drivers/clk/stm32/clk-stm32mp13.c b/drivers/clk/stm32/clk-stm32mp13.c index 55326d4d34dd..d93d92b5fe82 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), }; static u16 stm32mp13_cpt_gate[GATE_NB]; |