#include "bootFunctions.h"

#include "Common/MEMORY.h"
#include "EEMEM.h"
#include "keys.h"

uint8_t CheckFirmware(){
	boot_rww_enable_safe();
	uint32_t crcRead = flash_read_dword(STATIC_DATA_APP.AppCRC,0);
	uint32_t crcCalc = CRC32(NULL,locationInApp<<1);
	if (crcRead != crcCalc) return IDIFMW_REASON_NoFmwDetected;		
	return 0;
}

uint8_t CheckEEPROMFlag(){
	eeprom_busy_wait();
	uint8_t flag = eeprom_read_byte(&(EEBLOCK.BOOTFLAG));	
	if (flag != 0xFF) return IDIFMW_REASON_EnterFromApplication;
	return 0;
}

inline uint8_t CheckADDR(){
	if (RS_Address == IDIBUS_DEVELOPER_ADDR_3) return IDIFMW_REASON_EnterForced;
	return 0;
}

inline void DisableAES(){
	AES_DISABLED = 1;	
}

#ifdef _LONG_ADDR_SPACE_
uint32_t baseAddr;
#else
uint16_t baseAddr;
#endif

void  BootloaderDataHandler(){
	wdt_reset();
	boot_spm_busy_wait();
	if (BootloaderInfo.BlockNumber == -1) {

		if (AES_DISABLED == 0) {
			AES_PrepareKeys(key,IV);	
			AES_Decrypt(BootloaderData,FMW_INFO_SIZE);
		}

		_memcopy(BootloaderData,&FmwInfo,FMW_INFO_SIZE);
		uint32_t calculatedCRC = CRC32(&FmwInfo,FMW_INFO_SIZE-CRC32_SIZE);
		
		uint8_t Check = 1;

		if (calculatedCRC != FmwInfo.InfoHash) {
			Check = 0;
		}

		if (FmwInfo.FmwSizeInPages >= PAGE_COUNT) {
			Check = 0;
		}
		
		for (uint8_t i = 0; i < IDISN_FIXP_MODULE_TYPE_Length; i++)
		{
			if (flash_read_byte(STATIC_DATA_BL.ModuleType,i) != FmwInfo.ModuleType[i])	Check = 0;
		}
	
		for (uint8_t i = 0; i < IDISN_FIXP_HW_REV_Length; i++)
		{
			if (flash_read_byte(STATIC_DATA_BL.HW_revision,i) != FmwInfo.HwRevision[i]) Check = 0;
		}	

		if (Check == 0) {
			BootloaderInfo.Status = IDIFMW_STATUS_IncompatibleFmw;
			return;
		}	

		for (uint16_t page = 0; page < PAGE_COUNT; page++){
			boot_spm_busy_wait();
			#ifdef _LONG_ADDR_SPACE_
				boot_page_erase(((uint32_t)page)<<PAGE_SIZE_BIT);	
			#else
				boot_page_erase(page<<PAGE_SIZE_BIT);	
			#endif
			wdt_reset();
		}
	} else {

		if (AES_DISABLED == 0)
			AES_Decrypt(BootloaderData,PAGE_SIZE);

		//Fill page
		#ifdef _LONG_ADDR_SPACE_
		baseAddr = ((uint32_t)BootloaderInfo.BlockNumber)<<PAGE_SIZE_BIT;
		#else
		baseAddr = (BootloaderInfo.BlockNumber)<<PAGE_SIZE_BIT;
		#endif

		//boot_rww_enable_safe();

		//BE CAREFUL
		wdt_reset();

		#ifdef _LONG_ADDR_SPACE_
		for (uint16_t counter = 0; counter < PAGE_SIZE; counter+=2, baseAddr+=2)
		#else
		for (uint8_t counter = 0; counter < PAGE_SIZE; counter+=2, baseAddr+=2)
		#endif
		{
			uint16_t word = *((uint16_t *)&BootloaderData[counter]);
			boot_page_fill(baseAddr,word);	
		}

		baseAddr-=PAGE_SIZE;

		//Write page
		boot_spm_busy_wait();
		boot_page_write(baseAddr);
	}
	
	BootloaderInfo.BlockNumber++;

	if (BootloaderInfo.BlockNumber == FmwInfo.FmwSizeInPages){
		/*Write personal info*/

		wdt_reset();

		boot_spm_busy_wait();
		//boot_rww_enable();

		baseAddr = locationInApp<<1;

		boot_rww_enable();
	
		#ifdef _LONG_ADDR_SPACE_
		for (uint16_t counter = 0; counter < PAGE_SIZE; counter+=2, baseAddr+=2)
		#else
		for (uint8_t counter = 0; counter < PAGE_SIZE; counter+=2, baseAddr+=2)
		#endif
		{
			uint16_t word;
			if (counter < IDISN_FIXP_SW_REV_Pos)
				word = flash_read_word(STATIC_DATA_BL,counter);
			else if (counter < IDISN_FIXP_SW_REV_Pos+IDISN_FIXP_SW_REV_Length)
				word = FmwInfo.SwVersion;
			else if (counter < IDISN_FIXP_SW_REV_Pos+CRC32_SIZE)
				word = FmwInfo.FmwHash[0];
			else if (counter < IDISN_FIXP_SW_REV_Pos+IDISN_FIXP_SW_REV_Length+CRC32_SIZE)
				word = FmwInfo.FmwHash[1];
			else
				word = 0xFFFF;

			boot_page_fill(baseAddr,word);	
		}

		baseAddr-=PAGE_SIZE;

		boot_spm_busy_wait();
		boot_page_write(baseAddr);
		boot_spm_busy_wait();

		wdt_reset();
	
		if (CheckFirmware() == 0){
			BootloaderInfo.Reason &= ~(IDIFMW_REASON_NoFmwDetected);
			BootloaderInfo.Status = IDIFMW_STATUS_FmwUploadComplete;
		} else {
			BootloaderInfo.Status = IDIFMW_STATUS_FmwUploadCRCError;
		}
	}
	else
		BootloaderInfo.Status = IDIFMW_STATUS_ReadyToGetBlock;
}

void LaunchApplication() {
	//After flash remove boot flag from eeprom
	if (CheckEEPROMFlag() != 0) eeprom_write_byte(&EEBLOCK.BOOTFLAG, 0xff);		

	eeprom_busy_wait();	

	MCUCR=(1<<IVCE);
	MCUCR=(0<<IVSEL);

	boot_rww_enable_safe();				// enable application section

	asm volatile(
			//Clear SRAM
			//Should work needs testing
			"clr r28				\n\t"
			"LDI r30, %0		\n\t"
			"LDI r31, %1		\n\t"
			"CLEAR_%=:				\n\t"
			"adiw r30, 1			\n\t"
			"st Z, r28				\n\t"
			"cpi r30, %2		\n\t"
			"ldi r29, %3		\n\t"
			"cpc r31, r29			\n\t"
			"BRNE CLEAR_%=			\n\t"
			//Jmp to app
			"clr	r30		\n\t"
			"clr	r31		\n\t"
			"ijmp	\n\t"
			://No output
			:"M" (RAMSTART&0xFF), "M" (RAMSTART>>8), "M" (RAMEND&0xFF), "M" (RAMEND>>8));

	while(1);	
}

void FindBootloaderEnterReason(){
	BootloaderInfo.Reason = 0;
	BootloaderInfo.Reason |= CheckFirmware();		
	BootloaderInfo.Reason |= CheckEEPROMFlag();
	BootloaderInfo.Reason |= CheckADDR();
}

void  GetBootloaderInfo(volatile uint8_t *dst){
	for (uint8_t i = IDISN_FIXP_MODULE_TYPE_Pos; i < IDISN_FIXP_MAC_Pos; i++){
		*dst++ = flash_read_byte(STATIC_DATA_BL,i);
	}	
	*dst++ = flash_read_byte(STATIC_DATA_BL,IDISN_FIXP_SW_REV_Pos);
	*dst++ = flash_read_byte(STATIC_DATA_BL,IDISN_FIXP_SW_REV_Pos+1);
	
	*dst++ = BootloaderInfo.Reason;
	*dst = AES_DISABLED;
}

uint16_t GetNextBlockSize(){
	if (BootloaderInfo.Status < IDIFMW_STATUS_Busy){
		if (BootloaderInfo.BlockNumber != -1) {
			return PAGE_SIZE;
		} else {
			return FMW_INFO_SIZE; 
		}	
	}	
	return 0;
}

void GetBootloaderStatus(volatile uint8_t *dst){
	*dst++ = BootloaderInfo.Status;
	uint16_t BlockSize = GetNextBlockSize();
	*dst++ = (uint8_t)(BlockSize>>8);
	*dst++ = (uint8_t)BlockSize;
	*dst++ = (uint8_t)(BootloaderInfo.BlockNumber>>8);
	*dst = (uint8_t)BootloaderInfo.BlockNumber;
}