/******************************************************************************
 * Flash File System (ffs)
 * Idea, design and coding by Mads Meisner-Jensen, mmj@ti.com
 *
 * ffs low level flash driver
 *
 * $Id: drv.c 1.30.1.6.1.51.1.1.1.13.1.11 Tue, 06 Jan 2004 14:36:52 +0100 tsj $
 *
 ******************************************************************************/

#ifndef TARGET
#include "ffs.cfg"
#endif

#include "fc-target.h"

#include "ffs/ffs.h"
#include "ffs/board/drv.h"
#include "ffs/board/ffstrace.h"

#if (TARGET == 0)

#ifdef WIN32
#include "windows.h"
#else //WIN32
#include "sys/mman.h"
#include "unistd.h"
#endif //WIN32

#include "stdio.h"
#include "sys/types.h"
#include "sys/stat.h"
#include "fcntl.h"
#else
#include "nucleus.h"

#endif


// Note that all code notes and comments pertaining to single-bank flash
// drivers are located in the AMD SB driver (amdsbdrv.c). Consider this as
// the reference.


/******************************************************************************
 * Globals
 ******************************************************************************/

#if (TARGET == 1)
// NOTE: This is the size in bytes of the single-bank driver code that is
// copied to RAM. The only way to determine the amount of memory needed is
// to look into the linker output file (.map) or the assembler output of
// both amdsbdrv.obj and intelsbdrv.obj files.
#define FFSDRV_CODE_SIZE     (0x200)

uint8 ffsdrv_code[FFSDRV_CODE_SIZE];

#endif

#define INTEL_UNLOCK_SLOW 1

struct dev_s dev;
struct ffsdrv_s ffsdrv;

uint32 int_disable(void);
void int_enable(uint32 tmp);

/******************************************************************************
 * Macros
 ******************************************************************************/

#define addr2offset(address) ( (int) (address) - (int) dev.base )


/******************************************************************************
 * Generic Driver Functions
 ******************************************************************************/

void ffsdrv_generic_write(void *dst, const void *src, uint16 size)
{
    uint8 *mydst = dst;
    const uint8 *mysrc = src;

    if (size > 0)
    {
        if ((unsigned int) mydst & 1) {
            ffsdrv_write_byte(mydst++, *mysrc++);
            size--;
        }
        while (size >= 2) {
            ffsdrv.write_halfword((uint16 *) mydst,
                                  mysrc[0] | (mysrc[1] << 8));
            size -= 2;
            mysrc += 2;
            mydst += 2;
        }
        if (size == 1)
            ffsdrv_write_byte(mydst++, *mysrc++);
    }
}


/******************************************************************************
 * AMD Single Bank Driver Functions
 ******************************************************************************/

#if (TARGET == 1)

// Forward declaration of functions in file amdsbdrv.c
void ffsdrv_ram_amd_sb_write_halfword(volatile uint16 *addr, uint16 value);
void ffsdrv_ram_amd_sb_erase(uint8 block);

#else // (TARGET == 0)

// On PC these functions are empty
void ffsdrv_ram_amd_sb_write_halfword(volatile uint16 *addr, uint16 value) {}
void ffsdrv_ram_amd_sb_erase(uint8 block) {}

#endif // (TARGET == 1)


/******************************************************************************
 * AMD Pseudo Single Bank Driver Functions
 ******************************************************************************/

// This is a pseudo single-bank flash driver. It simulates a single-bank
// flash device on a dual-bank device.

#if (TARGET == 1)

void ffsdrv_amd_pseudo_sb_write_halfword(volatile uint16 *addr, uint16 value)
{
    volatile char *flash = dev.base;
    uint32 cpsr, i, x;

    ttw(ttr(TTrDrvWrite, "wh(%x,%x)" NL, addr, value));

    if (~*addr & value) {
        ttw(ttr(TTrFatal, "wh(%x,%x->%x) fatal" NL, addr, *addr, value));
        return;
    }

    cpsr = int_disable();
    tlw(led_on(LED_WRITE));

    flash[0xAAAA] = 0xAA; // unlock cycle 1
    flash[0x5555] = 0x55; // unlock cycle 2
    flash[0xAAAA] = 0xA0;
    *addr         = value;

    while ((*dev.addr ^ dev.data) & 0x80)
        ;

    tlw(led_off(LED_WRITE));
    int_enable(cpsr);
}

// This VERY simple way of erase suspending only works because we run under
// a pre-emptive operating system, so whenever an interrupt occurs, another
// task takes the CPU, and at the end of the interrupt, FFS gets the CPU
// again.
void ffsdrv_amd_pseudo_sb_erase(uint8 block)
{
    volatile char *flash = dev.base;
    volatile char *addr;
    uint32 cpsr;
    uint16 flashpoll;

    addr = block2addr(block);

    ttw(ttr(TTrDrvErase, "e(%d)" NL, block));

    cpsr = int_disable();
    tlw(led_on(LED_ERASE));

    flash[0xAAAA] = 0xAA; // unlock cycle 1
    flash[0x5555] = 0x55; // unlock cycle 2
    flash[0xAAAA] = 0x80; 
    flash[0xAAAA] = 0xAA; // unlock cycle 1
    flash[0x5555] = 0x55; // unlock cycle 2
    *addr         = 0x30; // AMD erase sector command

    // Wait for erase to finish.
    while ((*addr & 0x80) == 0) {
        tlw(led_toggle(LED_ERASE));
        // Poll interrupts, taking interrupt mask into account.
        if (INT_REQUESTED)
        {
            // 1. suspend erase
            // 2. enable interrupts
            // .. now the interrupt code executes
            // 3. disable interrupts
            // 4. resume erase

            tlw(led_on(LED_ERASE_SUSPEND));
            *addr = 0xB0;

            // wait for erase suspend to finish
            while ((*addr & 0x80) == 0)
                ;

            tlw(led_off(LED_ERASE_SUSPEND));
            int_enable(cpsr);

            // Other interrupts and tasks run now...

            cpsr = int_disable();
            tlw(led_on(LED_ERASE_SUSPEND));

            // Before resuming erase we must? check if the erase is really
            // suspended or if it did finish
            flashpoll = *addr;
            *addr = 0x30;

            tlw(led_off(LED_ERASE_SUSPEND));
        }
    }

    tlw(led_on(LED_ERASE));
    tlw(led_off(LED_ERASE));
    int_enable(cpsr);
}

#else // (TARGET == 0)

void ffsdrv_amd_pseudo_sb_write_halfword(volatile uint16 *addr, uint16 value) {}
void ffsdrv_amd_pseudo_sb_erase(uint8 block) {}

#endif // (TARGET == 1)


/******************************************************************************
 * AMD Dual/Multi Bank Driver Functions
 ******************************************************************************/

// All erase and write operations are performed atomically (interrupts
// disabled). Otherwise we cannot trust the value of dev.state and we cannot
// determine exactly how many of the command words have already been
// written.

// in ffs_end() when we resume an erasure that was previously suspended, how
// does that affect multiple tasks doing that simultaneously?

void ffsdrv_amd_write_end(void);
void ffsdrv_amd_erase_end(void);

void ffsdrv_amd_write_halfword(volatile uint16 *addr, uint16 value)
{
    volatile uint16 *flash = (volatile uint16 *)dev.base;
    uint32 cpsr;

    tlw(led_on(LED_WRITE));
    ttw(ttr(TTrDrvWrite, "wh(%x,%x)" NL, addr, value));

    dev.addr = addr;
    dev.data = value;

    if (~*addr & value) {
        ttw(ttr(TTrFatal, "wh(%x,%x->%x) fatal" NL, addr, *addr, value));
        return;
    }

    cpsr = int_disable();
    tlw(led_toggle(LED_WRITE_SUSPEND));
    dev.state = DEV_WRITE;
    flash[0x555] = 0xAA; // unlock cycle 1
    flash[0x2AA] = 0x55; // unlock cycle 2
    flash[0x555] = 0xA0;
    *addr         = value;
    int_enable(cpsr);
    tlw(led_toggle(LED_WRITE_SUSPEND));

    ffsdrv_amd_write_end();
}

void ffsdrv_amd_write(void *dst, const void *src, uint16 size)
{
    uint8 *mydst = dst;
    const uint8 *mysrc = src;

    if (size > 0)
    {
        if ((unsigned int) mydst & 1) {
            ffsdrv_write_byte(mydst++, *mysrc++);
            size--;
        }
        while (size >= 2) {
            ffsdrv_amd_write_halfword((uint16 *) mydst,
                                      mysrc[0] | (mysrc[1] << 8));
            size -= 2;
            mysrc += 2;
            mydst += 2;
        }
        if (size == 1)
            ffsdrv_write_byte(mydst++, *mysrc++);
    }
}

void ffsdrv_amd_write_end(void)
{
    while ((*dev.addr ^ dev.data) & 0x80)
        tlw(led_toggle(LED_WRITE_SUSPEND));

    dev.state = DEV_READ;

    tlw(led_off(LED_WRITE));
}

void ffsdrv_amd_erase(uint8 block)
{
    volatile uint16 *flash = (volatile uint16 *)dev.base;
    uint32 cpsr;

    tlw(led_on(LED_ERASE));
    ttw(ttr(TTrDrvErase, "e(%d)" NL, block));

    dev.addr = (uint16 *) block2addr(block);

    cpsr = int_disable();
    dev.state = DEV_ERASE;
    flash[0x555] = 0xAA; // unlock cycle 1
    flash[0x2AA] = 0x55; // unlock cycle 2
    flash[0x555] = 0x80; 
    flash[0x555] = 0xAA; // unlock cycle 1
    flash[0x2AA] = 0x55; // unlock cycle 2
    *dev.addr = 0x30; // AMD erase sector command
    int_enable(cpsr);

    ffsdrv_amd_erase_end();
}

void ffsdrv_amd_erase_end(void)
{
    while ((*dev.addr & 0x80) == 0)
        ;

    dev.state = DEV_READ;

    tlw(led_off(LED_ERASE));
}

void ffsdrv_amd_erase_suspend(void)
{
    uint32 cpsr;

    tlw(led_on(LED_ERASE_SUSPEND));
    ttw(str(TTrDrvErase, "es" NL));

    // if erase has finished then all is ok
    if (*dev.addr & 0x80) {
        ffsdrv_amd_erase_end();
        tlw(led_off(LED_ERASE_SUSPEND));
        return;
    }

    // NOTEME: As there is no way to be absolutely certain that erase
    // doesn't finish between last poll and the following erase suspend
    // command, we assume that the erase suspend is safe even though the
    // erase IS actually already finished.

    cpsr = int_disable();
    dev.state = DEV_ERASE_SUSPEND;
    *dev.addr = 0xB0;

    // Wait for erase suspend to finish
    while ((*dev.addr & 0x80) == 0)
        ;

    int_enable(cpsr);
}

void ffsdrv_amd_erase_resume(void)
{
    uint32 cpsr;

    ttw(str(TTrDrvErase, "er" NL));

    // NOTEME: See note in erase_suspend()... We assume that the erase
    // resume is safe even though the erase IS actually already finished.
    cpsr = int_disable();
    dev.state = DEV_ERASE;
    *dev.addr = 0x30;
    int_enable(cpsr);

    tlw(led_off(LED_ERASE_SUSPEND));
}


/******************************************************************************
 * SST Dual/Multi Bank Driver Functions
 ******************************************************************************/

// SST flashes use almost same command set as AMD flashes. Only the command
// addresses (4 more bits) and erase command data (0x50 instead of 0x30) are
// different. SST flashes have no erase suspend/resume commands because they
// are so fast at erasing!

void ffsdrv_sst_write_end(void);
void ffsdrv_sst_erase_end(void);

void ffsdrv_sst_write(void *dst, const void *src, uint16 size)
{
    uint8 *mydst = dst;
    const uint8 *mysrc = src;

    if (size > 0)
    {
        if ((unsigned int) mydst & 1) {
            ffsdrv_write_byte(mydst++, *mysrc++);
            size--;
        }
        while (size >= 2) {
            ffsdrv_amd_write_halfword((uint16 *) mydst,
                                      mysrc[0] | (mysrc[1] << 8));
            size -= 2;
            mysrc += 2;
            mydst += 2;
        }
        if (size == 1)
            ffsdrv_write_byte(mydst++, *mysrc++);
    }
}

// Note that SST flashes have smaller sectors than other flash families.
// Fortunately they support erasure of several of these sectors in a logical
// unit called a "block".
void ffsdrv_sst_erase(uint8 block)
{
    volatile char *flash = dev.base;
    uint32 cpsr;

    tlw(led_on(LED_ERASE));
    ttw(ttr(TTrDrvErase, "e(%d)" NL, block));

    dev.addr = (uint16 *) block2addr(block);

    cpsr = int_disable();
    dev.state = DEV_ERASE;
    flash[0xAAAA] = 0xAA; // unlock cycle 1
    flash[0x5555] = 0x55; // unlock cycle 2
    flash[0xAAAA] = 0x80; 
    flash[0xAAAA] = 0xAA; // unlock cycle 1
    flash[0x5555] = 0x55; // unlock cycle 2
    *dev.addr = 0x50; // SST erase block command
    int_enable(cpsr);

    ffsdrv_sst_erase_end();
}

void ffsdrv_sst_erase_end(void)
{
    // Wait for erase end
    while ((*dev.addr & 0x80) == 0)
        ;

    dev.state = DEV_READ;

    tlw(led_off(LED_ERASE));
}

// Erase suspend/resume commands do not exist for SST flashes, so we just
// poll for the end of the erase operation...

void ffsdrv_sst_erase_suspend(void)
{
    ttw(str(TTrDrvErase, "es" NL));

    ffsdrv_sst_erase_end();
}


/******************************************************************************
 * Intel Single Bank Driver Functions
 ******************************************************************************/

#if (TARGET == 1)

// Forward declaration of functions in file intelsbdrv.c
int  ffsdrv_ram_intel_sb_init(void);
void ffsdrv_ram_intel_sb_write_halfword(volatile uint16 *addr, uint16 value);
void ffsdrv_ram_intel_sb_erase(uint8 block);
void ffsdrv_ram_intel_erase(uint8 block);

#else // (TARGET == 0)

// On PC these functions are empty
void ffsdrv_ram_intel_sb_write_halfword(volatile uint16 *addr, uint16 value) {}
void ffsdrv_ram_intel_sb_erase(uint8 block) {}
void ffsdrv_ram_intel_erase(uint8 block) {}

#endif // (TARGET == 1)


/******************************************************************************
 * Intel Dual/Multi Bank Driver Functions
 ******************************************************************************/

void ffsdrv_intel_write_end(void);
void ffsdrv_intel_erase_end(void);


// ffsdrv_intel_write_halfword and ffsdrv_intel_write_end is not used
// because of the bug in the intel flash device. Instead is the functions
// ffsdrv_ram_intel_sb_write_halfword and ffsdrv_ram_intel_erase used.
void ffsdrv_intel_write_halfword(volatile uint16 *addr, uint16 value)
{
    uint32 cpsr;

    tlw(led_on(LED_WRITE));
    ttw(ttr(TTrDrvWrite, "wh(%x,%x)" NL, addr, value));

    dev.addr = addr;

    if (~*addr & value) {
        ttw(ttr(TTrFatal, "wh(%x,%x->%x) fatal" NL, addr, *addr, value));
        return;
    }

    cpsr = int_disable();
    dev.state = DEV_WRITE;

#if (INTEL_UNLOCK_SLOW == 1)
    *addr = 0x60; // Intel Config setup
    *addr = 0xD0; // Intel Unlock block
    *addr = 0x50; // Intel Clear Status Register
#endif

    *addr = 0x40; // Intel program byte/word
    *addr = value;

    int_enable(cpsr);

    ffsdrv_intel_write_end();
}

void ffsdrv_intel_write(void *dst, const void *src, uint16 size)
{
    uint8 *mydst = dst;
    const uint8 *mysrc = src;

    if (size > 0)
    {
        if ((unsigned int) mydst & 1) {
            ffsdrv_write_byte(mydst++, *mysrc++);
            size--;
        }
        while (size >= 2) {
            ffsdrv_intel_write_halfword((uint16 *) mydst,
                                        mysrc[0] | (mysrc[1] << 8));
            size -= 2;
            mysrc += 2;
            mydst += 2;
        }
        if (size == 1)
            ffsdrv_write_byte(mydst++, *mysrc++);
    }
}

void ffsdrv_intel_write_end(void)
{
    uint32 cpsr;
	// We can be interrupted and reentered thus the state can have been changed.
    while ((*dev.addr & 0x80) == 0 && dev.state == DEV_WRITE)
        ;

    // The flash state and dev.state must be in sync thus the stat changes
    // must be protect from being interrupted
    cpsr = int_disable();
    *dev.addr = 0xFF; // Intel read array
    dev.state = DEV_READ;
    int_enable(cpsr);

    tlw(led_off(LED_WRITE));
}


void ffsdrv_intel_erase(uint8 block) 
{
    uint32 cpsr;

    ttw(ttr(TTrDrvErase, "e(%d)" NL, block));
    tlw(led_on(LED_ERASE));

    dev.addr = (uint16 *) block2addr(block);

    cpsr = int_disable();
    dev.state = DEV_ERASE;

#if (INTEL_UNLOCK_SLOW == 1)
    *dev.addr = 0x60; // Intel Config setup
    *dev.addr = 0xD0; // Intel Unlock block
#endif

    *dev.addr = 0x50; // Intel clear status register (not really necessary)
    *dev.addr = 0x20; // Intel erase setup
    *dev.addr = 0xD0; // Intel erase confirm

    int_enable(cpsr);

    ffsdrv_intel_erase_end();
}

void ffsdrv_intel_erase_end(void)
{
    while ((*dev.addr & 0x80) == 0 && dev.state == DEV_ERASE)
        ;

    *dev.addr = 0xFF; // Intel read array

    dev.state = DEV_READ;
    tlw(led_off(LED_ERASE));
}

void ffsdrv_intel_erase_suspend(void)
{
    uint32 cpsr;
    uint16 poll;

    ttw(str(TTrDrvErase, "es" NL));
    tlw(led_on(LED_ERASE_SUSPEND));

    cpsr = int_disable();
    dev.state = DEV_ERASE_SUSPEND;
    *dev.addr = 0xB0; // Intel Erase Suspend
    *dev.addr = 0x70; // Intel Read Status Register
    while (((poll = *dev.addr) & 0x80) == 0)
        ;

    if ((poll & 0x40) == 0) {
        // Block erase has completed
        tlw(led_off(LED_ERASE_SUSPEND));
        dev.state = DEV_READ;
        tlw(led_off(LED_ERASE));
    }
    *dev.addr = 0xFF; // Intel read array
    int_enable(cpsr);
}

void ffsdrv_intel_erase_resume(void)
{
    uint32 cpsr;

    ttw(str(TTrDrvErase, "er" NL));

    cpsr = int_disable();
    dev.state = DEV_ERASE;
    *dev.addr = 0xD0; // Intel erase resume

    // The following "extra" Read Status command is required because Intel
    // has changed the specification of the W30 flash! (See "1.8 Volt Intel®
    // Wireless Flash Memory with 3 Volt I/O 28F6408W30, 28F640W30,
    // 28F320W30 Specification Update")
	*dev.addr = 0x70; // Intel Read Status Register

    int_enable(cpsr);

    tlw(led_off(LED_ERASE_SUSPEND));
}


/******************************************************************************
 * RAM Family Functions
 ******************************************************************************/

void ffsdrv_ram_write_halfword(volatile uint16 *dst, uint16 value)
{
  *dst = value;
}

void ffsdrv_ram_write(void *dst, const void *src, uint16 size)
{
    uint8 *mydst = dst;
    const uint8 *mysrc = src;
  
    if (size == 0)
        return;
    else if (size == 1)
        ffsdrv_write_byte(mydst, *mysrc);
    else {
        if ((int) mydst & 1) {
            ffsdrv_write_byte(mydst++, *mysrc++);
            size--;
        }
        while (size >= 2) {
            ffsdrv_ram_write_halfword((uint16 *) mydst, mysrc[0]|(mysrc[1] << 8));
            size -= 2;
            mysrc += 2;
            mydst += 2;
        }
        if (size == 1)
            ffsdrv_write_byte(mydst++, *mysrc++);
    }
}

void ffsdrv_ram_erase(uint8 block) 
{
    int i;
    char *addr;

    addr = block2addr(block);
    
    for (i = 0; i < (1 << dev.binfo[block].size_ld); i++) {
        *addr++ = 0xFF;
    }
}

/******************************************************************************
 * Void Functions
 ******************************************************************************/

int ffsdrv_null_init(void)
{
    ttw(ttr(TTrDrvOther, "ffsdrv_null_init()" NL));

    return 0;
}

void ffsdrv_null_erase(uint8 block)
{
    ttw(ttr(TTrDrvErase, "ffsdrv_null_erase(%d)" NL, block));
}

void ffsdrv_null_write_halfword(volatile uint16 *addr, uint16 value)
{
    ttw(ttr(TTrDrvWrite, "ffsdrv_null_write_halfword(0x%x, 0x%x)" NL, addr, value));
}

void ffsdrv_null_write(void *dst, const void *src, uint16 size)
{
    ttw(ttr(TTrDrvWrite, "ffsdrv_null_write(0x%x, 0x%x, %d)" NL, dst, src, size));
}

void ffsdrv_null_erase_suspend(void)
{
    ttw(str(TTrDrvErase, "ffsdrv_null_erase_suspend()" NL));
}

void ffsdrv_null_erase_resume(void)
{
    ttw(str(TTrDrvErase, "ffsdrv_null_erase_resume()" NL));
}

void ffsdrv_null_write_end(void)
{
    ttw(str(TTrDrvWrite, "ffsdrv_null_write_end()" NL));
}

void ffsdrv_null_erase_end(void)
{
    ttw(str(TTrDrvErase, "ffsdrv_null_erase_end()" NL));
}


/******************************************************************************
 * Test Driver Functions
 ******************************************************************************/

#if (TARGET == 0)

static char *image_addr = 0;
static int image_size = 0;
#ifdef WIN32
HANDLE image_fd, map_fd;
#else //WIN32
static int image_fd = 0;
#endif //WIN32

extern int   arg_removeimage;
extern char *arg_imagename;

extern void test_fatal_printf(char *format, ...);

void ffsdrv_write_check(char *addr, int size)
{
    offset_t offset, last;

    offset = addr2offset(addr);
    last = dev.binfo[dev.numblocks-1].offset
        + (1 << dev.binfo[dev.numblocks-1].size_ld);

    if (offset < 0 || (offset + size) > last) {
        fprintf(stderr, "ffsdrv_write_check() failed (addr = 0x%x, size = %d)\n",
                (int) addr, size);
        fprintf(stdout, "ffsdrv_write_check() failed (addr = 0x%x, size = %d)\n",
                (int) addr, size);
        exit (1);
    }
}


void ffsdrv_write_error(uint16 old, uint16 new)
{
    test_fatal_printf("FATAL: Attempt to rewrite 0 to 1 bit "
                      "(old:0x%x/%c new:0x%x/%c)\n",
                      old, (old < ' ' ? '?' : old),
                      new, (new < ' ' ? '?' : new));
}

void ffsdrv_test_write_halfword(volatile uint16 *addr, uint16 value)
{
    tw(tr(TR_FUNC, TrDrvWrite, "test_write_halfword(0x%05x, 0x%x)\n",
          addr2offset(addr), value));

    ffsdrv_write_check((uint8 *) addr, 2);

    if (~*addr & value)
        ffsdrv_write_error(*addr, value);

    *addr = value;
}

void ffsdrv_test_write(void *dst, const void *src, uint16 size)
{
    uint8 *mydst = dst;
    const uint8 *mysrc = src;

    tw(tr(TR_FUNC, TrDrvWrite, "test_write(0x%05x, 0x%x, %d)\n",
          addr2offset(mydst), mysrc, size));

    if (size > 0)
    {
        if ((int) mydst & 1) {
            ffsdrv_write_byte(mydst++, *mysrc++);
            size--;
        }
        while (size >= 2) {
            ffsdrv_test_write_halfword((uint16 *) mydst, mysrc[0]|(mysrc[1] << 8));
            size -= 2;
            mysrc += 2;
            mydst += 2;
        }
        if (size == 1)
            ffsdrv_write_byte(mydst++, *mysrc++);
    }
}

void ffsdrv_test_erase(uint8 block) 
{
    int i;
    uint8 *addr;

    addr = block2addr(block);

    tw(tr(TR_FUNC, TrDrvErase, "ffsdrv_test_erase(%d)\n", block));

    for (i = 0; i < 1 << dev.binfo[block].size_ld; i++) {
        *addr++ = 0xFF;
    }
}

char *ffsdrv_test_create(void)
{
    // If flash image file already exists, open the file, and mmap it.
    // Otherwise, create file, fill file with 1's, then mmap it.
    
    int i;
    struct stat statbuf;
#ifdef WIN32
    OFSTRUCT lpReOpenBuff;
    DWORD last_error;
  	SECURITY_ATTRIBUTES	lpAttributes;	

  	lpAttributes.nLength				= sizeof (lpAttributes);
    lpAttributes.lpSecurityDescriptor	= NULL;
    lpAttributes.bInheritHandle			= TRUE;
#endif
    image_size = (int) dev.binfo[dev.numblocks - 1].offset +
        (1 << dev.binfo[dev.numblocks - 1].size_ld);

    tw(tr(TR_BEGIN, TrDrvInit, "ffsdrv_test_create() {\n"));
    tw(tr(TR_FUNC, TrDrvInit, "%s image: '%s', size = %d\n",
          arg_removeimage ? "new" : "current", arg_imagename, image_size));

    // create file if it does not exist
#ifdef WIN32
    if( arg_removeimage || OpenFile( arg_imagename, &lpReOpenBuff, OF_EXIST) == HFILE_ERROR )
#else 
        if (arg_removeimage || lstat(arg_imagename, &statbuf) == -1)
#endif
        {
            char data[64];
#ifdef WIN32
            DWORD bwritten;
#endif
            // only the first run should remove the flash image file
            arg_removeimage = 0;

            tw(tr(TR_FUNC, TrDrvInit, "creating new flash image file '%s'\n",
                  arg_imagename));
#ifdef WIN32
            image_fd = CreateFile(arg_imagename,
                                  GENERIC_WRITE | GENERIC_READ,
                                  0,
                                  NULL,
                                  OPEN_ALWAYS,
                                  FILE_ATTRIBUTE_NORMAL,
                                  NULL );
#else 
            image_fd = open(arg_imagename , O_RDWR|O_CREAT,
                            (S_IRWXU & ~S_IXUSR) |
                            ( S_IRWXG & ~S_IXGRP) | (S_IRWXO & ~S_IXOTH));
#endif 
            if (image_fd == -1) {
                perror("Failed to create flash image");
                exit(1);
            }
        
            // write 1's to the file.
            for (i = 0; i < 64; i++)
                data[i] = 0xff;
        
#ifdef WIN32
            for (i = 0; i < image_size/64; i++)
                WriteFile(image_fd, data, 64, &bwritten, NULL);
            CloseHandle(image_fd);
#else
            for (i = 0; i < image_size/64; i++)
                write(image_fd, data, 64);
        
            close(image_fd);
#endif
            image_fd = 0;
            tw(tr(TR_FUNC, TrDrvInit, "flash image file created\n"));
        }

    // only open image file if this is the first initialization.
    if (image_fd > 0) {
        tw(tr(TR_FUNC, TrDrvInit, "re-opening '%s' file of size %d\n",
              arg_imagename, image_size));
    }
    else {
        tw(tr(TR_FUNC, TrDrvInit, "opening '%s' file of size %d\n",
              arg_imagename, image_size));

#ifdef WIN32
        image_fd = OpenFile( arg_imagename, &lpReOpenBuff, OF_READWRITE);
        map_fd = CreateFileMapping (image_fd,
                                    &lpAttributes,
                                    PAGE_READWRITE,
                                    0,
                                    0,
                                    arg_imagename);
#else 
        image_fd = open(arg_imagename, O_RDWR, S_IRWXU|S_IRWXG|S_IRWXO);
#endif
        if (image_fd == -1) {
            perror("Failed to open flash image");
            exit(1);
        }

        // memory map the file and update block addresses of binfo array
#ifdef WIN32
        image_addr = MapViewOfFile( map_fd,
                                    FILE_MAP_ALL_ACCESS,
                                    0,
                                    0,
                                    0);
#else 
        image_addr = mmap(0, image_size, PROT_READ|PROT_WRITE,
                          MAP_FILE|MAP_SHARED, image_fd, 0);
#endif
    }

    tw(tr(TR_END, TrDrvInit, "}\n"));

    return image_addr;
}

#endif // (TARGET == 0)


/******************************************************************************
 * Device Detection and Copying of Driver to RAM
 ******************************************************************************/

#if (TARGET == 1)

// Note that this function reads device code of any of the three known flash
// families; Intel, AMD and SST. This works because Intel and AMD use
// the same command data for entering READ_IDENTIFIER mode (0x90).
// The function should be copied and executed from RAM!
void ffsdrv_device_id_read(uint16 *manufact, uint16 *device)
{
#if defined(CONFIG_TARGET_FCFAM) || defined(CONFIG_TARGET_PIRELLI) || \
	defined(__GNUC__)
    /*
     * This new FreeCalypso version of the device ID read function
     * should work for all current targets, but we are being conservative
     * and only enabling it for those targets for which it is required,
     * i.e., where TI's original version does not work.
     *
     * Selenite change: for the gcc version we need to use this new
     * autodetect code for all targets, TI's original version won't work
     * because we run with the Calypso boot ROM at 0.
     */

    int addr, half, base, i;

    /*
     * We operate at a non-zero erase block boundary so that this ID read
     * operation will still work in our newer FreeCalypso environments
     * where we have the Calypso boot ROM mapped at 0.
     */
    base = 0x40000;

    /*
     * muckery similar to TI's original to avoid literal pool loads,
     * but we produce and use 0xAAA and 0x554 offsets instead of TI's
     * original 0xAAAA and 0x5555.
     */
    for (i = 0, addr = 0; i < 3; i++)
        addr = addr << 4 | 0xA;
    half = (addr >> 1) & ~1;

    FLASH_WRITE_HALFWORD (base + addr, 0xAA);
    FLASH_WRITE_HALFWORD (base + half, 0x55);
    FLASH_WRITE_HALFWORD (base + addr, 0x90); // Intel/AMD read id command

    *manufact = FLASH_READ_HALFWORD (base + 0); // flash a0 = 0
    *device   = FLASH_READ_HALFWORD (base + 2); // flash a0 = 1

    // Read extended id
    device[1] = FLASH_READ_HALFWORD (base + (0xE << 1));
    device[2] = FLASH_READ_HALFWORD (base + (0xF << 1));
    FLASH_WRITE_HALFWORD (base, 0xFF); // Intel read-array command

    // AMD devices do not need the two unlock cycles but SST devices do,
    // even though the SST datasheets states otherwise ;-)
    FLASH_WRITE_HALFWORD (base + addr, 0xAA);
    FLASH_WRITE_HALFWORD (base + half, 0x55);
    FLASH_WRITE_HALFWORD (base + addr, 0xF0); // AMD read-array/reset command
#else
    /* TI's original version */
    int addr, i;

    // This silly looking code has one purpose; to set addr = 0xAAAA. It is
    // necessary in order to force the compiler NOT to produce code that
    // uses LDR opcode(s) with PC-relative addressing. The assember code
    // produced from this C code is completely relocatable!
    for (i = 0, addr = 0; i < 2; i++)
        addr |= addr << 8 | 0xAA;

    FLASH_WRITE_HALFWORD (addr,      0xAA);
    FLASH_WRITE_HALFWORD (addr >> 1, 0x55);
    FLASH_WRITE_HALFWORD (addr,      0x90); // Intel/AMD read id command

    *manufact = FLASH_READ_HALFWORD (0); // flash a0 = 0
    *device   = FLASH_READ_HALFWORD (2); // flash a0 = 1

    // Read extended id
    device[1] = FLASH_READ_HALFWORD (0xE << 1); 
    device[2] = FLASH_READ_HALFWORD (0xF << 1); 
    FLASH_WRITE_HALFWORD (0, 0xFF); // Intel read-array command

    // AMD devices do not need the two unlock cycles but SST devices do,
    // even though the SST datasheets states otherwise ;-)
    FLASH_WRITE_HALFWORD (addr,      0xAA);
    FLASH_WRITE_HALFWORD (addr >> 1, 0x55);
    FLASH_WRITE_HALFWORD (addr,      0xF0); // AMD read-array/reset command
#endif
}

// Copy ffsdrv_device_id_read() function code to RAM. The only known way to
// determine the size of the code is to look either in the linker-generated
// map file or in the assember output file.
void ffsdrv_device_id_read_copy_to_ram(uint16 *dst, int size)
{
    uint16 *src = (uint16 *) &ffsdrv_device_id_read;

    // The ARM7TDMI compiler sets bit 0 for thumb mode function pointers, so
    // we need to clear this in order to copy *all* bytes. Otherwise we
    // exclude first byte and the resulting copy becomes garbage
    src = (uint16 *) (~1 & (int) src);
    size /= 2;

    while (size--)
        *dst++ = *src++;
}

// Copy ffsdrv_xxx_sb_erase() and ffsdrv_xxx_sb_write_halfword() functions
// to RAM. The only known way to determine the size of the code is to look
// either in the linker-generated map file or in the assember output file.
int ffsdrv_driver_copy_to_ram(int type)
{
    int size;
    uint16 *src, *dst;
    extern uint16 ffsdrv_ram_amd_begin[];
    extern uint16 ffsdrv_ram_intel_begin[];
    uint32 offset_of_init;
    uint32 offset_of_erase;
    uint32 offset_of_write_halfword;
    
    ttw(ttr(TTrDrvOther, "ffsdrv_driver_copy_to_ram() {" NL));

    switch (type) {
    case FFS_DRIVER_AMD:
    case FFS_DRIVER_AMD_SB:
        src = ffsdrv_ram_amd_begin;
        offset_of_erase =
            (uint32) ffsdrv_ram_amd_sb_erase - (uint32) src;
        offset_of_write_halfword =
            (uint32) ffsdrv_ram_amd_sb_write_halfword - (uint32) src;
        break;
    case FFS_DRIVER_INTEL_SB:
        src = ffsdrv_ram_intel_begin;
        offset_of_init =
            (uint32) ffsdrv_ram_intel_sb_init - (uint32) src;
        offset_of_erase =
            (uint32) ffsdrv_ram_intel_sb_erase - (uint32) src;
        offset_of_write_halfword =
            (uint32) ffsdrv_ram_intel_sb_write_halfword - (uint32) src;
        break;
    case FFS_DRIVER_INTEL:
        src = ffsdrv_ram_intel_begin;
        offset_of_init =
            (uint32) ffsdrv_ram_intel_sb_init - (uint32) src;
        offset_of_erase =
            (uint32) ffsdrv_ram_intel_erase - (uint32) src;
        offset_of_write_halfword =
            (uint32) ffsdrv_ram_intel_sb_write_halfword - (uint32) src;
        break;
    default:
        ttw(ttr(TTrDrvOther, "}" NL));
        return 0;
    }

    // Make sure we are handling a half-word aligned address (Thumb mode
    // function pointers have lsb set!)
    src = (uint16 *) (~1 & (int) src);

    // If we detect that the linker allocated the driver to RUN in RAM, the
    // user has obviously NOT removed those linker lines and we bail out!
    if (offset_of_erase > FFSDRV_CODE_SIZE)
        return EFFS_DRIVER;

    dst = (uint16 *) &ffsdrv_code;

    // Code size in halfwords
    size = FFSDRV_CODE_SIZE / 2;

    // Rebind the two changed driver functions
    if (type == FFS_DRIVER_AMD_SB || type == FFS_DRIVER_INTEL_SB) {
        ffsdrv.erase =
            (void (*)(uint8))
            (offset_of_erase + (uint32) dst);
        ffsdrv.write_halfword =
            (void (*)(volatile uint16 *, uint16))
            (offset_of_write_halfword + (uint32) dst);
    }
    if (type == FFS_DRIVER_INTEL_SB) {
        ffsdrv.init =
            (int (*)(void))
            (offset_of_init + (uint32) dst);
    }

    ttw(ttr(TTrDrvOther, "ffsdrv_code, init, write, erase = 0x%07x, 0x%07x, 0x%07x, 0x%07x" NL,
            dst, (uint32) ffsdrv.init,
            (uint32) ffsdrv.write_halfword, (uint32) ffsdrv.erase));

    ttw(ttr(TTrDrvOther, "amd_begin,   init, write, erase = 0x%07x, 0x%07x, 0x%07x, 0x%07x" NL,
            ffsdrv_ram_amd_begin, ffsdrv_null_init,
            ffsdrv_ram_amd_sb_write_halfword, ffsdrv_ram_amd_sb_erase));

    ttw(ttr(TTrDrvOther, "intel_begin, init, write, erase = 0x%07x, 0x%07x, 0x%07x, 0x%07x" NL,
            ffsdrv_ram_intel_begin, ffsdrv_ram_intel_sb_init,
            ffsdrv_ram_intel_sb_write_halfword, ffsdrv_ram_intel_sb_erase));
    
    // Copy the code to RAM
    while (size--)
        *dst++ = *src++;

    ttw(ttr(TTrDrvOther, "}" NL));

    return 0;
}

#if defined(CONFIG_TARGET_PIRELLI) || defined(CONFIG_TARGET_FCFAM)

#ifdef CONFIG_TARGET_FCFAM
#define	FLASH2_BASE_ADDR	0x01800000
#elif defined(CONFIG_TARGET_PIRELLI)
#define	FLASH2_BASE_ADDR	0x02000000
#endif

int ffsdrv_is_new_spansion_flash(void)
{
	uint16 cfi_hi, cfi_lo, cfi_ver;

	/* CFI query */
	FLASH_WRITE_HALFWORD(FLASH2_BASE_ADDR + 0xAAA, 0x98);
	cfi_hi = FLASH_READ_HALFWORD(FLASH2_BASE_ADDR + 0x86);
	cfi_lo = FLASH_READ_HALFWORD(FLASH2_BASE_ADDR + 0x88);
	cfi_ver = (cfi_hi << 8) | (cfi_lo & 0xFF);

	/* return to read array mode */
	FLASH_WRITE_HALFWORD(FLASH2_BASE_ADDR + 0xAAA, 0xF0);

	if (cfi_ver >= 0x3134)
		return 1;
	else
		return 0;
}
#endif

#else // (TARGET == 0)

void ffsdrv_device_id_read(uint16 *manufact, uint16 *device) {}
int  ffsdrv_driver_copy_to_ram(int type) { return 0; }

#endif // (TARGET == 1)


/******************************************************************************
 * Initialization
 ******************************************************************************/

const struct ffsdrv_s ffsdrv_amd = {
    ffsdrv_null_init,
    ffsdrv_amd_erase,
    ffsdrv_amd_write_halfword,
    ffsdrv_amd_write,
    ffsdrv_amd_write_end,
    ffsdrv_amd_erase_suspend,
    ffsdrv_amd_erase_resume
};

const struct ffsdrv_s ffsdrv_amd_sb = {
    ffsdrv_null_init,
    ffsdrv_ram_amd_sb_erase,
    ffsdrv_ram_amd_sb_write_halfword,
    ffsdrv_generic_write,
    ffsdrv_null_write_end,
    ffsdrv_null_erase_suspend,
    ffsdrv_null_erase_resume
};

const struct ffsdrv_s ffsdrv_sst = {
    ffsdrv_null_init,
    ffsdrv_sst_erase,
    ffsdrv_amd_write_halfword, // Use AMD driver function
    ffsdrv_sst_write,
    ffsdrv_amd_write_end,      // Use AMD driver function
    ffsdrv_sst_erase_suspend,
    ffsdrv_null_erase_resume
};

const struct ffsdrv_s ffsdrv_sst_sb = {
    ffsdrv_null_init,
    ffsdrv_null_erase,
    ffsdrv_null_write_halfword,
    ffsdrv_null_write,
    ffsdrv_null_write_end,
    ffsdrv_null_erase_suspend,
    ffsdrv_null_erase_resume
};

// We use the functions ffsdrv_ram_intel_sb_write_halfword and
// ffsdrv_ram_intel_erase due to the bug in the intel wireless flash
// device. See 28F640W30.pdf specification Errata 5.
const struct ffsdrv_s ffsdrv_intel = {
    ffsdrv_null_init,
	ffsdrv_intel_erase,
    ffsdrv_intel_write_halfword,
    ffsdrv_generic_write,
    ffsdrv_intel_write_end,
    ffsdrv_intel_erase_suspend,
    ffsdrv_intel_erase_resume
};

const struct ffsdrv_s ffsdrv_intel_sb = {
    ffsdrv_null_init,
    ffsdrv_ram_intel_sb_erase,
    ffsdrv_ram_intel_sb_write_halfword,
    ffsdrv_generic_write,
    ffsdrv_null_write_end,
    ffsdrv_null_erase_suspend,
    ffsdrv_null_erase_resume
};

const struct ffsdrv_s ffsdrv_null = {
    ffsdrv_null_init,
    ffsdrv_null_erase,
    ffsdrv_null_write_halfword,
    ffsdrv_null_write,
    ffsdrv_null_write_end,
    ffsdrv_null_erase_suspend,
    ffsdrv_null_erase_resume
};

const struct ffsdrv_s ffsdrv_amd_pseudo_sb = {
    ffsdrv_null_init,
    ffsdrv_amd_pseudo_sb_erase,
    ffsdrv_amd_pseudo_sb_write_halfword,
    ffsdrv_generic_write,
    ffsdrv_null_write_end,
    ffsdrv_null_erase_suspend,
    ffsdrv_null_erase_resume
};

const struct ffsdrv_s ffsdrv_ram = {
    ffsdrv_null_init,
    ffsdrv_ram_erase,
    ffsdrv_ram_write_halfword,
    ffsdrv_ram_write,
    ffsdrv_null_write_end,
    ffsdrv_null_erase_suspend,
    ffsdrv_null_erase_resume
};

#if (TARGET == 0)
const struct ffsdrv_s ffsdrv_test = {
    ffsdrv_null_init,
    ffsdrv_test_erase,
    ffsdrv_test_write_halfword,
    ffsdrv_test_write,
    ffsdrv_null_write_end,
    ffsdrv_null_erase_suspend,
    ffsdrv_null_erase_resume
};
#endif

// Note: This function is designed for little-endian memory addressing!
void ffsdrv_write_byte(void *dst, uint8 value)
{
    uint16 halfword;

    tw(tr(TR_FUNC, TrDrvWrite, "ffsdrv_write_byte(0x%05x, 0x%x)\n",
       (int) (addr2offset(dst)), value));
    ttw(str(TTrDrvWrite, "wb" NL));

    if ((int) dst & 1)
        halfword =                (value << 8) | *((uint8 *) dst - 1);
    else
        halfword = (*((uint8 *) dst + 1) << 8) | (value);

    ffsdrv.write_halfword((uint16 *) ((int) dst & ~1), halfword);
}


extern uint16 ffs_flash_manufact;
extern uint16 ffs_flash_device;

effs_t ffsdrv_init(void)
{
    const struct ffsdrv_s *p;
    const struct flash_info_s *flash = &flash_info[0];
    int error;

    tw(tr(TR_BEGIN, TrDrvInit, "drv_init() {\n"));
    ttw(str(TTrDrvOther, "ffsdrv_init() {" NL));

    dev.state = DEV_READ;
    dev.binfo = 0;
    dev.base = 0;
    dev.numblocks = 0;

    // If ffs_flash_device is zero, detect device automatically by copying
    // the detect function into RAM and execute it from there...
    if (ffs_flash_manufact == 0 && ffs_flash_device == 0)
    {
#if (TARGET == 1)
        char detect_code[0x80];
        typedef (*pf_t)(uint16 *, uint16 *);
        pf_t myfp;
        uint16 device_id[3];

        ffsdrv_device_id_read_copy_to_ram((uint16 *) detect_code, 
                                          sizeof(detect_code));
        // Combine bit 0 of the thumb mode function pointer with the address
        // of the code in RAM. Then call the detect function in RAM.
#ifdef __GNUC__
        /* gcc fails to do the needed trick, so force Thumb */
        myfp = (pf_t) (((int) detect_code) | 1);
#else
        myfp = (pf_t) (((int) &ffsdrv_device_id_read & 1) | (int) detect_code);
#endif
        (*myfp)(&dev.manufact, device_id);

        if ((dev.manufact == MANUFACT_AMD || dev.manufact == MANUFACT_FUJITSU) && 
            device_id[0] == 0x227E) {
            // This is a multi-id device
            dev.device = (device_id[1] << 8) | (device_id[2] & 0xFF);
          #if defined(CONFIG_TARGET_PIRELLI) || defined(CONFIG_TARGET_FCFAM)
            if (device_id[1] == 0x2221 && device_id[2] == 0x2200)
              dev.device += ffsdrv_is_new_spansion_flash();
          #endif
        }
        else if (dev.manufact == MANUFACT_SAMSUNG && device_id[0] == 0x257E) {
            /* Samsung's version of extended ID */
            dev.device = (device_id[1] << 8) | (device_id[2] & 0xFF);
        }
        else 
            dev.device = device_id[0];
#endif
    }
    else {
        dev.manufact = ffs_flash_manufact;
        dev.device   = ffs_flash_device;
    }

    tw(tr(TR_FUNC, TrDrvInit, "TARGET = %d\n", TARGET));
    tw(tr(TR_FUNC, TrDrvInit, "Looking up device (0x%2x,0x%4x): ",
          dev.manufact, dev.device));
    while (flash->manufact) {
        tw(tr(TR_NULL, TrDrvInit, "(0x%02x,0x%04x) ",
              flash->manufact, flash->device));
        if (dev.manufact == flash->manufact && dev.device == flash->device) {
            tw(tr(TR_NULL, TrDrvInit, "FOUND "));
            break;
        }
        flash++;
    }
    tw(tr(TR_NULL, TrDrvInit, "\n"));

    if (flash->manufact == 0) {
        tw(tr(TR_END, TrDrvInit, "} (%d)\n", EFFS_NODEVICE));
        return EFFS_NODEVICE;
    }

    dev.binfo     = (struct block_info_s *) flash->binfo;

    if (flash->driver == FFS_DRIVER_RAM && flash->base == 0) {
        if (ffs_ram_image_address <= 0) {
            tw(tr(TR_END, TrDrvInit, "} (%d)\n", EFFS_DRIVER));
            return EFFS_DRIVER;
        }
        dev.base = (char *) ffs_ram_image_address;
    }
    else 
        dev.base = (char *) flash->base;

    dev.numblocks = flash->numblocks;
    dev.driver    = flash->driver;

    // We assume that ALL blocks are of equal size
    dev.blocksize_ld = dev.binfo[0].size_ld;
    dev.blocksize    = (1 << dev.blocksize_ld);

    dev.atomlog2 = FFS_ATOM_LOG2;
    dev.atomsize = 1 << dev.atomlog2;
    dev.atomnotmask = dev.atomsize - 1;

#if (TARGET == 0)
    if (dev.manufact == MANUFACT_TEST)
        dev.base = ffsdrv_test_create();

    p = &ffsdrv_test;

#else // (TARGET == 1)

    // Initialize hardware independent driver functions array
    switch (dev.driver) {
    case FFS_DRIVER_AMD:           p = &ffsdrv_amd; break;
    case FFS_DRIVER_AMD_SB:        p = &ffsdrv_amd_sb; break;
    case FFS_DRIVER_SST:           p = &ffsdrv_sst; break;
    case FFS_DRIVER_SST_SB:        p = &ffsdrv_sst_sb; break;
    case FFS_DRIVER_INTEL:         p = &ffsdrv_intel; break;
    case FFS_DRIVER_INTEL_SB:      p = &ffsdrv_intel_sb; break;
    case FFS_DRIVER_AMD_PSEUDO_SB: p = &ffsdrv_amd_pseudo_sb; break;
    case FFS_DRIVER_RAM:           p = &ffsdrv_ram; break;
    default:                       p = &ffsdrv_null; break;
    }

#endif // (TARGET == 0)

    // Bind the driver functions
    ffsdrv.init           = p->init;
    ffsdrv.erase          = p->erase;
    ffsdrv.write_halfword = p->write_halfword;
    ffsdrv.write          = p->write;
    ffsdrv.write_end      = p->write_end;
    ffsdrv.erase_suspend  = p->erase_suspend;
    ffsdrv.erase_resume   = p->erase_resume;

    // Copy single bank driver code to RAM (and possibly re-bind some of the
    // driver functions)
    error = ffsdrv_driver_copy_to_ram(dev.driver);

    if (error >= 0)
        error = ffsdrv.init();

    tw(tr(TR_FUNC, TrDrvInit, "dev.binfo     = 0x%x\n", (unsigned int) dev.binfo));
    tw(tr(TR_FUNC, TrDrvInit, "dev.base      = 0x%x\n", (unsigned int) dev.base));
    tw(tr(TR_FUNC, TrDrvInit, "dev.numblocks = %d\n", dev.numblocks));
    tw(tr(TR_FUNC, TrDrvInit, "dev.blocksize = %d\n", dev.blocksize));
    tw(tr(TR_FUNC, TrDrvInit, "dev.atomlog2/atomsize/atomnotmask = %d/%d/%x\n",
          dev.atomlog2, dev.atomsize, dev.atomnotmask));
    tw(tr(TR_END, TrDrvInit, "} %d\n", error));
    ttw(ttr(TTrDrvOther, "} %d" NL, error));

    return error;
}


/******************************************************************************
 * Interrupt Enable/Disable
 ******************************************************************************/

// IMPORTANT NOTE! Apparently, locating this ARM assembly code at the top of
// this file will make the compiler trash the A1 register between the calls
// of arm_int_disable and arm_int_enable() thus crashing the whole system.
// If the code is placed AFTER the usage of the functions, the compiler
// saves the A1 register. Strange but true.

// IMPORTANT NOTE! Apparently, another strange thing is that if the
// functions are declared static, they don't work!

// Executing code from RAM is NOT trivial when we need to jump between ROM
// (flash) and RAM memory. The ARM only supports 26-bit relative branch
// offsets. This is the reason why we have a local copy of the
// arm_int_disable/enable() functions in this file plus each of the
// single-bank drivers.

#if (TARGET == 1)
// Note that we use our own interrupt disable/enable function because
// Nucleus allegedly should have a bug in its implementation for this.

#ifdef __GNUC__
#define NOINLINE __attribute__ ((noinline))
#else
#define NOINLINE
#endif

uint32 NOINLINE int_disable(void)
{
#ifdef __GNUC__
    asm("        .code 16");
#else
    asm("        .state16");
#endif
    asm("        mov       A1, #0xC0");
    asm("        ldr       A2, tct_disable");
    asm("        bx        A2      ");

#ifdef __GNUC__
    asm(".balign 4");
    asm("tct_disable:");
    asm("        .word     TCT_Control_Interrupts");
#else
    asm("tct_disable    .field     _TCT_Control_Interrupts+0,32");
    asm("	            .global	   _TCT_Control_Interrupts");
#endif
}

void NOINLINE int_enable(uint32 cpsr)
{
#ifdef __GNUC__
    asm("        .code 16");
#else
    asm("        .state16");
#endif
    asm("        ldr       A2, tct_enable");
    asm("        bx        A2      ");

#ifdef __GNUC__
    asm(".balign 4");
    asm("tct_enable:");
    asm("        .word     TCT_Control_Interrupts");
#else
    asm("tct_enable 	.field     _TCT_Control_Interrupts+0,32");
    asm("	            .global	   _TCT_Control_Interrupts");
#endif
}

#else

uint32 int_disable(void) { return 0; }
void int_enable(uint32 tmp) {}

#endif // (TARGET == 1)