Merge with /home/wd/git/u-boot/master

14 files changed, 542 insertions, 99 deletions

diff --git a/cpu/ixp/npe/include/IxEthDB.h b/cpu/ixp/npe/include/IxEthDB.h
index 43ee802..1189c9a 100644
--- a/cpu/ixp/npe/include/IxEthDB.h
+++ b/cpu/ixp/npe/include/IxEthDB.h
@@ -799,10 +799,10 @@ IxEthDBStatus ixEthDBFilteringDatabaseShowRecords(IxEthDBPortId portID, IxEthDBR
  * @verbatim
     IxEthDBPortMap portMap;
     
-    /* clear all ports from port map */
+    // clear all ports from port map
     memset(portMap, 0, sizeof (portMap)); 
     
-    /* include portID in port map  */
+    // include portID in port map 
     portMap[portID / 8] = 1 << (portID % 8);
    @endverbatim
  *
diff --git a/cpu/mcf52x2/cpu.c b/cpu/mcf52x2/cpu.c
index e6e5d9b..aa6b2bd 100644
--- a/cpu/mcf52x2/cpu.c
+++ b/cpu/mcf52x2/cpu.c
@@ -2,6 +2,10 @@
  * (C) Copyright 2003
  * Josef Baumgartner <josef.baumgartner@telex.de>
  *
+ * MCF5282 additionals
+ * (C) Copyright 2005
+ * BuS Elektronik GmbH & Co. KG <esw@bus-elektronik.de>
+ *
  * See file CREDITS for list of people who contributed to this
  * project.
  *
@@ -36,7 +40,8 @@
 #endif
 
 #ifdef	CONFIG_M5282
-
+#include <asm/m5282.h>
+#include <asm/immap_5282.h>
 #endif
 
 #ifdef	CONFIG_M5249
@@ -116,7 +121,6 @@ int checkcpu(void) {
 	return 0;
 };
 
-
 #if defined(CONFIG_WATCHDOG)
 /* Called by macro WATCHDOG_RESET */
 void watchdog_reset (void)
@@ -158,11 +162,25 @@ int watchdog_init (void)
 #ifdef	CONFIG_M5282
 int checkcpu (void)
 {
-	puts ("CPU:   Freescale Coldfire MCF5282\n");
+	unsigned char resetsource = MCFRESET_RSR;
+
+	printf ("CPU:   Freescale Coldfire MCF5282 (PIN: %2.2x REV: %2.2x)\n",
+		MCFCCM_CIR>>8,MCFCCM_CIR & MCFCCM_CIR_PRN_MASK);
+	printf ("Reset:%s%s%s%s%s%s%s\n",
+		(resetsource & MCFRESET_RSR_LOL)  ? " Loss of Lock"	: "",
+		(resetsource & MCFRESET_RSR_LOC)  ? " Loss of Clock"	: "",
+		(resetsource & MCFRESET_RSR_EXT)  ? " External"		: "",
+		(resetsource & MCFRESET_RSR_POR)  ? " Power On"		: "",
+		(resetsource & MCFRESET_RSR_WDR)  ? " Watchdog"		: "",
+		(resetsource & MCFRESET_RSR_SOFT) ? " Software"		: "",
+		(resetsource & MCFRESET_RSR_LVD)  ? " Low Voltage"	: ""
+	);
 	return 0;
 }
 
-int do_reset (cmd_tbl_t *cmdtp, bd_t *bd, int flag, int argc, char *argv[]) {
+int do_reset (cmd_tbl_t *cmdtp, bd_t *bd, int flag, int argc, char *argv[])
+{
+	MCFRESET_RCR = MCFRESET_RCR_SOFTRST;
 	return 0;
 };
 #endif
diff --git a/cpu/mcf52x2/cpu_init.c b/cpu/mcf52x2/cpu_init.c
index 451e3f3..1748ea9 100644
--- a/cpu/mcf52x2/cpu_init.c
+++ b/cpu/mcf52x2/cpu_init.c
@@ -2,6 +2,10 @@
  * (C) Copyright 2003
  * Josef Baumgartner <josef.baumgartner@telex.de>
  *
+ * MCF5282 additionals
+ * (C) Copyright 2005
+ * BuS Elektronik GmbH & Co. KG <esw@bus-elektronik.de>
+ *
  * See file CREDITS for list of people who contributed to this
  * project.
  *
@@ -172,7 +176,180 @@ int cpu_init_r	(void)
  */
 void cpu_init_f (void)
 {
+#ifndef CONFIG_WATCHDOG
+	/* disable watchdog if we aren't using it */
+	MCFWTM_WCR = 0;
+#endif
+
+#ifndef CONFIG_MONITOR_IS_IN_RAM
+	/* Set speed /PLL */
+	MCFCLOCK_SYNCR =  MCFCLOCK_SYNCR_MFD(CFG_MFD) | MCFCLOCK_SYNCR_RFD(CFG_RFD);
+
+	/* Set up the GPIO ports */
+#ifdef CFG_PEPAR
+	MCFGPIO_PEPAR = CFG_PEPAR;
+#endif
+#ifdef	CFG_PFPAR
+	MCFGPIO_PFPAR = CFG_PFPAR;
+#endif
+#ifdef CFG_PJPAR
+	MCFGPIO_PJPAR = CFG_PJPAR;
+#endif
+#ifdef CFG_PSDPAR
+	MCFGPIO_PSDPAR = CFG_PSDPAR;
+#endif
+#ifdef CFG_PASPAR
+	MCFGPIO_PASPAR = CFG_PASPAR;
+#endif
+#ifdef CFG_PEHLPAR
+	MCFGPIO_PEHLPAR = CFG_PEHLPAR;
+#endif
+#ifdef CFG_PQSPAR
+	MCFGPIO_PQSPAR = CFG_PQSPAR;
+#endif
+#ifdef CFG_PTCPAR
+	MCFGPIO_PTCPAR = CFG_PTCPAR;
+#endif
+#ifdef CFG_PTDPAR
+	MCFGPIO_PTDPAR = CFG_PTDPAR;
+#endif
+#ifdef CFG_PUAPAR
+	MCFGPIO_PUAPAR = CFG_PUAPAR;
+#endif
+
+#ifdef CFG_DDRUA
+	MCFGPIO_DDRUA = CFG_DDRUA;
+#endif
+
+	/* This is probably a bad place to setup chip selects, but everyone
+	   else is doing it! */
+
+#if defined(CFG_CS0_BASE) & defined(CFG_CS0_SIZE) & \
+    defined(CFG_CS0_WIDTH) & defined(CFG_CS0_RO) & \
+	defined(CFG_CS0_WS)
+
+	MCFCSM_CSAR0 =	(CFG_CS0_BASE >> 16) & 0xFFFF;
+
+	#if (CFG_CS0_WIDTH == 8)
+		#define	 CFG_CS0_PS  MCFCSM_CSCR_PS_8
+	#elif (CFG_CS0_WIDTH == 16)
+		#define	 CFG_CS0_PS  MCFCSM_CSCR_PS_16
+	#elif (CFG_CS0_WIDTH == 32)
+		#define	 CFG_CS0_PS  MCFCSM_CSCR_PS_32
+	#else
+		#error	"CFG_CS0_WIDTH: Fault - wrong bus with for CS0"
+	#endif
+	MCFCSM_CSCR0 =	MCFCSM_CSCR_WS(CFG_CS0_WS)
+			|CFG_CS0_PS
+			|MCFCSM_CSCR_AA;
+
+	#if (CFG_CS0_RO != 0)
+		MCFCSM_CSMR0 =	MCFCSM_CSMR_BAM(CFG_CS0_SIZE-1)
+				|MCFCSM_CSMR_WP|MCFCSM_CSMR_V;
+ 	#else
+		MCFCSM_CSMR0 =	MCFCSM_CSMR_BAM(CFG_CS0_SIZE-1)|MCFCSM_CSMR_V;
+	#endif
+#else
+	#waring "Chip Select 0 are not initialized/used"
+#endif
+
+#if defined(CFG_CS1_BASE) & defined(CFG_CS1_SIZE) & \
+    defined(CFG_CS1_WIDTH) & defined(CFG_CS1_RO) & \
+	defined(CFG_CS1_WS)
+
+	MCFCSM_CSAR1 = (CFG_CS1_BASE >> 16) & 0xFFFF;
+
+	#if (CFG_CS1_WIDTH == 8)
+		#define	 CFG_CS1_PS  MCFCSM_CSCR_PS_8
+	#elif (CFG_CS1_WIDTH == 16)
+		#define	 CFG_CS1_PS  MCFCSM_CSCR_PS_16
+	#elif (CFG_CS1_WIDTH == 32)
+		#define	 CFG_CS1_PS  MCFCSM_CSCR_PS_32
+	#else
+		#error	"CFG_CS1_WIDTH: Fault - wrong bus with for CS1"
+	#endif
+	MCFCSM_CSCR1 =	MCFCSM_CSCR_WS(CFG_CS1_WS)
+			|CFG_CS1_PS
+			|MCFCSM_CSCR_AA;
+
+	#if (CFG_CS1_RO != 0)
+		MCFCSM_CSMR1 =	MCFCSM_CSMR_BAM(CFG_CS1_SIZE-1)
+				|MCFCSM_CSMR_WP
+				|MCFCSM_CSMR_V;
+ 	#else
+		MCFCSM_CSMR1 =	MCFCSM_CSMR_BAM(CFG_CS1_SIZE-1)
+				|MCFCSM_CSMR_V;
+	#endif
+#else
+	#warning "Chip Select 1 are not initialized/used"
+#endif
+
+#if defined(CFG_CS2_BASE) & defined(CFG_CS2_SIZE) & \
+    defined(CFG_CS2_WIDTH) & defined(CFG_CS2_RO) & \
+	defined(CFG_CS2_WS)
+
+	MCFCSM_CSAR2 = (CFG_CS2_BASE >> 16) & 0xFFFF;
+
+	#if (CFG_CS2_WIDTH == 8)
+		#define	 CFG_CS2_PS  MCFCSM_CSCR_PS_8
+	#elif (CFG_CS2_WIDTH == 16)
+		#define	 CFG_CS2_PS  MCFCSM_CSCR_PS_16
+	#elif (CFG_CS2_WIDTH == 32)
+		#define	 CFG_CS2_PS  MCFCSM_CSCR_PS_32
+	#else
+		#error	"CFG_CS2_WIDTH: Fault - wrong bus with for CS2"
+	#endif
+	MCFCSM_CSCR2 =	MCFCSM_CSCR_WS(CFG_CS2_WS)
+			|CFG_CS2_PS
+			|MCFCSM_CSCR_AA;
+
+	#if (CFG_CS2_RO != 0)
+		MCFCSM_CSMR2 =	MCFCSM_CSMR_BAM(CFG_CS2_SIZE-1)
+				|MCFCSM_CSMR_WP
+				|MCFCSM_CSMR_V;
+ 	#else
+		MCFCSM_CSMR2 =	MCFCSM_CSMR_BAM(CFG_CS2_SIZE-1)
+				|MCFCSM_CSMR_V;
+	#endif
+#else
+	#warning "Chip Select 2 are not initialized/used"
+#endif
+
+#if defined(CFG_CS3_BASE) & defined(CFG_CS3_SIZE) & \
+    defined(CFG_CS3_WIDTH) & defined(CFG_CS3_RO) & \
+	defined(CFG_CS3_WS)
+
+	MCFCSM_CSAR3 = (CFG_CS3_BASE >> 16) & 0xFFFF;
+
+	#if (CFG_CS3_WIDTH == 8)
+		#define	 CFG_CS3_PS  MCFCSM_CSCR_PS_8
+	#elif (CFG_CS3_WIDTH == 16)
+		#define	 CFG_CS3_PS  MCFCSM_CSCR_PS_16
+	#elif (CFG_CS3_WIDTH == 32)
+		#define	 CFG_CS3_PS  MCFCSM_CSCR_PS_32
+	#else
+		#error	"CFG_CS3_WIDTH: Fault - wrong bus with for CS1"
+	#endif
+	MCFCSM_CSCR3 =	MCFCSM_CSCR_WS(CFG_CS3_WS)
+			|CFG_CS3_PS
+			|MCFCSM_CSCR_AA;
+
+	#if (CFG_CS3_RO != 0)
+		MCFCSM_CSMR3 =	MCFCSM_CSMR_BAM(CFG_CS3_SIZE-1)
+				|MCFCSM_CSMR_WP
+				|MCFCSM_CSMR_V;
+ 	#else
+		MCFCSM_CSMR3 =	MCFCSM_CSMR_BAM(CFG_CS3_SIZE-1)
+				|MCFCSM_CSMR_V;
+	#endif
+#else
+	#warning "Chip Select 3 are not initialized/used"
+#endif
+
+#endif /* CONFIG_MONITOR_IS_IN_RAM */
 
+	/* defer enabling cache until boot (see do_go) */
+	/* icache_enable(); */
 }
 
 /*
diff --git a/cpu/mcf52x2/fec.c b/cpu/mcf52x2/fec.c
index f207dd6..6db6214 100644
--- a/cpu/mcf52x2/fec.c
+++ b/cpu/mcf52x2/fec.c
@@ -205,7 +205,9 @@ int eth_rx (void)
 
 int eth_init (bd_t * bd)
 {
-
+#ifndef CFG_ENET_BD_BASE
+	DECLARE_GLOBAL_DATA_PTR;
+#endif
 	int i;
 	volatile fec_t *fecp = (fec_t *) (FEC_ADDR);
 
@@ -258,6 +260,10 @@ int eth_init (bd_t * bd)
 #else
 	/* Clear multicast address hash table
 	 */
+#ifdef	CONFIG_M5282
+	fecp->fec_ihash_table_high = 0;
+	fecp->fec_ihash_table_low = 0;
+#else
 	fecp->fec_hash_table_high = 0;
 	fecp->fec_hash_table_low = 0;
 #endif
@@ -273,7 +279,16 @@ int eth_init (bd_t * bd)
 	txIdx = 0;
 
 	if (!rtx) {
+#ifdef CFG_ENET_BD_BASE
 		rtx = (RTXBD *) CFG_ENET_BD_BASE;
+#else
+		rtx = (RTXBD *) (CFG_MONITOR_BASE+gd->reloc_off -
+				 (((PKTBUFSRX+TX_BUF_CNT)*+sizeof(cbd_t)
+				  +0xFF)
+				  & ~0xFF)
+				);
+		debug("set ENET_DB_BASE to %lX\n",(long) rtx);
+#endif
 	}
 
 	/*
@@ -307,18 +322,18 @@ int eth_init (bd_t * bd)
 
 	/* Enable MII mode
 	 */
-#if 0				/* Full duplex mode */
+
+#if 0	/* Full duplex mode */
 	fecp->fec_r_cntrl = FEC_RCNTRL_MII_MODE;
 	fecp->fec_x_cntrl = FEC_TCNTRL_FDEN;
-#else  /* Half duplex mode */
-	fecp->fec_r_cntrl = FEC_RCNTRL_MII_MODE | FEC_RCNTRL_DRT;
-#ifdef	CONFIG_M5271
-	fecp->fec_r_cntrl |= (PKT_MAXBUF_SIZE << 16); /* set max frame length */
-#endif
+#else	/* Half duplex mode */
+	fecp->fec_r_cntrl = (PKT_MAXBUF_SIZE << 16); /* set max frame length */
+	fecp->fec_r_cntrl |= FEC_RCNTRL_MII_MODE | FEC_RCNTRL_DRT;
 	fecp->fec_x_cntrl = 0;
 #endif
 	/* Set MII speed */
-	fecp->fec_mii_speed = 0x0e;
+	fecp->fec_mii_speed = (((CFG_CLK / 2) / (2500000 / 10)) + 5) / 10;
+	fecp->fec_mii_speed *= 2;
 
 	/* Configure port B for MII.
 	 */
@@ -422,7 +437,7 @@ static void mii_discover_phy (void)
 			 */
 			udelay (10000);	/* wait 10ms */
 		}
-		for (phyno = 0; phyno < 32 && phyaddr < 0; ++phyno) {
+		for (phyno = 1; phyno < 32 && phyaddr < 0; ++phyno) {
 			phytype = mii_send (mk_mii_read (phyno, PHY_PHYIDR1));
 #ifdef ET_DEBUG
 			printf ("PHY type 0x%x pass %d type ", phytype, pass);
diff --git a/cpu/mcf52x2/serial.c b/cpu/mcf52x2/serial.c
index 1011788..8be09e3 100644
--- a/cpu/mcf52x2/serial.c
+++ b/cpu/mcf52x2/serial.c
@@ -55,45 +55,75 @@ void rs_serial_setbaudrate(int port,int baudrate)
 {
 #if defined(CONFIG_M5272) || defined(CONFIG_M5249) || defined(CONFIG_M5271)
 	volatile unsigned char	*uartp;
-#ifndef CONFIG_M5271
+# ifndef CONFIG_M5271
 	double fraction;
-#endif
+# endif
 	double clock;
 
 	if (port == 0)
-	  uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
+		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
 	else
-	  uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE2);
+		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE2);
 
-	clock = DoubleClock(baudrate);      /* Set baud above */
+	clock = DoubleClock(baudrate);	/* Set baud above */
 
-	uartp[MCFUART_UBG1] = (((int)clock >> 8) & 0xff);  /* set msb baud */
-	uartp[MCFUART_UBG2] = ((int)clock & 0xff);  /* set lsb baud */
+	uartp[MCFUART_UBG1] = (((int)clock >> 8) & 0xff);	/* set msb baud */
+	uartp[MCFUART_UBG2] = ((int)clock & 0xff);		/* set lsb baud */
 
-#ifndef CONFIG_M5271
+# ifndef CONFIG_M5271
 	fraction = ((clock - (int)clock) * 16.0) + 0.5;
-	uartp[MCFUART_UFPD] = ((int)fraction & 0xf);  /* set baud fraction adjust */
+	uartp[MCFUART_UFPD] = ((int)fraction & 0xf);	/* set baud fraction adjust */
+# endif
 #endif
+
+#if  defined(CONFIG_M5282)
+	volatile unsigned char	*uartp;
+	long clock;
+
+	switch (port) {
+	case 1:
+		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE2);
+		break;
+	case 2:
+		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE3);
+		break;
+	default:
+		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
+	}
+
+	clock = (long) CFG_CLK / ((long) 32 * baudrate);	/* Set baud above */
+
+	uartp[MCFUART_UBG1] = (((int)clock >> 8) & 0xff);	/* set msb baud */
+	uartp[MCFUART_UBG2] = ((int) clock & 0xff);		/* set lsb baud */
+
 #endif
 };
 
-void rs_serial_init(int port,int baudrate)
+void rs_serial_init (int port, int baudrate)
 {
-	volatile unsigned char	*uartp;
+	volatile unsigned char *uartp;
 
 	/*
-	 *	Reset UART, get it into known state...
+	 *      Reset UART, get it into known state...
 	 */
-	if (port == 0)
-		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
-	else
+	switch (port) {
+	case 1:
 		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE2);
+		break;
+#if  defined(CONFIG_M5282)
+	case 2:
+		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE3);
+		break;
+#endif
+	default:
+		uartp = (volatile unsigned char *) (CFG_MBAR + MCFUART_BASE1);
+	}
 
-	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETTX;  /* reset TX */
-	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETRX;  /* reset RX */
+	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETTX;	/* reset TX */
+	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETRX;	/* reset RX */
 
-	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETMRPTR;  /* reset MR pointer */
-	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETERR;  /* reset Error pointer */
+	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETMRPTR;	/* reset MR pointer */
+	uartp[MCFUART_UCR] = MCFUART_UCR_CMDRESETERR;	/* reset Error pointer */
 
 	/*
 	 * Set port for CONSOLE_BAUD_RATE, 8 data bits, 1 stop bit, no parity.
@@ -107,7 +137,7 @@ void rs_serial_init(int port,int baudrate)
 	/* Set clock Select Register: Tx/Rx clock is timer */
 	uartp[MCFUART_UCSR] = MCFUART_UCSR_RXCLKTIMER | MCFUART_UCSR_TXCLKTIMER;
 
-	rs_serial_setbaudrate(port,baudrate);
+	rs_serial_setbaudrate (port, baudrate);
 
 	/* Enable Tx/Rx */
 	uartp[MCFUART_UCR] = MCFUART_UCR_RXENABLE | MCFUART_UCR_TXENABLE;
@@ -169,9 +199,8 @@ void serial_putc(const char c) {
 }
 
 void serial_puts (const char *s) {
-	while (*s) {
+	while (*s)
 		serial_putc(*s++);
-	}
 }
 
 int serial_getc(void) {
diff --git a/cpu/mcf52x2/start.S b/cpu/mcf52x2/start.S
index 3ab812b..8a83ca5 100644
--- a/cpu/mcf52x2/start.S
+++ b/cpu/mcf52x2/start.S
@@ -55,11 +55,15 @@
  */
 _vectors:
 
-#ifndef	CONFIG_R5200
-.long	0x00000000, _START
+.long	0x00000000		/* Flash offset is 0 until we setup CS0 */
+#if defined(CONFIG_R5200)
+.long	0x400
+#elif defined(CONFIG_M5282)
+.long	_start - TEXT_BASE
 #else
-.long	0x00000000, 0x400	/* Flash offset is 0 until we setup CS0 */
+.long	_START
 #endif
+
 .long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
 .long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
 .long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
@@ -100,20 +104,23 @@ _vectors:
 
 	.text
 
+
+#if defined(CFG_INT_FLASH_BASE) && \
+    (defined(CONFIG_M5282) || defined(CONFIG_M5281))
+	#if (TEXT_BASE == CFG_INT_FLASH_BASE)
+		.long	0x55AA55AA,0xAA55AA55 /* CFM Backdoorkey */
+		.long	0xFFFFFFFF /* all sectors protected */
+		.long	0x00000000 /* supervisor/User restriction */
+		.long	0x00000000 /* programm/data space restriction */
+		.long	0x00000000 /* Flash security */
+	#endif
+#endif
 	.globl	_start
 _start:
 	nop
 	nop
 	move.w #0x2700,%sr
 
-	/* if we come from a pre-loader we have no exception table and
-	 * therefore no VBR to set
-	 */
-#if !defined(CONFIG_MONITOR_IS_IN_RAM)
-	move.l	#CFG_FLASH_BASE, %d0
-	movec	%d0, %VBR
-#endif
-
 #if defined(CONFIG_M5272) || defined(CONFIG_M5249)
 	move.l	#(CFG_MBAR + 1), %d0		/* set MBAR address + valid flag */
 	move.c	%d0, %MBAR
@@ -128,20 +135,48 @@ _start:
 	movec	%d0, %RAMBAR0
 #endif /* #if defined(CONFIG_M5272) || defined(CONFIG_M5249) */
 
-#if	defined(CONFIG_M5282) || defined(CONFIG_M5271)
+#if defined(CONFIG_M5282) || defined(CONFIG_M5271)
 	/* Initialize IPSBAR */
 	move.l	#(CFG_MBAR + 1), %d0		/* set IPSBAR address + valid flag */
 	move.l	%d0, 0x40000000
 
-#ifdef	CONFIG_M5282
+	/* Initialize RAMBAR1: locate SRAM and validate it */
+	move.l	#(CFG_INIT_RAM_ADDR + 0x21), %d0
+	movec	%d0, %RAMBAR1
+
+#if (TEXT_BASE == CFG_INT_FLASH_BASE)
+	/* Setup code in SRAM to initialize FLASHBAR, if start from internal Flash */
+
+	move.l #(_flashbar_setup-CFG_INT_FLASH_BASE), %a0
+	move.l #(_flashbar_setup_end-CFG_INT_FLASH_BASE), %a1
+	move.l #(CFG_INIT_RAM_ADDR), %a2
+_copy_flash:
+	move.l (%a0)+, (%a2)+
+	cmp.l %a0, %a1
+	bgt.s _copy_flash
+	jmp CFG_INIT_RAM_ADDR
+
+_flashbar_setup:
 	/* Initialize FLASHBAR: locate internal Flash and validate it */
 	move.l	#(CFG_INT_FLASH_BASE + 0x21), %d0
 	movec	%d0, %RAMBAR0
-#endif
+	jmp _after_flashbar_copy.L	/* Force jump to absolute address */
+_flashbar_setup_end:
+	nop
+_after_flashbar_copy:
+#else
+	/* Setup code to initialize FLASHBAR, if start from external Memory */
+	move.l	#(CFG_INT_FLASH_BASE + 0x21), %d0
+	movec	%d0, %RAMBAR0
+#endif /* (TEXT_BASE == CFG_INT_FLASH_BASE) */
 
-	/* Initialize RAMBAR1: locate SRAM and validate it */
-	move.l	#(CFG_INIT_RAM_ADDR + 0x21), %d0
-	movec	%d0, %RAMBAR1
+#endif
+	/* if we come from a pre-loader we have no exception table and
+	 * therefore no VBR to set
+	 */
+#if !defined(CONFIG_MONITOR_IS_IN_RAM)
+	move.l	#CFG_FLASH_BASE, %d0
+	movec	%d0, %VBR
 #endif
 
 #ifdef	CONFIG_R5200
@@ -218,7 +253,6 @@ relocate_code:
 	move.l #CFG_MONITOR_BASE, %a1
 	move.l #__init_end, %a2
 	move.l %a0, %a3
-
 	/* copy the code to RAM */
 1:
 	move.l (%a1)+, (%a3)+
@@ -229,14 +263,14 @@ relocate_code:
  * We are done. Do not return, instead branch to second part of board
  * initialization, now running from RAM.
  */
-	move.l	%a0, %a1
+ 	move.l	%a0, %a1
 	add.l	#(in_ram - CFG_MONITOR_BASE), %a1
 	jmp	(%a1)
 
 in_ram:
 
 clear_bss:
-	/*
+ 	/*
 	 * Now clear BSS segment
 	 */
 	move.l	%a0, %a1
@@ -266,6 +300,23 @@ clear_bss:
 	cmp.l	%a2, %a1
 	bne	7b
 
+#if defined(CONFIG_M5281) || defined(CONFIG_M5282)
+	/* patch the 3 accesspoints to 3 ichache_state */
+	/* quick and dirty */
+
+	move.l	%a0,%d1
+	add.l	#(icache_state - CFG_MONITOR_BASE),%d1
+	move.l	%a0,%a1
+	add.l	#(icache_state_access_1+2 - CFG_MONITOR_BASE),%a1
+	move.l  %d1,(%a1)
+	move.l	%a0,%a1
+	add.l	#(icache_state_access_2+2 - CFG_MONITOR_BASE),%a1
+	move.l  %d1,(%a1)
+	move.l	%a0,%a1
+	add.l	#(icache_state_access_3+2 - CFG_MONITOR_BASE),%a1
+	move.l  %d1,(%a1)
+#endif
+
 	/* calculate relative jump to board_init_r in ram */
 	move.l %a0, %a1
 	add.l #(board_init_r - CFG_MONITOR_BASE), %a1
@@ -273,6 +324,10 @@ clear_bss:
 	/* set parameters for board_init_r */
 	move.l %a0,-(%sp)		/* dest_addr */
 	move.l %d0,-(%sp)		/* gd */
+	#if defined(DEBUG) && (TEXT_BASE != CFG_INT_FLASH_BASE) && \
+	    defined(CFG_HALT_BEFOR_RAM_JUMP)
+ 		halt
+	#endif
 	jsr	(%a1)
 
 /*------------------------------------------------------------------------------*/
@@ -327,6 +382,7 @@ icache_enable:
 	move.l	#0x80400100, %d0		/* Setup cache mask, data cache disabel*/
 	movec	%d0, %CACR			/* Enable cache */
 	moveq	#1, %d0
+icache_state_access_1:
 	move.l	%d0, icache_state
 	rts
 #endif
@@ -361,18 +417,19 @@ icache_disable:
 	movec	%d0, %ACR0			/* Enable cache */
 	movec	%d0, %ACR1			/* Enable cache */
 	moveq	#0, %d0
+icache_state_access_2:
 	move.l	%d0, icache_state
 	rts
 
 	.globl	icache_status
 icache_status:
+icache_state_access_3:
 	move.l	icache_state, %d0
 	rts
 
 	.data
 icache_state:
-	.long	1
-
+	.long	0	/* cache is diabled on inirialization */
 
 /*------------------------------------------------------------------------------*/
 
diff --git a/cpu/nios2/epcs.c b/cpu/nios2/epcs.c
index a8851e9..414c38c 100644
--- a/cpu/nios2/epcs.c
+++ b/cpu/nios2/epcs.c
@@ -25,7 +25,7 @@
 
 #if defined(CFG_NIOS_EPCSBASE)
 #include <command.h>
-#include <nios2.h>
+#include <asm/io.h>
 #include <nios2-io.h>
 #include <nios2-epcs.h>
 
@@ -72,8 +72,7 @@
  */
 #define EPCS_TIMEOUT		100	/* 100 msec timeout */
 
-static nios_spi_t *epcs =
-	(nios_spi_t *)CACHE_BYPASS(CFG_NIOS_EPCSBASE);
+static nios_spi_t *epcs = (nios_spi_t *)CFG_NIOS_EPCSBASE;
 
 /***********************************************************************
  * Device access
@@ -81,16 +80,20 @@ static nios_spi_t *epcs =
 static int epcs_cs (int assert)
 {
 	ulong start;
+	unsigned tmp;
+
 
 	if (assert) {
-		epcs->control |= NIOS_SPI_SSO;
+		tmp = readl (&epcs->control);
+		writel (&epcs->control, tmp | NIOS_SPI_SSO);
 	} else {
 		/* Let all bits shift out */
 		start = get_timer (0);
-		while ((epcs->status & NIOS_SPI_TMT) == 0)
+		while ((readl (&epcs->status) & NIOS_SPI_TMT) == 0)
 			if (get_timer (start) > EPCS_TIMEOUT)
 				return (-1);
-		epcs->control &= ~NIOS_SPI_SSO;
+		tmp = readl (&epcs->control);
+		writel (&epcs->control, tmp & ~NIOS_SPI_SSO);
 	}
 	return (0);
 }
@@ -100,10 +103,10 @@ static int epcs_tx (unsigned char c)
 	ulong start;
 
 	start = get_timer (0);
-	while ((epcs->status & NIOS_SPI_TRDY) == 0)
+	while ((readl (&epcs->status) & NIOS_SPI_TRDY) == 0)
 		if (get_timer (start) > EPCS_TIMEOUT)
 			return (-1);
-	epcs->txdata = c;
+	writel (&epcs->txdata, c);
 	return (0);
 }
 
@@ -112,10 +115,10 @@ static int epcs_rx (void)
 	ulong start;
 
 	start = get_timer (0);
-	while ((epcs->status & NIOS_SPI_RRDY) == 0)
+	while ((readl (&epcs->status) & NIOS_SPI_RRDY) == 0)
 		if (get_timer (start) > EPCS_TIMEOUT)
 			return (-1);
-	return (epcs->rxdata);
+	return (readl (&epcs->rxdata));
 }
 
 static unsigned char bitrev[] = {
@@ -207,6 +210,21 @@ static struct epcs_devinfo_t devinfo[] = {
 	{ 0, 0, 0, 0, 0, 0 }
 };
 
+int epcs_reset (void)
+{
+	/* When booting from an epcs controller, the epcs bootrom
+	 * code may leave the slave select in an asserted state.
+	 * This causes two problems: (1) The initial epcs access
+	 * will fail -- not a big deal, and (2) a software reset
+	 * will cause the bootrom code to hang since it does not
+	 * ensure the select is negated prior to first access -- a
+	 * big deal. Here we just negate chip select and everything
+	 * gets better :-)
+	 */
+	epcs_cs (0); /* Negate chip select */
+	return (0);
+}
+
 epcs_devinfo_t *epcs_dev_find (void)
 {
 	unsigned char buf[4];
diff --git a/cpu/nios2/exceptions.S b/cpu/nios2/exceptions.S
index d3b95cf..b9c7a58 100644
--- a/cpu/nios2/exceptions.S
+++ b/cpu/nios2/exceptions.S
@@ -30,6 +30,9 @@
 
 	.global _exception
 
+	.set noat
+	.set nobreak
+
 _exception:
 	/* SAVE ALL REGS -- this allows trap and unimplemented
 	 * instruction handlers to be coded conveniently in C
diff --git a/cpu/nios2/interrupts.c b/cpu/nios2/interrupts.c
index 4a6da58..4685161 100644
--- a/cpu/nios2/interrupts.c
+++ b/cpu/nios2/interrupts.c
@@ -27,6 +27,7 @@
 
 #include <nios2.h>
 #include <nios2-io.h>
+#include <asm/io.h>
 #include <asm/ptrace.h>
 #include <common.h>
 #include <command.h>
@@ -79,7 +80,7 @@ void tmr_isr (void *arg)
 	/* Interrupt is cleared by writing anything to the
 	 * status register.
 	 */
-	tmr->status = 0;
+	writel (&tmr->status, 0);
 	timestamp += CFG_NIOS_TMRMS;
 #ifdef CONFIG_STATUS_LED
 	status_led_tick(timestamp);
@@ -88,16 +89,17 @@ void tmr_isr (void *arg)
 
 static void tmr_init (void)
 {
-	nios_timer_t *tmr =(nios_timer_t *)CACHE_BYPASS(CFG_NIOS_TMRBASE);
+	nios_timer_t *tmr =(nios_timer_t *)CFG_NIOS_TMRBASE;
+
+	writel (&tmr->status, 0);
+	writel (&tmr->control, 0);
+	writel (&tmr->control, NIOS_TIMER_STOP);
 
-	tmr->control &= ~(NIOS_TIMER_START | NIOS_TIMER_ITO);
-	tmr->control |= NIOS_TIMER_STOP;
 #if defined(CFG_NIOS_TMRCNT)
-	tmr->periodl = CFG_NIOS_TMRCNT & 0xffff;
-	tmr->periodh = (CFG_NIOS_TMRCNT >> 16) & 0xffff;
+	writel (&tmr->periodl, CFG_NIOS_TMRCNT & 0xffff);
+	writel (&tmr->periodh, (CFG_NIOS_TMRCNT >> 16) & 0xffff);
 #endif
-	tmr->control |= ( NIOS_TIMER_ITO |
-			  NIOS_TIMER_CONT |
+	writel (&tmr->control, NIOS_TIMER_ITO | NIOS_TIMER_CONT |
 			  NIOS_TIMER_START );
 	irq_install_handler (CFG_NIOS_TMRIRQ, tmr_isr, (void *)tmr);
 }
diff --git a/cpu/nios2/serial.c b/cpu/nios2/serial.c
index 3d76603..0bd3821 100644
--- a/cpu/nios2/serial.c
+++ b/cpu/nios2/serial.c
@@ -24,7 +24,7 @@
 
 #include <common.h>
 #include <watchdog.h>
-#include <nios2.h>
+#include <asm/io.h>
 #include <nios2-io.h>
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -34,8 +34,7 @@ DECLARE_GLOBAL_DATA_PTR;
  *-----------------------------------------------------------------*/
 #if defined(CONFIG_CONSOLE_JTAG)
 
-static nios_jtag_t *jtag =
-	(nios_jtag_t *)CACHE_BYPASS(CFG_NIOS_CONSOLE);
+static nios_jtag_t *jtag = (nios_jtag_t *)CFG_NIOS_CONSOLE;
 
 void serial_setbrg( void ){ return; }
 int serial_init( void ) { return(0);}
@@ -44,9 +43,9 @@ void serial_putc (char c)
 {
 	unsigned val;
 
-	while (NIOS_JTAG_WSPACE (jtag->control) == 0)
+	while (NIOS_JTAG_WSPACE ( readl (&jtag->control)) == 0)
 		WATCHDOG_RESET ();
-	jtag->data = (unsigned char)c;
+	writel (&jtag->data, (unsigned char)c);
 }
 
 void serial_puts (const char *s)
@@ -57,7 +56,7 @@ void serial_puts (const char *s)
 
 int serial_tstc (void)
 {
-	return (jtag->control & NIOS_JTAG_RRDY);
+	return ( readl (&jtag->control) & NIOS_JTAG_RRDY);
 }
 
 int serial_getc (void)
@@ -67,7 +66,7 @@ int serial_getc (void)
 
 	while (1) {
 		WATCHDOG_RESET ();
-		val = jtag->data;
+		val = readl (&jtag->data);
 		if (val & NIOS_JTAG_RVALID)
 			break;
 	}
@@ -80,8 +79,7 @@ int serial_getc (void)
  *-----------------------------------------------------------------*/
 #else
 
-static nios_uart_t *uart = (nios_uart_t *)
-	CACHE_BYPASS(CFG_NIOS_CONSOLE);
+static nios_uart_t *uart = (nios_uart_t *) CFG_NIOS_CONSOLE;
 
 #if defined(CFG_NIOS_FIXEDBAUD)
 
@@ -98,7 +96,7 @@ void serial_setbrg (void)
 	unsigned div;
 
 	div = (CONFIG_SYS_CLK_FREQ/gd->baudrate)-1;
-	uart->divisor = div;
+	writel (&uart->divisor,div);
 	return;
 }
 
@@ -118,9 +116,9 @@ void serial_putc (char c)
 {
 	if (c == '\n')
 		serial_putc ('\r');
-	while ((uart->status & NIOS_UART_TRDY) == 0)
+	while ((readl (&uart->status) & NIOS_UART_TRDY) == 0)
 		WATCHDOG_RESET ();
-	uart->txdata = (unsigned char)c;
+	writel (&uart->txdata,(unsigned char)c);
 }
 
 void serial_puts (const char *s)
@@ -132,14 +130,14 @@ void serial_puts (const char *s)
 
 int serial_tstc (void)
 {
-	return (uart->status & NIOS_UART_RRDY);
+	return (readl (&uart->status) & NIOS_UART_RRDY);
 }
 
 int serial_getc (void)
 {
 	while (serial_tstc () == 0)
 		WATCHDOG_RESET ();
-	return( uart->rxdata & 0x00ff );
+	return (readl (&uart->rxdata) & 0x00ff );
 }
 
 #endif /* CONFIG_JTAG_CONSOLE */
diff --git a/cpu/nios2/sysid.c b/cpu/nios2/sysid.c
index 2b7a569..b5a2959 100644
--- a/cpu/nios2/sysid.c
+++ b/cpu/nios2/sysid.c
@@ -26,20 +26,21 @@
 #if defined (CFG_NIOS_SYSID_BASE)
 
 #include <command.h>
-#include <nios2.h>
+#include <asm/io.h>
 #include <nios2-io.h>
 #include <linux/time.h>
 
 void display_sysid (void)
 {
-	struct nios_sysid_t *sysid =
-		(struct nios_sysid_t *)CACHE_BYPASS(CFG_NIOS_SYSID_BASE);
+	struct nios_sysid_t *sysid = (struct nios_sysid_t *)CFG_NIOS_SYSID_BASE;
 	struct tm t;
 	char asc[32];
+	time_t stamp;
 
-	localtime_r ((time_t *)&sysid->timestamp, &t);
+	stamp = readl (&sysid->timestamp);
+	localtime_r (&stamp, &t);
 	asctime_r (&t, asc);
-	printf ("SYSID : %08x, %s", sysid->id, asc);
+	printf ("SYSID : %08x, %s", readl (&sysid->id), asc);
 
 }
 
diff --git a/cpu/ppc4xx/cpu_init.c b/cpu/ppc4xx/cpu_init.c
index 1a139d7..b27567f 100644
--- a/cpu/ppc4xx/cpu_init.c
+++ b/cpu/ppc4xx/cpu_init.c
@@ -1,5 +1,5 @@
 /*
- * (C) Copyright 2000
+ * (C) Copyright 2000-2006
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
  *
  * See file CREDITS for list of people who contributed to this
@@ -101,6 +101,117 @@ DECLARE_GLOBAL_DATA_PTR;
 # endif
 #endif /* CFG_INIT_DCACHE_CS */
 
+#if defined(CFG_440_GPIO_TABLE)
+gpio_param_s gpio_tab[GPIO_GROUP_MAX][GPIO_MAX] = CFG_440_GPIO_TABLE;
+
+void set_chip_gpio_configuration(gpio_param_s (*gpio_tab)[GPIO_GROUP_MAX][GPIO_MAX])
+{
+	unsigned char i=0, j=0, reg_offset = 0, gpio_core;
+	unsigned long gpio_reg, gpio_core_add;
+
+	for (gpio_core=0; gpio_core<GPIO_GROUP_MAX; gpio_core++) {
+		j = 0;
+		reg_offset = 0;
+		/* GPIO config of the GPIOs 0 to 31 */
+		for (i=0; i<GPIO_MAX; i++, j++) {
+			if (i == GPIO_MAX/2) {
+				reg_offset = 4;
+				j = i-16;
+			}
+
+			gpio_core_add = (*gpio_tab)[gpio_core][i].add;
+
+			if (((*gpio_tab)[gpio_core][i].in_out == GPIO_IN) ||
+			     ((*gpio_tab)[gpio_core][i].in_out == GPIO_BI)) {
+
+				switch ((*gpio_tab)[gpio_core][i].alt_nb) {
+				case GPIO_SEL:
+					break;
+
+				case GPIO_ALT1:
+					gpio_reg = in32(GPIO_IS1(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					gpio_reg = gpio_reg | (GPIO_IN_SEL >> (j*2));
+					out32(GPIO_IS1(gpio_core_add+reg_offset), gpio_reg);
+					break;
+
+				case GPIO_ALT2:
+					gpio_reg = in32(GPIO_IS2(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					gpio_reg = gpio_reg | (GPIO_IN_SEL >> (j*2));
+					out32(GPIO_IS2(gpio_core_add+reg_offset), gpio_reg);
+					break;
+
+				case GPIO_ALT3:
+					gpio_reg = in32(GPIO_IS3(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					gpio_reg = gpio_reg | (GPIO_IN_SEL >> (j*2));
+					out32(GPIO_IS3(gpio_core_add+reg_offset), gpio_reg);
+					break;
+				}
+			}
+
+			if (((*gpio_tab)[gpio_core][i].in_out == GPIO_OUT) ||
+			     ((*gpio_tab)[gpio_core][i].in_out == GPIO_BI)) {
+
+				switch ((*gpio_tab)[gpio_core][i].alt_nb) {
+				case GPIO_SEL:
+					if (gpio_core == GPIO0) {
+						gpio_reg = in32(GPIO0_TCR) | (0x80000000 >> (j));
+						out32(GPIO0_TCR, gpio_reg);
+					}
+
+					if (gpio_core == GPIO1) {
+						gpio_reg = in32(GPIO1_TCR) | (0x80000000 >> (j));
+						out32(GPIO1_TCR, gpio_reg);
+					}
+
+					gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
+					gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
+					break;
+
+				case GPIO_ALT1:
+					gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					gpio_reg = gpio_reg | (GPIO_ALT1_SEL >> (j*2));
+					out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
+					gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					gpio_reg = gpio_reg | (GPIO_ALT1_SEL >> (j*2));
+					out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
+					break;
+
+				case GPIO_ALT2:
+					gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					gpio_reg = gpio_reg | (GPIO_ALT2_SEL >> (j*2));
+					out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
+					gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					gpio_reg = gpio_reg | (GPIO_ALT2_SEL >> (j*2));
+					out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
+					break;
+
+				case GPIO_ALT3:
+					gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					gpio_reg = gpio_reg | (GPIO_ALT3_SEL >> (j*2));
+					out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
+					gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
+						& ~(GPIO_MASK >> (j*2));
+					gpio_reg = gpio_reg | (GPIO_ALT3_SEL >> (j*2));
+					out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
+					break;
+				}
+			}
+		}
+	}
+}
+#endif /* CFG_440_GPIO_TABLE */
 
 /*
  * Breath some life into the CPU...
@@ -129,10 +240,16 @@ cpu_init_f (void)
 	mtdcr(cpc0_epctl, CPC0_EPRCSR_E0NFE | CPC0_EPRCSR_E1NFE);
 #endif /* CONFIG_405EP */
 
+#if defined(CFG_440_GPIO_TABLE)
+	set_chip_gpio_configuration(&gpio_tab);
+#endif /* CFG_440_GPIO_TABLE */
+
 	/*
 	 * External Bus Controller (EBC) Setup
 	 */
 #if (defined(CFG_EBC_PB0AP) && defined(CFG_EBC_PB0CR))
+#if (defined(CONFIG_405GP) || defined(CONFIG_405CR) || \
+     defined(CONFIG_405EP) || defined(CONFIG_405))
 	/*
 	 * Move the next instructions into icache, since these modify the flash
 	 * we are running from!
@@ -148,6 +265,7 @@ cpu_init_f (void)
 	asm volatile("	ori	3, 3, 0xA000"	::: "r3");
 	asm volatile("	mtctr	3"		::: "ctr");
 	asm volatile("2:	bdnz	2b"		::: "ctr", "cr0");
+#endif
 
 	mtebc(pb0ap, CFG_EBC_PB0AP);
 	mtebc(pb0cr, CFG_EBC_PB0CR);
diff --git a/cpu/ppc4xx/spd_sdram.c b/cpu/ppc4xx/spd_sdram.c
index ebd5f39..c0a6933 100644
--- a/cpu/ppc4xx/spd_sdram.c
+++ b/cpu/ppc4xx/spd_sdram.c
@@ -745,7 +745,7 @@ long int spd_sdram(void) {
 	 */
 	check_volt_type(dimm_populated, iic0_dimm_addr, num_dimm_banks);
 
-#if defined(CONFIG_440GX)
+#if defined(CONFIG_440GX) || defined(CONFIG_440EP) || defined(CONFIG_440GR) || defined(CONFIG_440SP)
 	/*
 	 * Soft-reset SDRAM controller.
 	 */
diff --git a/cpu/ppc4xx/start.S b/cpu/ppc4xx/start.S
index 948de43..647088f 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -1198,12 +1198,19 @@ ppcSync:
 	.globl	relocate_code
 relocate_code:
 #if defined(CONFIG_440EP) || defined(CONFIG_440GR)
-	dccci	0,0			    /* Invalidate data cache, now no longer our stack */
+	/*
+	 * On some 440er platforms the cache is enabled in the first TLB (Boot-CS)
+	 * to speed up the boot process. Now this cache needs to be disabled.
+	 */
+	iccci	0,0			/* Invalidate inst cache */
+	dccci	0,0			/* Invalidate data cache, now no longer our stack */
 	sync
+	isync
 	addi	r1,r0,0x0000		/* TLB entry #0 */
 	tlbre	r0,r1,0x0002		/* Read contents */
 	ori	r0,r0,0x0c00		/* Or in the inhibit, write through bit */
 	tlbwe	r0,r1,0x0002		/* Save it out */
+	sync
 	isync
 #endif
 	mr	r1,  r3		/* Set new stack pointer		*/