358 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (c) 2020 MediaTek Inc.4 */5 6#include <linux/bitfield.h>7#include <linux/cpufreq.h>8#include <linux/energy_model.h>9#include <linux/init.h>10#include <linux/iopoll.h>11#include <linux/kernel.h>12#include <linux/module.h>13#include <linux/of.h>14#include <linux/of_platform.h>15#include <linux/platform_device.h>16#include <linux/regulator/consumer.h>17#include <linux/slab.h>18 19#define LUT_MAX_ENTRIES 32U20#define LUT_FREQ GENMASK(11, 0)21#define LUT_ROW_SIZE 0x422#define CPUFREQ_HW_STATUS BIT(0)23#define SVS_HW_STATUS BIT(1)24#define POLL_USEC 100025#define TIMEOUT_USEC 30000026 27enum {28 REG_FREQ_LUT_TABLE,29 REG_FREQ_ENABLE,30 REG_FREQ_PERF_STATE,31 REG_FREQ_HW_STATE,32 REG_EM_POWER_TBL,33 REG_FREQ_LATENCY,34 35 REG_ARRAY_SIZE,36};37 38struct mtk_cpufreq_data {39 struct cpufreq_frequency_table *table;40 void __iomem *reg_bases[REG_ARRAY_SIZE];41 struct resource *res;42 void __iomem *base;43 int nr_opp;44};45 46static const u16 cpufreq_mtk_offsets[REG_ARRAY_SIZE] = {47 [REG_FREQ_LUT_TABLE] = 0x0,48 [REG_FREQ_ENABLE] = 0x84,49 [REG_FREQ_PERF_STATE] = 0x88,50 [REG_FREQ_HW_STATE] = 0x8c,51 [REG_EM_POWER_TBL] = 0x90,52 [REG_FREQ_LATENCY] = 0x110,53};54 55static int __maybe_unused56mtk_cpufreq_get_cpu_power(struct device *cpu_dev, unsigned long *uW,57 unsigned long *KHz)58{59 struct mtk_cpufreq_data *data;60 struct cpufreq_policy *policy;61 int i;62 63 policy = cpufreq_cpu_get_raw(cpu_dev->id);64 if (!policy)65 return 0;66 67 data = policy->driver_data;68 69 for (i = 0; i < data->nr_opp; i++) {70 if (data->table[i].frequency < *KHz)71 break;72 }73 i--;74 75 *KHz = data->table[i].frequency;76 /* Provide micro-Watts value to the Energy Model */77 *uW = readl_relaxed(data->reg_bases[REG_EM_POWER_TBL] +78 i * LUT_ROW_SIZE);79 80 return 0;81}82 83static int mtk_cpufreq_hw_target_index(struct cpufreq_policy *policy,84 unsigned int index)85{86 struct mtk_cpufreq_data *data = policy->driver_data;87 88 writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);89 90 return 0;91}92 93static unsigned int mtk_cpufreq_hw_get(unsigned int cpu)94{95 struct mtk_cpufreq_data *data;96 struct cpufreq_policy *policy;97 unsigned int index;98 99 policy = cpufreq_cpu_get_raw(cpu);100 if (!policy)101 return 0;102 103 data = policy->driver_data;104 105 index = readl_relaxed(data->reg_bases[REG_FREQ_PERF_STATE]);106 index = min(index, LUT_MAX_ENTRIES - 1);107 108 return data->table[index].frequency;109}110 111static unsigned int mtk_cpufreq_hw_fast_switch(struct cpufreq_policy *policy,112 unsigned int target_freq)113{114 struct mtk_cpufreq_data *data = policy->driver_data;115 unsigned int index;116 117 index = cpufreq_table_find_index_dl(policy, target_freq, false);118 119 writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);120 121 return policy->freq_table[index].frequency;122}123 124static int mtk_cpu_create_freq_table(struct platform_device *pdev,125 struct mtk_cpufreq_data *data)126{127 struct device *dev = &pdev->dev;128 u32 temp, i, freq, prev_freq = 0;129 void __iomem *base_table;130 131 data->table = devm_kcalloc(dev, LUT_MAX_ENTRIES + 1,132 sizeof(*data->table), GFP_KERNEL);133 if (!data->table)134 return -ENOMEM;135 136 base_table = data->reg_bases[REG_FREQ_LUT_TABLE];137 138 for (i = 0; i < LUT_MAX_ENTRIES; i++) {139 temp = readl_relaxed(base_table + (i * LUT_ROW_SIZE));140 freq = FIELD_GET(LUT_FREQ, temp) * 1000;141 142 if (freq == prev_freq)143 break;144 145 data->table[i].frequency = freq;146 147 dev_dbg(dev, "index=%d freq=%d\n", i, data->table[i].frequency);148 149 prev_freq = freq;150 }151 152 data->table[i].frequency = CPUFREQ_TABLE_END;153 data->nr_opp = i;154 155 return 0;156}157 158static int mtk_cpu_resources_init(struct platform_device *pdev,159 struct cpufreq_policy *policy,160 const u16 *offsets)161{162 struct mtk_cpufreq_data *data;163 struct device *dev = &pdev->dev;164 struct resource *res;165 struct of_phandle_args args;166 void __iomem *base;167 int ret, i;168 int index;169 170 data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);171 if (!data)172 return -ENOMEM;173 174 ret = of_perf_domain_get_sharing_cpumask(policy->cpu, "performance-domains",175 "#performance-domain-cells",176 policy->cpus, &args);177 if (ret < 0)178 return ret;179 180 index = args.args[0];181 of_node_put(args.np);182 183 res = platform_get_resource(pdev, IORESOURCE_MEM, index);184 if (!res) {185 dev_err(dev, "failed to get mem resource %d\n", index);186 return -ENODEV;187 }188 189 if (!request_mem_region(res->start, resource_size(res), res->name)) {190 dev_err(dev, "failed to request resource %pR\n", res);191 return -EBUSY;192 }193 194 base = ioremap(res->start, resource_size(res));195 if (!base) {196 dev_err(dev, "failed to map resource %pR\n", res);197 ret = -ENOMEM;198 goto release_region;199 }200 201 data->base = base;202 data->res = res;203 204 for (i = REG_FREQ_LUT_TABLE; i < REG_ARRAY_SIZE; i++)205 data->reg_bases[i] = base + offsets[i];206 207 ret = mtk_cpu_create_freq_table(pdev, data);208 if (ret) {209 dev_info(dev, "Domain-%d failed to create freq table\n", index);210 return ret;211 }212 213 policy->freq_table = data->table;214 policy->driver_data = data;215 216 return 0;217release_region:218 release_mem_region(res->start, resource_size(res));219 return ret;220}221 222static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)223{224 struct platform_device *pdev = cpufreq_get_driver_data();225 int sig, pwr_hw = CPUFREQ_HW_STATUS | SVS_HW_STATUS;226 struct mtk_cpufreq_data *data;227 unsigned int latency;228 int ret;229 230 /* Get the bases of cpufreq for domains */231 ret = mtk_cpu_resources_init(pdev, policy, platform_get_drvdata(pdev));232 if (ret) {233 dev_info(&pdev->dev, "CPUFreq resource init failed\n");234 return ret;235 }236 237 data = policy->driver_data;238 239 latency = readl_relaxed(data->reg_bases[REG_FREQ_LATENCY]) * 1000;240 if (!latency)241 latency = CPUFREQ_ETERNAL;242 243 policy->cpuinfo.transition_latency = latency;244 policy->fast_switch_possible = true;245 246 /* HW should be in enabled state to proceed now */247 writel_relaxed(0x1, data->reg_bases[REG_FREQ_ENABLE]);248 if (readl_poll_timeout(data->reg_bases[REG_FREQ_HW_STATE], sig,249 (sig & pwr_hw) == pwr_hw, POLL_USEC,250 TIMEOUT_USEC)) {251 if (!(sig & CPUFREQ_HW_STATUS)) {252 pr_info("cpufreq hardware of CPU%d is not enabled\n",253 policy->cpu);254 return -ENODEV;255 }256 257 pr_info("SVS of CPU%d is not enabled\n", policy->cpu);258 }259 260 return 0;261}262 263static void mtk_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy)264{265 struct mtk_cpufreq_data *data = policy->driver_data;266 struct resource *res = data->res;267 void __iomem *base = data->base;268 269 /* HW should be in paused state now */270 writel_relaxed(0x0, data->reg_bases[REG_FREQ_ENABLE]);271 iounmap(base);272 release_mem_region(res->start, resource_size(res));273}274 275static void mtk_cpufreq_register_em(struct cpufreq_policy *policy)276{277 struct em_data_callback em_cb = EM_DATA_CB(mtk_cpufreq_get_cpu_power);278 struct mtk_cpufreq_data *data = policy->driver_data;279 280 em_dev_register_perf_domain(get_cpu_device(policy->cpu), data->nr_opp,281 &em_cb, policy->cpus, true);282}283 284static struct cpufreq_driver cpufreq_mtk_hw_driver = {285 .flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK |286 CPUFREQ_HAVE_GOVERNOR_PER_POLICY |287 CPUFREQ_IS_COOLING_DEV,288 .verify = cpufreq_generic_frequency_table_verify,289 .target_index = mtk_cpufreq_hw_target_index,290 .get = mtk_cpufreq_hw_get,291 .init = mtk_cpufreq_hw_cpu_init,292 .exit = mtk_cpufreq_hw_cpu_exit,293 .register_em = mtk_cpufreq_register_em,294 .fast_switch = mtk_cpufreq_hw_fast_switch,295 .name = "mtk-cpufreq-hw",296 .attr = cpufreq_generic_attr,297};298 299static int mtk_cpufreq_hw_driver_probe(struct platform_device *pdev)300{301 const void *data;302 int ret, cpu;303 struct device *cpu_dev;304 struct regulator *cpu_reg;305 306 /* Make sure that all CPU supplies are available before proceeding. */307 for_each_possible_cpu(cpu) {308 cpu_dev = get_cpu_device(cpu);309 if (!cpu_dev)310 return dev_err_probe(&pdev->dev, -EPROBE_DEFER,311 "Failed to get cpu%d device\n", cpu);312 313 cpu_reg = devm_regulator_get(cpu_dev, "cpu");314 if (IS_ERR(cpu_reg))315 return dev_err_probe(&pdev->dev, PTR_ERR(cpu_reg),316 "CPU%d regulator get failed\n", cpu);317 }318 319 320 data = of_device_get_match_data(&pdev->dev);321 if (!data)322 return -EINVAL;323 324 platform_set_drvdata(pdev, (void *) data);325 cpufreq_mtk_hw_driver.driver_data = pdev;326 327 ret = cpufreq_register_driver(&cpufreq_mtk_hw_driver);328 if (ret)329 dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n");330 331 return ret;332}333 334static void mtk_cpufreq_hw_driver_remove(struct platform_device *pdev)335{336 cpufreq_unregister_driver(&cpufreq_mtk_hw_driver);337}338 339static const struct of_device_id mtk_cpufreq_hw_match[] = {340 { .compatible = "mediatek,cpufreq-hw", .data = &cpufreq_mtk_offsets },341 {}342};343MODULE_DEVICE_TABLE(of, mtk_cpufreq_hw_match);344 345static struct platform_driver mtk_cpufreq_hw_driver = {346 .probe = mtk_cpufreq_hw_driver_probe,347 .remove_new = mtk_cpufreq_hw_driver_remove,348 .driver = {349 .name = "mtk-cpufreq-hw",350 .of_match_table = mtk_cpufreq_hw_match,351 },352};353module_platform_driver(mtk_cpufreq_hw_driver);354 355MODULE_AUTHOR("Hector Yuan <hector.yuan@mediatek.com>");356MODULE_DESCRIPTION("Mediatek cpufreq-hw driver");357MODULE_LICENSE("GPL v2");358