From 729d39cc089854892e2fb91aefd28024e6bf4a20 Mon Sep 17 00:00:00 2001
From: =?utf8?q?=E9=BB=84=E6=B6=9B?= <huangtao@rock-chips.com>
Date: Fri, 11 Feb 2011 18:56:20 +0800
Subject: [PATCH] rk29: work around CRU_CLKGATE3_CON bit21~20 &
 CRU_SOFTRST0_CON bit29~27 bug

---
 arch/arm/mach-rk29/clock.c            | 62 ++++++++++++++++++++++++---
 arch/arm/mach-rk29/include/mach/cru.h | 20 +--------
 2 files changed, 58 insertions(+), 24 deletions(-)

diff --git a/arch/arm/mach-rk29/clock.c b/arch/arm/mach-rk29/clock.c
index 2cf337024b7d..3d5959da06df 100755
--- a/arch/arm/mach-rk29/clock.c
+++ b/arch/arm/mach-rk29/clock.c
@@ -179,6 +179,9 @@ static int clksel_set_parent(struct clk *clk, struct clk *parent)
 	return -EINVAL;
 }
 
+/* Work around CRU_CLKGATE3_CON bit21~20 bug */
+static volatile u32 cru_clkgate3_con_mirror;
+
 static int gate_mode(struct clk *clk, int on)
 {
 	u32 reg;
@@ -192,17 +195,55 @@ static int gate_mode(struct clk *clk, int on)
 	reg += (idx >> 5) << 2;
 	idx &= 0x1F;
 
-	v = cru_readl(reg);
-	if (on) {
+	if (reg == CRU_CLKGATE3_CON)
+		v = cru_clkgate3_con_mirror;
+	else
+		v = cru_readl(reg);
+
+	if (on)
 		v &= ~(1 << idx);	// clear bit 
-	} else {
+	else
 		v |= (1 << idx);	// set bit
-	}
+
+	if (reg == CRU_CLKGATE3_CON)
+		cru_clkgate3_con_mirror = v;
 	cru_writel(v, reg);
 
 	return 0;
 }
 
+/* Work around CRU_SOFTRST0_CON bit29~27 bug */
+static volatile u32 cru_softrst0_con_mirror;
+
+void cru_set_soft_reset(enum cru_soft_reset idx, bool on)
+{
+	unsigned long flags;
+	u32 reg = CRU_SOFTRST0_CON + ((idx >> 5) << 2);
+	u32 mask = 1 << (idx & 31);
+	u32 v;
+
+	if (idx >= SOFT_RST_MAX)
+		return;
+
+	local_irq_save(flags);
+
+	if (reg == CRU_SOFTRST0_CON)
+		v = cru_softrst0_con_mirror;
+	else
+		v = cru_readl(reg);
+
+	if (on)
+		v |= mask;
+	else
+		v &= ~mask;
+
+	if (reg == CRU_SOFTRST0_CON)
+		cru_softrst0_con_mirror = v;
+	cru_writel(v, reg);
+
+	local_irq_restore(flags);
+}
+
 static struct clk xin24m = {
 	.name		= "xin24m",
 	.rate		= 24 * MHZ,
@@ -2259,6 +2300,9 @@ void __init rk29_clock_init(void)
 {
 	struct clk_lookup *lk;
 
+	cru_clkgate3_con_mirror = cru_readl(CRU_CLKGATE3_CON);
+	cru_softrst0_con_mirror = cru_readl(CRU_SOFTRST0_CON);
+
 	for (lk = clks; lk < clks + ARRAY_SIZE(clks); lk++)
 		clk_preinit(lk->clk);
 
@@ -2311,7 +2355,10 @@ static void dump_clock(struct seq_file *s, struct clk *clk, int deep)
 		reg += (idx >> 5) << 2;
 		idx &= 0x1F;
 
-		v = cru_readl(reg) & (1 << idx);
+		if (reg == CRU_CLKGATE3_CON)
+			v = cru_clkgate3_con_mirror & (1 << idx);
+		else
+			v = cru_readl(reg) & (1 << idx);
 		
 		seq_printf(s, "%s ", v ? "off" : "on ");
 	}
@@ -2409,6 +2456,11 @@ static int proc_clk_show(struct seq_file *s, void *v)
 	seq_printf(s, "CLKGATE1 : 0x%08x\n", cru_readl(CRU_CLKGATE1_CON));
 	seq_printf(s, "CLKGATE2 : 0x%08x\n", cru_readl(CRU_CLKGATE2_CON));
 	seq_printf(s, "CLKGATE3 : 0x%08x\n", cru_readl(CRU_CLKGATE3_CON));
+	seq_printf(s, "CLKGATE3M: 0x%08x\n", cru_clkgate3_con_mirror);
+	seq_printf(s, "SOFTRST0 : 0x%08x\n", cru_readl(CRU_SOFTRST0_CON));
+	seq_printf(s, "SOFTRST0M: 0x%08x\n", cru_softrst0_con_mirror);
+	seq_printf(s, "SOFTRST1 : 0x%08x\n", cru_readl(CRU_SOFTRST1_CON));
+	seq_printf(s, "SOFTRST2 : 0x%08x\n", cru_readl(CRU_SOFTRST2_CON));
 
 	seq_printf(s, "\nPMU Registers:\n");
 	seq_printf(s, "WAKEUP_EN0 : 0x%08x\n", pmu_readl(PMU_WAKEUP_EN0));
diff --git a/arch/arm/mach-rk29/include/mach/cru.h b/arch/arm/mach-rk29/include/mach/cru.h
index 27a0a8e76a1a..b6602e9ea9d9 100644
--- a/arch/arm/mach-rk29/include/mach/cru.h
+++ b/arch/arm/mach-rk29/include/mach/cru.h
@@ -296,24 +296,6 @@ enum cru_soft_reset {
 #define CRU_SOFTRST1_CON	0x70
 #define CRU_SOFTRST2_CON	0x74
 
-static inline void cru_set_soft_reset(enum cru_soft_reset idx, bool on)
-{
-	unsigned long flags;
-	u32 addr = RK29_CRU_BASE + CRU_SOFTRST0_CON + ((idx >> 5) << 2);
-	u32 mask = 1 << (idx & 31);
-	u32 v;
-
-	if (idx >= SOFT_RST_MAX)
-		return;
-
-	local_irq_save(flags);
-	v = readl(addr);
-	if (on)
-		v |= mask;
-	else
-		v &= ~mask;
-	writel(v, addr);
-	local_irq_restore(flags);
-}
+void cru_set_soft_reset(enum cru_soft_reset idx, bool on);
 
 #endif
-- 
2.34.1