Support for MPC5643L.

Rework of the start sequence to reduce the amount assembler code and to
support configuration tables which may be provided by the application.

19 files changed, 1150 insertions, 473 deletions

diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/bspstart.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/bspstart.c
index 6555a6e3de..66cfa17f14 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/bspstart.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/bspstart.c
@@ -43,11 +43,9 @@
 #include <bsp/start.h>
 #include <bsp/mpc55xx-config.h>
 
+extern Heap_Control *RTEMS_Malloc_Heap;
+
 /* Symbols defined in linker command file */
-LINKER_SYMBOL(bsp_ram_start);
-LINKER_SYMBOL(bsp_ram_end);
-LINKER_SYMBOL(bsp_external_ram_start);
-LINKER_SYMBOL(bsp_external_ram_size);
 LINKER_SYMBOL(mpc55xx_exc_vector_base);
 
 unsigned int bsp_clock_speed = 0;
@@ -147,5 +145,20 @@ void bsp_start(void)
 	}
 
 	mpc55xx_edma_init();
-	mpc55xx_emios_initialize(MPC55XX_EMIOS_PRESCALER);
+	#ifdef MPC55XX_EMIOS_PRESCALER
+		mpc55xx_emios_initialize(MPC55XX_EMIOS_PRESCALER);
+	#endif
+}
+
+void bsp_pretasking_hook(void)
+{
+	#if MPC55XX_CHIP_TYPE / 10 == 564
+		_Heap_Extend(
+			RTEMS_Malloc_Heap,
+			bsp_section_rwextra_end,
+			(uintptr_t) bsp_ram_end
+				- (uintptr_t) bsp_section_rwextra_end,
+			NULL
+		);
+	#endif
 }
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/early-init.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/early-init.c
deleted file mode 100644
index 3b213f228a..0000000000
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/early-init.c
+++ /dev/null
@@ -1,157 +0,0 @@
-/**
- * @file
- *
- * @ingroup mpc55xx
- *
- * @brief BSP early initialization code.
- */
-
-/*
- * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
- *
- *  embedded brains GmbH
- *  Obere Lagerstr. 30
- *  82178 Puchheim
- *  Germany
- *  <rtems@embedded-brains.de>
- *
- * The license and distribution terms for this file may be
- * found in the file LICENSE in this distribution or at
- * http://www.rtems.com/license/LICENSE.
- *
- * $Id$
- */
-
-#include <bsp/mpc55xx-config.h>
-#include <bsp/linker-symbols.h>
-#include <bsp/start.h>
-#include <bsp.h>
-
-#include <mpc55xx/mpc55xx.h>
-
-#include <string.h>
-
-#ifdef MPC55XX_BOOTFLAGS
-extern uint32_t mpc55xx_bootflag_0 [];
-#endif
-
-static void BSP_START_TEXT_SECTION mpc55xx_siu_init(void)
-{
-  size_t i = 0;
-
-#if defined(MPC55XX_BOARD_GWLCFM)
-  SIU.GPDO[122].B.PDO=1; /* make sure USB reset is kept high */
-  SIU.GPDO[121].B.PDO=1; /* make sure Ethernet reset is kept high */
-  SIU.GPDO[113].B.PDO=1; /* make sure MOST Companion reset is kept high */
-#endif
-
-  for (i = 0; i < mpc55xx_siu_pcr_config_count [0]; ++i) {
-     const mpc55xx_siu_pcr_config_entry *e = &mpc55xx_siu_pcr_config [i];
-     int j = e->index;
-     int n = j + e->count;
-     uint32_t pcr = e->pcr.R;
-
-     while (j < n) {
-       SIU.PCR [j].R = pcr;
-       ++j;
-     }
-  }
-}
-
-static void BSP_START_TEXT_SECTION mpc55xx_ebi_chip_select_init(void)
-{
-  size_t i = 0;
-
-  for (i = 0; i < mpc55xx_ebi_cs_config_count [0]; ++i) {
-     EBI.CS [i] = mpc55xx_ebi_cs_config [i];
-  }
-
-  for (i = 0; i < mpc55xx_ebi_cal_cs_config_count [0]; ++i) {
-     EBI.CAL_CS [i] = mpc55xx_ebi_cal_cs_config [i];
-  }
-}
-
-static void BSP_START_TEXT_SECTION mpc55xx_ebi_init(void)
-{
-#if defined(MPC55XX_BOARD_GWLCFM)
-	/*
-	 * init EBI for Muxed AD bus
-	 */
-	EBI.MCR.B.DBM = 1;
-	EBI.MCR.B.AD_MUX = 1; /* use multiplexed bus */
-	EBI.MCR.B.D16_31 = 1; /* use lower AD bus    */
-
-	SIU.ECCR.B.EBDF = 3;  /* use CLK/4 as bus clock */
-#elif defined(MPC55XX_BOARD_MPC5674FEVB)
-  struct EBI_tag ebi = {
-    .MCR = {
-      .B = {
-        .ACGE = 0,
-        .MDIS = 0,
-        .D16_31 = 0,
-        .AD_MUX = 0,
-        .DBM = 0
-      }
-    }
-  };
-
-  EBI.MCR.R = ebi.MCR.R;
-#endif
-}
-
-static void BSP_START_TEXT_SECTION mpc55xx_mmu_init(void)
-{
-#ifdef MPC55XX_BOOTFLAGS
-  /* If the low bit of bootflag 0 is clear don't change the MMU.  */
-  bool do_mmu_init = (mpc55xx_bootflag_0 [0] & 1) != 0;
-#else
-  bool do_mmu_init = true;
-#endif
-
-  if (do_mmu_init) {
-    size_t i = 0;
-
-    for (i = 0; i < mpc55xx_mmu_config_count [0]; ++i) {
-      const struct MMU_tag *tag = &mpc55xx_mmu_config [i];
-      PPC_SET_SPECIAL_PURPOSE_REGISTER( FSL_EIS_MAS0, tag->MAS0.R);
-      PPC_SET_SPECIAL_PURPOSE_REGISTER( FSL_EIS_MAS1, tag->MAS1.R);
-      PPC_SET_SPECIAL_PURPOSE_REGISTER( FSL_EIS_MAS2, tag->MAS2.R);
-      PPC_SET_SPECIAL_PURPOSE_REGISTER( FSL_EIS_MAS3, tag->MAS3.R);
-      __asm__ volatile ("tlbwe");
-    }
-  }
-}
-
-static void BSP_START_TEXT_SECTION mpc55xx_load_section(
-  void *dst,
-  const void *src,
-  size_t n
-)
-{
-  if (dst != src) {
-    memcpy(dst, src, n);
-  }
-}
-
-void BSP_START_TEXT_SECTION mpc55xx_early_init(void)
-{
-  mpc55xx_load_section(
-    &bsp_section_fast_text_begin [0],
-    &bsp_section_fast_text_load_begin [0],
-    (size_t) bsp_section_fast_text_size
-  );
-  mpc55xx_load_section(
-    &bsp_section_fast_data_begin [0],
-    &bsp_section_fast_data_load_begin [0],
-    (size_t) bsp_section_fast_data_size
-  );
-  mpc55xx_load_section(
-    &bsp_section_data_begin [0],
-    &bsp_section_data_load_begin [0],
-    (size_t) bsp_section_data_size
-  );
-  mpc55xx_siu_init();
-  mpc55xx_ebi_chip_select_init();
-  mpc55xx_ebi_init();
-  mpc55xx_mmu_init();
-}
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/fmpll-syncr-vals.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/fmpll-syncr-vals.c
deleted file mode 100644
index 0fb7a8d4ae..0000000000
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/fmpll-syncr-vals.c
+++ /dev/null
@@ -1,92 +0,0 @@
-/**
- * @file
- *
- * @ingroup mpc55xx
- *
- * @brief BSP startup code.
- */
-
-/*
- * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
- *
- *  embedded brains GmbH
- *  Obere Lagerstr. 30
- *  82178 Puchheim
- *  Germany
- *  <rtems@embedded-brains.de>
- *
- * The license and distribution terms for this file may be
- * found in the file LICENSE in this distribution or at
- * http://www.rtems.com/license/LICENSE.
- *
- * $Id$
- */
-
-#include <mpc55xx/regs.h>
-
-#if MPC55XX_CHIP_TYPE / 10 == 551 || MPC55XX_CHIP_TYPE / 10 == 567
-/*
- * define init values for FMPLL ESYNCRx
- * (used in start.S/fmpll.S)
- */
-#define EPREDIV_VAL (MPC55XX_FMPLL_PREDIV-1)
-#define EMFD_VAL    (MPC55XX_FMPLL_MFD-16)
-#define VCO_CLK_REF (MPC55XX_FMPLL_REF_CLOCK/(EPREDIV_VAL+1))
-#define VCO_CLK_OUT (VCO_CLK_REF*(EMFD_VAL+16))
-#define ERFD_VAL    ((VCO_CLK_OUT/MPC55XX_FMPLL_CLK_OUT)-1)
-
-const struct fmpll_syncr_vals_t {
-  union FMPLL_ESYNCR2_tag esyncr2_temp;
-  union FMPLL_ESYNCR2_tag esyncr2_final;
-  union FMPLL_ESYNCR1_tag esyncr1_final;
-} mpc55xx_fmpll_config =
-  {
-    { /* esyncr2_temp */
-      .B.LOCEN=0,
-      .B.LOLRE=0,
-      .B.LOCRE=0,
-      .B.LOLIRQ=0,
-      .B.LOCIRQ=0,
-      .B.ERATE=0,
-      .B.EDEPTH=0,
-      .B.ERFD=ERFD_VAL+2 /* reduce output clock during init */
-    },
-    { /* esyncr2_final */
-      .B.LOCEN=0,
-      .B.LOLRE=0,
-      .B.LOCRE=0,
-      .B.LOLIRQ=0,
-      .B.LOCIRQ=0,
-      .B.ERATE=0,
-#if MPC55XX_CHIP_TYPE / 10 == 567
-      .B.CLKCFG_DIS=1,
-#endif
-      .B.EDEPTH=0,
-      .B.ERFD=ERFD_VAL /* nominal output clock after init */
-    },
-    { /* esyncr1_final */
-      .B.CLKCFG=7,
-      .B.EPREDIV=EPREDIV_VAL,
-      .B.EMFD=EMFD_VAL
-    }
-  };
-#else /* !(MPC55XX_CHIP_TYPE / 10 == 551 || MPC55XX_CHIP_TYPE / 10 == 567) */
-const struct fmpll_syncr_vals_t {
-  union FMPLL_SYNCR_tag syncr_temp;
-  union FMPLL_SYNCR_tag syncr_final;
-} mpc55xx_fmpll_config =
-  {
-    { /* syncr_temp */
-      .B.PREDIV=MPC55XX_FMPLL_PREDIV-1,
-      .B.MFD=MPC55XX_FMPLL_MFD,
-      .B.RFD=2,
-      .B.LOCEN=1
-    },
-    { /* syncr_final */
-      .B.PREDIV=MPC55XX_FMPLL_PREDIV-1,
-      .B.MFD=MPC55XX_FMPLL_MFD,
-      .B.RFD=0,
-      .B.LOCEN=1
-    }
-  };
-#endif /* !(MPC55XX_CHIP_TYPE / 10 == 551 || MPC55XX_CHIP_TYPE / 10 == 567) */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/get-system-clock.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/get-system-clock.c
new file mode 100644
index 0000000000..5e0c608d41
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/get-system-clock.c
@@ -0,0 +1,85 @@
+/**
+ * @file
+ *
+ * @ingroup mpc55xx
+ *
+ * @brief System clock calculation.
+ */
+
+/*
+ * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp.h>
+#include <bsp/start.h>
+#include <bsp/mpc55xx-config.h>
+
+uint32_t mpc55xx_get_system_clock(void)
+{
+  uint32_t system_clock = 0;
+
+  #ifdef MPC55XX_HAS_FMPLL
+    volatile struct FMPLL_tag *fmpll = &FMPLL;
+    union FMPLL_SYNSR_tag synsr = { .R = fmpll->SYNSR.R };
+    uint32_t reference_clock = MPC55XX_FMPLL_REF_CLOCK;
+    bool pll_clock_mode = synsr.B.MODE != 0;
+    bool crystal_or_external_reference_mode = synsr.B.PLLSEL != 0;
+
+    if (pll_clock_mode) {
+      if (crystal_or_external_reference_mode) {
+        union FMPLL_SYNCR_tag syncr = { .R = fmpll->SYNCR.R };
+        uint32_t prediv = syncr.B.PREDIV;
+        uint32_t mfd = syncr.B.MFD;
+        uint32_t rfd = syncr.B.RFD;
+
+        system_clock = ((reference_clock * (mfd + 4)) >> rfd) / (prediv + 1);
+      } else {
+        system_clock = 2 * reference_clock;
+      }
+    } else {
+      system_clock = reference_clock;
+    }
+  #endif
+
+  #ifdef MPC55XX_HAS_FMPLL_ENHANCED
+    volatile struct FMPLL_tag *fmpll = &FMPLL;
+    union FMPLL_ESYNCR1_tag esyncr1 = { .R = fmpll->ESYNCR1.R };
+    uint32_t reference_clock = MPC55XX_FMPLL_REF_CLOCK;
+    bool normal_mode = (esyncr1.B.CLKCFG & 0x4U) != 0;
+
+    if (normal_mode) {
+      union FMPLL_ESYNCR2_tag esyncr2 = { .R = fmpll->ESYNCR2.R };
+      uint32_t eprediv = esyncr1.B.EPREDIV;
+      uint32_t emfd = esyncr1.B.EMFD;
+      uint32_t erfd = esyncr2.B.ERFD;
+
+      system_clock = (reference_clock * (emfd + 16))
+        / ((erfd + 1) * (eprediv + 1));
+    } else {
+      system_clock = reference_clock;
+    }
+  #endif
+
+  #ifdef MPC55XX_HAS_MODE_CONTROL
+    /* FIXME: Assumes normal mode and external oscillator */
+    PLLD_CR_32B_tag cr = { . R = CGM.FMPLL [0].CR.R };
+    uint32_t xosc = MPC55XX_FMPLL_REF_CLOCK;
+    uint32_t ldf = cr.B.NDIV;
+    uint32_t idf = cr.B.IDF + 1;
+    uint32_t odf = 2U << cr.B.ODF;
+
+    system_clock = (xosc * ldf) / (idf * odf);
+  #endif
+
+  return system_clock;
+}
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/linkcmds.xkt564levb b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/linkcmds.xkt564levb
new file mode 100644
index 0000000000..503f52a887
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/linkcmds.xkt564levb
@@ -0,0 +1,36 @@
+MEMORY {
+	ROM : ORIGIN = 0x0, LENGTH = 1M
+	RAM_0 : ORIGIN = 0x40000000, LENGTH = 64K
+	RAM_1 : ORIGIN = 0x50000000, LENGTH = 64K
+	NIRVANA : ORIGIN = 0x0, LENGTH = 0
+}
+
+bsp_ram_start = ORIGIN (RAM_0);
+bsp_ram_size = LENGTH (RAM_0);
+bsp_ram_end = bsp_ram_start + bsp_ram_size;
+
+bsp_ram_1_start = ORIGIN (RAM_1);
+bsp_ram_1_size = LENGTH (RAM_1);
+bsp_ram_1_end = bsp_ram_1_start + bsp_ram_1_size;
+
+bsp_rom_start = ORIGIN (ROM);
+bsp_rom_size = LENGTH (ROM);
+bsp_rom_end = bsp_rom_start + bsp_rom_size;
+
+REGION_ALIAS ("REGION_START", ROM);
+REGION_ALIAS ("REGION_FAST_TEXT", RAM_0);
+REGION_ALIAS ("REGION_FAST_TEXT_LOAD", ROM);
+REGION_ALIAS ("REGION_TEXT", ROM);
+REGION_ALIAS ("REGION_TEXT_LOAD", ROM);
+REGION_ALIAS ("REGION_RODATA", ROM);
+REGION_ALIAS ("REGION_RODATA_LOAD", ROM);
+REGION_ALIAS ("REGION_FAST_DATA", RAM_0);
+REGION_ALIAS ("REGION_FAST_DATA_LOAD", ROM);
+REGION_ALIAS ("REGION_DATA", RAM_0);
+REGION_ALIAS ("REGION_DATA_LOAD", ROM);
+REGION_ALIAS ("REGION_BSS", RAM_0);
+REGION_ALIAS ("REGION_RWEXTRA", RAM_0);
+REGION_ALIAS ("REGION_WORK", RAM_1);
+REGION_ALIAS ("REGION_STACK", RAM_1);
+
+INCLUDE linkcmds.base
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/reset.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/reset.c
index d9a04972ab..e0318fc573 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/reset.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/reset.c
@@ -31,6 +31,10 @@
 void bsp_reset(void)
 {
   while (true) {
-    SIU.SRCR.R = 0x1;
+    #if MPC55XX_CHIP_TYPE / 10 == 564
+      /* TODO */
+    #else
+      SIU.SRCR.R = 1U << (31 - 0);
+    #endif
   }
 }
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/sd-card-init.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/sd-card-init.c
index 1f756a77b1..316dc81e3e 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/sd-card-init.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/sd-card-init.c
@@ -28,6 +28,8 @@
 
 #include <rtems/status-checks.h>
 
+#ifdef MPC55XX_BOARD_MPC5566EVB
+
 static rtems_status_code mpc55xx_dspi_init(void)
 {
 	int rv = 0;
@@ -38,10 +40,8 @@ static rtems_status_code mpc55xx_dspi_init(void)
 	rv = rtems_libi2c_initialize();
 	RTEMS_CHECK_RV_SC( rv, "rtems_libi2c_initialize");
 
-#if MPC55XX_CHIP_TYPE / 10 != 551
 	/* DSPI D inputs are taken from DSPI C */
 	SIU.DISR.R = 0x000000FC;
-#endif
 
 	/* DSPI A signals */
 	pcr.B.PA = 1;
@@ -158,3 +158,5 @@ rtems_status_code mpc55xx_sd_card_init( bool mount)
 
 	return RTEMS_SUCCESSFUL;
 }
+
+#endif /* MPC55XX_BOARD_MPC5566EVB */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-cache.S b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-cache.S
new file mode 100644
index 0000000000..f966ab6665
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-cache.S
@@ -0,0 +1,106 @@
+/**
+ * @file
+ *
+ * @ingroup mpc55xx_asm
+ *
+ * @brief Cache initialization.
+ */
+
+/*
+ * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <libcpu/powerpc-utility.h>
+
+#include <mpc55xx/regs.h>
+
+	.globl	mpc55xx_start_cache
+
+	.section	".bsp_start_text", "ax"
+
+mpc55xx_start_cache:
+
+	/* Load zero, CINV, and CABT) */
+	li	r0, 0
+	li	r3, 0x2
+	li	r4, 0x4
+
+#ifdef MPC55XX_HAS_INSTRUCTION_CACHE
+
+start_instruction_cache_invalidation:
+
+	/* Clear instruction cache invalidation abort */
+	mtspr	FSL_EIS_L1CSR1, r0
+
+	/* Start instruction cache invalidation */
+	mtspr	FSL_EIS_L1CSR1, r3
+
+get_instruction_cache_invalidation_status:
+
+	/* Get instruction cache invalidation status */
+	mfspr	r5, FSL_EIS_L1CSR1
+
+	/* Check CABT */
+	and.	r6, r5, r4
+	bne	start_instruction_cache_invalidation
+
+	/* Check CINV */
+	and.	r6, r5, r3
+	bne	get_instruction_cache_invalidation_status
+
+	/* Save instruction cache settings */
+	LWI	r6, 0x00010001
+	isync
+	msync
+	mtspr	FSL_EIS_L1CSR1, r6
+
+#endif /* MPC55XX_HAS_INSTRUCTION_CACHE */
+
+#if defined(MPC55XX_HAS_DATA_CACHE) || defined(MPC55XX_HAS_UNIFIED_CACHE)
+
+start_data_cache_invalidation:
+
+	/* Clear data cache invalidation abort */
+	mtspr	FSL_EIS_L1CSR0, r0
+
+	/* Start data cache invalidation */
+	mtspr	FSL_EIS_L1CSR0, r3
+
+get_data_cache_invalidation_status:
+
+	/* Get data cache invalidation status */
+	mfspr	r5, FSL_EIS_L1CSR0
+
+	/* Check CABT */
+	and.	r6, r5, r4
+	bne	start_data_cache_invalidation
+
+	/* Check CINV */
+	and.	r6, r5, r3
+	bne	get_data_cache_invalidation_status
+
+	/* Save data cache settings */
+#if MPC55XX_CHIP_TYPE / 10 != 567
+	/* FIXME: CORG??? 0x00180011 */
+	LWI	r6, 0x00100001
+#else
+	LWI	r6, 0x00190001
+#endif
+	isync
+	msync
+	mtspr	FSL_EIS_L1CSR0, r6
+
+#endif /* defined(MPC55XX_HAS_DATA_CACHE) || defined(MPC55XX_HAS_UNIFIED_CACHE) */
+
+	/* Return */
+	blr
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-clock.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-clock.c
new file mode 100644
index 0000000000..6b8ff1eb77
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-clock.c
@@ -0,0 +1,99 @@
+/**
+ * @file
+ *
+ * @ingroup mpc55xx
+ *
+ * @brief Clock and FMPLL initialization code.
+ */
+
+/*
+ * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp.h>
+#include <bsp/start.h>
+#include <bsp/bootcard.h>
+#include <bsp/mpc55xx-config.h>
+
+#if defined(MPC55XX_HAS_FMPLL) || defined(MPC55XX_HAS_FMPLL_ENHANCED)
+  static BSP_START_TEXT_SECTION void fmpll_wait_for_lock(void)
+  {
+    int i = 0;
+    bool lock = false;
+
+    while (!lock && i < 6000) {
+      lock = FMPLL.SYNSR.B.LOCK != 0;
+      ++i;
+    }
+
+    if (!lock) {
+      bsp_reset();
+    }
+  }
+#endif
+
+BSP_START_TEXT_SECTION void mpc55xx_start_clock(void)
+{
+  const mpc55xx_clock_config *cfg = mpc55xx_start_config_clock;
+
+  #ifdef MPC55XX_HAS_FMPLL
+    volatile struct FMPLL_tag *fmpll = &FMPLL;
+
+    fmpll->SYNCR.R = cfg->syncr_tmp.R;
+    fmpll->SYNCR.R;
+    fmpll_wait_for_lock();
+
+    fmpll->SYNCR.R = cfg->syncr_final.R;
+    fmpll->SYNCR.R;
+    fmpll_wait_for_lock();
+  #endif
+
+  #ifdef MPC55XX_HAS_FMPLL_ENHANCED
+    volatile struct FMPLL_tag *fmpll = &FMPLL;
+
+    fmpll->ESYNCR2.R = cfg->esyncr2_tmp.R;
+    fmpll->ESYNCR2.R;
+    fmpll->ESYNCR1.R = cfg->esyncr1_final.R;
+    fmpll->ESYNCR1.R;
+    fmpll_wait_for_lock();
+
+    fmpll->ESYNCR2.R = cfg->esyncr2_final.R;
+    fmpll->ESYNCR2.R;
+    fmpll_wait_for_lock();
+
+    #if MPC55XX_CHIP_TYPE / 10 == 551
+      /* System clock supplied by PLL */
+      SIU.SYSCLK.B.SYSCLKSEL = 2;
+    #endif
+  #endif
+
+  #ifdef MPC55XX_HAS_MODE_CONTROL
+    volatile CGM_tag *cgm = &CGM;
+    size_t fmpll_count = sizeof(cfg->fmpll) / sizeof(cfg->fmpll [0]);
+    size_t auxclk_count = sizeof(cfg->auxclk) / sizeof(cfg->auxclk [0]);
+    size_t i = 0;
+
+    for (i = 0; i < auxclk_count; ++i) {
+      cgm->AUXCLK [i].AC_SC.R = cfg->auxclk [i].AC_SC.R;
+      cgm->AUXCLK [i].AC_DC0_3.R = cfg->auxclk [i].AC_DC0_3.R;
+    }
+
+    for (i = 0; i < fmpll_count; ++i) {
+      cgm->FMPLL [i].CR.R = cfg->fmpll [i].cr.R;
+      cgm->FMPLL [i].MR.R = cfg->fmpll [i].mr.R;
+    }
+
+    cgm->OC_EN.R = cfg->oc_en.R;
+    cgm->OCDS_SC.R = cfg->ocds_sc.R;
+  #endif
+}
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-clock.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-clock.c
new file mode 100644
index 0000000000..0b0d568d8b
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-clock.c
@@ -0,0 +1,122 @@
+/**
+ * @file
+ *
+ * @ingroup mpc55xx
+ *
+ * @brief Clock and FMPLL configuration.
+ */
+
+/*
+ * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp/mpc55xx-config.h>
+
+BSP_START_TEXT_SECTION const mpc55xx_clock_config
+  mpc55xx_start_config_clock [1] = { {
+  #ifdef MPC55XX_HAS_FMPLL
+    .syncr_tmp = {
+      .B = {
+        .PREDIV = MPC55XX_FMPLL_PREDIV - 1,
+        .MFD = MPC55XX_FMPLL_MFD,
+        .RFD = 2,
+        .LOCEN = 1
+      }
+    },
+    .syncr_final = {
+      .B = {
+        .PREDIV = MPC55XX_FMPLL_PREDIV - 1,
+        .MFD = MPC55XX_FMPLL_MFD,
+        .RFD = 0,
+        .LOCEN = 1,
+        .LOLIRQ = 1,
+        .LOCIRQ = 1
+      }
+    }
+  #endif
+  #ifdef MPC55XX_HAS_FMPLL_ENHANCED
+    #define EPREDIV_VAL (MPC55XX_FMPLL_PREDIV-1)
+    #define EMFD_VAL    (MPC55XX_FMPLL_MFD-16)
+    #define VCO_CLK_REF (MPC55XX_FMPLL_REF_CLOCK/(EPREDIV_VAL+1))
+    #define VCO_CLK_OUT (VCO_CLK_REF*(EMFD_VAL+16))
+    #define ERFD_VAL    ((VCO_CLK_OUT/MPC55XX_FMPLL_CLK_OUT)-1)
+
+    .esyncr2_tmp = {
+      .B = {
+        .LOCEN = 0,
+        .LOLRE = 0,
+        .LOCRE = 0,
+        .LOLIRQ = 0,
+        .LOCIRQ = 0,
+        .ERATE = 0,
+        .EDEPTH = 0,
+        .ERFD = ERFD_VAL + 2 /* reduce output clock during init */
+      }
+    },
+    .esyncr2_final = {
+      .B = {
+        .LOCEN = 0,
+        .LOLRE = 0,
+        .LOCRE = 0,
+        .LOLIRQ = 0,
+        .LOCIRQ = 0,
+        .ERATE = 0,
+        #if MPC55XX_CHIP_TYPE / 10  == 567
+          .CLKCFG_DIS = 1,
+        #endif
+        .EDEPTH = 0,
+        .ERFD = ERFD_VAL /* nominal output clock after init */
+      }
+    },
+    .esyncr1_final = {
+      .B = {
+        .CLKCFG = 7,
+        .EPREDIV = EPREDIV_VAL,
+        .EMFD = EMFD_VAL
+      }
+    }
+  #endif
+  #ifdef MPC55XX_HAS_MODE_CONTROL
+    .fmpll = {
+      {
+        .cr = {
+          .B = { .IDF = 3, .ODF = 1, .NDIV = 48, .I_LOCK = 1, .PLL_ON = 1 }
+        }
+      },
+      {
+        .cr = {
+          .B = { .IDF = 3, .ODF = 2, .NDIV = 32, .I_LOCK = 1, .PLL_ON = 1 }
+        }
+      }
+    },
+    .ocds_sc = {
+      .B = { .SELDIV = 2, .SELCTL = 2 }
+    },
+    .auxclk = {
+      [1] = {
+        .AC_SC = { .B = { .SELCTL = 4 } },
+        .AC_DC0_3 = { .B = { .DE0 = 1, .DIV0 = 11 } }
+      },
+      [2] = {
+        .AC_SC = { .B = { .SELCTL = 4 } },
+        .AC_DC0_3 = { .B = { .DE0 = 1, .DIV0 = 11 } }
+      },
+      [3] = {
+        .AC_SC = { .B = { .SELCTL = 1 } }
+      },
+      [4] = {
+        .AC_SC = { .B = { .SELCTL = 1 } }
+      }
+    }
+  #endif
+} };
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/ebi-cal-cs-config.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-ebi-cs-cal.c
index 4496c69eb9..5095252a38 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/ebi-cal-cs-config.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-ebi-cs-cal.c
@@ -3,7 +3,7 @@
  *
  * @ingroup mpc55xx
  *
- * @brief MPC55XX EBI calibration chip-select configuration.
+ * @brief EBI calibration chip-select configuration.
  */
 
 /*
@@ -19,16 +19,19 @@
  * found in the file LICENSE in this distribution or at
  * http://www.rtems.com/license/LICENSE.
  *
- * $Id$
+ * $Id: ebi-cal-cs-config.c,v 1.1 2011/08/31 16:03:09 sh Exp $
  */
 
 #include <bsp/mpc55xx-config.h>
 #include <bsp/start.h>
 #include <bsp.h>
 
-const BSP_START_TEXT_SECTION struct EBI_CAL_CS_tag
-mpc55xx_ebi_cal_cs_config [] = {
+#ifdef MPC55XX_HAS_EBI
+
+BSP_START_TEXT_SECTION const struct EBI_CAL_CS_tag
+  mpc55xx_start_config_ebi_cal_cs [] = {
 #if defined(MPC55XX_BOARD_MPC5674FEVB)
+  /* External SRAM */
   {
     .BR = {
       .B = {
@@ -50,10 +53,35 @@ mpc55xx_ebi_cal_cs_config [] = {
         .BSCY = 0
       }
     }
+  },
+  /* External Ethernet controller */
+  {
+    .BR = {
+      .B = {
+        .BA = 0x3fff8000 >> 15,
+        .PS = 0,
+        .AD_MUX = 1,
+        .BL = 0,
+        .WEBS = 0,
+        .TBDIP = 0,
+        .SETA = 0,
+        .BI = 1,
+        .V = 1
+      }
+    },
+    .OR = {
+      .B = {
+        .AM = 0xfff80000 >> 15,
+        .SCY = 1,
+        .BSCY = 0
+      }
+    }
   }
 #endif
 };
 
-const BSP_START_TEXT_SECTION size_t mpc55xx_ebi_cal_cs_config_count [] = {
-  sizeof(mpc55xx_ebi_cal_cs_config) / sizeof(mpc55xx_ebi_cal_cs_config [0])
+BSP_START_TEXT_SECTION const size_t mpc55xx_start_config_ebi_cal_cs_count [] = {
+  sizeof(mpc55xx_start_config_ebi_cal_cs) / sizeof(mpc55xx_start_config_ebi_cal_cs [0])
 };
+
+#endif /* MPC55XX_HAS_EBI */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/ebi-cs-config.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-ebi-cs.c
index a130f6f802..0f4e6184e4 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/ebi-cs-config.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-ebi-cs.c
@@ -3,7 +3,7 @@
  *
  * @ingroup mpc55xx
  *
- * @brief MPC55XX EBI chip-select configuration.
+ * @brief EBI chip-select configuration.
  */
 
 /*
@@ -19,15 +19,17 @@
  * found in the file LICENSE in this distribution or at
  * http://www.rtems.com/license/LICENSE.
  *
- * $Id$
+ * $Id: ebi-cs-config.c,v 1.1 2011/08/31 16:03:09 sh Exp $
  */
 
 #include <bsp/mpc55xx-config.h>
 #include <bsp/start.h>
 #include <bsp.h>
 
-const BSP_START_TEXT_SECTION struct EBI_CS_tag
-mpc55xx_ebi_cs_config [] = {
+#ifdef MPC55XX_HAS_EBI
+
+BSP_START_TEXT_SECTION const struct EBI_CS_tag
+  mpc55xx_start_config_ebi_cs [] = {
 #if defined(MPC55XX_BOARD_GWLCFM)
 	/* CS0: External SRAM (16 bit, 1 wait states, 512kB, no burst) */
 	{
@@ -87,7 +89,7 @@ mpc55xx_ebi_cs_config [] = {
             .B.AD_MUX = 1,
             .B.WEBS = 0,
             .B.TBDIP = 0,
-            .B.BI = 1, 
+            .B.BI = 1,
             .B.V = 1
         },
 
@@ -137,7 +139,7 @@ mpc55xx_ebi_cs_config [] = {
             .B.SCY = 0,
             .B.BSCY = 0
         }
-    }, 
+    },
     { { .R = 0 }, { .R = 0 } },   /* CS1: Unused. */
     { { .R = 0 }, { .R = 0 } },   /* CS2: Unused. */
     {   /* CS3: ethernet? */
@@ -160,6 +162,8 @@ mpc55xx_ebi_cs_config [] = {
 #endif
 };
 
-const BSP_START_TEXT_SECTION size_t mpc55xx_ebi_cs_config_count [] = {
-  sizeof(mpc55xx_ebi_cs_config) / sizeof(mpc55xx_ebi_cs_config [0])
+BSP_START_TEXT_SECTION const size_t mpc55xx_start_config_ebi_cs_count [] = {
+  sizeof(mpc55xx_start_config_ebi_cs) / sizeof(mpc55xx_start_config_ebi_cs [0])
 };
+
+#endif /* MPC55XX_HAS_EBI */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-mmu-early.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-mmu-early.c
new file mode 100644
index 0000000000..d243bee245
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-mmu-early.c
@@ -0,0 +1,42 @@
+/**
+ * @file
+ *
+ * @ingroup mpc55xx
+ *
+ * @brief MMU early configuration.
+ */
+
+/*
+ * Copyright (c) 2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp/start.h>
+#include <bsp/mpc55xx-config.h>
+
+BSP_START_TEXT_SECTION const struct MMU_tag
+  mpc55xx_start_config_mmu_early [] = {
+#if MPC55XX_CHIP_TYPE / 10 == 564
+  /* Internal flash 1M */
+  MPC55XX_MMU_TAG_INITIALIZER(0, 0x00000000, MPC55XX_MMU_1M, 1, 0, 1, 1),
+  /* IO */
+  MPC55XX_MMU_TAG_INITIALIZER(1, 0xffe00000, MPC55XX_MMU_2M, 0, 1, 1, 1),
+  MPC55XX_MMU_TAG_INITIALIZER(2, 0xc3f00000, MPC55XX_MMU_1M, 0, 1, 1, 1),
+  /* Internal SRAM 64k */
+  MPC55XX_MMU_TAG_INITIALIZER(3, 0x40000000, MPC55XX_MMU_64K, 1, 1, 1, 1)
+#endif
+};
+
+BSP_START_TEXT_SECTION const size_t mpc55xx_start_config_mmu_early_count [] = {
+  sizeof(mpc55xx_start_config_mmu_early)
+    / sizeof(mpc55xx_start_config_mmu_early [0])
+};
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/mmu-config.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-mmu.c
index f4561f620e..d5df9ffbb4 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/mmu-config.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-mmu.c
@@ -3,7 +3,7 @@
  *
  * @ingroup mpc55xx
  *
- * @brief MPC55XX MMU configuration.
+ * @brief MMU configuration.
  */
 
 /*
@@ -18,21 +18,19 @@
  * The license and distribution terms for this file may be
  * found in the file LICENSE in this distribution or at
  * http://www.rtems.com/license/LICENSE.
- *
- * $Id$
  */
 
-#include <bsp/mpc55xx-config.h>
-#include <bsp/start.h>
 #include <bsp.h>
+#include <bsp/start.h>
+#include <bsp/mpc55xx-config.h>
 
-const BSP_START_TEXT_SECTION struct MMU_tag
-mpc55xx_mmu_config [] = {
+BSP_START_TEXT_SECTION const struct MMU_tag
+  mpc55xx_start_config_mmu [] = {
 #if defined(MPC55XX_BOARD_GWLCFM)
   /* External Ethernet Controller 64k */
-  MPC55XX_MMU_TAG_INITIALIZER(5, 0x3fff8000, 6, 0, 1, 1, 1)
+  MPC55XX_MMU_TAG_INITIALIZER(5, 0x3fff8000, MPC55XX_MMU_64K, 0, 1, 1, 1)
 #elif defined(MPC55XX_BOARD_PHYCORE_MPC5554)
-    /* XXX I'm not using TLB1 entry 2 the same way as 
+    /* XXX I'm not using TLB1 entry 2 the same way as
 	 * in the BAM.
      */
     /*  Set up MMU TLB1 entry 2 for external ram. */
@@ -87,39 +85,50 @@ mpc55xx_mmu_config [] = {
 	 */
 #elif defined(MPC55XX_BOARD_MPC5566EVB)
   /* Internal flash 3M */
-  MPC55XX_MMU_TAG_INITIALIZER(1, 0x00000000, 6, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(5, 0x00010000, 6, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(6, 0x00020000, 6, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(7, 0x00030000, 6, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(8, 0x00040000, 8, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(9, 0x00080000, 8, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(10, 0x000c0000, 8, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(11, 0x00100000, 10, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(12, 0x00200000, 10, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(1, 0x00000000, MPC55XX_MMU_64K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(5, 0x00010000, MPC55XX_MMU_64K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(6, 0x00020000, MPC55XX_MMU_64K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(7, 0x00030000, MPC55XX_MMU_64K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(8, 0x00040000, MPC55XX_MMU_256K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(9, 0x00080000, MPC55XX_MMU_256K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(10, 0x000c0000, MPC55XX_MMU_256K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(11, 0x00100000, MPC55XX_MMU_1M, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(12, 0x00200000, MPC55XX_MMU_1M, 1, 0, 1, 0),
   /* External SRAM 512k */
-  MPC55XX_MMU_TAG_INITIALIZER(2, 0x20000000, 8, 0, 1, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(13, 0x20040000, 8, 0, 1, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(2, 0x20000000, MPC55XX_MMU_256K, 0, 1, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(13, 0x20040000, MPC55XX_MMU_256K, 0, 1, 1, 0),
   /* Internal SRAM 128k */
-  MPC55XX_MMU_TAG_INITIALIZER(3, 0x40010000, 6, 0, 1, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(14, 0x40000000, 6, 0, 1, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(3, 0x40010000, MPC55XX_MMU_64K, 0, 1, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(14, 0x40000000, MPC55XX_MMU_64K, 0, 1, 1, 0),
   /* External Ethernet Controller 64k */
-  MPC55XX_MMU_TAG_INITIALIZER(15, 0x3fff8000, 6, 0, 1, 1, 1)
+  MPC55XX_MMU_TAG_INITIALIZER(15, 0x3fff8000, MPC55XX_MMU_64K, 0, 1, 1, 1)
 #elif defined(MPC55XX_BOARD_MPC5674FEVB)
   /* Internal flash 4M */
-  MPC55XX_MMU_TAG_INITIALIZER(1, 0x00000000, 6, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(5, 0x00010000, 6, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(6, 0x00020000, 7, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(7, 0x00040000, 8, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(8, 0x00080000, 9, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(9, 0x00100000, 10, 1, 0, 1, 0),
-  MPC55XX_MMU_TAG_INITIALIZER(10, 0x00200000, 11, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(1, 0x00000000, MPC55XX_MMU_64K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(5, 0x00010000, MPC55XX_MMU_64K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(6, 0x00020000, MPC55XX_MMU_128K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(7, 0x00040000, MPC55XX_MMU_256K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(8, 0x00080000, MPC55XX_MMU_512K, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(9, 0x00100000, MPC55XX_MMU_1M, 1, 0, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(10, 0x00200000, MPC55XX_MMU_2M, 1, 0, 1, 0),
   /* External SRAM 512k */
-  MPC55XX_MMU_TAG_INITIALIZER(2, 0x20000000, 9, 0, 1, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(2, 0x20000000, MPC55XX_MMU_512K, 0, 1, 1, 0),
   /* Internal SRAM 256k */
-  MPC55XX_MMU_TAG_INITIALIZER(3, 0x40000000, 8, 0, 1, 1, 0)
+  MPC55XX_MMU_TAG_INITIALIZER(3, 0x40000000, MPC55XX_MMU_256K, 0, 1, 1, 0),
+  /* External Ethernet controller */
+  MPC55XX_MMU_TAG_INITIALIZER(15, 0x3fff8000, MPC55XX_MMU_64K, 0, 1, 1, 1)
+#elif MPC55XX_CHIP_TYPE / 10 == 564
+  /* Internal flash 1M */
+  MPC55XX_MMU_TAG_INITIALIZER(0, 0x00000000, MPC55XX_MMU_1M, 1, 0, 1, 0),
+  /* IO */
+  MPC55XX_MMU_TAG_INITIALIZER(1, 0xffe00000, MPC55XX_MMU_2M, 0, 1, 1, 1),
+  MPC55XX_MMU_TAG_INITIALIZER(2, 0xc3f00000, MPC55XX_MMU_1M, 0, 1, 1, 1),
+  /* Internal SRAM 64k + 64k */
+  MPC55XX_MMU_TAG_INITIALIZER(3, 0x40000000, MPC55XX_MMU_64K, 0, 1, 1, 0),
+  MPC55XX_MMU_TAG_INITIALIZER(4, 0x50000000, MPC55XX_MMU_64K, 0, 1, 1, 0)
 #endif
 };
 
-const BSP_START_TEXT_SECTION size_t mpc55xx_mmu_config_count [] = {
-  sizeof(mpc55xx_mmu_config) / sizeof(mpc55xx_mmu_config [0])
+BSP_START_TEXT_SECTION const size_t mpc55xx_start_config_mmu_count [] = {
+  sizeof(mpc55xx_start_config_mmu) / sizeof(mpc55xx_start_config_mmu [0])
 };
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/siu-pcr-config.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-siu-pcr.c
index ab73ccc9b3..f198eaf2ec 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/siu-pcr-config.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-siu-pcr.c
@@ -3,7 +3,7 @@
  *
  * @ingroup mpc55xx
  *
- * @brief MPC55XX SIU PCR configuration.
+ * @brief SIU PCR configuration.
  */
 
 /*
@@ -19,15 +19,15 @@
  * found in the file LICENSE in this distribution or at
  * http://www.rtems.com/license/LICENSE.
  *
- * $Id$
+ * $Id: siu-pcr-config.c,v 1.1 2011/08/31 16:03:10 sh Exp $
  */
 
 #include <bsp/mpc55xx-config.h>
 #include <bsp/start.h>
 #include <bsp.h>
 
-const BSP_START_TEXT_SECTION mpc55xx_siu_pcr_config_entry
-mpc55xx_siu_pcr_config [] = {
+BSP_START_TEXT_SECTION const mpc55xx_siu_pcr_config
+  mpc55xx_start_config_siu_pcr [] = {
 #if defined(MPC55XX_BOARD_GWLCFM)
   {  0,16,{.B.PA = 1,           .B.WPE = 0}}, /* PA[ 0..15] analog input */
   { 16, 4,{.B.PA = 0,.B.OBE = 1,.B.WPE = 0}}, /* PB[ 0.. 4] LED/CAN_STBN out */
@@ -56,15 +56,15 @@ mpc55xx_siu_pcr_config [] = {
   { 53, 1,{.B.PA = 1,.B.IBE = 1,.B.WPE = 0}}, /* PD[ 5    ] LS_CAN_RX in  */
   { 54, 1,{.B.PA = 1,.B.OBE = 1,.B.WPE = 0}}, /* PD[ 6    ] HS_CAN_TX out */
   { 55, 1,{.B.PA = 1,.B.IBE = 1,.B.WPE = 0}}, /* PD[ 7    ] HS_CAN_RX in  */
-  { 56, 1,{.B.PA = 2,.B.IBE = 1,.B.OBE = 1,.B.WPE = 0}}, 
+  { 56, 1,{.B.PA = 2,.B.IBE = 1,.B.OBE = 1,.B.WPE = 0}},
   /* PD[ 8    ] I2C_SCL in/out */
-  { 57, 1,{.B.PA = 2,.B.IBE = 1,.B.OBE = 1,.B.WPE = 0}}, 
+  { 57, 1,{.B.PA = 2,.B.IBE = 1,.B.OBE = 1,.B.WPE = 0}},
   /* PD[ 9    ] I2C_SDA in/out */
-  
+
   { 58, 1,{.B.PA = 0,.B.OBE = 1,.B.WPE = 0}}, /* PD[10] LS_CAN_EN     out*/
-  { 59, 3,{.B.PA = 0,.B.IBE = 1,.B.WPE = 0}}, 
+  { 59, 3,{.B.PA = 0,.B.IBE = 1,.B.WPE = 0}},
   /* PD[11..13] PWO1_OC, MOCO_INT in */
-  
+
   { 62, 4,{.B.PA = 0,.B.IBE = 1,.B.WPE = 0}}, /* PD[14..15] USB_FLGA/B    in */
 
   { 64, 5,{.B.PA = 0,.B.OBE = 1,.B.WPE = 0}}, /* PE[ 0.. 4] LED_EXT1-5.   out*/
@@ -99,7 +99,7 @@ mpc55xx_siu_pcr_config [] = {
   {  3, 1,{.B.PA = 1,.B.DSC = 1,.B.WPE=1,.B.WPS=1}}, /* !CS  [3]      */
   {  4,24,{.B.PA = 1,.B.DSC = 1                  }}, /* ADDR [8 : 31] */
   { 28,16,{.B.PA = 1,.B.DSC = 1                  }}, /* DATA [0 : 15] */
-  { 62, 8,{.B.PA = 1,.B.DSC = 1,.B.WPE=1,.B.WPS=1}}, /* RD_!WR, BDIP, 
+  { 62, 8,{.B.PA = 1,.B.DSC = 1,.B.WPE=1,.B.WPS=1}}, /* RD_!WR, BDIP,
 							!WE, !OE, !TS */
   { 89, 2,{.B.PA = 1                             }} /* ESCI_B        */
 #elif defined(MPC55XX_BOARD_MPC5674FEVB)
@@ -110,10 +110,11 @@ mpc55xx_siu_pcr_config [] = {
   { 263, 15, { .B = { .PA = 2, .DSC = 1 } } }, /* D_ADD_DAT16 .. D_ADD_DAT30 */
   { 278, 16, { .B = { .PA = 1, .DSC = 1 } } }, /* D_ADD_DAT0 .. D_ADD_DAT15 */
   { 294,  6, { .B = { .PA = 1, .DSC = 1 } } }, /* D_RD_WR, D_WE0, D_WE1, D_OE, D_TS, D_ALE */
+  { 301,  1, { .B = { .PA = 1, .DSC = 1 } } }, /* D_CS1 */
   { 302,  6, { .B = { .PA = 1, .DSC = 1 } } } /* D_BDIP, D_WE2, D_WE3, D_ADD9 .. D_ADD11 */
 #endif
 };
 
-const BSP_START_TEXT_SECTION size_t mpc55xx_siu_pcr_config_count [] = {
-  sizeof(mpc55xx_siu_pcr_config) / sizeof(mpc55xx_siu_pcr_config [0])
+BSP_START_TEXT_SECTION const size_t mpc55xx_start_config_siu_pcr_count [] = {
+  sizeof(mpc55xx_start_config_siu_pcr) / sizeof(mpc55xx_start_config_siu_pcr [0])
 };
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c
new file mode 100644
index 0000000000..4d0c35da58
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c
@@ -0,0 +1,175 @@
+/**
+ * @file
+ *
+ * @ingroup mpc55xx
+ *
+ * @brief Early initialization code.
+ */
+
+/*
+ * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp.h>
+#include <bsp/start.h>
+#include <bsp/mpc55xx-config.h>
+
+static BSP_START_TEXT_SECTION void mpc55xx_start_mmu(void)
+{
+  #ifdef MPC55XX_BOOTFLAGS
+    /* If the low bit of bootflag 0 is clear don't change the MMU.  */
+    bool do_mmu_config = (mpc55xx_bootflag_0 [0] & 1) != 0;
+  #else
+    bool do_mmu_config = true;
+  #endif
+
+  if (do_mmu_config) {
+    mpc55xx_start_mmu_apply_config(
+      &mpc55xx_start_config_mmu [0],
+      mpc55xx_start_config_mmu_count [0]
+    );
+  }
+}
+
+static BSP_START_TEXT_SECTION void mpc55xx_start_internal_ram(void)
+{
+  /* Initialize internal SRAM to zero (ECC) */
+  bsp_start_zero(
+    (char *) bsp_ram_start + MPC55XX_EARLY_STACK_SIZE,
+    (size_t) bsp_ram_size - MPC55XX_EARLY_STACK_SIZE
+  );
+  #ifdef MPC55XX_HAS_SECOND_INTERNAL_RAM_AREA
+    bsp_start_zero(&bsp_ram_1_start [0], (size_t) bsp_ram_1_size);
+  #endif
+}
+
+static BSP_START_TEXT_SECTION void mpc55xx_start_mode_change(void)
+{
+  #ifdef MPC55XX_HAS_MODE_CONTROL
+    uint32_t mctl_key1 = 0x5af0;
+    uint32_t mctl_key2 = 0xa50f;
+    int i = 0;
+
+    /* Clear any pending RGM status */
+    RGM.FES.R = 0xffff;
+    RGM.DES.R = 0xffff;
+
+    /* Make sure XOSC and PLLs are on in RUN0 state */
+    ME.DRUN_MC.R = 0x001f0074;
+    ME.RUN_MC [0].R = 0x001f0074;
+
+    /*
+     * Make sure all peripherals are active in DRUN and RUN0 state.
+     *
+     * FIXME: This might be optimized to reduce power consumtion.
+     */
+    for (i = 0; i < 8; ++i) {
+      ME_RUN_PC_32B_tag run_pc = { .R = ME.RUN_PC [i].R };
+
+      run_pc.B.DRUN = 1;
+      run_pc.B.RUN0 = 1;
+
+      ME.RUN_PC [i].R = run_pc.R;
+    }
+
+    /* Switch to RUN0 state */
+    ME.MCTL.R = 0x40000000 | mctl_key1;
+    ME.MCTL.R = 0x40000000 | mctl_key2;
+
+    while (ME.GS.B.S_MTRANS) {
+      /* Wait for mode switch to be completed */
+    }
+  #endif
+}
+
+static BSP_START_TEXT_SECTION void mpc55xx_start_siu(void)
+{
+  size_t i = 0;
+
+  #ifdef MPC55XX_BOARD_GWLCFM
+    SIU.GPDO[122].B.PDO=1; /* make sure USB reset is kept high */
+    SIU.GPDO[121].B.PDO=1; /* make sure Ethernet reset is kept high */
+    SIU.GPDO[113].B.PDO=1; /* make sure MOST Companion reset is kept high */
+  #endif
+
+  for (i = 0; i < mpc55xx_start_config_siu_pcr_count [0]; ++i) {
+     const mpc55xx_siu_pcr_config *e = &mpc55xx_start_config_siu_pcr [i];
+     int j = e->index;
+     int n = j + e->count;
+     uint16_t pcr = e->pcr.R;
+
+     while (j < n) {
+       SIU.PCR [j].R = pcr;
+       ++j;
+     }
+  }
+}
+
+static BSP_START_TEXT_SECTION void mpc55xx_start_ebi_chip_select(void)
+{
+  #ifdef MPC55XX_HAS_EBI
+    size_t i = 0;
+
+    for (i = 0; i < mpc55xx_start_config_ebi_cs_count [0]; ++i) {
+       EBI.CS [i] = mpc55xx_start_config_ebi_cs [i];
+    }
+
+    for (i = 0; i < mpc55xx_start_config_ebi_cal_cs_count [0]; ++i) {
+       EBI.CAL_CS [i] = mpc55xx_start_config_ebi_cal_cs [i];
+    }
+  #endif
+}
+
+static BSP_START_TEXT_SECTION void mpc55xx_start_ebi(void)
+{
+  #if defined(MPC55XX_BOARD_GWLCFM)
+    /*
+     * init EBI for Muxed AD bus
+     */
+    EBI.MCR.B.DBM = 1;
+    EBI.MCR.B.AD_MUX = 1; /* use multiplexed bus */
+    EBI.MCR.B.D16_31 = 1; /* use lower AD bus    */
+
+    SIU.ECCR.B.EBDF = 3;  /* use CLK/4 as bus clock */
+  #elif defined(MPC55XX_BOARD_MPC5674FEVB)
+    struct EBI_tag ebi = {
+      .MCR = {
+        .B = {
+          .ACGE = 0,
+          .MDIS = 0,
+          .D16_31 = 0,
+          .AD_MUX = 0,
+          .DBM = 0
+        }
+      }
+    };
+
+    EBI.MCR.R = ebi.MCR.R;
+  #endif
+}
+
+BSP_START_TEXT_SECTION void mpc55xx_start_early(void)
+{
+  mpc55xx_start_watchdog();
+  mpc55xx_start_clock();
+  mpc55xx_start_flash();
+  #if defined(BSP_DATA_CACHE_ENABLED) || defined(BSP_INSTRUCTION_CACHE_ENABLED)
+    mpc55xx_start_cache();
+  #endif
+  mpc55xx_start_mmu();
+  mpc55xx_start_internal_ram();
+  mpc55xx_start_mode_change();
+  mpc55xx_start_siu();
+  mpc55xx_start_ebi_chip_select();
+  mpc55xx_start_ebi();
+}
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-flash.S b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-flash.S
new file mode 100644
index 0000000000..a6ed92eda7
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-flash.S
@@ -0,0 +1,115 @@
+/**
+ * @file
+ *
+ * @ingroup mpc55xx_asm
+ *
+ * @brief Flash configuration.
+ */
+
+/*
+ * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ *
+ * $Id: flash.S,v 1.2 2011/08/31 15:50:30 sh Exp $
+ */
+
+#include <libcpu/powerpc-utility.h>
+#include <mpc55xx/reg-defs.h>
+
+	.section	".bsp_start_text", "ax"
+
+/* Optimized flash configurations (Table 13-15 [MPC5567 Microcontroller Reference Manual]) */
+.equ FLASH_SETTINGS_RESET, 0xff00
+.equ FLASH_SETTINGS_82, FLASH_BUICR_CPU_PREFTCH | FLASH_BUICR_APC_1 | FLASH_BUICR_RWSC_1 | FLASH_BUICR_WWSC_1 | FLASH_BUICR_DPFEN_3 | FLASH_BUICR_IPFEN_3 | FLASH_BUICR_PFLIM_6 | FLASH_BUICR_BFEN
+.equ FLASH_SETTINGS_102, FLASH_BUICR_CPU_PREFTCH | FLASH_BUICR_APC_1 | FLASH_BUICR_RWSC_2 | FLASH_BUICR_WWSC_1 | FLASH_BUICR_DPFEN_3 | FLASH_BUICR_IPFEN_3 | FLASH_BUICR_PFLIM_6 | FLASH_BUICR_BFEN
+.equ FLASH_SETTINGS_132, FLASH_BUICR_CPU_PREFTCH | FLASH_BUICR_APC_2 | FLASH_BUICR_RWSC_3 | FLASH_BUICR_WWSC_1 | FLASH_BUICR_DPFEN_3 | FLASH_BUICR_IPFEN_3 | FLASH_BUICR_PFLIM_6 | FLASH_BUICR_BFEN
+.equ FLASH_SETTINGS_264, 0x01716B15
+
+/**
+ * @fn void mpc55xx_start_flash()
+ * @brief Optimized flash configuration.
+ * @warning Code will be copied and executed on the stack.
+ */
+GLOBAL_FUNCTION mpc55xx_start_flash
+#if MPC55XX_CHIP_TYPE / 10 == 564
+	blr
+#else
+	.equ	stack_size, 20
+	.equ	lr_offset, 28
+
+	/* Reserve stack frame */
+	stwu	r1, -stack_size(r1)
+	mflr	r0
+	stw	r0, lr_offset(r1)
+
+	/* Flash settings dependent on system clock */
+	bl	mpc55xx_get_system_clock
+	LWI	r4, 82000000
+	cmpw	r3, r4
+	ble	clock_82
+	LWI	r4, 102000000
+	cmpw	r3, r4
+	ble	clock_102
+	LWI	r4, 132000000
+	cmpw	r3, r4
+	ble	clock_132
+	LWI	r4, 264000000
+	cmpw	r3, r4
+	ble	clock_264
+	LWI	r3, FLASH_SETTINGS_RESET
+	b	settings_done
+clock_82:
+	LWI	r3, FLASH_SETTINGS_82
+	b	settings_done
+clock_102:
+	LWI	r3, FLASH_SETTINGS_102
+	b	settings_done
+clock_132:
+	LWI	r3, FLASH_SETTINGS_132
+	b settings_done
+clock_264:
+	LWI	r3, FLASH_SETTINGS_264
+	b	settings_done
+settings_done:
+
+	/* Copy store code on the stack */
+	LA	r4, store_start
+	lwz	r6, 0(r4)
+	lwz	r7, 4(r4)
+	lwz	r8, 8(r4)
+	stw	r6, 8(r1)
+	stw	r7, 12(r1)
+	stw	r8, 16(r1)
+
+	/* Execute store code */
+	LA	r4, FLASH_BIUCR
+	addi	r5, r1, 8
+	mtctr	r5
+	bctrl
+
+	/* Return */
+	lwz	r0, lr_offset(r1)
+	addi	r1, r1, stack_size
+	mtlr	r0
+	blr
+
+/*
+ * Store flash settings
+ */
+
+store_start:
+
+	stw	r3, 0(r4)
+	isync
+	blr
+
+#endif
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-watchdog.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-watchdog.c
new file mode 100644
index 0000000000..209405df0c
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-watchdog.c
@@ -0,0 +1,37 @@
+/**
+ * @file
+ *
+ * @ingroup mpc55xx
+ *
+ * @brief Watchdog initialization code.
+ */
+
+/*
+ * Copyright (c) 2011 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp.h>
+#include <bsp/start.h>
+#include <bsp/mpc55xx-config.h>
+
+BSP_START_TEXT_SECTION void mpc55xx_start_watchdog(void)
+{
+  #ifdef MPC55XX_HAS_SWT
+    /* Write keys to clear soft lock bit */
+    SWT.SR.R = 0x0000c520;
+    SWT.SR.R = 0x0000d928;
+
+    /* Clear watchdog enable (WEN) */
+    SWT.CR.R = 0x8000010A;
+  #endif
+}
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start.S b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start.S
index ee18d41a5f..f05e2c7f1c 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start.S
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start.S
@@ -18,134 +18,226 @@
  * The license and distribution terms for this file may be
  * found in the file LICENSE in this distribution or at
  * http://www.rtems.com/license/LICENSE.
- *
- * $Id$
- */
-
-/**
- * @defgroup mpc55xx_asm Assembler files
- *
- * @ingroup mpc55xx
  */
 
 #include <bspopts.h>
 
 #include <libcpu/powerpc-utility.h>
 
-#include <mpc55xx/reg-defs.h>
+#if MPC55XX_CHIP_TYPE / 10 != 551
+  #define HAS_SPE
+#endif
 
-#include <bsp/vectors.h>
+#if MPC55XX_CHIP_TYPE / 10 == 564
+  #define INIT_REGISTERS_FOR_LSM
+#endif
 
-#define HAS_CACHE (BSP_DATA_CACHE_ENABLED || BSP_INSTRUCTION_CACHE_ENABLED)
+#ifdef HAS_SPE
+  #define ZERO_GPR(reg) evxor	reg, reg, reg
+#else
+  #define ZERO_GPR(reg) xor	reg, reg, reg
+#endif
 
-	.extern mpc55xx_fmpll_config
-	.extern mpc55xx_fmpll_init
-	.extern mpc55xx_flash_init
-	.extern mpc55xx_early_init
-	.extern bsp_start_zero
-	.extern bsp_ram_start
-	.extern bsp_ram_size
-	.extern bsp_ram_end
-	.extern __eabi
-	.extern boot_card
+	.extern	__eabi
+	.extern	boot_card
+	.extern	bsp_ram_start
+	.extern	bsp_section_data_begin
+	.extern	bsp_section_data_load_begin
+	.extern	bsp_section_data_size
+	.extern	bsp_section_fast_data_begin
+	.extern	bsp_section_fast_data_load_begin
+	.extern	bsp_section_fast_data_size
+	.extern	bsp_section_fast_text_begin
+	.extern	bsp_section_fast_text_load_begin
+	.extern	bsp_section_fast_text_size
+	.extern	mpc55xx_start_config_mmu_early
+	.extern	mpc55xx_start_config_mmu_early_count
+	.extern	mpc55xx_start_early
+
+	.globl	_start
+	.globl	mpc55xx_start_mmu_apply_config
 
-	.globl _start
-	
 #ifdef MPC55XX_BOOTFLAGS
-        .globl mpc55xx_bootflag_0
-        .globl mpc55xx_bootflag_1
+	.globl	mpc55xx_bootflag_0
+	.globl	mpc55xx_bootflag_1
 #endif
 
-	.section ".bsp_start_text", "ax"
+	.section	".bsp_start_text", "ax"
 
 	/* BAM: RCHW */
-	.int 0x005a0000
+	.int	0x005a0000
 
 	/* BAM: Address of start instruction */
-        .int _start
+	.int	_start
 
 #ifdef MPC55XX_BOOTFLAGS
-        /*
-         * We skip over the next two boot flag words to the next 64-bit
-         * aligned start address. It is 64-bit aligned to play well with
-         * FLASH programming.  These boot flags can be set by debuggers
-         * and emulators to customize boot.  Currently bit0 of
-         * bootflag_0 means to "skip setting up the MMU", allowing
-         * external MMU setup in a debugger before branching to 0x10.
-         * This can be used e.g., to map FLASH into RAM.
-         */
+	/*
+	 * We skip over the next two boot flag words to the next 64-bit
+	 * aligned start address. It is 64-bit aligned to play well with
+	 * FLASH programming.  These boot flags can be set by debuggers
+	 * and emulators to customize boot.  Currently bit0 of
+	 * bootflag_0 means to "skip setting up the MMU", allowing
+	 * external MMU setup in a debugger before branching to 0x10.
+	 * This can be used e.g., to map FLASH into RAM.
+	 */
 mpc55xx_bootflag_0:
-        .int 0xffffffff
+	.int	0xffffffff
 mpc55xx_bootflag_1:
-        .int 0xffffffff
+	.int	0xffffffff
 #endif
 
 _start:
 
-	/* Enable time base */
-	li	r0, 0
-	mtspr	TBWU, r0
+	/* Enable SPE */
+#ifdef HAS_SPE
+	mfmsr	r3
+	oris	r3, r3, MSR_SPE >> 16
+	mtmsr	r3
+	isync
+#endif
+
+	/*
+	 * Initialization of core registers according to "e200z4 Power
+	 * Architecture Core Reference Manual" section 2.6 "Reset Settings"
+	 * table 2-16 "Reset Settings of e200 Resources".  This is necessary
+	 * for lock step mode (LSM).
+	 */
+	ZERO_GPR(r0)
+#ifdef INIT_REGISTERS_FOR_LSM
+	ZERO_GPR(r1)
+	ZERO_GPR(r2)
+	ZERO_GPR(r4)
+	ZERO_GPR(r5)
+	ZERO_GPR(r6)
+	ZERO_GPR(r7)
+	ZERO_GPR(r8)
+	ZERO_GPR(r9)
+	ZERO_GPR(r10)
+	ZERO_GPR(r11)
+	ZERO_GPR(r12)
+	ZERO_GPR(r13)
+	ZERO_GPR(r14)
+	ZERO_GPR(r15)
+	ZERO_GPR(r16)
+	ZERO_GPR(r17)
+	ZERO_GPR(r18)
+	ZERO_GPR(r19)
+	ZERO_GPR(r20)
+	ZERO_GPR(r21)
+	ZERO_GPR(r22)
+	ZERO_GPR(r23)
+	ZERO_GPR(r24)
+	ZERO_GPR(r25)
+	ZERO_GPR(r26)
+	ZERO_GPR(r27)
+	ZERO_GPR(r28)
+	ZERO_GPR(r29)
+	ZERO_GPR(r30)
+	ZERO_GPR(r31)
+	mtcrf	0xff, r0
+	mtcsrr0	r0
+	mtcsrr1	r0
+	mtctr	r0
+	mtspr	FSL_EIS_DBCNT, r0
+	mtspr	DEAR_BOOKE, r0
+	mtdec	r0
+	mtspr	BOOKE_DECAR, r0
+	mtspr	FSL_EIS_DSRR0, r0
+	mtspr	FSL_EIS_DSRR1, r0
+	mtspr	BOOKE_DVC1, r0
+	mtspr	BOOKE_DVC2, r0
+	mtspr	BOOKE_IVPR, r0
+	mtlr	r0
+	mtspr	FSL_EIS_MCAR, r0
+	mtmcsrr0	r0
+	mtmcsrr1	r0
+	mtspr	SPRG0, r0
+	mtspr	SPRG1, r0
+	mtspr	SPRG2, r0
+	mtspr	SPRG3, r0
+	mtspr	SPRG4, r0
+	mtspr	SPRG5, r0
+	mtspr	SPRG6, r0
+	mtspr	SPRG7, r0
+	mtspr	FSL_EIS_SPRG8, r0
+	mtspr	FSL_EIS_SPRG9, r0
+	mtsrr0	r0
+	mtsrr1	r0
+	mtspr	USPRG0, r0
+#ifdef HAS_SPE
+	evmra	r0, r0
+#endif
+#endif /* INIT_REGISTERS_FOR_LSM */
 	mtspr	TBWL, r0
-	mfspr	r2, HID0
-	ori	r2, r2, 0x4000
-	mtspr	HID0, r2
+	mtspr	TBWU, r0
 
-	/* FMPLL setup */
-	LWI	r3, mpc55xx_fmpll_config
-	bl	mpc55xx_fmpll_init
+	/* Enable time base */
+	mfspr	r3, HID0
+	ori	r3, r3, 0x4000
+	mtspr	HID0, r3
 
 	/* Enable branch prediction */
-	LWI	r2, BUCSR_BBFI | BUCSR_BPEN
-	mtspr	BUCSR, r2
+	LWI	r3, FSL_EIS_BUCSR_BBFI | FSL_EIS_BUCSR_BPEN
+	mtspr	FSL_EIS_BUCSR, r3
 
-	/* Set intermediate stack start to end of internal SRAM */
-	LA	r1, bsp_ram_end
-	subi	r1, r1, 16
+	/* MMU early initialization */
+	LA	r3, mpc55xx_start_config_mmu_early
+	LW	r4, mpc55xx_start_config_mmu_early_count
+	bl	mpc55xx_start_mmu_apply_config
 
-	/* Enable SPE */
-	mfmsr	r2
-	oris	r2, r2, 0x200
-	mtmsr	r2
-
-	/* Config internal flash */
-	bl	mpc55xx_flash_init
-
-#if HAS_CACHE
-	bl config_cache
-
-	/* Enable cache in the MMU for the internal SRAM */
-	LWI	r3, 0x10030000
-	mtspr	FSL_EIS_MAS0, r3
-	tlbre
-	LWI	r4, ~0x00000008
-	mfspr	r3, FSL_EIS_MAS2
-	and	r3, r3, r4
-	mtspr	FSL_EIS_MAS2, r3
-	tlbwe
-#endif
+	/* Initialize intermediate stack (ECC) */
 
-	/* Zero internal SRAM (needed to get proper ECC) */
 	LA	r3, bsp_ram_start
-	LA	r4, bsp_ram_size
-	bl	bsp_start_zero
+	addi	r4, r3, MPC55XX_EARLY_STACK_SIZE
 
-	/* Initialize intermediate start stack */
-	li	r0, 0
-	stw	r0, 0(r1)
-	stw	r0, 4(r1)
+zero_intermediate_stack_loop:
 
-	/* Do early initialization */
-	bl	mpc55xx_early_init
+#ifdef HAS_SPE
+	evstdd	r0, 0(r3)
+	evstdd	r0, 8(r3)
+	evstdd	r0, 16(r3)
+	evstdd	r0, 24(r3)
+#else
+	stw	r0, 0(r3)
+	stw	r0, 4(r3)
+	stw	r0, 8(r3)
+	stw	r0, 12(r3)
+	stw	r0, 16(r3)
+	stw	r0, 20(r3)
+	stw	r0, 24(r3)
+	stw	r0, 28(r3)
+#endif
+	addi	r3, r3, 32
+	cmpw	cr7, r3, r4
+	bne	cr7, zero_intermediate_stack_loop
+	subi	r1, r3, 16
 
-	/* Set up EABI and SYSV environment */
-	bl	__eabi
+	/* Next steps in C */
+	bl	mpc55xx_start_early
 
 	/* Initialize start stack */
-	LWI	r1, start_stack_end
+	LA	r1, start_stack_end
 	subi	r1, r1, 16
 	li	r0, 0
 	stw	r0, 0(r1)
 
+	/* Load sections */
+	LA	r3, bsp_section_fast_text_begin
+	LA	r4, bsp_section_fast_text_load_begin
+	LA	r5, bsp_section_fast_text_size
+	bl	load_section
+	LA	r3, bsp_section_fast_data_begin
+	LA	r4, bsp_section_fast_data_load_begin
+	LA	r5, bsp_section_fast_data_size
+	bl	load_section
+	LA	r3, bsp_section_data_begin
+	LA	r4, bsp_section_data_load_begin
+	LA	r5, bsp_section_data_size
+	bl	load_section
+
+	/* Set up EABI and SYSV environment */
+	bl	__eabi
+
 	/* Clear command line */
 	li	r3, 0
 
@@ -154,83 +246,39 @@ _start:
 
 	/* Spin around */
 twiddle:
-	b	twiddle
-
-#if HAS_CACHE
-config_cache:
-
-	/* Load zero, CINV, and CABT) */
-	li	r0, 0
-	li	r3, 0x2
-	li	r4, 0x4
-
-#if MPC55XX_CHIP_TYPE / 10 == 567
-start_instruction_cache_invalidation:
-
-	/* Clear instruction cache invalidation abort */
-	mtspr	FSL_EIS_L1CSR1, r0
-
-	/* Start instruction cache invalidation */
-	mtspr	FSL_EIS_L1CSR1, r3
-
-get_instruction_cache_invalidation_status:
-
-	/* Get instruction cache invalidation status */
-	mfspr	r5, FSL_EIS_L1CSR1
 
-	/* Check CABT */
-	and.	r6, r5, r4
-	bne	start_instruction_cache_invalidation
-
-	/* Check CINV */
-	and.	r6, r5, r3
-	bne	get_instruction_cache_invalidation_status
-
-	/* Save instruction cache settings */
-	LWI	r6, 0x00010001
-	isync
-	msync
-	mtspr	FSL_EIS_L1CSR1, r6
-#endif /* MPC55XX_CHIP_TYPE / 10 == 567 */
-
-start_data_cache_invalidation:
-
-	/* Clear data cache invalidation abort */
-	mtspr	FSL_EIS_L1CSR0, r0
+	b	twiddle
 
-	/* Start data cache invalidation */
-	mtspr	FSL_EIS_L1CSR0, r3
+mpc55xx_start_mmu_apply_config:
 
-get_data_cache_invalidation_status:
+	cmpwi	cr7, r4, r0
+	beqlr	cr7
+	mtctr	r4
 
-	/* Get data cache invalidation status */
-	mfspr	r5, FSL_EIS_L1CSR0
+mmu_init_loop:
 
-	/* Check CABT */
-	and.	r6, r5, r4
-	bne	start_data_cache_invalidation
+	lwz	r4, 0(r3)
+	lwz	r5, 4(r3)
+	lwz	r6, 8(r3)
+	lwz	r7, 12(r3)
+	mtspr	FSL_EIS_MAS0, r4
+	mtspr	FSL_EIS_MAS1, r5
+	mtspr	FSL_EIS_MAS2, r6
+	mtspr	FSL_EIS_MAS3, r7
+	tlbwe
+	addi	r3, r3, 16
+	bdnz	mmu_init_loop
+	blr
 
-	/* Check CINV */
-	and.	r6, r5, r3
-	bne	get_data_cache_invalidation_status
+load_section:
+	cmpw	cr7, r3, r4
+	beqlr	cr7
+	b	memcpy
 
-	/* Save data cache settings */
-#if MPC55XX_CHIP_TYPE / 10 != 567
-	/* FIXME: CORG??? 0x00180011 */
-	LWI	r6, 0x00100001
-#else
-	LWI	r6, 0x00190001
-#endif
-	isync
-	msync
-	mtspr	FSL_EIS_L1CSR0, r6
+	/* Start stack area */
 
-	/* Return */
-	blr
-#endif /* HAS_CACHE */
+	.section	".bsp_rwextra", "aw", @nobits
+	.align	4
+	.space	4096
 
-	/* Start stack area */
-	.section ".bsp_rwextra", "aw", @nobits
-	.align 4
-	.space 4096
 start_stack_end: