/******************************************************************************  
  Copyright (c) 2009 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 <targets/LPC3131.h>
#include <libmem.h>
#include <libmem_loader.h>
#include <string.h>
#include "spi_flash.h"
#define ISRAMFLASHPAGES ((192*1024)/FLASHPAGESIZE)
unsigned char current_block[FLASHPAGESIZE];
unsigned current_block_size;
uint8_t *current_block_address;
libmem_geometry_t geometry0[2] =
{
  { ISRAMFLASHPAGES, FLASHPAGESIZE }, // ISRAM 
  { 0, 0 }
};
libmem_geometry_t geometry1[2] =
{
  { NUMFLASHPAGES-ISRAMFLASHPAGES, FLASHPAGESIZE}, // SDRAM
  { 0, 0 }
};
libmem_driver_functions_t driver_functions;
libmem_ext_driver_functions_t ext_driver_functions;
libmem_driver_handle_t libmem_handle0, libmem_handle1;  
#define GEOM0_START ((uint8_t *)0x11028000)
#define GEOM0_SIZE (ISRAMFLASHPAGES*FLASHPAGESIZE)
#define GEOM1_START (uint8_t *)0x30000000
#define GEOM1_SIZE ((NUMFLASHPAGES-ISRAMFLASHPAGES)*FLASHPAGESIZE)
unsigned geom0_lowest=0xffffffff, geom0_highest=0;
unsigned geom1_lowest=0xffffffff, geom1_highest=0;

static int
dataflash_get_page_number(const uint8_t *start)
{
  if (start >= GEOM0_START && start < GEOM0_START+GEOM0_SIZE)
    {
      return (start-GEOM0_START)/FLASHPAGESIZE;
    }
  else
    {
      return (start-GEOM1_START)/FLASHPAGESIZE+ISRAMFLASHPAGES;
    }
}

static uint8_t *
dataflash_get_address(unsigned page_number)
{
  if (page_number < ISRAMFLASHPAGES)
    return GEOM0_START+page_number*FLASHPAGESIZE;
  else
    return GEOM1_START+page_number*FLASHPAGESIZE;
}

static int
dataflash_erase_impl(libmem_driver_handle_t *h, uint8_t *start, size_t size,
                    uint8_t **erase_start, size_t *erase_size)
{   
  unsigned erase_start_flash_page = dataflash_get_page_number(start);
  unsigned flash_page = erase_start_flash_page;
  while (size)
    {      
      spi_flash_erase_page(flash_page);
      flash_page++;
      if (size > FLASHPAGESIZE)                                    
        size -= FLASHPAGESIZE;        
      else
        size = 0;        
    }  
  if (erase_start)
    *erase_start = dataflash_get_address(erase_start_flash_page);
  if (erase_size)
    *erase_size = ((flash_page-erase_start_flash_page)+1)*FLASHPAGESIZE;
  return LIBMEM_STATUS_SUCCESS;
}

static void
write_current_block()
{
  unsigned flash_page = dataflash_get_page_number(current_block_address);
  spi_flash_write_page(flash_page, current_block);  

  if (flash_page < ISRAMFLASHPAGES)
    {
      if ((unsigned)current_block_address < geom0_lowest)
        geom0_lowest = (unsigned)current_block_address;
      if ((unsigned)current_block_address > geom0_highest)
        geom0_highest = (unsigned)current_block_address;
    }
  else
    {
      if ((unsigned)current_block_address < geom1_lowest)
        geom1_lowest = (unsigned)current_block_address;
      if ((unsigned)current_block_address > geom1_highest)
        geom1_highest = (unsigned)current_block_address;
    }
  current_block_address = 0;
  current_block_size = 0;
}

static int
dataflash_write_impl(libmem_driver_handle_t *h, uint8_t *dest, const uint8_t *src, size_t size)
{
  while (size)
    {
      if (current_block_address+current_block_size==dest)
        {
          unsigned s;
          if (size < (FLASHPAGESIZE-current_block_size))
            s = size;
          else
            s = FLASHPAGESIZE-current_block_size;
          memcpy(current_block+current_block_size, src, s);
          current_block_size += s;
          dest += s;
          src += s;
          size -= s;          
          if (current_block_size == FLASHPAGESIZE)
            write_current_block();
        }
      else if (current_block_address)
        write_current_block();
      else        
        current_block_address = dest;
    }
  return LIBMEM_STATUS_SUCCESS;
}

static int
dataflash_flush_impl(libmem_driver_handle_t *h)
{
  if (current_block_size)
    write_current_block();
  return LIBMEM_STATUS_SUCCESS;
}

static int 
dataflash_read_impl(libmem_driver_handle_t *h, uint8_t *dest, const uint8_t *src, size_t size)
{
  unsigned flash_page = dataflash_get_page_number(src);
  while (size)
    {    
      spi_flash_read_page(flash_page, current_block);
      if (size > FLASHPAGESIZE)
        {
          memcpy(dest, current_block, FLASHPAGESIZE);
          dest += FLASHPAGESIZE;          
          flash_page++;
          size -= FLASHPAGESIZE;
        }
      else
        {
          memcpy(dest, current_block, size);
          size = 0;
        }
    }  
  return LIBMEM_STATUS_SUCCESS;
}

static uint32_t
dataflash_crc32_impl(libmem_driver_handle_t *h, const uint8_t *start, size_t size, uint32_t crc)
{  
  while (size)
    {    
      unsigned flash_page = dataflash_get_page_number(start);
      spi_flash_read_page(flash_page, current_block);
      if ((unsigned)start % FLASHPAGESIZE)
        {
          unsigned offset = (unsigned)start % FLASHPAGESIZE;
          crc = libmem_crc32_direct(current_block+offset, FLASHPAGESIZE-offset, crc); 
          start += FLASHPAGESIZE-offset;
          size -= FLASHPAGESIZE-offset;
        }
      else if (size > FLASHPAGESIZE)
        {
          crc = libmem_crc32_direct(current_block, FLASHPAGESIZE, crc);                    
          start += FLASHPAGESIZE;
          size -= FLASHPAGESIZE;
        }
      else
        {
          crc = libmem_crc32_direct(current_block, size, crc); 
          size = 0;
        }
    }  
  return crc;
}

extern unsigned __ISRAM_segment_used_end__, __ISRAM_segment_end__;
extern unsigned secondary_loader_begin, secondary_loader_end;

int
main(void)
{  
  int res = LIBMEM_STATUS_SUCCESS;
  if (!spi_flash_init())
    {
      libmem_rpc_loader_exit(LIBMEM_STATUS_ERROR, "SPI flash init failed");
      return 0;
    }   

  driver_functions.write = dataflash_write_impl;
  driver_functions.fill = 0;      
  driver_functions.erase = dataflash_erase_impl;   
  driver_functions.lock = 0; 
  driver_functions.unlock = 0;  
  driver_functions.flush = dataflash_flush_impl;  

  ext_driver_functions.inrange = 0;
  ext_driver_functions.read = dataflash_read_impl;
  ext_driver_functions.crc32 = dataflash_crc32_impl;
 
  current_block_size = 0;
  current_block_address = 0;

  libmem_register_driver(&libmem_handle0, GEOM0_START, GEOM0_SIZE, geometry0, 0, &driver_functions, &ext_driver_functions);
  libmem_register_driver(&libmem_handle1, GEOM1_START, GEOM1_SIZE, geometry1, 0, &driver_functions, &ext_driver_functions);

#if 0
  {   
    static unsigned buffer[129];
    unsigned i;
    int res;

    //for (i=0;i<sizeof(buffer)/4;i++) 
    //  buffer[i] = i;
    //res = libmem_write((uint8_t *)0x1102C000, (uint8_t *)buffer, sizeof(buffer));   
    //res = libmem_write((uint8_t *)0x30000000, (uint8_t *)buffer, sizeof(buffer));    
    //res = libmem_flush();
      
    //for (i=0;i<sizeof(buffer)/4;i++) 
    //  buffer[i] = 0;
    //res = libmem_write((uint8_t *)0x11029000, buffer, sizeof(buffer));
    //res = libmem_flush();
    res = libmem_read( (uint8_t *)buffer, (uint8_t *)0x11028000, sizeof(buffer));  
    res = libmem_flush();
  }
#endif

  res = libmem_rpc_loader_start(&__ISRAM_segment_used_end__, &__ISRAM_segment_end__ - 1);
  if (res == LIBMEM_STATUS_SUCCESS) // write the secondary boot loader
    {
      unsigned *cust_reserved = &secondary_loader_begin+12;
      if (geom0_lowest!=0xffffffff)
        {
          *cust_reserved++ = geom0_lowest;
          *cust_reserved++ = (geom0_lowest-(unsigned)GEOM0_START)/FLASHPAGESIZE;       
          *cust_reserved++ = (geom0_highest-geom0_lowest)/FLASHPAGESIZE+1;
        }
      if (geom1_lowest!=0xffffffff)
        {
          *cust_reserved++ = geom1_lowest;
          *cust_reserved++ = (geom1_lowest-(unsigned)GEOM1_START)/FLASHPAGESIZE+ISRAMFLASHPAGES;
          *cust_reserved++ = (geom1_highest-geom1_lowest)/FLASHPAGESIZE+1;
        }
      libmem_write(GEOM0_START, (unsigned char *)&secondary_loader_begin, (unsigned char *)&secondary_loader_end - (unsigned char *)&secondary_loader_begin);  
      libmem_flush();
    }
  libmem_rpc_loader_exit(res, 0);
  return 0;
}