ARM: pm: add function to set WFI low-power mode for SMP CPUs

Add a function to set the SCU low-power mode for SMP CPUs.  This
centralizes this functionality rather than having to expose the
SCU register definitions to each platform.

Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

diff --git a/arch/arm/include/asm/smp_scu.h b/arch/arm/include/asm/smp_scu.h
index 2376835015d62ce991abe4aef1f5946ac19707f5..4eb6d005ffaa4b625c9f86fc8532b046d0cf1f6a 100644 (file)
--- a/arch/arm/include/asm/smp_scu.h
+++ b/arch/arm/include/asm/smp_scu.h
@@ -1,7 +1,14 @@
 #ifndef __ASMARM_ARCH_SCU_H
 #define __ASMARM_ARCH_SCU_H
 
+#define SCU_PM_NORMAL  0
+#define SCU_PM_DORMANT 2
+#define SCU_PM_POWEROFF        3
+
+#ifndef __ASSEMBLER__
 unsigned int scu_get_core_count(void __iomem *);
 void scu_enable(void __iomem *);
+int scu_power_mode(void __iomem *, unsigned int);
+#endif
 
 #endif

diff --git a/arch/arm/kernel/smp_scu.c b/arch/arm/kernel/smp_scu.c
index 9ab4149bd9830883f88d2b1d20739c981a344551..a1e757c3439bcb8155f5f93f940788854fd364d0 100644 (file)
--- a/arch/arm/kernel/smp_scu.c
+++ b/arch/arm/kernel/smp_scu.c
@@ -50,3 +50,26 @@ void __init scu_enable(void __iomem *scu_base)
         */
        flush_cache_all();
 }
+
+/*
+ * Set the executing CPUs power mode as defined.  This will be in
+ * preparation for it executing a WFI instruction.
+ *
+ * This function must be called with preemption disabled, and as it
+ * has the side effect of disabling coherency, caches must have been
+ * flushed.  Interrupts must also have been disabled.
+ */
+int scu_power_mode(void __iomem *scu_base, unsigned int mode)
+{
+       unsigned int val;
+       int cpu = smp_processor_id();
+
+       if (mode > 3 || mode == 1 || cpu > 3)
+               return -EINVAL;
+
+       val = __raw_readb(scu_base + SCU_CPU_STATUS + cpu) & ~0x03;
+       val |= mode;
+       __raw_writeb(val, scu_base + SCU_CPU_STATUS + cpu);
+
+       return 0;
+}