/*****************************************************************************
  LPD SDKLH79520_10 Loader Program.

  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"
#include <targets/LH79520.h>

#define FLASH_START_ADDRESS 0x44000000
#define FLASH_LENGTH 0x01000000
#define FLASH_END_ADDRESS FLASH_START_ADDRESS + FLASH_LENGTH

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

#define CMD_READ_ARRAY          0xFFFF
#define CMD_READ_ID_CODE        0x9090
#define CMD_READ_QUERY          0x9898
#define CMD_READ_STATUS         0x7070
#define CMD_CLEAR_STATUS        0x5050
#define CMD_BLOCK_ERASE         0x2020
#define CMD_PROGRAM             0x4040
#define CMD_PAGE_BUFFER_PROGRAM 0xE8E8
#define CMD_SUSPEND             0xB0B0
#define CMD_CONFIRM             0xD0D0
#define CMD_RESUME              0xD0D0
#define CMD_STS_CONFIG          0xB8B8
#define CMD_BLOCK_LOCK          0x6060
#define CMD_BLOCK_LOCK          0x6060
#define CMD_OTP_PROGRAM         0xC0C0

#define CSR_WSM_STATUS_READY       0x0080
#define CSR_ERASE_SUSPEND_STATUS   0x0040
#define CSR_ERASE_STATUS           0x0020
#define CSR_PROGRAM_STATUS         0x0010
#define CSR_VPEN_STATUS            0x0008
#define CSR_PROGRAM_SUSPEND_STATUS 0x0004
#define CSR_DEVICE_PROTECT_STATUS  0x0002

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

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


static int
flash_write_uint16(volatile unsigned short *address, unsigned short data)
{
  unsigned short old = *address;
  *address = CMD_PROGRAM;
  *address = data | ~old;
  *address = CMD_READ_STATUS;
  while (!(*address & CSR_WSM_STATUS_READY));
  *address = CMD_CLEAR_STATUS;
  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)
{
  *address = CMD_BLOCK_ERASE;
  *address = CMD_CONFIRM;
  *address = CMD_READ_STATUS;
  while (!(*address & CSR_WSM_STATUS_READY));
  *address = CMD_CLEAR_STATUS;
  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 *)0x4C800000) |= 0x1;
} 
  
void
loaderEnd()
{     
  /* Disable FLASH writes */
  *((volatile unsigned char *)0x4C800000) &= ~0x1;
}     

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 highAddress = (unsigned int)address + length - 1;
  if (RANGE_OCCLUDES_RANGE((unsigned int)address, highAddress, FLASH_START_ADDRESS, FLASH_END_ADDRESS))
    {
      unsigned int sector = 0;
      const sectdef_t *sdptr = sectors;
      unsigned int sectorStartAddress = FLASH_START_ADDRESS;
      unsigned int i;

      while (sdptr->n)
        {
          for (i = sdptr->n; i ; --i)
            {
              unsigned int sectorEndAddress = sectorStartAddress + sdptr->size - 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((volatile unsigned short *)sectorStartAddress))
                    return 0;

                }
              sectorStartAddress = sectorEndAddress + 1;
              sector++;
            }
          sdptr++;
        }
    }
  return 1;
}

int
loaderEraseAll()
{
  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 (!is_erased(sectorStartAddress, sectorEndAddress))
            {
              /* Sector hasn't been erased and it needs to be, erase it. */
              if (!flash_erase_sector((volatile unsigned short *)sectorStartAddress))
                return 0;
            }
          sectorStartAddress = sectorEndAddress + 1;
        }
      sdptr++;
    }
  return 1;
}

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

