/*****************************************************************************
  TI TMS470 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 FMBAC1 (*(volatile unsigned *)0xffe88000)
#define FMBAC2 (*(volatile unsigned *)0xffe88004)
#define FMBSEA (*(volatile unsigned *)0xffe88008)
#define FMBSEB (*(volatile unsigned *)0xffe8800c)
#define FMBRDY (*(volatile unsigned *)0xffe88010)
#define FMREGOPT (*(volatile unsigned *)0xffe89c00)
#define FMBBUSY (*(volatile unsigned *)0xffe89c08)
#define FMPKEY (*(volatile unsigned *)0xffe89c0c)
#define FMPRDY (*(volatile unsigned *)0xffe8a814)
#define FMMAC1 (*(volatile unsigned *)0xffe8bc00)
#define FMMAC2 (*(volatile unsigned *)0xffe8bc04)
#define FMPAGP (*(volatile unsigned *)0xffe8bc08)
#define FMMSTAT (*(volatile unsigned *)0xffe8bc0c)

#define GLBCTRL (*(volatile unsigned *)0xffffffdc)
#define DEV (*(volatile unsigned *)0xfffffff0)

#define FLASH_START_ADDRESS 0x10000000

#define KEY1 *(volatile unsigned *)(FLASH_START_ADDRESS + 0x1FF0)
#define KEY2 *(volatile unsigned *)(FLASH_START_ADDRESS + 0x1FF4)
#define KEY3 *(volatile unsigned *)(FLASH_START_ADDRESS + 0x1FF8)
#define KEY4 *(volatile unsigned *)(FLASH_START_ADDRESS + 0x1FFC)

#ifndef KEY1_VAL
#define KEY1_VAL 0xFFFFFFFF
#endif

#ifndef KEY2_VAL
#define KEY2_VAL 0xFFFFFFFF
#endif

#ifndef KEY3_VAL
#define KEY3_VAL 0xFFFFFFFF
#endif

#ifndef KEY4_VAL
#define KEY4_VAL 0xFFFFFFFF
#endif

#define FLASH_COMMAND_PROGRAM 0x0010
#define FLASH_COMMAND_ERASE_SECTOR 0x0020
#define FLASH_COMMAND_CLEAR_STATUS_REGISTER 0x0040

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

static const sectdef_t tms470r1a64_geometry[] =
{
  { 2, 0x00002000 },   /* 2 * 8K Sectors */
  { 3, 0x00004000 },   /* 3 * 16K Sectors */
  {0, 0}               /* Terminator */
};

static const sectdef_t tms470r1a128_geometry[] =
{
  { 2, 0x00002000 },   /* 2 * 8K Sectors */
  { 6, 0x00004000 },   /* 6 * 16K Sectors */
  { 2, 0x00002000 },   /* 2 * 8K Sectors */
  {0, 0}               /* Terminator */
};

static const sectdef_t tms470r1a256_geometry[] =
{
  { 4, 0x00002000 },   /* 4 * 8K Sectors */
  { 6, 0x00008000 },   /* 6 * 16K Sectors */
  { 4, 0x00002000 },   /* 4 * 8K Sectors */
  {0, 0}               /* Terminator */
};

static const sectdef_t *geometry = tms470r1a256_geometry;

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 unsigned short fms;

static int
flash_erase_sector(volatile unsigned short *sectorAddress)
{
  *sectorAddress = FLASH_COMMAND_ERASE_SECTOR;
  *sectorAddress = 0;
  while ((fms = FMMSTAT) & 0x100);
  return 1;
}                                       

static int
flash_erase(unsigned int address, unsigned int length)
{
  unsigned int highAddress = (unsigned int)address + length - 1;
  unsigned int sector = 0;
  const sectdef_t *sdptr = geometry;
  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_erase_all(void)
{
  const sectdef_t *sdptr = geometry;
  unsigned int sectorStartAddress = FLASH_START_ADDRESS;
  unsigned int i;
  while (sdptr->n)
    {
      for (i = sdptr->n; i ; --i)
        {
          if (!flash_erase_sector((volatile unsigned short *)sectorStartAddress))
            return 0;
          sectorStartAddress += sdptr->size;
        }
      sdptr++;
    }
  return 1;
}

static int 
flash_write_word(volatile unsigned short *addr, unsigned short value)
{
#ifndef PERMIT_KEY_WRITE
  if ((unsigned long)addr >= (FLASH_START_ADDRESS + 0x1FF0) && (unsigned long)addr <= (FLASH_START_ADDRESS + 0x1FFF))
    return 0;
#endif
  *addr = FLASH_COMMAND_PROGRAM;
  *addr = value;
  while ((fms = FMMSTAT) & 0x100);
  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()
{
  // FIXME: work out the FLASH geometry.
  unsigned short id = DEV & 0x0FFF;
  switch (id)
    {
      case 0x083F:
        // 128
        // 64...
        break;
      case 0x0857:
        // 256
        break;
    }

  /* Enable write access to FLASH registers */
  GLBCTRL |= 1 << 4;
  /* FLASH bank = 0 */
  FMMAC2 &= 0xFFF8;
  /* Enable writing to sector enable regs */
  FMMAC1 |= 1 << 15;
  /* Enable program/erase for sectors 0-31 */
  FMBSEA = 0xFFFF;
  FMBSEB = 0xFFFF;
  /* Disable writing to sector enable regs */
  FMMAC1 &= ~(1 << 15);
  /* Disable protection key level 2 */
  KEY1;
  FMPKEY = KEY1_VAL;
  KEY2;
  FMPKEY = KEY2_VAL;
  KEY3;
  FMPKEY = KEY3_VAL;
  KEY4;
  FMPKEY = KEY4_VAL;
  /* Make sure one-time programming is disabled */
  FMREGOPT &= ~(1 << 1);
}

void
loaderEnd()
{
  /* Enable writing to sector enable regs */
  FMMAC1 |= 1 << 15;
  /* Enable program/erase for sectors 0-31 */
  FMBSEA = 0x0000;
  FMBSEB = 0x0000;
  /* Disable writing to sector enable regs */
  FMMAC1 &= ~(1 << 15);
  /* Disable write access to FLASH registers */
  GLBCTRL &= ~(1 << 4);
}


static unsigned short
u16ByteSwap(unsigned short v)
{
  unsigned char *p = (unsigned char *)&v;
  unsigned char tmp = p[1];
  p[1] = p[0];
  p[0] = tmp;
  return v;
}

int
loaderPoke(unsigned char *address, unsigned int length)
{
  // Write aligned data
  if (((unsigned int)address & 1) == 0)
    {
      while (length >= 4)
        {
          unsigned int data = loaderReadWord();
          flash_write_word((unsigned short *)address,  u16ByteSwap((unsigned short)data));
          address += 2;
          data >>= 16;
          flash_write_word((unsigned short *)address,  u16ByteSwap((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--;
        }
    }
  return 1;
}

int
loaderMemset(unsigned char *address, unsigned int length,  unsigned char c)
{
  // Write aligned data
  if (((unsigned int)address & 1) == 0)
    {
      unsigned short cc = c | (c << 8);
      while (length >= 4)
        {
          flash_write_word((unsigned short *)address,  cc);
          address += 2;
          flash_write_word((unsigned short *)address,  cc);
          address += 2;
          length -= 4;
        }
    }
  // Write unaligned data
  while (length--)
    flash_write_byte(address++, c);
  return 1;
}

int
loaderErase(unsigned char *address, unsigned int length)
{
  return flash_erase((unsigned int)address, length);
}

int
loaderEraseAll()
{
  return 1;
}

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

