diff options
Diffstat (limited to 'drivers/clk/renesas/rzv2h-cpg.c')
| -rw-r--r-- | drivers/clk/renesas/rzv2h-cpg.c | 1693 |
1 files changed, 1693 insertions, 0 deletions
diff --git a/drivers/clk/renesas/rzv2h-cpg.c b/drivers/clk/renesas/rzv2h-cpg.c new file mode 100644 index 000000000000..3f6299b9fec0 --- /dev/null +++ b/drivers/clk/renesas/rzv2h-cpg.c @@ -0,0 +1,1693 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Renesas RZ/V2H(P) Clock Pulse Generator + * + * Copyright (C) 2024 Renesas Electronics Corp. + * + * Based on rzg2l-cpg.c + * + * Copyright (C) 2015 Glider bvba + * Copyright (C) 2013 Ideas On Board SPRL + * Copyright (C) 2015 Renesas Electronics Corp. + */ + +#include <linux/bitfield.h> +#include <linux/clk.h> +#include <linux/clk-provider.h> +#include <linux/clk/renesas.h> +#include <linux/delay.h> +#include <linux/init.h> +#include <linux/iopoll.h> +#include <linux/limits.h> +#include <linux/math.h> +#include <linux/math64.h> +#include <linux/minmax.h> +#include <linux/mod_devicetable.h> +#include <linux/module.h> +#include <linux/of.h> +#include <linux/platform_device.h> +#include <linux/pm_clock.h> +#include <linux/pm_domain.h> +#include <linux/refcount.h> +#include <linux/reset-controller.h> +#include <linux/string_choices.h> +#include <linux/units.h> + +#include <dt-bindings/clock/renesas-cpg-mssr.h> + +#include "rzv2h-cpg.h" + +#ifdef DEBUG +#define WARN_DEBUG(x) WARN_ON(x) +#else +#define WARN_DEBUG(x) do { } while (0) +#endif + +#define GET_CLK_ON_OFFSET(x) (0x600 + ((x) * 4)) +#define GET_CLK_MON_OFFSET(x) (0x800 + ((x) * 4)) +#define GET_RST_OFFSET(x) (0x900 + ((x) * 4)) +#define GET_RST_MON_OFFSET(x) (0xA00 + ((x) * 4)) + +#define CPG_BUS_1_MSTOP (0xd00) +#define CPG_BUS_MSTOP(m) (CPG_BUS_1_MSTOP + ((m) - 1) * 4) + +#define CPG_PLL_STBY(x) ((x)) +#define CPG_PLL_STBY_RESETB BIT(0) +#define CPG_PLL_STBY_SSC_EN BIT(2) +#define CPG_PLL_STBY_RESETB_WEN BIT(16) +#define CPG_PLL_STBY_SSC_EN_WEN BIT(18) +#define CPG_PLL_CLK1(x) ((x) + 0x004) +#define CPG_PLL_CLK1_KDIV GENMASK(31, 16) +#define CPG_PLL_CLK1_MDIV GENMASK(15, 6) +#define CPG_PLL_CLK1_PDIV GENMASK(5, 0) +#define CPG_PLL_CLK2(x) ((x) + 0x008) +#define CPG_PLL_CLK2_SDIV GENMASK(2, 0) +#define CPG_PLL_MON(x) ((x) + 0x010) +#define CPG_PLL_MON_RESETB BIT(0) +#define CPG_PLL_MON_LOCK BIT(4) + +#define DDIV_DIVCTL_WEN(shift) BIT((shift) + 16) + +#define GET_MOD_CLK_ID(base, index, bit) \ + ((base) + ((((index) * (16))) + (bit))) + +#define CPG_CLKSTATUS0 (0x700) + +/* On RZ/G3E SoC we have two DSI PLLs */ +#define MAX_CPG_DSI_PLL 2 + +/** + * struct rzv2h_pll_dsi_info - PLL DSI information, holds the limits and parameters + * + * @pll_dsi_limits: PLL DSI parameters limits + * @pll_dsi_parameters: Calculated PLL DSI parameters + * @req_pll_dsi_rate: Requested PLL DSI rate + */ +struct rzv2h_pll_dsi_info { + const struct rzv2h_pll_limits *pll_dsi_limits; + struct rzv2h_pll_div_pars pll_dsi_parameters; + unsigned long req_pll_dsi_rate; +}; + +/** + * struct rzv2h_cpg_priv - Clock Pulse Generator Private Data + * + * @dev: CPG device + * @base: CPG register block base address + * @rmw_lock: protects register accesses + * @clks: Array containing all Core and Module Clocks + * @num_core_clks: Number of Core Clocks in clks[] + * @num_mod_clks: Number of Module Clocks in clks[] + * @resets: Array of resets + * @num_resets: Number of Module Resets in info->resets[] + * @last_dt_core_clk: ID of the last Core Clock exported to DT + * @ff_mod_status_ops: Fixed Factor Module Status Clock operations + * @mstop_count: Array of mstop values + * @rcdev: Reset controller entity + * @pll_dsi_info: Array of PLL DSI information, holds the limits and parameters + */ +struct rzv2h_cpg_priv { + struct device *dev; + void __iomem *base; + spinlock_t rmw_lock; + + struct clk **clks; + unsigned int num_core_clks; + unsigned int num_mod_clks; + struct rzv2h_reset *resets; + unsigned int num_resets; + unsigned int last_dt_core_clk; + + struct clk_ops *ff_mod_status_ops; + + atomic_t *mstop_count; + + struct reset_controller_dev rcdev; + + struct rzv2h_pll_dsi_info pll_dsi_info[MAX_CPG_DSI_PLL]; +}; + +#define rcdev_to_priv(x) container_of(x, struct rzv2h_cpg_priv, rcdev) + +struct pll_clk { + struct rzv2h_cpg_priv *priv; + struct clk_hw hw; + struct pll pll; +}; + +#define to_pll(_hw) container_of(_hw, struct pll_clk, hw) + +/** + * struct mod_clock - Module clock + * + * @priv: CPG private data + * @mstop_data: mstop data relating to module clock + * @hw: handle between common and hardware-specific interfaces + * @no_pm: flag to indicate PM is not supported + * @on_index: register offset + * @on_bit: ON/MON bit + * @mon_index: monitor register offset + * @mon_bit: monitor bit + * @ext_clk_mux_index: mux index for external clock source, or -1 if internal + */ +struct mod_clock { + struct rzv2h_cpg_priv *priv; + unsigned int mstop_data; + struct clk_hw hw; + bool no_pm; + u8 on_index; + u8 on_bit; + s8 mon_index; + u8 mon_bit; + s8 ext_clk_mux_index; +}; + +#define to_mod_clock(_hw) container_of(_hw, struct mod_clock, hw) + +/** + * struct ddiv_clk - DDIV clock + * + * @priv: CPG private data + * @div: divider clk + * @mon: monitor bit in CPG_CLKSTATUS0 register + */ +struct ddiv_clk { + struct rzv2h_cpg_priv *priv; + struct clk_divider div; + u8 mon; +}; + +#define to_ddiv_clock(_div) container_of(_div, struct ddiv_clk, div) + +/** + * struct rzv2h_ff_mod_status_clk - Fixed Factor Module Status Clock + * + * @priv: CPG private data + * @conf: fixed mod configuration + * @fix: fixed factor clock + */ +struct rzv2h_ff_mod_status_clk { + struct rzv2h_cpg_priv *priv; + struct fixed_mod_conf conf; + struct clk_fixed_factor fix; +}; + +#define to_rzv2h_ff_mod_status_clk(_hw) \ + container_of(_hw, struct rzv2h_ff_mod_status_clk, fix.hw) + +/** + * struct rzv2h_plldsi_div_clk - PLL DSI DDIV clock + * + * @dtable: divider table + * @priv: CPG private data + * @hw: divider clk + * @ddiv: divider configuration + */ +struct rzv2h_plldsi_div_clk { + const struct clk_div_table *dtable; + struct rzv2h_cpg_priv *priv; + struct clk_hw hw; + struct ddiv ddiv; +}; + +#define to_plldsi_div_clk(_hw) \ + container_of(_hw, struct rzv2h_plldsi_div_clk, hw) + +#define RZ_V2H_OSC_CLK_IN_MEGA (24 * MEGA) +#define RZV2H_MAX_DIV_TABLES (16) + +/** + * rzv2h_get_pll_pars - Finds the best combination of PLL parameters + * for a given frequency. + * + * @limits: Pointer to the structure containing the limits for the PLL parameters + * @pars: Pointer to the structure where the best calculated PLL parameters values + * will be stored + * @freq_millihz: Target output frequency in millihertz + * + * This function calculates the best set of PLL parameters (M, K, P, S) to achieve + * the desired frequency. + * There is no direct formula to calculate the PLL parameters, as it's an open + * system of equations, therefore this function uses an iterative approach to + * determine the best solution. The best solution is one that minimizes the error + * (desired frequency - actual frequency). + * + * Return: true if a valid set of parameters values is found, false otherwise. + */ +bool rzv2h_get_pll_pars(const struct rzv2h_pll_limits *limits, + struct rzv2h_pll_pars *pars, u64 freq_millihz) +{ + u64 fout_min_millihz = mul_u32_u32(limits->fout.min, MILLI); + u64 fout_max_millihz = mul_u32_u32(limits->fout.max, MILLI); + struct rzv2h_pll_pars p, best; + + if (freq_millihz > fout_max_millihz || + freq_millihz < fout_min_millihz) + return false; + + /* Initialize best error to maximum possible value */ + best.error_millihz = S64_MAX; + + for (p.p = limits->p.min; p.p <= limits->p.max; p.p++) { + u32 fref = RZ_V2H_OSC_CLK_IN_MEGA / p.p; + u16 divider; + + for (divider = 1 << limits->s.min, p.s = limits->s.min; + p.s <= limits->s.max; p.s++, divider <<= 1) { + for (p.m = limits->m.min; p.m <= limits->m.max; p.m++) { + u64 output_m, output_k_range; + s64 pll_k, output_k; + u64 fvco, output; + + /* + * The frequency generated by the PLL + divider + * is calculated as follows: + * + * With: + * Freq = Ffout = Ffvco / 2^(pll_s) + * Ffvco = (pll_m + (pll_k / 65536)) * Ffref + * Ffref = 24MHz / pll_p + * + * Freq can also be rewritten as: + * Freq = Ffvco / 2^(pll_s) + * = ((pll_m + (pll_k / 65536)) * Ffref) / 2^(pll_s) + * = (pll_m * Ffref) / 2^(pll_s) + ((pll_k / 65536) * Ffref) / 2^(pll_s) + * = output_m + output_k + * + * Every parameter has been determined at this + * point, but pll_k. + * + * Considering that: + * limits->k.min <= pll_k <= limits->k.max + * Then: + * -0.5 <= (pll_k / 65536) < 0.5 + * Therefore: + * -Ffref / (2 * 2^(pll_s)) <= output_k < Ffref / (2 * 2^(pll_s)) + */ + + /* Compute output M component (in mHz) */ + output_m = DIV_ROUND_CLOSEST_ULL(mul_u32_u32(p.m, fref) * MILLI, + divider); + /* Compute range for output K (in mHz) */ + output_k_range = DIV_ROUND_CLOSEST_ULL(mul_u32_u32(fref, MILLI), + 2 * divider); + /* + * No point in continuing if we can't achieve + * the desired frequency + */ + if (freq_millihz < (output_m - output_k_range) || + freq_millihz >= (output_m + output_k_range)) { + continue; + } + + /* + * Compute the K component + * + * Since: + * Freq = output_m + output_k + * Then: + * output_k = Freq - output_m + * = ((pll_k / 65536) * Ffref) / 2^(pll_s) + * Therefore: + * pll_k = (output_k * 65536 * 2^(pll_s)) / Ffref + */ + output_k = freq_millihz - output_m; + pll_k = div_s64(output_k * 65536ULL * divider, + fref); + pll_k = DIV_S64_ROUND_CLOSEST(pll_k, MILLI); + + /* Validate K value within allowed limits */ + if (pll_k < limits->k.min || + pll_k > limits->k.max) + continue; + + p.k = pll_k; + + /* Compute (Ffvco * 65536) */ + fvco = mul_u32_u32(p.m * 65536 + p.k, fref); + if (fvco < mul_u32_u32(limits->fvco.min, 65536) || + fvco > mul_u32_u32(limits->fvco.max, 65536)) + continue; + + /* PLL_M component of (output * 65536 * PLL_P) */ + output = mul_u32_u32(p.m * 65536, RZ_V2H_OSC_CLK_IN_MEGA); + /* PLL_K component of (output * 65536 * PLL_P) */ + output += p.k * RZ_V2H_OSC_CLK_IN_MEGA; + /* Make it in mHz */ + output *= MILLI; + output = DIV_U64_ROUND_CLOSEST(output, 65536 * p.p * divider); + + /* Check output frequency against limits */ + if (output < fout_min_millihz || + output > fout_max_millihz) + continue; + + p.error_millihz = freq_millihz - output; + p.freq_millihz = output; + + /* If an exact match is found, return immediately */ + if (p.error_millihz == 0) { + *pars = p; + return true; + } + + /* Update best match if error is smaller */ + if (abs(best.error_millihz) > abs(p.error_millihz)) + best = p; + } + } + } + + /* If no valid parameters were found, return false */ + if (best.error_millihz == S64_MAX) + return false; + + *pars = best; + return true; +} +EXPORT_SYMBOL_NS_GPL(rzv2h_get_pll_pars, "RZV2H_CPG"); + +/* + * rzv2h_get_pll_divs_pars - Finds the best combination of PLL parameters + * and divider value for a given frequency. + * + * @limits: Pointer to the structure containing the limits for the PLL parameters + * @pars: Pointer to the structure where the best calculated PLL parameters and + * divider values will be stored + * @table: Pointer to the array of valid divider values + * @table_size: Size of the divider values array + * @freq_millihz: Target output frequency in millihertz + * + * This function calculates the best set of PLL parameters (M, K, P, S) and divider + * value to achieve the desired frequency. See rzv2h_get_pll_pars() for more details + * on how the PLL parameters are calculated. + * + * freq_millihz is the desired frequency generated by the PLL followed by a + * a gear. + */ +bool rzv2h_get_pll_divs_pars(const struct rzv2h_pll_limits *limits, + struct rzv2h_pll_div_pars *pars, + const u8 *table, u8 table_size, u64 freq_millihz) +{ + struct rzv2h_pll_div_pars p, best; + + best.div.error_millihz = S64_MAX; + p.div.error_millihz = S64_MAX; + for (unsigned int i = 0; i < table_size; i++) { + if (!rzv2h_get_pll_pars(limits, &p.pll, freq_millihz * table[i])) + continue; + + p.div.divider_value = table[i]; + p.div.freq_millihz = DIV_U64_ROUND_CLOSEST(p.pll.freq_millihz, table[i]); + p.div.error_millihz = freq_millihz - p.div.freq_millihz; + + if (p.div.error_millihz == 0) { + *pars = p; + return true; + } + + if (abs(best.div.error_millihz) > abs(p.div.error_millihz)) + best = p; + } + + if (best.div.error_millihz == S64_MAX) + return false; + + *pars = best; + return true; +} +EXPORT_SYMBOL_NS_GPL(rzv2h_get_pll_divs_pars, "RZV2H_CPG"); + +static unsigned long rzv2h_cpg_plldsi_div_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct rzv2h_plldsi_div_clk *dsi_div = to_plldsi_div_clk(hw); + struct rzv2h_cpg_priv *priv = dsi_div->priv; + struct ddiv ddiv = dsi_div->ddiv; + u32 div; + + div = readl(priv->base + ddiv.offset); + div >>= ddiv.shift; + div &= clk_div_mask(ddiv.width); + div = dsi_div->dtable[div].div; + + return DIV_ROUND_CLOSEST_ULL(parent_rate, div); +} + +static int rzv2h_cpg_plldsi_div_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) +{ + struct rzv2h_plldsi_div_clk *dsi_div = to_plldsi_div_clk(hw); + struct pll_clk *pll_clk = to_pll(clk_hw_get_parent(hw)); + struct rzv2h_cpg_priv *priv = dsi_div->priv; + u8 table[RZV2H_MAX_DIV_TABLES] = { 0 }; + struct rzv2h_pll_div_pars *dsi_params; + struct rzv2h_pll_dsi_info *dsi_info; + const struct clk_div_table *div; + unsigned int i = 0; + u64 rate_millihz; + + dsi_info = &priv->pll_dsi_info[pll_clk->pll.instance]; + dsi_params = &dsi_info->pll_dsi_parameters; + + rate_millihz = mul_u32_u32(req->rate, MILLI); + if (rate_millihz == dsi_params->div.error_millihz + dsi_params->div.freq_millihz) + goto exit_determine_rate; + + for (div = dsi_div->dtable; div->div; div++) { + if (i >= RZV2H_MAX_DIV_TABLES) + return -EINVAL; + table[i++] = div->div; + } + + if (!rzv2h_get_pll_divs_pars(dsi_info->pll_dsi_limits, dsi_params, table, i, + rate_millihz)) { + dev_err(priv->dev, "failed to determine rate for req->rate: %lu\n", + req->rate); + return -EINVAL; + } + +exit_determine_rate: + req->rate = DIV_ROUND_CLOSEST_ULL(dsi_params->div.freq_millihz, MILLI); + req->best_parent_rate = req->rate * dsi_params->div.divider_value; + dsi_info->req_pll_dsi_rate = req->best_parent_rate; + + return 0; +} + +static int rzv2h_cpg_plldsi_div_set_rate(struct clk_hw *hw, + unsigned long rate, + unsigned long parent_rate) +{ + struct rzv2h_plldsi_div_clk *dsi_div = to_plldsi_div_clk(hw); + struct pll_clk *pll_clk = to_pll(clk_hw_get_parent(hw)); + struct rzv2h_cpg_priv *priv = dsi_div->priv; + struct rzv2h_pll_div_pars *dsi_params; + struct rzv2h_pll_dsi_info *dsi_info; + struct ddiv ddiv = dsi_div->ddiv; + const struct clk_div_table *clkt; + bool divider_found = false; + u32 val, shift; + + dsi_info = &priv->pll_dsi_info[pll_clk->pll.instance]; + dsi_params = &dsi_info->pll_dsi_parameters; + + for (clkt = dsi_div->dtable; clkt->div; clkt++) { + if (clkt->div == dsi_params->div.divider_value) { + divider_found = true; + break; + } + } + + if (!divider_found) + return -EINVAL; + + shift = ddiv.shift; + val = readl(priv->base + ddiv.offset) | DDIV_DIVCTL_WEN(shift); + val &= ~(clk_div_mask(ddiv.width) << shift); + val |= clkt->val << shift; + writel(val, priv->base + ddiv.offset); + + return 0; +} + +static const struct clk_ops rzv2h_cpg_plldsi_div_ops = { + .recalc_rate = rzv2h_cpg_plldsi_div_recalc_rate, + .determine_rate = rzv2h_cpg_plldsi_div_determine_rate, + .set_rate = rzv2h_cpg_plldsi_div_set_rate, +}; + +static struct clk * __init +rzv2h_cpg_plldsi_div_clk_register(const struct cpg_core_clk *core, + struct rzv2h_cpg_priv *priv) +{ + struct rzv2h_plldsi_div_clk *clk_hw_data; + struct clk **clks = priv->clks; + struct clk_init_data init; + const struct clk *parent; + const char *parent_name; + struct clk_hw *clk_hw; + int ret; + + parent = clks[core->parent]; + if (IS_ERR(parent)) + return ERR_CAST(parent); + + clk_hw_data = devm_kzalloc(priv->dev, sizeof(*clk_hw_data), GFP_KERNEL); + if (!clk_hw_data) + return ERR_PTR(-ENOMEM); + + clk_hw_data->priv = priv; + clk_hw_data->ddiv = core->cfg.ddiv; + clk_hw_data->dtable = core->dtable; + + parent_name = __clk_get_name(parent); + init.name = core->name; + init.ops = &rzv2h_cpg_plldsi_div_ops; + init.flags = core->flag; + init.parent_names = &parent_name; + init.num_parents = 1; + + clk_hw = &clk_hw_data->hw; + clk_hw->init = &init; + + ret = devm_clk_hw_register(priv->dev, clk_hw); + if (ret) + return ERR_PTR(ret); + + return clk_hw->clk; +} + +static int rzv2h_cpg_plldsi_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) +{ + struct pll_clk *pll_clk = to_pll(hw); + struct rzv2h_cpg_priv *priv = pll_clk->priv; + struct rzv2h_pll_dsi_info *dsi_info; + u64 rate_millihz; + + dsi_info = &priv->pll_dsi_info[pll_clk->pll.instance]; + /* check if the divider has already invoked the algorithm */ + if (req->rate == dsi_info->req_pll_dsi_rate) + return 0; + + /* If the req->rate doesn't match we do the calculation assuming there is no divider */ + rate_millihz = mul_u32_u32(req->rate, MILLI); + if (!rzv2h_get_pll_pars(dsi_info->pll_dsi_limits, + &dsi_info->pll_dsi_parameters.pll, rate_millihz)) { + dev_err(priv->dev, + "failed to determine rate for req->rate: %lu\n", + req->rate); + return -EINVAL; + } + + req->rate = DIV_ROUND_CLOSEST_ULL(dsi_info->pll_dsi_parameters.pll.freq_millihz, MILLI); + dsi_info->req_pll_dsi_rate = req->rate; + + return 0; +} + +static int rzv2h_cpg_pll_set_rate(struct pll_clk *pll_clk, + struct rzv2h_pll_pars *params, + bool ssc_disable) +{ + struct rzv2h_cpg_priv *priv = pll_clk->priv; + u16 offset = pll_clk->pll.offset; + u32 val; + int ret; + + /* Put PLL into standby mode */ + writel(CPG_PLL_STBY_RESETB_WEN, priv->base + CPG_PLL_STBY(offset)); + ret = readl_poll_timeout_atomic(priv->base + CPG_PLL_MON(offset), + val, !(val & CPG_PLL_MON_LOCK), + 100, 2000); + if (ret) { + dev_err(priv->dev, "Failed to put PLLDSI into standby mode"); + return ret; + } + + /* Output clock setting 1 */ + writel(FIELD_PREP(CPG_PLL_CLK1_KDIV, (u16)params->k) | + FIELD_PREP(CPG_PLL_CLK1_MDIV, params->m) | + FIELD_PREP(CPG_PLL_CLK1_PDIV, params->p), + priv->base + CPG_PLL_CLK1(offset)); + + /* Output clock setting 2 */ + val = readl(priv->base + CPG_PLL_CLK2(offset)); + writel((val & ~CPG_PLL_CLK2_SDIV) | FIELD_PREP(CPG_PLL_CLK2_SDIV, params->s), + priv->base + CPG_PLL_CLK2(offset)); + + /* Put PLL to normal mode */ + if (ssc_disable) + val = CPG_PLL_STBY_SSC_EN_WEN; + else + val = CPG_PLL_STBY_SSC_EN_WEN | CPG_PLL_STBY_SSC_EN; + writel(val | CPG_PLL_STBY_RESETB_WEN | CPG_PLL_STBY_RESETB, + priv->base + CPG_PLL_STBY(offset)); + + /* PLL normal mode transition, output clock stability check */ + ret = readl_poll_timeout_atomic(priv->base + CPG_PLL_MON(offset), + val, (val & CPG_PLL_MON_LOCK), + 100, 2000); + if (ret) { + dev_err(priv->dev, "Failed to put PLLDSI into normal mode"); + return ret; + } + + return 0; +} + +static int rzv2h_cpg_plldsi_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct pll_clk *pll_clk = to_pll(hw); + struct rzv2h_pll_dsi_info *dsi_info; + struct rzv2h_cpg_priv *priv = pll_clk->priv; + + dsi_info = &priv->pll_dsi_info[pll_clk->pll.instance]; + + return rzv2h_cpg_pll_set_rate(pll_clk, &dsi_info->pll_dsi_parameters.pll, true); +} + +static int rzv2h_cpg_pll_clk_is_enabled(struct clk_hw *hw) +{ + struct pll_clk *pll_clk = to_pll(hw); + struct rzv2h_cpg_priv *priv = pll_clk->priv; + u32 val = readl(priv->base + CPG_PLL_MON(pll_clk->pll.offset)); + + /* Ensure both RESETB and LOCK bits are set */ + return (val & (CPG_PLL_MON_RESETB | CPG_PLL_MON_LOCK)) == + (CPG_PLL_MON_RESETB | CPG_PLL_MON_LOCK); +} + +static int rzv2h_cpg_pll_clk_enable(struct clk_hw *hw) +{ + struct pll_clk *pll_clk = to_pll(hw); + struct rzv2h_cpg_priv *priv = pll_clk->priv; + struct pll pll = pll_clk->pll; + u32 stby_offset; + u32 mon_offset; + u32 val; + int ret; + + if (rzv2h_cpg_pll_clk_is_enabled(hw)) + return 0; + + stby_offset = CPG_PLL_STBY(pll.offset); + mon_offset = CPG_PLL_MON(pll.offset); + + writel(CPG_PLL_STBY_RESETB_WEN | CPG_PLL_STBY_RESETB, + priv->base + stby_offset); + + /* + * Ensure PLL enters into normal mode + * + * Note: There is no HW information about the worst case latency. + * + * Since this latency might depend on external crystal or PLL rate, + * use a "super" safe timeout value. + */ + ret = readl_poll_timeout_atomic(priv->base + mon_offset, val, + (val & (CPG_PLL_MON_RESETB | CPG_PLL_MON_LOCK)) == + (CPG_PLL_MON_RESETB | CPG_PLL_MON_LOCK), 200, 2000); + if (ret) + dev_err(priv->dev, "Failed to enable PLL 0x%x/%pC\n", + stby_offset, hw->clk); + + return ret; +} + +static unsigned long rzv2h_cpg_pll_clk_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct pll_clk *pll_clk = to_pll(hw); + struct rzv2h_cpg_priv *priv = pll_clk->priv; + struct pll pll = pll_clk->pll; + unsigned int clk1, clk2; + u64 rate; + + if (!pll.has_clkn) + return 0; + + clk1 = readl(priv->base + CPG_PLL_CLK1(pll.offset)); + clk2 = readl(priv->base + CPG_PLL_CLK2(pll.offset)); + + rate = mul_u64_u32_shr(parent_rate, (FIELD_GET(CPG_PLL_CLK1_MDIV, clk1) << 16) + + (s16)FIELD_GET(CPG_PLL_CLK1_KDIV, clk1), + 16 + FIELD_GET(CPG_PLL_CLK2_SDIV, clk2)); + + return DIV_ROUND_CLOSEST_ULL(rate, FIELD_GET(CPG_PLL_CLK1_PDIV, clk1)); +} + +static const struct clk_ops rzv2h_cpg_plldsi_ops = { + .recalc_rate = rzv2h_cpg_pll_clk_recalc_rate, + .determine_rate = rzv2h_cpg_plldsi_determine_rate, + .set_rate = rzv2h_cpg_plldsi_set_rate, +}; + +static const struct clk_ops rzv2h_cpg_pll_ops = { + .is_enabled = rzv2h_cpg_pll_clk_is_enabled, + .enable = rzv2h_cpg_pll_clk_enable, + .recalc_rate = rzv2h_cpg_pll_clk_recalc_rate, +}; + +static struct clk * __init +rzv2h_cpg_pll_clk_register(const struct cpg_core_clk *core, + struct rzv2h_cpg_priv *priv, + const struct clk_ops *ops) +{ + struct device *dev = priv->dev; + struct clk_init_data init; + const struct clk *parent; + const char *parent_name; + struct pll_clk *pll_clk; + int ret; + + parent = priv->clks[core->parent]; + if (IS_ERR(parent)) + return ERR_CAST(parent); + + pll_clk = devm_kzalloc(dev, sizeof(*pll_clk), GFP_KERNEL); + if (!pll_clk) + return ERR_PTR(-ENOMEM); + + if (core->type == CLK_TYPE_PLLDSI) + priv->pll_dsi_info[core->cfg.pll.instance].pll_dsi_limits = + core->cfg.pll.limits; + + parent_name = __clk_get_name(parent); + init.name = core->name; + init.ops = ops; + init.flags = 0; + init.parent_names = &parent_name; + init.num_parents = 1; + + pll_clk->hw.init = &init; + pll_clk->pll = core->cfg.pll; + pll_clk->priv = priv; + + ret = devm_clk_hw_register(dev, &pll_clk->hw); + if (ret) + return ERR_PTR(ret); + + return pll_clk->hw.clk; +} + +static unsigned long rzv2h_ddiv_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct clk_divider *divider = to_clk_divider(hw); + unsigned int val; + + val = readl(divider->reg) >> divider->shift; + val &= clk_div_mask(divider->width); + + return divider_recalc_rate(hw, parent_rate, val, divider->table, + divider->flags, divider->width); +} + +static int rzv2h_ddiv_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) +{ + struct clk_divider *divider = to_clk_divider(hw); + + return divider_determine_rate(hw, req, divider->table, divider->width, + divider->flags); +} + +static inline int rzv2h_cpg_wait_ddiv_clk_update_done(void __iomem *base, u8 mon) +{ + u32 bitmask = BIT(mon); + u32 val; + + if (mon == CSDIV_NO_MON) + return 0; + + return readl_poll_timeout_atomic(base + CPG_CLKSTATUS0, val, !(val & bitmask), 10, 200); +} + +static int rzv2h_ddiv_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct clk_divider *divider = to_clk_divider(hw); + struct ddiv_clk *ddiv = to_ddiv_clock(divider); + struct rzv2h_cpg_priv *priv = ddiv->priv; + unsigned long flags = 0; + int value; + u32 val; + int ret; + + value = divider_get_val(rate, parent_rate, divider->table, + divider->width, divider->flags); + if (value < 0) + return value; + + spin_lock_irqsave(divider->lock, flags); + + ret = rzv2h_cpg_wait_ddiv_clk_update_done(priv->base, ddiv->mon); + if (ret) + goto ddiv_timeout; + + val = readl(divider->reg) | DDIV_DIVCTL_WEN(divider->shift); + val &= ~(clk_div_mask(divider->width) << divider->shift); + val |= (u32)value << divider->shift; + writel(val, divider->reg); + + ret = rzv2h_cpg_wait_ddiv_clk_update_done(priv->base, ddiv->mon); + +ddiv_timeout: + spin_unlock_irqrestore(divider->lock, flags); + return ret; +} + +static const struct clk_ops rzv2h_ddiv_clk_divider_ops = { + .recalc_rate = rzv2h_ddiv_recalc_rate, + .determine_rate = rzv2h_ddiv_determine_rate, + .set_rate = rzv2h_ddiv_set_rate, +}; + +static struct clk * __init +rzv2h_cpg_ddiv_clk_register(const struct cpg_core_clk *core, + struct rzv2h_cpg_priv *priv) +{ + struct ddiv cfg_ddiv = core->cfg.ddiv; + struct clk_init_data init = {}; + struct device *dev = priv->dev; + u8 shift = cfg_ddiv.shift; + u8 width = cfg_ddiv.width; + const struct clk *parent; + const char *parent_name; + struct clk_divider *div; + struct ddiv_clk *ddiv; + int ret; + + parent = priv->clks[core->parent]; + if (IS_ERR(parent)) + return ERR_CAST(parent); + + parent_name = __clk_get_name(parent); + + if ((shift + width) > 16) + return ERR_PTR(-EINVAL); + + ddiv = devm_kzalloc(priv->dev, sizeof(*ddiv), GFP_KERNEL); + if (!ddiv) + return ERR_PTR(-ENOMEM); + + init.name = core->name; + if (cfg_ddiv.no_rmw) + init.ops = &clk_divider_ops; + else + init.ops = &rzv2h_ddiv_clk_divider_ops; + init.parent_names = &parent_name; + init.num_parents = 1; + init.flags = CLK_SET_RATE_PARENT; + + ddiv->priv = priv; + ddiv->mon = cfg_ddiv.monbit; + div = &ddiv->div; + div->reg = priv->base + cfg_ddiv.offset; + div->shift = shift; + div->width = width; + div->flags = core->flag; + div->lock = &priv->rmw_lock; + div->hw.init = &init; + div->table = core->dtable; + + ret = devm_clk_hw_register(dev, &div->hw); + if (ret) + return ERR_PTR(ret); + + return div->hw.clk; +} + +static struct clk * __init +rzv2h_cpg_mux_clk_register(const struct cpg_core_clk *core, + struct rzv2h_cpg_priv *priv) +{ + struct smuxed mux = core->cfg.smux; + const struct clk_hw *clk_hw; + + clk_hw = devm_clk_hw_register_mux(priv->dev, core->name, + core->parent_names, core->num_parents, + core->flag, priv->base + mux.offset, + mux.shift, mux.width, + core->mux_flags, &priv->rmw_lock); + if (IS_ERR(clk_hw)) + return ERR_CAST(clk_hw); + + return clk_hw->clk; +} + +static int +rzv2h_clk_ff_mod_status_is_enabled(struct clk_hw *hw) +{ + struct rzv2h_ff_mod_status_clk *fix = to_rzv2h_ff_mod_status_clk(hw); + struct rzv2h_cpg_priv *priv = fix->priv; + u32 offset = GET_CLK_MON_OFFSET(fix->conf.mon_index); + u32 bitmask = BIT(fix->conf.mon_bit); + u32 val; + + val = readl(priv->base + offset); + return !!(val & bitmask); +} + +static struct clk * __init +rzv2h_cpg_fixed_mod_status_clk_register(const struct cpg_core_clk *core, + struct rzv2h_cpg_priv *priv) +{ + struct rzv2h_ff_mod_status_clk *clk_hw_data; + struct clk_init_data init = { }; + struct clk_fixed_factor *fix; + const struct clk *parent; + const char *parent_name; + int ret; + + WARN_DEBUG(core->parent >= priv->num_core_clks); + parent = priv->clks[core->parent]; + if (IS_ERR(parent)) + return ERR_CAST(parent); + + parent_name = __clk_get_name(parent); + parent = priv->clks[core->parent]; + if (IS_ERR(parent)) + return ERR_CAST(parent); + + clk_hw_data = devm_kzalloc(priv->dev, sizeof(*clk_hw_data), GFP_KERNEL); + if (!clk_hw_data) + return ERR_PTR(-ENOMEM); + + clk_hw_data->priv = priv; + clk_hw_data->conf = core->cfg.fixed_mod; + + init.name = core->name; + init.ops = priv->ff_mod_status_ops; + init.flags = CLK_SET_RATE_PARENT; + init.parent_names = &parent_name; + init.num_parents = 1; + + fix = &clk_hw_data->fix; + fix->hw.init = &init; + fix->mult = core->mult; + fix->div = core->div; + + ret = devm_clk_hw_register(priv->dev, &clk_hw_data->fix.hw); + if (ret) + return ERR_PTR(ret); + + return clk_hw_data->fix.hw.clk; +} + +static struct clk +*rzv2h_cpg_clk_src_twocell_get(struct of_phandle_args *clkspec, + void *data) +{ + unsigned int clkidx = clkspec->args[1]; + struct rzv2h_cpg_priv *priv = data; + struct device *dev = priv->dev; + const char *type; + struct clk *clk; + + switch (clkspec->args[0]) { + case CPG_CORE: + type = "core"; + if (clkidx > priv->last_dt_core_clk) { + dev_err(dev, "Invalid %s clock index %u\n", type, clkidx); + return ERR_PTR(-EINVAL); + } + clk = priv->clks[clkidx]; + break; + + case CPG_MOD: + type = "module"; + if (clkidx >= priv->num_mod_clks) { + dev_err(dev, "Invalid %s clock index %u\n", type, clkidx); + return ERR_PTR(-EINVAL); + } + clk = priv->clks[priv->num_core_clks + clkidx]; + break; + + default: + dev_err(dev, "Invalid CPG clock type %u\n", clkspec->args[0]); + return ERR_PTR(-EINVAL); + } + + if (IS_ERR(clk)) + dev_err(dev, "Cannot get %s clock %u: %ld", type, clkidx, + PTR_ERR(clk)); + else + dev_dbg(dev, "clock (%u, %u) is %pC at %lu Hz\n", + clkspec->args[0], clkspec->args[1], clk, + clk_get_rate(clk)); + return clk; +} + +static void __init +rzv2h_cpg_register_core_clk(const struct cpg_core_clk *core, + struct rzv2h_cpg_priv *priv) +{ + struct clk *clk = ERR_PTR(-EOPNOTSUPP), *parent; + unsigned int id = core->id, div = core->div; + struct device *dev = priv->dev; + const char *parent_name; + struct clk_hw *clk_hw; + + WARN_DEBUG(id >= priv->num_core_clks); + WARN_DEBUG(PTR_ERR(priv->clks[id]) != -ENOENT); + + switch (core->type) { + case CLK_TYPE_IN: + clk = of_clk_get_by_name(priv->dev->of_node, core->name); + break; + case CLK_TYPE_FF: + WARN_DEBUG(core->parent >= priv->num_core_clks); + parent = priv->clks[core->parent]; + if (IS_ERR(parent)) { + clk = parent; + goto fail; + } + + parent_name = __clk_get_name(parent); + clk_hw = devm_clk_hw_register_fixed_factor(dev, core->name, + parent_name, CLK_SET_RATE_PARENT, + core->mult, div); + if (IS_ERR(clk_hw)) + clk = ERR_CAST(clk_hw); + else + clk = clk_hw->clk; + break; + case CLK_TYPE_FF_MOD_STATUS: + if (!priv->ff_mod_status_ops) { + priv->ff_mod_status_ops = + devm_kzalloc(dev, sizeof(*priv->ff_mod_status_ops), GFP_KERNEL); + if (!priv->ff_mod_status_ops) { + clk = ERR_PTR(-ENOMEM); + goto fail; + } + memcpy(priv->ff_mod_status_ops, &clk_fixed_factor_ops, + sizeof(const struct clk_ops)); + priv->ff_mod_status_ops->is_enabled = rzv2h_clk_ff_mod_status_is_enabled; + } + clk = rzv2h_cpg_fixed_mod_status_clk_register(core, priv); + break; + case CLK_TYPE_PLL: + clk = rzv2h_cpg_pll_clk_register(core, priv, &rzv2h_cpg_pll_ops); + break; + case CLK_TYPE_DDIV: + clk = rzv2h_cpg_ddiv_clk_register(core, priv); + break; + case CLK_TYPE_SMUX: + clk = rzv2h_cpg_mux_clk_register(core, priv); + break; + case CLK_TYPE_PLLDSI: + clk = rzv2h_cpg_pll_clk_register(core, priv, &rzv2h_cpg_plldsi_ops); + break; + case CLK_TYPE_PLLDSI_DIV: + clk = rzv2h_cpg_plldsi_div_clk_register(core, priv); + break; + default: + goto fail; + } + + if (IS_ERR(clk)) + goto fail; + + dev_dbg(dev, "Core clock %pC at %lu Hz\n", clk, clk_get_rate(clk)); + priv->clks[id] = clk; + return; + +fail: + dev_err(dev, "Failed to register core clock %s: %ld\n", + core->name, PTR_ERR(clk)); +} + +static void rzv2h_mod_clock_mstop_enable(struct rzv2h_cpg_priv *priv, + u32 mstop_data) +{ + unsigned long mstop_mask = FIELD_GET(BUS_MSTOP_BITS_MASK, mstop_data); + u16 mstop_index = FIELD_GET(BUS_MSTOP_IDX_MASK, mstop_data); + atomic_t *mstop = &priv->mstop_count[mstop_index * 16]; + unsigned long flags; + unsigned int i; + u32 val = 0; + + spin_lock_irqsave(&priv->rmw_lock, flags); + for_each_set_bit(i, &mstop_mask, 16) { + if (!atomic_read(&mstop[i])) + val |= BIT(i) << 16; + atomic_inc(&mstop[i]); + } + if (val) + writel(val, priv->base + CPG_BUS_MSTOP(mstop_index)); + spin_unlock_irqrestore(&priv->rmw_lock, flags); +} + +static void rzv2h_mod_clock_mstop_disable(struct rzv2h_cpg_priv *priv, + u32 mstop_data) +{ + unsigned long mstop_mask = FIELD_GET(BUS_MSTOP_BITS_MASK, mstop_data); + u16 mstop_index = FIELD_GET(BUS_MSTOP_IDX_MASK, mstop_data); + atomic_t *mstop = &priv->mstop_count[mstop_index * 16]; + unsigned long flags; + unsigned int i; + u32 val = 0; + + spin_lock_irqsave(&priv->rmw_lock, flags); + for_each_set_bit(i, &mstop_mask, 16) { + if (!atomic_read(&mstop[i]) || + atomic_dec_and_test(&mstop[i])) + val |= BIT(i) << 16 | BIT(i); + } + if (val) + writel(val, priv->base + CPG_BUS_MSTOP(mstop_index)); + spin_unlock_irqrestore(&priv->rmw_lock, flags); +} + +static int rzv2h_parent_clk_mux_to_index(struct clk_hw *hw) +{ + struct clk_hw *parent_hw; + struct clk *parent_clk; + struct clk_mux *mux; + u32 val; + + /* This will always succeed, so no need to check for IS_ERR() */ + parent_clk = clk_get_parent(hw->clk); + + parent_hw = __clk_get_hw(parent_clk); + mux = to_clk_mux(parent_hw); + + val = readl(mux->reg) >> mux->shift; + val &= mux->mask; + return clk_mux_val_to_index(parent_hw, mux->table, 0, val); +} + +static int rzv2h_mod_clock_is_enabled(struct clk_hw *hw) +{ + struct mod_clock *clock = to_mod_clock(hw); + struct rzv2h_cpg_priv *priv = clock->priv; + int mon_index = clock->mon_index; + u32 bitmask; + u32 offset; + + if (clock->ext_clk_mux_index >= 0 && + rzv2h_parent_clk_mux_to_index(hw) == clock->ext_clk_mux_index) + mon_index = -1; + + if (mon_index >= 0) { + offset = GET_CLK_MON_OFFSET(mon_index); + bitmask = BIT(clock->mon_bit); + + if (!(readl(priv->base + offset) & bitmask)) + return 0; + } + + offset = GET_CLK_ON_OFFSET(clock->on_index); + bitmask = BIT(clock->on_bit); + + return readl(priv->base + offset) & bitmask; +} + +static int rzv2h_mod_clock_endisable(struct clk_hw *hw, bool enable) +{ + bool enabled = rzv2h_mod_clock_is_enabled(hw); + struct mod_clock *clock = to_mod_clock(hw); + unsigned int reg = GET_CLK_ON_OFFSET(clock->on_index); + struct rzv2h_cpg_priv *priv = clock->priv; + u32 bitmask = BIT(clock->on_bit); + struct device *dev = priv->dev; + u32 value; + int error; + + dev_dbg(dev, "CLK_ON 0x%x/%pC %s\n", reg, hw->clk, + str_on_off(enable)); + + if (enabled == enable) + return 0; + + value = bitmask << 16; + if (enable) { + value |= bitmask; + writel(value, priv->base + reg); + if (clock->mstop_data != BUS_MSTOP_NONE) + rzv2h_mod_clock_mstop_enable(priv, clock->mstop_data); + } else { + if (clock->mstop_data != BUS_MSTOP_NONE) + rzv2h_mod_clock_mstop_disable(priv, clock->mstop_data); + writel(value, priv->base + reg); + } + + if (!enable || clock->mon_index < 0) + return 0; + + reg = GET_CLK_MON_OFFSET(clock->mon_index); + bitmask = BIT(clock->mon_bit); + error = readl_poll_timeout_atomic(priv->base + reg, value, + value & bitmask, 0, 10); + if (error) + dev_err(dev, "Failed to enable CLK_ON 0x%x/%pC\n", + GET_CLK_ON_OFFSET(clock->on_index), hw->clk); + + return error; +} + +static int rzv2h_mod_clock_enable(struct clk_hw *hw) +{ + return rzv2h_mod_clock_endisable(hw, true); +} + +static void rzv2h_mod_clock_disable(struct clk_hw *hw) +{ + rzv2h_mod_clock_endisable(hw, false); +} + +static const struct clk_ops rzv2h_mod_clock_ops = { + .enable = rzv2h_mod_clock_enable, + .disable = rzv2h_mod_clock_disable, + .is_enabled = rzv2h_mod_clock_is_enabled, +}; + +static void __init +rzv2h_cpg_register_mod_clk(const struct rzv2h_mod_clk *mod, + struct rzv2h_cpg_priv *priv) +{ + struct mod_clock *clock = NULL; + struct device *dev = priv->dev; + struct clk_init_data init; + struct clk *parent, *clk; + const char *parent_name; + unsigned int id; + int ret; + + id = GET_MOD_CLK_ID(priv->num_core_clks, mod->on_index, mod->on_bit); + WARN_DEBUG(id >= priv->num_core_clks + priv->num_mod_clks); + WARN_DEBUG(mod->parent >= priv->num_core_clks + priv->num_mod_clks); + WARN_DEBUG(PTR_ERR(priv->clks[id]) != -ENOENT); + + parent = priv->clks[mod->parent]; + if (IS_ERR(parent)) { + clk = parent; + goto fail; + } + + clock = devm_kzalloc(dev, sizeof(*clock), GFP_KERNEL); + if (!clock) { + clk = ERR_PTR(-ENOMEM); + goto fail; + } + + init.name = mod->name; + init.ops = &rzv2h_mod_clock_ops; + init.flags = CLK_SET_RATE_PARENT; + if (mod->critical) + init.flags |= CLK_IS_CRITICAL; + + parent_name = __clk_get_name(parent); + init.parent_names = &parent_name; + init.num_parents = 1; + + clock->on_index = mod->on_index; + clock->on_bit = mod->on_bit; + clock->mon_index = mod->mon_index; + clock->mon_bit = mod->mon_bit; + clock->no_pm = mod->no_pm; + clock->ext_clk_mux_index = mod->ext_clk_mux_index; + clock->priv = priv; + clock->hw.init = &init; + clock->mstop_data = mod->mstop_data; + + ret = devm_clk_hw_register(dev, &clock->hw); + if (ret) { + clk = ERR_PTR(ret); + goto fail; + } + + priv->clks[id] = clock->hw.clk; + + /* + * Ensure the module clocks and MSTOP bits are synchronized when they are + * turned ON by the bootloader. Enable MSTOP bits for module clocks that were + * turned ON in an earlier boot stage. + */ + if (clock->mstop_data != BUS_MSTOP_NONE && + !mod->critical && rzv2h_mod_clock_is_enabled(&clock->hw)) { + rzv2h_mod_clock_mstop_enable(priv, clock->mstop_data); + } else if (clock->mstop_data != BUS_MSTOP_NONE && mod->critical) { + unsigned long mstop_mask = FIELD_GET(BUS_MSTOP_BITS_MASK, clock->mstop_data); + u16 mstop_index = FIELD_GET(BUS_MSTOP_IDX_MASK, clock->mstop_data); + atomic_t *mstop = &priv->mstop_count[mstop_index * 16]; + unsigned long flags; + unsigned int i; + u32 val = 0; + + /* + * Critical clocks are turned ON immediately upon registration, and the + * MSTOP counter is updated through the rzv2h_mod_clock_enable() path. + * However, if the critical clocks were already turned ON by the initial + * bootloader, synchronize the atomic counter here and clear the MSTOP bit. + */ + spin_lock_irqsave(&priv->rmw_lock, flags); + for_each_set_bit(i, &mstop_mask, 16) { + if (atomic_read(&mstop[i])) + continue; + val |= BIT(i) << 16; + atomic_inc(&mstop[i]); + } + if (val) + writel(val, priv->base + CPG_BUS_MSTOP(mstop_index)); + spin_unlock_irqrestore(&priv->rmw_lock, flags); + } + + return; + +fail: + dev_err(dev, "Failed to register module clock %s: %ld\n", + mod->name, PTR_ERR(clk)); +} + +static int __rzv2h_cpg_assert(struct reset_controller_dev *rcdev, + unsigned long id, bool assert) +{ + struct rzv2h_cpg_priv *priv = rcdev_to_priv(rcdev); + unsigned int reg = GET_RST_OFFSET(priv->resets[id].reset_index); + u32 mask = BIT(priv->resets[id].reset_bit); + u8 monbit = priv->resets[id].mon_bit; + u32 value = mask << 16; + int ret; + + dev_dbg(rcdev->dev, "%s id:%ld offset:0x%x\n", + assert ? "assert" : "deassert", id, reg); + + if (!assert) + value |= mask; + writel(value, priv->base + reg); + + reg = GET_RST_MON_OFFSET(priv->resets[id].mon_index); + mask = BIT(monbit); + + ret = readl_poll_timeout_atomic(priv->base + reg, value, + assert == !!(value & mask), 10, 200); + if (ret && !assert) { + value = mask << 16; + writel(value, priv->base + GET_RST_OFFSET(priv->resets[id].reset_index)); + } + + return ret; +} + +static int rzv2h_cpg_assert(struct reset_controller_dev *rcdev, + unsigned long id) +{ + return __rzv2h_cpg_assert(rcdev, id, true); +} + +static int rzv2h_cpg_deassert(struct reset_controller_dev *rcdev, + unsigned long id) +{ + return __rzv2h_cpg_assert(rcdev, id, false); +} + +static int rzv2h_cpg_reset(struct reset_controller_dev *rcdev, + unsigned long id) +{ + int ret; + + ret = rzv2h_cpg_assert(rcdev, id); + if (ret) + return ret; + + return rzv2h_cpg_deassert(rcdev, id); +} + +static int rzv2h_cpg_status(struct reset_controller_dev *rcdev, + unsigned long id) +{ + struct rzv2h_cpg_priv *priv = rcdev_to_priv(rcdev); + unsigned int reg = GET_RST_MON_OFFSET(priv->resets[id].mon_index); + u8 monbit = priv->resets[id].mon_bit; + + return !!(readl(priv->base + reg) & BIT(monbit)); +} + +static const struct reset_control_ops rzv2h_cpg_reset_ops = { + .reset = rzv2h_cpg_reset, + .assert = rzv2h_cpg_assert, + .deassert = rzv2h_cpg_deassert, + .status = rzv2h_cpg_status, +}; + +static int rzv2h_cpg_reset_xlate(struct reset_controller_dev *rcdev, + const struct of_phandle_args *reset_spec) +{ + struct rzv2h_cpg_priv *priv = rcdev_to_priv(rcdev); + unsigned int id = reset_spec->args[0]; + u8 rst_index = id / 16; + u8 rst_bit = id % 16; + unsigned int i; + + for (i = 0; i < rcdev->nr_resets; i++) { + if (rst_index == priv->resets[i].reset_index && + rst_bit == priv->resets[i].reset_bit) + return i; + } + + return -EINVAL; +} + +static int rzv2h_cpg_reset_controller_register(struct rzv2h_cpg_priv *priv) +{ + priv->rcdev.ops = &rzv2h_cpg_reset_ops; + priv->rcdev.of_node = priv->dev->of_node; + priv->rcdev.dev = priv->dev; + priv->rcdev.of_reset_n_cells = 1; + priv->rcdev.of_xlate = rzv2h_cpg_reset_xlate; + priv->rcdev.nr_resets = priv->num_resets; + + return devm_reset_controller_register(priv->dev, &priv->rcdev); +} + +/** + * struct rzv2h_cpg_pd - RZ/V2H power domain data structure + * @priv: pointer to CPG private data structure + * @genpd: generic PM domain + */ +struct rzv2h_cpg_pd { + struct rzv2h_cpg_priv *priv; + struct generic_pm_domain genpd; +}; + +static bool rzv2h_cpg_is_pm_clk(struct rzv2h_cpg_pd *pd, + const struct of_phandle_args *clkspec) +{ + if (clkspec->np != pd->genpd.dev.of_node || clkspec->args_count != 2) + return false; + + switch (clkspec->args[0]) { + case CPG_MOD: { + struct rzv2h_cpg_priv *priv = pd->priv; + unsigned int id = clkspec->args[1]; + struct mod_clock *clock; + + if (id >= priv->num_mod_clks) + return false; + + if (priv->clks[priv->num_core_clks + id] == ERR_PTR(-ENOENT)) + return false; + + clock = to_mod_clock(__clk_get_hw(priv->clks[priv->num_core_clks + id])); + + return !clock->no_pm; + } + + case CPG_CORE: + default: + return false; + } +} + +static int rzv2h_cpg_attach_dev(struct generic_pm_domain *domain, struct device *dev) +{ + struct rzv2h_cpg_pd *pd = container_of(domain, struct rzv2h_cpg_pd, genpd); + struct device_node *np = dev->of_node; + struct of_phandle_args clkspec; + bool once = true; + struct clk *clk; + unsigned int i; + int error; + + for (i = 0; !of_parse_phandle_with_args(np, "clocks", "#clock-cells", i, &clkspec); i++) { + if (!rzv2h_cpg_is_pm_clk(pd, &clkspec)) { + of_node_put(clkspec.np); + continue; + } + + if (once) { + once = false; + error = pm_clk_create(dev); + if (error) { + of_node_put(clkspec.np); + goto err; + } + } + clk = of_clk_get_from_provider(&clkspec); + of_node_put(clkspec.np); + if (IS_ERR(clk)) { + error = PTR_ERR(clk); + goto fail_destroy; + } + + error = pm_clk_add_clk(dev, clk); + if (error) { + dev_err(dev, "pm_clk_add_clk failed %d\n", + error); + goto fail_put; + } + } + + return 0; + +fail_put: + clk_put(clk); + +fail_destroy: + pm_clk_destroy(dev); +err: + return error; +} + +static void rzv2h_cpg_detach_dev(struct generic_pm_domain *unused, struct device *dev) +{ + if (!pm_clk_no_clocks(dev)) + pm_clk_destroy(dev); +} + +static void rzv2h_cpg_genpd_remove_simple(void *data) +{ + pm_genpd_remove(data); +} + +static int __init rzv2h_cpg_add_pm_domains(struct rzv2h_cpg_priv *priv) +{ + struct device *dev = priv->dev; + struct device_node *np = dev->of_node; + struct rzv2h_cpg_pd *pd; + int ret; + + pd = devm_kzalloc(dev, sizeof(*pd), GFP_KERNEL); + if (!pd) + return -ENOMEM; + + pd->genpd.name = np->name; + pd->priv = priv; + pd->genpd.flags |= GENPD_FLAG_ALWAYS_ON | GENPD_FLAG_PM_CLK | GENPD_FLAG_ACTIVE_WAKEUP; + pd->genpd.attach_dev = rzv2h_cpg_attach_dev; + pd->genpd.detach_dev = rzv2h_cpg_detach_dev; + ret = pm_genpd_init(&pd->genpd, &pm_domain_always_on_gov, false); + if (ret) + return ret; + + ret = devm_add_action_or_reset(dev, rzv2h_cpg_genpd_remove_simple, &pd->genpd); + if (ret) + return ret; + + return of_genpd_add_provider_simple(np, &pd->genpd); +} + +static void rzv2h_cpg_del_clk_provider(void *data) +{ + of_clk_del_provider(data); +} + +static int __init rzv2h_cpg_probe(struct platform_device *pdev) +{ + struct device *dev = &pdev->dev; + struct device_node *np = dev->of_node; + const struct rzv2h_cpg_info *info; + struct rzv2h_cpg_priv *priv; + unsigned int nclks, i; + struct clk **clks; + int error; + + info = of_device_get_match_data(dev); + + priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + spin_lock_init(&priv->rmw_lock); + + priv->dev = dev; + + priv->base = devm_platform_ioremap_resource(pdev, 0); + if (IS_ERR(priv->base)) + return PTR_ERR(priv->base); + + nclks = info->num_total_core_clks + info->num_hw_mod_clks; + clks = devm_kmalloc_array(dev, nclks, sizeof(*clks), GFP_KERNEL); + if (!clks) + return -ENOMEM; + + priv->mstop_count = devm_kcalloc(dev, info->num_mstop_bits, + sizeof(*priv->mstop_count), GFP_KERNEL); + if (!priv->mstop_count) + return -ENOMEM; + + /* Adjust for CPG_BUS_m_MSTOP starting from m = 1 */ + priv->mstop_count -= 16; + + priv->resets = devm_kmemdup_array(dev, info->resets, info->num_resets, + sizeof(*info->resets), GFP_KERNEL); + if (!priv->resets) + return -ENOMEM; + + dev_set_drvdata(dev, priv); + priv->clks = clks; + priv->num_core_clks = info->num_total_core_clks; + priv->num_mod_clks = info->num_hw_mod_clks; + priv->last_dt_core_clk = info->last_dt_core_clk; + priv->num_resets = info->num_resets; + + for (i = 0; i < nclks; i++) + clks[i] = ERR_PTR(-ENOENT); + + for (i = 0; i < info->num_core_clks; i++) + rzv2h_cpg_register_core_clk(&info->core_clks[i], priv); + + for (i = 0; i < info->num_mod_clks; i++) + rzv2h_cpg_register_mod_clk(&info->mod_clks[i], priv); + + error = of_clk_add_provider(np, rzv2h_cpg_clk_src_twocell_get, priv); + if (error) + return error; + + error = devm_add_action_or_reset(dev, rzv2h_cpg_del_clk_provider, np); + if (error) + return error; + + error = rzv2h_cpg_add_pm_domains(priv); + if (error) + return error; + + error = rzv2h_cpg_reset_controller_register(priv); + if (error) + return error; + + return 0; +} + +static const struct of_device_id rzv2h_cpg_match[] = { +#ifdef CONFIG_CLK_R9A09G047 + { + .compatible = "renesas,r9a09g047-cpg", + .data = &r9a09g047_cpg_info, + }, +#endif +#ifdef CONFIG_CLK_R9A09G056 + { + .compatible = "renesas,r9a09g056-cpg", + .data = &r9a09g056_cpg_info, + }, +#endif +#ifdef CONFIG_CLK_R9A09G057 + { + .compatible = "renesas,r9a09g057-cpg", + .data = &r9a09g057_cpg_info, + }, +#endif + { /* sentinel */ } +}; + +static struct platform_driver rzv2h_cpg_driver = { + .driver = { + .name = "rzv2h-cpg", + .of_match_table = rzv2h_cpg_match, + }, +}; + +static int __init rzv2h_cpg_init(void) +{ + return platform_driver_probe(&rzv2h_cpg_driver, rzv2h_cpg_probe); +} + +subsys_initcall(rzv2h_cpg_init); + +MODULE_DESCRIPTION("Renesas RZ/V2H CPG Driver"); |