--- a/packages/io/flash/current/src/flash.c
+++ b/packages/io/flash/current/src/flash.c
@@ -10,6 +10,7 @@
 // This file is part of eCos, the Embedded Configurable Operating System.
 // Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.
 // Copyright (C) 2003 Gary Thomas
+// Copyright (C) 2004 Andrew Lunn
 //
 // eCos is free software; you can redistribute it and/or modify it under
 // the terms of the GNU General Public License as published by the Free
@@ -42,7 +43,7 @@
 //#####DESCRIPTIONBEGIN####
 //
 // Author(s):    gthomas
-// Contributors: gthomas
+// Contributors: gthomas, Andrew Lunn
 // Date:         2000-07-26
 // Purpose:      
 // Description:  
@@ -53,503 +54,803 @@
 
 #include <pkgconf/system.h>
 #include <pkgconf/io_flash.h>
-
+#ifdef CYGPKG_KERNEL
+#include <cyg/kernel/kapi.h>
+#endif
 #include <cyg/hal/hal_arch.h>
 #include <cyg/hal/hal_intr.h>
 #include <cyg/hal/hal_cache.h>
+#include <cyg/hal/hal_tables.h>
+#include <cyg/infra/cyg_ass.h>
 #include <string.h>
 
 #define  _FLASH_PRIVATE_
 #include <cyg/io/flash.h>
 
 // When this flag is set, do not actually jump to the relocated code.
-// This can be used for running the function in place (RAM startup only),
-// allowing calls to diag_printf() and similar.
+// This can be used for running the function in place (RAM startup
+// only), allowing calls to diag_printf() and similar.
 #undef RAM_FLASH_DEV_DEBUG
 #if !defined(CYG_HAL_STARTUP_RAM) && defined(RAM_FLASH_DEV_DEBUG)
 # warning "Can only enable the flash debugging when configured for RAM startup"
 #endif
-
-struct flash_info flash_info;
-
-// These are the functions in the HW specific driver we need to call.
-typedef void code_fun(void*);
+#ifndef MIN
+#define MIN(x,y) ((x)<(y) ? (x) : (y))
+#endif
 
-externC code_fun flash_query;
-externC code_fun flash_erase_block;
-externC code_fun flash_program_buf;
-externC code_fun flash_read_buf;
-externC code_fun flash_lock_block;
-externC code_fun flash_unlock_block;
+// Has the FLASH IO library been initialise?
+static bool init = false;
+// We have a linked list of flash drivers. This is the head of the
+// list
+static struct cyg_flash_dev *flash_head = NULL;
+
+// This array contains entries for all flash devices that are
+// installed in the system.
+__externC struct cyg_flash_dev cyg_flashdevtab[];
+CYG_HAL_TABLE_BEGIN(cyg_flashdevtab, cyg_flashdev);
+
+// end of the flashdev table
+__externC struct cyg_flash_dev cyg_flashdevtab_end;
+CYG_HAL_TABLE_END(cyg_flashdevtab_end, cyg_flashdev);
 
-int
-flash_init(_printf *pf)
+// Sort the linked list into ascending order of flash address. Use a
+// primitive ripple sort, but since we don't expect to have many
+// devices this should be OK.
+static void flash_sort(void) 
 {
-    int err;
+  bool moved;
+  struct cyg_flash_dev *dev, **previous_next;
+  
+  // If there is zero or one device, short cut
+  if (flash_head == NULL || flash_head->next == NULL)
+    return;
+  
+  do {
+    moved=false;
+    for (dev=flash_head, previous_next=&flash_head; 
+         dev->next; 
+         previous_next = &dev->next, dev=dev->next ){
+      if (dev->start > dev->next->start) {
+        *previous_next=dev->next;
+        dev->next = (*previous_next)->next;
+        (*previous_next)->next = dev;
+        moved=true;          
+        break;
+      }
+    }
+  } while (moved);
+}
 
-    if (flash_info.init) return FLASH_ERR_OK;
-    flash_info.pf = pf; // Do this before calling into the driver
-    if ((err = flash_hwr_init()) != FLASH_ERR_OK) {
-        return err;
-    }
-    flash_info.block_mask = ~(flash_info.block_size-1);
-    flash_info.init = 1;
-    return FLASH_ERR_OK;
+// Walk the linked list and see if there are any overlaps in the
+// addresses the devices claim to use using.
+static bool flash_check_overlap(void) 
+{
+  struct cyg_flash_dev *dev;
+  
+  // If there is zero or one device, short cut
+  if (flash_head == NULL || flash_head->next == NULL)
+    return false;
+
+  for (dev=flash_head; dev->next; dev=dev->next){
+    if (dev->end >= dev->next->start)
+      return true;
+  }
+  return false;
 }
 
-// Use this function to make function pointers anonymous - forcing the
-// compiler to use jumps instead of branches when calling driver
-// services.
-static void* __anonymizer(void* p)
+// Initialise all registered device. Any device that fails to
+// initialise we leave dev->init as false. Then sort the devices into
+// ascending order of address and put them into a linked list. Lastly
+// check if we have any overlap of the addresses.
+__externC int 
+cyg_flash_init(cyg_flash_printf *pf) 
 {
-  return p;
+  int err;
+  struct cyg_flash_dev * dev;
+  
+  if (init) return CYG_FLASH_ERR_OK;
+  init = true;
+  
+  for (dev = &cyg_flashdevtab[0]; dev != &cyg_flashdevtab_end; dev++) {
+    dev->pf = pf;
+#ifdef CYGPKG_KERNEL
+    cyg_mutex_init(&dev->mutex);
+#endif
+    
+    err = dev->funs->flash_init(dev);
+    if (err != CYG_FLASH_ERR_OK) {
+      continue;
+    }
+    CYG_ASSERT(dev->funs, "No flash functions");
+    CYG_ASSERT(dev->num_block_infos, "No number of block infoss");
+    CYG_ASSERT(dev->block_info, "No block infos");
+    CYG_ASSERT(!(((cyg_flashaddr_t)dev->block_info >= dev->start) && 
+                 ((cyg_flashaddr_t)dev->block_info < dev->end)),
+               "Block info is in the flash");
+    CYG_ASSERT(dev->funs->flash_erase_block, "No erase function");
+    CYG_ASSERT(dev->funs->flash_program, "No program function");
+    CYG_ASSERT(dev->funs->flash_hwr_map_error, "No hwr map error function");
+
+    dev->init = true;
+    dev->next = flash_head;
+    flash_head = dev;
+  }
+  
+  flash_sort();
+
+  if (flash_check_overlap()) {
+    return CYG_FLASH_ERR_INVALID;
+  }
+  return CYG_FLASH_ERR_OK;
 }
 
-// FIXME: Want to change all drivers to use this function. But it may
-// make sense to wait till device structure pointer arguments get
-// added as well.
-void
-flash_dev_query(void* data)
+// Is the address within one of the flash drivers?
+__externC int
+cyg_flash_verify_addr(const cyg_flashaddr_t address)
 {
-    typedef void code_fun(void*);
-    code_fun *_flash_query;
-    int d_cache, i_cache;
-
-    _flash_query = (code_fun*) __anonymizer(&flash_query);
-
-    HAL_FLASH_CACHES_OFF(d_cache, i_cache);
-    (*_flash_query)(data);
-    HAL_FLASH_CACHES_ON(d_cache, i_cache);
+  struct cyg_flash_dev * dev;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
+  for (dev = flash_head; dev; dev=dev->next) {
+    if ((dev->start <= address) && (dev->end > address))
+      return CYG_FLASH_ERR_OK;
+  }
+  return CYG_FLASH_ERR_INVALID;
 }
 
-int
-flash_verify_addr(void *target)
+// See if a range of FLASH addresses overlaps currently running code
+__externC bool cyg_flash_code_overlaps(const cyg_flashaddr_t start, 
+                                       const cyg_flashaddr_t end)
 {
-    if (!flash_info.init) {
-        return FLASH_ERR_NOT_INIT;
-    }
-    if (((CYG_ADDRESS)target >= (CYG_ADDRESS)flash_info.start) &&
-        ((CYG_ADDRESS)target <= ( ((CYG_ADDRESS)flash_info.end) - 1) )) {
-        return FLASH_ERR_OK;
-    } else {
-        return FLASH_ERR_INVALID;
-    }
+  extern char _stext[], _etext[];
+  
+  return ((((unsigned long)&_stext >= (unsigned long)start) &&
+           ((unsigned long)&_stext < (unsigned long)end)) ||
+          (((unsigned long)&_etext >= (unsigned long)start) &&
+           ((unsigned long)&_etext < (unsigned long)end)));
 }
 
-int
-flash_get_limits(void *target, void **start, void **end)
+// Get the start and end address of the first driver
+__externC int
+cyg_flash_get_limits(cyg_flashaddr_t *start, cyg_flashaddr_t *end)
 {
-    if (!flash_info.init) {
-        return FLASH_ERR_NOT_INIT;
+    if (!init) {
+        return CYG_FLASH_ERR_NOT_INIT;
     }
-    *start = flash_info.start;
-    *end = flash_info.end;
-    return FLASH_ERR_OK;
+    if (flash_head) {
+      *start = flash_head->start;
+      *end = flash_head->end;
+      return CYG_FLASH_ERR_OK;
+    }
+    return CYG_FLASH_ERR_INVALID;
 }
 
-int
-flash_get_block_info(int *block_size, int *blocks)
+// Get the block size and number of blocks for the first driver. For
+// devices with boot blocks, consolidate the boot blocks into the
+// figure.
+__externC int
+cyg_flash_get_block_info(size_t *block_size, cyg_uint32 *blocks)
 {
-    if (!flash_info.init) {
-        return FLASH_ERR_NOT_INIT;
+  size_t biggest_size=0;
+  cyg_uint32 i;
+  
+  
+  if (!init) {
+    return CYG_FLASH_ERR_NOT_INIT;
+  }
+  if (!flash_head) return CYG_FLASH_ERR_INVALID;
+
+  // Find the biggest size of blocks
+  for (i=0; i < flash_head->num_block_infos; i++) {
+    if (flash_head->block_info[i].block_size > biggest_size) {
+      biggest_size = flash_head->block_info[i].block_size;
     }
-    *block_size = flash_info.block_size;
-    *blocks = flash_info.blocks;
-    return FLASH_ERR_OK;
+  }
+  
+  // Calculate the number of biggest size blocks
+  *block_size = biggest_size;
+  *blocks = 0;
+  for (i=0; i < flash_head->num_block_infos; i++) {
+    *blocks += (flash_head->block_info[i].block_size *
+                flash_head->block_info[i].blocks) /
+      biggest_size;
+  }
+  return CYG_FLASH_ERR_OK;
+}
+
+// Return information about the Nth driver
+__externC int
+cyg_flash_get_info(cyg_uint32 Nth, cyg_flash_info_t * info)
+{
+  struct cyg_flash_dev * dev;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
+  for (dev = flash_head; dev && Nth; dev=dev->next, Nth--)
+    ;
+  if (dev && !Nth) {
+    info->start = dev->start;
+    info->end = dev->end;
+    info->num_block_infos = dev->num_block_infos;
+    info->block_info = dev->block_info;
+    return CYG_FLASH_ERR_OK;
+  }
+  return CYG_FLASH_ERR_INVALID;
 }
 
-int
-flash_erase(void *addr, int len, void **err_addr)
+// Return information about the flash at the given address
+__externC int
+cyg_flash_get_info_addr(cyg_flashaddr_t flash_base, cyg_flash_info_t * info)
 {
-    unsigned short *block, *end_addr;
-    int stat = 0;
-    typedef int code_fun(unsigned short *, unsigned int);
-    code_fun *_flash_erase_block;
-    int d_cache, i_cache;
-
-    if (!flash_info.init) {
-        return FLASH_ERR_NOT_INIT;
+  struct cyg_flash_dev *dev;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
+  for (dev = flash_head; dev ; dev=dev->next) {
+    if ((dev->start <= flash_base) && ( dev->end > flash_base)) {
+    info->start = dev->start;
+    info->end = dev->end;
+    info->num_block_infos = dev->num_block_infos;
+    info->block_info = dev->block_info;
+    return CYG_FLASH_ERR_OK;
     }
-
-#ifdef CYGSEM_IO_FLASH_SOFT_WRITE_PROTECT
-    if (plf_flash_query_soft_wp(addr,len))
-        return FLASH_ERR_PROTECT;
-#endif
-
-     _flash_erase_block = (code_fun*) __anonymizer(&flash_erase_block);
-
-    block = (unsigned short *)((CYG_ADDRESS)addr & flash_info.block_mask);
-    end_addr = (unsigned short *)((CYG_ADDRESS)addr+len);
+  }
+  return CYG_FLASH_ERR_INVALID;
+}
 
-    /* Check to see if end_addr overflowed */
-    if( (end_addr < block) && (len > 0) ){
-        end_addr = (unsigned short *) ((CYG_ADDRESS) flash_info.end - 1);
+#ifdef CYGPKG_KERNEL
+//Lock the mutex's for a range of addresses
+__externC int
+cyg_flash_mutex_lock(const cyg_flashaddr_t from, const size_t len) 
+{
+  struct cyg_flash_dev * dev;
+  int err;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
+  for (dev = flash_head; dev ; dev=dev->next) {
+    if ((dev->start <= from) && ( dev->end > from)) {
+      cyg_mutex_lock(&dev->mutex);
+      if (dev->end > from+len) {
+        return CYG_FLASH_ERR_OK;
+      } else {                                     // Check off by one?
+        //The region is bigger than this driver. Recurse
+        err = cyg_flash_mutex_lock(dev->end+1, (len - (dev->end - from)));
+        if (err != CYG_FLASH_ERR_OK) {
+          // Something went wrong, unlock what we just locked
+          cyg_mutex_unlock(&dev->mutex);
+        }
+        return err;
+      }
     }
-
-#ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("... Erase from %p-%p: ", (void*)block, (void*)end_addr);
-#endif
+  }
+  return CYG_FLASH_ERR_INVALID;
+}
 
-    HAL_FLASH_CACHES_OFF(d_cache, i_cache);
-    FLASH_Enable(block, end_addr);
-    while (block < end_addr) {
-        // Supply the blocksize for a gross check for erase success
-        unsigned short *tmp_block;
-#if !defined(CYGSEM_IO_FLASH_READ_INDIRECT)
-        int i;
-        unsigned char *dp;
-        bool erased = true;
-
-        dp = (unsigned char *)block;
-        for (i = 0;  i < flash_info.block_size;  i++) {
-            if (*dp++ != (unsigned char)0xFF) {
-                erased = false;
-                break;
-            }
-        }
-#else
-        bool erased = false;
+//Unlock the mutex's for a range of addresses
+__externC int
+cyg_flash_mutex_unlock(const cyg_flashaddr_t from, const size_t len) 
+{
+  struct cyg_flash_dev * dev;
+  int err;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
+  for (dev = flash_head; dev ; dev=dev->next) {
+    if ((dev->start <= from) && ( dev->end > from)) {
+      cyg_mutex_unlock(&dev->mutex);
+      if (dev->end > from+len) {
+        return CYG_FLASH_ERR_OK;
+      } else {                                     // Check off by one?
+        //The region is bigger than this driver. Recurse
+        err = cyg_flash_mutex_unlock(dev->end+1, (len - (dev->end - from)));
+        return err;
+      }
+    }
+  }
+  return CYG_FLASH_ERR_INVALID;
+}
 #endif
 
-        if (!erased) {
-            stat = (*_flash_erase_block)(block, flash_info.block_size);
-            stat = flash_hwr_map_error(stat);
-        }
-        if (stat) {
-            *err_addr = (void *)block;
-            break;
-        }
+// Return the size of the block which is at the given address
+static size_t 
+flash_block_size(struct cyg_flash_dev *dev, const cyg_flashaddr_t addr)
+{
+  int i;
+  size_t offset;
+  
+  
+  CYG_ASSERT((addr >= dev->start) && (addr <= dev->end), "Not inside device");
+  
+  offset = addr - dev->start;
+  for (i=0; i < dev->num_block_infos; i++) {
+    if (offset < (dev->block_info[i].blocks * dev->block_info[i].block_size))
+      return dev->block_info[i].block_size;
+    offset = offset - 
+      (dev->block_info[i].blocks * dev->block_info[i].block_size);
+  }
+  CYG_FAIL("Programming error");
+  return 0;
+}
+
+// Return the size of the block which is at the given address
+__externC size_t
+cyg_flash_block_size(const cyg_flashaddr_t flash_base) 
+{
+  struct cyg_flash_dev * dev;
+
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+
+  for (dev = flash_head; 
+       dev && !((dev->start <= flash_base) && ( dev->end > flash_base));
+       dev=dev->next)
+    ;
+  if (!dev) return CYG_FLASH_ERR_INVALID;
+  
+  return flash_block_size(dev, flash_base);
+}
+
 
-        // Check to see if block will overflow
-        tmp_block = block + flash_info.block_size / sizeof(*block);
-        if(tmp_block < block){
-            // If block address overflows, set block value to end on this loop
-            block = end_addr;
-        }
-        else{
-            block = tmp_block;
-        }
+__externC int 
+cyg_flash_erase(const cyg_flashaddr_t flash_base, 
+                const size_t len, 
+                cyg_flashaddr_t *err_address)
+{
+  cyg_flashaddr_t block, end_addr, addr;
+  struct cyg_flash_dev * dev;
+  size_t block_size;
+  int stat = CYG_FLASH_ERR_OK;
+  int d_cache, i_cache;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
+#ifdef CYGSEM_IO_FLASH_SOFT_WRITE_PROTECT
+  if (plf_flash_query_soft_wp(addr,len))
+    return CYG_FLASH_ERR_PROTECT;
+#endif
+  
+  for (dev = flash_head; 
+       dev && !((dev->start <= flash_base) && ( dev->end > flash_base));
+       dev=dev->next)
+    ;
+  if (!dev) return CYG_FLASH_ERR_INVALID;
+  
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_lock(&dev->mutex);
+#endif
+  addr = flash_base;
+  end_addr = flash_base + len - 1;
+  if (end_addr > dev->end) {
+    end_addr = dev->end;
+  }
+  
+  block_size = flash_block_size(dev, addr);
+  block = (cyg_flashaddr_t)((size_t)addr & ~(block_size - 1));
+  
 #ifdef CYGSEM_IO_FLASH_CHATTER
-        (*flash_info.pf)(".");
+  dev->pf("... Erase from %p-%p: ", (void*)block, (void*)end_addr);
 #endif
+  
+  HAL_FLASH_CACHES_OFF(d_cache, i_cache);
+  FLASH_Enable(block, end_addr);
+  while (block <= end_addr) {
+    int i;
+    unsigned char *dp;
+    bool erased = true;
+
+    // If there is a read function it probably means the flash
+    // cannot be read directly.
+    if (!dev->funs->flash_read) {
+      dp = (unsigned char *)block;
+      for (i = 0;  i < block_size;  i++) {
+        if (*dp++ != (unsigned char)0xFF) {
+          erased = false;
+          break;
+        }
+      }
+    } else {
+      erased=false;
+    }
+    if (!erased) {
+      stat = dev->funs->flash_erase_block(dev,block);
+      stat = dev->funs->flash_hwr_map_error(dev,stat);
     }
-    FLASH_Disable(block, end_addr);
-    HAL_FLASH_CACHES_ON(d_cache, i_cache);
+    if (CYG_FLASH_ERR_OK != stat && err_address) {
+      *err_address = block;
+      break;
+    }
+    block += flash_block_size(dev, block);
+#ifdef CYGSEM_IO_FLASH_CHATTER
+    dev->pf(".");
+#endif
+  }
+  FLASH_Disable(block, end_addr);
+  HAL_FLASH_CACHES_ON(d_cache, i_cache);
 #ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("\n");
+  dev->pf("\n");
+#endif
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_unlock(&dev->mutex);
 #endif
-    return (stat);
+  if (stat != CYG_FLASH_ERR_OK) {
+    return stat;
+  }
+  
+  if (flash_base + len - 1 > dev->end) {        
+    // The region to erase if bigger than this driver handles. Recurse
+    return cyg_flash_erase(dev->end+1, 
+                           len - (dev->end - flash_base) - 1,
+                           err_address);
+  }
+  return CYG_FLASH_ERR_OK;
 }
 
-int
-flash_program(void *_addr, void *_data, int len, void **err_addr)
+__externC int 
+cyg_flash_program(const cyg_flashaddr_t flash_base, 
+                  void *ram_base, 
+                  const size_t len, 
+                  cyg_flashaddr_t *err_address)
 {
-    int stat = 0;
-    int size;
-    typedef int code_fun(void *, void *, int, unsigned long, int);
-    code_fun *_flash_program_buf;
-    unsigned char *addr = (unsigned char *)_addr;
-    unsigned char *data = (unsigned char *)_data;
-    CYG_ADDRESS tmp;
-    int d_cache, i_cache;
-
-    if (!flash_info.init) {
-        return FLASH_ERR_NOT_INIT;
-    }
-
+  struct cyg_flash_dev * dev;
+  cyg_flashaddr_t addr, end_addr;
+  unsigned char * ram = ram_base;
+  size_t block_size, size, length, offset;
+  int stat = CYG_FLASH_ERR_OK;
+  int d_cache, i_cache;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
 #ifdef CYGSEM_IO_FLASH_SOFT_WRITE_PROTECT
-    if (plf_flash_query_soft_wp(addr,len))
-        return FLASH_ERR_PROTECT;
+  if (plf_flash_query_soft_wp(addr,len))
+    return CYG_FLASH_ERR_PROTECT;
 #endif
-
-    _flash_program_buf = (code_fun*) __anonymizer(&flash_program_buf);
-
+  
+  for (dev = flash_head; 
+       dev && !((dev->start <= flash_base) && ( dev->end > flash_base));
+       dev=dev->next)
+    ;
+  if (!dev) return CYG_FLASH_ERR_INVALID;
+  
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_lock(&dev->mutex);
+#endif
+  addr = flash_base;
+  end_addr = flash_base + len - 1;
+  if (end_addr > dev->end) {
+    end_addr = dev->end;
+  }
+  length = end_addr - addr + 1;
+  
 #ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("... Program from %p-%p at %p: ", (void*)data, 
-                     (void*)(((CYG_ADDRESS)data)+len), (void*)addr);
+  dev->pf("... Program from %p-%p to %p: ", ram_base, 
+          ((CYG_ADDRESS)ram_base)+length, addr);
 #endif
-
-    HAL_FLASH_CACHES_OFF(d_cache, i_cache);
-    FLASH_Enable((unsigned short*)addr, (unsigned short *)(addr+len));
-    while (len > 0) {
-        size = len;
-        if (size > flash_info.block_size) size = flash_info.block_size;
-
-        tmp = (CYG_ADDRESS)addr & ~flash_info.block_mask;
-        if (tmp) {
-                tmp = flash_info.block_size - tmp;
-                if (size>tmp) size = tmp;
-
-        }
-
-        stat = (*_flash_program_buf)(addr, data, size, 
-                                     flash_info.block_mask, flash_info.buffer_size);
-        stat = flash_hwr_map_error(stat);
+  
+  HAL_FLASH_CACHES_OFF(d_cache, i_cache);
+  FLASH_Enable((unsigned short*)addr, (unsigned short *)(addr+len));
+  while (addr <= end_addr) {
+    block_size = flash_block_size(dev, addr);
+    size = length;
+    // Only one block at once
+    if (size > block_size) size = block_size;
+    
+    // Writing from the middle of a block?
+    offset = (size_t)addr & (block_size-1);
+    if (offset)
+      size = MIN(block_size - offset, size);
+    stat = dev->funs->flash_program(dev, addr, ram, size);
+    stat = dev->funs->flash_hwr_map_error(dev,stat);
 #ifdef CYGSEM_IO_FLASH_VERIFY_PROGRAM
-        if (0 == stat) // Claims to be OK
-            if (memcmp(addr, data, size) != 0) {                
-                stat = 0x0BAD;
+    if (CYG_FLASH_ERR_OK == stat) // Claims to be OK
+      if (!dev->funs->flash_read && memcmp((void *)addr, ram, size) != 0) {                
+        stat = CYG_FLASH_ERR_DRV_VERIFY;
 #ifdef CYGSEM_IO_FLASH_CHATTER
-                (*flash_info.pf)("V");
+        dev->pf("V");
 #endif
-            }
+      }
 #endif
-        if (stat) {
-            *err_addr = (void *)addr;
-            break;
-        }
+    if (CYG_FLASH_ERR_OK != stat && err_address) {
+      *err_address = addr;
+      break;
+    }
 #ifdef CYGSEM_IO_FLASH_CHATTER
-        (*flash_info.pf)(".");
+    dev->pf(".");
 #endif
-        len -= size;
-        addr += size/sizeof(*addr);
-        data += size/sizeof(*data);
-    }
-    FLASH_Disable((unsigned short*)addr, (unsigned short *)(addr+len));
-    HAL_FLASH_CACHES_ON(d_cache, i_cache);
+    length -= size;
+    addr += size;
+    ram += size;
+  }
+  FLASH_Disable((unsigned short*)addr, (unsigned short *)(addr+len));
+  HAL_FLASH_CACHES_ON(d_cache, i_cache);
 #ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("\n");
+    dev->pf("\n");
+#endif
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_unlock(&dev->mutex);
 #endif
+  if (stat != CYG_FLASH_ERR_OK) {
     return (stat);
+  }
+  if ( flash_base + len - 1 > dev->end) {
+    return cyg_flash_program(dev->end+1, ram, 
+                             len - (dev->end - flash_base) - 1,
+                             err_address);
+  }
+  return CYG_FLASH_ERR_OK;
 }
 
-int
-flash_read(void *_addr, void *_data, int len, void **err_addr)
+__externC int 
+cyg_flash_read(cyg_flashaddr_t flash_base, 
+               const void *ram_base, 
+               const size_t len, 
+               cyg_flashaddr_t *err_address)
 {
-#ifdef CYGSEM_IO_FLASH_READ_INDIRECT
-    int stat = 0;
-    int size;
-    typedef int code_fun(void *, void *, int, unsigned long, int);
-    code_fun *_flash_read_buf;
-    unsigned char *addr = (unsigned char *)_addr;
-    unsigned char *data = (unsigned char *)_data;
-    CYG_ADDRESS tmp;
-    int d_cache, i_cache;
-
-    if (!flash_info.init) {
-        return FLASH_ERR_NOT_INIT;
-    }
-
-    _flash_read_buf = (code_fun*) __anonymizer(&flash_read_buf);
-
-#ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("... Read from %p-%p at %p: ", (void*)data, 
-                     (void*)(((CYG_ADDRESS)data)+len), (void*)addr);
-#endif
-
-    HAL_FLASH_CACHES_OFF(d_cache, i_cache);
-    FLASH_Enable((unsigned short*)addr, (unsigned short *)(addr+len));
-    while (len > 0) {
-        size = len;
-        if (size > flash_info.block_size) size = flash_info.block_size;
-
-        tmp = (CYG_ADDRESS)addr & ~flash_info.block_mask;
-        if (tmp) {
-                tmp = flash_info.block_size - tmp;
-                if (size>tmp) size = tmp;
-
-        }
+  struct cyg_flash_dev * dev;
+  cyg_flashaddr_t addr, end_addr;
+  unsigned char * ram = (unsigned char *)ram_base;
+  size_t block_size, size, length, offset;
+  int stat = CYG_FLASH_ERR_OK;
+  int d_cache, i_cache;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
+  for (dev = flash_head; 
+       dev && !((dev->start <= flash_base) && ( dev->end > flash_base));
+       dev=dev->next)
+    ;
+  if (!dev) return CYG_FLASH_ERR_INVALID;
 
-        stat = (*_flash_read_buf)(addr, data, size, 
-                                     flash_info.block_mask, flash_info.buffer_size);
-        stat = flash_hwr_map_error(stat);
-#ifdef CYGSEM_IO_FLASH_VERIFY_PROGRAM_
-        if (0 == stat) // Claims to be OK
-            if (memcmp(addr, data, size) != 0) {                
-                stat = 0x0BAD;
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_lock(&dev->mutex);
+#endif
+  addr = flash_base;
+  end_addr = flash_base + len - 1;
+  if (end_addr > dev->end) {
+    end_addr = dev->end;
+  }
+  length = end_addr - addr + 1;
+  
 #ifdef CYGSEM_IO_FLASH_CHATTER
-                (*flash_info.pf)("V");
-#endif
-            }
-#endif
-        if (stat) {
-            *err_addr = (void *)addr;
-            break;
-        }
-#ifdef CYGSEM_IO_FLASH_CHATTER
-        (*flash_info.pf)(".");
+  dev->pf("... Read from %p-%p to %p: ", addr, addr+len, ram_base);
 #endif
-        len -= size;
-        addr += size/sizeof(*addr);
-        data += size/sizeof(*data);
+  
+  HAL_FLASH_CACHES_OFF(d_cache, i_cache);
+  FLASH_Enable((unsigned short*)addr, (unsigned short *)(addr+len));
+  while (addr <= end_addr) {
+    block_size = flash_block_size(dev, addr);
+    size = length;
+    // Only one block at once
+    if (size > block_size) size = block_size;
+    
+    // Reading from the middle of a block?
+    offset = (size_t)addr & (block_size-1);
+    if (offset)
+      size = MIN(block_size - offset, size);
+    if (dev->funs->flash_read) {
+      stat = dev->funs->flash_read(dev, addr, ram, size);
+      stat = dev->funs->flash_hwr_map_error(dev,stat);
+    } else {
+      memcpy(ram, (void *)addr, size);
+      stat = CYG_FLASH_ERR_OK;
     }
-    FLASH_Disable((unsigned short*)addr, (unsigned short *)(addr+len));
-    HAL_FLASH_CACHES_ON(d_cache, i_cache);
+    if (CYG_FLASH_ERR_OK != stat && err_address) {
+      *err_address = addr;
+      break;
+    }
 #ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("\n");
+    dev->pf(".");
 #endif
+    length -= size;
+    addr += size;
+    ram += size;
+  }
+  FLASH_Disable((unsigned short*)addr, (unsigned short *)(addr+len));
+  HAL_FLASH_CACHES_ON(d_cache, i_cache);
+#ifdef CYGSEM_IO_FLASH_CHATTER
+  dev->pf("\n");
+#endif
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_unlock(&dev->mutex);
+#endif
+  if (stat != CYG_FLASH_ERR_OK) {
     return (stat);
-#else // CYGSEM_IO_FLASH_READ_INDIRECT
-    // Direct access to FLASH memory is possible - just move the requested bytes
-    if (!flash_info.init) {
-        return FLASH_ERR_NOT_INIT;
-    }
-    memcpy(_data, _addr, len);
-    return FLASH_ERR_OK;
-#endif
+  }
+  if ( flash_base + len - 1 > dev->end) {
+    return cyg_flash_read(dev->end+1, ram,
+                          len - (dev->end - flash_base) - 1,
+                          err_address);
+  }
+  return CYG_FLASH_ERR_OK;
 }
 
 #ifdef CYGHWR_IO_FLASH_BLOCK_LOCKING
-
-int
-flash_lock(void *addr, int len, void **err_addr)
+__externC int 
+cyg_flash_lock(const cyg_flashaddr_t flash_base, 
+               const size_t len, 
+               cyg_flashaddr_t *err_address)
 {
-    unsigned short *block, *end_addr;
-    int stat = 0;
-    typedef int code_fun(unsigned short *);
-    code_fun *_flash_lock_block;
-    int d_cache, i_cache;
-
-    if (!flash_info.init) {
-        return FLASH_ERR_NOT_INIT;
-    }
-
+  cyg_flashaddr_t block, end_addr, addr;
+  struct cyg_flash_dev * dev;
+  size_t block_size;
+  int stat = CYG_FLASH_ERR_OK;
+  int d_cache, i_cache;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
 #ifdef CYGSEM_IO_FLASH_SOFT_WRITE_PROTECT
-    if (plf_flash_query_soft_wp(addr,len))
-        return FLASH_ERR_PROTECT;
+  if (plf_flash_query_soft_wp(addr,len))
+    return CYG_FLASH_ERR_PROTECT;
 #endif
-
-    _flash_lock_block = (code_fun*) __anonymizer(&flash_lock_block);
+  
+  for (dev = flash_head; 
+       dev && !((dev->start <= flash_base) && ( dev->end > flash_base));
+       dev=dev->next)
+    ;
+  if (!dev) return CYG_FLASH_ERR_INVALID;
+  if (!dev->funs->flash_block_lock) return CYG_FLASH_ERR_INVALID;
 
-    block = (unsigned short *)((CYG_ADDRESS)addr & flash_info.block_mask);
-    end_addr = (unsigned short *)((CYG_ADDRESS)addr+len);
-
-    /* Check to see if end_addr overflowed */
-    if( (end_addr < block) && (len > 0) ){
-        end_addr = (unsigned short *) ((CYG_ADDRESS) flash_info.end - 1);
-    }
-
-#ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("... Lock from %p-%p: ", block, end_addr);
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_lock(&dev->mutex);
 #endif
-
-    HAL_FLASH_CACHES_OFF(d_cache, i_cache);
-    FLASH_Enable(block, end_addr);
-    while (block < end_addr) {
-        unsigned short *tmp_block;
-        stat = (*_flash_lock_block)(block);
-        stat = flash_hwr_map_error(stat);
-        if (stat) {
-            *err_addr = (void *)block;
-            break;
-        }
-
-        // Check to see if block will overflow
-        tmp_block = block + flash_info.block_size / sizeof(*block);
-        if(tmp_block < block){
-            // If block address overflows, set block value to end on this loop
-            block = end_addr;
-        }
-        else{
-            block = tmp_block;
-        }
+  addr = flash_base;
+  end_addr = flash_base + len - 1;
+  if (end_addr > dev->end) {
+    end_addr = dev->end;
+  }
+  
+  block_size = flash_block_size(dev, addr);
+  block = (cyg_flashaddr_t)((size_t)addr & ~(block_size - 1));
+  
 #ifdef CYGSEM_IO_FLASH_CHATTER
-        (*flash_info.pf)(".");
+  dev->pf("... Locking from %p-%p: ", (void*)block, (void*)end_addr);
 #endif
+  
+  HAL_FLASH_CACHES_OFF(d_cache, i_cache);
+  FLASH_Enable(block, end_addr);
+  while (block <= end_addr) {
+    stat = dev->funs->flash_block_lock(dev,block);
+    stat = dev->funs->flash_hwr_map_error(dev,stat);
+    
+    if (CYG_FLASH_ERR_OK != stat && err_address) {
+      *err_address = (void *)block;
+      break;
     }
-    FLASH_Disable(block, end_addr);
-    HAL_FLASH_CACHES_ON(d_cache, i_cache);
+    block += flash_block_size(dev, addr);
+#ifdef CYGSEM_IO_FLASH_CHATTER
+    dev->pf(".");
+#endif
+  }
+  FLASH_Disable(block, end_addr);
+  HAL_FLASH_CACHES_ON(d_cache, i_cache);
 #ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("\n");
+  dev->pf("\n");
+#endif
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_unlock(&dev->mutex);
 #endif
-    return (stat);
+  if (stat != CYG_FLASH_ERR_OK) {
+    return stat;
+  }
+  
+  if (flash_base + len - 1 > dev->end) {        // Off by one?
+    // The region to erase if bigger than this driver handles. Recurse
+    return cyg_flash_lock(dev->end+1, 
+                          len - (dev->end - flash_base) - 1,
+                          err_address);
+  }
+  return CYG_FLASH_ERR_OK;
 }
 
-int
-flash_unlock(void *addr, int len, void **err_addr)
+__externC int 
+cyg_flash_unlock(const cyg_flashaddr_t flash_base, 
+                 const size_t len, 
+                 cyg_flashaddr_t *err_address)
 {
-    unsigned short *block, *end_addr;
-    int stat = 0;
-    typedef int code_fun(unsigned short *, int, int);
-    code_fun *_flash_unlock_block;
-    int d_cache, i_cache;
-
-    if (!flash_info.init) {
-        return FLASH_ERR_NOT_INIT;
-    }
-
+  cyg_flashaddr_t block, end_addr, addr;
+  struct cyg_flash_dev * dev;
+  size_t block_size;
+  int stat = CYG_FLASH_ERR_OK;
+  int d_cache, i_cache;
+  
+  if (!init) return CYG_FLASH_ERR_NOT_INIT;
+  
 #ifdef CYGSEM_IO_FLASH_SOFT_WRITE_PROTECT
-    if (plf_flash_query_soft_wp(addr,len))
-        return FLASH_ERR_PROTECT;
+  if (plf_flash_query_soft_wp(addr,len))
+    return CYG_FLASH_ERR_PROTECT;
 #endif
-
-    _flash_unlock_block = (code_fun*) __anonymizer(&flash_unlock_block);
-
-    block = (unsigned short *)((CYG_ADDRESS)addr & flash_info.block_mask);
-    end_addr = (unsigned short *)((CYG_ADDRESS)addr+len);
-
-    /* Check to see if end_addr overflowed */
-    if( (end_addr < block) && (len > 0) ){
-        end_addr = (unsigned short *) ((CYG_ADDRESS) flash_info.end - 1);
-    }
-
-#ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("... Unlock from %p-%p: ", block, end_addr);
+  
+  for (dev = flash_head; 
+       dev && !((dev->start <= flash_base) && ( dev->end > flash_base));
+       dev=dev->next)
+    ;
+  if (!dev) return CYG_FLASH_ERR_INVALID;
+  if (!dev->funs->flash_block_lock) return CYG_FLASH_ERR_INVALID;
+  
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_lock(&dev->mutex);
 #endif
-
-    HAL_FLASH_CACHES_OFF(d_cache, i_cache);
-    FLASH_Enable(block, end_addr);
-    while (block < end_addr) {
-        unsigned short *tmp_block;
-        stat = (*_flash_unlock_block)(block, flash_info.block_size, flash_info.blocks);
-        stat = flash_hwr_map_error(stat);
-        if (stat) {
-            *err_addr = (void *)block;
-            break;
-        }
-
-        tmp_block = block + flash_info.block_size / sizeof(*block);
-        if(tmp_block < block){
-            // If block address overflows, set block value to end on this loop
-            block = end_addr;
-        }
-        else{
-            block = tmp_block;
-        }
+  addr = flash_base;
+  end_addr = flash_base + len - 1;
+  if (end_addr > dev->end) {
+    end_addr = dev->end;
+  }
+  
+  block_size = flash_block_size(dev, addr);
+  block = (cyg_flashaddr_t)((size_t)addr & ~(block_size - 1));
+  
 #ifdef CYGSEM_IO_FLASH_CHATTER
-        (*flash_info.pf)(".");
+  dev->pf("... Unlocking from %p-%p: ", (void*)block, (void*)end_addr);
 #endif
+  
+  HAL_FLASH_CACHES_OFF(d_cache, i_cache);
+  FLASH_Enable(block, end_addr);
+  while (block <= end_addr) {
+    stat = dev->funs->flash_block_unlock(dev,block);
+    stat = dev->funs->flash_hwr_map_error(dev,stat);
+    
+    if (CYG_FLASH_ERR_OK != stat && err_address) {
+      *err_address = (void *)block;
+      break;
     }
-    FLASH_Disable(block, end_addr);
-    HAL_FLASH_CACHES_ON(d_cache, i_cache);
+    block += flash_block_size(dev, addr);
+#ifdef CYGSEM_IO_FLASH_CHATTER
+    dev->pf(".");
+#endif
+  }
+  FLASH_Disable(block, end_addr);
+  HAL_FLASH_CACHES_ON(d_cache, i_cache);
 #ifdef CYGSEM_IO_FLASH_CHATTER
-    (*flash_info.pf)("\n");
+  dev->pf("\n");
+#endif
+#ifdef CYGPKG_KERNEL
+  cyg_mutex_unlock(&dev->mutex);
 #endif
-    return (stat);
+  if (stat != CYG_FLASH_ERR_OK) {
+    return stat;
+  }
+  
+  if (flash_base + len - 1 > dev->end) {        // Off by one?
+    // The region to erase if bigger than this driver handles. Recurse
+    return cyg_flash_lock(dev->end+1, 
+                          len - (dev->end - flash_base) - 1,
+                          err_address);
+  }
+  return CYG_FLASH_ERR_OK;
 }
 #endif
 
-char *
-flash_errmsg(int err)
+const char *
+cyg_flash_errmsg(const int err)
 {
     switch (err) {
-    case FLASH_ERR_OK:
+    case CYG_FLASH_ERR_OK:
         return "No error - operation complete";
-    case FLASH_ERR_ERASE_SUSPEND:
+    case CYG_FLASH_ERR_ERASE_SUSPEND:
         return "Device is in erase suspend state";
-    case FLASH_ERR_PROGRAM_SUSPEND:
+    case CYG_FLASH_ERR_PROGRAM_SUSPEND:
         return "Device is in program suspend state";
-    case FLASH_ERR_INVALID:
+    case CYG_FLASH_ERR_INVALID:
         return "Invalid FLASH address";
-    case FLASH_ERR_ERASE:
+    case CYG_FLASH_ERR_ERASE:
         return "Error trying to erase";
-    case FLASH_ERR_LOCK:
+    case CYG_FLASH_ERR_LOCK:
         return "Error trying to lock/unlock";
-    case FLASH_ERR_PROGRAM:
+    case CYG_FLASH_ERR_PROGRAM:
         return "Error trying to program";
-    case FLASH_ERR_PROTOCOL:
+    case CYG_FLASH_ERR_PROTOCOL:
         return "Generic error";
-    case FLASH_ERR_PROTECT:
+    case CYG_FLASH_ERR_PROTECT:
         return "Device/region is write-protected";
-    case FLASH_ERR_NOT_INIT:
+    case CYG_FLASH_ERR_NOT_INIT:
         return "FLASH sub-system not initialized";
-    case FLASH_ERR_DRV_VERIFY:
+    case CYG_FLASH_ERR_DRV_VERIFY:
         return "Data verify failed after operation";
-    case FLASH_ERR_DRV_TIMEOUT:
+    case CYG_FLASH_ERR_DRV_TIMEOUT:
         return "Driver timed out waiting for device";
-    case FLASH_ERR_DRV_WRONG_PART:
+    case CYG_FLASH_ERR_DRV_WRONG_PART:
         return "Driver does not support device";
-    case FLASH_ERR_LOW_VOLTAGE:
+    case CYG_FLASH_ERR_LOW_VOLTAGE:
         return "Device reports low voltage";
     default:
         return "Unknown error";