
#include "bootloader_cfg.h"
#include "bootloader_lib.h"
#include "flash.h"

const UInt8 tabFCCOB_Index[] =
{
	FTFL_FCCOB1_OFFSET,
	FTFL_FCCOB2_OFFSET,
	FTFL_FCCOB3_OFFSET,
	FTFL_FCCOB4_OFFSET,
	FTFL_FCCOB5_OFFSET,
	FTFL_FCCOB6_OFFSET,
	FTFL_FCCOB7_OFFSET,
	FTFL_FCCOB8_OFFSET,
	FTFL_FCCOB9_OFFSET,	
	FTFL_FCCOBA_OFFSET,
	FTFL_FCCOBB_OFFSET	
};


TFTFL_CmdOBJ  FTFL_cmdTab[FTFL_ERASE_SECTOR+1] =
{
	{
		1, 0x03					// read1s block
	},
	{
		1, 0x07					// read1s section for p-flash, 0x03 for d-flash
	},	
	{
		5, 0x03					// program check
	},
	{
		5, 0x03					// read resource
	},
	{
		0, 0x03					// program byte/reserved
	},
	{
		0, 0x03					// program word/reserved
	},
	{
		4, 0x03					// program longword
	},
	{
		0, 0x03					// reserved
	},
	
	{
		0, 0x03					// erase flash block
	},
	{
		0, 0x07					// erase flash sector, 0x03 for d-flash
	},
};
// Externals
extern UInt16 status;	// stores error code

// Local function prototypes
UInt32 MemoryRangeCheck(UInt32 targetAddr, UInt8 isInDataMem);
void FTFL_cmd( UInt8 cmd, UInt8 FCCOBNo, UInt8* pParams);


//++++++++++++++++++++++++ FUNCTION DEFINITIONS ++++++++++++++++++++++++++++++++++
#ifdef	DYNAMIC_BOOT
#pragma section pram_code begin
#endif

UInt8  FTFL_ProtectionCheck(void)
{
	UInt8 bProtected = 0;
	if((FTFL_FPROT0 != 0xFF) 
	   || (FTFL_FPROT1 != 0xFF)			
	   || (FTFL_FPROT2 != 0xFF)			
	   || (FTFL_FPROT3 != 0xFF)	
	   )
	{
		bProtected = 1;
	}
	return (bProtected);
}

UInt32 MemoryRangeCheck(UInt32 targetAddr, UInt8 isInDataMem)
{	
	UInt32 ftfl_addr = targetAddr;
	UInt8  bIsDFlash = 0;
	
	if(!isInDataMem)
	{
		// in code/program memory

		if(ftfl_addr >= PP_FLASH_START && ftfl_addr <= PP_FLASH_END)
		{
			// is p-flash
			ftfl_addr -= PP_FLASH_START;	// calculate the offset to the FTFL memory base
		}
		else if(ftfl_addr >= PD_FLASH_START && ftfl_addr <= PD_FLASH_END && (PD_FLASH_END >0))
			{
				// is d-flash
				bIsDFlash = 1;
				ftfl_addr -= PD_FLASH_START;	// calculate the offset to the FTFL memory base			
			}
		else
		{
			// error: out of flash
			status |= ERR_ADDR_RANGE;
		}
	}
	else
	{
		// in data/x memory
		if((ftfl_addr >= DD_FLASH_START) && (ftfl_addr <= DD_FLASH_END) && (DD_FLASH_END >0))
		{
			bIsDFlash = 1;
	    	ftfl_addr -= DD_FLASH_START;	// calculate the offset to the FTFL memory base			
		}
		else
		if(ftfl_addr >= DP_FLASH_START && ftfl_addr <= DP_FLASH_END)
		{
	    	ftfl_addr -= DP_FLASH_START;	// calculate the offset to the FTFL memory base			
		}
		else
		{
			// target address is non-flash area, report error
			status |= ERR_ADDR_RANGE;
		}		
	}
	// Convert word address to byte address used by FTFL
    ftfl_addr <<= 1;
	
	// If it d-flash, set address bit 23   
	if(bIsDFlash)		   		
	{
		ftfl_addr |= FTFL_DFLASH_ADDR_MARK;  			
	}
	
	return (ftfl_addr);
} 

/*****************************************************************************
* UInt16 hfm_command(int flash, int cmd, int *addr, int *data, int cnt)
*
* Write an array of data to flash memory. The flash memory can be program flash
* or data flash depending on the address mask as below:
* #define DATA_MEM_ADDRESS_MASK           0x02000000L
* Both program and data flash may contain primary flash (p-flash) or secondary
* flash (d-flash).
* For FTFL command, the address bit 23 selects between p-flash and d-flash:
*	1 for d-flash, 0 for p-flash.
* In:  cmd  - HFM command
*      addr - Starting address in word
*      data - Interger array of data to write in word address
*      cnt  - Number of words in array
*
* Out: Returns executed Flash Command code.
*****************************************************************************/
UInt16 hfm_command(UInt16 cmd, UInt16 *addr, UInt16 *data, UInt16 cnt)
{
   UInt32 ftfl_addr = (UInt32)addr & ~DATA_MEM_ADDRESS_MASK;
   UInt8 *pDataByte = (UInt8*)data;	// convert word address to byte address
   UInt16 err;
   UInt8  bIsDFlash;
   UInt16 bByteCountLeft = cnt<<1;
   UInt8 byteCount2Shift;
   UInt8 cmdBytesParamCount;
   UInt8  cmdParams[11];
   UInt8  bProgBuf[11];
   Int16 i;
   
    // Check the flash memory range 
	// Check if it is in the data flash memory and if it is a p-flash or d-flash		
	ftfl_addr = MemoryRangeCheck(ftfl_addr, ((UInt32)addr & DATA_MEM_ADDRESS_MASK)? 1: 0);

    // Check the flash block and determine the address alignment mask
    // prhase aligned or word aligned
	if((ftfl_addr & FTFL_DFLASH_ADDR_MARK) )
	{
		FTFL_cmdTab[cmd].AddrAlignMask = 0x03;
	}
	// Align address to longword and calculate the byte count to move
	byteCount2Shift = ftfl_addr & FTFL_cmdTab[cmd].AddrAlignMask;
	ftfl_addr &= ~FTFL_cmdTab[cmd].AddrAlignMask;
	
	// Pad the bytes with all FFs in the prog buffer
	for(i = 0; i < byteCount2Shift; i++)
	{
		bProgBuf[i] = 0xFF;			
	}
	do
	{		
		// Start to fill prog buffer with 4 bytes in each round and fill the
		// command objects
		for(i=byteCount2Shift;i < 4; i++)
		{
			if(bByteCountLeft)
			{
				bProgBuf[i] = *pDataByte++;	
				bByteCountLeft--;					
			}
			else
			{
				bProgBuf[i] = 0xFF;
			}
		}
		
		
		// Fill in the address fields
		cmdParams[0] = ((UInt32)ftfl_addr >> 16) & 0xff;
		cmdParams[1] = ((UInt32)ftfl_addr >> 8) & 0xff;
		cmdParams[2] =  (UInt32)ftfl_addr  & 0xff;
		
		// Fill in the bytes fields
		cmdBytesParamCount = FTFL_cmdTab[cmd].ByteCount;
		for(i = cmdBytesParamCount-1; i >= 0; i--)
		{
			cmdParams[3+i] = bProgBuf[3-i];	// bytes swapped
		}	
		// Wait till the previous command complete
		while(!(FTFL_FSTAT & FTFL_FSTAT_CCIF))
		{		
		}	
		// Error check
		if(FTFL_FSTAT & FTFL_FSTAT_ACCERR)
		{
			// access error
			FTFL_FSTAT =  (FTFL_FSTAT_FPVIOL | FTFL_FSTAT_ACCERR);
			status |= ERR_HFM_ACCERR;
		}
		if(FTFL_FSTAT & FTFL_FSTAT_FPVIOL)
		{
			// protection error
			FTFL_FSTAT =  (FTFL_FSTAT_FPVIOL | FTFL_FSTAT_ACCERR);
			status |= ERR_HFM_PVIOL;			
		}
		// Write FTFL registers and lauch the command
		FTFL_cmd(cmd,3 + cmdBytesParamCount,cmdParams);
			
		// Move to the next 4 data bytes
		ftfl_addr += 4;
		byteCount2Shift = 0;
		   
	}while(bByteCountLeft>0);
	
   return (cmd);
}


#ifndef	DYNAMIC_BOOT
#pragma section pram_code begin
#endif
void FTFL_cmd( UInt8 cmd, UInt8 FCCOBNo, UInt8* pParams)
{
	Int16 i;
	UInt8 *pFTFL_FCCOB_Reg = (UInt8*)&FTFL_FSTAT;

    
    FTFL_FCCOB0 = cmd;
    for(i = 0; i < FCCOBNo; i++)
    {   	

		pFTFL_FCCOB_Reg[tabFCCOB_Index[i]] = pParams[i];		
    }
	
    /* clear CCIF bit to launch the command*/
    FTFL_FSTAT = FTFL_FSTAT_CCIF;  	
#ifndef	 DYNAMIC_BOOT
    /* For static bootloader, wait till command complete */
    /* check CCIF bit of the flash status register */
    while(!(FTFL_FSTAT & FTFL_FSTAT_CCIF))
    {
        /* wait till CCIF bit is set */
    }
#endif        
}

//#ifdef	DYNAMIC_BOOT
#pragma section pram_code end
//#endif