marvell/linux/drivers/cpufreq/asr-cpufreq.c

// 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);