Merge branch 'davem-fixes' of master.kernel.org:/pub/scm/linux/kernel/git/jgarzik...

[linux-2.6] / include / asm-blackfin / mach-bf548 / cdefBF54x_base.h

diff --git a/include/asm-blackfin/mach-bf548/cdefBF54x_base.h b/include/asm-blackfin/mach-bf548/cdefBF54x_base.h
index 33c67500717cbe5a3ebab6f06c1308559481e49c..57ac8cb9b1f6be7c968aa28b4e3ca2fbee284d7e 100644 (file)
--- a/include/asm-blackfin/mach-bf548/cdefBF54x_base.h
+++ b/include/asm-blackfin/mach-bf548/cdefBF54x_base.h
@@ -43,7 +43,33 @@
 /* PLL Registers */
 
 #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, iwr2;
+
+       if (val == bfin_read_PLL_CTL())
+               return;
+
+       local_irq_save(flags);
+       /* Enable the PLL Wakeup bit in SIC IWR */
+       iwr0 = bfin_read32(SIC_IWR0);
+       iwr1 = bfin_read32(SIC_IWR1);
+       iwr2 = bfin_read32(SIC_IWR2);
+       /* Only allow PPL Wakeup) */
+       bfin_write32(SIC_IWR0, IWR_ENABLE(0));
+       bfin_write32(SIC_IWR1, 0);
+       bfin_write32(SIC_IWR2, 0);
+
+       bfin_write16(PLL_CTL, val);
+       SSYNC();
+       asm("IDLE;");
+
+       bfin_write32(SIC_IWR0, iwr0);
+       bfin_write32(SIC_IWR1, iwr1);
+       bfin_write32(SIC_IWR2, iwr2);
+       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)
@@ -52,6 +78,10 @@ static __inline__ void bfin_write_VR_CTL(unsigned int val)
 {
        unsigned long flags, iwr0, iwr1, iwr2;
 
+       if (val == bfin_read_VR_CTL())
+               return;
+
+       local_irq_save(flags);
        /* Enable the PLL Wakeup bit in SIC IWR */
        iwr0 = bfin_read32(SIC_IWR0);
        iwr1 = bfin_read32(SIC_IWR1);
@@ -63,13 +93,12 @@ 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(SIC_IWR0, iwr0);
        bfin_write32(SIC_IWR1, iwr1);
        bfin_write32(SIC_IWR2, iwr2);
+       local_irq_restore(flags);
 }
 #define bfin_read_PLL_STAT()           bfin_read16(PLL_STAT)
 #define bfin_write_PLL_STAT(val)       bfin_write16(PLL_STAT, val)