2011-08-30 Peter Dufault <dufault@hda.com>

	* mpc55xx/misc/flash_support.c: New file.
	* Makefile.am: Reflect change above.
	* mpc55xx/include/mpc55xx.h: Add definitions for the FLASH interface
	and two memory protect interfaces.  Add modifications to eliminate
	warnings in some of the cache macros.
	* mpc55xx/include/regs.h: Add some structure tag names for some
	structures that I needed access to.  Don't define the ALTCADR for the
	MPC5554 - it is reserved and acess casues an exception.  Hide the C99
	designated initializers when compiling with C++.  Add some support for
	the EQADC.
	* mpc55xx/include/esci.h, mpc55xx/include/watchdog.h: Add C++
	protection.

7 files changed, 820 insertions, 9 deletions

diff --git a/c/src/lib/libcpu/powerpc/ChangeLog b/c/src/lib/libcpu/powerpc/ChangeLog
index a0c91c752a..749a31ab04 100644
--- a/c/src/lib/libcpu/powerpc/ChangeLog
+++ b/c/src/lib/libcpu/powerpc/ChangeLog
@@ -1,3 +1,18 @@
+2011-08-30	Peter Dufault <dufault@hda.com>
+
+	* mpc55xx/misc/flash_support.c: New file.
+	* Makefile.am: Reflect change above.
+	* mpc55xx/include/mpc55xx.h: Add definitions for the FLASH interface
+	and two memory protect interfaces.  Add modifications to eliminate
+	warnings in some of the cache macros.
+	* mpc55xx/include/regs.h: Add some structure tag names for some
+	structures that I needed access to.  Don't define the ALTCADR for the
+	MPC5554 - it is reserved and acess casues an exception.  Hide the C99
+	designated initializers when compiling with C++.  Add some support for
+	the EQADC.
+	* mpc55xx/include/esci.h, mpc55xx/include/watchdog.h: Add C++
+	protection.
+
 2011-08-24	Sebastian Huber <sebastian.huber@embedded-brains.de>
 
 	* mpc6xx/clock/c_clock.c, mpc6xx/mmu/mmuAsm.S,
diff --git a/c/src/lib/libcpu/powerpc/Makefile.am b/c/src/lib/libcpu/powerpc/Makefile.am
index 777ccc5ea6..93881cf4af 100644
--- a/c/src/lib/libcpu/powerpc/Makefile.am
+++ b/c/src/lib/libcpu/powerpc/Makefile.am
@@ -468,7 +468,8 @@ mpc55xx_dspi_rel_LDFLAGS = $(RTEMS_RELLDFLAGS)
 noinst_PROGRAMS += mpc55xx/misc.rel
 mpc55xx_misc_rel_SOURCES = mpc55xx/misc/copy.S \
 	mpc55xx/misc/fmpll.S \
-	mpc55xx/misc/flash.S
+	mpc55xx/misc/flash.S \
+	mpc55xx/misc/flash_support.c
 mpc55xx_misc_rel_LDFLAGS = $(RTEMS_RELLDFLAGS)
 
 endif
diff --git a/c/src/lib/libcpu/powerpc/mpc55xx/include/esci.h b/c/src/lib/libcpu/powerpc/mpc55xx/include/esci.h
index 3066f23a3a..04ca06e128 100644
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/esci.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/esci.h
@@ -29,6 +29,10 @@
 
 #include <rtems/termiostypes.h>
 
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
 #define MPC55XX_ESCI_NUMBER 2
 
 struct ESCI_tag;
@@ -45,4 +49,8 @@ typedef struct {
 
 extern mpc55xx_esci_driver_entry mpc55xx_esci_driver_table [ /* MPC55XX_ESCI_NUMBER */ ];
 
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
 #endif /* LIBCPU_POWERPC_MPC55XX_ESCI_H */
diff --git a/c/src/lib/libcpu/powerpc/mpc55xx/include/mpc55xx.h b/c/src/lib/libcpu/powerpc/mpc55xx/include/mpc55xx.h
index 4cd6abab8b..284ccb9af5 100644
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/mpc55xx.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/mpc55xx.h
@@ -61,6 +61,44 @@ void mpc55xx_system_reset();
 /* Defined in flash.S */
 void mpc55xx_flash_config();
 
+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);
+int mpc55xx_flash_size(uint32_t *p_size);
+int mpc55xx_flash_writable(void);
+uint32_t mpc55xx_flash_address(void);
+void mpc55xx_flash_set_read_only(void);
+void mpc55xx_flash_set_read_write(void);
+
+int mpc55xx_physical_address(const void *addr, uint32_t *p_result);
+int mpc55xx_mapped_address(const void *addr, uint32_t *p_result);
+
+/* Bits for opmask. */
+#define MPC55XX_FLASH_BLANK_CHECK 0x01
+#define MPC55XX_FLASH_UNLOCK      0x02
+#define MPC55XX_FLASH_ERASE       0x04
+#define MPC55XX_FLASH_PROGRAM     0x08
+#define MPC55XX_FLASH_VERIFY      0x10
+
+/* Error returns.  CONFIG or SIZE might mean you just
+ * need to check for new configuration bits.
+ * SIZE and RANGE mean you are outside of a known flash region.
+ * ERASE means the erase failed,
+ * PROGRAM means the program failed,
+ * BLANK means it wasn't blank and BLANK_CHECK was specified,
+ * VERIFY means VERIFY was set and it didn't match the source,
+ * and LOCK means either the locking failed or you needed to
+ * specify MPC55XX_FLASH_UNLOCK and didn't.
+ */
+#define MPC55XX_FLASH_CONFIG_ERR    (-1)
+#define MPC55XX_FLASH_SIZE_ERR      (-2)
+#define MPC55XX_FLASH_RANGE_ERR     (-3)
+#define MPC55XX_FLASH_ERASE_ERR     (-4)
+#define MPC55XX_FLASH_PROGRAM_ERR   (-5)
+#define MPC55XX_FLASH_NOT_BLANK_ERR (-6)
+#define MPC55XX_FLASH_VERIFY_ERR    (-7)
+#define MPC55XX_FLASH_LOCK_ERR      (-8)
+
 #define MPC55XX_CACHE_ALIGNED_MASK ((uintptr_t) 0x1f)
 
 #define MPC55XX_CACHE_LINE_SIZE 32
@@ -73,9 +111,9 @@ static inline int mpc55xx_is_cache_aligned( const void *s, size_t n)
 	return !(((uintptr_t) s & MPC55XX_CACHE_ALIGNED_MASK) || (n & MPC55XX_CACHE_ALIGNED_MASK));
 }
 
-static inline void* mpc55xx_cache_aligned_start( const void *s)
+static inline uintptr_t mpc55xx_cache_aligned_start( const void *s)
 {
-	return ((uintptr_t) s & MPC55XX_CACHE_ALIGNED_MASK) ? (((uintptr_t) s & ~MPC55XX_CACHE_ALIGNED_MASK) + MPC55XX_CACHE_LINE_SIZE) : s;
+	return ((uintptr_t) s & MPC55XX_CACHE_ALIGNED_MASK) ? (((uintptr_t) s & ~MPC55XX_CACHE_ALIGNED_MASK) + MPC55XX_CACHE_LINE_SIZE) : (uintptr_t)s;
 }
 
 static inline size_t mpc55xx_non_cache_aligned_size( const void *s)
diff --git a/c/src/lib/libcpu/powerpc/mpc55xx/include/regs.h b/c/src/lib/libcpu/powerpc/mpc55xx/include/regs.h
index c59847ee83..f7e36decf7 100644
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/regs.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/regs.h
@@ -562,7 +562,7 @@ extern "C" {
             } B;
         } MCR;
 
-        union {                 /* LML Register */
+        union LMLR_tag {                 /* LML Register */
             uint32_t R;
             struct {
                 uint32_t LME:1;
@@ -573,7 +573,7 @@ extern "C" {
             } B;
         } LMLR;
 
-        union {                 /* HL Register */
+        union HLR_tag {                 /* HL Register */
             uint32_t R;
             struct {
                 uint32_t HBE:1;
@@ -582,7 +582,7 @@ extern "C" {
             } B;
         } HLR;
 
-        union {                 /* SLML Register */
+        union SLMLR_tag {                 /* SLML Register */
             uint32_t R;
             struct {
                 uint32_t SLE:1;
@@ -1130,9 +1130,15 @@ extern "C" {
                 } B;
             } CSR;              /* Channel Status Register */
 
+#if MPC55XX_CHIP_TYPE == 5554
+          /* ALTCADR is reserved on the MPC5554 and writes will cause an exception.
+           */
+            uint32_t altcadr_reserved;
+#else
             union {
                 uint32_t R;    /* Alternate Channel A Data Register */
             } ALTCADR;
+#endif
 
             uint32_t emios_channel_reserved[2];
 
@@ -2838,6 +2844,7 @@ extern "C" {
         } TCD[64];              /* transfer_control_descriptor */
     };
 
+#ifndef __cplusplus
     static const struct tcd_t EDMA_TCD_DEFAULT = {
         .SADDR = 0,
         .SDF = { .R = 0 },
@@ -2848,6 +2855,7 @@ extern "C" {
         .DLAST_SGA = 0,
         .BMF = { .R = 0 }
     };
+#endif
 
 #define EDMA_TCD_BITER_MASK 0x7fff
 
@@ -2982,7 +2990,7 @@ extern "C" {
 
         uint32_t eqadc_reserved4;
 
-        union {
+        union EQADC_CFCR_tag {
             uint16_t R;
             struct {
                 uint16_t:5;
@@ -2996,7 +3004,7 @@ extern "C" {
 
         uint32_t eqadc_reserved5;
 
-        union {
+        union EQADC_IDCR_tag {
             uint16_t R;
             struct {
                 uint16_t NCIE:1;
@@ -3017,7 +3025,7 @@ extern "C" {
 
         uint32_t eqadc_reserved6;
 
-        union {
+        union EQADC_FISR_tag {
             uint32_t R;
             struct {
                 uint32_t NCF:1;
@@ -4381,6 +4389,7 @@ extern "C" {
         } MAS6;
     };
 
+#ifndef __cplusplus
     static const struct MMU_tag MMU_DEFAULT = {
         .MAS0 = { .R = 0x10000000 },
         .MAS1 = { .R = 0 },
@@ -4389,6 +4398,41 @@ extern "C" {
         .MAS4 = { .R = 0 },
         .MAS6 = { .R = 0 }
     };
+#endif
+
+/* Message Formats for On-Chip ADC Operation
+ */
+union EQADC_CONVERSION_COMMAND_tag {
+    uint32_t R;
+    struct {
+        uint32_t EOQ:1;
+        uint32_t PAUSE:1;
+            uint32_t:3;
+        uint32_t EB:1;
+        uint32_t BN:1;
+        uint32_t CAL:1;
+        uint32_t MESSAGE_TAG:4;
+        uint32_t LST:2;
+        uint32_t TSR:1;
+        uint32_t FMT:1;
+        uint32_t CHANNEL_NUMBER:8;
+            uint32_t:8;
+    } B;
+}; /* Conversion command */
+
+union EQADC_WRITE_CONFIGURATION_COMMAND_tag {
+    uint32_t R;
+    struct {
+        uint32_t EOQ:1;
+        uint32_t PAUSE:1;
+            uint32_t:3;
+        uint32_t EB:1;
+        uint32_t BN:1;
+        uint32_t RW:1;
+        uint32_t VALUE:16;
+        uint32_t ADDR:8;
+    } B;
+}; /* Write configuration command */
 
 #if ((MPC55XX_CHIP_TYPE >= 5510) && (MPC55XX_CHIP_TYPE <= 5517))
 /* Define memories */
diff --git a/c/src/lib/libcpu/powerpc/mpc55xx/include/watchdog.h b/c/src/lib/libcpu/powerpc/mpc55xx/include/watchdog.h
index 2c00621076..f0506afbc9 100644
--- a/c/src/lib/libcpu/powerpc/mpc55xx/include/watchdog.h
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/include/watchdog.h
@@ -27,6 +27,10 @@
 
 #include <libcpu/powerpc-utility.h>
 
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
 static inline void mpc55xx_watchdog_clear()
 {
 	PPC_SET_SPECIAL_PURPOSE_REGISTER( BOOKE_TSR, BOOKE_TSR_WIS);
@@ -56,4 +60,8 @@ static inline rtems_status_code mpc55xx_watchdog_set_time_base_bit( uint32_t bit
 	return RTEMS_SUCCESSFUL;
 }
 
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
 #endif /* LIBCPU_POWERPC_MPC55XX_WATCHDOG_H */
diff --git a/c/src/lib/libcpu/powerpc/mpc55xx/misc/flash_support.c b/c/src/lib/libcpu/powerpc/mpc55xx/misc/flash_support.c
new file mode 100644
index 0000000000..4df9ab8de0
--- /dev/null
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/misc/flash_support.c
@@ -0,0 +1,697 @@
+/**
+ * @file
+ *
+ * @ingroup mpc55xx
+ *
+ *  @brief MPC55XX flash memory support.
+ *
+ *  I set my MMU up to map what will finally be in flash into RAM and at the
+ *  same time I map the flash to a different location.  When the software
+ *  is tested I can use this to copy the RAM version of the program into
+ *  the flash and when I reboot I'm running out of flash.
+ *
+ *  I use a flag word located after the boot configuration half-word to
+ *  indicate that the MMU should be left alone, and I don't include the RCHW
+ *  or that flag in my call to this routine.
+ *
+ *  There are obviously other uses for this.
+ **/
+
+/*
+ * Copyright (c) 2009-2011
+ * HD Associates, Inc.
+ * 18 Main Street
+ * Pepperell, MA 01463
+ * USA
+ * dufault@hda.com
+ *
+ * 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 <errno.h>
+#include <sys/types.h>
+#include <mpc55xx/regs.h>
+#include <mpc55xx/mpc55xx.h>
+
+#include <libcpu/powerpc-utility.h>
+#include <rtems/powerpc/registers.h>
+
+/* Set up the memory ranges for the flash on
+ * the MPC5553, MPC5554, MPC5566 and MPC5567.
+ * I check if it is an unknown CPU and return an error.
+ *
+ * These CPUS have a low, mid, and high space of memory.
+ *
+ * Only the low space really needs a table like this, but for simplicity
+ * I do low, mid, and high the same way.
+ */
+struct range {          /* A memory range. */
+    uint32_t lower;
+    uint32_t upper;
+};
+
+/* The ranges of the memory banks for the low space.  All the
+ * chips I'm looking at share this low format, identified by LAS=6:
+ * 2 16K banks,
+ * 2 48K banks,
+ * 2 64K banks.
+ */
+static const struct range lsel_ranges[] = {
+    {                        0, (1*16              )*1024 - 1},
+    {(1*16              )*1024, (2*16              )*1024 - 1},
+    {(2*16              )*1024, (2*16 + 1*48       )*1024 - 1},
+    {(2*16 + 1*48       )*1024, (2*16 + 2*48       )*1024 - 1},
+    {(2*16 + 2*48       )*1024, (2*16 + 2*48 + 1*64)*1024 - 1},
+    {(2*16 + 2*48 + 1*64)*1024, (2*16 + 2*48 + 2*64)*1024 - 1},
+};
+
+/* The ranges of the memory blocks for the mid banks, 2 128K banks.
+ * Again, all the chips share this, identified by MAS=0.
+ */
+#define MBSTART ((2*16+2*48+2*64)*1024)
+static const struct range msel_ranges[] = {
+    {MBSTART             , MBSTART + 1*128*1024 - 1},
+    {MBSTART + 1*128*1024, MBSTART + 2*128*1024 - 1},
+};
+
+/* The ranges of the memory blocks for the high banks.
+ * There are N 128K banks, where N <= 20,
+ * and is identified by looking at the SIZE field.
+ *
+ * This could benefit from being redone to save a few bytes
+ * and provide for bigger flash spaces.
+ */
+#define HBSTART (MBSTART+2*128*1024)
+static const struct range hbsel_ranges[] = {
+    {HBSTART              , HBSTART +  1*128*1024 - 1},
+    {HBSTART +  1*128*1024, HBSTART +  2*128*1024 - 1},
+    {HBSTART +  2*128*1024, HBSTART +  3*128*1024 - 1},
+    {HBSTART +  3*128*1024, HBSTART +  4*128*1024 - 1},
+    {HBSTART +  4*128*1024, HBSTART +  5*128*1024 - 1},
+    {HBSTART +  5*128*1024, HBSTART +  6*128*1024 - 1},
+    {HBSTART +  6*128*1024, HBSTART +  7*128*1024 - 1},
+    {HBSTART +  7*128*1024, HBSTART +  8*128*1024 - 1},
+    {HBSTART +  8*128*1024, HBSTART +  9*128*1024 - 1},
+    {HBSTART +  9*128*1024, HBSTART + 10*128*1024 - 1},
+    {HBSTART + 10*128*1024, HBSTART + 11*128*1024 - 1},
+    {HBSTART + 11*128*1024, HBSTART + 12*128*1024 - 1},
+    {HBSTART + 12*128*1024, HBSTART + 13*128*1024 - 1},
+    {HBSTART + 13*128*1024, HBSTART + 14*128*1024 - 1},
+    {HBSTART + 14*128*1024, HBSTART + 15*128*1024 - 1},
+    {HBSTART + 15*128*1024, HBSTART + 16*128*1024 - 1},
+    {HBSTART + 16*128*1024, HBSTART + 17*128*1024 - 1},
+    {HBSTART + 17*128*1024, HBSTART + 18*128*1024 - 1},
+    {HBSTART + 18*128*1024, HBSTART + 19*128*1024 - 1},
+    {HBSTART + 19*128*1024, HBSTART + 20*128*1024 - 1},
+};
+
+/* Set bits in a bitmask to indicate which banks are
+ * within the range "first" and "last".
+ */
+static void
+range_set(
+  uint32_t first,
+  uint32_t last,
+  int *p_bits,
+  const struct range *pr,
+  int n_range
+)
+{
+    int i;
+    int bits = 0;
+    for (i = 0; i < n_range; i++) {
+        /* If the upper limit is less than "first" or the lower limit
+         * is greater than "last" then the block is not in range.
+         */
+        if ( !(pr[i].upper < first || pr[i].lower > last)) {
+            bits |= (1 << i);   /* This block is in the range, set the bit. */
+        }
+
+    }
+    *p_bits = bits;
+}
+
+#define N(ARG) (sizeof(ARG)/sizeof(ARG[0]))
+
+/** Return the size of the on-chip flash
+ *  verifying that this is a device that we know about.
+ * @return 0 for OK, non-zero for error:
+ *  - MPC55XX_FLASH_VERIFY_ERR for LAS not 6 or MAS not 0.
+ *    @note This is overriding what verify means!
+ *  - MPC55XX_FLASH_SIZE_ERR Not a chip I've checked against the manual,
+ *                           athat is, SIZE not 5, 7, or 11.
+ */
+int
+mpc55xx_flash_size(
+  uint32_t *p_size  /**< The size is returned here. */
+)
+{
+    /* On the MPC5553, MPC5554, MPC5566, and MP5567 the
+     *  low address space LAS field is 0x6 and all have
+     *  six blocks sized 2*16k, 2*48k, and 2*64k.
+     *
+     * All the mid and high address spaces have 128K blocks.
+     *
+     * The mid address space MAS size field is 0 for the above machines,
+     * and they all have 2 128K blocks.
+     *
+     * For the high address space we look at the
+     * size field to figure out the size.  The SIZE field is:
+     *
+     * 5 for 1.5MB (MPC5553)
+     * 7 for 2MB (MPC5554, MPC5567)
+     * 11 for 3MB  (MPC5566)
+     */
+    int hblocks;    /* The number of blocks in the high address space. */
+
+    /* Verify the configuration matches one of the chips that I've checked out.
+     */
+    if (FLASH.MCR.B.LAS != 6 || FLASH.MCR.B.MAS != 0) {
+        return MPC55XX_FLASH_VERIFY_ERR;
+    }
+
+    switch(FLASH.MCR.B.SIZE) {
+        case 5:
+        hblocks = 8;
+        break;
+
+        case 7:
+        hblocks = 12;
+        break;
+
+        case 11:
+        hblocks = 20;
+        break;
+
+        default:
+        return MPC55XX_FLASH_SIZE_ERR;
+    }
+
+    /* The first two banks are 256K.
+     * The high block has "hblocks" 128K blocks.
+     */
+    *p_size = 256*1024 + 256*1024 + hblocks * 128*1024;
+    return 0;
+}
+
+/* Unlock the flash blocks if "p_locked" points to something that is 0.
+ * If it is a NULL pointer then we aren't allowed to do the unlock.
+ */
+static int
+unlock_once(int lsel, int msel, int hbsel, int *p_locked)
+{
+    union LMLR_tag lmlr;
+    union SLMLR_tag slmlr;
+    union HLR_tag hlr;
+    rtems_interrupt_level level;
+
+    /* If we're already locked return.
+     */
+    if (p_locked && (*p_locked == 1)) {
+        return 0;
+    }
+
+    /* Do we have to lock something in the low or mid block?
+     */
+    rtems_interrupt_disable(level);
+    lmlr = FLASH.LMLR;
+    if ((lsel || msel) && (lmlr.B.LME == 0)) {
+        union LMLR_tag lmlr_unlock;
+        lmlr_unlock.B.LLOCK=~lsel;
+        lmlr_unlock.B.MLOCK=~msel;
+        lmlr_unlock.B.SLOCK=1;
+
+        if (lmlr.B.LLOCK != lmlr_unlock.B.LLOCK ||
+        lmlr.B.MLOCK != lmlr_unlock.B.MLOCK) {
+            if (p_locked == 0) {
+                rtems_interrupt_enable(level);
+                return MPC55XX_FLASH_LOCK_ERR;
+            } else {
+                *p_locked = 1;
+            }
+            FLASH.LMLR.R = 0xA1A11111;  /* Unlock. */
+            FLASH.LMLR = lmlr_unlock;
+        }
+    }
+    rtems_interrupt_enable(level);
+
+    rtems_interrupt_disable(level);
+    slmlr = FLASH.SLMLR;
+    if ((lsel || msel) && (slmlr.B.SLE == 0)) {
+        union SLMLR_tag slmlr_unlock;
+        slmlr_unlock.B.SLLOCK=~lsel;
+        slmlr_unlock.B.SMLOCK=~msel;
+        slmlr_unlock.B.SSLOCK=1;
+
+        if (slmlr.B.SLLOCK != slmlr_unlock.B.SLLOCK ||
+        slmlr.B.SMLOCK != slmlr_unlock.B.SMLOCK) {
+            if (p_locked == 0) {
+                rtems_interrupt_enable(level);
+                return MPC55XX_FLASH_LOCK_ERR;
+            } else {
+                *p_locked = 1;
+            }
+            FLASH.SLMLR.R = 0xC3C33333;  /* Unlock. */
+            FLASH.SLMLR = slmlr_unlock;
+        }
+    }
+    rtems_interrupt_enable(level);
+
+    /* Do we have to unlock something in the high block?
+     */
+    rtems_interrupt_disable(level);
+    hlr = FLASH.HLR;
+    if (hbsel && (hlr.B.HBE == 0)) {
+        union HLR_tag hlr_unlock;
+        hlr_unlock.B.HBLOCK = ~hbsel;
+
+        if (hlr.B.HBLOCK != hlr_unlock.B.HBLOCK) {
+            if (p_locked == 0) {
+                return MPC55XX_FLASH_LOCK_ERR;
+                rtems_interrupt_enable(level);
+            } else {
+                *p_locked = 1;
+            }
+            FLASH.HLR.R = 0xB2B22222;   /* Unlock. */
+            FLASH.HLR = hlr_unlock;
+        }
+    }
+    rtems_interrupt_enable(level);
+
+    return 0;
+}
+
+static inline uint32_t
+tsize(int i)
+{
+  return 1 << (10 + 2 * i);
+}
+
+static int
+addr_map(
+  int to_phys,         /* If 1 lookup physical else lookup mapped. */
+  const void *addr,    /* The address to look up. */
+  uint32_t *p_result   /* Result is here. */
+)
+{
+    uint32_t u_addr = (uint32_t)addr;
+    uint32_t mas0, mas1, mas2, mas3;
+    uint32_t start, end;
+    rtems_interrupt_level level;
+    int i;
+
+    for (i = 0; i < 32; i++) {
+      mas0 = 0x10000000 | (i << 16);
+      rtems_interrupt_disable(level);
+      PPC_SET_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS0, mas0);
+      asm volatile("tlbre");
+      mas1 = PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS1);
+      mas2 = PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS2);
+      mas3 = PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS3);
+      rtems_interrupt_enable(level);
+
+      if (mas1 & 0x80000000) {
+        /* Valid. */
+        start = (to_phys ? mas2 : mas3) & 0xFFFFF000;
+        end = start + tsize((mas1 >> 8) & 0x0000000F);
+        /* Are we within range?
+         */
+        if (start <= u_addr && end >= u_addr) {
+          uint32_t offset = (to_phys ? mas3 : mas2) & 0xFFFFF000;
+          *p_result = u_addr - offset;
+          return 0;
+        }
+      }
+    }
+
+    /* Not found in a TLB.
+     */
+    return ESRCH;
+}
+
+/** Return the physical address corresponding to a mapped address.
+  @return 0 if OK, ESRCH if not found in TLB1.
+ **/
+int
+mpc55xx_physical_address(
+  const void *addr,     /**< Mapped address. */
+  uint32_t *p_result    /**< Result returned here. */
+)
+{
+    return addr_map(1, addr, p_result);
+}
+
+/** Return the mapped address corresponding to a mapped address.
+  @return 0 if OK, ESRCH if not found in TLB1.
+ **/
+int
+mpc55xx_mapped_address(
+  const void *addr,     /**< Mapped address. */
+  uint32_t *p_result    /**< Result returned here. */
+)
+{
+    return addr_map(0, addr, p_result);
+}
+
+/**
+ * Copy memory from an address into the flash when flash is relocated
+ * If programming fails the address that it failed at can be returned.
+ @note At end of operation the flash may be left writable.
+ *     Use mpc55xx_flash_read_only() to set read-only.
+ @return Zero for OK, non-zero for error:
+ * - ESRCH                       Can't lookup where something lives.
+ * - EPERM                       Attempt to write to non-writable flash.
+ * - ETXTBSY                     Attempt to flash overlapping regions.
+ * - MPC55XX_FLASH_CONFIG_ERR    for LAS not 6 or MAS not 0.
+ * - MPC55XX_FLASH_SIZE_ERR      for SIZE not 5, 7, or 11.
+ * - MPC55XX_FLASH_RANGE_ERR     for illegal access:
+ *                               - first or first+last outside of flash;
+ *                               - first not on a mod(8) boundary;
+ *                               - nbytes not multiple of 8.
+ * - MPC55XX_FLASH_ERASE_ERR     Erase requested but failed.
+ * - MPC55XX_FLASH_PROGRAM_ERR   Program requested but failed.
+ * - MPC55XX_FLASH_NOT_BLANK_ERR Blank check requested but not blank.
+ * - MPC55XX_FLASH_VERIFY_ERR    Verify requested but failed.
+ * - MPC55XX_FLASH_LOCK_ERR      Unlock requested but failed.
+ **/
+
+int
+mpc55xx_flash_copy_op(
+  void *dest,       /**< An address in the flash to copy to. */
+  const void *src,  /**< An address in memory to copy from. */
+  size_t nbytes,    /**< The number of bytes to copy. */
+  uint32_t opmask,  /**< Bitmask of operations to perform.
+                     * - MPC55XX_FLASH_UNLOCK:      Unlock the blocks.
+                     * - MPC55XX_FLASH_ERASE:       Erase the blocks.
+                     * - MPC55XX_FLASH_BLANK_CHECK: Verify the blocks are blank.
+                     * - MPC55XX_FLASH_PROGRAM:     Program the FLASH.
+                     * - MPC55XX_FLASH_VERIFY:      Verify the regions match.
+                     **/
+  uint32_t *p_fail  /**< If not NULL then the address where the operation
+                     *   failed is returned here.
+                     **/
+)
+{
+    uint32_t udest, usrc, flash_size;
+    int r;
+    int peg;            /* Program or Erase Good - Did it work? */
+
+    int lsel;           /* Low block select bits. */
+    int msel;           /* Mid block select bits. */
+    int hbsel;          /* High block select bits. */
+
+    int s_lsel;           /* Source Low block select bits. */
+    int s_msel;           /* Source Mid block select bits. */
+    int s_hbsel;          /* Source High block select bits. */
+
+    int unlocked = 0;
+    int *p_unlocked;
+    int i;
+    int nwords;         /* The number of 32 bit words to write. */
+    volatile uint32_t *flash;    /* Where the flash is mapped in. */
+    volatile uint32_t *memory;   /* What to copy into flash. */
+    uint32_t offset;    /* Where the FLASH is mapped into memory. */
+    rtems_interrupt_level level;
+
+    if ( (r = mpc55xx_flash_size(&flash_size))) {
+        return r;
+    }
+
+    /* Get where the flash is mapped in.
+     */
+    offset = mpc55xx_flash_address();
+
+    udest = ((uint32_t)dest) - offset;
+    if ( (r = mpc55xx_physical_address(src, &usrc)) ) {
+      return r;
+    }
+
+    /* Verify that the address being programmed is in flash and that it is
+     * a multiple of 64 bits.
+     * Someone else can remove the 64-bit restriction.
+     */
+    if (udest > flash_size ||
+    udest + nbytes > flash_size ||
+    (udest & 0x7) != 0 ||
+    (nbytes & 0x7) != 0) {
+        return MPC55XX_FLASH_RANGE_ERR;
+    }
+
+    if (opmask == 0) {
+        return 0;
+    }
+
+    /* If we're going to do a write-style operation the flash must be writable.
+     */
+    if ((opmask &
+        (MPC55XX_FLASH_UNLOCK | MPC55XX_FLASH_ERASE | MPC55XX_FLASH_PROGRAM)) &&
+        !mpc55xx_flash_writable()
+    ) {
+      return EPERM;
+    }
+
+    /* If we aren't allowed to unlock then set the pointer to zero.
+     * That is how "unlock_once" decides we can't unlock.
+     */
+    p_unlocked = (opmask & MPC55XX_FLASH_UNLOCK) ? &unlocked : 0;
+
+    /* Set up the bit masks for the blocks to program or erase.
+     */
+    range_set(udest, udest + nbytes, &lsel,   lsel_ranges, N( lsel_ranges));
+    range_set(udest, udest + nbytes, &msel,   msel_ranges, N( msel_ranges));
+    range_set(udest, udest + nbytes, &hbsel, hbsel_ranges, N(hbsel_ranges));
+
+    range_set(usrc, usrc + nbytes, &s_lsel,   lsel_ranges, N( lsel_ranges));
+    range_set(usrc, usrc + nbytes, &s_msel,   msel_ranges, N( msel_ranges));
+    range_set(usrc, usrc + nbytes, &s_hbsel, hbsel_ranges, N(hbsel_ranges));
+
+    /* Are we attempting overlapping flash?
+     */
+    if ((lsel & s_lsel) | (msel & s_msel) | (hbsel & s_hbsel)) {
+      return ETXTBSY;
+    }
+
+    nwords = nbytes / 4;
+    flash = (volatile uint32_t *)dest;
+    memory = (volatile uint32_t *)src;
+
+  /* In the following sections any "Step N" notes refer to
+   * the steps in "13.4.2.3 Flash Programming" in the reference manual.
+   * XXX Do parts of this neeed to be protected by interrupt locks?
+   */
+
+    if (opmask & MPC55XX_FLASH_ERASE) {   /* Erase. */
+        if ( (r = unlock_once(lsel, msel, hbsel, p_unlocked)) ) {
+            return r;
+        }
+
+        rtems_interrupt_disable(level);
+        FLASH.MCR.B.ERS = 1;        /* Step 1: Select erase. */
+
+        FLASH.LMSR.B.LSEL = lsel;   /* Step 2: Select blocks to be erased. */
+        FLASH.LMSR.B.MSEL = msel;
+        FLASH.HSR.B.HBSEL = hbsel;
+
+        flash[0] = 1;               /* Step 3: Write to any address in the flash
+                                     * (the "erase interlock write)".
+                                     */
+        FLASH.MCR.B.EHV = 1;         /* Step 4: Enable high V to start erase. */
+        rtems_interrupt_enable(level);
+        while (FLASH.MCR.B.DONE == 0) { /* Step 5: Wait until done. */
+        }
+        rtems_interrupt_disable(level);
+        peg = FLASH.MCR.B.PEG;       /* Save result. */
+        FLASH.MCR.B.EHV = 0;         /* Disable high voltage. */
+        FLASH.MCR.B.ERS = 0;         /* De-select erase. */
+        rtems_interrupt_enable(level);
+        if (peg == 0) {
+            return MPC55XX_FLASH_ERASE_ERR; /* Flash erase failed. */
+        }
+    }
+
+    if (opmask & MPC55XX_FLASH_BLANK_CHECK) {    /* Verify blank. */
+        for (i = 0; i < nwords; i++) {
+           if (flash[i] != 0xffffffff) {
+                if (p_fail) {
+                    *p_fail = (uint32_t)(flash + i);
+                }
+                return MPC55XX_FLASH_NOT_BLANK_ERR; /* Not blank. */
+           }
+        }
+    }
+
+  /* Program.
+   */
+    if (opmask & MPC55XX_FLASH_PROGRAM) {
+        int chunk = 0;  /* Used to collect programming into 256 bit chunks. */
+
+        if ( (r = unlock_once(lsel, msel, hbsel, p_unlocked)) ) {
+            return r;
+        }
+        FLASH.MCR.B.PGM = 1;                /* Step 1 */
+
+       rtems_interrupt_disable(level);
+
+        for (i = 0; i < nwords; i += 2) {
+           flash[i] = memory[i];            /* Step 2 */
+           flash[i + 1] = memory[i + 1];    /* Always program in min 64 bits. */
+
+          /* Step 3 is "write additional words" */
+
+           /* Try to program in chunks of 256 bits.
+            * Collect the 64 bit writes into 256 bit ones:
+            */
+           chunk++;
+           if (chunk == 4) {
+                /* Collected 4 64-bits for a 256 bit chunk. */
+                FLASH.MCR.B.EHV = 1;            /* Step 4: Enable high V. */
+
+                rtems_interrupt_enable(level);
+                while (FLASH.MCR.B.DONE == 0) { /* Step 5: Wait until done. */
+                }
+                rtems_interrupt_disable(level);
+
+                peg = FLASH.MCR.B.PEG;          /* Step 6: Save result. */
+                FLASH.MCR.B.EHV = 0;            /* Step 7: Disable high V. */
+                if (peg == 0) {
+                    FLASH.MCR.B.PGM = 0;
+                    rtems_interrupt_enable(level);
+                    if (p_fail) {
+                        *p_fail = (uint32_t)(flash + i);
+                    }
+                    return MPC55XX_FLASH_PROGRAM_ERR; /* Programming failed. */
+                }
+                chunk = 0;                       /* Reset chunk counter. */
+            }
+                                                 /* Step 8: Back to step 2. */
+        }
+
+       if (!chunk) {
+            FLASH.MCR.B.PGM = 0;
+            rtems_interrupt_enable(level);
+       } else {
+           /* If there is anything left in that last chunk flush it out:
+            */
+            FLASH.MCR.B.EHV = 1;
+
+            rtems_interrupt_enable(level);
+            while (FLASH.MCR.B.DONE == 0) {     /* Wait until done. */
+            }
+            rtems_interrupt_disable(level);
+
+            peg = FLASH.MCR.B.PEG;              /* Save result. */
+            FLASH.MCR.B.EHV = 0;                /* Disable high voltage. */
+            FLASH.MCR.B.PGM = 0;
+            rtems_interrupt_enable(level);
+
+            if (peg == 0) {
+                if (p_fail) {
+                    *p_fail = (uint32_t)(flash + i);
+                }
+                return MPC55XX_FLASH_PROGRAM_ERR; /* Programming failed. */
+            }
+        }
+    }
+
+    if (opmask & MPC55XX_FLASH_VERIFY) {        /* Verify memory matches. */
+        for (i = 0; i < nwords; i++) {
+           if (flash[i] != memory[i]) {
+                if (p_fail) {              /* Return the failed address. */
+                    *p_fail = (uint32_t)(flash + i);
+                }
+                return MPC55XX_FLASH_VERIFY_ERR; /* Verification failed. */
+           }
+        }
+    }
+
+    return 0;
+}
+
+/** Simple flash copy with a signature that matches memcpy.
+ @note At end of operation the flash may be left writable.
+ *     Use mpc55xx_flash_read_only() to set read-only.
+ @return Zero for OK, non-zero for error.
+ *       see flash_copy_op() for possible errors.
+ **/
+int
+mpc55xx_flash_copy(
+  void *dest,       /**< An address in the flash to copy to. */
+  const void *src,  /**< An address in the flash copy from. */
+  size_t nbytes     /**< The number of bytes to copy. */
+)
+{
+    return mpc55xx_flash_copy_op(dest, src, nbytes,
+        (MPC55XX_FLASH_UNLOCK      |
+         MPC55XX_FLASH_ERASE       |
+         MPC55XX_FLASH_BLANK_CHECK |
+         MPC55XX_FLASH_PROGRAM     |
+         MPC55XX_FLASH_VERIFY      ), 0);
+}
+
+/** Make the flash read-write.
+ @note This assumes the flash is mapped by TLB1 entry 1.
+ */
+void
+mpc55xx_flash_set_read_write(void)
+{
+    rtems_interrupt_level level;
+    rtems_interrupt_disable(level);
+    PPC_SET_SPECIAL_PURPOSE_REGISTER( FSL_EIS_MAS0, 0x10010000);
+    asm volatile("tlbre");
+    PPC_SET_SPECIAL_PURPOSE_REGISTER_BITS(FSL_EIS_MAS3, 0x0000000C);
+    asm volatile("tlbwe");
+    rtems_interrupt_enable(level);
+}
+
+/** Make the flash read-only.
+ @note This assumes the flash is mapped by TLB1 entry 1.
+ */
+void
+mpc55xx_flash_set_read_only(void)
+{
+    rtems_interrupt_level level;
+    rtems_interrupt_disable(level);
+    PPC_SET_SPECIAL_PURPOSE_REGISTER( FSL_EIS_MAS0, 0x10010000);
+    asm volatile("tlbre");
+    PPC_CLEAR_SPECIAL_PURPOSE_REGISTER_BITS(FSL_EIS_MAS3, 0x0000000C);
+    asm volatile("tlbwe");
+    rtems_interrupt_enable(level);
+}
+
+/** See if the flash is writable.
+ *  @note This assumes the flash is mapped by TLB1 entry 1.
+ *  @note It needs to be writable by both user and supervisor.
+ */
+int
+mpc55xx_flash_writable(void)
+{
+    uint32_t mas3;
+    rtems_interrupt_level level;
+
+    rtems_interrupt_disable(level);
+    PPC_SET_SPECIAL_PURPOSE_REGISTER( FSL_EIS_MAS0, 0x10010000);
+    asm volatile("tlbre");
+    mas3 = PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS3);
+    rtems_interrupt_enable(level);
+
+    return ((mas3 & 0x0000000C) == 0x0000000C) ? 1 : 0;
+}
+
+/** Return the address where the flash is mapped in.
+ @note This assumes the flash is mapped by TLB1 entry 1.
+ **/
+uint32_t
+mpc55xx_flash_address(void)
+{
+    uint32_t mas2;
+    rtems_interrupt_level level;
+
+    rtems_interrupt_disable(level);
+    PPC_SET_SPECIAL_PURPOSE_REGISTER( FSL_EIS_MAS0, 0x10010000);
+    asm volatile("tlbre");
+    mas2 = PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS2);
+    rtems_interrupt_enable(level);
+
+    return mas2 & 0xFFFFF000;
+}