/*****************************************************************************
  R O Software AT91 SBC 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"

#define PIO_PER (*(volatile unsigned *)0xffff0000)
#define PIO_OER (*(volatile unsigned *)0xffff0010)
#define PIO_SODR (*(volatile unsigned *)0xffff0030)
#define PIO_CODR (*(volatile unsigned *)0xffff0034)

#define FLASH_START_ADDRESS 0x01000000
#define FLASH_LENGTH 0x00400000
#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_SEQ_ADD1              (0x5555)
#define FLASH_SEQ_ADD2              (0x2AAA)                          

#define FLASH_CODE1                 (0xAA)
#define FLASH_CODE2                 (0x55)
#define ID_IN_CODE                  (0x90)
#define ID_OUT_CODE                 (0xF0)
#define WRITE_CODE                  (0xA0)
#define ERASE_SECTOR_CODE1          (0x80)
#define ERASE_SECTOR_CODE2          (0x30)
#define CHIP_ERASE_CODE             (0x10)

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

static const sectdef_t sectors[] = 
{
  /* 8 * 8K Sectors */
  { 8, 0x2000 },
  /* 63 * 64K Sectors */
  { 63, 0x10000 },
  /* Terminator */
  {0, 0}
};

#define FLASH_WRITE(o, v) *(((volatile unsigned short *)FLASH_START_ADDRESS) + o) = v

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(volatile unsigned short *sectorAddress)
{
  unsigned int i;

  // Enter Sector Erase Sequence codes
  FLASH_WRITE(FLASH_SEQ_ADD1, FLASH_CODE1);
  FLASH_WRITE(FLASH_SEQ_ADD2, FLASH_CODE2);
  FLASH_WRITE(FLASH_SEQ_ADD1, ERASE_SECTOR_CODE1);
  FLASH_WRITE(FLASH_SEQ_ADD1, FLASH_CODE1);
  FLASH_WRITE(FLASH_SEQ_ADD2, FLASH_CODE2);

  *sectorAddress = ERASE_SECTOR_CODE2 ;
 
  while (*sectorAddress != 0xFFFF);

  return 1;
}                                       

static int
flash_erase_chip(void)
{
  int cmpt;

  // Enter Chip Erase Sequence codes
  FLASH_WRITE(FLASH_SEQ_ADD1, FLASH_CODE1);
  FLASH_WRITE(FLASH_SEQ_ADD2, FLASH_CODE2);
  FLASH_WRITE(FLASH_SEQ_ADD1, ERASE_SECTOR_CODE1);
  FLASH_WRITE(FLASH_SEQ_ADD1, FLASH_CODE1);
  FLASH_WRITE(FLASH_SEQ_ADD2, FLASH_CODE2);
  FLASH_WRITE(FLASH_SEQ_ADD1, CHIP_ERASE_CODE);

  while (*((volatile unsigned short *)FLASH_START_ADDRESS) != 0xFFFF);

  return 1;
}

static int
flash_erase(unsigned int 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;
}

static int 
flash_write_word(volatile unsigned short *addr, unsigned short value)
{
  FLASH_WRITE(FLASH_SEQ_ADD1, FLASH_CODE1);
  FLASH_WRITE(FLASH_SEQ_ADD2, FLASH_CODE2);
  FLASH_WRITE(FLASH_SEQ_ADD1, WRITE_CODE);
  *addr = value;
  while (*addr != value);
  return 1;
}

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

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

  return result;
}

void
loaderBegin()
{
  /* Disable FLASH protection */
  PIO_PER = 1 << 24;
  PIO_OER = 1 << 24; 
  PIO_SODR = 1 << 24;
}

void
loaderEnd()
{
  /* Re-enable FLASH protection */
  PIO_CODR = 1 << 24;
}

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_word((unsigned short *)address,  (unsigned short)data);
              address += 2;
              data >>= 16;
              flash_write_word((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)
{
  return flash_erase((unsigned int)address, length);
}

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

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

