blob: 0c6230a48eb916035c5340c5000d8727d1f3ebe4 [file] [log] [blame]
// 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);