mpcore_wdt: Set correct watchdog rate

mpcore_wdt: Use notifiers to modify WD rate
mpcore_wdt: Stop watchdog of non-crashing cores
mpcore_wdt: don't sleep in atomic context

Signed-off-by: Robert Marklund <robert.marklund@stericsson.com>

1 files changed, 91 insertions, 5 deletions

diff --git a/drivers/watchdog/mpcore_wdt.c b/drivers/watchdog/mpcore_wdt.c
index 7c741dc987b..8d814e0470d 100644
--- a/drivers/watchdog/mpcore_wdt.c
+++ b/drivers/watchdog/mpcore_wdt.c
@@ -35,6 +35,8 @@
 #include <linux/uaccess.h>
 #include <linux/slab.h>
 #include <linux/io.h>
+#include <linux/cpufreq.h>
+#include <linux/kexec.h>
 
 #include <asm/smp_twd.h>
 
@@ -50,6 +52,8 @@ struct mpcore_wdt {
 static struct platform_device *mpcore_wdt_pdev;
 static DEFINE_SPINLOCK(wdt_lock);
 
+static DEFINE_PER_CPU(unsigned long, mpcore_wdt_rate);
+
 #define TIMER_MARGIN	60
 static int mpcore_margin = TIMER_MARGIN;
 module_param(mpcore_margin, int, 0);
@@ -70,6 +74,8 @@ MODULE_PARM_DESC(mpcore_noboot, "MPcore watchdog action, "
 	"set to 1 to ignore reboots, 0 to reboot (default="
 					__MODULE_STRING(ONLY_TESTING) ")");
 
+#define MPCORE_WDT_PERIPHCLK_PRESCALER 2
+
 /*
  *	This is the interrupt handler.  Note that we only use this
  *	in testing mode, so don't actually do a reboot here.
@@ -102,9 +108,8 @@ static void mpcore_wdt_keepalive(struct mpcore_wdt *wdt)
 
 	spin_lock(&wdt_lock);
 	/* Assume prescale is set to 256 */
-	count =  __raw_readl(wdt->base + TWD_WDOG_COUNTER);
-	count = (0xFFFFFFFFU - count) * (HZ / 5);
-	count = (count / 256) * mpcore_margin;
+	count = per_cpu(mpcore_wdt_rate, smp_processor_id()) / 256;
+	count = count*mpcore_margin;
 
 	/* Reload the counter */
 	writel(count + wdt->perturb, wdt->base + TWD_WDOG_LOAD);
@@ -112,6 +117,56 @@ static void mpcore_wdt_keepalive(struct mpcore_wdt *wdt)
 	spin_unlock(&wdt_lock);
 }
 
+static void mpcore_wdt_set_rate(unsigned long new_rate)
+{
+	unsigned long count;
+	unsigned long long rate_tmp;
+	unsigned long old_rate;
+
+	spin_lock(&wdt_lock);
+	old_rate = per_cpu(mpcore_wdt_rate, smp_processor_id());
+	per_cpu(mpcore_wdt_rate, smp_processor_id()) = new_rate;
+
+	if (mpcore_wdt_dev) {
+		struct mpcore_wdt *wdt = platform_get_drvdata(mpcore_wdt_dev);
+		count = readl(wdt->base + TWD_WDOG_COUNTER);
+		/* The goal: count = count * (new_rate/old_rate); */
+		rate_tmp = (unsigned long long)count * new_rate;
+		do_div(rate_tmp, old_rate);
+		count = rate_tmp;
+		writel(count + wdt->perturb, wdt->base + TWD_WDOG_LOAD);
+		wdt->perturb = wdt->perturb ? 0 : 1;
+	}
+	spin_unlock(&wdt_lock);
+}
+
+static void mpcore_wdt_update_cpu_frequency_on_cpu(void *data)
+{
+	struct cpufreq_freqs *freq = data;
+	mpcore_wdt_set_rate((freq->new * 1000) /
+			    MPCORE_WDT_PERIPHCLK_PRESCALER);
+}
+
+static int mpcore_wdt_cpufreq_notifier(struct notifier_block *nb,
+				       unsigned long event, void *data)
+{
+	struct cpufreq_freqs *freq = data;
+
+	if (event == CPUFREQ_RESUMECHANGE ||
+	    (event == CPUFREQ_PRECHANGE && freq->new > freq->old) ||
+	    (event == CPUFREQ_POSTCHANGE && freq->new < freq->old))
+		smp_call_function_single(freq->cpu,
+					 mpcore_wdt_update_cpu_frequency_on_cpu,
+					 freq, 1);
+
+	return 0;
+}
+
+static struct notifier_block mpcore_wdt_cpufreq_notifier_block = {
+	.notifier_call = mpcore_wdt_cpufreq_notifier,
+};
+
+
 static void mpcore_wdt_stop(struct mpcore_wdt *wdt)
 {
 	spin_lock(&wdt_lock);
@@ -146,6 +201,20 @@ static int mpcore_wdt_set_heartbeat(int t)
 	return 0;
 }
 
+static int mpcore_wdt_stop_notifier(struct notifier_block *this,
+				unsigned long event, void *ptr)
+{
+	struct mpcore_wdt *wdt = platform_get_drvdata(mpcore_wdt_dev);
+	printk(KERN_INFO "Stopping watchdog on non-crashing core %u\n",
+	       smp_processor_id());
+	mpcore_wdt_stop(wdt);
+	return NOTIFY_STOP;
+}
+
+static struct notifier_block mpcore_wdt_stop_block = {
+	.notifier_call = mpcore_wdt_stop_notifier,
+};
+
 /*
  *	/dev/watchdog handling
  */
@@ -153,7 +222,7 @@ static int mpcore_wdt_open(struct inode *inode, struct file *file)
 {
 	struct mpcore_wdt *wdt = platform_get_drvdata(mpcore_wdt_pdev);
 
-	if (test_and_set_bit(0, &wdt->timer_alive))
+	if (cpumask_test_and_set_cpu(smp_processor_id(), &wdt->timer_alive))
 		return -EBUSY;
 
 	if (nowayout)
@@ -161,6 +230,9 @@ static int mpcore_wdt_open(struct inode *inode, struct file *file)
 
 	file->private_data = wdt;
 
+	atomic_notifier_chain_register(&crash_percpu_notifier_list,
+				       &mpcore_wdt_stop_block);
+
 	/*
 	 *	Activate timer
 	 */
@@ -184,7 +256,7 @@ static int mpcore_wdt_release(struct inode *inode, struct file *file)
 				"unexpected close, not stopping watchdog!\n");
 		mpcore_wdt_keepalive(wdt);
 	}
-	clear_bit(0, &wdt->timer_alive);
+	cpumask_clear_cpu(smp_processor_id(), &wdt->timer_alive);
 	wdt->expect_close = 0;
 	return 0;
 }
@@ -427,6 +499,8 @@ static struct platform_driver mpcore_wdt_driver = {
 
 static int __init mpcore_wdt_init(void)
 {
+	int i;
+
 	/*
 	 * Check that the margin value is within it's range;
 	 * if not reset to the default
@@ -437,6 +511,18 @@ static int __init mpcore_wdt_init(void)
 			TIMER_MARGIN);
 	}
 
+	cpufreq_register_notifier(&mpcore_wdt_cpufreq_notifier_block,
+				  CPUFREQ_TRANSITION_NOTIFIER);
+
+	for_each_online_cpu(i)
+		per_cpu(mpcore_wdt_rate, i) =
+		(cpufreq_get(i) * 1000) / MPCORE_WDT_PERIPHCLK_PRESCALER;
+
+	for_each_online_cpu(i)
+		pr_info("mpcore_wdt: rate for core %d is %lu.%02luMHz.\n", i,
+		       per_cpu(mpcore_wdt_rate, i) / 1000000,
+		       (per_cpu(mpcore_wdt_rate, i) / 10000) % 100);
+
 	pr_info("MPcore Watchdog Timer: 0.1. mpcore_noboot=%d mpcore_margin=%d sec (nowayout= %d)\n",
 		mpcore_noboot, mpcore_margin, nowayout);