/*****************************************************************************
  Cogent CSB637 Loader Program (Intel StrataFlash 28F640 or 28F128).

  Copyright (c) 2005 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 FLASH_START_ADDRESS 0x10000000
#define FLASH_LENGTH 0x01000000
#define FLASH_END_ADDRESS (FLASH_START_ADDRESS + FLASH_LENGTH - 1)
#define ADDRESS_IN_FLASH(address) ADDRESS_IN_RANGE(address, FLASH_START_ADDRESS, FLASH_END_ADDRESS)

#define FLASH_REG(a) *((volatile unsigned short *)a)

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

static const sectdef_t sectors[] = 
{
  /* 128 * 128K Sectors */
  { 128, 0x20000 },
  /* Terminator */
  {0, 0}
};

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 unsigned char
flash_status(void)
{
  unsigned char status;
  FLASH_REG(FLASH_START_ADDRESS) = 0x70;
  status = FLASH_REG(FLASH_START_ADDRESS);
  return status;
}

static void
flash_lock_sector(volatile unsigned short *address)
{
  FLASH_REG(address) = 0x60;
  FLASH_REG(address)=  0x01;
  while ((flash_status() & 0x80) == 0);
  FLASH_REG(FLASH_START_ADDRESS) = 0xFF;
}

static void
flash_unlock_sector(volatile unsigned short *address)
{
  unsigned char status;
  FLASH_REG(address) = 0x60;
  FLASH_REG(address)=  0xD0;
  while (((status = flash_status()) & 0x80) == 0);
  FLASH_REG(FLASH_START_ADDRESS) = 0xFF;
}

static int
flash_unlock_all()
{
  const sectdef_t *sdptr = sectors;
  unsigned int sectorStartAddress = FLASH_START_ADDRESS;
  while (sdptr->n)
    {
      int i;
      for (i = sdptr->n; i; --i)
        {
          unsigned int sectorEndAddress = sectorStartAddress + sdptr->size - 1;
          FLASH_REG(sectorStartAddress) = 0x60;
          FLASH_REG(sectorStartAddress)=  0xD0;
          while ((flash_status() & 0x80) == 0);
          sectorStartAddress = sectorEndAddress + 1;
        }
      sdptr++;
    }
  FLASH_REG(FLASH_START_ADDRESS) = 0xFF;
  return 1;
}

static int
flash_erase_sector(volatile unsigned short *address)
{ 
  unsigned char status;
  FLASH_REG(address) = 0x20;
  FLASH_REG(address)=  0xD0;
  while (((status = flash_status()) & 0x80) == 0);
  FLASH_REG(FLASH_START_ADDRESS) = 0xFF;
  return 1;
}

static int
flash_erase_all()
{ 
  const sectdef_t *sdptr = sectors;
  unsigned int sectorStartAddress = FLASH_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 ((flash_status() & 0x80) == 0);
          sectorStartAddress = sectorEndAddress + 1;
        }
      sdptr++;
    }
  FLASH_REG(FLASH_START_ADDRESS) = 0xFF;
  return 1;
}

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

static int
flash_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 = flash_write_uint16((unsigned short *)alignedAddress, *(unsigned short *)tmp);
  return result;
}

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 & 1) == 0)
        {
          while (length >= 4)
            {
              unsigned int data = loaderReadWord();
              flash_write_uint16((unsigned short *)address,  (unsigned short)data);
              address += 2;
              data >>= 16;
              flash_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)
            {
              flash_write_byte(address++, (unsigned char)data);
              data >>= 8;
              length--;
            }
        }
    }
  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);
      else
        *address++ = c;
    }
  return 1;
}

int
loaderErase(unsigned char *address, unsigned int length)
{
  unsigned int highAddress = (unsigned int)address + length - 1;
  if (RANGE_OCCLUDES_RANGE((unsigned int)address, highAddress, FLASH_START_ADDRESS, FLASH_END_ADDRESS))
    {
      const sectdef_t *sdptr = sectors;
      unsigned int sectorStartAddress = FLASH_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))
                {
                  flash_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 (!flash_erase_sector((unsigned short *)sectorStartAddress))
                        return 0;
                    }
                }
              sectorStartAddress = sectorEndAddress + 1;
            }
          sdptr++;
        }
    }
  return 1;
}

int
loaderEraseAll()
{
  if (!is_erased(FLASH_START_ADDRESS, FLASH_END_ADDRESS))
    flash_erase_all();
  return 1;
}

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

