diff -drupN a/drivers/cpufreq/sunxi-cpufreq.c b/drivers/cpufreq/sunxi-cpufreq.c --- a/drivers/cpufreq/sunxi-cpufreq.c 1970-01-01 03:00:00.000000000 +0300 +++ b/drivers/cpufreq/sunxi-cpufreq.c 2022-06-12 05:28:14.000000000 +0300 @@ -0,0 +1,735 @@ +/* + * drivers/cpufreq/sunxi-cpufreq.c + * + * Copyright (c) 2014 softwinner. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef CONFIG_SUNXI_ARISC +#include +#endif + +#define CPUFREQ_DBG(format, args...) \ + pr_debug("[cpu_freq] DBG: "format, ##args) +#define CPUFREQ_ERR(format, args...) \ + pr_err("[cpu_freq] ERR: "format, ##args) + +#ifdef CONFIG_DEBUG_FS +/* sunxi CPUFreq driver data structure */ +static struct { + s64 cpufreq_set_us; + s64 cpufreq_get_us; +} sunxi_cpufreq; +#endif + +static struct thermal_cooling_device *cdev; + +/* cpufreq_dvfs_table is global default dvfs table */ +static struct cpufreq_dvfs_table cpufreq_dvfs_table[DVFS_VF_TABLE_MAX] = { +#ifdef CONFIG_ARM_SUNXI_AVS + /* freq voltage axi_div pval*/ + {900000000, 1200, 3, 1300}, + {600000000, 1200, 3, 1300}, + {420000000, 1200, 3, 1300}, + {360000000, 1200, 3, 1300}, + {300000000, 1200, 3, 1300}, + {240000000, 1200, 3, 1300}, + {120000000, 1200, 3, 1300}, + {60000000, 1200, 3, 1300}, + {0, 1200, 3, 1300}, + {0, 1200, 3, 1300}, + {0, 1200, 3, 1300}, + {0, 1200, 3, 1300}, + {0, 1200, 3, 1300}, + {0, 1200, 3, 1300}, + {0, 1200, 3, 1300}, + {0, 1200, 3, 1300}, +#else + /* + * cluster0 + * cpu0 vdd is 1.20v if cpu freq is (600Mhz, 1008Mhz] + * cpu0 vdd is 1.20v if cpu freq is (420Mhz, 600Mhz] + * cpu0 vdd is 1.20v if cpu freq is (360Mhz, 420Mhz] + * cpu0 vdd is 1.20v if cpu freq is (300Mhz, 360Mhz] + * cpu0 vdd is 1.20v if cpu freq is (240Mhz, 300Mhz] + * cpu0 vdd is 1.20v if cpu freq is (120Mhz, 240Mhz] + * cpu0 vdd is 1.20v if cpu freq is (60Mhz, 120Mhz] + * cpu0 vdd is 1.20v if cpu freq is (0Mhz, 60Mhz] + */ + /* freq voltage axi_div */ + {900000000, 1200, 3}, + {600000000, 1200, 3}, + {420000000, 1200, 3}, + {360000000, 1200, 3}, + {300000000, 1200, 3}, + {240000000, 1200, 3}, + {120000000, 1200, 3}, + {60000000, 1200, 3}, + {0, 1200, 3}, + {0, 1200, 3}, + {0, 1200, 3}, + {0, 1200, 3}, + {0, 1200, 3}, + {0, 1200, 3}, + {0, 1200, 3}, + {0, 1200, 3} +#endif +}; + +#ifdef CONFIG_ARM_SUNXI_PSENSOR_BIN +struct psensor_range { + unsigned int min; + unsigned int max; +}; + +static struct psensor_range psensor_range[8]; +static int psensor_range_count; +#ifdef CONFIG_ARCH_SUN8IW17P1 +#define PSENSOR_REG 0x03006224 +#else +#define PSENSOR_REG 0x0300621c +#endif + +#endif + +#ifdef CONFIG_SCHED_SMP_DCMP +#ifdef CONFIG_ARCH_SUN8IW17P1 +#define CLUSTER_MAX_CORES 3 +struct clk *cluster1_clk; +#endif +#endif + +extern unsigned long dev_pm_opp_axi_bus_divide_ratio(struct dev_pm_opp *opp); +extern int dev_pm_opp_of_get_sharing_cpus_by_soc_bin(struct device *cpu_dev, + cpumask_var_t cpumask, int soc_bin); +extern int dev_pm_opp_of_cpumask_add_table_by_soc_bin(cpumask_var_t cpumask, + int soc_bin); + + +static unsigned int sunxi_cpufreq_get(unsigned int cpu) +{ + unsigned int current_freq = 0; +#ifdef CONFIG_DEBUG_FS + ktime_t calltime = ktime_get(); +#endif + + current_freq = cpufreq_generic_get(cpu); + +#ifdef CONFIG_DEBUG_FS + sunxi_cpufreq.cpufreq_get_us = + ktime_to_us(ktime_sub(ktime_get(), calltime)); +#endif + + return current_freq; +} + +#ifdef CONFIG_SUNXI_ARISC +static int sunxi_set_cpufreq_and_voltage(struct cpufreq_policy *policy, + unsigned long freq) +{ + int ret = 0; + +#ifdef CONFIG_SCHED_SMP_DCMP + int cpu; +#endif + +#ifdef CONFIG_SUNXI_CPUFREQ_ASYN + unsigned long timeout; + + arisc_dvfs_set_cpufreq(freq, ARISC_DVFS_PLL1, ARISC_DVFS_ASYN, + NULL, NULL); + /* CPUS max latency for cpu freq*/ + timeout = 15; + +#ifdef CONFIG_SCHED_SMP_DCMP + for_each_online_cpu(cpu) { + if (cpu >= CLUSTER_MAX_CORES) { + // CPUFREQ_DBG("Set other cluster freq: %d\n", freq); + arisc_dvfs_set_cpufreq(freq, ARISC_DVFS_PLL2, + ARISC_DVFS_ASYN, NULL, NULL); + break; + } + } + while (timeout-- && (clk_get_rate(policy->clk) != freq*1000) && + (clk_get_rate(policy->clk) != freq*1000)) + usleep_range(900, 1000); + if ((clk_get_rate(policy->clk) != freq*1000) || (clk_get_rate(policy->clk) != freq*1000)) + ret = -1; +#else + while (timeout-- && (clk_get_rate(policy->clk) != freq*1000)) + msleep(1); + if (clk_get_rate(policy->clk) != freq*1000) + ret = -1; +#endif + +#else + + ret = arisc_dvfs_set_cpufreq(freq, ARISC_DVFS_PLL1, ARISC_DVFS_SYN, + NULL, NULL); +#ifdef CONFIG_SCHED_SMP_DCMP + if (ret) + return ret; + for_each_online_cpu(cpu) { + if (cpu >= CLUSTER_MAX_CORES) { + // CPUFREQ_DBG("Set other cluster freq: %d\n", freq); + ret = arisc_dvfs_set_cpufreq(freq, ARISC_DVFS_PLL2, ARISC_DVFS_SYN, + NULL, NULL); + break; + } + } +#endif + +#endif + return ret; +} +#else +static int sunxi_set_cpufreq_and_voltage(struct cpufreq_policy *policy, + unsigned long freq) +{ + struct device *cpu_dev; + + cpu_dev = get_cpu_device(policy->cpu); + + + return dev_pm_opp_set_rate(cpu_dev, freq * 1000); +} +#endif + +static int sunxi_cpufreq_target_index(struct cpufreq_policy *policy, + unsigned int index) +{ + int ret = 0; + unsigned long freq; +#ifdef CONFIG_DEBUG_FS + ktime_t calltime; +#endif + + freq = policy->freq_table[index].frequency; + +#ifdef CONFIG_DEBUG_FS + calltime = ktime_get(); +#endif + /* try to set cpu frequency */ + ret = sunxi_set_cpufreq_and_voltage(policy, freq); + if (ret) + CPUFREQ_ERR("Set cpu frequency to %luKHz failed!\n", freq); + +#ifdef CONFIG_DEBUG_FS + sunxi_cpufreq.cpufreq_set_us = ktime_to_us(ktime_sub(ktime_get(), + calltime)); +#endif + + return ret; +} +static int sunxi_cpufreq_set_vf(struct cpufreq_frequency_table *table, + unsigned int cpu) +{ + struct cpufreq_frequency_table *pos; + struct dev_pm_opp *opp; + struct device *dev; + unsigned long freq; + int ret, num = 0; + int max_opp_num = 0; + void *kvir = NULL; + + dev = get_cpu_device(cpu); + max_opp_num = dev_pm_opp_get_opp_count(dev); + +#ifndef CONFIG_ARCH_SUN8IW12P1 + for (pos = table; max_opp_num > 0; --max_opp_num) { + freq = pos[max_opp_num - 1].frequency * 1000; +#else + cpufreq_for_each_valid_entry(pos, table) { + freq = pos->frequency * 1000; +#endif + CPUFREQ_DBG("freq: %lu\n", freq); + rcu_read_lock(); + opp = dev_pm_opp_find_freq_ceil(dev, &freq); + if (IS_ERR(opp)) { + ret = PTR_ERR(opp); + dev_err(dev, + "%s: failed to find OPP for freq %lu (%d)\n", + __func__, freq, ret); + rcu_read_unlock(); + return ret; + } + + cpufreq_dvfs_table[num].voltage = + dev_pm_opp_get_voltage(opp) / 1000; + cpufreq_dvfs_table[num].freq = dev_pm_opp_get_freq(opp); + cpufreq_dvfs_table[num].axi_div = + dev_pm_opp_axi_bus_divide_ratio(opp); +#ifdef CONFIG_ARM_SUNXI_AVS + cpufreq_dvfs_table[num].pval = + dev_pm_opp_get_pval(opp); +#endif + rcu_read_unlock(); + CPUFREQ_DBG("num:%d, volatge:%d, freq:%d, axi_div:%d ,%s\n", + num, cpufreq_dvfs_table[num].voltage, + cpufreq_dvfs_table[num].freq, + cpufreq_dvfs_table[num].axi_div, __func__); + + num++; + } + + kvir = + kmalloc(num * sizeof(struct cpufreq_dvfs_table), GFP_KERNEL); + if (kvir == NULL) { + CPUFREQ_ERR("kmalloc error for transmiting vf table\n"); + return -1; + } + memcpy((void *)kvir, (void *)cpufreq_dvfs_table, + num * sizeof(struct cpufreq_dvfs_table)); + __dma_flush_area((void *)kvir, num * sizeof(struct cpufreq_dvfs_table)); + +#ifdef CONFIG_SUNXI_ARISC + arisc_dvfs_cfg_vf_table(0, num, virt_to_phys(kvir)); +#endif + + kfree(kvir); + kvir = NULL; + + return 0; +} + +#ifdef CONFIG_ARM_SUNXI_PSENSOR_BIN +static int get_psensor_range(void) +{ + int i = 0; + char name[20]; + int psensor_count = 0; + struct device_node *psensor_node = NULL; + + psensor_node = of_find_node_by_path("/psensor_table"); + if (psensor_node == NULL) { + pr_err("%s(%d) has no pensor node\n", __func__, __LINE__); + return -1; + } + + if (of_property_read_u32(psensor_node, "psensor_count", + &psensor_count)) { + pr_err("%s(%d) get psensor count error\n", __func__, __LINE__); + return -1; + } + psensor_range_count = psensor_count; + for (i = 0; i < psensor_count; i++) { + sprintf(name, "prange_min_%d", i); + if (of_property_read_u32(psensor_node, name, + &psensor_range[i].min)) { + pr_err("get prange_min_%d failed\n", i); + return -1; + } + + sprintf(name, "prange_max_%d", i); + if (of_property_read_u32(psensor_node, name, + &psensor_range[i].max)) { + pr_err("get prange_max_%d failed\n", i); + return -1; + } + } + + for (i = 0; i < psensor_count; ++i) { + pr_debug("psensor_range_%d.min=%d; psensor_range_%d.max=%d\n", + i, psensor_range[i].min, i, psensor_range[i].max); + } + return 0; +} + +static int get_psensor_bin(void) +{ + unsigned int soc_bin; + void __iomem *bin_reg = NULL; + int i; + + bin_reg = ioremap(PSENSOR_REG, 16); + + soc_bin = readl(bin_reg); + +#ifdef CONFIG_ARCH_SUN8IW17P1 + soc_bin &= 0xFFFF; +#else + /*use the high 16 bit*/ + soc_bin >>= 16; +#endif + iounmap(bin_reg); + + for (i = 0; i < psensor_range_count; ++i) { + if (soc_bin >= psensor_range[i].min && soc_bin <= + psensor_range[i].max) + break; + } + + if (i >= psensor_range_count) + return -1; + + return i; +} +#endif + +static int sunxi_cpufreq_init(struct cpufreq_policy *policy) +{ + struct device *cpu_dev; + struct cpufreq_frequency_table *freq_table; + struct dev_pm_opp *suspend_opp; + struct clk *cpu_clk; + const char *regulator_name; + struct opp_table *table; + unsigned int transition_latency; + int ret, soc_bin; + unsigned int table_count; + struct device_node *dvfs_main_np; +#ifdef CONFIG_SCHED_SMP_DCMP + struct device *cluster1_cpu_dev; +#endif + + dvfs_main_np = of_find_node_by_path("/opp_dvfs_table"); + if (!dvfs_main_np) { + CPUFREQ_ERR("No opp dvfs table node found\n"); + return -ENODEV; + } + +#ifdef CONFIG_ARM_SUNXI_PSENSOR_BIN + ret = get_psensor_range(); + if (ret < 0) + return -EINVAL; +#endif + if (of_property_read_u32(dvfs_main_np, "opp_table_count", + &table_count)) { + CPUFREQ_ERR("get vf_table_count failed\n"); + return -EINVAL; + } + + if (table_count == 1) { + pr_info("%s: only one opp_table\n", __func__); + soc_bin = 0; + } else { +#ifdef CONFIG_ARM_SUNXI_PSENSOR_BIN + soc_bin = get_psensor_bin(); +#else + soc_bin = sunxi_get_soc_bin(); + if (soc_bin == 0) + soc_bin = 2; + + soc_bin--; +#endif + if (soc_bin < 0) { + pr_err("%s: get the wrong soc bin!\n", __func__); + return -EINVAL; + } + pr_info("%s: support more opp_table and soc bin is %d\n", + __func__, soc_bin); + } + + cpu_dev = get_cpu_device(policy->cpu); + CPUFREQ_ERR("DEBUG: get cpu %d device\n", policy->cpu); + if (!cpu_dev) { + CPUFREQ_ERR("Failed to get cpu%d device\n", policy->cpu); + return -ENODEV; + } + + cpu_clk = clk_get(cpu_dev, NULL); + if (IS_ERR_OR_NULL(cpu_clk)) { + ret = PTR_ERR(cpu_clk); + CPUFREQ_ERR("Unable to get PLL CPU clock\n"); + return ret; + } + policy->clk = cpu_clk; + +#ifdef CONFIG_SCHED_SMP_DCMP + cluster1_cpu_dev = get_cpu_device(CLUSTER_MAX_CORES); + if (!cluster1_cpu_dev) { + CPUFREQ_ERR("Failed to get other cluster cpu%d device\n", policy->cpu); + return -ENODEV; + } + cluster1_clk = clk_get(cluster1_cpu_dev, NULL); + if (IS_ERR_OR_NULL(cluster1_clk)) { + ret = PTR_ERR(cluster1_clk); + CPUFREQ_ERR("Unable to get OTHER cluster PLL CPU clock\n"); + return ret; + } +#endif + regulator_name = of_get_property(cpu_dev->of_node, "regulators", NULL); + if (!regulator_name) { + CPUFREQ_ERR("Unable to get regulator\n"); + goto lable_1; + } + + table = dev_pm_opp_set_regulator(cpu_dev, regulator_name); + if (!table) { + CPUFREQ_ERR("Failed to set regulator for cpu\n"); + goto out_err_clk_pll; + } + +lable_1: + /* set policy->cpus according to operating-points-v2 */ + ret = dev_pm_opp_of_get_sharing_cpus_by_soc_bin(cpu_dev, + policy->cpus, soc_bin); + if (ret) { + CPUFREQ_ERR("OPP-v2 opp-shared Error\n"); + goto out_err_clk_pll; + } + + ret = dev_pm_opp_of_cpumask_add_table_by_soc_bin(policy->cpus, + soc_bin); + if (ret) { + CPUFREQ_ERR("Failed to add opp table\n"); + goto out_err_clk_pll; + } + + ret = dev_pm_opp_init_cpufreq_table(cpu_dev, &freq_table); + if (ret) { + CPUFREQ_ERR("Failed to init cpufreq table: %d\n", ret); + goto out_err_free_opp; + } + + ret = cpufreq_table_validate_and_show(policy, freq_table); + if (ret) { + CPUFREQ_ERR("Invalid frequency table: %d\n", ret); + goto out_err_free_opp; + } + + transition_latency = dev_pm_opp_get_max_transition_latency(cpu_dev); + if (!transition_latency) + transition_latency = CPUFREQ_ETERNAL; + policy->cpuinfo.transition_latency = transition_latency; + + rcu_read_lock(); + suspend_opp = dev_pm_opp_get_suspend_opp(cpu_dev); + if (suspend_opp) + policy->suspend_freq = dev_pm_opp_get_freq(suspend_opp) / 1000; + rcu_read_unlock(); + + ret = sunxi_cpufreq_set_vf(freq_table, policy->cpu); + if (ret) { + CPUFREQ_ERR("sunxi_cpufreq_set_vf failed: %d\n", ret); + goto out_err_free_opp; + } + + return 0; + +out_err_free_opp: + dev_pm_opp_of_cpumask_remove_table(policy->cpus); +out_err_clk_pll: + clk_put(cpu_clk); + + return ret; +} + +static int sunxi_cpufreq_exit(struct cpufreq_policy *policy) +{ + struct device *cpu_dev; + + cpu_dev = get_cpu_device(policy->cpu); + + dev_pm_opp_free_cpufreq_table(cpu_dev, &policy->freq_table); + + dev_pm_opp_of_cpumask_remove_table(policy->related_cpus); + + //dev_pm_opp_put_regulator(cpu_dev); + + clk_put(policy->clk); + + return 0; +} + +static void sunxi_cpufreq_ready(struct cpufreq_policy *policy) +{ + struct device *cpu_dev = get_cpu_device(policy->cpu); + struct device_node *np; + + np = of_node_get(cpu_dev->of_node); + if (WARN_ON(!np)) + return; + + if (of_find_property(np, "#cooling-cells", NULL)) { + u32 power_coefficient = 0; + + of_property_read_u32(np, "dynamic-power-coefficient", + &power_coefficient); + + cdev = of_cpufreq_power_cooling_register(np, + policy->related_cpus, power_coefficient, NULL); + if (IS_ERR(cdev)) { + dev_err(cpu_dev, + "running cpufreq without cooling device: %ld\n", + PTR_ERR(cdev)); + cdev = NULL; + } + } + of_node_put(np); +} + +static struct freq_attr *sunxi_cpufreq_attr[] = { + &cpufreq_freq_attr_scaling_available_freqs, + NULL, +}; + +static struct cpufreq_driver sunxi_cpufreq_driver = { + .name = "cpufreq-sunxi", + .flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK, + .attr = sunxi_cpufreq_attr, + .init = sunxi_cpufreq_init, + .get = sunxi_cpufreq_get, + .target_index = sunxi_cpufreq_target_index, + .exit = sunxi_cpufreq_exit, + .ready = sunxi_cpufreq_ready, + .verify = cpufreq_generic_frequency_table_verify, + .suspend = cpufreq_generic_suspend, +}; + +#ifdef CONFIG_ARCH_SUN8IW7P1 +static int set_pll_cpu_lock_time(void) +{ + unsigned int value; + void __iomem *lock_time_vbase = NULL; +#define PLL_CPU_LOCK_TIME_REG (0x01c20000 + 0x204) + + lock_time_vbase = ioremap(PLL_CPU_LOCK_TIME_REG, 4); + if (lock_time_vbase == NULL) { + pr_err("ioremap pll cpu lock time error\n"); + return -1; + } + + value = readl(lock_time_vbase); + value &= ~(0xffff); + value |= 0x400; + writel(value, lock_time_vbase); + + iounmap(lock_time_vbase); + return 0; +} +#endif + +static int __init sunxi_cpufreq_initcall(void) +{ + int ret; + +#ifdef CONFIG_DEBUG_FS + sunxi_cpufreq.cpufreq_set_us = 0; + sunxi_cpufreq.cpufreq_get_us = 0; +#endif + +#ifdef CONFIG_ARCH_SUN8IW7P1 + if (set_pll_cpu_lock_time()) + return -1; +#endif + ret = cpufreq_register_driver(&sunxi_cpufreq_driver); + if (ret) + CPUFREQ_ERR("Failed register driver\n"); + + return ret; +} + +static void __exit sunxi_cpufreq_exitcall(void) +{ + cpufreq_unregister_driver(&sunxi_cpufreq_driver); +} + +module_init(sunxi_cpufreq_initcall); +module_exit(sunxi_cpufreq_exitcall); + +#ifdef CONFIG_DEBUG_FS +#include + +static struct dentry *debugfs_cpufreq_root; + +static int cpufreq_debugfs_gettime_show(struct seq_file *s, void *data) +{ + seq_printf(s, "%lld\n", sunxi_cpufreq.cpufreq_get_us); + return 0; +} + +static int cpufreq_debugfs_gettime_open(struct inode *inode, struct file *file) +{ + return single_open(file, cpufreq_debugfs_gettime_show, + inode->i_private); +} + +static const struct file_operations cpufreq_debugfs_gettime_fops = { + .open = cpufreq_debugfs_gettime_open, + .read = seq_read, +}; + +static int cpufreq_debugfs_settime_show(struct seq_file *s, void *data) +{ + seq_printf(s, "%lld\n", sunxi_cpufreq.cpufreq_set_us); + return 0; +} + +static int cpufreq_debugfs_settime_open(struct inode *inode, struct file *file) +{ + return single_open(file, cpufreq_debugfs_settime_show, + inode->i_private); +} + +static const struct file_operations cpufreq_debugfs_settime_fops = { + .open = cpufreq_debugfs_settime_open, + .read = seq_read, +}; + +static int __init cpufreq_debugfs_init(void) +{ + int err = 0; + + debugfs_cpufreq_root = debugfs_create_dir("cpufreq", 0); + if (!debugfs_cpufreq_root) + return -ENOMEM; + + if (!debugfs_create_file("get_time", 0444, debugfs_cpufreq_root, NULL, + &cpufreq_debugfs_gettime_fops)) { + err = -ENOMEM; + goto out; + } + + if (!debugfs_create_file("set_time", 0444, debugfs_cpufreq_root, NULL, + &cpufreq_debugfs_settime_fops)) { + err = -ENOMEM; + goto out; + } + + return 0; + +out: + debugfs_remove_recursive(debugfs_cpufreq_root); + return err; +} + +static void __exit cpufreq_debugfs_exit(void) +{ + debugfs_remove_recursive(debugfs_cpufreq_root); +} + +late_initcall(cpufreq_debugfs_init); +module_exit(cpufreq_debugfs_exit); + +#endif /* CONFIG_DEBUG_FS */ + +MODULE_DESCRIPTION("cpufreq driver for sunxi SOCs"); +MODULE_LICENSE("GPL");