/*****************************************************************************
 * FLASH loader for STR71x.                                                  *
 *                                                                           *
 * NOTE: This loader also supports programming the MB393B STR7 evaluation    *
 * board's external FLASH if it has been re-compiled with "EXTERNAL_FLASH"   *
 * defined.                                                                  *
 *                                                                           *
 * Copyright (c) 2001, 2002 Rowley Associates Limited.                       *
 *                                                                           *
 * This file may be distributed under the terms of the License Agreement     *
 * provided with this software.                                              *
 *                                                                           *
 * THIS FILE IS PROVIDED AS IS WITH NO WARRANTY OF ANY KIND, INCLUDING THE   *
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. *
 *****************************************************************************/

#include "../loader/loader.h"

#define SECTORS 10

#define FLASH_START_ADDRESS 0x40000000
#define FLASH_END_ADDRESS (FLASH_START_ADDRESS + 0xD0000)
#define ADDRESS_IN_FLASH(address) ADDRESS_IN_RANGE(address, FLASH_START_ADDRESS, FLASH_END_ADDRESS)

#define FCR0_OFFSET 0x100000 /* Flash Control Register 0 */
#define FCR1_OFFSET 0x100004 /* Flash Control Register 1 */
#define FDR0_OFFSET 0x100008 /* Flash Data Register 0 */
#define FDR1_OFFSET 0x10000c /* Flash Data Register 1 */
#define FAR_OFFSET  0x100010 /* Flash Address Register */
#define FER_OFFSET  0x100014 /* Flash Error Register */
#define WPAR_OFFSET 0x10dfb0 /* Flash Non Volatile Write Protection Register */
#define APR0_OFFSET 0x10dfb8 /* Flash Non Volatile Access Protection Register 0 */
#define APR1_OFFSET 0x10dfbc /* Flash Non Volatile Access Protection Register 1 */

/* Flash Control Register 0 bitmasks */
#define FCR0_WMS   0x80000000
#define FCR0_SUSP  0x40000000
#define FCR0_WPG   0x20000000
#define FCR0_DWPG  0x10000000
#define FCR0_SER   0x8000000
#define FCR0_BER   0x4000000
#define FCR0_MER   0x2000000
#define FCR0_SPR   0x1000000
#define FCR0_INTM  0x200000
#define FCR0_INTP  0x100000
#define FCR0_LPS   0x8000
#define FCR0_LOCK  0x10
#define FCR0_BUSY1 0x4
#define FCR0_BUSY0 0x2

/* Flash Control Register 1 bitmasks */
#define FCR1_B1S  0x2000000
#define FCR1_B0S  0x1000000
#define FCR1_B1F1 0x20000
#define FCR1_B1F0 0x10000
#define FCR1_B0F7 0x80
#define FCR1_B0F6 0x40
#define FCR1_B0F5 0x20
#define FCR1_B0F4 0x10
#define FCR1_B0F3 0x8
#define FCR1_B0F2 0x4
#define FCR1_B0F1 0x2
#define FCR1_B0F0 0x1

/* Flash Error Register bitmasks */
#define FER_WPF   0x100
#define FER_RESER 0x80
#define FER_SEQER 0x40
#define FER_IOER  0x8
#define FER_PGER  0x4
#define FER_ERER  0x2
#define FER_ERR   0x1
#define FER_MASK  0xffffffff

static const unsigned int sectorAddresses[SECTORS] = 
{  
  FLASH_START_ADDRESS | 0x000000, /* Bank 0 Flash 0 */
  FLASH_START_ADDRESS | 0x002000, /* Bank 0 Flash 1 */
  FLASH_START_ADDRESS | 0x004000, /* Bank 0 Flash 2 */
  FLASH_START_ADDRESS | 0x006000, /* Bank 0 Flash 3 */
  FLASH_START_ADDRESS | 0x008000, /* Bank 0 Flash 4 */
  FLASH_START_ADDRESS | 0x010000, /* Bank 0 Flash 5 */
  FLASH_START_ADDRESS | 0x020000, /* Bank 0 Flash 6 */
  FLASH_START_ADDRESS | 0x030000, /* Bank 0 Flash 7 */
  FLASH_START_ADDRESS | 0x0c0000, /* Bank 1 Flash 0 */
  FLASH_START_ADDRESS | 0x0c2000  /* Bank 1 Flash 1 */
};

static const int sectorLengths[SECTORS] = 
{
  0x2000,  /* Bank 0 Flash 0 */
  0x2000,  /* Bank 0 Flash 1 */
  0x2000,  /* Bank 0 Flash 2 */
  0x2000,  /* Bank 0 Flash 3 */
  0x8000,  /* Bank 0 Flash 4 */
  0x10000, /* Bank 0 Flash 5 */
  0x10000, /* Bank 0 Flash 6 */
  0x10000, /* Bank 0 Flash 7 */
  0x2000,  /* Bank 1 Flash 0 */
  0x2000,  /* Bank 1 Flash 1 */  
};

static const unsigned int sectorMasks[SECTORS] =
{
  FCR1_B0F0,
  FCR1_B0F1,
  FCR1_B0F2,
  FCR1_B0F3,
  FCR1_B0F4,
  FCR1_B0F5,
  FCR1_B0F6,
  FCR1_B0F7,
  FCR1_B1F0,
  FCR1_B1F1
};

static int
is_erased(unsigned int address, unsigned int highAddress)
{
  unsigned int length = highAddress - address + 1;
  while (address & 3)
    {
      if (*(unsigned char *)address != 0xFF)
        return 0;
      length--;
      address++;
    }
  while (length > 4)
    {
      if (*(unsigned long *)address != 0xFFFFFFFF)
        return 0;
      length -= 4;
      address += 4;
    }
  while (length)
    {
      if (*(unsigned char *)address != 0xFF)
        return 0;
      length--;
      address++;
    }
  return 1;
}

static int
flash_erase_sector(int sector)
{
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET) |= (unsigned int)FCR0_SER;
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR1_OFFSET) |= (unsigned int)sectorMasks[sector];
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET) |= (unsigned int)FCR0_WMS;

  while (*(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET));

  if ((*(volatile unsigned int*)(FLASH_START_ADDRESS | FER_OFFSET)) & FER_MASK)
    {
      *(volatile unsigned int*)(FLASH_START_ADDRESS | FER_OFFSET) = 0;
      return 0;
    }

  return 1;
}

static int
flash_erase_module(void)
{
  // NOTE: The erase module operation does not appear to behave reliably, so 
  // two sector erases are used instead.

  // erase all Bank 0 sectors
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET) |= (unsigned int)FCR0_SER;
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR1_OFFSET) |= (unsigned int)0x000000FF; // select bank 0 sectors
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET) |= (unsigned int)FCR0_WMS;
  
  while (*(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET));

  if ((*(volatile unsigned int*)(FLASH_START_ADDRESS | FER_OFFSET)) & FER_MASK)
    {
      *(volatile unsigned int*)(FLASH_START_ADDRESS | FER_OFFSET) = 0;
      return 0;
    }

  // erase all Bank 1 sectors - cannot be done in conjunction with Bank 0
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET) |= (unsigned int)FCR0_SER;
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR1_OFFSET) |= (unsigned int)0x00030000; // select bank 1 sectors
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET) |= (unsigned int)FCR0_WMS;
  
  while (*(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET));

  if ((*(volatile unsigned int*)(FLASH_START_ADDRESS | FER_OFFSET)) & FER_MASK)
    {
      *(volatile unsigned int*)(FLASH_START_ADDRESS | FER_OFFSET) = 0;
      return 0;
    }

  return 1;
}

int 
flash_write_word(volatile unsigned int *addr, unsigned int value)
{
  /* send flash write command to the flash */
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET) |= (unsigned int)FCR0_WPG;
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FAR_OFFSET) = (unsigned int)addr;
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FDR0_OFFSET) = value;
  /**(volatile unsigned int*)(FLASH_START_ADDRESS | FDR1_OFFSET)  = (unsigned int)data_word1;*/
  *(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET) |= (unsigned int)FCR0_WMS;

  while (*(volatile unsigned int*)(FLASH_START_ADDRESS | FCR0_OFFSET));

  if (*(volatile unsigned int*)(FLASH_START_ADDRESS | FER_OFFSET))
    {
      *(volatile unsigned int*)(FLASH_START_ADDRESS | FER_OFFSET) = 0;
      return 0;
    }

  return 1;
}

static int
flash_write_byte(volatile unsigned char *addr, unsigned char value)
{ 
  int result = 1;
  unsigned char tmp[4];
  volatile unsigned int *alignedAddr = (unsigned int *)((unsigned int)addr & ~3); 
  *(unsigned int *)tmp = *alignedAddr;
  tmp[(unsigned int)addr & 3] = value;

  if (*(unsigned int *)tmp != *alignedAddr)
    result = flash_write_word((volatile unsigned int *)alignedAddr, *(unsigned int *)tmp);

  return result;
}

#ifdef EXTERNAL_FLASH

// External FLASH (M28W320CB) Routines

#define FLASH2_START_ADDRESS 0x60000000
#define FLASH2_LENGTH 0x00400000
#define FLASH2_END_ADDRESS FLASH2_START_ADDRESS + FLASH2_LENGTH
#define ADDRESS_IN_FLASH2(address) ADDRESS_IN_RANGE(address, FLASH2_START_ADDRESS, FLASH2_END_ADDRESS)

#define FLASH2_BUSY(ptr) ((*ptr ^ *ptr) != 0)
#define FLASH2_RESET() *((volatile unsigned short *)FLASH_START_ADDRESS) = 0xF0
#define FLASH_REG(a) *((volatile unsigned short *)a)

typedef struct
{
  unsigned int n;
  unsigned int size;
} sectdef_t;

static const sectdef_t flash2Sectors[] =
{
  /* 8 * 8K Sectors */
  { 8, 0x00002000 },
  /* 63 * 64K Sectors */
  { 63, 0x00010000 },
  /* Terminator */
  {0, 0} 
};

static unsigned char
flash2_status(void)
{
  unsigned char status;
  FLASH_REG(FLASH2_START_ADDRESS) = 0x70;
  status = FLASH_REG(FLASH2_START_ADDRESS);
  return status;
}

static void
flash2_lock_sector(volatile unsigned short *address)
{
  FLASH_REG(address) = 0x60;
  FLASH_REG(address)=  0x01;
  while ((flash2_status() & 0x80) == 0);
  FLASH_REG(FLASH2_START_ADDRESS) = 0xFF;
}

static void
flash2_unlock_sector(volatile unsigned short *address)
{
  FLASH_REG(address) = 0x60;
  FLASH_REG(address)=  0xD0;
  while ((flash2_status() & 0x80) == 0);
  FLASH_REG(FLASH2_START_ADDRESS) = 0xFF;
}

static int
flash2_unlock_all()
{
  const sectdef_t *sdptr = flash2Sectors;
  unsigned int sectorStartAddress = FLASH2_START_ADDRESS;
  while (sdptr->n)
    {
      int i;
      for (i = sdptr->n; i; --i)
        {
          unsigned int sectorEndAddress = sectorStartAddress + sdptr->size - 1;
          /* Sector hasn't been erased and it needs to be, erase it. */
          FLASH_REG(sectorStartAddress) = 0x60;
          FLASH_REG(sectorStartAddress)=  0xD0;
          while ((flash2_status() & 0x80) == 0);
          sectorStartAddress = sectorEndAddress + 1;
        }
      sdptr++;
    }
  FLASH_REG(FLASH2_START_ADDRESS) = 0xFF;
  return 1;
}

static int
flash2_erase_sector(volatile unsigned short *address)
{
  FLASH_REG(address) = 0x20;
  FLASH_REG(address)=  0xD0;
  while ((flash2_status() & 0x80) == 0);
  FLASH_REG(FLASH2_START_ADDRESS) = 0xFF;
  return 1;
}

static int
flash2_erase_all()
{
  const sectdef_t *sdptr = flash2Sectors;
  unsigned int sectorStartAddress = FLASH2_START_ADDRESS;
  while (sdptr->n)
    {
      int i;
      for (i = sdptr->n; i; --i)
        {
          unsigned int sectorEndAddress = sectorStartAddress + sdptr->size - 1;
          /* Sector hasn't been erased and it needs to be, erase it. */
          FLASH_REG(sectorStartAddress) = 0x20;
          FLASH_REG(sectorStartAddress)=  0xD0;
          while ((flash2_status() & 0x80) == 0);
          sectorStartAddress = sectorEndAddress + 1;
        }
      sdptr++;
    }
  FLASH_REG(FLASH2_START_ADDRESS) = 0xFF;
  return 1;
}

static int
flash2_write_uint16(volatile unsigned short *address, unsigned short data)
{
  FLASH_REG(address) = 0x40;
  FLASH_REG(address) = data;
  while ((flash2_status() & 0x80) == 0);
  FLASH_REG(FLASH2_START_ADDRESS) = 0xFF;
  return 1;
}

static int
flash2_write_byte(volatile unsigned char *address, unsigned char data)
{
  int result = 1;
  unsigned char tmp[2];
  unsigned short *alignedAddress = (unsigned short *)((unsigned int)address & ~1);
  *(unsigned short *)tmp = *alignedAddress;
  tmp[(unsigned int)address & 1] = data;
  if (*(unsigned short *)tmp != *alignedAddress)
    result = flash2_write_uint16((unsigned short *)alignedAddress, *(unsigned short *)tmp);
  return result;
}

#endif

void
loaderBegin()
{
}

void
loaderEnd()
{
}

int
loaderPoke(unsigned char *address, unsigned int length)
{
  if (ADDRESS_IN_FLASH((unsigned int)address))
    {
      // Write aligned data
      if (((unsigned int)address & 3) == 0)
        {
          while (length >= 4)
            {
              unsigned int data = loaderReadWord();
              flash_write_word((unsigned int *)address,  data);
              address += 4;
              length -= 4;
            }
        }

      // Write unaligned data
      while (length)
        {
          unsigned int data = loaderReadWord();
          int i;
          for(i = 4; i && length; --i)
            {
              flash_write_byte(address++, (unsigned char)data);
              data >>= 8;
              length--;
            }
        }
    }
#ifdef EXTERNAL_FLASH
  else if (ADDRESS_IN_FLASH2((unsigned int)address))
    {
      int res = 1;

      // Write aligned data
      if (((unsigned int)address & 1) == 0)
        {
          while (length >= 4)
            {
              unsigned int data = loaderReadWord();
              res &= flash2_write_uint16((unsigned short *)address,  (unsigned short)data);
              address += 2;
              data >>= 16;
              flash2_write_uint16((unsigned short *)address,  (unsigned short)data);
              address += 2;
              length -= 4;
            }
        }

      // Write unaligned data
      while (length)
        {
          unsigned int data = loaderReadWord();
          int i;
          for(i = 4; i && length; --i)
            {
              flash2_write_byte(address++, (unsigned char)data);
              data >>= 8;
              length--;
            }
        }
    }
#endif
  else
    {
      while (length)
        {
          unsigned int data = loaderReadWord();
          int i;
          for(i = 4; i && length; --i)
            {
              *address++ = (unsigned char)data;                  
              data >>= 8;
              length--;
            }
        }
    }
  return 1;
}

int
loaderMemset(unsigned char *address, unsigned int length,  unsigned char c)
{
  while(length--)
    {
      if(ADDRESS_IN_FLASH((unsigned int)address))
        flash_write_byte(address++, c);
#ifdef EXTERNAL_FLASH
      else if (ADDRESS_IN_FLASH2((unsigned int)address))
        flash2_write_byte(address++, c);
#endif
      else
        *address++ = c;
    }
  return 1;
}

int
loaderErase(unsigned char *address, unsigned int length)
{
  unsigned int highAddress = (unsigned int)address + length - 1;
  int i;

  if (RANGE_OCCLUDES_RANGE((unsigned int)address, highAddress, FLASH_START_ADDRESS, FLASH_END_ADDRESS))
    {
      for (i = 0; i < SECTORS; ++i)
        {
          unsigned int sectorStartAddress = sectorAddresses[i];
          unsigned int sectorEndAddress = sectorStartAddress + sectorLengths[i] - 1;
          if (RANGE_OCCLUDES_RANGE((unsigned int)address, highAddress, sectorStartAddress, sectorEndAddress) && 
              !is_erased((unsigned int)address > sectorStartAddress ? (unsigned int)address : sectorStartAddress, highAddress < sectorEndAddress ? highAddress : sectorEndAddress))
            {
              /* Sector hasn't been erased and it needs to be, erase it. */
              if (!flash_erase_sector(i))
                return 0;
            }
        }
    }

#ifdef EXTERNAL_FLASH
  if (RANGE_OCCLUDES_RANGE((unsigned int)address, highAddress, FLASH2_START_ADDRESS, FLASH2_END_ADDRESS))
    {
      const sectdef_t *sdptr = flash2Sectors;
      unsigned int sectorStartAddress = FLASH2_START_ADDRESS;
      while (sdptr->n)
        {
          int i;
          for (i = sdptr->n; i; --i)
            {
              unsigned int sectorEndAddress = sectorStartAddress + sdptr->size - 1;
              if (RANGE_OCCLUDES_RANGE((unsigned int)address, highAddress, sectorStartAddress, sectorEndAddress))
                {
                  flash2_unlock_sector((unsigned short *)sectorStartAddress);
                  if (!is_erased((unsigned int)address > sectorStartAddress ? (unsigned int)address : sectorStartAddress, highAddress < sectorEndAddress ? highAddress : sectorEndAddress))
                    {
                      /* Sector hasn't been erased and it needs to be, erase it. */
                      if (!flash2_erase_sector((unsigned short *)sectorStartAddress))
                        return 0;
                    }
                }
              sectorStartAddress = sectorEndAddress + 1;
            }
          sdptr++;
        }
    }
#endif
  return 1;
}

int
loaderEraseAll()
{
  if (!is_erased(FLASH_START_ADDRESS, FLASH_END_ADDRESS))
    return flash_erase_module();
#ifdef EXTERNAL_FLASH
  flash2_unlock_all();
  if (!is_erased(FLASH2_START_ADDRESS, FLASH2_END_ADDRESS))
    return flash2_erase_all();
#endif
  return 1;
}

int
loaderSetParameter(unsigned int parameter, unsigned int value)
{
  return 1;
}

