| // SPDX-License-Identifier: GPL-2.0 |
| /* |
| * Support for asr spi controller |
| * |
| * Copyright (C) 2021 ASR Micro Limited |
| * |
| */ |
| |
| #include <linux/clk.h> |
| #include <linux/clk-provider.h> |
| #include <linux/cpufreq.h> |
| #include <linux/cpumask.h> |
| #include <linux/delay.h> |
| #include <linux/err.h> |
| #include <linux/init.h> |
| #include <linux/io.h> |
| #include <linux/kernel.h> |
| #include <linux/module.h> |
| #include <linux/types.h> |
| #include <linux/sched.h> |
| #include <linux/suspend.h> |
| //#include <asm/system.h> |
| #include <asm/cpu.h> |
| #include <linux/pm_qos.h> |
| |
| #define NUM_CPUS num_possible_cpus() |
| |
| #define KHZ_TO_HZ (1000) |
| #define MHZ_TO_KHZ (1000) |
| #define MHZ_TO_HZ (1000000) |
| |
| static struct clk *cpu_clk; |
| static DEFINE_MUTEX(asr18xx_cpu_lock); |
| static bool is_suspended; |
| static struct cpufreq_frequency_table *freq_table; |
| static bool is_qosreq_inited; |
| |
| /* Qos min request client cpufreq driver policy->cpuinfo.min */ |
| static struct pm_qos_request cpufreq_qos_req_min = { |
| .name = "cpu_freqmin", |
| }; |
| /* Qos max request client cpufreq driver policy->cpuinfo.min */ |
| static struct pm_qos_request cpufreq_qos_req_max = { |
| .name = "cpu_freqmax", |
| }; |
| |
| int asr18xx_verify_speed(struct cpufreq_policy_data *policy) |
| { |
| return cpufreq_frequency_table_verify(policy, freq_table); |
| } |
| |
| unsigned int asr18xx_getspeed(unsigned int cpu) |
| { |
| unsigned long rate; |
| |
| if (cpu >= NUM_CPUS) |
| return 0; |
| |
| rate = clk_get_rate(cpu_clk) / KHZ_TO_HZ; |
| return rate; |
| } |
| |
| static int asr18xx_update_cpu_speed(unsigned long rate) |
| { |
| struct cpufreq_freqs freqs; |
| struct cpufreq_policy *policy; |
| int ret = 0; |
| int cpu = 0; |
| |
| freqs.old = asr18xx_getspeed(0); |
| freqs.new = rate; |
| |
| if (freqs.old == freqs.new) |
| return 0; |
| |
| policy = cpufreq_cpu_get(cpu); |
| BUG_ON(!policy); |
| #ifdef CONFIG_CPU_FREQ_DEBUG |
| printk(KERN_DEBUG "cpufreq-asr18xx: transition: %u --> %u\n", |
| freqs.old, freqs.new); |
| #endif |
| /* cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE); */ |
| cpufreq_freq_transition_begin(policy, &freqs); |
| |
| ret = clk_set_rate(cpu_clk, freqs.new * KHZ_TO_HZ); |
| |
| if (ret) |
| freqs.new = freqs.old; |
| |
| /* cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE); */ |
| cpufreq_freq_transition_end(policy, &freqs, 0); |
| cpufreq_cpu_put(policy); |
| return ret; |
| } |
| |
| /* Same as cpufreq_frequency_table_target, but no policy min/max check */ |
| static int cpufreq_table_target(struct cpufreq_policy *policy, |
| struct cpufreq_frequency_table *table, |
| unsigned int target_freq, |
| unsigned int relation, |
| unsigned int *index) |
| { |
| struct cpufreq_frequency_table optimal = { |
| .driver_data = ~0, |
| .frequency = 0, |
| }; |
| struct cpufreq_frequency_table suboptimal = { |
| .driver_data = ~0, |
| .frequency = 0, |
| }; |
| unsigned int diff; |
| unsigned int i; |
| |
| pr_debug("request for target %u kHz (relation: %u) for cpu %u\n", |
| target_freq, relation, policy->cpu); |
| |
| switch (relation) { |
| case CPUFREQ_RELATION_H: |
| suboptimal.frequency = ~0; |
| break; |
| case CPUFREQ_RELATION_L: |
| case CPUFREQ_RELATION_C: |
| optimal.frequency = ~0; |
| break; |
| } |
| |
| if (!cpu_online(policy->cpu)) { |
| WARN(1, "%s error: cpu%d offline\n", __func__, policy->cpu); |
| return 0; |
| } |
| |
| for (i = 0; (table[i].frequency != CPUFREQ_TABLE_END); i++) { |
| unsigned int freq = table[i].frequency; |
| if (freq == CPUFREQ_ENTRY_INVALID) |
| continue; |
| if (freq == target_freq) { |
| optimal.driver_data = i; |
| break; |
| } |
| switch (relation) { |
| case CPUFREQ_RELATION_H: |
| if (freq <= target_freq) { |
| if (freq >= optimal.frequency) { |
| optimal.frequency = freq; |
| optimal.driver_data = i; |
| } |
| } else { |
| if (freq <= suboptimal.frequency) { |
| suboptimal.frequency = freq; |
| suboptimal.driver_data = i; |
| } |
| } |
| break; |
| case CPUFREQ_RELATION_L: |
| if (freq >= target_freq) { |
| if (freq <= optimal.frequency) { |
| optimal.frequency = freq; |
| optimal.driver_data = i; |
| } |
| } else { |
| if (freq >= suboptimal.frequency) { |
| suboptimal.frequency = freq; |
| suboptimal.driver_data = i; |
| } |
| } |
| break; |
| case CPUFREQ_RELATION_C: |
| diff = abs(freq - target_freq); |
| if (diff < optimal.frequency || |
| (diff == optimal.frequency && |
| freq > table[optimal.driver_data].frequency)) { |
| optimal.frequency = diff; |
| optimal.driver_data = i; |
| } |
| break; |
| } |
| } |
| if (optimal.driver_data > i) { |
| if (suboptimal.driver_data > i) { |
| WARN(1, "%s Invalid frequency table: %d\n", __func__, policy->cpu); |
| return 0; |
| } |
| *index = suboptimal.driver_data; |
| } else |
| *index = optimal.driver_data; |
| |
| pr_debug("target is %u (%u kHz, %u)\n", *index, table[*index].frequency, |
| table[*index].driver_data); |
| |
| return 0; |
| } |
| |
| static int asr18xx_target(struct cpufreq_policy *policy, |
| unsigned int target_freq, |
| unsigned int relation) |
| { |
| int idx = 0; |
| unsigned int freq, qos_min, qos_max; |
| int ret = 0; |
| |
| mutex_lock(&asr18xx_cpu_lock); |
| |
| if (is_suspended) { |
| ret = -EBUSY; |
| goto out; |
| } |
| |
| /* |
| * Policy min and qos min have lower priority |
| * than policy max and qos max. |
| * Policy min and qos min has no priority order |
| * Policy max and qos max has no proirity order |
| */ |
| qos_min = (unsigned int)pm_qos_request(PM_QOS_CPUFREQ_MIN); |
| qos_max = (unsigned int)pm_qos_request(PM_QOS_CPUFREQ_MAX); |
| target_freq = max(policy->min, target_freq); |
| target_freq = max(qos_min, target_freq); |
| target_freq = min(policy->max, target_freq); |
| target_freq = min(qos_max, target_freq); |
| if ((target_freq == policy->max) || (target_freq == qos_max)) |
| relation = CPUFREQ_RELATION_H; |
| cpufreq_table_target(policy, freq_table, target_freq, relation, &idx); |
| freq = freq_table[idx].frequency; |
| pr_debug("Qos_min:%d Qos_max:%d, Target:%d(KHZ)\n", |
| pm_qos_request(PM_QOS_CPUFREQ_MIN), |
| pm_qos_request(PM_QOS_CPUFREQ_MAX), |
| freq); |
| |
| ret = asr18xx_update_cpu_speed(freq); |
| out: |
| mutex_unlock(&asr18xx_cpu_lock); |
| return ret; |
| } |
| |
| |
| static int asr18xx_pm_notify(struct notifier_block *nb, unsigned long event, |
| void *dummy) |
| { |
| static unsigned int saved_cpuclk = 0; |
| mutex_lock(&asr18xx_cpu_lock); |
| if (event == PM_SUSPEND_PREPARE) { |
| /* scaling to the min frequency before entering suspend */ |
| saved_cpuclk = asr18xx_getspeed(0); |
| asr18xx_update_cpu_speed(freq_table[0].frequency); |
| is_suspended = true; |
| pr_pm_debug("%s: disable cpu freq-chg before suspend, cur"\ |
| " rate %dKhz\n", |
| __func__, asr18xx_getspeed(0)); |
| } else if (event == PM_POST_SUSPEND) { |
| is_suspended = false; |
| asr18xx_update_cpu_speed(saved_cpuclk); |
| pr_pm_debug("%s: enable cpu freq-chg after resume, cur"\ |
| " rate %dKhz\n", |
| __func__, asr18xx_getspeed(0)); |
| } |
| mutex_unlock(&asr18xx_cpu_lock); |
| return NOTIFY_OK; |
| } |
| |
| static struct notifier_block asr18xx_cpu_pm_notifier = { |
| .notifier_call = asr18xx_pm_notify, |
| }; |
| |
| /* cpufreq qos min/max notifier unit Khz */ |
| static int cpufreq_min_notify(struct notifier_block *b, |
| unsigned long min, void *v) |
| { |
| int ret; |
| unsigned long freq, val = min; |
| struct cpufreq_policy *policy; |
| int cpu = 0; |
| |
| freq = asr18xx_getspeed(cpu); |
| if (freq >= val) |
| return NOTIFY_OK; |
| |
| policy = cpufreq_cpu_get(cpu); |
| if (!policy) |
| return NOTIFY_BAD; |
| |
| ret = asr18xx_target(policy, val, CPUFREQ_RELATION_L); |
| cpufreq_cpu_put(policy); |
| if (ret < 0) |
| return NOTIFY_BAD; |
| |
| return NOTIFY_OK; |
| } |
| |
| static struct notifier_block cpufreq_min_notifier = { |
| .notifier_call = cpufreq_min_notify, |
| }; |
| |
| static int cpufreq_max_notify(struct notifier_block *b, |
| unsigned long max, void *v) |
| { |
| int ret; |
| unsigned long freq, val = max; |
| struct cpufreq_policy *policy; |
| int cpu = 0; |
| |
| freq = asr18xx_getspeed(cpu); |
| |
| policy = cpufreq_cpu_get(cpu); |
| if (!policy) |
| return NOTIFY_BAD; |
| |
| ret = asr18xx_target(policy, val, CPUFREQ_RELATION_H); |
| cpufreq_cpu_put(policy); |
| if (ret < 0) |
| return NOTIFY_BAD; |
| |
| return NOTIFY_OK; |
| } |
| |
| |
| static struct notifier_block cpufreq_max_notifier = { |
| .notifier_call = cpufreq_max_notify, |
| }; |
| |
| |
| static int asr18xx_cpufreq_init(struct cpufreq_policy *policy) |
| { |
| if (policy->cpu >= NUM_CPUS) |
| return -EINVAL; |
| |
| if (unlikely(!cpu_clk)) { |
| cpu_clk = __clk_lookup("cpu"); |
| if (IS_ERR(cpu_clk)) |
| return PTR_ERR(cpu_clk); |
| } |
| |
| freq_table = cpufreq_frequency_get_table(policy->cpu); |
| BUG_ON(!freq_table); |
| cpufreq_frequency_table_cpuinfo(policy, freq_table); |
| policy->cur = asr18xx_getspeed(policy->cpu); |
| |
| /* |
| * FIXME: what's the actual transition time? |
| * use 10ms as sampling rate for bring up |
| */ |
| policy->cpuinfo.transition_latency = 10 * 1000; |
| policy->freq_table = freq_table; |
| cpumask_setall(policy->cpus); |
| if (unlikely(!is_qosreq_inited)) { |
| pm_qos_add_request(&cpufreq_qos_req_min, |
| PM_QOS_CPUFREQ_MIN, policy->cpuinfo.min_freq); |
| pm_qos_add_request(&cpufreq_qos_req_max, |
| PM_QOS_CPUFREQ_MAX, policy->cpuinfo.max_freq); |
| is_qosreq_inited = true; |
| } |
| |
| return 0; |
| } |
| |
| static int asr18xx_cpufreq_exit(struct cpufreq_policy *policy) |
| { |
| cpufreq_frequency_table_cpuinfo(policy, freq_table); |
| return 0; |
| } |
| |
| static struct freq_attr *asr18xx_cpufreq_attr[] = { |
| &cpufreq_freq_attr_scaling_available_freqs, |
| NULL, |
| }; |
| |
| static struct cpufreq_driver asr18xx_cpufreq_driver = { |
| #ifdef CONFIG_CPU_ASR18XX |
| .flags = 0, |
| #else |
| .flags = CPUFREQ_CONST_LOOPS, |
| #endif |
| .verify = asr18xx_verify_speed, |
| .target = asr18xx_target, |
| .get = asr18xx_getspeed, |
| .init = asr18xx_cpufreq_init, |
| .exit = asr18xx_cpufreq_exit, |
| .name = "asr18xx-cpufreq", |
| .attr = asr18xx_cpufreq_attr, |
| }; |
| |
| static int __init cpufreq_init(void) |
| { |
| register_pm_notifier(&asr18xx_cpu_pm_notifier); |
| pm_qos_add_notifier(PM_QOS_CPUFREQ_MIN, |
| &cpufreq_min_notifier); |
| pm_qos_add_notifier(PM_QOS_CPUFREQ_MAX, |
| &cpufreq_max_notifier); |
| return cpufreq_register_driver(&asr18xx_cpufreq_driver); |
| } |
| |
| static void __exit cpufreq_exit(void) |
| { |
| unregister_pm_notifier(&asr18xx_cpu_pm_notifier); |
| pm_qos_remove_notifier(PM_QOS_CPUFREQ_MIN, |
| &cpufreq_min_notifier); |
| pm_qos_remove_notifier(PM_QOS_CPUFREQ_MAX, |
| &cpufreq_max_notifier); |
| cpufreq_unregister_driver(&asr18xx_cpufreq_driver); |
| } |
| |
| |
| MODULE_DESCRIPTION("cpufreq driver for ASR ASR18XX"); |
| MODULE_LICENSE("GPL"); |
| module_init(cpufreq_init); |
| module_exit(cpufreq_exit); |