/*****************************************************************************
 * FLASH loader for EB01.                                                    *
 *                                                                           *
 * NOTE: This program is very tight on memory and will not run in debug      *
 * configuration.                                                            *
 *                                                                           *
 * 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_SECTORS 512
#define FLASH_SECTOR_SIZE 256
#define FLASH_SECTOR_MASK ~(FLASH_SECTOR_SIZE - 1)
#define FLASH_START_ADDRESS 0x01000000
#define FLASH_END_ADDRESS (FLASH_START_ADDRESS + FLASH_SECTORS * FLASH_SECTOR_SIZE)

#define FLASH_SEQ_ADD_1  (0x5555 << 1)
#define FLASH_SEQ_ADD_2  (0x2AAA << 1)

#define FLASH_WORD_COM_1 ((short)0xAAAA)
#define FLASH_WORD_COM_2 ((short)0x5555)
#define PROT_WORD_COM    ((short)0xA0A0)
#define ID_IN_WORD_COM   ((short)0x9090)
#define ID_OUT_WORD_COM   ((short)0xF0F0)

#define ADDRESS_IN_FLASH(address) ADDRESS_IN_RANGE(address, FLASH_START_ADDRESS, FLASH_END_ADDRESS)

#define BIT_MARK(a, n) a[n >> 3] |= 1 << (n % 8)
#define BIT_MARKED(a, n) (a[n >> 3] & (1 << (n % 8)))

static unsigned char sectorWritten[FLASH_SECTORS >> 3];

int
flashWriteSector(unsigned int address, int length, const unsigned char *src)
{
  unsigned int a = address;
  int l = length;
  const unsigned char *s = src;
  int count;

  *((volatile short *)(FLASH_START_ADDRESS + FLASH_SEQ_ADD_1)) = FLASH_WORD_COM_1;
  *((volatile short *)(FLASH_START_ADDRESS + FLASH_SEQ_ADD_2)) = FLASH_WORD_COM_2;
  *((volatile short *)(FLASH_START_ADDRESS + FLASH_SEQ_ADD_1)) = PROT_WORD_COM;

  if (a & 1)
    {
      *((volatile short *)(a & ~1)) = *s << 8 | 0xFF;
      ++a;
      --l;
      ++s;
    }

  while (l & ~1)
    {
      *((volatile short *)(a)) = *s | *(s + 1) << 8;
      a += 2;
      l -= 2;
      s += 2;
    }

  if (l)
    {
      *((volatile short *)(a)) = *s | 0xFF00;
    }

  count = 0;
  while (memcmp((void *)address, src, length) != 0)
    if (++count == 100000)
      return 0;

  return 1;
}

int
testMemset(const unsigned char *p, char c, int l)
{
  while (l)
    {
      if (*p > c)
        return 1;
      if (*p < c)
        return -1;
      l--;
      p++;
    }
  return 0;
}

int
flashSetSector(unsigned int address, int length, unsigned char c)
{
  unsigned int a = address;
  int l = length;
  int count;

  *((volatile short *)(FLASH_START_ADDRESS + FLASH_SEQ_ADD_1)) = FLASH_WORD_COM_1;
  *((volatile short *)(FLASH_START_ADDRESS + FLASH_SEQ_ADD_2)) = FLASH_WORD_COM_2;
  *((volatile short *)(FLASH_START_ADDRESS + FLASH_SEQ_ADD_1)) = PROT_WORD_COM;

  if (a & 1)
    {
      *((volatile short *)(a & ~1)) = c | 0xFF;
      ++a;
      --l;
    }

  while (l & ~1)
    {
      *((volatile short *)(a)) = c | c << 8;
      a += 2;
      l -= 2;
    }

  if (l)
    *((volatile short *)(a)) = c | 0xFF00;

  count = 0;
  while (testMemset((unsigned char *)address, c, length) != 0)
    if (++count == 100000)
      return 0;

  return 1;
}

int
flashMergeWriteSector(unsigned int sectorStartAddress, unsigned int address, int length, const unsigned char *src)
{
  unsigned char tmp[FLASH_SECTOR_SIZE];
  memcpy(tmp, (void *)sectorStartAddress, FLASH_SECTOR_SIZE);
  memcpy(tmp + (address - sectorStartAddress), src, length);
  return flashWriteSector(sectorStartAddress, FLASH_SECTOR_SIZE, tmp);  
}

int
flashMergeSetSector(unsigned int sectorStartAddress, unsigned int address, int length, unsigned char c)
{
  unsigned char tmp[FLASH_SECTOR_SIZE];
  memcpy(tmp, (void *)sectorStartAddress, FLASH_SECTOR_SIZE);
  memset(tmp + (address - sectorStartAddress), c, length);
  return flashWriteSector(sectorStartAddress, FLASH_SECTOR_SIZE, tmp);  
}

int
flashWriteBytes(unsigned int address, int length, const unsigned char *src)
{
  unsigned int sector = (address - FLASH_START_ADDRESS) / FLASH_SECTOR_SIZE;
  unsigned int sectorStartAddress = address & FLASH_SECTOR_MASK;

  // Sort out non-sector aligned start address.
  if (address != sectorStartAddress)
    {
      unsigned int frag = FLASH_SECTOR_SIZE - (address - sectorStartAddress);
      if (length < frag)
        frag = length;

      if (BIT_MARKED(sectorWritten, sector))
        {
          // Sector already written, merge writes.
          if (!flashMergeWriteSector(sectorStartAddress, address, frag, src))
            return 0;
        }
      else
        {
          // Sector has not already been written, just write what we have.
          if (!flashWriteSector(address, frag, src))
            return 0;
          BIT_MARK(sectorWritten, sector);
        }
      ++sector;
      address += frag;
      length -= frag;
      src += frag;
    }

  // Sort out sector aligned addresses and lengths
  while (length & FLASH_SECTOR_MASK)
    {
      if (!flashWriteSector(address, FLASH_SECTOR_SIZE, src))
        return 0;
      BIT_MARK(sectorWritten, sector);
      ++sector;
      address += FLASH_SECTOR_SIZE;
      length -= FLASH_SECTOR_SIZE;
      src += FLASH_SECTOR_SIZE;
    }

  // Sort out non-sector aligned lengths.
  if (length)
    {    
      if (BIT_MARKED(sectorWritten, sector))
        {
          // Sector already written, merge writes.
          if (!flashMergeWriteSector(address, address, length, src))
            return 0;
        }
      else
        {
          // Sector has not already been written, just write what we have.
          if (!flashWriteSector(address, length, src))
            return 0;
          BIT_MARK(sectorWritten, sector);
        }
    }
  return 1;
}

int
flashSetBytes(unsigned int address, int length, unsigned char c)
{
  unsigned int sector = (address - FLASH_START_ADDRESS) / FLASH_SECTOR_SIZE;
  unsigned int sectorStartAddress = address & FLASH_SECTOR_MASK;

  // Sort out non-sector aligned start address.
  if (address != sectorStartAddress)
    {
      unsigned int frag = FLASH_SECTOR_SIZE - (address - sectorStartAddress);
      if (length < frag)
        frag = length;

      if (BIT_MARKED(sectorWritten, sector))
        {
          // Sector already written, merge writes.
          if (!flashMergeSetSector(sectorStartAddress, address, frag, c))
            return 0;
        }
      else
        {
          // Sector has not already been written, just write what we have.
          if (!flashSetSector(address, frag, c))
            return 0;
          BIT_MARK(sectorWritten, sector);
        }
      ++sector;
      address += frag;
      length -= frag;
    }

  // Sort out sector aligned addresses and lengths
  while (length & FLASH_SECTOR_MASK)
    {
      if (!flashSetSector(address, FLASH_SECTOR_SIZE, c))
        return 0;
      BIT_MARK(sectorWritten, sector);
      ++sector;
      address += FLASH_SECTOR_SIZE;
      length -= FLASH_SECTOR_SIZE;
    }

  // Sort out non-sector aligned lengths.
  if (length)
    {    
      if (BIT_MARKED(sectorWritten, sector))
        {
          // Sector already written, merge writes.
          if (!flashMergeSetSector(address, address, length, c))
            return 0;
        }
      else
        {
          // Sector has not already been written, just write what we have.
          if (!flashSetSector(address, length, c))
            return 0;
          BIT_MARK(sectorWritten, sector);
        }
    }
  return 1;
}

void
loaderBegin()
{
}

void
loaderEnd()
{
}

int
loaderPoke(unsigned char *address, unsigned int length)
{
  if (ADDRESS_IN_FLASH((unsigned int)address))
    flashWriteBytes((unsigned int)address, length, (const unsigned char *)loaderGetReadPtr());
  return 1;
}

int
loaderMemset(unsigned char *address, unsigned int length,  unsigned char c)
{
  if(ADDRESS_IN_FLASH((unsigned int)address))
    flashSetBytes((unsigned int)address, length, c);
  return 1;
}

int
loaderErase(unsigned char *address, unsigned int length)
{
  // AT29LV1024 has no erase.
  return 1;
}

int
loaderEraseAll()
{
  // AT29LV1024 has no erase.
  return 1;
}

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

