arm: zynq: slcr: Use read-modify-write for register writes

zynq_slcr_cpu_start/stop() ignored the current register state when
writing to a register. Fixing this by implementing proper
read-modify-write.

Signed-off-by: Soren Brinkmann <soren.brinkmann@xilinx.com>
Signed-off-by: Michal Simek <michal.simek@xilinx.com>

diff --git a/arch/arm/mach-zynq/slcr.c b/arch/arm/mach-zynq/slcr.c
index 3af4b86c7028a35c8e40c4d18e0a00fe5c766d82..3c688da69bf537688d3e5a94b8e51e90ed633d33 100644 (file)
--- a/arch/arm/mach-zynq/slcr.c
+++ b/arch/arm/mach-zynq/slcr.c
@@ -2055,11 +2055,12 @@ module_init(xslcr_arch_init);
  */
 void zynq_slcr_cpu_start(int cpu)
 {
-       /* enable CPUn */
-       writel(SLCR_A9_CPU_CLKSTOP << cpu,
-              zynq_slcr_base + SLCR_A9_CPU_RST_CTRL);
-       /* enable CLK for CPUn */
-       writel(0x0 << cpu, zynq_slcr_base + SLCR_A9_CPU_RST_CTRL);
+       u32 reg = readl(zynq_slcr_base + SLCR_A9_CPU_RST_CTRL);
+
+       reg &= ~(SLCR_A9_CPU_RST << cpu);
+       writel(reg, zynq_slcr_base + SLCR_A9_CPU_RST_CTRL);
+       reg &= ~(SLCR_A9_CPU_CLKSTOP << cpu);
+       writel(reg, zynq_slcr_base + SLCR_A9_CPU_RST_CTRL);
 }
 
 /**
@@ -2068,9 +2069,9 @@ void zynq_slcr_cpu_start(int cpu)
  */
 void zynq_slcr_cpu_stop(int cpu)
 {
-       /* stop CLK and reset CPUn */
-       writel((SLCR_A9_CPU_CLKSTOP | SLCR_A9_CPU_RST) << cpu,
-              zynq_slcr_base + SLCR_A9_CPU_RST_CTRL);
+       u32 reg = readl(zynq_slcr_base + SLCR_A9_CPU_RST_CTRL);
+       reg |= (SLCR_A9_CPU_CLKSTOP | SLCR_A9_CPU_RST) << cpu;
+       writel(reg, zynq_slcr_base + SLCR_A9_CPU_RST_CTRL);
 }
 
 /**