/*****************************************************************************
 * Loader for Aeroflex AX07CF192                                             *
 *                                                                           *
 * 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 FMAR (*(volatile unsigned long *)0x9000204)
#define FMCR (*(volatile unsigned long *)0x900020c)
#define FESR (*(volatile unsigned long *)0x9000210)
#define FMPR (*(volatile unsigned long *)0x9000214)

#define PROCESSOR_CLOCK_FREQUENCY 33000000
#define DELAY_CYCLES 5

#define delay_nS(n) delay(n * (PROCESSOR_CLOCK_FREQUENCY / 1000000000) / DELAY_CYCLES)
#define delay_uS(n) delay(n * (PROCESSOR_CLOCK_FREQUENCY / 1000000) / DELAY_CYCLES)
#define delay_mS(n) delay(n * (PROCESSOR_CLOCK_FREQUENCY / 1000) / DELAY_CYCLES)

#define SECTORS 8

const unsigned int sectorLengths[SECTORS] =
{
  0x2000,
  0x2000,
  0x6000,
  0x6000,
  0x8000,
  0x8000,
  0x8000,
  0x8000,
};

const unsigned int sectorAddresses[SECTORS] =
{
  0x08000000,
  0x08002000,
  0x08004000,
  0x0800A000,
  0x08010000,
  0x08018000,
  0x08020000,
  0x08028000,
};

static int
flash_write_uint16(unsigned short *address, unsigned short data)
{
  int n;
  for (n = 20; n; --n)
    {
      unsigned short readdata;
      /* Program */
      *address = data;
      FMCR = 0x05;
      delay_uS(10);
      FMCR = 0x00;
      delay_nS(200);
      *address = 0xFFFF;    
      /* Verify */
      delay_uS(2);
      FMAR = (unsigned int)address;
      delay_nS(200);
      FMPR = 0x02;
      delay_nS(200);
      FMCR = 0x10;
      delay_uS(3);
      readdata = *address;
      FMCR = 0x00;
      FMPR = 0x00;
      delay_uS(2);
      if (readdata == data)
        return 1;
    }
  return 0;
}

static int
flash_write_byte(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(int sector)
{
  int n;
  unsigned short *address = (unsigned short *)sectorAddresses[sector];
  unsigned short *endAddress = address + sectorLengths[sector] / 2;

  /* Pre-program sector */
  while (address != endAddress)
    {
      if (*address != 0 && !flash_write_uint16(address, 0))
        return 0;
      ++address;
    }

  /* Erase Sector */
  address = (unsigned short *)sectorAddresses[sector];
  for (n = 20; n; --n)
    {
      /* Erase */
      FMCR = 0x02;
      delay_uS(10);
      FESR = 1 << sector;
      FMCR = 0x0A;
      delay_mS(1);
      /* Verify */
      FMPR = 0x00;
      delay_nS(200);
      FMCR = 0x20;
      delay_nS(200);
      for (;;)
        {
          *address = 0xFFFF;
          if (*address == 0xFFFF)
            {
              ++address;
              if (address == endAddress)
                {
                  FMCR = 0x00;
                  delay_uS(2);
                  return 1;                  
                }
            }
          else
            break;
        }
    }
  return 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;
}

void
loaderBegin()
{
}

void
loaderEnd()
{
}

int
loaderPoke(unsigned char *address, unsigned int length)
{
  // Write aligned data
  int res = 1;
  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--;
        }
    }
  return res;
}

int
loaderMemset(unsigned char *address, unsigned int length,  unsigned char c)
{
  // 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;
    }
  return 1;
}

int
loaderErase(unsigned char *address, unsigned int length)
{
  unsigned int i;
  unsigned int highAddress = (unsigned int)address + length - 1;
  for (i = 0; i < SECTORS; ++i)
    {
      unsigned int sectorStartAddress = sectorAddresses[i];
      unsigned int sectorEndAddress = sectorStartAddress + sectorLengths[i] - 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(i))
            return 0;
        }
    }
  return 1;
}

int
loaderEraseAll()
{
  unsigned int i;
  for (i = 0; i < SECTORS; ++i)
    if (!flash_erase_sector(i))
      return 0;
  return 1;
}

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

