/* ---------------------------------------------------------------------------
 * IspProtocol.c - v0.1                                (c) 2008 Micro-key bv
 * ---------------------------------------------------------------------------
 * Micro-key bv
 * Industrieweg 28, 9804 TG Noordhorn
 * Postbus 92, 9800 AB Zuidhorn
 * The Netherlands
 * Tel: +31 594 503020
 * Fax: +31 594 505825
 * Email: support@microkey.nl  
 * Web: www.microkey.nl
 * ---------------------------------------------------------------------------
 * Description: 
 * ---------------------------------------------------------------------------
 * Version(s):  0.1, Feb 13, 2008, FSc
 *                   Creation.
 * ---------------------------------------------------------------------------
 */

/* ---------------------------------------------------------------------------
 * System include files
 * ---------------------------------------------------------------------------
 */

/* ---------------------------------------------------------------------------
 * Application include files
 * ---------------------------------------------------------------------------
 */
#include "types.h"
#include "IspProtocol.h"
#include "InternalFlash.h"
#include "Crc.h"
#include "Leds.h"
#include "appImage.h"

/* ---------------------------------------------------------------------------
 * Local constant and macro definitions
 * ---------------------------------------------------------------------------
 */
#define MAX_ADMINS              (4)
#define START_BYTE              (0xAA)
#define MAX_PAYLOAD_SIZE        (80) 

#define MSGID_ACKNOWLEDGE       (0x01)
#define MSGID_UNLOCK            (0x02)
#define MSGID_ERASEBLOCK        (0x03)
#define MSGID_PROGRAMFLASH      (0x04)
#define MSGID_VERIFYFLASH       (0x05)
#define MSGID_FINISHPROGRAMMING (0x06)
#define MSGID_STARTPROGRAM      (0x07)

#define APP_FLASH_START_ADDR    (0x00005000)
#define APP_FLASH_END_ADDR      (0x0007DFFF)
#define APP_FLASH_START_SECTOR  (5)
#define APP_FLASH_END_SECTOR    (27)
#define APP_IMAGE_LENGTH_OFFS   0x0008
#define APP_IMAGE_CRC_OFFS      0x000C

#define PROGRAM_BLOCK_SIZE      (256)
#define INVALID_ADDRESS         (0xFFFFFFFF)


/* ---------------------------------------------------------------------------
 * Global variable definitions
 * ---------------------------------------------------------------------------
 */

/* ---------------------------------------------------------------------------
 * Local variable definitions
 * ---------------------------------------------------------------------------
 */
typedef enum
{
	IDLE,
	MESSAGE_ID,
	PAYLOAD_SIZE,
	PAYLOAD,
	CRC
} t_isp_decodestatus;

typedef struct t_ISP_MESSAGE {
	UINT8                  messageId;
	UINT8                  payloadSize;
	UINT8                  payload[MAX_PAYLOAD_SIZE];
	UINT16                 crc;
} t_isp_message;

typedef struct t_ISP_ADMIN {
	t_isp_decodestatus   status; 
	UINT8                rxIndex;
	UINT16               rxCrc;
	t_isp_message        rxMessage;
	t_isp_ack_callback   ackCallback;
} t_isp_admin;

static t_isp_admin ispAdmins[MAX_ADMINS];
static UINT8 lastReservedAdmin = 0;
static UINT32 maxProgrammedAddress = 0;
static UINT32 startAddress = INVALID_ADDRESS;
static UINT8 verifyBlock[PROGRAM_BLOCK_SIZE] __attribute__((aligned(PROGRAM_BLOCK_SIZE)));
static UINT8 programBlock[PROGRAM_BLOCK_SIZE] __attribute__((aligned(PROGRAM_BLOCK_SIZE)));
static UINT8 programFirstBlock[PROGRAM_BLOCK_SIZE] __attribute__((aligned(PROGRAM_BLOCK_SIZE)));

/* ---------------------------------------------------------------------------
 * Local function definitions
 * ---------------------------------------------------------------------------
 */
void sendAcknowledge(t_isp_admin *handle, t_isp_responses response);
BOOLEAN ValidMessageId( UINT8 messageId);
void handleMessage(t_isp_admin *admin);
void handleUnlockMessage(t_isp_admin *admin);
void handleEraseBlockMessage(t_isp_admin *admin);
void handleStartProgram(t_isp_admin *admin);
void handleFinishProgramming(t_isp_admin *admin);
void handleVerifyFlashMessage(t_isp_admin *admin);
void handleProgramFlashMessage(t_isp_admin *admin);
iflashresult fillProgramBlock( UINT32 address, UINT8 size, UINT8 *data);
iflashresult programFlash();
void resetRamBlock( UINT32 address );
iflashresult StoreCrcAndLength( UINT32 imageSize );



/** \brief Initialises a instance of the ISP-protocol handler. 
 * 
 * For each communication port, one ISP-protocol handler must be initialised.
 * 
 * \param ackCallback Callback-function used for parent to send acknowledge
 * \returns handle Handle for this ISP-protocol handler.
 */
int ispInitProtocol(t_isp_ack_callback ackCallback )
{
	int result;
	if (lastReservedAdmin < MAX_ADMINS)
	{
		ispAdmins[lastReservedAdmin].status = IDLE;
		ispAdmins[lastReservedAdmin].ackCallback = ackCallback;
		          
		result = lastReservedAdmin;
		lastReservedAdmin++;
	}
	else
	{
		result = -1;
	}
	return result;
}

/** \brief Does the protocol handling by parsing each byte received on the communication port. 
 * 
 * \param handle Handle for the ISP-protocol handler
 * \param byte The received byte on the communication port
 */
void ispHandleRxByte( int handle, UINT8 byte )
{
	t_isp_admin *admin = &(ispAdmins[handle]);

	switch( admin->status )
	{
	case(IDLE):
		if (byte == START_BYTE)  
		{
			admin->status = MESSAGE_ID;
		}
		break;
	case(MESSAGE_ID):
		if (ValidMessageId(byte) == TRUE )
		{
			// Determine Payload-size
			admin->rxCrc = crcCalc(&byte, 1, 0);
			admin->rxMessage.messageId = byte;
			admin->status = PAYLOAD_SIZE;
		}
		else
		{
			sendAcknowledge( admin, ISP_INVALID_MESSAGE_ID );
			admin->status = IDLE;
		}
		break;
	case(PAYLOAD_SIZE):
		// Determine Payload-size
		if (byte <=MAX_PAYLOAD_SIZE)
		{
			admin->rxMessage.payloadSize = byte;
			admin->rxCrc = crcCalc(&byte, 1, admin->rxCrc);
			admin->rxIndex = 0;
			if (byte > 0)
			{
				admin->status = PAYLOAD;
			}
			else
			{
				admin->status = CRC;
			}
		}
		else
		{
			sendAcknowledge( admin, ISP_PAYLOAD_TOO_LARGE );
			admin->status = IDLE;
		}
		break;
	case(PAYLOAD):
		admin->rxMessage.payload[ admin->rxIndex ] = byte;
		admin->rxCrc = crcCalc(&byte, 1, admin->rxCrc);
		admin->rxIndex++;
	
		if (admin->rxIndex >= admin->rxMessage.payloadSize)
		{
			admin->rxIndex = 0;
			admin->status = CRC;
		}
		break;
	case(CRC):
		// Receive and check CRC
		if (admin->rxIndex == 0)
		{
			admin->rxMessage.crc = (byte << 8);
		}
		else
		{
			admin->rxMessage.crc |= (byte & 0x00FF);
		}
	
		admin->rxIndex++;
	
		if (admin->rxIndex >= 2)
		{
			if (admin->rxCrc == admin->rxMessage.crc)
			{
				/* Crc was succesfully checked */
				handleMessage(admin);
			}
			else
			{
				sendAcknowledge( admin, ISP_RECV_BAD_CRC );
			}
			admin->status = IDLE; 
		}
		break;
	default:
		admin->status = IDLE;
		break;
	}
}

void handleMessage(t_isp_admin *admin)
{
	switch( admin->rxMessage.messageId )
	{
	case(MSGID_UNLOCK):
		//sendAcknowledge( admin, ISP_CMD_SUCCESS);
		handleUnlockMessage( admin );
		break;
	case(MSGID_ERASEBLOCK):
		handleEraseBlockMessage( admin );
		break;
	case(MSGID_PROGRAMFLASH):
		handleProgramFlashMessage( admin );
		break;
	case(MSGID_VERIFYFLASH):
		handleVerifyFlashMessage( admin );
		break;
	case(MSGID_FINISHPROGRAMMING):
		handleFinishProgramming( admin );
		break;
	case(MSGID_STARTPROGRAM):
		handleStartProgram( admin );
		break;
	default:
		sendAcknowledge( admin, ISP_INVALID_MESSAGE_ID);
		break;
	}
}

BOOLEAN ValidMessageId( UINT8 messageId)
{
	if ((messageId >= 0x01) && (messageId <= 0x07))
	{
		return TRUE;
	}
	else
	{
		return FALSE;
	}
}

void sendAcknowledge(t_isp_admin *handle, t_isp_responses response)
{
	handle->ackCallback( response );
}


void handleUnlockMessage(t_isp_admin *admin)
{
	iflashresult unlockResult = ISP_CMD_SUCCESS;

	maxProgrammedAddress = 0;
	unlockResult =iflashPrepare( APP_FLASH_START_SECTOR, APP_FLASH_END_SECTOR); 
	
	admin->ackCallback( (t_isp_responses)unlockResult );
}

void handleEraseBlockMessage(t_isp_admin *admin)
{
	iflashresult result;
	UINT8 blockNr;
	
	// Extract block nr from message
	blockNr = admin->rxMessage.payload[0];
	
	if (    ( blockNr >= APP_FLASH_START_SECTOR)
	     && ( blockNr <= APP_FLASH_END_SECTOR)) 
	{
		result = iflashErase( blockNr );
	}
	else
	{
		result = ISP_INVALID_SECTOR;
	}
	
	admin->ackCallback( (t_isp_responses)result );
}

void handleProgramFlashMessage(t_isp_admin *admin)
{
	iflashresult result;
	UINT32 address;
	UINT8 size;
	UINT8 *data;
	UINT8 index = 0;

	// Extract address, datalength & data from message
	address = ispGet32bit(admin->rxMessage.payload, &index);
	size = ispGet8bit(admin->rxMessage.payload, &index);
	data = &(admin->rxMessage.payload[index]);
	
	if (maxProgrammedAddress < address) maxProgrammedAddress = address;
	
	if (    (address >= APP_FLASH_START_ADDR)
		 && (address <= APP_FLASH_END_ADDR)
	   )
	{
		result = fillProgramBlock(address, size, data );
	}
	else
	{
		result = ISP_DST_ADDR_ERROR;
	}
	
	admin->ackCallback( (t_isp_responses)result );
}

void handleVerifyFlashMessage(t_isp_admin *admin)
{
	iflashresult result;
	UINT32 address;
	UINT8 size;
	UINT8 index = 0;
	UINT8 bufferIdx;

	// Extract address, datalength & data from message
	address = ispGet32bit(admin->rxMessage.payload, &index);
	size = ispGet8bit(admin->rxMessage.payload, &index);
	for (bufferIdx = 0; bufferIdx < size; bufferIdx++)
	{
		verifyBlock[bufferIdx] = ispGet8bit(admin->rxMessage.payload, &index);
	}
	
	// Make size dividable by 4
	size = size - (size % 4); 
	
	if ((size >= 4) && (address != APP_FLASH_START_ADDR))
	{
		result = iflashVerify( address, size, verifyBlock );
	}
	else
	{
		result = ISP_CMD_SUCCESS;
	}
	
	admin->ackCallback( (t_isp_responses)result );
}

void handleFinishProgramming(t_isp_admin *admin)
{
	iflashresult result;
	UINT32 imageSize;

	// Program remaining block on flash
	fillProgramBlock(0, 0, 0 );

	// Calculate image length
	imageSize = maxProgrammedAddress  - APP_FLASH_START_ADDR;
	
	// Calculate and store Crc & imageSize on flash
	result = StoreCrcAndLength( imageSize );
	
	admin->ackCallback( (t_isp_responses)result );
}

void handleStartProgram(t_isp_admin *admin)
{
	//if (appiValidAppImageAvail() == TRUE)
	{
		ledSet( LED1, 0 ); 
		ledSet( LED0, 0 ); 

		appiJumpToAppImage();
	}
	//else
	//{
	//	admin->ackCallback( ISP_BUSY );
	//}
}


void ispAdd16bit(UINT8 *payloadlocation, UINT16 data)
{
	UINT8 index = 0;
	
	payloadlocation[index] = (UINT8)(data >> 8);
	index++;
	payloadlocation[index] = (UINT8)(data & 0x00FF);
}


void ispAdd32bit(UINT8 *payloadlocation, UINT32 data)
{
	UINT8 index = 0;
	
	payloadlocation[index] = (UINT8)(data >> 24);
	index++;
	payloadlocation[index] = (UINT8)(data >> 16);
	index++;
	payloadlocation[index] = (UINT8)(data >> 8);
	index++;
	payloadlocation[index] = (UINT8)(data & 0xFF);
	
}

UINT8 ispGet8bit(UINT8 *payload, UINT8 *payloadIndex)
{
	UINT8 result;
	
	result = (UINT8)payload[*payloadIndex];
	(*payloadIndex)++;
	
	return result;
}


UINT16 ispGet16bit(UINT8 *payload, UINT8 *payloadIndex)
{
	UINT16 result;
	
	result = ((UINT16)payload[*payloadIndex]) << 8;
	(*payloadIndex)++;
	result += ((UINT16)payload[*payloadIndex] & 0x00FF);
	(*payloadIndex)++;
	
	return result;
}


UINT32 ispGet32bit(UINT8 *payload, UINT8 *payloadIndex)
{
	UINT32 result;
	
	result = ((UINT32)payload[*payloadIndex]) << 24;
	(*payloadIndex)++;
	result += ((UINT32)payload[*payloadIndex]) << 16;
	(*payloadIndex)++;
	result += ((UINT32)payload[*payloadIndex]) << 8;
	(*payloadIndex)++;
	result += ((UINT32)payload[*payloadIndex] & 0x000000FF);
	(*payloadIndex)++;
	
	return result;
}


iflashresult fillProgramBlock( UINT32 address, UINT8 size, UINT8 *data)
{
	UINT32 programIndex;
	UINT32 beginIndex;
	UINT32 dataIndex;
	UINT32 endIndex;
	iflashresult result = ISP_CMD_SUCCESS;
	
	if (data == 0)
	{
		// finish sending last block
		if (startAddress > 0)
		{
			result = programFlash();
		}
		else
		{
			result = ISP_CMD_SUCCESS;
		}
		startAddress = INVALID_ADDRESS;
		return result;
	}
	
	// Determine startAddress. 
	if (startAddress == INVALID_ADDRESS)
	{
		// Make sure address is alligned to 256
		resetRamBlock(address - (address % PROGRAM_BLOCK_SIZE));
	}
	else
	{
		if (address > (startAddress + PROGRAM_BLOCK_SIZE))
		{
			result = programFlash();
			resetRamBlock( address );
		}
	}

	// Determine endIndex;
	if ( (startAddress + PROGRAM_BLOCK_SIZE) > (address + size) )
	{
		endIndex = (address - startAddress) + size;
	}
	else
	{
		endIndex = PROGRAM_BLOCK_SIZE; 
	}
	
	// Copy data into programblock
	dataIndex = 0;
	beginIndex = address - startAddress;
	for (programIndex = beginIndex; programIndex < endIndex; programIndex++)
	{
		programBlock[programIndex] = data[dataIndex];
		dataIndex++;
	}
	
	// if program block is full, program it
	if (dataIndex < size)
	{
		result = programFlash();
		resetRamBlock( startAddress +  PROGRAM_BLOCK_SIZE );
		
		// Copy rest in new block
		endIndex = size - dataIndex; 
		for (programIndex = 0; programIndex < endIndex; programIndex++)
		{
			programBlock[programIndex] = data[dataIndex];
			dataIndex++;
		}
	}
	else
	{
		if ((startAddress + PROGRAM_BLOCK_SIZE) == (address + size))
		{
			result = programFlash();
			resetRamBlock( INVALID_ADDRESS );
		}
	}
	
	return result;
}

void resetRamBlock( UINT32 address )
{
	UINT32 programIndex;
	
	startAddress = address;
	for (programIndex = 0; programIndex < PROGRAM_BLOCK_SIZE; programIndex++)
	{
		programBlock[programIndex] = 0x00;
	}
}

iflashresult programFlash()
{
	UINT16 index;
	
	if (startAddress != APP_FLASH_START_ADDR)
	{
		iflashPrepare( APP_FLASH_START_SECTOR, APP_FLASH_END_SECTOR); 
		return iflashProgram( startAddress, PROGRAM_BLOCK_SIZE, programBlock );
	}
	else
	{
		// keep copy of first block, programmed later at FinishedProgramming
		for (index = 0; index < PROGRAM_BLOCK_SIZE; index++)
		{
			programFirstBlock[index] = programBlock[index];
		}
		
		return ISP_CMD_SUCCESS;
	}
}

iflashresult StoreCrcAndLength( UINT32 imageSize )
{
	static UINT16 crc = 0;
	static UINT8 *image = 0;

	// Calculate Crc over part before Length & CRC
	crc = crcCalc(programFirstBlock, APP_IMAGE_LENGTH_OFFS, crc);

	// Calculate Crc over part after Length & CRC in first program block
	image = (UINT8 *)(programFirstBlock + APP_IMAGE_CRC_OFFS + 4);
	crc = crcCalc(image, PROGRAM_BLOCK_SIZE - (APP_IMAGE_CRC_OFFS + 4), crc);

	// Calculate Crc over rest of image already programmed
	image = (UINT8 *)(APP_FLASH_START_ADDR + PROGRAM_BLOCK_SIZE);
	crc = crcCalc(image, imageSize - PROGRAM_BLOCK_SIZE, crc);
	
	// Fill in Length & CRC
	ispAdd32bit(programFirstBlock + APP_IMAGE_LENGTH_OFFS, imageSize);
	ispAdd16bit(programFirstBlock + APP_IMAGE_CRC_OFFS, crc);
	
	// Program first block on flash
	iflashPrepare( APP_FLASH_START_SECTOR, APP_FLASH_START_SECTOR); 
	return iflashProgram( APP_FLASH_START_ADDR, PROGRAM_BLOCK_SIZE, programFirstBlock );
}