/*-----------------------------------------------------------------------
 * This function assumes that flash_addr is aligned on 512 bytes boundary
 * in flash. This function also assumes that prepare have been called
 * for the sector in question.
 */
int lpc2292_copy_buffer_to_flash(flash_info_t * info, ulong flash_addr)
{
	int first_sector;
	int last_sector;

	first_sector = get_flash_sector(info, flash_addr);
	last_sector = get_flash_sector(info, flash_addr + 512 - 1);

	/* prepare sectors for write */
	command[0] = IAP_CMD_PREPARE;
	command[1] = first_sector;
	command[2] = last_sector;
	iap_entry(command, result);
	if (result[0] != IAP_RET_CMD_SUCCESS) {
		printf("IAP prepare failed\n");
		return ERR_PROG_ERROR;
	}

	command[0] = IAP_CMD_COPY;
	command[1] = flash_addr;
	command[2] = COPY_BUFFER_LOCATION;
	command[3] = 512;
	command[4] = CONFIG_SYS_SYS_CLK_FREQ >> 10;
	iap_entry(command, result);
	if (result[0] != IAP_RET_CMD_SUCCESS) {
		printf("IAP copy failed\n");
		return 1;
	}

	return 0;
}

static int32_t pflash_program_array(FlashInfo* flash,
                                    uint32_t dest,
                                    uint32_t src) {
  uint32_t command[5], result[3];
  IAP iap_entry;

  iap_entry = (IAP) IAP_LOCATION;
  
  /* prepare page/sector */
  command[0] = IAP_PREPARE_SECTORS;
  command[1] = flash->page_nr;
  command[2] = flash->page_nr;
  disableIRQ();
  iap_entry(command, result);
  enableIRQ();
  if (result[0] != 0) return result[0];
  
  /* flash from ram */
  command[0] = IAP_COPY_RAM_TO_FLASH;
  command[1] = dest;
  command[2] = src;
  command[3] = BOUND;
  command[4] = CCLK/1000;
  disableIRQ();
  iap_entry(command, result);
  enableIRQ();
  if (result[0] != 0) return result[0];
  
  return 0;
}

static int32_t pflash_erase_page(FlashInfo* flash) {
  uint32_t command[5], result[3];
  IAP iap_entry;

  iap_entry = (IAP) IAP_LOCATION;

  /* prepare page/sector */
  command[0] = IAP_PREPARE_SECTORS;
  command[1] = flash->page_nr;
  command[2] = flash->page_nr;
  disableIRQ();
  iap_entry(command, result);
  enableIRQ();
  if (result[0] != 0) return result[0];
  
  /* erase page/sector */
  command[0] = IAP_ERASE_SECTORS;
  command[1] = flash->page_nr;
  command[2] = flash->page_nr;
  disableIRQ();
  iap_entry(command, result);
  enableIRQ();
  if (result[0] != 0) return result[0];

  /* verify erase */
  command[0] = IAP_BLANK_CHECK_SECTORS;
  command[1] = flash->page_nr;
  command[2] = flash->page_nr;
  iap_entry(command, result);
  if (result[0] != 0) return result[0];
  
  return 0;
}

uint8_t write_nonvolatile_parameters(void){
	unsigned int command[5];
	unsigned int result[4];
/* First we will prepare the sector for erase. */
	command[0] = PREP_WR;
	command[1] = FLASHSEC;
	command[2] = FLASHSEC;
	__disable_irq();
	iap_entry (command, result);
	__enable_irq();
	if (result[0] != 0) {
	/* This is an error. */
		result[0] = 0xff;
		return 0;
	}
	/* Next we will erase the sector. */
	command[0] = ERASE_SEC;
	command[1] = FLASHSEC;
	command[2] = FLASHSEC;
	command[3] = SystemFrequency / 1000;
	command[4] = 0;
	__disable_irq();
	iap_entry (command, result);
	__enable_irq();
	if (result[0] != 0) {
		/* This is an error. */
		result[0] = 0xff;
		return 0;
	}

	/* We will prepare the sector for write. */
	command[0] = PREP_WR;
	command[1] = FLASHSEC;
	command[2] = FLASHSEC;
	__disable_irq();
	iap_entry (command, result);
	__enable_irq();
	if (result[0] != 0) {
		/* This is an error. */
		result[0] = 0xff;
		return 0;
	}
	/* Then we will write the sector. */
	command[0] = FLASH_WR;
	command[1] = FLASHDIR;
	command[2] = (uint32_t)&Flash;
	command[3] = 256;
	command[4] = SystemFrequency / 1000;
	__disable_irq();
	iap_entry (command, result);
	__enable_irq();
	if (result[0] != 0) {
		/* This is an error. */
		result[0] = 0xff;
		return 0;
	}
	return 1;
}

/*
  Prepares sector(s) for writing / erasing
  Must be executed before copy_ram_flash() or erase_sector()
    int start => start sector
    int end   => end sector (use same as start to prepare a single sector)
*/
void prepare_sector_write(int start, int end) {
    command[0] = 50; // command code
    command[1] = (unsigned) start;
    command[2] = (unsigned) end;

    iap_entry(command, output);
}

/* This function resets some microcontroller peripherals to reset
 hardware configuration to ensure that the In-System Programming module
 will work properly. ISP is normally called from reset and assumes some reset
 configuration settings for the MCU.
 Some of the peripheral configurations may be redundant in your specific
 project.
 #define SystemFrequency (SystemCoreClock*LPC_SYSCTL->SYSAHBCLKDIV)
 */
void ReinvokeISP(void) {
	uint32_t command[5];
	uint32_t result[4];

	/* make sure 32-bit Timer 1 is turned on before calling ISP */
	LPC_SYSCTL->SYSAHBCLKCTRL |= 0x00400;
	/* make sure GPIO clock is turned on before calling ISP */
	LPC_SYSCTL->SYSAHBCLKCTRL |= 0x00040;
	/* make sure IO configuration clock is turned on before calling ISP */
	LPC_SYSCTL->SYSAHBCLKCTRL |= 0x10000;
	/* make sure AHB clock divider is 1:1 */
	LPC_SYSCTL->SYSAHBCLKDIV = 1;

	/* Send Reinvoke ISP command to ISP entry point*/
	command[0] = 57;

	/* Set stack pointer to ROM value (reset default) This must be the last
	 piece of code executed before calling ISP, because most C expressions
	 and function returns will fail after the stack pointer is changed. */
	__set_MSP(*((uint32_t *) 0x1FFF0000)); /* inline asm function */

	/* Invoke ISP. We call "iap_entry" to invoke ISP because the ISP entry is done
	 through the same command interface as IAP. */
	iap_entry(command, result);
	// Not supposed to come back!
}

uint8_t check_valid_code()
{
	uint32_t *p, check,i;

	param_table[0] = BLANK_CHECK_SECTOR;
	param_table[1] = FLASH_START_SECTOR;
	param_table[2] = FLASH_START_SECTOR;
	iap_entry(param_table,result_table);

	if( result_table[0] == 0 ) {
		return 0;
	}

	check = 0;
	p = (uint32_t *)FLASH_START_ADDR;

	for (i = 0; i < 8; i++) {
		check += *p;
		p++;
	}

	if(check != 0) {
		return 0;
	}
	return 1;
}

uint8_t EEPROM_Write(uint16_t AddressToWrite, void* DataArray, uint16_t BytesToWrite)
{
	IAP_Result[0] = 0;

	//TODO: check to make sure the address is valid

#ifdef USE_EEPROM_LIB
	IAP_Command[0] = EELIB_IAP_COMMAND_EEPROM_WRITE;	//EEPROM library write command
#else
	IAP_Command[0] = IAP_EEPROM_WRITE;					//IAP EEPROM write command
#endif
	IAP_Command[1] = (uint32_t)AddressToWrite;			//EEPROM address to write to
	IAP_Command[2] = (uint32_t)DataArray;				//RAM address of the data to write
	IAP_Command[3] = (uint32_t)BytesToWrite;			//Number of bytes to write
	IAP_Command[4] = SystemCoreClock/1000;				//System clock frequency in kHz

#ifdef USE_EEPROM_LIB
	EELIB_entry(IAP_Command, IAP_Result);
#else
	vPortEnterCritical();
	iap_entry(IAP_Command, IAP_Result);
	vPortExitCritical();
#endif

	return (uint8_t)IAP_Result[0];
}

int lpc2292_flash_erase (flash_info_t * info, int s_first, int s_last)
{
	int flag;
	int prot;
	int sect;

	prot = 0;
	for (sect = s_first; sect <= s_last; ++sect) {
		if (info->protect[sect]) {
			prot++;
		}
	}
	if (prot)
		return ERR_PROTECTED;


	flag = disable_interrupts();

	printf ("Erasing %d sectors starting at sector %2d.\n"
	"This make take some time ... ",
	s_last - s_first + 1, s_first);

	command[0] = IAP_CMD_PREPARE;
	command[1] = s_first;
	command[2] = s_last;
	iap_entry(command, result);
	if (result[0] != IAP_RET_CMD_SUCCESS) {
		printf("IAP prepare failed\n");
		return ERR_PROTECTED;
	}

	command[0] = IAP_CMD_ERASE;
	command[1] = s_first;
	command[2] = s_last;
	command[3] = CONFIG_SYS_SYS_CLK_FREQ >> 10;
	iap_entry(command, result);
	if (result[0] != IAP_RET_CMD_SUCCESS) {
		printf("IAP erase failed\n");
		return ERR_PROTECTED;
	}

	if (flag)
		enable_interrupts();

	return ERR_OK;
}

void prepare_sector(unsigned start_sector,unsigned end_sector,unsigned cclk)
{
  param_table[0] = PREPARE_SECTOR_FOR_WRITE;
  param_table[1] = start_sector;
  param_table[2] = end_sector;
  param_table[3] = cclk;
  iap_entry(param_table,result_table);
}

void erase_sector(unsigned start_sector, unsigned end_sector, unsigned cclk)
{
  param_table[0] = ERASE_SECTOR;
  param_table[1] = start_sector;
  param_table[2] = end_sector;
  param_table[3] = cclk;
  iap_entry(param_table,result_table);
}

/*
  Compare 2 memory locations
    uint16_t *src_addr  => ram or flash address of bytes to be compared
    uint16_t *dest_addr => ram or flash address of bytes to be compared
    int size        => number of bytes to be compared
                       must be a multiple of 4
*/
void compare_flash_ram(uint16_t *src_addr, uint16_t *dest_addr, int size) {
    command[0] = 56; // command code
    command[1] = (unsigned) dest_addr;
    command[2] = (unsigned) src_addr;
    command[3] = (unsigned) size;

    iap_entry(command,output);
}

int IAP::read_serial( void ) {
    IAP_command[ 0 ]    = IAPCommand_Read_device_serial_number;
    
    iap_entry( IAP_command, IAP_result );
    
    //  return ( (int)IAP_result[ 0 ] );
    return ( (int)IAP_result[ 1 ] );    //  to return the number itself (this command always returns CMD_SUCCESS)
}

/* Reinvoke ISP */
uint8_t Chip_IAP_ReinvokeISP()
{
    unsigned int command[5], result[4];

	command[0] = IAP_REINVOKE_ISP_CMD;
	iap_entry(command, result);

	return result[0];
}

int IAP::blank_check( int start, int end ) {
    IAP_command[ 0 ]    = IAPCommand_Blank_check_sector;
    IAP_command[ 1 ]    = (unsigned int)start;  //  Start Sector Number
    IAP_command[ 2 ]    = (unsigned int)end;    //  End Sector Number (should be greater than or equal to start sector number)

    iap_entry( IAP_command, IAP_result );

    return ( (int)IAP_result[ 0 ] );
}

/* Read the unique ID */
uint32_t Chip_IAP_ReadUID()
{
    unsigned int command[5], result[4];

	command[0] = IAP_READ_UID_CMD;
	iap_entry(command, result);

	return result[1];
}

void write_data(unsigned cclk,unsigned flash_address,unsigned * flash_data_buf, unsigned count)
{
    param_table[0] = COPY_RAM_TO_FLASH;
    param_table[1] = flash_address;
    param_table[2] = (unsigned)flash_data_buf;
    param_table[3] = count;
    param_table[4] = cclk;
    iap_entry(param_table,result_table);
}

/* Read part identification number */
uint32_t Chip_IAP_ReadPID()
{
	uint32_t command[5], result[5];

	command[0] = IAP_REPID_CMD;
	iap_entry(command, result);

	return result[1];
}

/* Read boot code version number */
uint8_t Chip_IAP_ReadBootCode()
{
    unsigned int command[5], result[4];

	command[0] = IAP_READ_BOOT_CODE_CMD;
	iap_entry(command, result);

	return result[0];
}

/*
  Copies from RAM to Flash
  Writes to flash. prepare_sector_write() must be executed beforehand
    uint16_t *src_addr  => source ram address from which data bytes are to be read
    uint16_t *dest_addr => destination flash address where data bytes are to be written
                       this address must be a 256 byte boundary
    int size        => number of bytes to be written starting at dest_addr
                       must be 256 | 512 | 1024 | 4096
*/
void copy_ram_flash(uint16_t *src_addr, uint16_t *dest_addr, int size) {
    command[0] = 51; // command code
    command[1] = (unsigned) dest_addr;
    command[2] = (unsigned) src_addr;
    command[3] = (unsigned) size;
    command[4] = (unsigned) 100000; // CPU Clock Freq of MBED in KHz

    iap_entry(command,output);
}