| /* |
| * Gem5 Energy Controller support |
| * (code adapted from vexpress-spc) |
| * |
| * Copyright (C) 2013-2014 ARM Ltd. |
| * |
| * Authors: Akash Bagdia <akash.bagdia@arm.com> |
| * Vasileios Spiliopoulos <vasileios.spiliopoulos@arm.com> |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License version 2 as |
| * published by the Free Software Foundation. |
| * |
| * This program is distributed "as is" WITHOUT ANY WARRANTY of any |
| * kind, whether express or implied; without even the implied warranty |
| * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| * GNU General Public License for more details. |
| */ |
| |
| #include <linux/device.h> |
| #include <linux/delay.h> |
| #include <linux/err.h> |
| #include <linux/io.h> |
| #include <linux/module.h> |
| #include <linux/of_address.h> |
| #include <linux/of_irq.h> |
| #include <linux/slab.h> |
| #include <linux/spinlock.h> |
| #include <linux/gem5_energy_ctrl.h> |
| |
| // Register addresses |
| #define DVFS_HANDLER_STATUS 0x00 |
| #define DVFS_NUM_DOMAINS 0x04 |
| #define DVFS_DOMAINID_AT_INDEX 0x08 |
| #define DVFS_HANDLER_TRANS_LATENCY 0x0C |
| #define DOMAIN_ID 0x10 |
| #define PERF_LEVEL 0x14 |
| #define PERF_LEVEL_ACK 0x18 |
| #define NUM_OF_PERF_LEVELS 0x1C |
| #define PERF_LEVEL_TO_READ 0x20 |
| #define FREQ_AT_PERF_LEVEL 0x24 |
| #define VOLT_AT_PERF_LEVEL 0x28 |
| |
| #define TIME_OUT 100 |
| #define GEM5_MAX_NUM_DOMAINS 32 |
| |
| struct gem5_energy_ctrl_drvdata { |
| void __iomem *baseaddr; |
| bool dvfs_handler_status; |
| u32 num_gem5_domains; |
| u32 *freqs[GEM5_MAX_NUM_DOMAINS]; |
| u32 *voltages[GEM5_MAX_NUM_DOMAINS]; |
| int opp_cnt[GEM5_MAX_NUM_DOMAINS]; |
| u32 domain_ids[GEM5_MAX_NUM_DOMAINS]; |
| spinlock_t lock; |
| }; |
| |
| static struct gem5_energy_ctrl_drvdata *info; |
| |
| static int gem5_energy_ctrl_load_result = -EAGAIN; |
| |
| static bool gem5_energy_ctrl_initialized(void) |
| { |
| return gem5_energy_ctrl_load_result == 0; |
| } |
| |
| bool gem5_energy_ctrl_dvfs_enabled(void) |
| { |
| return info->dvfs_handler_status; |
| } |
| EXPORT_SYMBOL_GPL(gem5_energy_ctrl_dvfs_enabled); |
| |
| static u32 index_of_domain_id(u32 domain_id) |
| { |
| u32 i; |
| |
| for(i = 0; i < info->num_gem5_domains; i++) |
| if(domain_id == info->domain_ids[i]) |
| return i; |
| return GEM5_MAX_NUM_DOMAINS; |
| } |
| |
| u32 gem5_energy_ctrl_get_trans_latency(void) |
| { |
| u32 data; |
| |
| if (!gem5_energy_ctrl_initialized() || !info->dvfs_handler_status) |
| return -EINVAL; |
| |
| spin_lock(&info->lock); |
| data = readl(info->baseaddr + DVFS_HANDLER_TRANS_LATENCY); |
| spin_unlock(&info->lock); |
| |
| return data; |
| } |
| EXPORT_SYMBOL_GPL(gem5_energy_ctrl_get_trans_latency); |
| |
| /** |
| * gem5_energy_ctrl_get_performance - get current performance level of domain |
| * @domain_id: mpidr[15:8] bitfield describing domain's affinity level |
| * @freq: pointer to the performance level to be assigned |
| * |
| * Return: 0 on success |
| * < 0 on read error |
| */ |
| int gem5_energy_ctrl_get_performance(u32 domain_id, u32 *freq) |
| { |
| int perf; |
| u32 domain_index; |
| |
| if (!gem5_energy_ctrl_initialized() || !info->dvfs_handler_status) |
| return -EINVAL; |
| |
| domain_index = index_of_domain_id(domain_id); |
| if(domain_index >= info->num_gem5_domains) |
| return -EINVAL; |
| |
| spin_lock(&info->lock); |
| writel(domain_id, info->baseaddr + DOMAIN_ID); |
| perf = readl(info->baseaddr + PERF_LEVEL); |
| spin_unlock(&info->lock); |
| *freq = info->freqs[domain_index][perf]; |
| |
| return 0; |
| } |
| EXPORT_SYMBOL_GPL(gem5_energy_ctrl_get_performance); |
| |
| static int gem5_energy_ctrl_find_perf_index(u32 domain_index, u32 freq) |
| { |
| int idx; |
| |
| for (idx = 0; idx < info->opp_cnt[domain_index]; idx++) |
| if (info->freqs[domain_index][idx] == freq) |
| return idx; |
| |
| return -EINVAL; |
| } |
| |
| |
| static inline int read_wait_to(void __iomem *reg, int status, int timeout) |
| { |
| while (timeout-- && readl(reg) != status) { |
| cpu_relax(); |
| udelay(2); |
| } |
| if (!timeout) |
| return -EAGAIN; |
| else |
| return 0; |
| } |
| |
| /** |
| * gem5_energy_ctrl_set_performance - set current performance level of domain |
| * |
| * @domain_id: mpidr[15:8] bitfield describing domain's affinity level |
| * @freq: performance level to be programmed |
| * |
| * Returns: 0 on success |
| * < 0 on write error |
| */ |
| int gem5_energy_ctrl_set_performance(u32 domain_id, u32 freq) |
| { |
| int ret, perf; |
| u32 domain_index; |
| |
| ret = 0; |
| |
| if (!gem5_energy_ctrl_initialized() || !info->dvfs_handler_status) |
| return -EINVAL; |
| |
| domain_index = index_of_domain_id(domain_id); |
| if(domain_index >= info->num_gem5_domains) |
| return -EINVAL; |
| |
| spin_lock(&info->lock); |
| |
| writel(domain_id, info->baseaddr + DOMAIN_ID); |
| |
| perf = gem5_energy_ctrl_find_perf_index(domain_index, freq); |
| |
| if (perf < 0) |
| return -EINVAL; |
| |
| writel(perf, info->baseaddr + PERF_LEVEL); |
| |
| //Some logic to determine successful setting of perf level |
| if (read_wait_to(info->baseaddr + PERF_LEVEL_ACK, 1, TIME_OUT)) |
| ret = -EAGAIN; |
| |
| spin_unlock(&info->lock); |
| |
| return ret; |
| } |
| EXPORT_SYMBOL_GPL(gem5_energy_ctrl_set_performance); |
| |
| /** |
| * gem5_energy_ctrl_populate_opps() - initialize opp tables from energy ctrl |
| * |
| * @domain_id: mpidr[15:8] bitfield describing domain's affinity level |
| * |
| * Return: 0 on success |
| * < 0 on error |
| */ |
| static int gem5_energy_ctrl_populate_opps(u32 domain_id) |
| { |
| u32 data = 0, i; |
| u32 domain_index = index_of_domain_id(domain_id); |
| |
| if (!info->dvfs_handler_status || |
| WARN_ON_ONCE(domain_index >= info->num_gem5_domains)) |
| return -EINVAL; |
| |
| spin_lock(&info->lock); |
| |
| writel(domain_id, info->baseaddr + DOMAIN_ID); |
| |
| if (readl(info->baseaddr + DOMAIN_ID) != domain_id) { |
| spin_unlock(&info->lock); |
| return -EINVAL; |
| } |
| |
| data = readl(info->baseaddr + NUM_OF_PERF_LEVELS); |
| info->opp_cnt[domain_index] = data; |
| |
| info->freqs[domain_index] = kzalloc(sizeof(u32) * |
| info->opp_cnt[domain_index], GFP_KERNEL); |
| |
| info->voltages[domain_index] = kzalloc(sizeof(u32) * |
| info->opp_cnt[domain_index], GFP_KERNEL); |
| |
| for (i = 0; i < info->opp_cnt[domain_index] ; i++) { |
| writel(i, info->baseaddr + PERF_LEVEL_TO_READ); |
| data = readl(info->baseaddr + FREQ_AT_PERF_LEVEL); |
| info->freqs[domain_index][i] = data; |
| data = readl(info->baseaddr + VOLT_AT_PERF_LEVEL); |
| info->voltages[domain_index][i] = data; |
| } |
| |
| spin_unlock(&info->lock); |
| |
| return 0; |
| } |
| |
| /** |
| * gem5_energy_ctrl_get_opp_table() - Retrieve a pointer to the frequency, |
| * voltage tables for a given domain |
| * |
| * @domain_id: mpidr[15:8] bitfield describing domain's affinity level |
| * @fptr: pointer to be initialized |
| * Return: operating points count on success |
| * -EINVAL on pointer error |
| */ |
| int gem5_energy_ctrl_get_opp_table(u32 domain_id, u32 **fptr, u32 **vptr) |
| { |
| u32 domain_index; |
| |
| if (!gem5_energy_ctrl_initialized() || !fptr || !vptr || !info || |
| !info->dvfs_handler_status) |
| return -EINVAL; |
| |
| domain_index = index_of_domain_id(domain_id); |
| if(domain_index >= info->num_gem5_domains) |
| return -EINVAL; |
| |
| *fptr = info->freqs[domain_index]; |
| *vptr = info->voltages[domain_index]; |
| return info->opp_cnt[domain_index]; |
| } |
| EXPORT_SYMBOL_GPL(gem5_energy_ctrl_get_opp_table); |
| |
| static const struct of_device_id gem5_energy_ctrl_ids[] __initconst = { |
| { .compatible = "arm,gem5-energy-ctrl" }, |
| {}, |
| }; |
| |
| static int __init gem5_energy_ctrl_init(void) |
| { |
| int ret; |
| u32 i, data; |
| struct device_node *node = of_find_matching_node(NULL, |
| gem5_energy_ctrl_ids); |
| |
| if (!node) |
| return -ENODEV; |
| |
| info = kzalloc(sizeof(*info), GFP_KERNEL); |
| if (!info) { |
| pr_err("%s: unable to allocate mem\n", __func__); |
| return -ENOMEM; |
| } |
| |
| info->dvfs_handler_status = 0; |
| info->num_gem5_domains = 0; |
| |
| info->baseaddr = of_iomap(node, 0); |
| if (WARN_ON(!info->baseaddr)) { |
| ret = -ENXIO; |
| goto mem_free; |
| } |
| |
| spin_lock_init(&info->lock); |
| |
| info->dvfs_handler_status = readl(info->baseaddr + DVFS_HANDLER_STATUS); |
| |
| if (!info->dvfs_handler_status) { |
| pr_info("gem5 DVFS handler is disabled\n"); |
| } else { |
| info->num_gem5_domains = readl(info->baseaddr + |
| DVFS_NUM_DOMAINS); |
| if (info->num_gem5_domains > GEM5_MAX_NUM_DOMAINS) { |
| pr_err("gem5 DVFS handler manages more domains than\ |
| supported by the gem5 energy controller driver\n"); |
| ret = -ENODEV; |
| goto unmap; |
| } |
| else { |
| /* Get domain ID information |
| * Populate operation table for all the domains(clusters) |
| * managed by the controller |
| */ |
| for(i = 0;i < info->num_gem5_domains; i++) { |
| spin_lock(&info->lock); |
| writel(i, info->baseaddr + |
| DVFS_DOMAINID_AT_INDEX); |
| data = readl(info->baseaddr + |
| DVFS_DOMAINID_AT_INDEX); |
| spin_unlock(&info->lock); |
| info->domain_ids[i] = data; |
| if(gem5_energy_ctrl_populate_opps(info->domain_ids[i])) |
| { |
| pr_err("failed to build OPP table for\ |
| %d domain\n", |
| info->domain_ids[i]); |
| ret = -ENODEV; |
| goto unmap; |
| } |
| } |
| } |
| } |
| pr_info("gem5-energy-ctrl loaded at %p\n", info->baseaddr); |
| return 0; |
| |
| unmap: |
| pr_info("gem5-energy-ctrl unmapped at %p, possible error in syncing with "\ |
| "the device\n", info->baseaddr); |
| iounmap(info->baseaddr); |
| |
| mem_free: |
| kfree(info); |
| return ret; |
| } |
| |
| static bool __init __gem5_energy_ctrl_check_loaded(void); |
| /* |
| * Pointer spc_check_loaded is swapped after init hence it is safe |
| * to initialize it to a function in the __init section |
| */ |
| static bool (*energy_ctrl_check_loaded)(void) __refdata = \ |
| &__gem5_energy_ctrl_check_loaded; |
| |
| static bool __init __gem5_energy_ctrl_check_loaded(void) |
| { |
| if (gem5_energy_ctrl_load_result == -EAGAIN) |
| gem5_energy_ctrl_load_result = gem5_energy_ctrl_init(); |
| energy_ctrl_check_loaded = &gem5_energy_ctrl_initialized; |
| return gem5_energy_ctrl_initialized(); |
| } |
| |
| /* |
| * Function exported to manage early_initcall ordering. |
| * SPC code is needed very early in the boot process |
| * to bring CPUs out of reset and initialize power |
| * management back-end. After boot swap pointers to |
| * make the functionality check available to loadable |
| * modules, when early boot init functions have been |
| * already freed from kernel address space. |
| */ |
| bool gem5_energy_ctrl_check_loaded(void) |
| { |
| return energy_ctrl_check_loaded(); |
| } |
| EXPORT_SYMBOL_GPL(gem5_energy_ctrl_check_loaded); |
| |
| //static int __init vexpress_spc_early_init(void) |
| //{ |
| // __vexpress_spc_check_loaded(); |
| // return vexpress_spc_load_result; |
| //} |
| //early_initcall(vexpress_spc_early_init); |
| MODULE_LICENSE("GPL"); |