Implement DVFS support.

ok patrick@

2 files changed, 279 insertions, 2 deletions

diff --git a/sys/arch/arm64/arm64/cpu.c b/sys/arch/arm64/arm64/cpu.c
index 3db2dd4e4e7..2daa3714c4c 100644
--- a/sys/arch/arm64/arm64/cpu.c
+++ b/sys/arch/arm64/arm64/cpu.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: cpu.c,v 1.19 2018/06/05 09:45:08 jsg Exp $	*/
+/*	$OpenBSD: cpu.c,v 1.20 2018/08/03 16:45:17 kettenis Exp $	*/
 
 /*
  * Copyright (c) 2016 Dale Rahn <drahn@dalerahn.com>
@@ -23,6 +23,7 @@
 #include <sys/malloc.h>
 #include <sys/device.h>
 #include <sys/sysctl.h>
+#include <sys/task.h>
 
 #include <uvm/uvm.h>
 
@@ -30,6 +31,7 @@
 
 #include <dev/ofw/openfirm.h>
 #include <dev/ofw/ofw_clock.h>
+#include <dev/ofw/ofw_regulator.h>
 #include <dev/ofw/fdt.h>
 
 #include <machine/cpufunc.h>
@@ -119,6 +121,8 @@ struct cfdriver cpu_cd = {
 	NULL, "cpu", DV_DULL
 };
 
+void	cpu_opp_init(struct cpu_info *, uint32_t);
+
 void	cpu_flush_bp_noop(void);
 void	cpu_flush_bp_psci(void);
 
@@ -222,6 +226,7 @@ cpu_attach(struct device *parent, struct device *dev, void *aux)
 	struct fdt_attach_args *faa = aux;
 	struct cpu_info *ci;
 	uint64_t mpidr = READ_SPECIALREG(mpidr_el1);
+	uint32_t opp;
 
 	KASSERT(faa->fa_nreg > 0);
 
@@ -295,6 +300,10 @@ cpu_attach(struct device *parent, struct device *dev, void *aux)
 	}
 #endif
 
+	opp = OF_getpropint(ci->ci_node, "operating-points-v2", 0);
+	if (opp)
+		cpu_opp_init(ci, opp);
+
 	printf("\n");
 }
 
@@ -472,3 +481,267 @@ cpu_unidle(struct cpu_info *ci)
 }
 
 #endif
+
+/*
+ * Dynamic voltage and frequency scaling implementation.
+ */
+
+extern int perflevel;
+
+struct opp {
+	uint64_t opp_hz;
+	uint32_t opp_microvolt;
+};
+
+struct opp_table {
+	LIST_ENTRY(opp_table) ot_list;
+	uint32_t ot_phandle;
+
+	struct opp *ot_opp;
+	u_int ot_nopp;
+	uint64_t ot_opp_hz_min;
+	uint64_t ot_opp_hz_max;
+
+	struct cpu_info *ot_master;
+};
+
+LIST_HEAD(, opp_table) opp_tables = LIST_HEAD_INITIALIZER(opp_tables);
+struct task cpu_opp_task;
+
+void	cpu_opp_mountroot(struct device *);
+void	cpu_opp_dotask(void *);
+void	cpu_opp_setperf(int);
+
+void
+cpu_opp_init(struct cpu_info *ci, uint32_t phandle)
+{
+	struct opp_table *ot;
+	int count, node, child;
+	int i;
+
+	LIST_FOREACH(ot, &opp_tables, ot_list) {
+		if (ot->ot_phandle == phandle) {
+			ci->ci_opp_table = ot;
+			return;
+		}
+	}
+
+	node = OF_getnodebyphandle(phandle);
+	if (node == 0)
+		return;
+
+	if (!OF_is_compatible(node, "operating-points-v2"))
+		return;
+
+	count = 0;
+	for (child = OF_child(node); child != 0; child = OF_peer(child)) {
+		if (OF_getproplen(child, "turbo-mode") == 0)
+			continue;
+		count++;
+	}
+	if (count == 0)
+		return;
+
+	ot = malloc(sizeof(struct opp_table), M_DEVBUF, M_ZERO | M_WAITOK);
+	ot->ot_phandle = phandle;
+	ot->ot_opp = mallocarray(count, sizeof(struct opp),
+	    M_DEVBUF, M_ZERO | M_WAITOK);
+	ot->ot_nopp = count;
+
+	count = 0;
+	for (child = OF_child(node); child != 0; child = OF_peer(child)) {
+		if (OF_getproplen(child, "turbo-mode") == 0)
+			continue;
+		ot->ot_opp[count].opp_hz =
+		    OF_getpropint64(child, "opp-hz", 0);
+		ot->ot_opp[count].opp_microvolt =
+		    OF_getpropint(child, "opp-microvolt", 0);
+		count++;
+	}
+
+	ot->ot_opp_hz_min = ot->ot_opp[0].opp_hz;
+	ot->ot_opp_hz_max = ot->ot_opp[0].opp_hz;
+	for (i = 1; i < ot->ot_nopp; i++) {
+		if (ot->ot_opp[i].opp_hz < ot->ot_opp_hz_min)
+			ot->ot_opp_hz_min = ot->ot_opp[i].opp_hz;
+		if (ot->ot_opp[i].opp_hz > ot->ot_opp_hz_max)
+			ot->ot_opp_hz_max = ot->ot_opp[i].opp_hz;
+	}
+
+	if (OF_getproplen(node, "opp-shared") == 0)
+		ot->ot_master = ci;
+
+	LIST_INSERT_HEAD(&opp_tables, ot, ot_list);
+
+	ci->ci_opp_table = ot;
+	ci->ci_cpu_supply = OF_getpropint(ci->ci_node, "cpu-supply", 0);
+
+	/*
+	 * Do addional checks at mountroot when all the clocks and
+	 * regulators are available.
+	 */
+	config_mountroot(ci->ci_dev, cpu_opp_mountroot);
+}
+
+void
+cpu_opp_mountroot(struct device *self)
+{
+	struct cpu_info *ci;
+	CPU_INFO_ITERATOR cii;
+	int count = 0;
+	int level = 0;
+
+	if (cpu_setperf)
+		return;
+
+	CPU_INFO_FOREACH(cii, ci) {
+		struct opp_table *ot = ci->ci_opp_table;
+		uint64_t curr_hz;
+		uint32_t curr_microvolt;
+		int error;
+
+		if (ot == NULL)
+			continue;
+
+		/* Skip if this table is shared and we're not the master. */
+		if (ot->ot_master && ot->ot_master != ci)
+			continue;
+
+		curr_hz = clock_get_frequency(ci->ci_node, NULL);
+		curr_microvolt = regulator_get_voltage(ci->ci_cpu_supply);
+
+		/* Disable if clock isn't implemented. */
+		error = ENODEV;
+		if (curr_hz != 0)
+			error = clock_set_frequency(ci->ci_node, NULL, curr_hz);
+		if (error) {
+			ci->ci_opp_table = NULL;
+			printf("%s: clock not implemented\n",
+			       ci->ci_dev->dv_xname);
+			continue;
+		}
+
+		/* Disable if regulator isn't implemented. */
+		error = ENODEV;
+		if (curr_microvolt != 0)
+			error = regulator_set_voltage(ci->ci_cpu_supply,
+			    curr_microvolt);
+		if (error) {
+			ci->ci_opp_table = NULL;
+			printf("%s: regulator not implemented\n",
+			    ci->ci_dev->dv_xname);
+			continue;
+		}
+
+		/*
+		 * Initialize performance level based on the current
+		 * speed of the first CPU that supports DVFS.
+		 */
+		if (level == 0) {
+			uint64_t min, max;
+			uint64_t level_hz;
+
+			min = ot->ot_opp_hz_min;
+			max = ot->ot_opp_hz_max;
+			level_hz = clock_get_frequency(ci->ci_node, NULL);
+			level = howmany(100 * (level_hz - min), (max - min));
+		}
+
+		count++;
+	}
+
+	if (count > 0) {
+		task_set(&cpu_opp_task, cpu_opp_dotask, NULL);
+		cpu_setperf = cpu_opp_setperf;
+
+		perflevel = (level > 0) ? level : 0;
+		cpu_setperf(level);
+	}
+}
+
+void
+cpu_opp_dotask(void *arg)
+{
+	struct cpu_info *ci;
+	CPU_INFO_ITERATOR cii;
+
+	CPU_INFO_FOREACH(cii, ci) {
+		struct opp_table *ot = ci->ci_opp_table;
+		uint64_t curr_hz, opp_hz;
+		uint32_t curr_microvolt, opp_microvolt;
+		int opp_idx;
+		int error = 0;
+
+		if (ot == NULL)
+			continue;
+
+		/* Skip if this table is shared and we're not the master. */
+		if (ot->ot_master && ot->ot_master != ci)
+			continue;
+
+		opp_idx = ci->ci_opp_idx;
+		opp_hz = ot->ot_opp[opp_idx].opp_hz;
+		opp_microvolt = ot->ot_opp[opp_idx].opp_microvolt;
+
+		curr_hz = clock_get_frequency(ci->ci_node, NULL);
+		curr_microvolt = regulator_get_voltage(ci->ci_cpu_supply);
+
+		if (error == 0 && opp_hz < curr_hz)
+			error = clock_set_frequency(ci->ci_node, NULL, opp_hz);
+		if (error == 0 && opp_microvolt != 0 &&
+		    opp_microvolt != curr_microvolt) {
+			error = regulator_set_voltage(ci->ci_cpu_supply,
+			    opp_microvolt);
+		}
+		if (error == 0 && opp_hz > curr_hz)
+			error = clock_set_frequency(ci->ci_node, NULL, opp_hz);
+
+		if (error)
+			printf("%s: DVFS failed\n", ci->ci_dev->dv_xname);
+	}
+}
+
+void
+cpu_opp_setperf(int level)
+{
+	struct cpu_info *ci;
+	CPU_INFO_ITERATOR cii;
+
+	CPU_INFO_FOREACH(cii, ci) {
+		struct opp_table *ot = ci->ci_opp_table;
+		uint64_t min, max;
+		uint64_t level_hz, opp_hz;
+		uint32_t opp_microvolt;
+		int opp_idx;
+		int i;
+
+		if (ot == NULL)
+			continue;
+
+		/* Skip if this table is shared and we're not the master. */
+		if (ot->ot_master && ot->ot_master != ci)
+			continue;
+
+		min = ot->ot_opp_hz_min;
+		max = ot->ot_opp_hz_max;
+		level_hz = min + (level * (max - min)) / 100;
+		opp_hz = min;
+		for (i = 0; i < ot->ot_nopp; i++) {
+			if (ot->ot_opp[i].opp_hz <= level_hz &&
+			    ot->ot_opp[i].opp_hz >= opp_hz) {
+				opp_hz = ot->ot_opp[i].opp_hz;
+				opp_microvolt = ot->ot_opp[i].opp_microvolt;
+				opp_idx = i;
+			}
+		}
+		KASSERT(opp_idx >= 0);
+
+		ci->ci_opp_idx = opp_idx;
+	}
+
+	/*
+	 * Update the hardware from a task since setting the
+	 * regulators might need process context.
+	 */
+	task_add(systq, &cpu_opp_task);
+}
diff --git a/sys/arch/arm64/include/cpu.h b/sys/arch/arm64/include/cpu.h
index 9c7f1549836..1731846a109 100644
--- a/sys/arch/arm64/include/cpu.h
+++ b/sys/arch/arm64/include/cpu.h
@@ -1,4 +1,4 @@
-/* $OpenBSD: cpu.h,v 1.9 2018/06/30 10:20:21 kettenis Exp $ */
+/* $OpenBSD: cpu.h,v 1.10 2018/08/03 16:45:17 kettenis Exp $ */
 /*
  * Copyright (c) 2016 Dale Rahn <drahn@dalerahn.com>
  *
@@ -107,6 +107,10 @@ struct cpu_info {
 
 	void			(*ci_flush_bp)(void);
 
+	struct opp_table	*ci_opp_table;
+	volatile int		ci_opp_idx;
+	uint32_t		ci_cpu_supply;
+
 #ifdef MULTIPROCESSOR
 	struct srp_hazard	ci_srp_hazards[SRP_HAZARD_NUM];
 	volatile int		ci_flags;