1 files changed, 30 insertions, 4 deletions

diff --git a/include/asm-blackfin/mach-bf561/cdefBF561.h b/include/asm-blackfin/mach-bf561/cdefBF561.h
index 1bc8d2f89ccc..b07ffccd66dd 100644
--- a/include/asm-blackfin/mach-bf561/cdefBF561.h
+++ b/include/asm-blackfin/mach-bf561/cdefBF561.h
@@ -47,7 +47,30 @@
 
 /* Clock and System Control (0xFFC00000 - 0xFFC000FF) */
 #define bfin_read_PLL_CTL()                  bfin_read16(PLL_CTL)
-#define bfin_write_PLL_CTL(val)              bfin_write16(PLL_CTL,val)
+/* Writing to PLL_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_PLL_CTL(unsigned int val)
+{
+        unsigned long flags, iwr0, iwr1;
+
+        if (val == bfin_read_PLL_CTL())
+                return;
+
+        local_irq_save(flags);
+        /* Enable the PLL Wakeup bit in SIC IWR */
+        iwr0 = bfin_read32(SICA_IWR0);
+        iwr1 = bfin_read32(SICA_IWR1);
+        /* Only allow PPL Wakeup) */
+        bfin_write32(SICA_IWR0, IWR_ENABLE(0));
+        bfin_write32(SICA_IWR1, 0);
+
+        bfin_write16(PLL_CTL, val);
+        SSYNC();
+        asm("IDLE;");
+
+        bfin_write32(SICA_IWR0, iwr0);
+        bfin_write32(SICA_IWR1, iwr1);
+        local_irq_restore(flags);
+}
 #define bfin_read_PLL_DIV()                  bfin_read16(PLL_DIV)
 #define bfin_write_PLL_DIV(val)              bfin_write16(PLL_DIV,val)
 #define bfin_read_VR_CTL()                   bfin_read16(VR_CTL)
@@ -56,6 +79,10 @@ static __inline__ void bfin_write_VR_CTL(unsigned int val)
 {
         unsigned long flags, iwr0, iwr1;
 
+        if (val == bfin_read_VR_CTL())
+                return;
+
+        local_irq_save(flags);
         /* Enable the PLL Wakeup bit in SIC IWR */
         iwr0 = bfin_read32(SICA_IWR0);
         iwr1 = bfin_read32(SICA_IWR1);
@@ -65,12 +92,11 @@ static __inline__ void bfin_write_VR_CTL(unsigned int val)
 
         bfin_write16(VR_CTL, val);
         SSYNC();
-
-        local_irq_save(flags);
         asm("IDLE;");
-        local_irq_restore(flags);
+
         bfin_write32(SICA_IWR0, iwr0);
         bfin_write32(SICA_IWR1, iwr1);
+        local_irq_restore(flags);
 }
 #define bfin_read_PLL_STAT()                 bfin_read16(PLL_STAT)
 #define bfin_write_PLL_STAT(val)             bfin_write16(PLL_STAT,val)