[PATCH] cpufreq, highbank: enable ECME thermal notifications

[Mark Langsdorf]

The ECME sends thermal messages with a maximum and minimum allowed
frequency when the SoC status reaches certain trip points known to the
ECME. Use a notifier function to capture those messages and pass them
to a work-queued function that can trigger a policy re-evaluation by
cpufreq, capping the allowable frequencies.

The core of the policy adjusting code was taken from
drivers/thermal/cpu_cooling.c.

Signed-off-by: Mark Langsdorf <mark.langsdorf at calxeda.com>
---
 drivers/cpufreq/highbank-cpufreq.c | 117 +++++++++++++++++++++++++++++++++++++
 1 file changed, 117 insertions(+)

diff --git a/drivers/cpufreq/highbank-cpufreq.c b/drivers/cpufreq/highbank-cpufreq.c
index b61b5a3..f0521d3 100644
--- a/drivers/cpufreq/highbank-cpufreq.c
+++ b/drivers/cpufreq/highbank-cpufreq.c
@@ -17,15 +17,32 @@
 #include <linux/module.h>
 #include <linux/clk.h>
 #include <linux/cpu.h>
+#include <linux/cpufreq.h>
 #include <linux/err.h>
 #include <linux/of.h>
 #include <linux/mailbox.h>
 #include <linux/platform_device.h>
+#include <linux/cpumask.h>
+#include <linux/workqueue.h>
 
 #define HB_CPUFREQ_CHANGE_NOTE	0x80000001
+#define HB_CPUFREQ_HEALTH_NOTE  0x00001001
+#define HB_CPUFREQ_TPS_REPORT	1
 #define HB_CPUFREQ_IPC_LEN	7
 #define HB_CPUFREQ_VOLT_RETRIES	15
 
+#define NOTIFY_INVALID NULL
+
+struct hb_notify_device {
+	unsigned long max_freq;
+	unsigned long min_freq;
+	unsigned long thermal_state;
+	struct cpumask *allowed_cpus;
+};
+
+static struct hb_notify_device hb_records, *notify_device = NOTIFY_INVALID;
+static struct work_struct hb_thermal_wq;
+
 static int hb_voltage_change(unsigned int freq)
 {
 	u32 msg[HB_CPUFREQ_IPC_LEN] = {HB_CPUFREQ_CHANGE_NOTE, freq / 1000000};
@@ -33,6 +50,89 @@ static int hb_voltage_change(unsigned int freq)
 	return pl320_ipc_transmit(msg);
 }
 
+static int hb_thermal_cpufreq_notify(struct notifier_block *nb,
+				    unsigned long event, void *data)
+{
+	struct cpufreq_policy *policy = data;
+	unsigned long max_freq = 0, min_freq = 0;
+
+	if (event != CPUFREQ_ADJUST || notify_device == NOTIFY_INVALID)
+		return 0;
+
+	if (cpumask_test_cpu(policy->cpu, notify_device->allowed_cpus)) {
+		max_freq = notify_device->max_freq;
+		min_freq = notify_device->min_freq;
+	}
+
+	/* Never exceed user_policy.max */
+	if (max_freq > policy->user_policy.max)
+		max_freq = policy->user_policy.max;
+	if (min_freq < policy->user_policy.min)
+		min_freq = policy->user_policy.min;
+
+	if ((policy->max != max_freq) || (policy->min != min_freq))
+		cpufreq_verify_within_limits(policy, min_freq, max_freq);
+
+	return 0;
+}
+
+static struct notifier_block hb_thermal_cpufreq_nb = {
+	.notifier_call = hb_thermal_cpufreq_notify,
+};
+
+/*
+ * We can't call cpufreq_adjust_policy from inside a notifier, so
+ * do it from inside a workqueue
+ */
+static void hb_thermal_wq_task(struct work_struct *work)
+{
+	unsigned int cpuid;
+	struct cpumask *mask = hb_records.allowed_cpus;
+
+	notify_device = &hb_records;
+
+	for_each_cpu(cpuid, mask) {
+		struct cpufreq_policy policy;
+		if (cpufreq_get_policy(&policy, cpuid) == 0) {
+			cpufreq_update_policy(cpuid);
+			break;
+		}
+	}
+	notify_device = NOTIFY_INVALID;
+}
+
+static int hb_thermal_tps_notify(struct notifier_block *nb,
+				unsigned long action, void *mailbox)
+{
+	unsigned long *data = (unsigned long *) mailbox;
+
+	if ((action != HB_CPUFREQ_HEALTH_NOTE) &&
+			(data[0] != HB_CPUFREQ_TPS_REPORT))
+		return 0;
+
+	/*
+	 * If we're already at this thermal state, or if ECME isn't
+	 * sending frequency data, there's nothing to do
+	 */
+	if ((hb_records.thermal_state == data[1]) || !data[2] || !data[3])
+		return NOTIFY_DONE;
+
+	hb_records.thermal_state = data[1];
+	hb_records.min_freq = data[2];
+	hb_records.max_freq = data[3];
+	notify_device = &hb_records;
+
+	schedule_work(&hb_thermal_wq);
+
+	pr_warn("ECME TPS event: frequencies limited to %lu-%lu MHz\n",
+	       hb_records.min_freq, hb_records.max_freq);
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block hb_thermal_tps_nb = {
+	.notifier_call = hb_thermal_tps_notify,
+};
+
 static int hb_cpufreq_clk_notify(struct notifier_block *nb,
 				unsigned long action, void *hclk)
 {
@@ -100,6 +200,23 @@ static int hb_cpufreq_driver_init(void)
 		goto out_put_node;
 	}
 
+	/*
+	 * Set up thermal state notifications from ECME, but continue
+	 * running even if they don't work.
+	 */
+	ret = pl320_ipc_register_notifier(&hb_thermal_tps_nb);
+	if (!ret) {
+		ret = cpufreq_register_notifier(&hb_thermal_cpufreq_nb,
+				CPUFREQ_POLICY_NOTIFIER);
+		if (ret)
+			pl320_ipc_unregister_notifier(&hb_thermal_tps_nb);
+		else {
+			hb_records.allowed_cpus = (struct cpumask *)
+						cpu_possible_mask;
+			INIT_WORK(&hb_thermal_wq, hb_thermal_wq_task);
+		}
+	}
+
 	/* Instantiate cpufreq-cpu0 */
 	platform_device_register_full(&devinfo);
 
-- 
1.8.3.1