|
@@ -12,6 +12,7 @@
|
|
|
|
|
|
#include <linux/errno.h>
|
|
|
#include <asm/cacheflush.h>
|
|
|
+#include <asm/cp15.h>
|
|
|
#include <mach/common.h>
|
|
|
|
|
|
int platform_cpu_kill(unsigned int cpu)
|
|
@@ -19,6 +20,44 @@ int platform_cpu_kill(unsigned int cpu)
|
|
|
return 1;
|
|
|
}
|
|
|
|
|
|
+static inline void cpu_enter_lowpower(void)
|
|
|
+{
|
|
|
+ unsigned int v;
|
|
|
+
|
|
|
+ flush_cache_all();
|
|
|
+ asm volatile(
|
|
|
+ "mcr p15, 0, %1, c7, c5, 0\n"
|
|
|
+ " mcr p15, 0, %1, c7, c10, 4\n"
|
|
|
+ /*
|
|
|
+ * Turn off coherency
|
|
|
+ */
|
|
|
+ " mrc p15, 0, %0, c1, c0, 1\n"
|
|
|
+ " bic %0, %0, %3\n"
|
|
|
+ " mcr p15, 0, %0, c1, c0, 1\n"
|
|
|
+ " mrc p15, 0, %0, c1, c0, 0\n"
|
|
|
+ " bic %0, %0, %2\n"
|
|
|
+ " mcr p15, 0, %0, c1, c0, 0\n"
|
|
|
+ : "=&r" (v)
|
|
|
+ : "r" (0), "Ir" (CR_C), "Ir" (0x40)
|
|
|
+ : "cc");
|
|
|
+}
|
|
|
+
|
|
|
+static inline void cpu_leave_lowpower(void)
|
|
|
+{
|
|
|
+ unsigned int v;
|
|
|
+
|
|
|
+ asm volatile(
|
|
|
+ "mrc p15, 0, %0, c1, c0, 0\n"
|
|
|
+ " orr %0, %0, %1\n"
|
|
|
+ " mcr p15, 0, %0, c1, c0, 0\n"
|
|
|
+ " mrc p15, 0, %0, c1, c0, 1\n"
|
|
|
+ " orr %0, %0, %2\n"
|
|
|
+ " mcr p15, 0, %0, c1, c0, 1\n"
|
|
|
+ : "=&r" (v)
|
|
|
+ : "Ir" (CR_C), "Ir" (0x40)
|
|
|
+ : "cc");
|
|
|
+}
|
|
|
+
|
|
|
/*
|
|
|
* platform-specific code to shutdown a CPU
|
|
|
*
|
|
@@ -26,9 +65,10 @@ int platform_cpu_kill(unsigned int cpu)
|
|
|
*/
|
|
|
void platform_cpu_die(unsigned int cpu)
|
|
|
{
|
|
|
- flush_cache_all();
|
|
|
+ cpu_enter_lowpower();
|
|
|
imx_enable_cpu(cpu, false);
|
|
|
cpu_do_idle();
|
|
|
+ cpu_leave_lowpower();
|
|
|
|
|
|
/* We should never return from idle */
|
|
|
panic("cpu %d unexpectedly exit from shutdown\n", cpu);
|