From 00640deff9ec09bf6af7d73753c400b47c72c452 Mon Sep 17 00:00:00 2001
From: Rajendra Nayak <rnayak@ti.com>
Date: Fri, 26 Sep 2008 17:49:34 +0530
Subject: [PATCH] --- yaml --- r: 172736 b: refs/heads/master c:
 57f277b0122722ffa1de1b53aceb70646ce9a8e1 h: refs/heads/master v: v3

---
 [refs]                             |  2 +-
 trunk/arch/arm/mach-omap2/pm34xx.c | 35 ++++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/[refs] b/[refs]
index 6e24d5bb9c6f..008943d9bbce 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: fa3c2a4fc99fd7f8c245020303d7e11feadbbac9
+refs/heads/master: 57f277b0122722ffa1de1b53aceb70646ce9a8e1
diff --git a/trunk/arch/arm/mach-omap2/pm34xx.c b/trunk/arch/arm/mach-omap2/pm34xx.c
index 05ee05f012ad..8b5bf91dc070 100644
--- a/trunk/arch/arm/mach-omap2/pm34xx.c
+++ b/trunk/arch/arm/mach-omap2/pm34xx.c
@@ -29,6 +29,8 @@
 #include <plat/control.h>
 #include <plat/serial.h>
 
+#include <asm/tlbflush.h>
+
 #include "cm.h"
 #include "cm-regbits-34xx.h"
 #include "prm-regbits-34xx.h"
@@ -164,6 +166,35 @@ static irqreturn_t prcm_interrupt_handler (int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static void restore_control_register(u32 val)
+{
+	__asm__ __volatile__ ("mcr p15, 0, %0, c1, c0, 0" : : "r" (val));
+}
+
+/* Function to restore the table entry that was modified for enabling MMU */
+static void restore_table_entry(void)
+{
+	u32 *scratchpad_address;
+	u32 previous_value, control_reg_value;
+	u32 *address;
+
+	scratchpad_address = OMAP2_L4_IO_ADDRESS(OMAP343X_SCRATCHPAD);
+
+	/* Get address of entry that was modified */
+	address = (u32 *)__raw_readl(scratchpad_address +
+				     OMAP343X_TABLE_ADDRESS_OFFSET);
+	/* Get the previous value which needs to be restored */
+	previous_value = __raw_readl(scratchpad_address +
+				     OMAP343X_TABLE_VALUE_OFFSET);
+	address = __va(address);
+	*address = previous_value;
+	flush_tlb_all();
+	control_reg_value = __raw_readl(scratchpad_address
+					+ OMAP343X_CONTROL_REG_VALUE_OFFSET);
+	/* This will enable caches and prediction */
+	restore_control_register(control_reg_value);
+}
+
 static void omap_sram_idle(void)
 {
 	/* Variable to tell what needs to be saved and restored
@@ -220,6 +251,10 @@ static void omap_sram_idle(void)
 	_omap_sram_idle(NULL, save_state);
 	cpu_init();
 
+	/* Restore table entry modified during MMU restoration */
+	if (pwrdm_read_prev_pwrst(mpu_pwrdm) == PWRDM_POWER_OFF)
+		restore_table_entry();
+
 	if (core_next_state < PWRDM_POWER_ON) {
 		if (per_next_state < PWRDM_POWER_ON)
 			omap_uart_resume_idle(2);