clk: at91: modify PMC peripheral clock to deal with newer register layout

As some more information is added to the PCR register, we'd better use
a copy of its content and modify just the peripheral-related bits.
Implement a read-modify-write for the enable() and disable() callbacks.

Signed-off-by: Nicolas Ferre <nicolas.ferre@atmel.com>
Signed-off-by: Boris Brezillon <boris.brezillon@free-electrons.com>
Signed-off-by: Stephen Boyd <sboyd@codeaurora.org>

diff --git a/drivers/clk/at91/clk-peripheral.c b/drivers/clk/at91/clk-peripheral.c
index 0a66b959f99b9723fd72c08ea8bfd9b9044d95b1..58f3b568e9cb9024beaf0831c9ac3e0237a93d03 100644 (file)
--- a/drivers/clk/at91/clk-peripheral.c
+++ b/drivers/clk/at91/clk-peripheral.c
@@ -161,14 +161,18 @@ static int clk_sam9x5_peripheral_enable(struct clk_hw *hw)
 {
        struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
        struct at91_pmc *pmc = periph->pmc;
+       u32 tmp;
 
        if (periph->id < PERIPHERAL_ID_MIN)
                return 0;
 
-       pmc_write(pmc, AT91_PMC_PCR, (periph->id & AT91_PMC_PCR_PID_MASK) |
-                                    AT91_PMC_PCR_CMD |
-                                    AT91_PMC_PCR_DIV(periph->div) |
-                                    AT91_PMC_PCR_EN);
+       pmc_lock(pmc);
+       pmc_write(pmc, AT91_PMC_PCR, (periph->id & AT91_PMC_PCR_PID_MASK));
+       tmp = pmc_read(pmc, AT91_PMC_PCR) & ~AT91_PMC_PCR_DIV_MASK;
+       pmc_write(pmc, AT91_PMC_PCR, tmp | AT91_PMC_PCR_DIV(periph->div)
+                                        | AT91_PMC_PCR_CMD
+                                        | AT91_PMC_PCR_EN);
+       pmc_unlock(pmc);
        return 0;
 }
 
@@ -176,12 +180,16 @@ static void clk_sam9x5_peripheral_disable(struct clk_hw *hw)
 {
        struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
        struct at91_pmc *pmc = periph->pmc;
+       u32 tmp;
 
        if (periph->id < PERIPHERAL_ID_MIN)
                return;
 
-       pmc_write(pmc, AT91_PMC_PCR, (periph->id & AT91_PMC_PCR_PID_MASK) |
-                                    AT91_PMC_PCR_CMD);
+       pmc_lock(pmc);
+       pmc_write(pmc, AT91_PMC_PCR, (periph->id & AT91_PMC_PCR_PID_MASK));
+       tmp = pmc_read(pmc, AT91_PMC_PCR) & ~AT91_PMC_PCR_EN;
+       pmc_write(pmc, AT91_PMC_PCR, tmp | AT91_PMC_PCR_CMD);
+       pmc_unlock(pmc);
 }
 
 static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw)