Merge tag 'firewire-updates' of git://git.kernel.org/pub/scm/linux/kernel/git/ieee139...

[linux-2.6-microblaze.git] / drivers / clk / at91 / pmc.c

diff --git a/drivers/clk/at91/pmc.c b/drivers/clk/at91/pmc.c
index 775af47..1fa27f4 100644 (file)
--- a/drivers/clk/at91/pmc.c
+++ b/drivers/clk/at91/pmc.c
@@ -22,6 +22,7 @@
 #include "pmc.h"
 
 #define PMC_MAX_IDS 128
+#define PMC_MAX_PCKS 8
 
 int of_at91_get_clk_range(struct device_node *np, const char *propname,
                          struct clk_range *range)
@@ -50,6 +51,7 @@ EXPORT_SYMBOL_GPL(of_at91_get_clk_range);
 static struct regmap *pmcreg;
 
 static u8 registered_ids[PMC_MAX_IDS];
+static u8 registered_pcks[PMC_MAX_PCKS];
 
 static struct
 {
@@ -66,8 +68,13 @@ static struct
        u32 pcr[PMC_MAX_IDS];
        u32 audio_pll0;
        u32 audio_pll1;
+       u32 pckr[PMC_MAX_PCKS];
 } pmc_cache;
 
+/*
+ * As Peripheral ID 0 is invalid on AT91 chips, the identifier is stored
+ * without alteration in the table, and 0 is for unused clocks.
+ */
 void pmc_register_id(u8 id)
 {
        int i;
@@ -82,11 +89,30 @@ void pmc_register_id(u8 id)
        }
 }
 
+/*
+ * As Programmable Clock 0 is valid on AT91 chips, there is an offset
+ * of 1 between the stored value and the real clock ID.
+ */
+void pmc_register_pck(u8 pck)
+{
+       int i;
+
+       for (i = 0; i < PMC_MAX_PCKS; i++) {
+               if (registered_pcks[i] == 0) {
+                       registered_pcks[i] = pck + 1;
+                       break;
+               }
+               if (registered_pcks[i] == (pck + 1))
+                       break;
+       }
+}
+
 static int pmc_suspend(void)
 {
        int i;
+       u8 num;
 
-       regmap_read(pmcreg, AT91_PMC_IMR, &pmc_cache.scsr);
+       regmap_read(pmcreg, AT91_PMC_SCSR, &pmc_cache.scsr);
        regmap_read(pmcreg, AT91_PMC_PCSR, &pmc_cache.pcsr0);
        regmap_read(pmcreg, AT91_CKGR_UCKR, &pmc_cache.uckr);
        regmap_read(pmcreg, AT91_CKGR_MOR, &pmc_cache.mor);
@@ -103,14 +129,29 @@ static int pmc_suspend(void)
                regmap_read(pmcreg, AT91_PMC_PCR,
                            &pmc_cache.pcr[registered_ids[i]]);
        }
+       for (i = 0; registered_pcks[i]; i++) {
+               num = registered_pcks[i] - 1;
+               regmap_read(pmcreg, AT91_PMC_PCKR(num), &pmc_cache.pckr[num]);
+       }
 
        return 0;
 }
 
+static bool pmc_ready(unsigned int mask)
+{
+       unsigned int status;
+
+       regmap_read(pmcreg, AT91_PMC_SR, &status);
+
+       return ((status & mask) == mask) ? 1 : 0;
+}
+
 static void pmc_resume(void)
 {
-       int i, ret = 0;
+       int i;
+       u8 num;
        u32 tmp;
+       u32 mask = AT91_PMC_MCKRDY | AT91_PMC_LOCKA;
 
        regmap_read(pmcreg, AT91_PMC_MCKR, &tmp);
        if (pmc_cache.mckr != tmp)
@@ -119,7 +160,7 @@ static void pmc_resume(void)
        if (pmc_cache.pllar != tmp)
                pr_warn("PLLAR was not configured properly by the firmware\n");
 
-       regmap_write(pmcreg, AT91_PMC_IMR, pmc_cache.scsr);
+       regmap_write(pmcreg, AT91_PMC_SCER, pmc_cache.scsr);
        regmap_write(pmcreg, AT91_PMC_PCER, pmc_cache.pcsr0);
        regmap_write(pmcreg, AT91_CKGR_UCKR, pmc_cache.uckr);
        regmap_write(pmcreg, AT91_CKGR_MOR, pmc_cache.mor);
@@ -133,14 +174,16 @@ static void pmc_resume(void)
                             pmc_cache.pcr[registered_ids[i]] |
                             AT91_PMC_PCR_CMD);
        }
-
-       if (pmc_cache.uckr & AT91_PMC_UPLLEN) {
-               ret = regmap_read_poll_timeout(pmcreg, AT91_PMC_SR, tmp,
-                                              !(tmp & AT91_PMC_LOCKU),
-                                              10, 5000);
-               if (ret)
-                       pr_crit("USB PLL didn't lock when resuming\n");
+       for (i = 0; registered_pcks[i]; i++) {
+               num = registered_pcks[i] - 1;
+               regmap_write(pmcreg, AT91_PMC_PCKR(num), pmc_cache.pckr[num]);
        }
+
+       if (pmc_cache.uckr & AT91_PMC_UPLLEN)
+               mask |= AT91_PMC_LOCKU;
+
+       while (!pmc_ready(mask))
+               cpu_relax();
 }
 
 static struct syscore_ops pmc_syscore_ops = {