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.

34 files changed, 2609 insertions, 630 deletions

diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/Makefile.am b/c/src/lib/libbsp/powerpc/mpc55xxevb/Makefile.am
index 7c6e0df099..933dfe91da 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/Makefile.am
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/Makefile.am
@@ -30,6 +30,7 @@ dist_project_lib_DATA += startup/linkcmds.mpc5566evb
 dist_project_lib_DATA += startup/linkcmds.mpc5566evb_spe
 dist_project_lib_DATA += startup/linkcmds.mpc5674fevb
 dist_project_lib_DATA += startup/linkcmds.mpc5674fevb_spe
+dist_project_lib_DATA += startup/linkcmds.xkt564levb
 dist_project_lib_DATA += startup/linkcmds.phycore_mpc5554
 
 noinst_LIBRARIES += libbsp.a
@@ -41,16 +42,20 @@ include_HEADERS += ../../shared/include/tm27.h
 
 nodist_include_HEADERS = include/bspopts.h ../../shared/tod.h \
     ../../shared/include/coverhd.h
-include_bsp_HEADERS = include/mpc55xxevb.h \
-    include/smsc9218i.h \
-    include/mpc55xx-config.h \
-    ../../../libcpu/powerpc/mpc83xx/i2c/mpc83xx_i2cdrv.h \
-    ../../shared/include/irq-generic.h \
-    ../../shared/include/irq-info.h \
-    ../../shared/include/utility.h \
-    ../shared/include/linker-symbols.h \
-    ../shared/include/start.h \
-    ../shared/include/tictac.h
+include_bsp_HEADERS =
+include_bsp_HEADERS += ../../../libcpu/powerpc/mpc83xx/i2c/mpc83xx_i2cdrv.h
+include_bsp_HEADERS += ../../shared/include/irq-generic.h
+include_bsp_HEADERS += ../../shared/include/irq-info.h
+include_bsp_HEADERS += ../../shared/include/utility.h
+include_bsp_HEADERS += ../shared/include/linker-symbols.h
+include_bsp_HEADERS += ../shared/include/start.h
+include_bsp_HEADERS += ../shared/include/tictac.h
+include_bsp_HEADERS += include/mpc55xx-config.h
+include_bsp_HEADERS += include/mpc55xxevb.h
+include_bsp_HEADERS += include/smsc9218i.h
+include_bsp_HEADERS += include/console-esci.h
+include_bsp_HEADERS += include/console-generic.h
+include_bsp_HEADERS += include/console-linflex.h
 
 # startup
 libbsp_a_SOURCES += ../../shared/bootcard.c
@@ -58,25 +63,36 @@ libbsp_a_SOURCES += ../../shared/bspclean.c
 libbsp_a_SOURCES += ../../shared/bsplibc.c
 libbsp_a_SOURCES += ../../shared/bsppost.c
 libbsp_a_SOURCES += ../../shared/bsppredriverhook.c
-libbsp_a_SOURCES += ../../shared/bsppretaskinghook.c
 libbsp_a_SOURCES += ../../shared/bspgetworkarea.c
 libbsp_a_SOURCES += ../shared/src/bsp-start-zero.S
 libbsp_a_SOURCES += ../shared/src/memcpy.c
 libbsp_a_SOURCES += ../shared/src/tictac.c
 libbsp_a_SOURCES += ../shared/startup/bspidle.c
 libbsp_a_SOURCES += startup/bspstart.c
-libbsp_a_SOURCES += startup/fmpll-syncr-vals.c
 libbsp_a_SOURCES += startup/exc-vector-base.S
-libbsp_a_SOURCES += startup/ebi-cs-config.c
-libbsp_a_SOURCES += startup/ebi-cal-cs-config.c
-libbsp_a_SOURCES += startup/mmu-config.c
-libbsp_a_SOURCES += startup/siu-pcr-config.c
-libbsp_a_SOURCES += startup/early-init.c
+libbsp_a_SOURCES += startup/get-system-clock.c
 libbsp_a_SOURCES += startup/reset.c
+libbsp_a_SOURCES += startup/start-config-clock.c
+libbsp_a_SOURCES += startup/start-config-ebi-cs.c
+libbsp_a_SOURCES += startup/start-config-ebi-cs-cal.c
+libbsp_a_SOURCES += startup/start-config-mmu.c
+libbsp_a_SOURCES += startup/start-config-mmu-early.c
+libbsp_a_SOURCES += startup/start-config-siu-pcr.c
+libbsp_a_SOURCES += startup/start-early.c
+libbsp_a_SOURCES += startup/start-cache.S
+libbsp_a_SOURCES += startup/start-clock.c
+libbsp_a_SOURCES += startup/start-flash.S
+libbsp_a_SOURCES += startup/start-watchdog.c
 
 # clock
 libbsp_a_SOURCES += clock/clock-config.c
 
+# console
+libbsp_a_SOURCES += console/console-config.c
+libbsp_a_SOURCES += console/console-esci.c
+libbsp_a_SOURCES += console/console-generic.c
+libbsp_a_SOURCES += console/console-linflex.c
+
 # irq_generic
 libbsp_a_SOURCES += ../../shared/src/irq-generic.c \
     ../../shared/src/irq-legacy.c \
@@ -107,7 +123,6 @@ libbsp_a_LIBADD = ../../../libcpu/@RTEMS_CPU@/shared/cpuIdent.rel \
     ../../../libcpu/@RTEMS_CPU@/@RTEMS_CPU_MODEL@/edma.rel \
     ../../../libcpu/@RTEMS_CPU@/@RTEMS_CPU_MODEL@/emios.rel \
     ../../../libcpu/@RTEMS_CPU@/@RTEMS_CPU_MODEL@/dspi.rel \
-    ../../../libcpu/@RTEMS_CPU@/@RTEMS_CPU_MODEL@/esci.rel \
     ../../../libcpu/@RTEMS_CPU@/@exceptions@/rtems-cpu.rel \
     ../../../libcpu/@RTEMS_CPU@/@exceptions@/exc_bspsupport.rel \
     ../../../libcpu/@RTEMS_CPU@/@RTEMS_CPU_MODEL@/timer.rel
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/README b/c/src/lib/libbsp/powerpc/mpc55xxevb/README
index 476433d1ff..df4a8e8a52 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/README
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/README
@@ -1,64 +1,15 @@
-OVERVIEW
-========
+Supported MCUs:
 
-BSP NAME:           mpc55xxevb
-BOARD:              Freescale MPC5566 evaluation board MPC5566EVB
-BUS:                N/A
-CPU FAMILY:         ppc
-CPU:                PowerPC e200z6
-COPROCESSORS:       N/A
-MODE:               32 bit mode
+ o MPC5516
+ o MPC5554
+ o MPC5566
+ o MPC5643L
+ o MPC5674F
 
-PERIPHERALS
-===========
+Supported boards:
 
-TIMERS:             not yet supported
-  RESOLUTION:       not yet supported
-SERIAL PORTS:       2 internal eSCI
-REAL-TIME CLOCK:    N/A
-DMA:                eDMA
-VIDEO:              N/A
-SCSI:               N/A
-NETWORKING:         FEC (not yet supported)
-                    SMSC9218I (external)
-SPI:                DSPI
-
-DRIVER INFORMATION
-==================
-
-CLOCK DRIVER:       EMIOS channel 23
-IOSUPP DRIVER:      N/A
-SHMSUPP:            N/A
-TIMER DRIVER:       not yet supported
-TTY DRIVER:         BSP
-
-STDIO
-=====
-
-PORT:               ESCI A
-ELECTRICAL:         N/A
-BAUD:               115200
-BITS PER CHARACTER: 8
-PARITY:             N
-STOP BITS:          1
-
-NOTES
-=====
-
-BUS WIDTH:          32 bit Flash, 32 bit SDRAM
-FLASH:              3 MByte
-INTERNAL RAM:       128 kByte SDRAM 
-EXTERNAL RAM:       512 kByte SDRAM 
-
-
-DEBUGGING / CODE LOADING
-========================
-
-Tested using the Lauterbach TRACE32 ICD debugger.
-
-ISSUES
-======
-
-The memory blocks allocated by LibBlock are in general not cache aligned so we
-cannot use DMA transfers.  This is suboptimal in combination with a SD Card and
-SPI.
+ o embedded brains GmbH GWLCFM
+ o phyCORE MPC5554
+ o Freescale MPC5566EVB
+ o Freescale XKT564L KIT
+ o Axiom MPC567XADAT516 / MPC567XEVBFXMB
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/clock/clock-config.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/clock/clock-config.c
index 0d1561880f..d8a6594ad2 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/clock/clock-config.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/clock/clock-config.c
@@ -7,81 +7,76 @@
  */
 
 /*
- * Copyright (c) 2009
- * embedded brains GmbH
- * Obere Lagerstr. 30
- * D-82178 Puchheim
- * Germany
- * <rtems@embedded-brains.de>
+ * Copyright (c) 2009-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 <mpc55xx/regs.h>
-#include <mpc55xx/emios.h>
-
-#include <rtems.h>
-
 #include <bsp.h>
 #include <bsp/irq.h>
 
-#define RTEMS_STATUS_CHECKS_USE_PRINTK
+#include <mpc55xx/regs.h>
 
-#include <rtems/status-checks.h>
+static uint64_t mpc55xx_clock_factor;
 
-/* This is defined in clockdrv_shell.h */
-rtems_isr Clock_isr( rtems_vector_number vector);
+#if defined(MPC55XX_CLOCK_EMIOS_CHANNEL)
 
-#define Clock_driver_support_at_tick() \
- do { \
-    union EMIOS_CSR_tag csr = MPC55XX_ZERO_FLAGS; \
-    csr.B.FLAG = 1; \
-    EMIOS.CH [MPC55XX_CLOCK_EMIOS_CHANNEL].CSR.R = csr.R; \
- } while (0)
+#include <mpc55xx/emios.h>
 
-static uint64_t mpc55xx_clock_factor;
+static void mpc55xx_clock_at_tick(void)
+{
+  union EMIOS_CSR_tag csr = MPC55XX_ZERO_FLAGS;
+  csr.B.FLAG = 1;
+  EMIOS.CH [MPC55XX_CLOCK_EMIOS_CHANNEL].CSR.R = csr.R;
+}
 
-static void mpc55xx_clock_handler_install( rtems_isr_entry isr,
-					   rtems_isr_entry *old_isr)
+static void mpc55xx_clock_handler_install(rtems_isr_entry isr)
 {
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
   sc = mpc55xx_interrupt_handler_install(
-    MPC55XX_IRQ_EMIOS( MPC55XX_CLOCK_EMIOS_CHANNEL),
+    MPC55XX_IRQ_EMIOS(MPC55XX_CLOCK_EMIOS_CHANNEL),
     "clock",
     RTEMS_INTERRUPT_UNIQUE,
     MPC55XX_INTC_MIN_PRIORITY,
     (rtems_interrupt_handler) isr,
     NULL
   );
- *old_isr = NULL;
-  RTEMS_CHECK_SC_VOID( sc, "install clock interrupt handler");
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
 }
 
-static void mpc55xx_clock_initialize( void)
+static void mpc55xx_clock_initialize(void)
 {
   volatile struct EMIOS_CH_tag *regs = &EMIOS.CH [MPC55XX_CLOCK_EMIOS_CHANNEL];
   union EMIOS_CCR_tag ccr = MPC55XX_ZERO_FLAGS;
   union EMIOS_CSR_tag csr = MPC55XX_ZERO_FLAGS;
   unsigned prescaler = mpc55xx_emios_global_prescaler();
-  uint64_t interval = ((uint64_t) bsp_clock_speed
-    * (uint64_t) rtems_configuration_get_microseconds_per_tick()) / 1000000;
+  uint64_t reference_clock = bsp_clock_speed;
+  uint64_t us_per_tick = rtems_configuration_get_microseconds_per_tick();
+  uint64_t interval = (reference_clock * us_per_tick) / 1000000;
 
-  mpc55xx_clock_factor = (1000000000ULL << 32) / bsp_clock_speed;
+  mpc55xx_clock_factor = (1000000000ULL << 32) / reference_clock;
 
   /* Apply prescaler */
   if (prescaler > 0) {
     interval /= (uint64_t) prescaler;
   } else {
-    RTEMS_SYSLOG_ERROR( "unexpected global eMIOS prescaler\n");
+    rtems_fatal_error_occurred(0xdeadbeef);
   }
 
   /* Check interval */
   if (interval == 0 || interval > MPC55XX_EMIOS_VALUE_MAX) {
-    interval = MPC55XX_EMIOS_VALUE_MAX;
-    RTEMS_SYSLOG_ERROR( "clock timer interval out of range\n");
+    rtems_fatal_error_occurred(0xdeadbeef);
   }
 
   /* Configure eMIOS channel */
@@ -103,37 +98,28 @@ static void mpc55xx_clock_initialize( void)
   regs->CADR.R = (uint32_t) interval - 1;
 
   /* Set control register */
-#if MPC55XX_CHIP_TYPE / 10 == 551
-  ccr.B.MODE = MPC55XX_EMIOS_MODE_MCB_UP_INT_CLK;
-#else
-  ccr.B.MODE = MPC55XX_EMIOS_MODE_MC_UP_INT_CLK;
-#endif
+  #if MPC55XX_CHIP_TYPE / 10 == 551
+    ccr.B.MODE = MPC55XX_EMIOS_MODE_MCB_UP_INT_CLK;
+  #else
+    ccr.B.MODE = MPC55XX_EMIOS_MODE_MC_UP_INT_CLK;
+  #endif
   ccr.B.UCPREN = 1;
   ccr.B.FEN = 1;
   ccr.B.FREN = 1;
   regs->CCR.R = ccr.R;
 }
 
-static void mpc55xx_clock_cleanup( void)
+static void mpc55xx_clock_cleanup(void)
 {
-  rtems_status_code sc = RTEMS_SUCCESSFUL;
   volatile struct EMIOS_CH_tag *regs = &EMIOS.CH [MPC55XX_CLOCK_EMIOS_CHANNEL];
   union EMIOS_CCR_tag ccr = MPC55XX_ZERO_FLAGS;
 
   /* Set channel in GPIO mode */
   ccr.B.MODE = MPC55XX_EMIOS_MODE_GPIO_INPUT;
   regs->CCR.R = ccr.R;
-
-  /* Remove interrupt handler */
-  sc = rtems_interrupt_handler_remove(
-    MPC55XX_IRQ_EMIOS( MPC55XX_CLOCK_EMIOS_CHANNEL),
-    (rtems_interrupt_handler) Clock_isr,
-    NULL
-  );
-  RTEMS_CHECK_SC_VOID( sc, "remove clock interrupt handler");
 }
 
-static uint32_t mpc55xx_clock_nanoseconds_since_last_tick( void)
+static uint32_t mpc55xx_clock_nanoseconds_since_last_tick(void)
 {
   volatile struct EMIOS_CH_tag *regs = &EMIOS.CH [MPC55XX_CLOCK_EMIOS_CHANNEL];
   uint64_t c = regs->CCNTR.R;
@@ -147,13 +133,87 @@ static uint32_t mpc55xx_clock_nanoseconds_since_last_tick( void)
   return (uint32_t) ((c * k) >> 32);
 }
 
-#define Clock_driver_support_initialize_hardware() mpc55xx_clock_initialize()
+#elif defined(MPC55XX_CLOCK_PIT_CHANNEL)
+
+static void mpc55xx_clock_at_tick(void)
+{
+  volatile PIT_RTI_CHANNEL_tag *channel =
+    &PIT_RTI.CHANNEL [MPC55XX_CLOCK_PIT_CHANNEL];
+  PIT_RTI_TFLG_32B_tag tflg = { .B = { .TIF = 1 } };
+
+  channel->TFLG.R = tflg.R;
+}
+
+static void mpc55xx_clock_handler_install(rtems_isr_entry isr)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+
+  sc = mpc55xx_interrupt_handler_install(
+    MPC55XX_IRQ_PIT_CHANNEL(MPC55XX_CLOCK_PIT_CHANNEL),
+    "clock",
+    RTEMS_INTERRUPT_UNIQUE,
+    MPC55XX_INTC_MIN_PRIORITY,
+    (rtems_interrupt_handler) isr,
+    NULL
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+}
+
+static void mpc55xx_clock_initialize(void)
+{
+  volatile PIT_RTI_CHANNEL_tag *channel =
+    &PIT_RTI.CHANNEL [MPC55XX_CLOCK_PIT_CHANNEL];
+  uint64_t reference_clock = bsp_clock_speed;
+  uint64_t us_per_tick = rtems_configuration_get_microseconds_per_tick();
+  uint64_t interval = (reference_clock * us_per_tick) / 1000000;
+  PIT_RTI_PITMCR_32B_tag pitmcr = { .B = { .FRZ = 1 } };
+  PIT_RTI_TCTRL_32B_tag tctrl = { .B = { .TIE = 1, .TEN = 1 } };
+
+  mpc55xx_clock_factor = (1000000000ULL << 32) / reference_clock;
+
+  PIT_RTI.PITMCR.R = pitmcr.R;
+  channel->LDVAL.R = interval;
+  channel->TCTRL.R = tctrl.R;
+}
+
+static void mpc55xx_clock_cleanup(void)
+{
+  volatile PIT_RTI_CHANNEL_tag *channel =
+    &PIT_RTI.CHANNEL [MPC55XX_CLOCK_PIT_CHANNEL];
 
-#define Clock_driver_support_install_isr( isr, old_isr) \
-  mpc55xx_clock_handler_install(isr,&old_isr)
+  channel->TCTRL.R = 0;
+}
 
-#define Clock_driver_support_shutdown_hardware() mpc55xx_clock_cleanup()
+static uint32_t mpc55xx_clock_nanoseconds_since_last_tick(void)
+{
+  volatile PIT_RTI_CHANNEL_tag *channel =
+    &PIT_RTI.CHANNEL [MPC55XX_CLOCK_PIT_CHANNEL];
+  uint32_t c = channel->CVAL.R;
+  uint32_t i = channel->LDVAL.R;
+  uint64_t k = mpc55xx_clock_factor;
 
+  if (channel->TFLG.B.TIF != 0) {
+    c = channel->CVAL.R - i;
+  }
+
+  return (uint32_t) (((i - c) * k) >> 32);
+}
+
+#endif
+
+#define Clock_driver_support_at_tick() \
+  mpc55xx_clock_at_tick()
+#define Clock_driver_support_initialize_hardware() \
+  mpc55xx_clock_initialize()
+#define Clock_driver_support_install_isr(isr, old_isr) \
+  do { \
+    mpc55xx_clock_handler_install(isr); \
+    old_isr = NULL; \
+  } while (0)
+#define Clock_driver_support_shutdown_hardware() \
+  mpc55xx_clock_cleanup()
 #define Clock_driver_nanoseconds_since_last_tick \
   mpc55xx_clock_nanoseconds_since_last_tick
 
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/configure.ac b/c/src/lib/libbsp/powerpc/mpc55xxevb/configure.ac
index 9ec3dbe0d4..6bc0eb4bc3 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/configure.ac
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/configure.ac
@@ -24,6 +24,7 @@ RTEMS_CHECK_NETWORKING
 AM_CONDITIONAL(HAS_NETWORKING,test "$HAS_NETWORKING" = "yes")
 
 RTEMS_BSPOPTS_SET_DATA_CACHE_ENABLED([gwlcfm],[])
+RTEMS_BSPOPTS_SET_DATA_CACHE_ENABLED([xkt564levb],[])
 RTEMS_BSPOPTS_SET_DATA_CACHE_ENABLED([mpc5566evb*],[1])
 RTEMS_BSPOPTS_SET_DATA_CACHE_ENABLED([mpc5674fevb*],[1])
 RTEMS_BSPOPTS_SET_DATA_CACHE_ENABLED([*],[1])
@@ -36,6 +37,7 @@ RTEMS_BSPOPTS_SET_INSTRUCTION_CACHE_ENABLED([*],[1])
 RTEMS_BSPOPTS_HELP_INSTRUCTION_CACHE_ENABLED
 
 RTEMS_BSPOPTS_SET([BSP_INTERRUPT_HANDLER_TABLE_SIZE],[mpc5674fevb*],[255])
+RTEMS_BSPOPTS_SET([BSP_INTERRUPT_HANDLER_TABLE_SIZE],[xkt564levb*],[127])
 RTEMS_BSPOPTS_SET([BSP_INTERRUPT_HANDLER_TABLE_SIZE],[mpc5566evb*],[127])
 RTEMS_BSPOPTS_SET([BSP_INTERRUPT_HANDLER_TABLE_SIZE],[*],[63])
 RTEMS_BSPOPTS_HELP([BSP_INTERRUPT_HANDLER_TABLE_SIZE],
@@ -50,6 +52,7 @@ RTEMS_BSPOPTS_HELP([MPC55XX_ESCI_USE_INTERRUPTS],
 [define to zero or one to disable or enable interrupts for the eSCI devices])
 
 RTEMS_BSPOPTS_SET([MPC55XX_FMPLL_REF_CLOCK],[gwlcfm],[40000000])
+RTEMS_BSPOPTS_SET([MPC55XX_FMPLL_REF_CLOCK],[xkt564levb*],[40000000])
 RTEMS_BSPOPTS_SET([MPC55XX_FMPLL_REF_CLOCK],[mpc5674fevb*],[40000000])
 RTEMS_BSPOPTS_SET([MPC55XX_FMPLL_REF_CLOCK],[*]     ,[8000000])
 RTEMS_BSPOPTS_HELP([MPC55XX_FMPLL_REF_CLOCK],
@@ -74,6 +77,7 @@ RTEMS_BSPOPTS_SET([MPC55XX_FMPLL_MFD],[*]     ,[12])
 RTEMS_BSPOPTS_HELP([MPC55XX_FMPLL_MFD],
 [Must be defined to be the PLL multiplication factor for clock generation])
 
+RTEMS_BSPOPTS_SET([MPC55XX_EMIOS_PRESCALER],[xkt564levb*],[])
 RTEMS_BSPOPTS_SET([MPC55XX_EMIOS_PRESCALER],[gwlcfm],[66])
 RTEMS_BSPOPTS_SET([MPC55XX_EMIOS_PRESCALER],[*]     ,[1])
 RTEMS_BSPOPTS_HELP([MPC55XX_EMIOS_PRESCALER],
@@ -96,17 +100,28 @@ RTEMS_BSPOPTS_HELP([SMSC9218I_EDMA_TX_CHANNEL],
 [transmit eDMA channel for SMSC9218I network interface])
 
 RTEMS_BSPOPTS_SET([MPC55XX_CLOCK_EMIOS_CHANNEL],[mpc5674fevb*],[31])
+RTEMS_BSPOPTS_SET([MPC55XX_CLOCK_EMIOS_CHANNEL],[xkt564levb*],[])
 RTEMS_BSPOPTS_SET([MPC55XX_CLOCK_EMIOS_CHANNEL],[*],[23])
 RTEMS_BSPOPTS_HELP([MPC55XX_CLOCK_EMIOS_CHANNEL],
 [selects the eMIOS channel for the RTEMS system tick (the default is the last channel)])
 
+RTEMS_BSPOPTS_SET([MPC55XX_CLOCK_PIT_CHANNEL],[xkt564levb*],[3])
+RTEMS_BSPOPTS_SET([MPC55XX_CLOCK_PIT_CHANNEL],[*],[])
+RTEMS_BSPOPTS_HELP([MPC55XX_CLOCK_PIT_CHANNEL],
+[selects the PIT channel for the RTEMS system tick (the default is the last channel)])
+
 RTEMS_BSPOPTS_SET([MPC55XX_CHIP_TYPE],[mpc5674fevb*],[5674])
+RTEMS_BSPOPTS_SET([MPC55XX_CHIP_TYPE],[xkt564levb*],[5643])
 RTEMS_BSPOPTS_SET([MPC55XX_CHIP_TYPE],[mpc5566evb*],[5566])
 RTEMS_BSPOPTS_SET([MPC55XX_CHIP_TYPE],[gwlcfm]    ,[5516])
 RTEMS_BSPOPTS_SET([MPC55XX_CHIP_TYPE],[*]         ,[5554])
 RTEMS_BSPOPTS_HELP([MPC55XX_CHIP_TYPE],
 [specifies the chip type in use (e.g. 5554 for MPC5554)])
 
+RTEMS_BSPOPTS_SET([MPC55XX_EARLY_STACK_SIZE],[*],[1024])
+RTEMS_BSPOPTS_HELP([MPC55XX_EARLY_STACK_SIZE],
+[size of the early initialization stack in bytes])
+
 RTEMS_BSPOPTS_SET([MPC55XX_BOOTFLAGS],[*],[])
 RTEMS_BSPOPTS_HELP([MPC55XX_BOOTFLAGS],
 [if defined, builds in bootflags above the RCHW for setup in a debugger to avoid startup MMU setup])
@@ -114,6 +129,9 @@ RTEMS_BSPOPTS_HELP([MPC55XX_BOOTFLAGS],
 RTEMS_BSPOPTS_SET([MPC55XX_BOARD_MPC5674FEVB],[mpc5674fevb*],[1])
 RTEMS_BSPOPTS_HELP([MPC55XX_BOARD_MPC5674FEVB],[if defined, use custom settings for MPC5674FEVB board])
 
+RTEMS_BSPOPTS_SET([MPC55XX_BOARD_XKT564LEVB],[xkt564levb*],[1])
+RTEMS_BSPOPTS_HELP([MPC55XX_BOARD_XKT564LEVB],[if defined, use custom settings for XKT564LEVB board])
+
 RTEMS_BSPOPTS_SET([MPC55XX_BOARD_MPC5566EVB],[mpc5566evb*],[1])
 RTEMS_BSPOPTS_HELP([MPC55XX_BOARD_MPC5566EVB],[if defined, use custom settings for MPC5566EVB board])
 
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-config.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-config.c
new file mode 100644
index 0000000000..816c8552b6
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-config.c
@@ -0,0 +1,32 @@
+/*
+ * 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/console-generic.h>
+#include <bsp/console-esci.h>
+#include <bsp/console-linflex.h>
+
+CONSOLE_GENERIC_INFO_TABLE = {
+  #ifdef MPC55XX_HAS_ESCI
+    CONSOLE_GENERIC_INFO(mpc55xx_esci_devices + 0, &mpc55xx_esci_callbacks, "/dev/ttyS0"),
+    CONSOLE_GENERIC_INFO(mpc55xx_esci_devices + 1, &mpc55xx_esci_callbacks, "/dev/ttyS1")
+  #endif
+  #ifdef MPC55XX_HAS_LINFLEX
+    CONSOLE_GENERIC_INFO(mpc55xx_linflex_devices + 0, &mpc55xx_linflex_callbacks, "/dev/ttyS0"),
+    CONSOLE_GENERIC_INFO(mpc55xx_linflex_devices + 1, &mpc55xx_linflex_callbacks, "/dev/ttyS1")
+  #endif
+};
+
+CONSOLE_GENERIC_INFO_COUNT;
+
+CONSOLE_GENERIC_MINOR(0);
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-esci.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-esci.c
new file mode 100644
index 0000000000..737936b75f
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-esci.c
@@ -0,0 +1,339 @@
+/**
+ * @file
+ *
+ * @brief Console ESCI implementation.
+ */
+
+/*
+ * 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/console-esci.h>
+
+#include <bsp.h>
+#include <bsp/irq.h>
+
+#ifdef MPC55XX_HAS_ESCI
+
+mpc55xx_esci_context mpc55xx_esci_devices [] = {
+  {
+    .regs = &ESCI_A,
+    .irq = MPC55XX_IRQ_ESCI(0)
+  }, {
+    .regs = &ESCI_B,
+    .irq = MPC55XX_IRQ_ESCI(1)
+  }
+};
+
+static void mpc55xx_esci_poll_write(int minor, char c)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  const union ESCI_SR_tag clear_tdre = { .B = { .TDRE = 1 } };
+  volatile struct ESCI_tag *regs = self->regs;
+  rtems_interrupt_level level;
+  bool done = false;
+  bool wait_for_transmit_done = false;
+
+  rtems_interrupt_disable(level);
+  if (self->transmit_nest_level == 0) {
+    union ESCI_CR1_tag cr1 = { .R = regs->CR1.R };
+
+    if (cr1.B.TIE != 0) {
+      cr1.B.TIE = 0;
+      regs->CR1.R = cr1.R;
+      wait_for_transmit_done = !self->transmit_in_progress;
+      self->transmit_nest_level = 1;
+    }
+  } else {
+    ++self->transmit_nest_level;
+  }
+  rtems_interrupt_enable(level);
+
+  while (!done) {
+    rtems_interrupt_disable(level);
+    bool tx = self->transmit_in_progress;
+    if (!tx || (tx && regs->SR.B.TDRE)) {
+      regs->SR.R = clear_tdre.R;
+      regs->DR.B.D = c;
+      self->transmit_in_progress = true;
+      done = true;
+    }
+    rtems_interrupt_enable(level);
+  }
+
+  done = false;
+  while (!done) {
+    rtems_interrupt_disable(level);
+    if (wait_for_transmit_done) {
+      if (regs->SR.B.TDRE) {
+        regs->SR.R = clear_tdre.R;
+        self->transmit_in_progress = false;
+        done = true;
+      }
+    } else {
+      done = true;
+    }
+
+    if (done && self->transmit_nest_level > 0) {
+      --self->transmit_nest_level;
+
+      if (self->transmit_nest_level == 0) {
+        union ESCI_CR1_tag cr1 = { .R = regs->CR1.R };
+
+        cr1.B.TIE = 1;
+        regs->CR1.R = cr1.R;
+      }
+    }
+    rtems_interrupt_enable(level);
+  }
+}
+
+static inline void mpc55xx_esci_interrupts_clear_and_enable(
+  mpc55xx_esci_context *self
+)
+{
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_CR1_tag cr1 = MPC55XX_ZERO_FLAGS;
+  rtems_interrupt_level level;
+
+  rtems_interrupt_disable(level);
+  cr1.R = regs->CR1.R;
+  cr1.B.RIE = 1;
+  cr1.B.TIE = 1;
+  regs->CR1.R = cr1.R;
+  regs->SR.R = regs->SR.R;
+  rtems_interrupt_enable(level);
+}
+
+static inline void mpc55xx_esci_interrupts_disable(mpc55xx_esci_context *self)
+{
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_CR1_tag cr1 = MPC55XX_ZERO_FLAGS;
+  rtems_interrupt_level level;
+
+  rtems_interrupt_disable(level);
+  cr1.R = regs->CR1.R;
+  cr1.B.RIE = 0;
+  cr1.B.TIE = 0;
+  regs->CR1.R = cr1.R;
+  rtems_interrupt_enable(level);
+}
+
+static void mpc55xx_esci_interrupt_handler(void *arg)
+{
+  mpc55xx_esci_context *self = arg;
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_SR_tag sr = MPC55XX_ZERO_FLAGS;
+  union ESCI_SR_tag active = MPC55XX_ZERO_FLAGS;
+  rtems_interrupt_level level;
+
+  /* Status */
+  sr.R = regs->SR.R;
+
+  /* Receive data register full? */
+  if (sr.B.RDRF != 0) {
+    active.B.RDRF = 1;
+  }
+
+  /* Transmit data register empty? */
+  if (sr.B.TDRE != 0) {
+    active.B.TDRE = 1;
+  }
+
+  /* Clear flags */
+  rtems_interrupt_disable(level);
+  regs->SR.R = active.R;
+  self->transmit_in_progress = false;
+  rtems_interrupt_enable(level);
+
+  /* Enqueue */
+  if (active.B.RDRF != 0) {
+    char c = regs->DR.B.D;
+    rtems_termios_enqueue_raw_characters(self->tty, &c, 1);
+  }
+
+  /* Dequeue */
+  if (active.B.TDRE != 0) {
+    rtems_termios_dequeue_characters(self->tty, 1);
+  }
+}
+
+static int mpc55xx_esci_set_attributes(int minor, const struct termios *t)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_CR1_tag cr1 = { .R = regs->CR1.R };
+  union ESCI_CR2_tag cr2 = MPC55XX_ZERO_FLAGS;
+  rtems_termios_baud_t br = rtems_termios_baud_to_number(t->c_cflag);
+
+  /* Enable module */
+  cr2.B.MDIS = 0;
+
+  /* Interrupts */
+  cr1.B.TCIE = 0;
+  cr1.B.ILIE = 0;
+  cr2.B.IEBERR = 0;
+  cr2.B.ORIE = 0;
+  cr2.B.NFIE = 0;
+  cr2.B.FEIE = 0;
+  cr2.B.PFIE = 0;
+
+  /* Disable receiver wake-up standby */
+  cr1.B.RWU = 0;
+
+  /* Disable DMA channels */
+  cr2.B.RXDMA = 0;
+  cr2.B.TXDMA = 0;
+
+  /* Idle line type */
+  cr1.B.ILT = 0;
+
+  /* Disable loops */
+  cr1.B.LOOPS = 0;
+
+  /* Enable or disable receiver */
+  cr1.B.RE = (t->c_cflag & CREAD) ? 1 : 0;
+
+  /* Enable transmitter */
+  cr1.B.TE = 1;
+
+  /* Baud rate */
+  if (br > 0) {
+    br = bsp_clock_speed / (16 * br);
+    br = (br > 8191) ? 8191 : br;
+  } else {
+    br = 0;
+  }
+  cr1.B.SBR = br;
+
+  /* Number of data bits */
+  if ((t->c_cflag & CSIZE) != CS8) {
+    return -1;
+  }
+  cr1.B.M = 0;
+
+  /* Parity */
+  cr1.B.PE = (t->c_cflag & PARENB) ? 1 : 0;
+  cr1.B.PT = (t->c_cflag & PARODD) ? 1 : 0;
+
+  /* Stop bits */
+  if (t->c_cflag & CSTOPB ) {
+    /* Two stop bits */
+    return -1;
+  }
+
+  /* Disable LIN */
+  regs->LCR.R = 0;
+
+  /* Set control registers */
+  regs->CR2.R = cr2.R;
+  regs->CR1.R = cr1.R;
+
+  return 0;
+}
+
+static int mpc55xx_esci_first_open(int major, int minor, void *arg)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  int rv = 0;
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  struct rtems_termios_tty *tty = console_generic_get_tty_at_open(arg);
+
+  self->tty = tty;
+
+  rv = rtems_termios_set_initial_baud(tty, 115200);
+  if (rv != 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  rv = mpc55xx_esci_set_attributes(minor, &tty->termios);
+  if (rv != 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  sc = mpc55xx_interrupt_handler_install(
+    self->irq,
+    "eSCI",
+    RTEMS_INTERRUPT_UNIQUE,
+    MPC55XX_INTC_DEFAULT_PRIORITY,
+    mpc55xx_esci_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  mpc55xx_esci_interrupts_clear_and_enable(self);
+  self->transmit_in_progress = false;
+
+  return 0;
+}
+
+static int mpc55xx_esci_last_close(int major, int minor, void* arg)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+
+  mpc55xx_esci_interrupts_disable(self);
+  self->tty = NULL;
+
+  return 0;
+}
+
+static int mpc55xx_esci_poll_read(int minor)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  volatile struct ESCI_tag *regs = self->regs;
+  union ESCI_SR_tag sr = MPC55XX_ZERO_FLAGS;
+  rtems_interrupt_level level;
+  int c = -1;
+
+  rtems_interrupt_disable(level);
+  if (regs->SR.B.RDRF != 0) {
+    /* Clear flag */
+    sr.B.RDRF = 1;
+    regs->SR.R = sr.R;
+
+    /* Read */
+    c = regs->DR.B.D;
+  }
+  rtems_interrupt_enable(level);
+
+  return c;
+}
+
+static int mpc55xx_esci_write(int minor, const char *out, size_t n)
+{
+  mpc55xx_esci_context *self = console_generic_get_context(minor);
+  rtems_interrupt_level level;
+
+  rtems_interrupt_disable(level);
+  self->regs->DR.B.D = out [0];
+  self->transmit_in_progress = true;
+  rtems_interrupt_enable(level);
+
+  return 0;
+}
+
+const console_generic_callbacks mpc55xx_esci_callbacks = {
+  .termios_callbacks = {
+    .firstOpen = mpc55xx_esci_first_open,
+    .lastClose = mpc55xx_esci_last_close,
+    .write = mpc55xx_esci_write,
+    .setAttributes = mpc55xx_esci_set_attributes,
+    .outputUsesInterrupts = TERMIOS_IRQ_DRIVEN
+  },
+  .poll_read = mpc55xx_esci_poll_read,
+  .poll_write = mpc55xx_esci_poll_write
+};
+
+#endif /* MPC55XX_HAS_ESCI */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-generic.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-generic.c
new file mode 100644
index 0000000000..e6c2c587a2
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-generic.c
@@ -0,0 +1,167 @@
+/**
+ * @file
+ *
+ * @brief Generic console driver implementation.
+ */
+
+/*
+ * 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/console-generic.h>
+
+#include <rtems/console.h>
+
+static const struct termios console_generic_termios = {
+  .c_cflag = CS8 | CREAD | CLOCAL | B115200
+};
+
+static void console_generic_char_out(char c)
+{
+  int minor = (int) console_generic_minor;
+  const console_generic_callbacks *cb =
+    console_generic_info_table [minor].callbacks;
+
+  if (c == '\n') {
+    (*cb->poll_write)(minor, '\r');
+  }
+
+  (*cb->poll_write)(minor, c);
+}
+
+static int console_generic_char_in(void)
+{
+  int minor = (int) console_generic_minor;
+  const console_generic_callbacks *cb =
+    console_generic_info_table [minor].callbacks;
+
+  return (*cb->poll_read)(minor);
+}
+
+static void console_generic_char_out_do_init(void)
+{
+  int minor = (int) console_generic_minor;
+  const console_generic_callbacks *cb =
+    console_generic_info_table [minor].callbacks;
+  const struct termios *term = &console_generic_termios;
+
+  BSP_output_char = console_generic_char_out;
+  (*cb->termios_callbacks.setAttributes)(minor, term);
+}
+
+static void console_generic_char_out_init(char c)
+{
+  console_generic_char_out_do_init();
+  console_generic_char_out(c);
+}
+
+rtems_device_driver console_initialize(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  const console_generic_info *info_table = console_generic_info_table;
+  rtems_device_minor_number count = console_generic_info_count;
+  rtems_device_minor_number console = console_generic_minor;
+
+  if (count <= 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  rtems_termios_initialize();
+
+  for (minor = 0; minor < count; ++minor) {
+    const console_generic_info *info = info_table + minor;
+
+    sc = rtems_io_register_name(info->device_path, major, minor);
+    if (sc != RTEMS_SUCCESSFUL) {
+      rtems_fatal_error_occurred(0xdeadbeef);
+    }
+  }
+
+  sc = rtems_io_register_name(CONSOLE_DEVICE_NAME, major, console);
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  console_generic_char_out_do_init();
+
+  return sc;
+}
+
+rtems_device_driver console_open(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  rtems_device_minor_number count = console_generic_info_count;
+
+  if (minor < count) {
+    const console_generic_info *info = &console_generic_info_table [minor];
+
+    sc = rtems_termios_open(
+      major,
+      minor,
+      arg,
+      &info->callbacks->termios_callbacks
+    );
+  } else {
+    sc = RTEMS_INVALID_ID;
+  }
+
+  return sc;
+}
+
+rtems_device_driver console_close(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  return rtems_termios_close(arg);
+}
+
+rtems_device_driver console_read(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  return rtems_termios_read(arg);
+}
+
+rtems_device_driver console_write(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  return rtems_termios_write(arg);
+}
+
+rtems_device_driver console_control(
+  rtems_device_major_number major,
+  rtems_device_minor_number minor,
+  void *arg
+)
+{
+  return rtems_termios_ioctl(arg);
+}
+
+BSP_output_char_function_type BSP_output_char = console_generic_char_out_init;
+
+BSP_polling_getchar_function_type BSP_poll_char = console_generic_char_in;
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-linflex.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-linflex.c
new file mode 100644
index 0000000000..54ac08ed56
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-linflex.c
@@ -0,0 +1,419 @@
+/**
+ * @file
+ *
+ * @brief Console LINFlexD implementation.
+ */
+
+/*
+ * 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/console-linflex.h>
+
+#include <bsp.h>
+#include <bsp/irq.h>
+
+#ifdef MPC55XX_HAS_LINFLEX
+
+mpc55xx_linflex_context mpc55xx_linflex_devices [] = {
+  {
+    .regs = &LINFLEX0,
+    .irq_rxi = MPC55XX_IRQ_LINFLEX_RXI(0),
+    .irq_txi = MPC55XX_IRQ_LINFLEX_TXI(0),
+    .irq_err = MPC55XX_IRQ_LINFLEX_ERR(0),
+    .tx_pcr_register = &((SIU_tag *) &SIUL)->PCR18,
+    .tx_pa_value = 1,
+    .rx_pcr_register = &((SIU_tag *) &SIUL)->PCR19,
+    .rx_psmi_register = &((SIU_tag *) &SIUL)->PSMI31,
+    .rx_padsel_value = 0
+  }, {
+    .regs = &LINFLEX1,
+    .irq_rxi = MPC55XX_IRQ_LINFLEX_RXI(1),
+    .irq_txi = MPC55XX_IRQ_LINFLEX_TXI(1),
+    .irq_err = MPC55XX_IRQ_LINFLEX_ERR(1),
+    .tx_pcr_register = &((SIU_tag *) &SIUL)->PCR94,
+    .tx_pa_value = 1,
+    .rx_pcr_register = &((SIU_tag *) &SIUL)->PCR95,
+    .rx_psmi_register = &((SIU_tag *) &SIUL)->PSMI32,
+    .rx_padsel_value = 2
+  }
+};
+
+void enter_init_mode(volatile LINFLEX_tag *regs)
+{
+  LINFLEX_LINCR1_32B_tag cr1 = { .R = regs->LINCR1.R };
+  cr1.B.SLEEP = 0;
+  cr1.B.INIT = 1;
+  regs->LINCR1.R = cr1.R;
+}
+
+void enter_active_mode(volatile LINFLEX_tag *regs)
+{
+  LINFLEX_LINCR1_32B_tag cr1 = { .R = regs->LINCR1.R };
+  cr1.B.SLEEP = 0;
+  cr1.B.INIT = 0;
+  regs->LINCR1.R = cr1.R;
+}
+
+void enter_sleep_mode(volatile LINFLEX_tag *regs)
+{
+  LINFLEX_LINCR1_32B_tag cr1 = { .R = regs->LINCR1.R };
+  cr1.B.SLEEP = 1;
+  cr1.B.INIT = 0;
+  regs->LINCR1.R = cr1.R;
+}
+
+static void mpc55xx_linflex_poll_write(int minor, char c)
+{
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  const LINFLEX_UARTSR_32B_tag clear_dtf = { .B = { .DTF_TFF = 1 } };
+  rtems_interrupt_level level;
+  bool done = false;
+  bool wait_for_transmit_done = false;
+
+  rtems_interrupt_disable(level);
+  if (self->transmit_nest_level == 0) {
+	LINFLEX_LINIER_32B_tag ier = { .R = regs->LINIER.R };
+
+	if (ier.B.DTIE != 0) {
+	  ier.B.DTIE = 0;
+	  regs->LINIER.R = ier.R;
+	  wait_for_transmit_done = !self->transmit_in_progress;
+	  self->transmit_nest_level = 1;
+	}
+  } else {
+	++self->transmit_nest_level;
+  }
+  rtems_interrupt_enable(level);
+
+  while (!done) {
+	rtems_interrupt_disable(level);
+	bool tx = self->transmit_in_progress;
+	if (!tx || (tx && regs->UARTSR.B.DTF_TFF)) {
+	  regs->UARTSR.R = clear_dtf.R;
+	  regs->BDRL.B.DATA0 = c;
+	  self->transmit_in_progress = true;
+	  done = true;
+	}
+	rtems_interrupt_enable(level);
+  }
+
+  done = false;
+  while (!done) {
+	rtems_interrupt_disable(level);
+	if (wait_for_transmit_done) {
+	  if (regs->UARTSR.B.DTF_TFF) {
+		regs->UARTSR.R = clear_dtf.R;
+		self->transmit_in_progress = false;
+		done = true;
+	  }
+	} else {
+	  done = true;
+	}
+
+	if (done && self->transmit_nest_level > 0) {
+	  --self->transmit_nest_level;
+
+	  if (self->transmit_nest_level == 0) {
+        LINFLEX_LINIER_32B_tag ier = { .R = regs->LINIER.R };
+
+        ier.B.DTIE = 1;
+		regs->LINIER.R = ier.R;
+	  }
+	}
+	rtems_interrupt_enable(level);
+  }
+}
+
+static void mpc55xx_linflex_rx_interrupt_handler(void *arg)
+{
+  mpc55xx_linflex_context *self = arg;
+  volatile LINFLEX_tag *regs = self->regs;
+  char c = regs->BDRM.B.DATA4;
+  const LINFLEX_UARTSR_32B_tag clear_flags = { .B = { .RMB = 1, .DRF_RFE = 1 } };
+
+  regs->UARTSR.R = clear_flags.R;
+
+  rtems_termios_enqueue_raw_characters(self->tty, &c, 1);
+}
+
+static void mpc55xx_linflex_tx_interrupt_handler(void *arg)
+{
+  mpc55xx_linflex_context *self = arg;
+  volatile LINFLEX_tag *regs = self->regs;
+
+  regs->UARTSR.B.DTF_TFF = 1;	/* clear flag */
+  self->transmit_in_progress = false;
+
+  rtems_termios_dequeue_characters(self->tty, 1);
+}
+
+/*
+static void mpc55xx_linflex_err_interrupt_handler(void *arg)
+{
+  mpc55xx_linflex_context *self = arg;
+}
+*/
+
+static int mpc55xx_linflex_set_attributes(int minor, const struct termios *t)
+{
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  LINFLEX_UARTCR_32B_tag uartcr = { .R = 0 };
+  LINFLEX_GCR_32B_tag gcr = { .R = 0 };
+  LINFLEX_LINIER_32B_tag ier = { .R = 0 };
+  rtems_termios_baud_t br = rtems_termios_baud_to_number(t->c_cflag);
+  LINFLEX_LINFBRR_32B_tag fbrr = { .R = 0 };
+  LINFLEX_LINIBRR_32B_tag ibrr = { .R = 0 };
+
+  enter_init_mode(regs);
+
+  /* Set to UART-mode */
+  uartcr.B.UART = 1;
+  regs->UARTCR.R = uartcr.R;
+
+  /* Set to buffer mode with size 1 */
+  uartcr.B.TDFL_TFC = 0;
+  uartcr.B.RDFL_RFC0 = 0;
+  uartcr.B.RFBM = 0;
+  uartcr.B.TFBM = 0;
+
+  /* Enable receiver and transmitter */
+  uartcr.B.RXEN = 1;
+  uartcr.B.TXEN = 1;
+
+  /* Number of data bits */
+  uartcr.B.WL1 = 0;
+  if ((t->c_cflag & CSIZE) == CS8) {
+	uartcr.B.WL0 = 1;
+  } else if ((t->c_cflag & CSIZE) == CS7) {
+	uartcr.B.WL0 = 0;
+  } else {
+    return -1;
+  }
+
+  /* Parity */
+  uartcr.B.PCE = (t->c_cflag & PARENB) ? 1 : 0;
+  uartcr.B.PC1 = 0;
+  uartcr.B.PC0 = (t->c_cflag & PARODD) ? 1 : 0;
+
+  /* Stop bits */
+  gcr.B.STOP = (t->c_cflag & CSTOPB) ? 1 : 0;
+
+  /* Set control registers */
+  regs->UARTCR.R = uartcr.R;
+  regs->GCR.R = gcr.R;
+
+  /* Interrupts */
+  ier.B.DTIE = 1;
+  ier.B.DRIE = 1;
+  regs->LINIER.R = ier.R;
+
+  /* Baud rate */
+  if (br > 0) {
+	uint32_t lfdiv_mult_32 = bsp_clock_speed * 2 / br;
+	if((lfdiv_mult_32 & 0x1) != 0) {
+		++lfdiv_mult_32;
+	}
+	fbrr.B.FBR = (lfdiv_mult_32 >> 1) & 0xF;
+	ibrr.B.IBR = lfdiv_mult_32 >> 5;
+  } else {
+    return -1;
+  }
+  regs->LINFBRR.R = fbrr.R;
+  regs->LINIBRR.R = ibrr.R;
+
+  enter_active_mode(regs);
+
+  return 0;
+}
+
+static int mpc55xx_linflex_first_open(int major, int minor, void *arg)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  int rv = 0;
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  struct rtems_termios_tty *tty = console_generic_get_tty_at_open(arg);
+  SIU_PCR_tag pcr = { .R = 0 };
+  SIUL_PSMI_8B_tag psmi = { .R = 0 };
+
+  self->tty = tty;
+
+  pcr.B.IBE = 1;
+  self->rx_pcr_register->R = pcr.R;
+  psmi.B.PADSEL = self->rx_padsel_value;
+  self->rx_psmi_register->R = psmi.R;
+  pcr.R = 0;
+  pcr.B.OBE = 1;
+  pcr.B.PA = self->tx_pa_value;
+  self->tx_pcr_register->R = pcr.R;
+
+  rv = rtems_termios_set_initial_baud(tty, 115200);
+  if (rv != 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  rv = mpc55xx_linflex_set_attributes(minor, &tty->termios);
+  if (rv != 0) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  sc = mpc55xx_interrupt_handler_install(
+    self->irq_rxi,
+    "LINFlexD RXI",
+    RTEMS_INTERRUPT_UNIQUE,
+    MPC55XX_INTC_DEFAULT_PRIORITY,
+    mpc55xx_linflex_rx_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  sc = mpc55xx_interrupt_handler_install(
+    self->irq_txi,
+    "LINFlexD TXI",
+    RTEMS_INTERRUPT_UNIQUE,
+    MPC55XX_INTC_DEFAULT_PRIORITY,
+    mpc55xx_linflex_tx_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  /*
+  sc = mpc55xx_interrupt_handler_install(
+    self->irq_err,
+    "LINFlexD ERR",
+    RTEMS_INTERRUPT_UNIQUE,
+    MPC55XX_INTC_DEFAULT_PRIORITY,
+    mpc55xx_linflex_err_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+  */
+
+  return 0;
+}
+
+static int mpc55xx_linflex_last_close(int major, int minor, void* arg)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  SIU_PCR_tag pcr = { .R = 0 };
+  SIUL_PSMI_8B_tag psmi = { .R = 0 };
+
+  /* enter initialization mode */
+  enter_init_mode(regs);
+
+  /* disable interrupts */
+  regs->LINIER.R = 0;
+
+  /* set module to sleep mode */
+  enter_sleep_mode(regs);
+
+  sc = rtems_interrupt_handler_remove(
+    self->irq_rxi,
+    mpc55xx_linflex_rx_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  sc = rtems_interrupt_handler_remove(
+    self->irq_txi,
+    mpc55xx_linflex_tx_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+
+  /*
+  sc = rtems_interrupt_handler_remove(
+    self->irq_err,
+    mpc55xx_linflex_err_interrupt_handler,
+    self
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    rtems_fatal_error_occurred(0xdeadbeef);
+  }
+  */
+
+  pcr.B.IBE = 1;
+  self->rx_pcr_register->R = pcr.R;
+  self->tx_pcr_register->R = pcr.R;
+  psmi.R = 0;
+  self->rx_psmi_register->R = psmi.R;
+
+  self->tty = NULL;
+
+  return 0;
+}
+
+static int mpc55xx_linflex_poll_read(int minor)
+{
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  rtems_interrupt_level level;
+  int c = -1;
+
+  rtems_interrupt_disable(level);
+  if (regs->UARTSR.B.DRF_RFE != 0) {
+    /* Clear flag */
+    regs->UARTSR.B.DRF_RFE = 1;
+
+    /* Read */
+    c = regs->BDRM.B.DATA4;
+  }
+  rtems_interrupt_enable(level);
+
+  return c;
+}
+
+static int mpc55xx_linflex_write(int minor, const char *out, size_t n)
+{
+  mpc55xx_linflex_context *self = console_generic_get_context(minor);
+  volatile LINFLEX_tag *regs = self->regs;
+  rtems_interrupt_level level;
+
+  rtems_interrupt_disable(level);
+
+  regs->BDRL.B.DATA0 = out [0];
+  self->transmit_in_progress = true;
+  /* TODO: send more then one byte */
+
+  rtems_interrupt_enable(level);
+
+  return 0;
+}
+
+const console_generic_callbacks mpc55xx_linflex_callbacks = {
+  .termios_callbacks = {
+    .firstOpen = mpc55xx_linflex_first_open,
+    .lastClose = mpc55xx_linflex_last_close,
+    .write = mpc55xx_linflex_write,
+    .setAttributes = mpc55xx_linflex_set_attributes,
+    .outputUsesInterrupts = TERMIOS_IRQ_DRIVEN
+  },
+  .poll_read = mpc55xx_linflex_poll_read,
+  .poll_write = mpc55xx_linflex_poll_write
+};
+
+#endif /* MPC55XX_HAS_LINFLEX */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-esci.h b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-esci.h
new file mode 100644
index 0000000000..1e51ecde7e
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-esci.h
@@ -0,0 +1,57 @@
+/**
+ * @file
+ *
+ * @brief Console ESCI API.
+ */
+
+/*
+ * 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.
+ */
+
+#ifndef LIBBSP_POWERPC_MPC55XXEVB_CONSOLE_ESCI_H
+#define LIBBSP_POWERPC_MPC55XXEVB_CONSOLE_ESCI_H
+
+#include "console-generic.h"
+
+#undef CR0
+#undef CR1
+#undef CR2
+#undef CR3
+
+#include <mpc55xx/regs.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+#ifdef MPC55XX_HAS_ESCI
+
+extern const console_generic_callbacks mpc55xx_esci_callbacks;
+
+typedef struct {
+  volatile struct ESCI_tag *regs;
+  struct rtems_termios_tty *tty;
+  int transmit_nest_level;
+  bool transmit_in_progress;
+  rtems_vector_number irq;
+} mpc55xx_esci_context;
+
+extern mpc55xx_esci_context mpc55xx_esci_devices [];
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* LIBBSP_POWERPC_MPC55XXEVB_CONSOLE_ESCI_H */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-generic.h b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-generic.h
new file mode 100644
index 0000000000..630f9da873
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-generic.h
@@ -0,0 +1,81 @@
+/**
+ * @file
+ *
+ * @brief Generic console driver API.
+ */
+
+/*
+ * 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.
+ */
+
+#ifndef LIBBSP_SHARED_CONSOLE_GENERIC_H
+#define LIBBSP_SHARED_CONSOLE_GENERIC_H
+
+#include <rtems/libio.h>
+#include <rtems/termiostypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+typedef struct {
+  rtems_termios_callbacks termios_callbacks;
+  int (*poll_read)(int minor);
+  void (*poll_write)(int minor, char c);
+} console_generic_callbacks;
+
+typedef struct {
+  void *context;
+  const console_generic_callbacks *callbacks;
+  const char *device_path;
+} console_generic_info;
+
+extern const console_generic_info console_generic_info_table [];
+
+extern const size_t console_generic_info_count;
+
+extern const rtems_device_minor_number console_generic_minor;
+
+#define CONSOLE_GENERIC_INFO_TABLE \
+  const console_generic_info console_generic_info_table []
+
+#define CONSOLE_GENERIC_INFO(context, callbacks, device_path) \
+  { context, callbacks, device_path }
+
+#define CONSOLE_GENERIC_INFO_COUNT \
+  const size_t console_generic_info_count = \
+    sizeof(console_generic_info_table) / sizeof(console_generic_info_table [0])
+
+#define CONSOLE_GENERIC_MINOR(minor) \
+  const rtems_device_minor_number console_generic_minor = (minor)
+
+static inline void *console_generic_get_context(int minor)
+{
+  return console_generic_info_table [minor].context;
+}
+
+static inline struct rtems_termios_tty *console_generic_get_tty_at_open(
+  void *arg
+)
+{
+  const rtems_libio_open_close_args_t *oc =
+    (const rtems_libio_open_close_args_t *) arg;
+
+  return (struct rtems_termios_tty *) oc->iop->data1;
+}
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* LIBBSP_SHARED_CONSOLE_GENERIC_H */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-linflex.h b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-linflex.h
new file mode 100644
index 0000000000..791bb8c983
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-linflex.h
@@ -0,0 +1,64 @@
+/**
+ * @file
+ *
+ * @brief Console LINFlexD API.
+ */
+
+/*
+ * 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.
+ */
+
+#ifndef LIBBSP_POWERPC_MPC55XXEVB_CONSOLE_LINFLEX_H
+#define LIBBSP_POWERPC_MPC55XXEVB_CONSOLE_LINFLEX_H
+
+#include "console-generic.h"
+
+#undef CR0
+#undef CR1
+#undef CR2
+#undef CR3
+
+#include <mpc55xx/regs.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+#ifdef MPC55XX_HAS_LINFLEX
+
+extern const console_generic_callbacks mpc55xx_linflex_callbacks;
+
+typedef struct {
+  volatile LINFLEX_tag *regs;
+  struct rtems_termios_tty *tty;
+  rtems_vector_number irq_rxi;
+  rtems_vector_number irq_txi;
+  rtems_vector_number irq_err;
+  volatile SIU_PCR_tag *tx_pcr_register;
+  uint8_t tx_pa_value:2;
+  volatile SIU_PCR_tag *rx_pcr_register;
+  volatile SIUL_PSMI_8B_tag *rx_psmi_register;
+  uint8_t rx_padsel_value:4;
+  int transmit_nest_level;
+  bool transmit_in_progress;
+} mpc55xx_linflex_context;
+
+extern mpc55xx_linflex_context mpc55xx_linflex_devices [];
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* LIBBSP_POWERPC_MPC55XXEVB_CONSOLE_LINFLEX_H */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/mpc55xx-config.h b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/mpc55xx-config.h
index 181e519921..93d212f807 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/mpc55xx-config.h
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/mpc55xx-config.h
@@ -3,7 +3,7 @@
  *
  * @ingroup mpc55xx
  *
- * @brief MPC55XX low-level configuration.
+ * @brief Low-level configuration.
  */
 
 /*
@@ -27,6 +27,10 @@
 
 #include <stddef.h>
 
+#include <libcpu/powerpc-utility.h>
+
+#include <bsp/start.h>
+
 #include <mpc55xx/regs.h>
 #include <mpc55xx/regs-mmu.h>
 
@@ -38,25 +42,89 @@ typedef struct {
   uint16_t index;
   uint16_t count;
   union SIU_PCR_tag pcr;
-} mpc55xx_siu_pcr_config_entry;
+} mpc55xx_siu_pcr_config;
+
+extern const mpc55xx_siu_pcr_config mpc55xx_start_config_siu_pcr [];
+
+extern const size_t mpc55xx_start_config_siu_pcr_count [];
+
+extern const struct MMU_tag mpc55xx_start_config_mmu_early [];
+
+extern const size_t mpc55xx_start_config_mmu_early_count [];
+
+extern const struct MMU_tag mpc55xx_start_config_mmu [];
+
+extern const size_t mpc55xx_start_config_mmu_count [];
+
+#ifdef MPC55XX_HAS_FMPLL
+  typedef struct {
+    union FMPLL_SYNCR_tag syncr_tmp;
+    union FMPLL_SYNCR_tag syncr_final;
+  } mpc55xx_clock_config;
+#endif
+
+#ifdef MPC55XX_HAS_FMPLL_ENHANCED
+  typedef struct {
+    union FMPLL_ESYNCR2_tag esyncr2_tmp;
+    union FMPLL_ESYNCR2_tag esyncr2_final;
+    union FMPLL_ESYNCR1_tag esyncr1_final;
+  } mpc55xx_clock_config;
+#endif
+
+#ifdef MPC55XX_HAS_MODE_CONTROL
+  typedef struct {
+    struct {
+      PLLD_CR_32B_tag cr;
+      PLLD_MR_32B_tag mr;
+    } fmpll [2];
+    CGM_OC_EN_32B_tag oc_en;
+    CGM_OCDS_SC_32B_tag ocds_sc;
+    CGM_SC_DC0_3_32B_tag sc_dc0_3;
+    CGM_AUXCLK_tag auxclk [5];
+  } mpc55xx_clock_config;
+#endif
+
+extern const mpc55xx_clock_config mpc55xx_start_config_clock [];
+
+#ifdef MPC55XX_HAS_EBI
+  extern const struct EBI_CS_tag mpc55xx_start_config_ebi_cs [];
+
+  extern const size_t mpc55xx_start_config_ebi_cs_count [];
+
+  extern const struct EBI_CAL_CS_tag mpc55xx_start_config_ebi_cal_cs [];
+
+  extern const size_t mpc55xx_start_config_ebi_cal_cs_count [];
+#endif
+
+void mpc55xx_start_early(void);
+
+void mpc55xx_start_flash(void);
 
-extern const mpc55xx_siu_pcr_config_entry mpc55xx_siu_pcr_config [];
+void mpc55xx_start_cache(void);
 
-extern const size_t mpc55xx_siu_pcr_config_count [];
+void mpc55xx_start_clock(void);
 
-extern const struct MMU_tag mpc55xx_mmu_config [];
+void mpc55xx_start_watchdog(void);
 
-extern const size_t mpc55xx_mmu_config_count [];
+void mpc55xx_start_mmu_apply_config(const struct MMU_tag *config, size_t count);
 
-extern const struct EBI_CS_tag mpc55xx_ebi_cs_config [];
+uint32_t mpc55xx_get_system_clock(void);
 
-extern const size_t mpc55xx_ebi_cs_config_count [];
+LINKER_SYMBOL(bsp_ram_start)
+LINKER_SYMBOL(bsp_ram_end)
+LINKER_SYMBOL(bsp_ram_size)
 
-extern const struct EBI_CAL_CS_tag mpc55xx_ebi_cal_cs_config [];
+LINKER_SYMBOL(bsp_ram_1_start)
+LINKER_SYMBOL(bsp_ram_1_end)
+LINKER_SYMBOL(bsp_ram_1_size)
 
-extern const size_t mpc55xx_ebi_cal_cs_config_count [];
+LINKER_SYMBOL(bsp_rom_start)
+LINKER_SYMBOL(bsp_rom_end)
+LINKER_SYMBOL(bsp_rom_size)
 
-void mpc55xx_early_init(void);
+#ifdef MPC55XX_BOOTFLAGS
+  extern uint32_t mpc55xx_bootflag_0 [];
+#endif
 
 #ifdef __cplusplus
 }
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/make/custom/xkt564levb.cfg b/c/src/lib/libbsp/powerpc/mpc55xxevb/make/custom/xkt564levb.cfg
new file mode 100644
index 0000000000..eb9c0db185
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/make/custom/xkt564levb.cfg
@@ -0,0 +1,10 @@
+##
+#
+# @file
+#
+# @ingroup mpc55xx_config
+#
+# @brief Configuration file for XKT564LEVB evaluation board.
+#
+
+include $(RTEMS_ROOT)/make/custom/mpc55xx.inc
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/network/smsc9218i.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/network/smsc9218i.c
index c1a00b0498..4202a49066 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/network/smsc9218i.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/network/smsc9218i.c
@@ -22,6 +22,10 @@
  * $Id$
  */
 
+#include <rtems.h>
+
+#ifdef RTEMS_NETWORKING
+
 #define __INSIDE_RTEMS_BSD_TCPIP_STACK__ 1
 #define __BSD_VISIBLE 1
 
@@ -34,7 +38,6 @@
 #include <string.h>
 #include <inttypes.h>
 
-#include <rtems.h>
 #include <rtems/rtems_bsdnet.h>
 #include <rtems/rtems_mii_ioctl.h>
 
@@ -1909,3 +1912,5 @@ int smsc9218i_attach_detach(
   /* FIXME: Return value */
   return 0;
 }
+
+#endif /* RTEMS_NETWORKING */
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/preinstall.am b/c/src/lib/libbsp/powerpc/mpc55xxevb/preinstall.am
index 6e70981145..8992081670 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/preinstall.am
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/preinstall.am
@@ -77,6 +77,10 @@ $(PROJECT_LIB)/linkcmds.mpc5674fevb_spe: startup/linkcmds.mpc5674fevb_spe $(PROJ
 	$(INSTALL_DATA) $< $(PROJECT_LIB)/linkcmds.mpc5674fevb_spe
 PREINSTALL_FILES += $(PROJECT_LIB)/linkcmds.mpc5674fevb_spe
 
+$(PROJECT_LIB)/linkcmds.xkt564levb: startup/linkcmds.xkt564levb $(PROJECT_LIB)/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_LIB)/linkcmds.xkt564levb
+PREINSTALL_FILES += $(PROJECT_LIB)/linkcmds.xkt564levb
+
 $(PROJECT_LIB)/linkcmds.phycore_mpc5554: startup/linkcmds.phycore_mpc5554 $(PROJECT_LIB)/$(dirstamp)
 	$(INSTALL_DATA) $< $(PROJECT_LIB)/linkcmds.phycore_mpc5554
 PREINSTALL_FILES += $(PROJECT_LIB)/linkcmds.phycore_mpc5554
@@ -101,18 +105,6 @@ $(PROJECT_INCLUDE)/coverhd.h: ../../shared/include/coverhd.h $(PROJECT_INCLUDE)/
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/coverhd.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/coverhd.h
 
-$(PROJECT_INCLUDE)/bsp/mpc55xxevb.h: include/mpc55xxevb.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
-	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/mpc55xxevb.h
-PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/mpc55xxevb.h
-
-$(PROJECT_INCLUDE)/bsp/smsc9218i.h: include/smsc9218i.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
-	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/smsc9218i.h
-PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/smsc9218i.h
-
-$(PROJECT_INCLUDE)/bsp/mpc55xx-config.h: include/mpc55xx-config.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
-	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/mpc55xx-config.h
-PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/mpc55xx-config.h
-
 $(PROJECT_INCLUDE)/bsp/mpc83xx_i2cdrv.h: ../../../libcpu/powerpc/mpc83xx/i2c/mpc83xx_i2cdrv.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/mpc83xx_i2cdrv.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/mpc83xx_i2cdrv.h
@@ -141,3 +133,27 @@ $(PROJECT_INCLUDE)/bsp/tictac.h: ../shared/include/tictac.h $(PROJECT_INCLUDE)/b
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/tictac.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/tictac.h
 
+$(PROJECT_INCLUDE)/bsp/mpc55xx-config.h: include/mpc55xx-config.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/mpc55xx-config.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/mpc55xx-config.h
+
+$(PROJECT_INCLUDE)/bsp/mpc55xxevb.h: include/mpc55xxevb.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/mpc55xxevb.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/mpc55xxevb.h
+
+$(PROJECT_INCLUDE)/bsp/smsc9218i.h: include/smsc9218i.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/smsc9218i.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/smsc9218i.h
+
+$(PROJECT_INCLUDE)/bsp/console-esci.h: include/console-esci.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/console-esci.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/console-esci.h
+
+$(PROJECT_INCLUDE)/bsp/console-generic.h: include/console-generic.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/console-generic.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/console-generic.h
+
+$(PROJECT_INCLUDE)/bsp/console-linflex.h: include/console-linflex.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/console-linflex.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/console-linflex.h
+
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: