#include "bootFunctions.h"

#include "MEMORY.h"
#include "EEMEM.h"
#include "System.h"
#include "keys.c"
#include "idibus_hw.h"

void readDeviceData(flash_datablock_t* data, uint32_t addr) {
    uint8_t* dst = (uint8_t*)data;  // ????????? ?? ?????
    uint32_t src = addr * 2;          // ????? ?? flash (? ??????)
    
    for (size_t i = 0; i < sizeof(flash_datablock_t); i++) {
        dst[i] = pgm_read_byte_far(src + i);
    }
}
static inline 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;
}
static inline uint8_t CheckEEPROMFlag()
{
	eeprom_busy_wait();
	uint8_t flag = eeprom_read_byte(&(EEBLOCK.BOOTFLAG));
	if (flag != 0xFF)
    {
        //clear it
        eeprom_busy_wait();
        eeprom_write_byte(&(EEBLOCK.BOOTFLAG),0xFF);
        eeprom_busy_wait();
        return IDIFMW_REASON_EnterFromApplication;
    }
	else return 0;
}
static inline uint8_t CheckADDR()
{
    uint8_t RS_Address = RSLink_AddrDecode();
	if (RS_Address == IDIBUS_DEVELOPER_ADDR_3) return IDIFMW_REASON_EnterForced;
	return 0;
}
uint8_t FindBootloaderEnterReason()
{
    uint8_t Reason = 0;
	Reason |= IDIFMW_REASON_NoReason;
	Reason |= CheckFirmware();
	Reason |= CheckEEPROMFlag();
	Reason |= CheckADDR();
    return Reason;
}

static FMW_INFO	FmwInfo;

static volatile uint32_t BlockNumber;

static uint8_t update_in_procces = 0;

static uint8_t AES_DISABLED=1;

uint8_t BootloaderUpdateInit(FMW_INFO new_FmwInfo)
{
    AES_PrepareKeys(key,IV);
    
    AES_DISABLED = 1; // Disable AES
    update_in_procces = 0;
    BlockNumber = 0;
	
    FmwInfo = new_FmwInfo;
    //At this moment let's think that it's not ENCRYPTED
    
    // Check CRC32   
    uint32_t calculatedCRC = CRC32((uint8_t*)&FmwInfo,sizeof(FmwInfo)-CRC32_SIZE);
    
    if (calculatedCRC != FmwInfo.InfoHash)
	{
		//Seems like it's either non ENCRYPTED or damaged
        //Decrypt it and try again
        AES_Decrypt((uint8_t*)&FmwInfo,sizeof(FmwInfo));
        calculatedCRC = CRC32((uint8_t*)&FmwInfo,sizeof(FmwInfo)-CRC32_SIZE);
        
        //Check CRC again
        if (calculatedCRC != FmwInfo.InfoHash) return 1; // Bad. No need to continue
        //Otherwise enable AES
        AES_DISABLED = 0;
	}

	uint8_t Check = 0;

	if (FmwInfo.FmwSizeInPages >= PAGE_COUNT)
	{
		Check = 2;
        return Check;
	}
	
    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 = 3; 
            return Check;
        }
	}

	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 = 4; 
            return Check;
        }
	}

    //Erase all flash in APP
	for (uint32_t page = 0; page < PAGE_COUNT; page++)
	{
		boot_spm_busy_wait();
		boot_page_erase((page)*SPM_PAGESIZE);
		wdt_reset();
	}
    if (Check == 0) update_in_procces = 1;
    return Check;
}

uint8_t BootloaderUpdateHandler(uint8_t* chunk)
{
    if (update_in_procces == 0) return 1;
    
    if (AES_DISABLED == 0) AES_Decrypt(chunk, SPM_PAGESIZE); // 
    
    uint_farptr_t page_start = (uint32_t)BlockNumber * SPM_PAGESIZE;
    
    boot_spm_busy_wait();
    
    for (uint16_t counter = 0; counter < SPM_PAGESIZE; counter += 2)
    {
        uint16_t word = *((uint16_t*)&chunk[counter]);
        boot_page_fill(page_start + counter, word);
    }
    
    boot_spm_busy_wait();
    boot_page_write(page_start);
    boot_spm_busy_wait();

    BlockNumber++;
    return 0;
}


static inline void updateAppData()
{
    
    volatile flash_datablock_t newDataApp;
    readDeviceData(&newDataApp, locationInBoot);
    newDataApp.SW[0] = FmwInfo.SwVersion[0];
    newDataApp.SW[1] = FmwInfo.SwVersion[1];
    newDataApp.AppCRC = FmwInfo.FmwHash;
     
    uint8_t *ptr = (uint8_t*)&newDataApp;
    
    const uint32_t baseAddr = locationInApp * 2;
    boot_spm_busy_wait();
    for ( uint32_t Addr = baseAddr;Addr<baseAddr+SPM_PAGESIZE;Addr+=2)
    {
        if (Addr < baseAddr+sizeof(newDataApp))
        {
            uint16_t word = 0;
            word |= *ptr; 
            ptr++;
            word |= (((uint16_t)*ptr) << 8);
            ptr++;
            boot_page_fill(Addr,word);
        }
        else
        {
            boot_page_fill(Addr,0xFFFF);

        }
        wdt_reset();
    }
    
    boot_spm_busy_wait();
	boot_page_write(baseAddr);
	boot_spm_busy_wait();
}
uint8_t BootloaderUpdateEnd()
{
        if (update_in_procces == 0) return 1;
        //Last block
		/*Write personal info*/
		wdt_reset();

		boot_spm_busy_wait();
		boot_rww_enable();

        updateAppData();

        update_in_procces = 0;

		if (CheckFirmware() == 0)
		{
            // No errors
			return 0;
		}
		else
		{
            //CRC Fail
			return 1;
		}
}
void LaunchApplication()
{
	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);
}