Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 1 | // SPDX-License-Identifier: GPL-2.0+ |
| 2 | /* |
| 3 | * CPU frequency scaling support for Armada 37xx platform. |
| 4 | * |
| 5 | * Copyright (C) 2017 Marvell |
| 6 | * |
| 7 | * Gregory CLEMENT <gregory.clement@free-electrons.com> |
| 8 | */ |
| 9 | |
| 10 | #include <linux/clk.h> |
| 11 | #include <linux/cpu.h> |
| 12 | #include <linux/cpufreq.h> |
| 13 | #include <linux/err.h> |
| 14 | #include <linux/interrupt.h> |
| 15 | #include <linux/io.h> |
| 16 | #include <linux/mfd/syscon.h> |
| 17 | #include <linux/module.h> |
| 18 | #include <linux/of_address.h> |
| 19 | #include <linux/of_device.h> |
| 20 | #include <linux/of_irq.h> |
| 21 | #include <linux/platform_device.h> |
| 22 | #include <linux/pm_opp.h> |
| 23 | #include <linux/regmap.h> |
| 24 | #include <linux/slab.h> |
| 25 | |
Miquel Raynal | 9c28d6d | 2018-04-26 15:42:17 +0530 | [diff] [blame] | 26 | #include "cpufreq-dt.h" |
| 27 | |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 28 | /* Power management in North Bridge register set */ |
| 29 | #define ARMADA_37XX_NB_L0L1 0x18 |
| 30 | #define ARMADA_37XX_NB_L2L3 0x1C |
| 31 | #define ARMADA_37XX_NB_TBG_DIV_OFF 13 |
| 32 | #define ARMADA_37XX_NB_TBG_DIV_MASK 0x7 |
| 33 | #define ARMADA_37XX_NB_CLK_SEL_OFF 11 |
| 34 | #define ARMADA_37XX_NB_CLK_SEL_MASK 0x1 |
| 35 | #define ARMADA_37XX_NB_CLK_SEL_TBG 0x1 |
| 36 | #define ARMADA_37XX_NB_TBG_SEL_OFF 9 |
| 37 | #define ARMADA_37XX_NB_TBG_SEL_MASK 0x3 |
| 38 | #define ARMADA_37XX_NB_VDD_SEL_OFF 6 |
| 39 | #define ARMADA_37XX_NB_VDD_SEL_MASK 0x3 |
| 40 | #define ARMADA_37XX_NB_CONFIG_SHIFT 16 |
| 41 | #define ARMADA_37XX_NB_DYN_MOD 0x24 |
| 42 | #define ARMADA_37XX_NB_CLK_SEL_EN BIT(26) |
| 43 | #define ARMADA_37XX_NB_TBG_EN BIT(28) |
| 44 | #define ARMADA_37XX_NB_DIV_EN BIT(29) |
| 45 | #define ARMADA_37XX_NB_VDD_EN BIT(30) |
| 46 | #define ARMADA_37XX_NB_DFS_EN BIT(31) |
| 47 | #define ARMADA_37XX_NB_CPU_LOAD 0x30 |
| 48 | #define ARMADA_37XX_NB_CPU_LOAD_MASK 0x3 |
| 49 | #define ARMADA_37XX_DVFS_LOAD_0 0 |
| 50 | #define ARMADA_37XX_DVFS_LOAD_1 1 |
| 51 | #define ARMADA_37XX_DVFS_LOAD_2 2 |
| 52 | #define ARMADA_37XX_DVFS_LOAD_3 3 |
| 53 | |
| 54 | /* |
| 55 | * On Armada 37xx the Power management manages 4 level of CPU load, |
| 56 | * each level can be associated with a CPU clock source, a CPU |
| 57 | * divider, a VDD level, etc... |
| 58 | */ |
| 59 | #define LOAD_LEVEL_NR 4 |
| 60 | |
Miquel Raynal | 9c28d6d | 2018-04-26 15:42:17 +0530 | [diff] [blame] | 61 | struct armada37xx_cpufreq_state { |
| 62 | struct regmap *regmap; |
| 63 | u32 nb_l0l1; |
| 64 | u32 nb_l2l3; |
| 65 | u32 nb_dyn_mod; |
| 66 | u32 nb_cpu_load; |
| 67 | }; |
| 68 | |
| 69 | static struct armada37xx_cpufreq_state *armada37xx_cpufreq_state; |
| 70 | |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 71 | struct armada_37xx_dvfs { |
| 72 | u32 cpu_freq_max; |
| 73 | u8 divider[LOAD_LEVEL_NR]; |
| 74 | }; |
| 75 | |
| 76 | static struct armada_37xx_dvfs armada_37xx_dvfs[] = { |
| 77 | {.cpu_freq_max = 1200*1000*1000, .divider = {1, 2, 4, 6} }, |
| 78 | {.cpu_freq_max = 1000*1000*1000, .divider = {1, 2, 4, 5} }, |
| 79 | {.cpu_freq_max = 800*1000*1000, .divider = {1, 2, 3, 4} }, |
| 80 | {.cpu_freq_max = 600*1000*1000, .divider = {2, 4, 5, 6} }, |
| 81 | }; |
| 82 | |
| 83 | static struct armada_37xx_dvfs *armada_37xx_cpu_freq_info_get(u32 freq) |
| 84 | { |
| 85 | int i; |
| 86 | |
| 87 | for (i = 0; i < ARRAY_SIZE(armada_37xx_dvfs); i++) { |
| 88 | if (freq == armada_37xx_dvfs[i].cpu_freq_max) |
| 89 | return &armada_37xx_dvfs[i]; |
| 90 | } |
| 91 | |
| 92 | pr_err("Unsupported CPU frequency %d MHz\n", freq/1000000); |
| 93 | return NULL; |
| 94 | } |
| 95 | |
| 96 | /* |
| 97 | * Setup the four level managed by the hardware. Once the four level |
| 98 | * will be configured then the DVFS will be enabled. |
| 99 | */ |
| 100 | static void __init armada37xx_cpufreq_dvfs_setup(struct regmap *base, |
| 101 | struct clk *clk, u8 *divider) |
| 102 | { |
| 103 | int load_lvl; |
| 104 | struct clk *parent; |
| 105 | |
| 106 | for (load_lvl = 0; load_lvl < LOAD_LEVEL_NR; load_lvl++) { |
| 107 | unsigned int reg, mask, val, offset = 0; |
| 108 | |
| 109 | if (load_lvl <= ARMADA_37XX_DVFS_LOAD_1) |
| 110 | reg = ARMADA_37XX_NB_L0L1; |
| 111 | else |
| 112 | reg = ARMADA_37XX_NB_L2L3; |
| 113 | |
| 114 | if (load_lvl == ARMADA_37XX_DVFS_LOAD_0 || |
| 115 | load_lvl == ARMADA_37XX_DVFS_LOAD_2) |
| 116 | offset += ARMADA_37XX_NB_CONFIG_SHIFT; |
| 117 | |
| 118 | /* Set cpu clock source, for all the level we use TBG */ |
| 119 | val = ARMADA_37XX_NB_CLK_SEL_TBG << ARMADA_37XX_NB_CLK_SEL_OFF; |
| 120 | mask = (ARMADA_37XX_NB_CLK_SEL_MASK |
| 121 | << ARMADA_37XX_NB_CLK_SEL_OFF); |
| 122 | |
| 123 | /* |
| 124 | * Set cpu divider based on the pre-computed array in |
| 125 | * order to have balanced step. |
| 126 | */ |
| 127 | val |= divider[load_lvl] << ARMADA_37XX_NB_TBG_DIV_OFF; |
| 128 | mask |= (ARMADA_37XX_NB_TBG_DIV_MASK |
| 129 | << ARMADA_37XX_NB_TBG_DIV_OFF); |
| 130 | |
| 131 | /* Set VDD divider which is actually the load level. */ |
| 132 | val |= load_lvl << ARMADA_37XX_NB_VDD_SEL_OFF; |
| 133 | mask |= (ARMADA_37XX_NB_VDD_SEL_MASK |
| 134 | << ARMADA_37XX_NB_VDD_SEL_OFF); |
| 135 | |
| 136 | val <<= offset; |
| 137 | mask <<= offset; |
| 138 | |
| 139 | regmap_update_bits(base, reg, mask, val); |
| 140 | } |
| 141 | |
| 142 | /* |
| 143 | * Set cpu clock source, for all the level we keep the same |
| 144 | * clock source that the one already configured. For this one |
| 145 | * we need to use the clock framework |
| 146 | */ |
| 147 | parent = clk_get_parent(clk); |
| 148 | clk_set_parent(clk, parent); |
| 149 | } |
| 150 | |
Miquel Raynal | 9c28d6d | 2018-04-26 15:42:17 +0530 | [diff] [blame] | 151 | static void armada37xx_cpufreq_disable_dvfs(struct regmap *base) |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 152 | { |
| 153 | unsigned int reg = ARMADA_37XX_NB_DYN_MOD, |
| 154 | mask = ARMADA_37XX_NB_DFS_EN; |
| 155 | |
| 156 | regmap_update_bits(base, reg, mask, 0); |
| 157 | } |
| 158 | |
| 159 | static void __init armada37xx_cpufreq_enable_dvfs(struct regmap *base) |
| 160 | { |
| 161 | unsigned int val, reg = ARMADA_37XX_NB_CPU_LOAD, |
| 162 | mask = ARMADA_37XX_NB_CPU_LOAD_MASK; |
| 163 | |
| 164 | /* Start with the highest load (0) */ |
| 165 | val = ARMADA_37XX_DVFS_LOAD_0; |
| 166 | regmap_update_bits(base, reg, mask, val); |
| 167 | |
| 168 | /* Now enable DVFS for the CPUs */ |
| 169 | reg = ARMADA_37XX_NB_DYN_MOD; |
| 170 | mask = ARMADA_37XX_NB_CLK_SEL_EN | ARMADA_37XX_NB_TBG_EN | |
| 171 | ARMADA_37XX_NB_DIV_EN | ARMADA_37XX_NB_VDD_EN | |
| 172 | ARMADA_37XX_NB_DFS_EN; |
| 173 | |
| 174 | regmap_update_bits(base, reg, mask, mask); |
| 175 | } |
| 176 | |
Miquel Raynal | 9c28d6d | 2018-04-26 15:42:17 +0530 | [diff] [blame] | 177 | static int armada37xx_cpufreq_suspend(struct cpufreq_policy *policy) |
| 178 | { |
| 179 | struct armada37xx_cpufreq_state *state = armada37xx_cpufreq_state; |
| 180 | |
| 181 | regmap_read(state->regmap, ARMADA_37XX_NB_L0L1, &state->nb_l0l1); |
| 182 | regmap_read(state->regmap, ARMADA_37XX_NB_L2L3, &state->nb_l2l3); |
| 183 | regmap_read(state->regmap, ARMADA_37XX_NB_CPU_LOAD, |
| 184 | &state->nb_cpu_load); |
| 185 | regmap_read(state->regmap, ARMADA_37XX_NB_DYN_MOD, &state->nb_dyn_mod); |
| 186 | |
| 187 | return 0; |
| 188 | } |
| 189 | |
| 190 | static int armada37xx_cpufreq_resume(struct cpufreq_policy *policy) |
| 191 | { |
| 192 | struct armada37xx_cpufreq_state *state = armada37xx_cpufreq_state; |
| 193 | |
| 194 | /* Ensure DVFS is disabled otherwise the following registers are RO */ |
| 195 | armada37xx_cpufreq_disable_dvfs(state->regmap); |
| 196 | |
| 197 | regmap_write(state->regmap, ARMADA_37XX_NB_L0L1, state->nb_l0l1); |
| 198 | regmap_write(state->regmap, ARMADA_37XX_NB_L2L3, state->nb_l2l3); |
| 199 | regmap_write(state->regmap, ARMADA_37XX_NB_CPU_LOAD, |
| 200 | state->nb_cpu_load); |
| 201 | |
| 202 | /* |
| 203 | * NB_DYN_MOD register is the one that actually enable back DVFS if it |
| 204 | * was enabled before the suspend operation. This must be done last |
| 205 | * otherwise other registers are not writable. |
| 206 | */ |
| 207 | regmap_write(state->regmap, ARMADA_37XX_NB_DYN_MOD, state->nb_dyn_mod); |
| 208 | |
| 209 | return 0; |
| 210 | } |
| 211 | |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 212 | static int __init armada37xx_cpufreq_driver_init(void) |
| 213 | { |
Miquel Raynal | 9c28d6d | 2018-04-26 15:42:17 +0530 | [diff] [blame] | 214 | struct cpufreq_dt_platform_data pdata; |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 215 | struct armada_37xx_dvfs *dvfs; |
| 216 | struct platform_device *pdev; |
Viresh Kumar | cfd84631 | 2018-04-24 15:09:46 +0530 | [diff] [blame] | 217 | unsigned long freq; |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 218 | unsigned int cur_frequency; |
| 219 | struct regmap *nb_pm_base; |
| 220 | struct device *cpu_dev; |
| 221 | int load_lvl, ret; |
| 222 | struct clk *clk; |
| 223 | |
| 224 | nb_pm_base = |
| 225 | syscon_regmap_lookup_by_compatible("marvell,armada-3700-nb-pm"); |
| 226 | |
| 227 | if (IS_ERR(nb_pm_base)) |
| 228 | return -ENODEV; |
| 229 | |
| 230 | /* Before doing any configuration on the DVFS first, disable it */ |
| 231 | armada37xx_cpufreq_disable_dvfs(nb_pm_base); |
| 232 | |
| 233 | /* |
| 234 | * On CPU 0 register the operating points supported (which are |
| 235 | * the nominal CPU frequency and full integer divisions of |
| 236 | * it). |
| 237 | */ |
| 238 | cpu_dev = get_cpu_device(0); |
| 239 | if (!cpu_dev) { |
| 240 | dev_err(cpu_dev, "Cannot get CPU\n"); |
| 241 | return -ENODEV; |
| 242 | } |
| 243 | |
| 244 | clk = clk_get(cpu_dev, 0); |
| 245 | if (IS_ERR(clk)) { |
| 246 | dev_err(cpu_dev, "Cannot get clock for CPU0\n"); |
| 247 | return PTR_ERR(clk); |
| 248 | } |
| 249 | |
| 250 | /* Get nominal (current) CPU frequency */ |
| 251 | cur_frequency = clk_get_rate(clk); |
| 252 | if (!cur_frequency) { |
| 253 | dev_err(cpu_dev, "Failed to get clock rate for CPU\n"); |
Gregory CLEMENT | bbcc328 | 2018-04-04 16:44:44 +0200 | [diff] [blame] | 254 | clk_put(clk); |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 255 | return -EINVAL; |
| 256 | } |
| 257 | |
| 258 | dvfs = armada_37xx_cpu_freq_info_get(cur_frequency); |
Viresh Kumar | cfd84631 | 2018-04-24 15:09:46 +0530 | [diff] [blame] | 259 | if (!dvfs) { |
| 260 | clk_put(clk); |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 261 | return -EINVAL; |
Viresh Kumar | cfd84631 | 2018-04-24 15:09:46 +0530 | [diff] [blame] | 262 | } |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 263 | |
Miquel Raynal | 9c28d6d | 2018-04-26 15:42:17 +0530 | [diff] [blame] | 264 | armada37xx_cpufreq_state = kmalloc(sizeof(*armada37xx_cpufreq_state), |
| 265 | GFP_KERNEL); |
| 266 | if (!armada37xx_cpufreq_state) { |
| 267 | clk_put(clk); |
| 268 | return -ENOMEM; |
| 269 | } |
| 270 | |
| 271 | armada37xx_cpufreq_state->regmap = nb_pm_base; |
| 272 | |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 273 | armada37xx_cpufreq_dvfs_setup(nb_pm_base, clk, dvfs->divider); |
Gregory CLEMENT | bbcc328 | 2018-04-04 16:44:44 +0200 | [diff] [blame] | 274 | clk_put(clk); |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 275 | |
| 276 | for (load_lvl = ARMADA_37XX_DVFS_LOAD_0; load_lvl < LOAD_LEVEL_NR; |
| 277 | load_lvl++) { |
Viresh Kumar | cfd84631 | 2018-04-24 15:09:46 +0530 | [diff] [blame] | 278 | freq = cur_frequency / dvfs->divider[load_lvl]; |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 279 | |
| 280 | ret = dev_pm_opp_add(cpu_dev, freq, 0); |
Viresh Kumar | cfd84631 | 2018-04-24 15:09:46 +0530 | [diff] [blame] | 281 | if (ret) |
| 282 | goto remove_opp; |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 283 | } |
| 284 | |
| 285 | /* Now that everything is setup, enable the DVFS at hardware level */ |
| 286 | armada37xx_cpufreq_enable_dvfs(nb_pm_base); |
| 287 | |
Miquel Raynal | 9c28d6d | 2018-04-26 15:42:17 +0530 | [diff] [blame] | 288 | pdata.suspend = armada37xx_cpufreq_suspend; |
| 289 | pdata.resume = armada37xx_cpufreq_resume; |
| 290 | |
| 291 | pdev = platform_device_register_data(NULL, "cpufreq-dt", -1, &pdata, |
| 292 | sizeof(pdata)); |
Viresh Kumar | cfd84631 | 2018-04-24 15:09:46 +0530 | [diff] [blame] | 293 | ret = PTR_ERR_OR_ZERO(pdev); |
| 294 | if (ret) |
| 295 | goto disable_dvfs; |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 296 | |
Viresh Kumar | cfd84631 | 2018-04-24 15:09:46 +0530 | [diff] [blame] | 297 | return 0; |
| 298 | |
| 299 | disable_dvfs: |
| 300 | armada37xx_cpufreq_disable_dvfs(nb_pm_base); |
| 301 | remove_opp: |
| 302 | /* clean-up the already added opp before leaving */ |
| 303 | while (load_lvl-- > ARMADA_37XX_DVFS_LOAD_0) { |
| 304 | freq = cur_frequency / dvfs->divider[load_lvl]; |
| 305 | dev_pm_opp_remove(cpu_dev, freq); |
| 306 | } |
| 307 | |
Miquel Raynal | 9c28d6d | 2018-04-26 15:42:17 +0530 | [diff] [blame] | 308 | kfree(armada37xx_cpufreq_state); |
| 309 | |
Viresh Kumar | cfd84631 | 2018-04-24 15:09:46 +0530 | [diff] [blame] | 310 | return ret; |
Gregory CLEMENT | 92ce45f | 2017-12-14 16:00:05 +0100 | [diff] [blame] | 311 | } |
| 312 | /* late_initcall, to guarantee the driver is loaded after A37xx clock driver */ |
| 313 | late_initcall(armada37xx_cpufreq_driver_init); |
| 314 | |
| 315 | MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>"); |
| 316 | MODULE_DESCRIPTION("Armada 37xx cpufreq driver"); |
| 317 | MODULE_LICENSE("GPL"); |