/*****************************************************************************
 * Loader for ATMEL AT91SAM7                                                 *
 *                                                                           *
 * Copyright (c) 2004 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/AT91SAM7.h>

#define MAX_FLASH_SECTOR_SIZE 256

#define FLASH_START_ADDRESS   0x00100000
#define FLASH_INVALID_ADDRESS (FLASH_START_ADDRESS - 1)

#define MC_CORRECT_KEY ((unsigned int)0x5A << 24)
#define MC_FCMD_WRITE_PAGE 0x01
#define MC_FCMD_ERASE_ALL 0x08

#include "__armlib.h"

#define WAIT_FLASH_READY while ((MC_FSR & 1) != 1);

static unsigned char currentSectorData[MAX_FLASH_SECTOR_SIZE];
static unsigned int currentSectorAddress = FLASH_INVALID_ADDRESS;
static unsigned int flashSectorSize;
static unsigned int flashSectors;
static unsigned int flashEndAddress;
static unsigned int flashSectorAddressMask;

static int
is_erased(unsigned int address, unsigned int length)
{
  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;
}

void
flash_write_page(unsigned int address)
{
  int i = flashSectorSize / 4;
  const unsigned int *src = (const unsigned int *)currentSectorData;
  unsigned int *dest = (unsigned int *)address;
  unsigned int page = ((address - FLASH_START_ADDRESS) / flashSectorSize) & 0x3FF;
  while (i--)
    *dest++ = *src++;
  MC_FCR = MC_CORRECT_KEY | page << 8 | MC_FCMD_WRITE_PAGE;
  WAIT_FLASH_READY;
}

static int
writeCurrentSector()
{
  if (currentSectorAddress != FLASH_INVALID_ADDRESS)
    {
      // If current sector is in FLASH, write it.
      if (ADDRESS_IN_RANGE(currentSectorAddress, FLASH_START_ADDRESS, flashEndAddress))
        flash_write_page(currentSectorAddress);
      currentSectorAddress = FLASH_INVALID_ADDRESS;
    }
  return 1;
}

static int
setOscillatorFrequency(unsigned int freq)
{
  MC_FMR = ((freq / 1000000) << 16) | 0x100;
}

void
loaderBegin()
{
  unsigned long nvpsiz = (DBGU_CIDR >> 8) & 0xF;
  switch (nvpsiz)
    {
      case 3: /* NVPSIZ == 32K - AT91SAM7S32 */
        flashSectors = 256;
        flashSectorSize = 128;
        break;
      case 5:
      default:
        /* NVPSIZ == 64K - AT91SAM7S64 */
        flashSectors = 512;
        flashSectorSize = 128;
        break;
      case 7:
        /* NVPSIZ == 128K - AT91SAM7S128*/
        flashSectors = 512;
        flashSectorSize = 256;
        break;
      case 9:
        /* NVPSIZ == 256K - AT91SAM7S256 */
        flashSectors = 1024;
        flashSectorSize = 256;
        break;
    }
  flashEndAddress = flashSectors * flashSectorSize + FLASH_START_ADDRESS - 1;
  flashSectorAddressMask = ~(flashSectorSize - 1);

  /* Set default value for oscillator frequency, this may overridden from host
     by setting a loader parameter. */ 
  setOscillatorFrequency(50000000);
}

void
loaderEnd()
{
}

void
loaderFlushWrites()
{
  writeCurrentSector();
}

int
loaderPoke(unsigned char *address, unsigned int length)
{
  while (length)
    {
      unsigned long data = loaderReadWord();
      int i;
      for(i = 4; i && length; --i)
        {
          unsigned int sectorAddress = (unsigned int)address & flashSectorAddressMask;
          if (sectorAddress != currentSectorAddress)
            {
              writeCurrentSector();
              memcpy(currentSectorData, sectorAddress, flashSectorSize);
              currentSectorAddress = sectorAddress;
            }
          currentSectorData[(unsigned int)address - currentSectorAddress] = (unsigned char)data;
          data >>= 8;
          --length;
          ++address;
        }
    }
  return 1;
}

int
loaderMemset(unsigned char *address, unsigned int length,  unsigned char c)
{
  while (length)
    {
      int offset, frag;
      unsigned int sectorAddress = (unsigned int)address & flashSectorAddressMask;
      if (sectorAddress != currentSectorAddress)
        {
          writeCurrentSector();
          memset(currentSectorData, 0xFF, flashSectorSize);
          currentSectorAddress = sectorAddress;
        }
      offset = (unsigned int)address - currentSectorAddress;
      frag = flashSectorSize - offset;
      if (length < frag)
        frag = length;
      memset(currentSectorData + offset, c, frag);
      address += frag;
      length -= frag;
    }
  return 1;
}

int
loaderEraseAll()
{
  MC_FCR = MC_CORRECT_KEY | MC_FCMD_ERASE_ALL;
  WAIT_FLASH_READY;
  return 1;
}

int
loaderErase(unsigned char *address, unsigned int length)
{
  static int erased = 0;
  if (!erased)
    {
      loaderEraseAll();
      erased = 1;
    }
  return 1;
}

int
loaderSetParameter(unsigned int parameter, unsigned int value)
{
  unsigned int result = 1;
  switch (parameter)
    {
      case 0:
        setOscillatorFrequency(value);
        break;
      default:
        break;
    }
  return result;
}

