at91: when turning off the plls during suspend, don't wait for the lock

flag to be set, instead wait for it to be unset. Previously the code would
always run into the loop limitation of 1000 iterations because the flag is
never set when turning the plls off.

Signed-off-by: Julien Langer <julien.langer@gmail.com>
---

 arch/arm/mach-at91/pm_slowclock.S |   35 ++++++++++++++++++++++++++++++++---
 1 files changed, 32 insertions(+), 3 deletions(-)


diff --git a/arch/arm/mach-at91/pm_slowclock.S b/arch/arm/mach-at91/pm_slowclock.S
index 987fab3..dc6d730 100644
--- a/arch/arm/mach-at91/pm_slowclock.S
+++ b/arch/arm/mach-at91/pm_slowclock.S
@@ -45,7 +45,9 @@
 #define MCKRDY_TIMEOUT		1000
 #define MOSCRDY_TIMEOUT 	1000
 #define PLLALOCK_TIMEOUT	1000
+#define PLLAUNLOCK_TIMEOUT	1000
 #define PLLBLOCK_TIMEOUT	1000
+#define PLLBUNLOCK_TIMEOUT	1000
 
 
 /*
@@ -91,6 +93,20 @@
 	.endm
 
 /*
+ * Wait until PLLA has unlocked.
+ */
+	.macro wait_pllaunlock
+	mov	r4, #PLLAUNLOCK_TIMEOUT
+1:	sub	r4, r4, #1
+	cmp	r4, #0
+	beq	2f
+	ldr	r3, [r1, #(AT91_PMC_SR - AT91_PMC)]
+	tst	r3, #AT91_PMC_LOCKA
+	bne	1b
+2:
+	.endm
+
+/*
  * Wait until PLLB has locked.
  */
 	.macro wait_pllblock
@@ -104,6 +120,20 @@
 2:
 	.endm
 
+/*
+ * Wait until PLLB has unlocked.
+ */
+	.macro wait_pllbunlock
+	mov	r4, #PLLBUNLOCK_TIMEOUT
+1:	sub	r4, r4, #1
+	cmp	r4, #0
+	beq	2f
+	ldr	r3, [r1, #(AT91_PMC_SR - AT91_PMC)]
+	tst	r3, #AT91_PMC_LOCKB
+	bne	1b
+2:
+	.endm
+
 	.text
 
 ENTRY(at91_slow_clock)
@@ -175,7 +205,7 @@ ENTRY(at91_slow_clock)
 	orr	r3, r3, #(1 << 29)		/* bit 29 always set */
 	str	r3, [r1, #(AT91_CKGR_PLLAR - AT91_PMC)]
 
-	wait_pllalock
+	wait_pllaunlock
 
 	/* Save PLLB setting and disable it */
 	ldr	r3, [r1, #(AT91_CKGR_PLLBR - AT91_PMC)]
@@ -183,8 +213,7 @@ ENTRY(at91_slow_clock)
 
 	mov	r3, #AT91_PMC_PLLCOUNT
 	str	r3, [r1, #(AT91_CKGR_PLLBR - AT91_PMC)]
-
-	wait_pllblock
+	wait_pllbunlock
 
 	/* Turn off the main oscillator */
 	ldr	r3, [r1, #(AT91_CKGR_MOR - AT91_PMC)]