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.
2012-01-23 11:19:22 +01:00
parent 9bf3a86865
commit a762dc2a49
54 changed files with 23535 additions and 1105 deletions
--- 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
--- 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
--- 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 <rtems/status-checks.h>
-
-/* This is defined in clockdrv_shell.h */
-rtems_isr Clock_isr( rtems_vector_number vector);
-
-#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/regs.h>

 static uint64_t mpc55xx_clock_factor;

-static void mpc55xx_clock_handler_install( rtems_isr_entry isr,
-					   rtems_isr_entry *old_isr)
+#if defined(MPC55XX_CLOCK_EMIOS_CHANNEL)
+
+#include <mpc55xx/emios.h>
+
+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_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)

-#define Clock_driver_support_install_isr( isr, old_isr) \
-  mpc55xx_clock_handler_install(isr,&old_isr)
+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 } };

-#define Clock_driver_support_shutdown_hardware() mpc55xx_clock_cleanup()
+  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];
+
+  channel->TCTRL.R = 0;
+}
+
+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

--- 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])

--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-config.c
+++ 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);
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-esci.c
+++ 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 */
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-generic.c
+++ 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;
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/console/console-linflex.c
+++ 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 */
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-esci.h
+++ 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 */
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-generic.h
+++ 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 */
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/console-linflex.h
+++ 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 */
--- 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_entry mpc55xx_siu_pcr_config [];
+extern const mpc55xx_siu_pcr_config mpc55xx_start_config_siu_pcr [];

-extern const size_t mpc55xx_siu_pcr_config_count [];
+extern const size_t mpc55xx_start_config_siu_pcr_count [];

-extern const struct MMU_tag mpc55xx_mmu_config [];
+extern const struct MMU_tag mpc55xx_start_config_mmu_early [];

-extern const size_t mpc55xx_mmu_config_count [];
+extern const size_t mpc55xx_start_config_mmu_early_count [];

-extern const struct EBI_CS_tag mpc55xx_ebi_cs_config [];
+extern const struct MMU_tag mpc55xx_start_config_mmu [];

-extern const size_t mpc55xx_ebi_cs_config_count [];
+extern const size_t mpc55xx_start_config_mmu_count [];

-extern const struct EBI_CAL_CS_tag mpc55xx_ebi_cal_cs_config [];
+#ifdef MPC55XX_HAS_FMPLL
+  typedef struct {
+    union FMPLL_SYNCR_tag syncr_tmp;
+    union FMPLL_SYNCR_tag syncr_final;
+  } mpc55xx_clock_config;
+#endif

-extern const size_t mpc55xx_ebi_cal_cs_config_count [];
+#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

-void mpc55xx_early_init(void);
+#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);
+
+void mpc55xx_start_cache(void);
+
+void mpc55xx_start_clock(void);
+
+void mpc55xx_start_watchdog(void);
+
+void mpc55xx_start_mmu_apply_config(const struct MMU_tag *config, size_t count);
+
+uint32_t mpc55xx_get_system_clock(void);
+
+LINKER_SYMBOL(bsp_ram_start)
+LINKER_SYMBOL(bsp_ram_end)
+LINKER_SYMBOL(bsp_ram_size)
+
+LINKER_SYMBOL(bsp_ram_1_start)
+LINKER_SYMBOL(bsp_ram_1_end)
+LINKER_SYMBOL(bsp_ram_1_size)
+
+LINKER_SYMBOL(bsp_rom_start)
+LINKER_SYMBOL(bsp_rom_end)
+LINKER_SYMBOL(bsp_rom_size)
+
+#ifdef MPC55XX_BOOTFLAGS
+  extern uint32_t mpc55xx_bootflag_0 [];
+#endif

 #ifdef __cplusplus
 }
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/make/custom/xkt564levb.cfg
+++ 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
--- 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 */
--- 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
+
--- 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
 }
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/early-init.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/early-init.c
@@ -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();
-}
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/fmpll-syncr-vals.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/fmpll-syncr-vals.c
@@ -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) */
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/get-system-clock.c
+++ 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;
+}
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/linkcmds.xkt564levb
+++ 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
--- 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
  }
 }
--- 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 */
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-cache.S
+++ 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
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-clock.c
+++ 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
+}
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-clock.c
+++ 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
+} };
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-ebi-cs-cal.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 */
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-ebi-cs.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 */
--- 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
@@ -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])
+};
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-mmu.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])
 };
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-config-siu-pcr.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])
 };
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c
+++ 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();
+}
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-flash.S
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-flash.S
@@ -19,13 +19,13 @@
 * found in the file LICENSE in this distribution or at
 * http://www.rtems.com/license/LICENSE.
 *
- * $Id$
+ * $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"
+	.section	".bsp_start_text", "ax"

 /* Optimized flash configurations (Table 13-15 [MPC5567 Microcontroller Reference Manual]) */
 .equ FLASH_SETTINGS_RESET, 0xff00
@@ -35,78 +35,81 @@
 .equ FLASH_SETTINGS_264, 0x01716B15

 /**
- * @fn void mpc55xx_flash_init()
+ * @fn void mpc55xx_start_flash()
 * @brief Optimized flash configuration.
- * @warning Code will be copied and executed on the stack.  The stack pointer
- * will not be updated, since this function has to work before memory
- * initialization.
+ * @warning Code will be copied and executed on the stack.
 */
-GLOBAL_FUNCTION mpc55xx_flash_init
-	mflr r31
+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 r30, FLASH_SETTINGS_RESET
-	b settings_done
+	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 r30, FLASH_SETTINGS_82
-	b settings_done
+	LWI	r3, FLASH_SETTINGS_82
+	b	settings_done
 clock_102:
-	LWI r30, FLASH_SETTINGS_102
-	b settings_done
+	LWI	r3, FLASH_SETTINGS_102
+	b	settings_done
 clock_132:
-	LWI r30, FLASH_SETTINGS_132
+	LWI	r3, FLASH_SETTINGS_132
 	b settings_done
 clock_264:
-	LWI r30, FLASH_SETTINGS_264
-	b settings_done
+	LWI	r3, FLASH_SETTINGS_264
+	b	settings_done
 settings_done:

 	/* Copy store code on the stack */
-	LA r3, store_start
-	LA r5, store_end
-	subf r5, r3, r5
-	subf r4, r5, r1
-
-	/* Assert: Proper alignment of destination start */
-	andi. r6, r4, 0x7
-	bne twiddle
-
-	/* Copy */
-	bl mpc55xx_copy_8
-
-	LA r6, FLASH_BIUCR
+	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 */
-	mtctr r4
+	LA	r4, FLASH_BIUCR
+	addi	r5, r1, 8
+	mtctr	r5
 	bctrl

-	mtlr r31
+	/* Return */
+	lwz	r0, lr_offset(r1)
+	addi	r1, r1, stack_size
+	mtlr	r0
 	blr

 /*
 * Store flash settings
 */

-	.align 3
-	.set store_start, .
-	stw r30, 0(r6)
+store_start:
+
+	stw	r3, 0(r4)
 	isync
 	blr
-	.align 3
-	.set store_end, .

-twiddle:
-	b twiddle
+#endif
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-watchdog.c
+++ 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
+}
--- 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>
-
-#include <bsp/vectors.h>
-
-#define HAS_CACHE (BSP_DATA_CACHE_ENABLED || BSP_INSTRUCTION_CACHE_ENABLED)
-
-	.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
-
-	.globl _start
-	
-#ifdef MPC55XX_BOOTFLAGS
-        .globl mpc55xx_bootflag_0
-        .globl mpc55xx_bootflag_1
+#if MPC55XX_CHIP_TYPE / 10 != 551
+  #define HAS_SPE
 #endif

-	.section ".bsp_start_text", "ax"
+#if MPC55XX_CHIP_TYPE / 10 == 564
+  #define INIT_REGISTERS_FOR_LSM
+#endif

-	/* BAM: RCHW */
-	.int 0x005a0000
+#ifdef HAS_SPE
+  #define ZERO_GPR(reg) evxor	reg, reg, reg
+#else
+  #define ZERO_GPR(reg) xor	reg, reg, reg
+#endif

-	/* BAM: Address of start instruction */
-        .int _start
+	.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

 #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.
-         */
+	.globl	mpc55xx_bootflag_0
+	.globl	mpc55xx_bootflag_1
+#endif
+
+	.section	".bsp_start_text", "ax"
+
+	/* BAM: RCHW */
+	.int	0x005a0000
+
+	/* BAM: Address of start instruction */
+	.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.
+	 */
 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
-	mtspr	TBWL, r0
-	mfspr	r2, HID0
-	ori	r2, r2, 0x4000
-	mtspr	HID0, r2
-
-	/* FMPLL setup */
-	LWI	r3, mpc55xx_fmpll_config
-	bl	mpc55xx_fmpll_init
-
-	/* Enable branch prediction */
-	LWI	r2, BUCSR_BBFI | BUCSR_BPEN
-	mtspr	BUCSR, r2
-
-	/* Set intermediate stack start to end of internal SRAM */
-	LA	r1, bsp_ram_end
-	subi	r1, r1, 16
-
 	/* 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
+#ifdef HAS_SPE
+	mfmsr	r3
+	oris	r3, r3, MSR_SPE >> 16
+	mtmsr	r3
+	isync
 #endif

-	/* Zero internal SRAM (needed to get proper ECC) */
-	LA	r3, bsp_ram_start
-	LA	r4, bsp_ram_size
-	bl	bsp_start_zero
+	/*
+	 * 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
+	mtspr	TBWU, r0

-	/* Initialize intermediate start stack */
+	/* Enable time base */
+	mfspr	r3, HID0
+	ori	r3, r3, 0x4000
+	mtspr	HID0, r3
+
+	/* Enable branch prediction */
+	LWI	r3, FSL_EIS_BUCSR_BBFI | FSL_EIS_BUCSR_BPEN
+	mtspr	FSL_EIS_BUCSR, r3
+
+	/* MMU early initialization */
+	LA	r3, mpc55xx_start_config_mmu_early
+	LW	r4, mpc55xx_start_config_mmu_early_count
+	bl	mpc55xx_start_mmu_apply_config
+
+	/* Initialize intermediate stack (ECC) */
+
+	LA	r3, bsp_ram_start
+	addi	r4, r3, MPC55XX_EARLY_STACK_SIZE
+
+zero_intermediate_stack_loop:
+
+#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
+
+	/* Next steps in C */
+	bl	mpc55xx_start_early
+
+	/* Initialize start stack */
+	LA	r1, start_stack_end
+	subi	r1, r1, 16
 	li	r0, 0
 	stw	r0, 0(r1)
-	stw	r0, 4(r1)

-	/* Do early initialization */
-	bl	mpc55xx_early_init
+	/* 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

-	/* Initialize start stack */
-	LWI	r1, start_stack_end
-	subi	r1, r1, 16
-	li	r0, 0
-	stw	r0, 0(r1)
-
 	/* Clear command line */
 	li	r3, 0

@@ -154,83 +246,39 @@ _start:

 	/* Spin around */
 twiddle:
+
 	b	twiddle

-#if HAS_CACHE
-config_cache:
+mpc55xx_start_mmu_apply_config:

-	/* Load zero, CINV, and CABT) */
-	li	r0, 0
-	li	r3, 0x2
-	li	r4, 0x4
+	cmpwi	cr7, r4, r0
+	beqlr	cr7
+	mtctr	r4

-#if MPC55XX_CHIP_TYPE / 10 == 567
-start_instruction_cache_invalidation:
+mmu_init_loop:

-	/* 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
-
-	/* 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
-
-	/* Return */
+	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
-#endif /* HAS_CACHE */
+
+load_section:
+	cmpw	cr7, r3, r4
+	beqlr	cr7
+	b	memcpy

 	/* Start stack area */
-	.section ".bsp_rwextra", "aw", @nobits
-	.align 4
-	.space 4096
+
+	.section	".bsp_rwextra", "aw", @nobits
+	.align	4
+	.space	4096
+
 start_stack_end:
--- a/c/src/lib/libcpu/powerpc/Makefile.am
+++ b/c/src/lib/libcpu/powerpc/Makefile.am
@@ -418,13 +418,13 @@ include_mpc55xx_HEADERS += mpc55xx/include/dspi.h
 include_mpc55xx_HEADERS += mpc55xx/include/edma.h
 include_mpc55xx_HEADERS += mpc55xx/include/emios.h
 include_mpc55xx_HEADERS += mpc55xx/include/mpc55xx.h
-include_mpc55xx_HEADERS += mpc55xx/include/esci.h
 include_mpc55xx_HEADERS += mpc55xx/include/siu.h
 include_mpc55xx_HEADERS += mpc55xx/include/irq.h
 include_mpc55xx_HEADERS += mpc55xx/include/watchdog.h
 include_mpc55xx_HEADERS += mpc55xx/include/fsl-mpc551x.h
 include_mpc55xx_HEADERS += mpc55xx/include/fsl-mpc555x.h
 include_mpc55xx_HEADERS += mpc55xx/include/fsl-mpc556x.h
+include_mpc55xx_HEADERS += mpc55xx/include/fsl-mpc564xL.h
 include_mpc55xx_HEADERS += mpc55xx/include/fsl-mpc567x.h
 include_mpc55xx_HEADERS += mpc55xx/include/regs-edma.h
 include_mpc55xx_HEADERS += mpc55xx/include/regs-mmu.h
@@ -462,11 +462,6 @@ noinst_PROGRAMS += mpc55xx/siu.rel
 mpc55xx_siu_rel_SOURCES = mpc55xx/siu/siu.c
 mpc55xx_siu_rel_LDFLAGS = $(RTEMS_RELLDFLAGS)

-# eSCI
-noinst_PROGRAMS += mpc55xx/esci.rel
-mpc55xx_esci_rel_SOURCES = mpc55xx/esci/esci.c
-mpc55xx_esci_rel_LDFLAGS = $(RTEMS_RELLDFLAGS)
-
 # DSPI
 noinst_PROGRAMS += mpc55xx/dspi.rel
 mpc55xx_dspi_rel_SOURCES = mpc55xx/dspi/dspi.c
@@ -475,8 +470,6 @@ mpc55xx_dspi_rel_LDFLAGS = $(RTEMS_RELLDFLAGS)
 # Misc
 noinst_PROGRAMS += mpc55xx/misc.rel
 mpc55xx_misc_rel_SOURCES = mpc55xx/misc/copy.S \
-	mpc55xx/misc/fmpll.S \
-	mpc55xx/misc/flash.S \
 	mpc55xx/misc/flash_support.c
 mpc55xx_misc_rel_LDFLAGS = $(RTEMS_RELLDFLAGS)

--- a/c/src/lib/libcpu/powerpc/mpc55xx/dspi/dspi.c
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/dspi/dspi.c
@@ -747,6 +747,7 @@ mpc55xx_dspi_bus_entry mpc55xx_dspi_bus_table [MPC55XX_DSPI_NUMBER] = {
 		},
 		.idle_char = 0xffffffff,
 		.baud = 0
+#ifdef DSPI_D
 	}, {
 		/* DSPI D */
 		.bus = {
@@ -775,5 +776,6 @@ mpc55xx_dspi_bus_entry mpc55xx_dspi_bus_table [MPC55XX_DSPI_NUMBER] = {
 		},
 		.idle_char = 0xffffffff,
 		.baud = 0
+#endif
 	}
 };
--- a/c/src/lib/libcpu/powerpc/mpc55xx/edma/edma.c
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/edma/edma.c
@@ -7,15 +7,17 @@
 */

 /*
- * Copyright (c) 2008
- * Embedded Brains GmbH
- * Obere Lagerstr. 30
- * D-82178 Puchheim
- * Germany
- * rtems@embedded-brains.de
+ * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
 *
- * 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.
+ *  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>
@@ -29,6 +31,8 @@

 #if MPC55XX_CHIP_TYPE / 10 == 551
  #define EDMA_CHANNEL_COUNT 16U
+#elif MPC55XX_CHIP_TYPE / 10 == 564
+  #define EDMA_CHANNEL_COUNT 16U
 #elif MPC55XX_CHIP_TYPE / 10 == 567
  #define EDMA_CHANNEL_COUNT 96U
 #else
--- a/c/src/lib/libcpu/powerpc/mpc55xx/emios/emios.c
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/emios/emios.c
@@ -7,28 +7,22 @@
 */

 /*
- * 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 <mpc55xx/mpc55xx.h>

-#include <bsp/irq.h>
-#include <bsp/utility.h>
-
-#define RTEMS_STATUS_CHECKS_USE_PRINTK
-
-#include <rtems/status-checks.h>
+#ifdef MPC55XX_HAS_EMIOS

 /**
 * @brief Initialize the eMIOS module.
@@ -107,3 +101,5 @@ void mpc55xx_emios_set_global_prescaler( unsigned prescaler)
  /* Set MCR */
  EMIOS.MCR.R = mcr.R;
 }
+
+#endif /* MPC55XX_HAS_EMIOS */
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/emios.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/emios.h
@@ -7,12 +7,13 @@
 */

 /*
- * 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
@@ -22,18 +23,14 @@
 #ifndef LIBCPU_POWERPC_MPC55XX_EMIOS_H
 #define LIBCPU_POWERPC_MPC55XX_EMIOS_H

-#include <stdbool.h>
-#include <stdint.h>
-
-#include <rtems.h>
-#include <rtems/chain.h>
-
-#include <bspopts.h>
+#include <mpc55xx/regs.h>

 #ifdef __cplusplus
 extern "C" {
 #endif /* __cplusplus */

+#ifdef MPC55XX_HAS_EMIOS
+
 /**
 * @name eMIOS - Modes
 *
@@ -191,6 +188,8 @@ unsigned mpc55xx_emios_global_prescaler( void);

 void mpc55xx_emios_set_global_prescaler( unsigned prescaler);

+#endif /* MPC55XX_HAS_EMIOS */
+
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc551x.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc551x.h
@@ -52,6 +52,8 @@
 #ifndef _MPC5510_H_
 #define _MPC5510_H_

+#ifndef ASM
+
 #include <stdint.h>

 #include <mpc55xx/regs-edma.h>
@@ -3969,6 +3971,7 @@ extern "C" {
 #ifdef  __cplusplus
 }
 #endif
+#endif /* ASM */
 #endif                          /* ifdef _MPC5510_H */
 /*********************************************************************
 *
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc555x.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc555x.h
@@ -103,6 +103,8 @@
 #ifndef _MPC5554_H_
 #define _MPC5554_H_

+#ifndef ASM
+
 #include <stdint.h>

 #include <mpc55xx/regs-edma.h>
@@ -3347,6 +3349,7 @@ union EQADC_WRITE_CONFIGURATION_COMMAND_tag {
 #ifdef  __cplusplus
 }
 #endif
+#endif /* ASM */
 #endif                          /* ifdef _MPC5554_H */
 /*********************************************************************
 *
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc556x.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc556x.h
@@ -70,6 +70,8 @@
 #ifndef _MPC5567_H_
 #define _MPC5567_H_

+#ifndef ASM
+
 #include <stdint.h>

 #include <mpc55xx/regs-edma.h>
@@ -4527,6 +4529,7 @@ extern "C" {
 #ifdef  __cplusplus
 }
 #endif
+#endif /* ASM */
 #endif                          /* ifdef _MPC5567_H */
 /*********************************************************************
 *
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc564xL.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc564xL.h
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc567x.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/fsl-mpc567x.h
@@ -60,6 +60,8 @@
 #ifndef _MPC5674F_H_
 #define _MPC5674F_H_

+#ifndef ASM
+
 #include <stdint.h>

 #include <mpc55xx/regs-edma.h>
@@ -6592,6 +6594,7 @@ extern "C" {
 #ifdef  __cplusplus
 }
 #endif
+#endif /* ASM */
 #endif                          /* ifdef _MPC5674_H */
 /*********************************************************************
 *
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/irq.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/irq.h
@@ -7,15 +7,17 @@
 */

 /*
- * Copyright (c) 2008, 2010
- * Embedded Brains GmbH
- * Obere Lagerstr. 30
- * D-82178 Puchheim
- * Germany
- * rtems@embedded-brains.de
+ * Copyright (c) 2008-2011 embedded brains GmbH.  All rights reserved.
 *
- * 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.
+ *  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_IRQ_H
@@ -109,6 +111,105 @@ extern "C" {
  /* FlexRay */
  #define MPC55XX_IRQ_FLEXRAY_BASE(mod) \
    ((mod) == 0 ? 284U : MPC55XX_IRQ_INVALID)
+#elif MPC55XX_CHIP_TYPE / 10 == 564
+  #define MPC55XX_IRQ_MAX 255U
+
+  /* eDMA */
+  #define MPC55XX_IRQ_EDMA_ERROR(group) \
+    ((group) == 0 ? 10U : MPC55XX_IRQ_INVALID)
+  #define MPC55XX_IRQ_EDMA(ch) \
+    ((unsigned) (ch) < 16U ? 11U + (ch) : MPC55XX_IRQ_INVALID)
+
+  /* SWT */
+  #define MPC55XX_IRQ_SWT_0 28U
+  #define MPC55XX_IRQ_SWT_1 29U
+
+  /* STM */
+  #define MPC55XX_IRQ_STM_CHANNEL(ch) ((ch) + 30U)
+
+  /* ECSM */
+  #define MPC55XX_IRQ_ECSM_FAS 9U
+  #define MPC55XX_IRQ_ECSM_NCE 35U
+  #define MPC55XX_IRQ_ECSM_COR 36U
+
+  /* MC */
+  #define MPC55XX_IRQ_MC_ME_SAFE_MODE 51U
+  #define MPC55XX_IRQ_MC_ME_MODE_TRANSITION 52U
+  #define MPC55XX_IRQ_MC_ME_INVALID_MODE 53U
+  #define MPC55XX_IRQ_MC_ME_INVALID_CONFIG 54U
+  #define MPC55XX_IRQ_MC_RGM_FRAE 56U
+
+  /* XOSC */
+  #define MPC55XX_IRQ_XOSC 57U
+
+  /* PIT */
+  #define MPC55XX_IRQ_PIT_CHANNEL(ch) \
+    ((ch) == 3 ? 127U : ((ch) + 59U))
+
+  /* SIU external interrupts */
+  #define MPC55XX_IRQ_SIU_EXTERNAL_0 41U
+  #define MPC55XX_IRQ_SIU_EXTERNAL_1 42U
+  #define MPC55XX_IRQ_SIU_EXTERNAL_2 43U
+  #define MPC55XX_IRQ_SIU_EXTERNAL_3 44U
+
+  /* ADC */
+  #define MPC55XX_IRQ_ADC_BASE(mod) \
+    ((mod) == 0 ? 62U : \
+      ((mod) == 1 ? 82U : MPC55XX_IRQ_INVALID))
+
+  /* DSPI */
+  #define MPC55XX_IRQ_DSPI_BASE(mod) \
+    ((mod) == 0 ? 74U : \
+      ((mod) == 1 ? 94U : \
+        ((mod) == 2 ? 114U : MPC55XX_IRQ_INVALID)))
+
+  /* FlexCAN */
+  #define MPC55XX_IRQ_CAN_BASE(mod) \
+    ((mod) == 0 ? 65U : \
+      ((mod) == 1 ? 85U : MPC55XX_IRQ_INVALID))
+
+  /* FlexPWM */
+  #define MPC55XX_IRQ_FLEXPWM_BASE(mod) \
+    ((mod) == 0 ? 179U : \
+      ((mod) == 1 ? 233U : MPC55XX_IRQ_INVALID))
+
+  /* FlexRay */
+  #define MPC55XX_IRQ_FLEXRAY_BASE(mod) \
+    ((mod) == 0 ? 131U : MPC55XX_IRQ_INVALID)
+
+  /* LINFlexD */
+  #define MPC55XX_IRQ_LINFLEX_BASE(mod) \
+    ((mod) == 0 ? 79U : \
+      ((mod) == 1 ? 99U : MPC55XX_IRQ_INVALID))
+
+  /* eTimer */
+  #define MPC55XX_IRQ_ETIMER_BASE(mod) \
+    ((mod) == 0 ? 157U : \
+      ((mod) == 1 ? 168U : \
+        ((mod) == 2 ? 222U : MPC55XX_IRQ_INVALID)))
+
+  /* CTU */
+  #define MPC55XX_IRQ_CTU_MRS 193U
+  #define MPC55XX_IRQ_CTU_T(idx) ((idx) + 194U)
+  #define MPC55XX_IRQ_CTU_FIFO(idx) ((idx) + 202U)
+  #define MPC55XX_IRQ_CTU_ADC 206U
+  #define MPC55XX_IRQ_CTU_ERR 207U
+
+  /* SEMA */
+  #define MPC55XX_IRQ_SEMA_0 247U
+  #define MPC55XX_IRQ_SEMA_1 248U
+
+  /* FCCU */
+  #define MPC55XX_IRQ_FCCU_ALRM 250U
+  #define MPC55XX_IRQ_FCCU_CFG_TO 251U
+  #define MPC55XX_IRQ_FCCU_SC_RCC0_F 252U
+  #define MPC55XX_IRQ_FCCU_SC_RCC1_F 253U
+
+  /* PMU */
+  #define MPC55XX_IRQ_PMU 254U
+
+  /* SWG */
+  #define MPC55XX_IRQ_SWG 255U
 #else
  #if MPC55XX_CHIP_TYPE / 10 == 555
    #define MPC55XX_IRQ_MAX 307U
@@ -188,6 +289,22 @@ extern "C" {

 #define MPC55XX_IRQ_NUMBER (MPC55XX_IRQ_MAX + 1U)

+/* ADC */
+#define MPC55XX_IRQ_ADC_EOC(mod) \
+  (MPC55XX_IRQ_ADC_BASE(mod) + 0U)
+#define MPC55XX_IRQ_ADC_ER(mod) \
+  (MPC55XX_IRQ_ADC_BASE(mod) + 1U)
+#define MPC55XX_IRQ_ADC_WD(mod) \
+  (MPC55XX_IRQ_ADC_BASE(mod) + 2U)
+
+/* eTimer */
+#define MPC55XX_IRQ_ETIMER_TC(mod, ch) \
+  (MPC55XX_IRQ_ETIMER_BASE(mod) + (ch))
+#define MPC55XX_IRQ_ETIMER_WTIF(mod) \
+  (MPC55XX_IRQ_ETIMER_BASE(mod) + 8U)
+#define MPC55XX_IRQ_ETIMER_RCF(mod) \
+  (MPC55XX_IRQ_ETIMER_BASE(mod) + 10U)
+
 /* eTPU */
 #define MPC55XX_IRQ_ETPU(mod) \
  (MPC55XX_IRQ_ETPU_BASE(mod) + 0U)
@@ -219,36 +336,71 @@ extern "C" {
 #define MPC55XX_IRQ_ESCI(mod) (MPC55XX_IRQ_ESCI_BASE(mod) + 0U)

 /* FlexCAN */
-#define MPC55XX_IRQ_CAN_BOFF_TWRN_RWRN(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 0U)
-#define MPC55XX_IRQ_CAN_ERR(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 1U)
-#define MPC55XX_IRQ_CAN_BUF_0(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 3U)
-#define MPC55XX_IRQ_CAN_BUF_1(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 4U)
-#define MPC55XX_IRQ_CAN_BUF_2(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 5U)
-#define MPC55XX_IRQ_CAN_BUF_3(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 6U)
-#define MPC55XX_IRQ_CAN_BUF_4(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 7U)
-#define MPC55XX_IRQ_CAN_BUF_5(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 8U)
-#define MPC55XX_IRQ_CAN_BUF_6(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 9U)
-#define MPC55XX_IRQ_CAN_BUF_7(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 10U)
-#define MPC55XX_IRQ_CAN_BUF_8(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 12U)
-#define MPC55XX_IRQ_CAN_BUF_9(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 12U)
-#define MPC55XX_IRQ_CAN_BUF_10(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 13U)
-#define MPC55XX_IRQ_CAN_BUF_11(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 14U)
-#define MPC55XX_IRQ_CAN_BUF_12(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 15U)
-#define MPC55XX_IRQ_CAN_BUF_13(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 16U)
-#define MPC55XX_IRQ_CAN_BUF_14(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 17U)
-#define MPC55XX_IRQ_CAN_BUF_15(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 18U)
-#define MPC55XX_IRQ_CAN_BUF_16_31(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 19U)
-#define MPC55XX_IRQ_CAN_BUF_32_63(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 20U)
+#if MPC55XX_CHIP_TYPE / 10 == 564
+  #define MPC55XX_IRQ_CAN_ERR(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 0U)
+  #define MPC55XX_IRQ_CAN_BOFF_TWRN_RWRN(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 1U)
+  #define MPC55XX_IRQ_CAN_BUF_0_3(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 3U)
+  #define MPC55XX_IRQ_CAN_BUF_4_7(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 4U)
+  #define MPC55XX_IRQ_CAN_BUF_8_11(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 5U)
+  #define MPC55XX_IRQ_CAN_BUF_12_15(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 6U)
+  #define MPC55XX_IRQ_CAN_BUF_16_31(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 7U)
+#else
+  #define MPC55XX_IRQ_CAN_BOFF_TWRN_RWRN(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 0U)
+  #define MPC55XX_IRQ_CAN_ERR(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 1U)
+  #define MPC55XX_IRQ_CAN_BUF_0(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 3U)
+  #define MPC55XX_IRQ_CAN_BUF_1(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 4U)
+  #define MPC55XX_IRQ_CAN_BUF_2(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 5U)
+  #define MPC55XX_IRQ_CAN_BUF_3(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 6U)
+  #define MPC55XX_IRQ_CAN_BUF_4(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 7U)
+  #define MPC55XX_IRQ_CAN_BUF_5(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 8U)
+  #define MPC55XX_IRQ_CAN_BUF_6(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 9U)
+  #define MPC55XX_IRQ_CAN_BUF_7(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 10U)
+  #define MPC55XX_IRQ_CAN_BUF_8(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 12U)
+  #define MPC55XX_IRQ_CAN_BUF_9(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 12U)
+  #define MPC55XX_IRQ_CAN_BUF_10(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 13U)
+  #define MPC55XX_IRQ_CAN_BUF_11(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 14U)
+  #define MPC55XX_IRQ_CAN_BUF_12(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 15U)
+  #define MPC55XX_IRQ_CAN_BUF_13(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 16U)
+  #define MPC55XX_IRQ_CAN_BUF_14(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 17U)
+  #define MPC55XX_IRQ_CAN_BUF_15(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 18U)
+  #define MPC55XX_IRQ_CAN_BUF_16_31(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 19U)
+  #define MPC55XX_IRQ_CAN_BUF_32_63(mod) (MPC55XX_IRQ_CAN_BASE(mod) + 20U)
+#endif
+
+/* FlexPWM */
+#define MPC55XX_IRQ_FLEXPWM_RF(mod, ch) (MPC55XX_IRQ_FLEXPWM_BASE(mod) + 3U * (ch) + 0U)
+#define MPC55XX_IRQ_FLEXPWM_COF(mod, ch) (MPC55XX_IRQ_FLEXPWM_BASE(mod) + 3U * (ch) + 1U)
+#define MPC55XX_IRQ_FLEXPWM_CAF(mod, ch) (MPC55XX_IRQ_FLEXPWM_BASE(mod) + 3U * (ch) + 2U)
+#define MPC55XX_IRQ_FLEXPWM_FFLAG(mod) (MPC55XX_IRQ_FLEXPWM_BASE(mod) + 12U)
+#define MPC55XX_IRQ_FLEXPWM_REF(mod) (MPC55XX_IRQ_FLEXPWM_BASE(mod) + 13U)

 /* FlexRay */
-#define MPC55XX_IRQ_FLEXRAY_MIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 0U)
-#define MPC55XX_IRQ_FLEXRAY_PRIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 1U)
-#define MPC55XX_IRQ_FLEXRAY_CHIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 2U)
-#define MPC55XX_IRQ_FLEXRAY_WUP_IF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 3U)
-#define MPC55XX_IRQ_FLEXRAY_FBNE_F(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 4U)
-#define MPC55XX_IRQ_FLEXRAY_FANE_F(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 5U)
-#define MPC55XX_IRQ_FLEXRAY_RBIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 6U)
-#define MPC55XX_IRQ_FLEXRAY_TBIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 7U)
+#if MPC55XX_CHIP_TYPE / 10 == 564
+  #define MPC55XX_IRQ_FLEXRAY_LRNEIF_DRNEIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 0U)
+  #define MPC55XX_IRQ_FLEXRAY_LRCEIF_DRCEIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 1U)
+  #define MPC55XX_IRQ_FLEXRAY_FAFAIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 2U)
+  #define MPC55XX_IRQ_FLEXRAY_FAFVIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 3U)
+  #define MPC55XX_IRQ_FLEXRAY_WUPIEF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 4U)
+  #define MPC55XX_IRQ_FLEXRAY_PRIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 5U)
+  #define MPC55XX_IRQ_FLEXRAY_CHIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 6U)
+  #define MPC55XX_IRQ_FLEXRAY_TBIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 7U)
+  #define MPC55XX_IRQ_FLEXRAY_RBIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 8U)
+  #define MPC55XX_IRQ_FLEXRAY_MIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 9U)
+#else
+  #define MPC55XX_IRQ_FLEXRAY_MIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 0U)
+  #define MPC55XX_IRQ_FLEXRAY_PRIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 1U)
+  #define MPC55XX_IRQ_FLEXRAY_CHIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 2U)
+  #define MPC55XX_IRQ_FLEXRAY_WUP_IF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 3U)
+  #define MPC55XX_IRQ_FLEXRAY_FBNE_F(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 4U)
+  #define MPC55XX_IRQ_FLEXRAY_FANE_F(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 5U)
+  #define MPC55XX_IRQ_FLEXRAY_RBIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 6U)
+  #define MPC55XX_IRQ_FLEXRAY_TBIF(mod) (MPC55XX_IRQ_FLEXRAY_BASE(mod) + 7U)
+#endif
+
+/* LINFlexD */
+#define MPC55XX_IRQ_LINFLEX_RXI(mod) (MPC55XX_IRQ_LINFLEX_BASE(mod) + 0U)
+#define MPC55XX_IRQ_LINFLEX_TXI(mod) (MPC55XX_IRQ_LINFLEX_BASE(mod) + 1U)
+#define MPC55XX_IRQ_LINFLEX_ERR(mod) (MPC55XX_IRQ_LINFLEX_BASE(mod) + 2U)

 /* Checks */
 #define MPC55XX_IRQ_IS_VALID(v) \
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/mpc55xx.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/mpc55xx.h
@@ -47,12 +47,6 @@
 #include <stddef.h>
 #include <stdint.h>

-/* Defined in fmpll.S */
-int mpc55xx_get_system_clock(void);
-
-/* Defined in fmpll.S */
-void mpc55xx_system_reset(void);
-
 int mpc55xx_flash_copy(void *dest, const void *src, size_t nbytes);
 int mpc55xx_flash_copy_op(void *rdest, const void *src, size_t nbytes,
  uint32_t opmask, uint32_t *p_fail_addr);
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/reg-defs.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/reg-defs.h
@@ -26,132 +26,12 @@
 #define LIBCPU_POWERPC_MPC55XX_REG_DEFS_H

 #include <bspopts.h>
-/*
- * Register addresses
- */
-#if ((MPC55XX_CHIP_TYPE >= 5510) && (MPC55XX_CHIP_TYPE <= 5517))

-#define FMPLL_SYNSR   0xFFFF0004
-#define FMPLL_ESYNCR1 0xFFFF0008
-#define FMPLL_ESYNCR2 0xFFFF000C
-#define FLASH_BIUCR   0xFFFF801C
-#define SIU_ECCR      0xFFFE8984
-#define SIU_SYSCLK    0xFFFE89A0
-#define SIU_SRCR      0xFFFE8010
-
-/*
- * Definitions for SIU_SYSCLK 
- */
-#define SIU_SYSCLK_SYSCLKSEL_MASK 0xC0000000
-#define SIU_SYSCLK_SYSCLKSEL_IRC  0x00000000
-#define SIU_SYSCLK_SYSCLKSEL_XOSC 0x40000000
-#define SIU_SYSCLK_SYSCLKSEL_PLL  0x80000000
-
-#else /* ((MPC55XX_CHIP_TYPE >= 5510) && (MPC55XX_CHIP_TYPE <= 5517))*/
-
-#define FMPLL_SYNCR 0xC3F80000
-#define FMPLL_SYNSR 0xC3F80004
-#define FMPLL_ESYNCR1 0XC3F80008
-#define FMPLL_ESYNCR2 0XC3F8000C
-#define FLASH_BIUCR 0xC3F8801C
-#define SIU_ECCR 0xC3F90984
-#define SIU_SRCR 0xC3F90010
-#define SIU_SYSDIV 0xC3F909A0
-
-#endif /*((MPC55XX_CHIP_TYPE >= 5510) && (MPC55XX_CHIP_TYPE <= 5517))*/
-/*
- * Special purpose registers
- */
-
-#define BUCSR 1013
-
-/*
- * Branch Unit Control and Status Register (BUCSR)
- */
-
-#define BUCSR_BBFI 0x00000200
-#define BUCSR_BPEN 0x00000001
-
-/*
- * Definitions for FMPLL_SYNCR (FMPLL Synthesizer Control Register)
- */
-
-/* Fields used for PREDIV (Pre-Divider bits [1:3]) */
-#define FMPLL_SYNCR_PREDIV_0 0x00000000
-
-/* Fields used for MFD (Muliplication Factor Divider bits [4:8]) */
-#define FMPLL_SYNCR_MFD_0 0x00000000
-#define FMPLL_SYNCR_MFD_2 0x01000000
-#define FMPLL_SYNCR_MFD_4 0x02000000
-#define FMPLL_SYNCR_MFD_6 0x03000000
-#define FMPLL_SYNCR_MFD_8 0x04000000
-#define FMPLL_SYNCR_MFD_10 0x05000000
-#define FMPLL_SYNCR_MFD_12 0x06000000
-
-/* Fields used for RFD (Reduced Frequency Divider bits [10:12]) */
-#define FMPLL_SYNCR_RFD_0 0x00000000
-#define FMPLL_SYNCR_RFD_1 0x00080000
-#define FMPLL_SYNCR_RFD_2 0x00100000
-#define FMPLL_SYNCR_RFD_3 0x00180000
-#define FMPLL_SYNCR_RFD_4 0x00200000
-#define FMPLL_SYNCR_RFD_5 0x00280000
-#define FMPLL_SYNCR_RFD_6 0x00300000
-#define FMPLL_SYNCR_RFD_7 0x00380000
-
-/* Fields for LOCEN (Loss-of-clock enable bit [13]) */
-#define FMPLL_SYNCR_LOCEN 0x00040000
-
-/* Fields for LOLRE (Loss-of-lock reset enable bit [14]) */
-#define FMPLL_SYNCR_LOLRE 0x00020000
-
-/* Fields for LOCRE (Loss-of-clock reset enable bit [15]) */
-#define FMPLL_SYNCR_LOCRE 0x00010000
-
-/* Fields for DISCLK (Disable CLKOUT bit [16]) */
-#define FMPLL_SYNCR_DISCLK 0x00008000
-
-/* Fields for LOLIRQ (Loss-of-lock interrupt request bit [17]) */
-#define FMPLL_SYNCR_LOLIRQ 0x00004000
-
-/* Fields for LOCIRQ (Loss-of-clock interrupt request bit [18]) */
-#define FMPLL_SYNCR_LOCIRQ 0x00002000
-
-/* Fields for RATE (Modulation rate bit [19]) */
-#define FMPLL_SYNCR_RATE_FREF 0x00001000
-
-/* Fields for DEPTH (Modulation depth percentage bits [20:21]) */
-#define FMPLL_SYNCR_DEPTH_0 0x00000000
-#define FMPLL_SYNCR_DEPTH_1 0x00000400
-#define FMPLL_SYNCR_DEPTH_2 0x00000800
-
-/* Fields for EXP (Expected difference bits [22:31]) */
-#define FMPLL_SYNCR_EXP_0 0x00000000
-
-/*
- * Definitions for the FMPLL_SYNSR (Synthesizer Status Register)
- */
-
-/* Fields for LOLF (Loss-of-lock flag bit [22]) */
-#define FMPLL_SYNSR_LOLF 0x00000200
-
-/* Fields for LOCK (Lock status bit [28]) */
-#define FMPLL_SYNSR_LOCK 0x00000008
-
-/* Fields for LOCF (Loss-of-clock flag bit [29]) */
-#define FMPLL_SYNSR_LOCF 0x00000004
-
-/*
- * Definitions for the SIU_SRCR (System Reset Control Register)
- */
-
-/* Fields for SSR (software system reset bit [0]) */
-#define SIU_SRCR_SSR 0x80000000
-
-/* Fields for SER (external system reset bit [1]) */
-#define SIU_SRCR_SER 0x40000000
-
-/* Fields for CRE (checkstop reset enable bit [16]) */
-#define SIU_SRCR_CRE 0x00008000
+#if MPC55XX_CHIP_TYPE / 10 == 551
+  #define FLASH_BIUCR   0xFFFF801C
+#else
+  #define FLASH_BIUCR 0xC3F8801C
+#endif

 /*
 * Definitions for FLASH_BIUCR (Flash BIU Control Register)
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/regs-mmu.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/regs-mmu.h
@@ -165,6 +165,30 @@ extern "C" {
    } \
  }

+#define MPC55XX_MMU_1K 0
+#define MPC55XX_MMU_2K 1
+#define MPC55XX_MMU_4K 2
+#define MPC55XX_MMU_8K 3
+#define MPC55XX_MMU_16K 4
+#define MPC55XX_MMU_32K 5
+#define MPC55XX_MMU_64K 6
+#define MPC55XX_MMU_128K 7
+#define MPC55XX_MMU_256K 8
+#define MPC55XX_MMU_512K 9
+#define MPC55XX_MMU_1M 10
+#define MPC55XX_MMU_2M 11
+#define MPC55XX_MMU_4M 12
+#define MPC55XX_MMU_8M 13
+#define MPC55XX_MMU_16M 14
+#define MPC55XX_MMU_32M 15
+#define MPC55XX_MMU_64M 16
+#define MPC55XX_MMU_128M 17
+#define MPC55XX_MMU_256M 18
+#define MPC55XX_MMU_512M 19
+#define MPC55XX_MMU_1G 20
+#define MPC55XX_MMU_2G 21
+#define MPC55XX_MMU_4G 22
+
 #ifdef  __cplusplus
 }
 #endif /* __cplusplus */
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/regs.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/regs.h
@@ -30,12 +30,40 @@

 #if MPC55XX_CHIP_TYPE / 10 == 551
  #include <mpc55xx/fsl-mpc551x.h>
+  #define MPC55XX_HAS_EBI
+  #define MPC55XX_HAS_ESCI
+  #define MPC55XX_HAS_EMIOS
+  #define MPC55XX_HAS_FMPLL_ENHANCED
 #elif MPC55XX_CHIP_TYPE / 10 == 555
  #include <mpc55xx/fsl-mpc555x.h>
+  #define MPC55XX_HAS_EBI
+  #define MPC55XX_HAS_ESCI
+  #define MPC55XX_HAS_EMIOS
+  #define MPC55XX_HAS_FMPLL
+  #define MPC55XX_HAS_UNIFIED_CACHE
 #elif MPC55XX_CHIP_TYPE / 10 == 556
  #include <mpc55xx/fsl-mpc556x.h>
+  #define MPC55XX_HAS_EBI
+  #define MPC55XX_HAS_ESCI
+  #define MPC55XX_HAS_EMIOS
+  #define MPC55XX_HAS_FMPLL
+  #define MPC55XX_HAS_UNIFIED_CACHE
+#elif MPC55XX_CHIP_TYPE / 10 == 564
+  #include <mpc55xx/fsl-mpc564xL.h>
+  #define MPC55XX_HAS_STM
+  #define MPC55XX_HAS_SWT
+  #define MPC55XX_HAS_MODE_CONTROL
+  #define MPC55XX_HAS_INSTRUCTION_CACHE
+  #define MPC55XX_HAS_LINFLEX
+  #define MPC55XX_HAS_SECOND_INTERNAL_RAM_AREA
 #elif MPC55XX_CHIP_TYPE / 10 == 567
  #include <mpc55xx/fsl-mpc567x.h>
+  #define MPC55XX_HAS_EBI
+  #define MPC55XX_HAS_ESCI
+  #define MPC55XX_HAS_EMIOS
+  #define MPC55XX_HAS_FMPLL_ENHANCED
+  #define MPC55XX_HAS_INSTRUCTION_CACHE
+  #define MPC55XX_HAS_DATA_CACHE
 #else
  #error "unsupported chip type"
 #endif
--- a/c/src/lib/libcpu/powerpc/mpc55xx/misc/fmpll.S
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/misc/fmpll.S
@@ -1,193 +0,0 @@
-/**
- * @file
- *
- * @ingroup mpc55xx_asm
- *
- * @brief FMPLL setup.
- */
-
-/*
- * 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 <libcpu/powerpc-utility.h>
-#include <mpc55xx/reg-defs.h>
-#include <bspopts.h>
-
-#define FMPLL_IS_MPC551X (5510 <= MPC55XX_CHIP_TYPE && MPC55XX_CHIP_TYPE <= 5517)
-
-#define FMPLL_IS_MPC5674 (MPC55XX_CHIP_TYPE == 5674)
-
-#define FMPLL_HAS_ENHANCED_FMPLL (FMPLL_IS_MPC551X || FMPLL_IS_MPC5674)
-	
-	.section ".bsp_start_text", "ax"
-
-/* Timeout for delay in clocks */
-.equ FMPLL_TIMEOUT, 6000
-
-.macro DO_SETTING setting
-	lwz r5, \setting
-	stw r5, 0(r4)
-	msync
-	bl fmpll_wait_for_lock
-.endm
-
-/**
- * @fn void mpc55xx_fmpll_init()
- * @brief Configure FMPLL after reset.
- */
-GLOBAL_FUNCTION mpc55xx_fmpll_init
-	/* Save link register */
-	mflr r9
-
-#if FMPLL_HAS_ENHANCED_FMPLL
-	/*
-	 * for MPC5510: pass in ptr to array with:
-	 * off 0:  temp setting for ESYNCR2
-	 * off 4: final setting for ESYNCR2
-	 * off 8: final setting for ESYNCR1
-	 */
-	LA r4, FMPLL_ESYNCR2
-
-	lwz r5, 0(r3)
-	stw r5, 0(r4)
-	msync
-
-	lwz r5, 8(r3)
-	stw r5, (FMPLL_ESYNCR1-FMPLL_ESYNCR2)(r4)	
-	msync
-	bl fmpll_wait_for_lock
-
-	DO_SETTING 4(r3)
-
-#if FMPLL_IS_MPC551X
-	/*
-	 * switch to PLL clock in SIU
-	 */
-	LA r4, SIU_SYSCLK
-	lwz r5, 0(r4)
-	LWI r6, ~SIU_SYSCLK_SYSCLKSEL_MASK
-	and r5, r5, r6
-	LWI r6, SIU_SYSCLK_SYSCLKSEL_PLL
-	or  r5, r5, r6
-	stw r5, 0(r4)
-#endif /* FMPLL_IS_MPC551X */
-#else /* !FMPLL_HAS_ENHANCED_FMPLL */
-	/*
-	 * for MPC5566: pass in ptr to array with:
-	 * off 0:  temp setting for SYNCR
-	 * off 4: final setting for SYNCR
-	 */
-
-	LA r4, FMPLL_SYNCR
-
-	DO_SETTING 0(r3)
-	DO_SETTING 4(r3)
-
-	/* Enable loss-of-clock and loss-of-lock IRQs */
-	lwz r5, 0(r4)
-	LWI r6, FMPLL_SYNCR_LOCIRQ | FMPLL_SYNCR_LOLIRQ
-	or r5, r5, r6
-
-	/* Disable loss-of-clock and loss-of-lock resets */
-	LWI r6, ~FMPLL_SYNCR_LOCRE & ~FMPLL_SYNCR_LOLRE
-	and r5, r5, r6
-	stw r5, 0(r4)
-#endif /* !FMPLL_HAS_ENHANCED_FMPLL */
-
-	/* Restore link register and return */
-	mtlr r9
-	blr
-
-fmpll_wait_for_lock:
-	LWI r6, FMPLL_TIMEOUT
-	mtctr r6
-
-	LWI r7, FMPLL_SYNSR_LOCK
-
-	LA r6, FMPLL_SYNSR
-
-fmpll_not_locked:
-	bdnz fmpll_continue
-
-	b mpc55xx_system_reset
-fmpll_continue:
-	lwz r5, 0(r6)
-	and. r5, r5, r7
-	beq fmpll_not_locked
-
-	blr
-
-	.section ".text", "aw"
-
-/**
- * @fn int mpc55xx_get_system_clock()
- * @brief Returns the system clock.
- */
-GLOBAL_FUNCTION mpc55xx_get_system_clock
-#if FMPLL_HAS_ENHANCED_FMPLL
-	LA r4, FMPLL_ESYNCR1
-	lwz r3, 0(r4)
-	/* EPREDIV */
-	rlwinm r5, r3,16, 28, 31
-
-	/* MFD */
-	rlwinm r6, r3,0, 24, 31
-
-	LA r4, FMPLL_ESYNCR2
-	lwz r3, 0(r4)
-	/* ERFD */
-	rlwinm r7, r3,0, 26, 31
-
-	LWI r8, MPC55XX_FMPLL_REF_CLOCK
-	addi r5, r5, 1
-	addi r6, r6,16
-	addi r7, r7, 1
-	divw r3, r8, r5  /* REF_CLOCK/PREDIV */
-	mullw r3, r6, r3 /* REF_CLOCK/PREDIV*MFD */
-	divw r3, r3, r7  /* REF_CLOCK/PREDIV*MFD/RFD */
-#else /* !FMPLL_HAS_ENHANCED_FMPLL */
-	LA r4, FMPLL_SYNCR
-	lwz r3, 0(r4)
-
-	/* PREDIV */
-	rlwinm r5, r3, 4, 29, 31
-
-	/* MFD */
-	rlwinm r6, r3, 9, 27, 31
-
-	/* RFD */
-	rlwinm r7, r3, 13, 29, 31
-	/* Calculate system clock (Table 11-10 [MPC5567 Microcontroller Reference Manual]) */
-	LWI r8, MPC55XX_FMPLL_REF_CLOCK
-	addi r5, r5, 1
-	addi r6, r6, 4
-	mullw r6, r6, r8
-	sraw r6, r6, r7
-	divw r3, r6, r5
-#endif /* !FMPLL_HAS_ENHANCED_FMPLL */
-
-	blr
-
-/**
- * @fn void mpc55xx_system_reset()
- * @brief Software system reset.
- */
-GLOBAL_FUNCTION mpc55xx_system_reset
-	LA r8, SIU_SRCR
-	LWI r9, SIU_SRCR_SSR
-	stw r9, 0(r8)
-twiddle:
-	b twiddle
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_categories.c
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_categories.c
@@ -274,6 +274,7 @@ const ppc_exc_categories *ppc_exc_categories_for_cpu(ppc_cpu_id_t cpu)
      return &e500_category_table;
    case PPC_e200z0:
    case PPC_e200z1:
+    case PPC_e200z4:
    case PPC_e200z6:
    case PPC_e200z7:
      return &e200_category_table;
--- a/c/src/lib/libcpu/powerpc/preinstall.am
+++ b/c/src/lib/libcpu/powerpc/preinstall.am
@@ -270,10 +270,6 @@ $(PROJECT_INCLUDE)/mpc55xx/mpc55xx.h: mpc55xx/include/mpc55xx.h $(PROJECT_INCLUD
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/mpc55xx/mpc55xx.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/mpc55xx/mpc55xx.h

-$(PROJECT_INCLUDE)/mpc55xx/esci.h: mpc55xx/include/esci.h $(PROJECT_INCLUDE)/mpc55xx/$(dirstamp)
-	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/mpc55xx/esci.h
-PREINSTALL_FILES += $(PROJECT_INCLUDE)/mpc55xx/esci.h
-
 $(PROJECT_INCLUDE)/mpc55xx/siu.h: mpc55xx/include/siu.h $(PROJECT_INCLUDE)/mpc55xx/$(dirstamp)
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/mpc55xx/siu.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/mpc55xx/siu.h
@@ -298,6 +294,10 @@ $(PROJECT_INCLUDE)/mpc55xx/fsl-mpc556x.h: mpc55xx/include/fsl-mpc556x.h $(PROJEC
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/mpc55xx/fsl-mpc556x.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/mpc55xx/fsl-mpc556x.h

+$(PROJECT_INCLUDE)/mpc55xx/fsl-mpc564xL.h: mpc55xx/include/fsl-mpc564xL.h $(PROJECT_INCLUDE)/mpc55xx/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/mpc55xx/fsl-mpc564xL.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/mpc55xx/fsl-mpc564xL.h
+
 $(PROJECT_INCLUDE)/mpc55xx/fsl-mpc567x.h: mpc55xx/include/fsl-mpc567x.h $(PROJECT_INCLUDE)/mpc55xx/$(dirstamp)
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/mpc55xx/fsl-mpc567x.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/mpc55xx/fsl-mpc567x.h
--- a/c/src/lib/libcpu/powerpc/shared/include/cpuIdent.c
+++ b/c/src/lib/libcpu/powerpc/shared/include/cpuIdent.c
@@ -53,6 +53,7 @@ const char *get_ppc_cpu_type_name(ppc_cpu_id_t cpu)
    case PPC_PSIM:		return "PSIM";
    case PPC_e200z0:		return "e200z0";
    case PPC_e200z1:		return "e200z1";
+    case PPC_e200z4:		return "e200z4";
    case PPC_e200z6:		return "e200z6";
    case PPC_e200z7:		return "e200z7";
    case PPC_e500v2:		return "e500v2";
@@ -72,6 +73,7 @@ ppc_cpu_id_t get_ppc_cpu_type(void)
  const uint32_t ppc_cpu_id_version_nibble[] = {
    PPC_e200z0,
    PPC_e200z1,
+    PPC_e200z4,
    PPC_e200z6,
    PPC_e200z7
  };
@@ -122,6 +124,7 @@ ppc_cpu_id_t get_ppc_cpu_type(void)
    case PPC_8540:
    case PPC_e200z0:
    case PPC_e200z1:
+    case PPC_e200z4:
    case PPC_e200z6:
    case PPC_e200z7:
    case PPC_e300c1:
@@ -182,6 +185,7 @@ ppc_cpu_id_t get_ppc_cpu_type(void)
  	case PPC_8540:
  	case PPC_e200z0:
  	case PPC_e200z1:
+  	case PPC_e200z4:
  	case PPC_e200z6:
  	case PPC_e200z7:
  	case PPC_e500v2:
--- a/c/src/lib/libcpu/powerpc/shared/include/cpuIdent.h
+++ b/c/src/lib/libcpu/powerpc/shared/include/cpuIdent.h
@@ -58,6 +58,7 @@ typedef enum
  PPC_e300c3  = 0x8085, /* e300c3  core */
  PPC_e200z0  = 0x8170,
  PPC_e200z1  = 0x8140,
+  PPC_e200z4  = 0x8155,
  PPC_e200z6  = 0x8110,
  PPC_e200z7  = 0x8160,
  PPC_PSIM    = 0xfffe,  /* GDB PowerPC simulator -- fake version */