diff -drupN a/drivers/cpufreq/ingenic-cpufreq.c b/drivers/cpufreq/ingenic-cpufreq.c --- a/drivers/cpufreq/ingenic-cpufreq.c 1970-01-01 03:00:00.000000000 +0300 +++ b/drivers/cpufreq/ingenic-cpufreq.c 2022-06-09 05:02:28.000000000 +0300 @@ -0,0 +1,342 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define PARENT_APLL 1 +#define PARENT_MPLL 2 +#define BIU_FREQ_THRESHOLD 750000000 + +static struct ingenic_cpufreq { + struct device *dev; + unsigned int latency; + struct cpufreq_frequency_table *freq_table_dt; + struct cpufreq_frequency_table *freq_table; + unsigned int freq_count; + struct mutex mutex; + + struct clk *mux; + struct clk *mclk; + struct clk *sclka; + struct clk *clk_cpu_l2c_x1; + struct clk *clk_cpu_l2c_x2; + unsigned int arate; + unsigned int mrate; + + struct regulator *regulator; +}; + +static struct ingenic_cpufreq *ingenic_cpufreq; + + +static unsigned long lpj_ref; +static int ingenic_cpu_freq_notifier(struct notifier_block *nb, unsigned long val, void *data) +{ + struct cpufreq_freqs *freqs = data; + static unsigned int lpj_ref_freq; + + if (val == CPUFREQ_PRECHANGE) { + lpj_ref = loops_per_jiffy; + } + + if (val == CPUFREQ_POSTCHANGE) { + lpj_ref_freq = freqs->old; + loops_per_jiffy = cpufreq_scale(lpj_ref, lpj_ref_freq, freqs->new); + } + + return NOTIFY_OK; +} + +static struct notifier_block ingenic_cpufreq_notifier_block = { + .notifier_call = ingenic_cpu_freq_notifier, +}; + + +static void ingenic_cpufreq_get_freq_table(void) +{ + unsigned int a_rate = ingenic_cpufreq->arate; + unsigned int m_rate = ingenic_cpufreq->mrate; + unsigned int div[16] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + unsigned int count = 0, dt_freq; + int i, j; + + + for (i = 0; i < ingenic_cpufreq->freq_count; i++) { + dt_freq = ingenic_cpufreq->freq_table_dt[i].frequency * 1000; + for (j = 0; j < 16; j++) { + if (dt_freq == a_rate / div[j]) { + ingenic_cpufreq->freq_table[count].frequency = ingenic_cpufreq->freq_table_dt[i].frequency; + ingenic_cpufreq->freq_table[count].driver_data = PARENT_APLL; + ingenic_cpufreq->freq_table[count].flags = ingenic_cpufreq->freq_table_dt[i].flags; + count++; + break; + } + if (dt_freq == m_rate / div[j]) { + ingenic_cpufreq->freq_table[count].frequency = ingenic_cpufreq->freq_table_dt[i].frequency; + ingenic_cpufreq->freq_table[count].driver_data = PARENT_MPLL; + ingenic_cpufreq->freq_table[count].flags = ingenic_cpufreq->freq_table_dt[i].flags; + count++; + break; + } + } + } + ingenic_cpufreq->freq_count = count; + + ingenic_cpufreq->freq_table[count].driver_data = count; + ingenic_cpufreq->freq_table[count].frequency = CPUFREQ_TABLE_END; + +} + +static int ingenic_cpufreq_init(struct cpufreq_policy *policy) +{ + ingenic_cpufreq->mclk = clk_get(NULL, "mpll"); + ingenic_cpufreq->sclka = clk_get(NULL, "sclka"); + ingenic_cpufreq->mux = clk_get(NULL, "mux_cpu_l2c"); + ingenic_cpufreq->clk_cpu_l2c_x1 = clk_get(NULL, "div_cpu_l2c_x1"); + ingenic_cpufreq->clk_cpu_l2c_x2 = clk_get(NULL, "div_cpu_l2c_x2"); + + ingenic_cpufreq->arate = clk_get_rate(ingenic_cpufreq->sclka); + ingenic_cpufreq->mrate = clk_get_rate(ingenic_cpufreq->mclk); + + ingenic_cpufreq_get_freq_table(); + + if (ingenic_cpufreq->arate > BIU_FREQ_THRESHOLD) + policy->clk = ingenic_cpufreq->clk_cpu_l2c_x2; + else + policy->clk = ingenic_cpufreq->clk_cpu_l2c_x1; + + ingenic_cpufreq->latency = 4000; + + return cpufreq_generic_init(policy, ingenic_cpufreq->freq_table, ingenic_cpufreq->latency); +} + +static int ingenic_cpufreq_target(struct cpufreq_policy *policy, unsigned int index) +{ + struct cpufreq_frequency_table *freq_table = ingenic_cpufreq->freq_table; + unsigned long freq_new, freq_old; + unsigned long freq_round; + struct clk *mux, *sclka, *mclk, *clk_cpu_l2c; + unsigned int a_rate, m_rate, parent; + struct dev_pm_opp *opp; + unsigned long v; + + mutex_lock(&ingenic_cpufreq->mutex); + freq_new = freq_table[index].frequency * 1000; + freq_old = clk_get_rate(policy->clk); + + if (freq_old == freq_new) { + mutex_unlock(&ingenic_cpufreq->mutex); + return 0; + } + + parent = freq_table[index].driver_data; + mclk = ingenic_cpufreq->mclk; + sclka = ingenic_cpufreq->sclka; + mux = ingenic_cpufreq->mux; + + a_rate = ingenic_cpufreq->arate; + m_rate = ingenic_cpufreq->mrate; + + if (freq_new > BIU_FREQ_THRESHOLD) { + clk_cpu_l2c = ingenic_cpufreq->clk_cpu_l2c_x2; + } else { + clk_cpu_l2c = ingenic_cpufreq->clk_cpu_l2c_x1; + } + clk_get_rate(clk_cpu_l2c); + +#ifdef CONFIG_REGULATOR + freq_round = clk_round_rate(clk_cpu_l2c, freq_new); + + opp = dev_pm_opp_find_freq_ceil(ingenic_cpufreq->dev, &freq_round); + v = dev_pm_opp_get_voltage(opp); + + if (freq_round > freq_old) { + regulator_set_voltage(ingenic_cpufreq->regulator, v, v); + clk_set_rate(clk_cpu_l2c, freq_round); + } + if (freq_round < freq_old) { + clk_set_rate(clk_cpu_l2c, freq_round); + regulator_set_voltage(ingenic_cpufreq->regulator, v, v); + } +#else + clk_set_rate(clk_cpu_l2c, freq_new); +#endif + + if (parent == PARENT_APLL) { + clk_set_parent(mux, sclka); + } else if (parent == PARENT_MPLL) { + clk_set_parent(mux, mclk); + } else { + printk("wrong clk parent\n"); + } + +#ifdef CONFIG_REGULATOR + opp = dev_pm_opp_find_freq_exact(ingenic_cpufreq->dev, freq_new, 1); + v = dev_pm_opp_get_voltage(opp); + + if (freq_new >= freq_round) { + regulator_set_voltage(ingenic_cpufreq->regulator, v, v); + clk_set_rate(clk_cpu_l2c, freq_new); + } + if (freq_new < freq_round) { + clk_set_rate(clk_cpu_l2c, freq_new); + regulator_set_voltage(ingenic_cpufreq->regulator, v, v); + } +#else + clk_set_rate(clk_cpu_l2c, freq_new); +#endif + + policy->clk = clk_cpu_l2c; + policy->cur = freq_new / 1000; + +#ifdef CONFIG_REGULATOR + if ((freq_new != clk_get_rate(clk_cpu_l2c)) || (v != regulator_get_voltage(ingenic_cpufreq->regulator))) { + printk("warning : "); + printk("from %dKHZ to %dKHZ(%duV) now: %dKHZ(%duV)\n", freq_old/1000, freq_new/1000, v, clk_get_rate(clk_cpu_l2c)/1000, regulator_get_voltage(ingenic_cpufreq->regulator)); + } +#else + if (freq_new != clk_get_rate(clk_cpu_l2c)) { + printk("warning : "); + printk("from %dKHZ to %dKHZ now: %dKHZ\n", freq_old/1000, freq_new/1000, clk_get_rate(clk_cpu_l2c)/1000); + } +#endif + + mutex_unlock(&ingenic_cpufreq->mutex); + + return 0; +} + +static int ingenic_cpufreq_suspend(struct cpufreq_policy *policy) +{ + return 0; +} + +static int ingenic_cpufreq_resume(struct cpufreq_policy *policy) +{ + return 0; +} + +static struct cpufreq_driver ingenic_cpufreq_driver = { + .name = "ingenic-cpufreq", + .flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK, + .init = ingenic_cpufreq_init, + .verify = cpufreq_generic_frequency_table_verify, + .target_index = ingenic_cpufreq_target, + .get = cpufreq_generic_get, + .suspend = ingenic_cpufreq_suspend, + .resume = ingenic_cpufreq_resume, + .attr = cpufreq_generic_attr, +}; + +static const struct of_device_id ingenic_cpufreq_match[] = { + { + .compatible = "ingenic,t40-cpufreq", + .compatible = "ingenic,x2000-v12-cpufreq", + }, + {}, +}; + +MODULE_DEVICE_TABLE(of, ingenic_cpufreq_match); + +static int ingenic_cpufreq_probe(struct platform_device *pdev) +{ + int ret; + struct device_node *np; + + np = pdev->dev.of_node; + if (!np) + return -ENODEV; + + ingenic_cpufreq = devm_kzalloc(&pdev->dev, sizeof(struct ingenic_cpufreq), GFP_KERNEL); + if (!ingenic_cpufreq) { + ret = -ENOMEM; + goto err_put_node; + } + + ingenic_cpufreq->dev = &pdev->dev; + + ret = dev_pm_opp_of_add_table(ingenic_cpufreq->dev); + if (ret) { + dev_err(ingenic_cpufreq->dev, "failed to init OPP table : %d\n", ret); + goto err_put_node; + } + + ret = dev_pm_opp_init_cpufreq_table(ingenic_cpufreq->dev, &ingenic_cpufreq->freq_table_dt); + if (ret) { + dev_err(ingenic_cpufreq->dev, "failed to init cpufreq table : %d\n", ret); + goto err_free_opp; + } + + ingenic_cpufreq->freq_count = dev_pm_opp_get_opp_count(ingenic_cpufreq->dev); + + ingenic_cpufreq->freq_table = kcalloc(ingenic_cpufreq->freq_count + 1, sizeof(struct cpufreq_frequency_table), GFP_ATOMIC); + +#ifdef CONFIG_REGULATOR + ingenic_cpufreq->regulator = devm_regulator_get(ingenic_cpufreq->dev, "cpu-core"); + if (!ingenic_cpufreq->regulator) + printk("cannot get regulator for cpufreq\n"); + +#endif + + mutex_init(&ingenic_cpufreq->mutex); + + cpufreq_register_notifier(&ingenic_cpufreq_notifier_block, CPUFREQ_TRANSITION_NOTIFIER); + + ret = cpufreq_register_driver(&ingenic_cpufreq_driver); + if (ret) { + dev_err(ingenic_cpufreq->dev, "%s : failed to register cpufreq driver\n", __func__); + goto err_free_table; + } + + of_node_put(np); + + printk("cpufreq register succeed\n"); + + return 0; + +err_free_table: + dev_pm_opp_free_cpufreq_table(ingenic_cpufreq->dev, &ingenic_cpufreq->freq_table_dt); + +err_free_opp: + dev_pm_opp_of_remove_table(ingenic_cpufreq->dev); + +err_put_node: + of_node_put(np); + dev_err(&pdev->dev, "%s : failed initialization\n", __func__); + + return ret; +} + + +static int ingenic_cpufreq_remove(struct platform_device *pdev) +{ + cpufreq_unregister_driver(&ingenic_cpufreq_driver); + cpufreq_unregister_notifier(&ingenic_cpufreq_notifier_block, CPUFREQ_TRANSITION_NOTIFIER); + + return 0; +} + +static struct platform_driver ingenic_cpufreq_platdrv = { + .driver = { + .name = "ingenic-cpufreq", + .of_match_table = ingenic_cpufreq_match, + }, + .probe = ingenic_cpufreq_probe, + .remove = ingenic_cpufreq_remove, +}; +module_platform_driver(ingenic_cpufreq_platdrv); + +MODULE_AUTHOR("Liu Si Hui "); +MODULE_DESCRIPTION("ingenic t40 cpufreq driver"); +MODULE_LICENSE("GPL"); +