From e2262c8ab4755ab574580611d7da22509f07871c Mon Sep 17 00:00:00 2001 From: popcornmix Date: Wed, 21 Aug 2019 14:55:56 +0100 Subject: [PATCH] clk-raspberrypi: Also support v3d clock Signed-off-by: popcornmix --- drivers/clk/bcm/clk-raspberrypi.c | 501 ++++++++++++++++++++++++------ 1 file changed, 412 insertions(+), 89 deletions(-) --- a/drivers/clk/bcm/clk-raspberrypi.c +++ b/drivers/clk/bcm/clk-raspberrypi.c @@ -15,33 +15,103 @@ #include #include #include - +#include #include #define RPI_FIRMWARE_ARM_CLK_ID 0x00000003 +#define RPI_FIRMWARE_V3D_CLK_ID 0x00000005 #define RPI_FIRMWARE_STATE_ENABLE_BIT BIT(0) #define RPI_FIRMWARE_STATE_WAIT_BIT BIT(1) -/* - * Even though the firmware interface alters 'pllb' the frequencies are - * provided as per 'pllb_arm'. We need to scale before passing them trough. - */ -#define RPI_FIRMWARE_PLLB_ARM_DIV_RATE 2 - #define A2W_PLL_FRAC_BITS 20 +#define SOC_BCM2835 BIT(0) +#define SOC_BCM2711 BIT(1) +#define SOC_ALL (SOC_BCM2835 | SOC_BCM2711) + struct raspberrypi_clk { struct device *dev; struct rpi_firmware *firmware; struct platform_device *cpufreq; +}; + +typedef int (*raspberrypi_clk_register)(struct raspberrypi_clk *rpi, + const void *data); + + +/* assignment helper macros for different clock types */ +#define _REGISTER(f, s, ...) { .clk_register = (raspberrypi_clk_register)f, \ + .supported = s, \ + .data = __VA_ARGS__ } +#define REGISTER_PLL(s, ...) _REGISTER(&raspberrypi_register_pll, \ + s, \ + &(struct raspberrypi_pll_data) \ + {__VA_ARGS__}) +#define REGISTER_PLL_DIV(s, ...) _REGISTER(&raspberrypi_register_pll_divider, \ + s, \ + &(struct raspberrypi_pll_divider_data) \ + {__VA_ARGS__}) +#define REGISTER_CLK(s, ...) _REGISTER(&raspberrypi_register_clock, \ + s, \ + &(struct raspberrypi_clock_data) \ + {__VA_ARGS__}) + + +struct raspberrypi_pll_data { + const char *name; + const char *const *parents; + int num_parents; + u32 clock_id; +}; + +struct raspberrypi_clock_data { + const char *name; + const char *const *parents; + int num_parents; + u32 flags; + u32 clock_id; +}; + +struct raspberrypi_pll_divider_data { + const char *name; + const char *divider_name; + const char *lookup; + const char *source_pll; + + u32 fixed_divider; + u32 flags; + u32 clock_id; +}; - unsigned long min_rate; - unsigned long max_rate; +struct raspberrypi_clk_desc { + raspberrypi_clk_register clk_register; + unsigned int supported; + const void *data; +}; - struct clk_hw pllb; - struct clk_hw *pllb_arm; - struct clk_lookup *pllb_arm_lookup; +struct raspberrypi_clock { + struct clk_hw hw; + struct raspberrypi_clk *rpi; + u32 min_rate; + u32 max_rate; + const struct raspberrypi_clock_data *data; +}; + +struct raspberrypi_pll { + struct clk_hw hw; + struct raspberrypi_clk *rpi; + u32 min_rate; + u32 max_rate; + const struct raspberrypi_pll_data *data; +}; + +struct raspberrypi_pll_divider { + struct clk_divider div; + struct raspberrypi_clk *rpi; + u32 min_rate; + u32 max_rate; + const struct raspberrypi_pll_divider_data *data; }; /* @@ -83,56 +153,49 @@ static int raspberrypi_clock_property(st return 0; } -static int raspberrypi_fw_pll_is_on(struct clk_hw *hw) +static int raspberrypi_fw_is_on(struct raspberrypi_clk *rpi, u32 clock_id, const char *name) { - struct raspberrypi_clk *rpi = container_of(hw, struct raspberrypi_clk, - pllb); u32 val = 0; int ret; ret = raspberrypi_clock_property(rpi->firmware, RPI_FIRMWARE_GET_CLOCK_STATE, - RPI_FIRMWARE_ARM_CLK_ID, &val); + clock_id, &val); if (ret) return 0; return !!(val & RPI_FIRMWARE_STATE_ENABLE_BIT); } - -static unsigned long raspberrypi_fw_pll_get_rate(struct clk_hw *hw, - unsigned long parent_rate) +static unsigned long raspberrypi_fw_get_rate(struct raspberrypi_clk *rpi, + u32 clock_id, const char *name, unsigned long parent_rate) { - struct raspberrypi_clk *rpi = container_of(hw, struct raspberrypi_clk, - pllb); u32 val = 0; int ret; ret = raspberrypi_clock_property(rpi->firmware, RPI_FIRMWARE_GET_CLOCK_RATE, - RPI_FIRMWARE_ARM_CLK_ID, + clock_id, &val); if (ret) - return ret; - - return val * RPI_FIRMWARE_PLLB_ARM_DIV_RATE; + dev_err_ratelimited(rpi->dev, "Failed to get %s frequency: %d", + name, ret); + return val; } -static int raspberrypi_fw_pll_set_rate(struct clk_hw *hw, unsigned long rate, - unsigned long parent_rate) +static int raspberrypi_fw_set_rate(struct raspberrypi_clk *rpi, + u32 clock_id, const char *name, u32 rate, + unsigned long parent_rate) { - struct raspberrypi_clk *rpi = container_of(hw, struct raspberrypi_clk, - pllb); - u32 new_rate = rate / RPI_FIRMWARE_PLLB_ARM_DIV_RATE; int ret; ret = raspberrypi_clock_property(rpi->firmware, RPI_FIRMWARE_SET_CLOCK_RATE, - RPI_FIRMWARE_ARM_CLK_ID, - &new_rate); + clock_id, + &rate); if (ret) dev_err_ratelimited(rpi->dev, "Failed to change %s frequency: %d", - clk_hw_get_name(hw), ret); + name, ret); return ret; } @@ -141,16 +204,18 @@ static int raspberrypi_fw_pll_set_rate(s * Sadly there is no firmware rate rounding interface. We borrowed it from * clk-bcm2835. */ -static int raspberrypi_pll_determine_rate(struct clk_hw *hw, +static int raspberrypi_determine_rate(struct raspberrypi_clk *rpi, + u32 clock_id, const char *name, unsigned long min_rate, unsigned long max_rate, struct clk_rate_request *req) { - struct raspberrypi_clk *rpi = container_of(hw, struct raspberrypi_clk, - pllb); +#if 1 + req->rate = clamp(req->rate, min_rate, max_rate); +#else u64 div, final_rate; u32 ndiv, fdiv; /* We can't use req->rate directly as it would overflow */ - final_rate = clamp(req->rate, rpi->min_rate, rpi->max_rate); + final_rate = clamp(req->rate, min_rate, max_rate); div = (u64)final_rate << A2W_PLL_FRAC_BITS; do_div(div, req->best_parent_rate); @@ -163,9 +228,129 @@ static int raspberrypi_pll_determine_rat req->rate = final_rate >> A2W_PLL_FRAC_BITS; +#endif return 0; } +static int raspberrypi_fw_clock_is_on(struct clk_hw *hw) +{ + struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_clock_data *data = pll->data; + + return raspberrypi_fw_is_on(rpi, data->clock_id, data->name); +} + +static unsigned long raspberrypi_fw_clock_get_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_clock_data *data = pll->data; + + return raspberrypi_fw_get_rate(rpi, data->clock_id, data->name, parent_rate); +} + +static int raspberrypi_fw_clock_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_clock_data *data = pll->data; + + return raspberrypi_fw_set_rate(rpi, data->clock_id, data->name, rate, parent_rate); +} + +static int raspberrypi_clock_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) +{ + struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_clock_data *data = pll->data; + + return raspberrypi_determine_rate(rpi, data->clock_id, data->name, pll->min_rate, pll->max_rate, req); +} + +static int raspberrypi_fw_pll_is_on(struct clk_hw *hw) +{ + struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_pll_data *data = pll->data; + + return raspberrypi_fw_is_on(rpi, data->clock_id, data->name); +} + +static unsigned long raspberrypi_fw_pll_get_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_pll_data *data = pll->data; + + return raspberrypi_fw_get_rate(rpi, data->clock_id, data->name, parent_rate); +} + +static int raspberrypi_fw_pll_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_pll_data *data = pll->data; + + return raspberrypi_fw_set_rate(rpi, data->clock_id, data->name, rate, parent_rate); +} + +static int raspberrypi_pll_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) +{ + struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_pll_data *data = pll->data; + + return raspberrypi_determine_rate(rpi, data->clock_id, data->name, pll->min_rate, pll->max_rate, req); +} + + +static int raspberrypi_fw_pll_div_is_on(struct clk_hw *hw) +{ + struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_pll_divider_data *data = pll->data; + + return raspberrypi_fw_is_on(rpi, data->clock_id, data->name); +} + +static unsigned long raspberrypi_fw_pll_div_get_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_pll_divider_data *data = pll->data; + + return raspberrypi_fw_get_rate(rpi, data->clock_id, data->name, parent_rate); +} + +static int raspberrypi_fw_pll_div_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_pll_divider_data *data = pll->data; + + return raspberrypi_fw_set_rate(rpi, data->clock_id, data->name, rate, parent_rate); +} + +static int raspberrypi_pll_div_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) +{ + struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw); + struct raspberrypi_clk *rpi = pll->rpi; + const struct raspberrypi_pll_divider_data *data = pll->data; + + return raspberrypi_determine_rate(rpi, data->clock_id, data->name, pll->min_rate, pll->max_rate, req); +} + + static const struct clk_ops raspberrypi_firmware_pll_clk_ops = { .is_prepared = raspberrypi_fw_pll_is_on, .recalc_rate = raspberrypi_fw_pll_get_rate, @@ -173,87 +358,225 @@ static const struct clk_ops raspberrypi_ .determine_rate = raspberrypi_pll_determine_rate, }; -static int raspberrypi_register_pllb(struct raspberrypi_clk *rpi) +static const struct clk_ops raspberrypi_firmware_pll_divider_clk_ops = { + .is_prepared = raspberrypi_fw_pll_div_is_on, + .recalc_rate = raspberrypi_fw_pll_div_get_rate, + .set_rate = raspberrypi_fw_pll_div_set_rate, + .determine_rate = raspberrypi_pll_div_determine_rate, +}; + +static const struct clk_ops raspberrypi_firmware_clk_ops = { + .is_prepared = raspberrypi_fw_clock_is_on, + .recalc_rate = raspberrypi_fw_clock_get_rate, + .set_rate = raspberrypi_fw_clock_set_rate, + .determine_rate = raspberrypi_clock_determine_rate, +}; + + +static int raspberrypi_get_clock_range(struct raspberrypi_clk *rpi, u32 clock_id, u32 *min_rate, u32 *max_rate) { - u32 min_rate = 0, max_rate = 0; + int ret; + + /* Get min & max rates set by the firmware */ + ret = raspberrypi_clock_property(rpi->firmware, + RPI_FIRMWARE_GET_MIN_CLOCK_RATE, + clock_id, + min_rate); + if (ret) { + dev_err(rpi->dev, "Failed to get clock %d min freq: %d (%d)\n", + clock_id, *min_rate, ret); + return ret; + } + + ret = raspberrypi_clock_property(rpi->firmware, + RPI_FIRMWARE_GET_MAX_CLOCK_RATE, + clock_id, + max_rate); + if (ret) { + dev_err(rpi->dev, "Failed to get clock %d max freq: %d (%d)\n", + clock_id, *max_rate, ret); + return ret; + } + return 0; +} + + +static int raspberrypi_register_pll(struct raspberrypi_clk *rpi, + const struct raspberrypi_pll_data *data) +{ + struct raspberrypi_pll *pll; struct clk_init_data init; int ret; memset(&init, 0, sizeof(init)); /* All of the PLLs derive from the external oscillator. */ - init.parent_names = (const char *[]){ "osc" }; - init.num_parents = 1; - init.name = "pllb"; + init.parent_names = data->parents; + init.num_parents = data->num_parents; + init.name = data->name; init.ops = &raspberrypi_firmware_pll_clk_ops; init.flags = CLK_GET_RATE_NOCACHE | CLK_IGNORE_UNUSED; - /* Get min & max rates set by the firmware */ - ret = raspberrypi_clock_property(rpi->firmware, - RPI_FIRMWARE_GET_MIN_CLOCK_RATE, - RPI_FIRMWARE_ARM_CLK_ID, - &min_rate); + pll = kzalloc(sizeof(*pll), GFP_KERNEL); + if (!pll) + return -ENOMEM; + pll->rpi = rpi; + pll->data = data; + pll->hw.init = &init; + + ret = raspberrypi_get_clock_range(rpi, data->clock_id, &pll->min_rate, &pll->max_rate); if (ret) { - dev_err(rpi->dev, "Failed to get %s min freq: %d\n", - init.name, ret); + dev_err(rpi->dev, "%s: raspberrypi_get_clock_range(%s) failed: %d\n", __func__, init.name, ret); return ret; } - ret = raspberrypi_clock_property(rpi->firmware, - RPI_FIRMWARE_GET_MAX_CLOCK_RATE, - RPI_FIRMWARE_ARM_CLK_ID, - &max_rate); + ret = devm_clk_hw_register(rpi->dev, &pll->hw); if (ret) { - dev_err(rpi->dev, "Failed to get %s max freq: %d\n", - init.name, ret); + dev_err(rpi->dev, "%s: devm_clk_hw_register(%s) failed: %d\n", __func__, init.name, ret); return ret; } + return 0; +} - if (!min_rate || !max_rate) { - dev_err(rpi->dev, "Unexpected frequency range: min %u, max %u\n", - min_rate, max_rate); - return -EINVAL; - } +static int +raspberrypi_register_pll_divider(struct raspberrypi_clk *rpi, + const struct raspberrypi_pll_divider_data *data) +{ + struct raspberrypi_pll_divider *divider; + struct clk_init_data init; + int ret; + + memset(&init, 0, sizeof(init)); + + init.parent_names = &data->source_pll; + init.num_parents = 1; + init.name = data->name; + init.ops = &raspberrypi_firmware_pll_divider_clk_ops; + init.flags = data->flags | CLK_IGNORE_UNUSED; - dev_info(rpi->dev, "CPU frequency range: min %u, max %u\n", - min_rate, max_rate); + divider = devm_kzalloc(rpi->dev, sizeof(*divider), GFP_KERNEL); + if (!divider) + return -ENOMEM; + + divider->div.hw.init = &init; + divider->rpi = rpi; + divider->data = data; + + ret = raspberrypi_get_clock_range(rpi, data->clock_id, ÷r->min_rate, ÷r->max_rate); + if (ret) { + dev_err(rpi->dev, "%s: raspberrypi_get_clock_range(%s) failed: %d\n", __func__, init.name, ret); + return ret; + } - rpi->min_rate = min_rate * RPI_FIRMWARE_PLLB_ARM_DIV_RATE; - rpi->max_rate = max_rate * RPI_FIRMWARE_PLLB_ARM_DIV_RATE; + ret = devm_clk_hw_register(rpi->dev, ÷r->div.hw); + if (ret) { + dev_err(rpi->dev, "%s: devm_clk_hw_register(%s) failed: %d\n", __func__, init.name, ret); + return ret; + } - rpi->pllb.init = &init; + /* + * PLLH's channels have a fixed divide by 10 afterwards, which + * is what our consumers are actually using. + */ + if (data->fixed_divider != 0) { + struct clk_lookup *lookup; + struct clk_hw *clk = clk_hw_register_fixed_factor(rpi->dev, + data->divider_name, + data->name, + CLK_SET_RATE_PARENT, + 1, + data->fixed_divider); + if (IS_ERR(clk)) { + dev_err(rpi->dev, "%s: clk_hw_register_fixed_factor(%s) failed: %ld\n", __func__, init.name, PTR_ERR(clk)); + return PTR_ERR(clk); + } + if (data->lookup) { + lookup = clkdev_hw_create(clk, NULL, data->lookup); + if (IS_ERR(lookup)) { + dev_err(rpi->dev, "%s: clk_hw_register_fixed_factor(%s) failed: %ld\n", __func__, init.name, PTR_ERR(lookup)); + return PTR_ERR(lookup); + } + } + } - return devm_clk_hw_register(rpi->dev, &rpi->pllb); + return 0; } -static int raspberrypi_register_pllb_arm(struct raspberrypi_clk *rpi) +static int raspberrypi_register_clock(struct raspberrypi_clk *rpi, + const struct raspberrypi_clock_data *data) { - rpi->pllb_arm = clk_hw_register_fixed_factor(rpi->dev, - "pllb_arm", "pllb", - CLK_SET_RATE_PARENT | CLK_GET_RATE_NOCACHE, - 1, 2); - if (IS_ERR(rpi->pllb_arm)) { - dev_err(rpi->dev, "Failed to initialize pllb_arm\n"); - return PTR_ERR(rpi->pllb_arm); - } + struct raspberrypi_clock *clock; + struct clk_init_data init; + struct clk *clk; + int ret; + + memset(&init, 0, sizeof(init)); + init.parent_names = data->parents; + init.num_parents = data->num_parents; + init.name = data->name; + init.flags = data->flags | CLK_IGNORE_UNUSED; - rpi->pllb_arm_lookup = clkdev_hw_create(rpi->pllb_arm, NULL, "cpu0"); - if (!rpi->pllb_arm_lookup) { - dev_err(rpi->dev, "Failed to initialize pllb_arm_lookup\n"); - clk_hw_unregister_fixed_factor(rpi->pllb_arm); + init.ops = &raspberrypi_firmware_clk_ops; + + clock = devm_kzalloc(rpi->dev, sizeof(*clock), GFP_KERNEL); + if (!clock) return -ENOMEM; - } + clock->rpi = rpi; + clock->data = data; + clock->hw.init = &init; + + ret = raspberrypi_get_clock_range(rpi, data->clock_id, &clock->min_rate, &clock->max_rate); + if (ret) { + dev_err(rpi->dev, "%s: raspberrypi_get_clock_range(%s) failed: %d\n", __func__, init.name, ret); + return ret; + } + clk = devm_clk_register(rpi->dev, &clock->hw); + if (IS_ERR(clk)) { + dev_err(rpi->dev, "%s: devm_clk_register(%s) failed: %ld\n", __func__, init.name, PTR_ERR(clk)); + return PTR_ERR(clk); + } + ret = clk_register_clkdev(clk, init.name, NULL); + if (ret) { + dev_err(rpi->dev, "%s: clk_register_clkdev(%s) failed: %d\n", __func__, init.name, ret); + return ret; + } return 0; } + +/* + * the real definition of all the pll, pll_dividers and clocks + * these make use of the above REGISTER_* macros + */ +static const struct raspberrypi_clk_desc clk_desc_array[] = { + /* the PLL + PLL dividers */ + [BCM2835_CLOCK_V3D] = REGISTER_CLK( + SOC_ALL, + .name = "v3d", + .parents = (const char *[]){ "osc" }, + .num_parents = 1, + .clock_id = RPI_FIRMWARE_V3D_CLK_ID), + [BCM2835_PLLB_ARM] = REGISTER_PLL_DIV( + SOC_ALL, + .name = "pllb", + .source_pll = "osc", + .divider_name = "pllb_arm", + .lookup = "cpu0", + .fixed_divider = 1, + .clock_id = RPI_FIRMWARE_ARM_CLK_ID, + .flags = CLK_SET_RATE_PARENT), +}; + static int raspberrypi_clk_probe(struct platform_device *pdev) { struct device_node *firmware_node; struct device *dev = &pdev->dev; struct rpi_firmware *firmware; struct raspberrypi_clk *rpi; - int ret; + const struct raspberrypi_clk_desc *desc; + const size_t asize = ARRAY_SIZE(clk_desc_array); + int i; firmware_node = of_find_compatible_node(NULL, NULL, "raspberrypi,bcm2835-firmware"); @@ -275,16 +598,16 @@ static int raspberrypi_clk_probe(struct rpi->firmware = firmware; platform_set_drvdata(pdev, rpi); - ret = raspberrypi_register_pllb(rpi); - if (ret) { - dev_err(dev, "Failed to initialize pllb, %d\n", ret); - return ret; + for (i = 0; i < asize; i++) { + desc = &clk_desc_array[i]; + if (desc->clk_register && desc->data /*&& + (desc->supported & pdata->soc)*/) { + int ret = desc->clk_register(rpi, desc->data); + if (ret) + return ret; + } } - ret = raspberrypi_register_pllb_arm(rpi); - if (ret) - return ret; - rpi->cpufreq = platform_device_register_data(dev, "raspberrypi-cpufreq", -1, NULL, 0);