1 files changed, 71 insertions, 4 deletions
diff --git a/arch/arm/mach-mx6/mx6_wfi.S b/arch/arm/mach-mx6/mx6_wfi.S
index 3c6310263b38..6e2972faa293 100644
--- a/arch/arm/mach-mx6/mx6_wfi.S
+++ b/arch/arm/mach-mx6/mx6_wfi.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2010-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -17,6 +17,7 @@
  */
 
 #include <linux/linkage.h>
+#include <mach/hardware.h>
 
 /*
  *  mx6_wait
@@ -27,14 +28,80 @@
  */
 ENTRY(mx6_wait)
 
+    push {r4, r5, r6}
+
+      ldr     r2, =ANATOP_BASE_ADDR
+      add   r2, r2, #PERIPBASE_VIRT
+
+    /* get the flags variables into the cache */
+	ldr     r3, [r0]
+
+    /* get CPU ID */
+    mrc     p15,0,r5,c0,c0,5
+    and     r5, r5, #0x3
+
+    mov r4,#0xff
+    strb r4,[r0,r5]
+
+    dsb
+
+    mvn r4, #0x0
+    ldr r3, [r0]
+    cmp r3, r4
+    bne DO_WFI
+
+    mov r4, #0x1
+    ldrex r3, [r1]
+    cmp r3, #0x0
+    strexeq r3, r4, [r1]
+    cmpeq r3, #0x0
+    bne DO_WFI
+
+    mov r3, #0xff
+
+    ldr     r6, =(1 << 16)
+    str     r6, [r2, #0x04]
+
+    /* dmb */
+
+    str r3, [r1]
+
+    dsb
+
+    mvn r4, #0x0
+    ldr r3, [r0]
+    cmp r3, r4
+    movne r3, #0x0
+    strne r6, [r2, #0x08]
+    strne r3, [r1]
+
+DO_WFI:
 	dsb
 
 	wfi
 
- 	isb
-	isb
+    mov r4, #0x0
+    strb r4, [r0, r5]
+
+    dsb
+
+    ldr r3, [r1]
+    cmp r3, #0xff
+    bne DONE
+
+    mov r4, #0x0
+    ldr     r6, =(1 << 16)
+    str r6, [r2, #0x08]
+
+    mov r3, #0x0
+    str r3, [r1]
+
+DONE:
+
+    pop {r4,r5, r6}
 
-      mov     pc, lr
+    /* Restore registers */
+    mov     pc, lr
 
     .type   mx6_do_wait, #object
 ENTRY(mx6_do_wait)