/*****************************************************************************
 * Loader for Revely RMS101 SBC                                              *
 *                                                                           *
 * 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"
#include <targets/LH79520.h>

#define FLASH_START_ADDRESS 0x40000000
#define FLASH_LENGTH 0x400000
#define FLASH_END_ADDRESS FLASH_START_ADDRESS + FLASH_LENGTH

#define ADDRESS_IN_FLASH(a) ADDRESS_IN_RANGE((unsigned long)a, FLASH_START_ADDRESS, FLASH_END_ADDRESS)

#define CMD_READ_ARRAY       0xFFFF
#define CMD_DATA_WRITE       0x4040
#define CMD_BLOCK_ERASE      0x2020
#define CMD_FULL_CHIP_ERASE  0x3030
#define CMD_SUSPEND          0xB0B0
#define CMD_READ_STATUS      0x7070
#define CMD_CLEAR_STATUS     0x5050
#define CMD_CONFIRM          0xD0D0
#define CMD_RESUME           0xD0D0
#define CMD_READ_ID_CODE     0x9090
#define CMD_BLOCK_LOCK       0x6060
#define CMD_SET_LOCKBIT      0x0101
#define CMD_RESET_LOCKBIT    0xD0D0
#define CMD_SET_LOCKDOWNBIT  0x2F2F
#define CMD_SET_PCR          0x6060
#define CMD_PCR_CONFIRM      0x0404
#define CMD_OTP_WRITE        0xC0C0
#define CMD_MULTI_DATA_WRITE 0xE8E8

#define CSR_WSM_STATUS_READY 0x0080
#define CSR_ERASE_SUSPEND    0x0040
#define CSR_ERASE_ERROR      0x0020
#define CSR_WRITE_ERROR      0x0010
#define CSR_LOW_VPP          0x0008
#define CSR_WRITE_SUSPEND    0x0004
#define CSR_DEVICE_PROTECT   0x0002

#define SDRAM_BASE 0x20000000
#define SDRAM_INIT_NOP  0x03
#define SDRAM_INIT_MODE 0x02
#define SDRAM_INIT_PALL 0x01
#define SDRAM_INIT_NORMAL 0x0c
// Other bit fields
#define SDRAM_STATUS    0x20

#define SECTORS 71

const unsigned int sectorAddresses[SECTORS + 1] =
{
  0x000000, 0x002000, 0x004000, 0x006000, 0x008000, 0x00a000,
  0x00c000, 0x00e000, 0x010000, 0x020000, 0x030000, 0x040000,
  0x050000, 0x060000, 0x070000, 0x080000, 0x090000, 0x0a0000,
  0x0b0000, 0x0c0000, 0x0d0000, 0x0e0000, 0x0f0000, 0x100000,
  0x110000, 0x120000, 0x130000, 0x140000, 0x150000, 0x160000,
  0x170000, 0x180000, 0x190000, 0x1a0000, 0x1b0000, 0x1c0000,
  0x1d0000, 0x1e0000, 0x1f0000, 0x200000, 0x210000, 0x220000,
  0x230000, 0x240000, 0x250000, 0x260000, 0x270000, 0x280000,
  0x290000, 0x2a0000, 0x2b0000, 0x2c0000, 0x2d0000, 0x2e0000,
  0x2f0000, 0x300000, 0x310000, 0x320000, 0x330000, 0x340000,
  0x350000, 0x360000, 0x370000, 0x380000, 0x390000, 0x3a0000,
  0x3b0000, 0x3c0000, 0x3d0000, 0x3e0000, 0x3f0000,
  0x400000 
};

static int
flash_write_uint16(volatile unsigned short *address, unsigned short data)
{
  unsigned short old = *address;
  *address = CMD_DATA_WRITE;
  *address = data | ~old;
  *address = CMD_READ_STATUS;
  while (!(*address & CSR_WSM_STATUS_READY));
  *address = CMD_CLEAR_STATUS;
  return 1;
}

static int
flash_write_byte(volatile 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(volatile unsigned short *address)
{
  *address = CMD_BLOCK_ERASE;
  *address = CMD_CONFIRM;
  *address = CMD_READ_STATUS;
  while (!(*address & CSR_WSM_STATUS_READY));
  *address = CMD_CLEAR_STATUS;
  return 1;
}

static int
flash_erase_all(unsigned short *address)
{
  *address = CMD_FULL_CHIP_ERASE;
  *address = CMD_CONFIRM;
  *address = CMD_READ_STATUS;
  while (!(*address & CSR_WSM_STATUS_READY));
  *address = CMD_CLEAR_STATUS;
  return 1;
}

static int
is_erased(volatile unsigned int address, volatile unsigned int highAddress)
{
  unsigned int length = highAddress - address + 1;
  while (address & 3)
    {
      if (*(volatile unsigned char *)address != 0xFF)
        return 0;
      length--;
      address++;
    }
  while (length > 4)
    {
      if (*(volatile unsigned long *)address != 0xFFFFFFFF)
        return 0;
      length -= 4;
      address += 4;
    }
  while (length)
    {
      if (*(volatile unsigned char *)address != 0xFF)
        return 0;
      length--;
      address++;
    }
  return 1;
}

static void 
delay(unsigned long wDuration)
{
  volatile unsigned long wTemp, wTime;
  for (wTime=0; wTime < wDuration; wTime++)
    for (wTemp=0 ; wTemp < 500; wTemp++);
}

static void
initSDRAM()
{
  volatile int tmp;
  MemMux |= 0x00ff; /* Assign necessary I/Os to SDRC */
  AHBClkCtrl &= 0xfffffffd; /* Enable HCLK feed to SDRC */
  delay(200);
  SDRCConfig1 = SDRAM_INIT_NOP;
  SDRCConfig1 = SDRAM_INIT_NOP;
  delay(200); /* Allow sdram clocks to settle */
  SDRCConfig1 = SDRAM_INIT_PALL; /* Issue a "pre-charge all" command */
  delay(250);
  SDRCRefTimer = 16; /* refresh every 16 clock cycles */
  delay(250); /* Delay for at least two auto refresh cycles */
  SDRCRefTimer = 800; /* Assumes 52 MHz default clock and 16us refresh period */
  delay(250);
  SDRCConfig1 = SDRAM_INIT_MODE; /* Select mode register update mode */  
  tmp = *((volatile int *)(SDRAM_BASE + ((unsigned int) 0x22 << 12)));
  while (SDRCConfig1 & SDRAM_STATUS); /* Wait until idle */
  SDRCConfig0 = 0x01a40088; /* Latency = 2, 32 bits, 4 banks */
  while (SDRCConfig1 & SDRAM_STATUS); /* Wait until idle */
  SDRCConfig1 = SDRAM_INIT_NORMAL; /* Select normal operating mode */
  while (SDRCConfig1 & SDRAM_STATUS); /* Wait until idle */
  delay(128);
}

void
loaderBegin()
{
  CpuClkPrescale = 0x0003;                     // cpuclk is 50Mhz
  HclkPrescale = 0x0003;                       // hclk is 50Mhz
  RCPCCtrl = 0x00000263;  // start PLL

  // Config Flash SMC for normal operation
  SMCBCR0 = (6 << 5) + 0x400 + (6 << 11) + 0x10000000;

  initSDRAM();
}

void
loaderEnd()
{
}

int
loaderPoke(unsigned char *address, unsigned int length)
{
  int res = 1;
  if (ADDRESS_IN_FLASH(address))
    {
      // Write aligned data
      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--;
            }
        }
    }
  else
    {
      while(length)
        {
          unsigned int data = loaderReadWord();
          int i;
          for(i = 4; i && length; --i)
            {
              *address++ = (unsigned char)data;
              data >>= 8;
              length--;
            }
        }
    }
  return res;
}

int
loaderMemset(unsigned char *address, unsigned int length,  unsigned char c)
{
  if (ADDRESS_IN_FLASH(address))
    {
      // 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;
        }
    }
  else
    {
      while(length--)
        *address++ = (unsigned char)c;
    }
  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 = FLASH_START_ADDRESS + sectorAddresses[i];
      unsigned int sectorEndAddress = FLASH_START_ADDRESS + sectorAddresses[i + 1] - 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((unsigned short *)sectorStartAddress))
            return 0;
        }
    }
  return 1;
}

int
loaderEraseAll()
{
  return flash_erase_all((unsigned short *)FLASH_START_ADDRESS);
}

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

