//==========================================================================
//
//      39vfxxx_aux.c
//
//      Flash driver for the SST family - implementation. 
//
//==========================================================================
// ####ECOSGPLCOPYRIGHTBEGIN####                                            
// -------------------------------------------                              
// This file is part of eCos, the Embedded Configurable Operating System.   
// Copyright (C) 2004, 2005, 2006, 2007, 2008 Free Software Foundation, Inc.
// Copyright (C) 2004, 2005, 2006, 2007, 2008 eCosCentric Limited           
//
// 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     
// Software Foundation; either version 2 or (at your option) any later      
// version.                                                                 
//
// eCos is distributed in the hope that it will be useful, but WITHOUT      
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or    
// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License    
// for more details.                                                        
//
// You should have received a copy of the GNU General Public License        
// along with eCos; if not, write to the Free Software Foundation, Inc.,    
// 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.            
//
// As a special exception, if other files instantiate templates or use      
// macros or inline functions from this file, or you compile this file      
// and link it with other works to produce a work based on this file,       
// this file does not by itself cause the resulting work to be covered by   
// the GNU General Public License. However the source code for this file    
// must still be made available in accordance with section (3) of the GNU   
// General Public License v2.                                               
//
// This exception does not invalidate any other reasons why a work based    
// on this file might be covered by the GNU General Public License.         
// -------------------------------------------                              
// ####ECOSGPLCOPYRIGHTEND####                                              
//==========================================================================
//#####DESCRIPTIONBEGIN####
//
// Author(s):    bartv, nickg
// Contributors:
// Date:         2008-01-24
//              
//####DESCRIPTIONEND####
//
//==========================================================================

// This file is #include'd multiple times from the main 39vfxxx.c file,
// It serves to instantiate the various hardware operations in ways
// appropriate for all the bus configurations.

// The following macros are used to construct suitable function names
// for the current bus configuration. SST39_SUFFIX is #define'd before
// each #include of 39vfxxx_aux.c

#ifndef SST39_STR
# define SST39_STR1(_a_) # _a_
# define SST39_STR(_a_) SST39_STR1(_a_)
# define SST39_CONCAT3_AUX(_a_, _b_, _c_) _a_##_b_##_c_
# define SST39_CONCAT3(_a_, _b_, _c_) SST39_CONCAT3_AUX(_a_, _b_, _c_)
#endif

#define SST39_FNNAME(_base_) SST39_CONCAT3(_base_, _,  SST39_SUFFIX)

// Similarly construct a forward declaration, placing the function in
// the .2ram section. Each function must still be in a separate section
// for linker garbage collection.

# define SST39_RAMFNDECL(_base_, _args_) \
  SST39_FNNAME(_base_) _args_ __attribute__((section (".2ram." SST39_STR(_base_) "_" SST39_STR(SST39_SUFFIX))))

// Calculate the various offsets, based on the device count.
// The main code may override these settings for specific
// configurations, e.g. 16as8
#ifndef SST39_OFFSET_COMMAND
# define SST39_OFFSET_COMMAND            0x5555
#endif
#ifndef SST39_OFFSET_COMMAND2
# define SST39_OFFSET_COMMAND2           0x2AAA
#endif
#ifndef SST39_OFFSET_MANUFACTURER_ID
# define SST39_OFFSET_MANUFACTURER_ID    0x0000
#endif
#ifndef SST39_OFFSET_DEVID
# define SST39_OFFSET_DEVID              0x0001
#endif
#ifndef SST39_OFFSET_DEVID2
# define SST39_OFFSET_DEVID2             0x000E
#endif
#ifndef SST39_OFFSET_DEVID3
# define SST39_OFFSET_DEVID3             0x000F
#endif
#ifndef SST39_OFFSET_CFI
# define SST39_OFFSET_CFI                0x5555
#endif
#ifndef SST39_OFFSET_CFI_DATA
# define SST39_OFFSET_CFI_DATA(_idx_)    _idx_
#endif
#ifndef SST39_OFFSET_AT49_LOCK_STATUS
# define SST39_OFFSET_AT49_LOCK_STATUS   0x02
#endif

// For parallel operation commands are issued in parallel and status
// bits are checked in parallel.
#ifndef SST39_PARALLEL
# define SST39_PARALLEL(_cmd_)    (_cmd_)
#endif

// ----------------------------------------------------------------------------
// Diagnostic routines.

#if 0
#define sst_diag( __fmt, ... ) diag_printf("SST: %s[%d]: " __fmt, __FUNCTION__, __LINE__, ## __VA_ARGS__ );
#define sst_dump_buf( __addr, __size ) diag_dump_buf( __addr, __size )
#else
#define sst_diag( __fmt, ... )
#define sst_dump_buf( __addr, __size )
#endif

// ----------------------------------------------------------------------------
// When performing the various low-level operations like erase the flash
// chip can no longer support ordinary data reads. Obviously this is a
// problem if the current code is executing out of flash. The solution is
// to store the key functions in RAM rather than flash, via a special
// linker section .2ram which usually gets placed in the same area as
// .data.
//
// In a ROM startup application anything in .2ram will consume space
// in both the flash and RAM. Hence it is desirable to keep the .2ram
// functions as small as possible, responsible only for the actual
// hardware manipulation.
//
// All these .2ram functions should be invoked with interrupts
// disabled. Depending on the hardware it may also be necessary to
// have the data cache disabled. The .2ram functions must be
// self-contained, even macro invocations like HAL_DELAY_US() are
// banned because on some platforms those could be implemented as
// function calls.

// gcc requires forward declarations with the attributes, then the actual
// definitions.
static int  SST39_RAMFNDECL(sst39_hw_query, (volatile SST39_TYPE*));
static int  SST39_RAMFNDECL(sst39_hw_cfi, (struct cyg_flash_dev*, cyg_39vfxxx_dev*, volatile SST39_TYPE*));
static int  SST39_RAMFNDECL(sst39_hw_erase, (volatile SST39_TYPE*));
static int  SST39_RAMFNDECL(sst39_hw_program, (volatile SST39_TYPE*, volatile SST39_TYPE*, const cyg_uint8*, cyg_uint32 count, int retries));

// ----------------------------------------------------------------------------

// Read the device id. This involves a straightforward command
// sequence, followed by a reset to get back into array mode.
// All chips are accessed in parallel, but only the response
// from the least significant is used.
static int
SST39_FNNAME(sst39_hw_query)(volatile SST39_TYPE* addr)
{
    int devid;

    SST39_2RAM_ENTRY_HOOK();
    
    addr[SST39_OFFSET_COMMAND]   = SST39_COMMAND_SETUP1;
    HAL_MEMORY_BARRIER();
    addr[SST39_OFFSET_COMMAND2]  = SST39_COMMAND_SETUP2;
    HAL_MEMORY_BARRIER();
    addr[SST39_OFFSET_COMMAND]   = SST39_COMMAND_AUTOSELECT;
    HAL_MEMORY_BARRIER();
    devid                        = SST39_UNSWAP(addr[SST39_OFFSET_DEVID]) & 0xFFFF;

    sst_diag("devid %x\n", devid );
    sst_dump_buf(addr, 64 );
        
    SST39_2RAM_EXIT_HOOK();
    return devid;
}

// Perform a CFI query. This involves placing the device(s) into CFI
// mode, checking that this has really happened, and then reading the
// size and block info. The address corresponds to the start of the
// flash.
static int
SST39_FNNAME(sst39_hw_cfi)(struct cyg_flash_dev* dev, cyg_39vfxxx_dev* sst39_dev, volatile SST39_TYPE* addr)
{
    int     dev_size;
    int     erase_regions;

    SST39_2RAM_ENTRY_HOOK();
    
    sst_diag("addr %p %p cmd %08x\n", addr, &addr[SST39_OFFSET_COMMAND], SST39_COMMAND_SETUP1);
    sst_diag("addr %p %p cmd %08x\n", addr, &addr[SST39_OFFSET_COMMAND2], SST39_COMMAND_SETUP2);
    
    addr[SST39_OFFSET_COMMAND]   = SST39_COMMAND_SETUP1;
    HAL_MEMORY_BARRIER();
    addr[SST39_OFFSET_COMMAND2]  = SST39_COMMAND_SETUP2;
    HAL_MEMORY_BARRIER();
    addr[SST39_OFFSET_CFI]   = SST39_COMMAND_CFI;
    HAL_MEMORY_BARRIER();
    sst_diag("addr %p %p cmd %08x\n", addr, &addr[SST39_OFFSET_CFI], SST39_COMMAND_CFI);
    sst_diag("CFI data:\n");
    sst_dump_buf( addr, 256 );
    // Now check that we really are in CFI mode. There should be a 'Q'
    // at a specific address. This test is not 100% reliable, but should
    // be good enough.
    if ('Q' != (SST39_UNSWAP(addr[SST39_OFFSET_CFI_Q]) & 0x00FF)) {
        addr[SST39_OFFSET_COMMAND]   = SST39_COMMAND_RESET;
        HAL_MEMORY_BARRIER();
        SST39_2RAM_EXIT_HOOK();
        return CYG_FLASH_ERR_PROTOCOL;
    }
    // Device sizes are always a power of 2, and the shift is encoded
    // in a single byte
    dev_size = 0x01 << (SST39_UNSWAP(addr[SST39_OFFSET_CFI_SIZE]) & 0x00FF);
    dev->end = dev->start + dev_size - 1;


    // The region count does not indicate the number of different
    // regions the device supports. Instead it indicates first the
    // number of sectors and then the number of blocks, which are
    // alternate ways of viewing the flash. We only pay attention to
    // the second entry, which should contain the block view.

    erase_regions   = SST39_UNSWAP(addr[SST39_OFFSET_CFI_BLOCK_REGIONS]) & 0x00FF;    
    
    if (erase_regions != 2) {
        addr[SST39_OFFSET_COMMAND]   = SST39_COMMAND_RESET;
        HAL_MEMORY_BARRIER();
        SST39_2RAM_EXIT_HOOK();
        return CYG_FLASH_ERR_PROTOCOL;
    }
    dev->num_block_infos    = 1;

    {
        cyg_uint32 count, size;
        cyg_uint32 count_lsb   = SST39_UNSWAP(addr[SST39_OFFSET_CFI_BLOCK_COUNT_LSB(1)]) & 0x00FF;
        cyg_uint32 count_msb   = SST39_UNSWAP(addr[SST39_OFFSET_CFI_BLOCK_COUNT_MSB(1)]) & 0x00FF;
        cyg_uint32 size_lsb    = SST39_UNSWAP(addr[SST39_OFFSET_CFI_BLOCK_SIZE_LSB(1)]) & 0x00FF;
        cyg_uint32 size_msb    = SST39_UNSWAP(addr[SST39_OFFSET_CFI_BLOCK_SIZE_MSB(1)]) & 0x00FF;

        count = ((count_msb << 8) | count_lsb) + 1;
        size  = (size_msb << 16) | (size_lsb << 8);
        sst39_dev->block_info[0].block_size  = (size_t) size * SST39_DEVCOUNT;
        sst39_dev->block_info[0].blocks      = count;
    }

       
    // Get out of CFI mode
    addr[SST39_OFFSET_COMMAND]   = SST39_COMMAND_RESET;
    HAL_MEMORY_BARRIER();

    SST39_2RAM_EXIT_HOOK();
    return CYG_FLASH_ERR_OK;
}

// Erase a single sector, or resume an earlier erase. There is no API
// support for chip-erase. The generic code operates one sector at a
// time, invoking the driver for each sector, so there is no
// opportunity inside the driver for erasing multiple sectors in a
// single call. The address argument points at the start of the
// sector.
static int
SST39_FNNAME(sst39_hw_erase)(volatile SST39_TYPE* addr)
{
    int         retries;
    SST39_TYPE   datum;

    SST39_2RAM_ENTRY_HOOK();
    
    // Start the erase operation
    addr[SST39_OFFSET_COMMAND]   = SST39_COMMAND_SETUP1;
    HAL_MEMORY_BARRIER();
    addr[SST39_OFFSET_COMMAND2]  = SST39_COMMAND_SETUP2;
    HAL_MEMORY_BARRIER();
    addr[SST39_OFFSET_COMMAND]   = SST39_COMMAND_ERASE;
    HAL_MEMORY_BARRIER();
    addr[SST39_OFFSET_COMMAND]   = SST39_COMMAND_SETUP1;
    HAL_MEMORY_BARRIER();
    addr[SST39_OFFSET_COMMAND2]  = SST39_COMMAND_SETUP2;
    HAL_MEMORY_BARRIER();
    addr[0]                      = SST39_COMMAND_ERASE_BLOCK;
    HAL_MEMORY_BARRIER();

    // All chips are now erasing in parallel. Loop until all have
    // completed. This can be detected in a number of ways. The DQ7
    // bit will be 0 until the erase is complete, but there is a
    // problem if something went wrong (e.g. the sector is locked),
    // the erase has not actually started, and the relevant bit was 0
    // already. 

#if CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_ERASE_DELAY > 0    
    // It is necessary to delay a little to let the device get into erase
    // mode and present a zero for DQ7. Otherwise we might see the original
    // data and think the erase is finished too soon.
    
    { int i; for( i = 0; i < CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_ERASE_DELAY; i++ ); }
#endif
    
    for (retries = CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_ERASE_TIMEOUT;
         retries > 0;
         retries--) {
        
        datum  = addr[0];
        // The operation completes when all DQ7 bits are set.
        if ((datum & SST39_STATUS_DQ7) == SST39_STATUS_DQ7) {
            break;
        }
    }

    // A result of 0 indicates a timeout, and depending on configury
    // the block is in erase_suspend mode. A non-zero result indicates
    // that the erase completed or there has been a fatal error.
    SST39_2RAM_EXIT_HOOK();
    return retries;
}

// Write data to flash. At most one block will be processed at a time,
// but the write may be for a subset of the write. The destination
// address will be aligned in a way suitable for the bus. The source
// address need not be aligned. The count is in SST39_TYPE's, i.e.
// as per the bus alignment, not in bytes.
static int
SST39_FNNAME(sst39_hw_program)(volatile SST39_TYPE* block_start, volatile SST39_TYPE* addr, const cyg_uint8* buf, cyg_uint32 count, int retries)
{
    int     i;

    SST39_2RAM_ENTRY_HOOK();
    
    for (i = 0; (i < count) && (retries > 0); i++) {
        SST39_TYPE   datum;
        SST39_TYPE   current, masked_datum;
        
        // We can only clear bits, not set them, so any bits that were
        // already clear need to be preserved.
        current = addr[i];
        datum   = SST39_NEXT_DATUM(buf) & current;
        if (datum == current) {
            // No change, so just move on.
            continue;
        }
        
        block_start[SST39_OFFSET_COMMAND]    = SST39_COMMAND_SETUP1;
        HAL_MEMORY_BARRIER();
        block_start[SST39_OFFSET_COMMAND2]   = SST39_COMMAND_SETUP2;
        HAL_MEMORY_BARRIER();
        block_start[SST39_OFFSET_COMMAND]    = SST39_COMMAND_PROGRAM;
        HAL_MEMORY_BARRIER();
        addr[i] = datum;
        HAL_MEMORY_BARRIER();

        // The data is now being written. While the write is in progress
        // DQ7 will have an inverted value from what was written, so we
        // can poll, comparing just this bit. 
        masked_datum = datum & SST39_STATUS_DQ7;
        while (--retries > 0) {
#if CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_PROGRAM_DELAY > 0
            { int i; for( i = 0; i < CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_PROGRAM_DELAY; i++ ); }
#endif
            current = addr[i];
            if ((current & SST39_STATUS_DQ7) == masked_datum) {
                break;
            }
        }
    }

    // At this point retries holds the total number of retries left.
    //  0 indicates a timeout.
    // >0 indicates success or a fatal error.
    SST39_2RAM_EXIT_HOOK();
    return retries;
}

// ----------------------------------------------------------------------------
// Exported code, mostly for placing in a cyg_flash_dev_funs structure.

// Just read the device id, either for sanity checking that the system
// has been configured for the right device, or for filling in the
// block info by a platform-specific init routine if the platform may
// be manufactured with one of several different chips.
int
SST39_FNNAME(cyg_39vfxxx_read_devid) (struct cyg_flash_dev* dev)
{
    int                 (*query_fn)(volatile SST39_TYPE*);
    int                 devid;
    volatile SST39_TYPE* addr;
    SST39_INTSCACHE_STATE;

    CYG_CHECK_DATA_PTR(dev, "valid flash device pointer required");

    sst_diag("\n", 0 );
    
    addr     = SST39_UNCACHED_ADDRESS(dev->start);
    query_fn = (int (*)(volatile SST39_TYPE*)) cyg_flash_anonymizer( & SST39_FNNAME(sst39_hw_query) );
    SST39_INTSCACHE_BEGIN();
    devid    = (*query_fn)(addr);
    SST39_INTSCACHE_END();
    return devid;
}

// Validate that the device statically configured is the one on the
// board.
int
SST39_FNNAME(cyg_39vfxxx_init_check_devid)(struct cyg_flash_dev* dev)
{
    cyg_39vfxxx_dev*  sst39_dev;
    int                 devid;

    sst_diag("\n", 0 );
    
    sst39_dev = (cyg_39vfxxx_dev*) dev->priv;
    devid    = SST39_FNNAME(cyg_39vfxxx_read_devid)(dev);
    if (devid != sst39_dev->devid) {
        return CYG_FLASH_ERR_DRV_WRONG_PART;
    }

    // Successfully queried the device, and the id's match. That
    // should be a good enough indication that the flash is working.
    return CYG_FLASH_ERR_OK;
}

// Initialize via a CFI query, instead of statically specifying the
// boot block layout.
int
SST39_FNNAME(cyg_39vfxxx_init_cfi)(struct cyg_flash_dev* dev)
{
    int                 (*cfi_fn)(struct cyg_flash_dev*, cyg_39vfxxx_dev*, volatile SST39_TYPE*);
    volatile SST39_TYPE* addr;
    cyg_39vfxxx_dev*  sst39_dev;
    int                 result;
    SST39_INTSCACHE_STATE;
    
    sst_diag("\n");
    
    CYG_CHECK_DATA_PTR(dev, "valid flash device pointer required");
    sst39_dev    = (cyg_39vfxxx_dev*) dev->priv;    // Remove const, only place where this is needed.
    addr        = SST39_UNCACHED_ADDRESS(dev->start);
    cfi_fn      = (int (*)(struct cyg_flash_dev*, cyg_39vfxxx_dev*, volatile SST39_TYPE*))
        cyg_flash_anonymizer( & SST39_FNNAME(sst39_hw_cfi));

    SST39_INTSCACHE_BEGIN();
    result      = (*cfi_fn)(dev, sst39_dev, addr);
    SST39_INTSCACHE_END();

    // Now calculate the device size, and hence the end field.
    if (CYG_FLASH_ERR_OK == result) {
        int i;
        int size    = 0;
        for (i = 0; i < dev->num_block_infos; i++) {
            sst_diag("region %d: %p x %d\n", i, dev->block_info[i].block_size, dev->block_info[i].blocks );
            size += (dev->block_info[i].block_size * dev->block_info[i].blocks);
        }
        dev->end = dev->start + size - 1;

    }
    return result;
}

// Erase a single block. The calling code will have supplied a pointer
// aligned to a block boundary.
int
SST39_FNNAME(cyg_39vfxxx_erase)(struct cyg_flash_dev* dev, cyg_flashaddr_t addr)
{
    int                 (*erase_fn)(volatile SST39_TYPE*);
    volatile SST39_TYPE* block;
    cyg_flashaddr_t     block_start;
    size_t              block_size;
    int                 i;
    int                 result;
    SST39_INTSCACHE_STATE;

    CYG_CHECK_DATA_PTR(dev, "valid flash device pointer required");
    CYG_ASSERT((addr >= dev->start) && (addr <= dev->end), "flash address out of device range");

    sst39_get_block_info(dev, addr, &block_start, &block_size);
    CYG_ASSERT(addr == block_start, "erase address should be the start of a flash block");
    
    sst_diag("addr %p block %p[%d]\n", addr, block_start, block_size );
    
    block       = SST39_UNCACHED_ADDRESS(addr);
    erase_fn    = (int (*)(volatile SST39_TYPE*)) cyg_flash_anonymizer( & SST39_FNNAME(sst39_hw_erase) );

    SST39_INTSCACHE_BEGIN();
    result = (*erase_fn)(block);
    SST39_INTSCACHE_END();

    // The erase may have failed for a number of reasons.  The best
    // thing to do here is to check that the erase has succeeded.
    block = (SST39_TYPE*) addr;
    for (i = 0; i < (block_size / sizeof(SST39_TYPE)); i++) {
        if (block[i] != (SST39_TYPE)~0) {
            // There is no easy way of detecting the specific error,
            // e.g. locked flash block, timeout, ... So return a
            // useless catch-all error.
            sst_diag("erase fail: %08x[%d] = %08x\n",block,i,block[i]);
            return CYG_FLASH_ERR_ERASE;
        }
    }
    return CYG_FLASH_ERR_OK;
}

// Write some data to the flash. The destination must be aligned
// appropriately for the bus width (not the device width).
int
SST39_FNNAME(cyg_39vfxxx_program)(struct cyg_flash_dev* dev, cyg_flashaddr_t dest, const void* src, size_t len)
{
    int                 (*program_fn)(volatile SST39_TYPE*, volatile SST39_TYPE*, const cyg_uint8*, cyg_uint32, int);
    volatile SST39_TYPE* block;
    volatile SST39_TYPE* addr; 
    cyg_flashaddr_t     block_start;
    size_t              block_size;
    const cyg_uint8*    data;
    int                 retries;
    int                 to_write;
    int                 i;

    SST39_INTSCACHE_STATE;

    CYG_CHECK_DATA_PTR(dev, "valid flash device pointer required");

    sst_diag("dest %p src %p len %d\n", dest, src, len );
    
    // Only support writes that are aligned to the bus boundary. This
    // may be more restrictive than what the hardware is capable of.
    // However it ensures that the hw_program routine can write as
    // much data as possible each iteration, and hence significantly
    // improves performance. The length had better be a multiple of
    // the bus width as well
    if ((0 != ((CYG_ADDRWORD)dest & (sizeof(SST39_TYPE) - 1))) ||
        (0 != (len & (sizeof(SST39_TYPE) - 1)))) {
        return CYG_FLASH_ERR_INVALID;
    }

    addr        = SST39_UNCACHED_ADDRESS(dest);
    CYG_ASSERT((dest >= dev->start) && (dest <= dev->end), "flash address out of device range");

    sst39_get_block_info(dev, dest, &block_start, &block_size);
    CYG_ASSERT(((dest - block_start) + len) <= block_size, "write cannot cross block boundary");
    
    block       = SST39_UNCACHED_ADDRESS(block_start);
    data        = (const cyg_uint8*) src;
    retries     = CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_PROGRAM_TIMEOUT;
    to_write    = len / sizeof(SST39_TYPE);      // Number of words, not bytes

    program_fn  = (int (*)(volatile SST39_TYPE*, volatile SST39_TYPE*, const cyg_uint8*, cyg_uint32, int))
        cyg_flash_anonymizer( & SST39_FNNAME(sst39_hw_program) );

    SST39_INTSCACHE_BEGIN();
    while ((to_write > 0) && (retries > 0)) {
        size_t this_write = (to_write < CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_PROGRAM_BURST_SIZE) ?
            to_write : CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_PROGRAM_BURST_SIZE;

        retries = (*program_fn)(block, addr, data, this_write, retries);
        to_write -= CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_PROGRAM_BURST_SIZE;
        if (to_write > 0) {
            // There is still more to be written. The last write must have been a burst size
            addr += CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_PROGRAM_BURST_SIZE;
            data += sizeof(SST39_TYPE) * CYGNUM_DEVS_FLASH_SST_39VFXXX_V2_PROGRAM_BURST_SIZE;
            SST39_INTSCACHE_SUSPEND();
            SST39_INTSCACHE_RESUME();
        }
    }
    SST39_INTSCACHE_END();

    sst_diag("to_write %d retries %d\n", to_write, retries );
    
    // Too many things can go wrong when manipulating the h/w, so
    // verify the operation by actually checking the data.
    addr = (volatile SST39_TYPE*) dest;
    data = (const cyg_uint8*) src;
    for (i = 0; i < (len / sizeof(SST39_TYPE)); i++) {
        SST39_TYPE   datum   = SST39_NEXT_DATUM(data);
        SST39_TYPE   current = addr[i];
        if ((datum & current) != current) {
            sst_diag("data error %p addr[i] %p datum %08x current %08x\n", data-sizeof(SST39_TYPE), &addr[i], datum, current );
            return CYG_FLASH_ERR_PROGRAM;
        }
    }
    return CYG_FLASH_ERR_OK;
}

// ----------------------------------------------------------------------------
// Clean up the various #define's so this file can be #include'd again
#undef SST39_FNNAME
#undef SST39_RAMFNDECL
#undef SST39_OFFSET_COMMAND
#undef SST39_OFFSET_COMMAND2
#undef SST39_OFFSET_DEVID
#undef SST39_OFFSET_DEVID2
#undef SST39_OFFSET_DEVID3
#undef SST39_OFFSET_CFI
#undef SST39_OFFSET_CFI_DATA
#undef SST39_OFFSET_AT49_LOCK_STATUS
#undef SST39_PARALLEL
