/*****************************************************************************
 * Loader for LPD SDKLH75401-10                                              *
 *                                                                           *
 * Copyright (c) 2001, 2002, 2003 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 0x40000000
#define FLASH_SECTOR_SIZE 0x20000
#define FLASH_SECTORS 128
#define FLASH_SIZE (FLASH_SECTOR_SIZE * FLASH_SECTORS)
#define FLASH_END_ADDRESS FLASH_START_ADDRESS + FLASH_SIZE

#define ADDRESS_IN_FLASH(a) ADDRESS_IN_RANGE((unsigned long)a, FLASH_START_ADDRESS, FLASH_END_ADDRESS)

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

static unsigned char
flash_status(void)
{
  FLASH_REG(FLASH_START_ADDRESS) = 0x70;
  return FLASH_REG(FLASH_START_ADDRESS);
}

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;
}

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

static int
flash_erase_all()
{
  unsigned int sectorStartAddress = FLASH_START_ADDRESS;
  int i;
  for (i = 0; i < FLASH_SECTORS; ++i)
    {
      FLASH_REG(sectorStartAddress) = 0x20;
      FLASH_REG(sectorStartAddress)=  0xD0; 
      while ((flash_status() & 0x80) == 0);
      sectorStartAddress += FLASH_SECTOR_SIZE;
    }
  FLASH_REG(FLASH_START_ADDRESS) = 0xFF;
  return 1;
}

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

void
loaderBegin()
{
  /* Enable FLASH writes */
  *((volatile unsigned char *)0x4C400000) = 0x1;
}

void
loaderEnd()
{
  /* Disable FLASH writes */
  *((volatile unsigned char *)0x4C400000) = 0x0;
}

int
loaderPoke(unsigned char *address, unsigned int length)
{
  int res = 1;
  if (ADDRESS_IN_FLASH(address))
    {
      // Write aligned data
      if (((unsigned int)address & 1) == 0)
        {
          while (length >= 4)
            {
              unsigned int data = loaderReadWord();
              res &= 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)
            {
              res &= 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 res;
}

int
loaderMemset(unsigned char *address, unsigned int length,  unsigned char c)
{
  if (ADDRESS_IN_FLASH(address))
    {
      // Write aligned data
      unsigned short sc = c | (c << 8);
      if (((unsigned int)address & 1) == 0)
        {
          while (length >= 2)
            {
              if (!flash_write_uint16((unsigned short *)address,  sc))
                return 0;
              address += 2;
              length -= 2;
            }
        }
    
      // Write unaligned data
      while (length)
        {
          if (!flash_write_byte(address, c))
            return 0;
          ++address;
          --length;
        }
    }
  else
    {
      while(length--)
        *address++ = (unsigned char)c;
    }
  return 1;
}

int
loaderErase(unsigned char *address, unsigned int length)
{
  unsigned int i;
  unsigned int highAddress = (unsigned int)address + length - 1;
  unsigned int sectorStartAddress = FLASH_START_ADDRESS;
  unsigned int sectorEndAddress = FLASH_START_ADDRESS + FLASH_SECTOR_SIZE - 1;
  for (i = 0; i < FLASH_SECTORS; ++i)
    {
      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((unsigned short *)sectorStartAddress))
            return 0;
        }
      sectorStartAddress += FLASH_SECTOR_SIZE;
      sectorEndAddress += FLASH_SECTOR_SIZE;
    }
  return 1;
}

int
loaderEraseAll()
{
  return flash_erase_all((unsigned short *)FLASH_START_ADDRESS);
}

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

