ASR_BASE
Change-Id: Icf3719cc0afe3eeb3edc7fa80a2eb5199ca9dda1
diff --git a/marvell/linux/drivers/cpufreq/asr-cpufreq.c b/marvell/linux/drivers/cpufreq/asr-cpufreq.c
new file mode 100644
index 0000000..0c6230a
--- /dev/null
+++ b/marvell/linux/drivers/cpufreq/asr-cpufreq.c
@@ -0,0 +1,405 @@
+// 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);