riscv: Power Brake

    This is the commit for Power brake.
    In Andes extension, Power brake register divide
    cpu frequency into 16 parts, you can set the level
    you want to scale the cpu frequency(reference
    AndeStar V5 SPA).

    Also, the users can set value in '/${Path to cpufreq}/
    scaling_max/min_freq' to decide the range you want to
    scale cpu frequency. The cpufreq driver will help you
    to set the level in Power brake regsiter.

Signed-off-by: Nick Hu <nickhu@andestech.com>

Author: Nick Hu

arch/riscv/Kconfig

@@ -76,6 +76,18 @@ config ARCH_MMAP_RND_BITS_MIN
 config ARCH_MMAP_RND_BITS_MAX
 	default 24 if 64BIT # SV39 based
 	default 17
+	select MODULE_SECTIONS if MODULES
+config CPU_FREQ
+	def_bool y
+
+config RISCV_CPUFREQ
+	def_bool y
+
+config CPU_FREQ_GOV_POWERSAVE
+	def_bool y
+
+config CPU_FREQ_DEFAULT_GOV_POWERSAVE
+	def_bool y
 
 config MMU
 	def_bool y

arch/riscv/andesv5/sbi.c

@@ -28,6 +28,20 @@ void sbi_restart(int cpu_num)
 }
 EXPORT_SYMBOL(sbi_restart);
 
+void sbi_write_powerbrake(int val)
+{
+  sbi_ecall(SBI_EXT_ANDES, SBI_EXT_ANDES_WRITE_POWERBRAKE, val, 0, 0, 0, 0, 0);
+}
+EXPORT_SYMBOL(sbi_write_powerbrake);
+
+int sbi_read_powerbrake(void)
+{
+  struct sbiret ret;
+  ret = sbi_ecall(SBI_EXT_ANDES, SBI_EXT_ANDES_READ_POWERBRAKE, 0, 0, 0, 0, 0, 0);
+  return ret.value;
+}
+EXPORT_SYMBOL(sbi_read_powerbrake);
+
 void sbi_set_suspend_mode(int suspend_mode)
 {
 	sbi_ecall(SBI_EXT_ANDES, SBI_EXT_ANDES_SET_SUSPEND_MODE, suspend_mode, 0, 0, 0, 0, 0);

arch/riscv/include/asm/sbi.h

@@ -182,6 +182,8 @@ void sbi_set_reset_vec(int val);
 void sbi_set_pma(phys_addr_t offset, unsigned long vaddr, size_t size);
 void sbi_free_pma(unsigned long vaddr);
 long sbi_probe_pma(void);
+void sbi_write_powerbrake(int val);
+int sbi_read_powerbrake(void);
 
 #else /* CONFIG_RISCV_SBI */
 /* stubs for code that is only reachable under IS_ENABLED(CONFIG_RISCV_SBI): */

drivers/cpufreq/Makefile

@@ -101,6 +101,10 @@ obj-$(CONFIG_CPU_FREQ_PMAC64)		+= pmac64-cpufreq.o
 obj-$(CONFIG_PPC_PASEMI_CPUFREQ)	+= pasemi-cpufreq.o
 obj-$(CONFIG_POWERNV_CPUFREQ)		+= powernv-cpufreq.o
 
+##################################################################################
+# RISCV platform drivers
+obj-$(CONFIG_RISCV_CPUFREQ)            += riscv-cpufreq.o
+
 ##################################################################################
 # Other platform drivers
 obj-$(CONFIG_BMIPS_CPUFREQ)		+= bmips-cpufreq.o

drivers/cpufreq/riscv-cpufreq.c

@@ -0,0 +1,150 @@
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/timex.h>
+
+#include <asm/sbi.h>
+
+static unsigned int riscv_cpufreq_get(unsigned int cpu)
+{
+	int val;
+
+	if (cpu)
+		return 0;
+
+	val = sbi_read_powerbrake();
+	val = val & 0xf0;
+	val = val >> 4;
+
+	if (val == 0)
+		return 60 * 1024;
+	else if (val == 15)
+		return 10 * 1024;
+
+	return (52 - val * 3 + 2) * 1024;
+}
+
+void write_powerbrake(void *arg)
+{
+	int *val = arg;
+
+	sbi_write_powerbrake(*val);
+}
+
+static int riscv_cpufreq_set_policy(struct cpufreq_policy *policy)
+{
+	int val, i;
+	unsigned int cpu = policy->cpu;
+
+	if (!policy)
+		return -EINVAL;
+
+	val = (policy->min + policy->max) / 2;
+	switch (policy->policy) {
+		case CPUFREQ_POLICY_POWERSAVE:
+			val = (val + policy->min) / 2 / 1024;
+			break;
+		case CPUFREQ_POLICY_PERFORMANCE:
+			val = (val + policy->max) / 2 / 1024;
+			break;
+		default:
+			pr_err ("Not Support this governor\n");
+			break;
+	}
+
+	if (val < 10)
+		val = 15;
+	else if (val > 51)
+		val = 0;
+	else {
+		// search for level
+		for (i = 1; i < 15 ; i++) {
+			if ((val >= 52 - i * 3) &&
+			     (val <= 52 - i * 3 + 2)) {
+				val = i;
+				break;
+			}
+		}
+	}
+
+	val = val << 4;
+	return smp_call_function_single(cpu, write_powerbrake, &val, 1);
+}
+
+static int riscv_cpufreq_verify_policy(struct cpufreq_policy *policy)
+{
+	if (!policy)
+		return -EINVAL;
+
+	cpufreq_verify_within_cpu_limits(policy);
+
+	if((policy->policy != CPUFREQ_POLICY_POWERSAVE) &&
+	   (policy->policy != CPUFREQ_POLICY_PERFORMANCE))
+		return -EINVAL;
+
+	return 0;
+}
+
+static void riscv_get_policy(struct cpufreq_policy *policy)
+{
+	int val;
+
+	val = sbi_read_powerbrake();
+	val = val & 0xf0;
+	val = val >> 4;
+	/*
+	 * Powerbrake register has 16 level,
+	 * so we divide the 60Mhz into 16 parts.
+	 *	|    |  |........|  |    |
+	 * Mhz	0    10 13       49 52   60
+	 */
+	if (val == 0) { // highest performance
+		policy->min = 52 * 1024;
+		policy->max = 60 * 1024;
+	} else if (val == 15) { // lowest performance
+		policy->min = 0;
+		policy->max = 10 * 1024;
+	} else {
+		policy->min = 52 - val * 3;
+		policy->max = 52 - val * 3 + 2;
+	}
+
+	policy->policy = CPUFREQ_POLICY_POWERSAVE;
+}
+
+static int riscv_cpufreq_cpu_init(struct cpufreq_policy *policy)
+{
+
+	policy->cpuinfo.min_freq = 0;
+	policy->cpuinfo.max_freq = 60*1024; /* 60Mhz=60*1024khz */
+
+	riscv_get_policy(policy);
+
+	return 0;
+}
+
+static struct cpufreq_driver riscv_cpufreq_driver = {
+	.flags		= CPUFREQ_CONST_LOOPS,
+	.verify		= riscv_cpufreq_verify_policy,
+	.setpolicy	= riscv_cpufreq_set_policy,
+	.get		= riscv_cpufreq_get,
+	.init		= riscv_cpufreq_cpu_init,
+	.name		= "riscv_cpufreq",
+};
+
+static int __init riscv_cpufreq_init(void)
+{
+	return cpufreq_register_driver(&riscv_cpufreq_driver);
+}
+
+static void __exit riscv_cpufreq_exit(void)
+{
+	cpufreq_unregister_driver(&riscv_cpufreq_driver);
+}
+
+MODULE_AUTHOR("Nick Hu <nickhu@andestech.com>");
+MODULE_DESCRIPTION("Riscv cpufreq driver.");
+MODULE_LICENSE("GPL");
+module_init(riscv_cpufreq_init);
+module_exit(riscv_cpufreq_exit);