/*
 * USB_Bootloader.c
 *
 * Created: 12/20/2021 12:16:15 PM
 * Author : david
 */ 


#include "main.h"
#include <string.h>
#include "sw_ver.h"

// Linker script defined constants
// Note that these variables have no values. The linker inserts just the addresses
// of defined variables into the symbol table, does not allocate any memory for them.
// These can only be accessed by reference to grab their address, for example:
//
// Linker script:
//			MyVal = 0x12345678;
// C code:
//			extern uint32_t MyVal;
//			if(&MyVal > 0x01234567) {
//				// do something...
//			}

extern uint32_t __eram;
extern uint32_t __eflash;
extern uint32_t __sram;
extern uint32_t __sflash;
extern uint32_t __app_address;
extern uint32_t __app_size;
extern uint32_t __bl_address;
extern uint32_t __bl_size;

void USBInit(void);
void PinsInit(void);
void usb3803_init(void);

static uint32_t read_bl_length(void);
static uint32_t read_bl_crc(void);
static uint32_t calc_bl_crc(void);

#define MIN_FLASH_WRITE_BYTES		(8u)
#define MAX_FLASH_WRITE_BYTES		(32u)

static volatile uint32_t systick_count = 0;
static uint32_t flash_page_buf[FLASH_PAGE_SIZE/sizeof(uint32_t)];
static uint32_t sig_page_buf[FLASH_PAGE_SIZE/sizeof(uint32_t)];

static uint8_t hid_in_buffer[CFG_TUD_HID_EP_BUFSIZE];

static Flash_Error_T cache_app_data(const uint8_t* hid_buf);
static Flash_Error_T erase_flash_command(const uint8_t* hid_buf);
static Flash_Error_T program_saved_cache(const uint8_t* hid_buf);
static Flash_Error_T cache_sig_data(const uint8_t* hid_buf);
static Flash_Error_T program_cached_sig(const uint8_t* hid_buf);

uint32_t nvmbits;
uint32_t boot_on_check = 1;

// I2C handle - used to initialize the on-board USB hub chip
I2C_Handle h_i2c4;

#define USB3803_ADDRESS_1	(0x10u) // 1st hub address
#define USB3803_ADDRESS_2	(0x12u) // 2nd hub address


// CRC Calculation testing
uint8_t crc_test_data[] = {
0x88, 0x7B, 0x00, 0x20, 0x19, 0x11, 0x41, 0x00, 0x15, 0x11, 0x41, 0x00, 0x15, 0x11, 0x41, 0x00,
0x15, 0x11, 0x41, 0x00, 0x15, 0x11, 0x41, 0x00, 0x15, 0x11, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB1, 0x14, 0x41, 0x00,
0x15, 0x11, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11, 0x15, 0x41, 0x00, 0x75, 0x15, 0x41, 0x00
};

uint32_t crc_test_len = sizeof(crc_test_data);

uint32_t crc_from_hw;

int main(void)
{
    /* Pause watchdog timer */
    WDT->WDT_MR = WDT_MR_WDDIS;
	__disable_irq();
    /* Initialize the SAM system */
    SystemInit();
    /* Configure clock as customized */
    ClockConfig();
	
	// Clear flash page buffer
	memset(flash_page_buf, 0xFF, FLASH_PAGE_SIZE);
	
    /* Setup pin configuration */
    PinsInit();
	/* Setup systick */
	SysTick_Config((CONFIG_CLOCK_PLLAMULT * FREQ_XTAL_32K)/1000u);
    /* Setup low-level USB clocking, etc. */
    USBInit();
	/* Enable IRQs in CPU */
	__DMB();
	__enable_irq();
	/* Initialize USB hub chip configuration */
#if   defined(__SAMG55G19__) || defined(__ATSAMG55G19__)
	usb3803_init();
#endif
	
    /* Initialize USB device stack */
    tusb_init();

    /* Replace with your application code */
    while (1) 
    {
		tud_task();
    }
}

void UDP_Handler(void) {
	tud_int_handler(0);
}

void SysTick_Handler(void) {
	systick_count++;
}

void Delay_Millis(uint32_t delayms) {
	volatile uint32_t start_time = board_millis();
	uint32_t current_time = board_millis();
	
	while(current_time < (start_time + delayms)) {
		current_time = board_millis();
	}
}

void USBInit(void) {
	/* Enable USB clock in PMC */
	PMC->PMC_USB = PMC_USB_USBS; // USB clock is PLLB
	PMC->PMC_SCER = PMC_SCER_UDP;
	PMC->PMC_PCER1 = (1UL << (ID_UDP-32));
}

void PinsInit(void) {
	// Init ioports (enable clocks)
	PMC->PMC_PCER0 = (1 << ID_PIOA) | (1 << ID_PIOB);
	
	/* Clear SYSIO 10 & 11 for USB DM & DP */
	MATRIX->CCFG_SYSIO &= ~((1 << 10) | (1 << 11));
	/* USB Device Mode */
	MATRIX->CCFG_USBMR |= 1;
	// I2C init
	HUB_I2C_PORT->PIO_PDR = ((1 << HUB_I2C_SCL_PIN) | (1 << HUB_I2C_SDA_PIN)); // Enables peripheral control of SCL/SDA pin 
	HUB_I2C_PORT->PIO_ABCDSR[0] &= ~((1 << HUB_I2C_SCL_PIN) | (1 << HUB_I2C_SDA_PIN)); // I2C4 is on Periph A connection (clear ABCDSR bit)
	// Logic init
	// Both Clock Enable and Reset are outputs, set them low for now
	MATRIX->CCFG_SYSIO |= (1 << 5); // Allows GPIO control of PB5 (default is JTAG TDO / TRACESWO)
	HUB_CLK_EN_PORT->PIO_PER = (1 << HUB_CLK_EN_PIN);
	HUB_CLK_EN_PORT->PIO_OER = (1 << HUB_CLK_EN_PIN);
	HUB_CLK_EN_PORT->PIO_CODR = (1 << HUB_CLK_EN_PIN);
	HUB_RESET_PORT->PIO_PER = (1 << HUB_RESET_PIN);
	HUB_RESET_PORT->PIO_OER = (1 << HUB_RESET_PIN);
	HUB_RESET_PORT->PIO_CODR = (1 << HUB_RESET_PIN);
}

//--------------------------------------------------------------------+
// USB HID
//--------------------------------------------------------------------+

// Invoked when received GET_REPORT control request
// Application must fill buffer report's content and return its length.
// Return zero will cause the stack to STALL request
uint16_t tud_hid_get_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t* buffer, uint16_t reqlen)
{
	// TODO not Implemented
	(void) itf;
	(void) report_id;
	(void) report_type;
	(void) buffer;
	(void) reqlen;

	return 0;
}

// Invoked when received SET_REPORT control request or
// received data on OUT endpoint ( Report ID = 0, Type = 0 )
void tud_hid_set_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t const* buffer, uint16_t bufsize)
{
	// This example doesn't use multiple report and report ID
	(void) itf;
	(void) report_id;
	(void) report_type;

	Flash_Error_T status;
	uint32_t crc_val;
	
	uint32_t rstc_mode_reg;
	
	switch(buffer[0])
	{
		case HID_CODE_FOR_SOFTWARE_VERSION:
			// Returns the software version string
			memset(hid_in_buffer, 0, bufsize);
			strncpy(hid_in_buffer, SW_VER_STRING, SW_VER_STRING_LEN);
			tud_hid_report(0, hid_in_buffer, bufsize);
		break;
		
		case HID_CODE_FOR_BOOTLOADER:
			// go to the bootloader
			// Set the appropriate backup registers
			GPBR->SYS_GPBR[0] = 0xBB;
			GPBR->SYS_GPBR[1] = 0x1A;
			// Just reboot the processor. Bootloader will run
			rstc_mode_reg = RSTC->RSTC_MR;
			rstc_mode_reg &= ~(RSTC_MR_ERSTL_Msk | RSTC_MR_KEY_Msk);
			// rstc_mode_reg |= RSTC_MR_ERSTL(0xFF); // longest possible reset time (about 2 seconds)
			rstc_mode_reg |= RSTC_MR_ERSTL(0x07); // About 2^8 / 32768 seconds (7.8ms)
			rstc_mode_reg |= RSTC_MR_KEY_PASSWD;
			RSTC->RSTC_MR = rstc_mode_reg;
			RSTC->RSTC_CR = RSTC_CR_KEY_PASSWD | RSTC_CR_EXTRST; // assert reset pin
		break;
		case HID_CODE_FOR_CLEAR_BOOTLOADER:
			// Clear the entire bootloader region
			// WARNING: Unless a new bootloader is flashed, this will 
			// prevent the device from booting after the next reset!
			status = flash_erase_bl_region();
			memset(hid_in_buffer, 0, bufsize);
			hid_in_buffer[0] = (uint8_t) status;
			tud_hid_report(0, hid_in_buffer, bufsize);
		break;
		case HID_CODE_FOR_ERASE_FLASH:
			// Erase a selected amount of Flash memory at a particular address.
			// Encoding of the HID report:
			// 0 : command (0x65, equivalent to 'e' in ASCII)
			// 1 : selection of size to delete. Can be any of the following:
			// 		0: Delete 8kB (16 pages). Note: page = 512B. 16 pages is minimum erasable size
			//		1: Delete 16kB (32 pages)
			//		2: Erase sector. There are various sector sizes, so this is a little tricky.
			//			There are 4 sectors in total, each with 128kB. But the first sectors is split
			//			into subsectors.
			//			Starting from lowest address in Flash (0x0040.0000), there are two 8kB small sub-sectors
			//			So those are from 0x0040.0000 to 0x0040.2000 and 0x0040.2000 to 0x0040.4000
			//			Then, there is a 112kB larger subsector from 0x0040.4000 to 0x0042.0000
			//			And then there are 3 128kB sectors. 
			//			Final layout looks like:
			//				- Sector 0: 0x0040.0000 to 0x0042.0000 (128kB)
			//				-- Subsector 0: 0x0040.0000 to 0x0040.2000 (8kB)
			//				-- Subsector 1: 0x0040.2000 to 0x0040.4000 (8kB)
			//				-- Subsector 2: 0x0040.4000 to 0x0042.0000 (112kB)
			//				- Sector 1: 0x0042.0000 to 0x0044.0000 (128kB)
			//				- Sector 2: 0x0044.0000 to 0x0046.0000 (128kB)
			//				- Sector 3: 0x0046.0000 to 0x0048.0000 (128kB)
			//			Note that the bootloader is using the first two small subsectors. Application
			//			firmware starts at 0x0040.4000 and can use the rest of the Flash.
			//	2, 3, 4, 5 : address (little endian), must be within app space (between 0x00404000 and 0x0047FFF8)
			//			Note that the address must be aligned on the erase size boundary, depending on
			//			the size of erase chosen. If erasing 16kB, then the address must be a multiple of
			//			16kB, etc.
			// 6 : crc-8 single byte crc to ensure data integrity. CRC is calculated on ENTIRE command, including byte 0
			//			uses 0x7 as the generator polynomial, initial input is 0x00
			status = erase_flash_command(buffer);
			memset(hid_in_buffer, 0, bufsize);
			hid_in_buffer[0] = (uint8_t) status;
			tud_hid_report(0, hid_in_buffer, bufsize);
		break;
		case HID_CODE_FOR_WRITE_DATA:
			// Write 8 to 32 bytes of data to specified location in flash
			// Encoding of the HID report:
			// 0 : command (this one is 0x44, as shown above)
			// 1 : length of bytes to write (must be multiple of 8, flash writes are minimum 64 bits
			// 2,3,4,5 : address (little endian), must be within app space (between 0x00404000 and 0x0047FFF8)
			// 6->6+length-1 : data
			// 6+length : crc-8 single byte crc to ensure data integrity. CRC is calculated on ENTIRE command, including byte 0
			//			uses 0x7 as the generator polynomial, initial input is 0x00
			
			status = cache_app_data(buffer);
			if(status != Flash_No_Error) {
				memset(hid_in_buffer, 0, bufsize);
				hid_in_buffer[0] = (uint8_t) status;
				tud_hid_report(0, hid_in_buffer, bufsize);
			}
		break;
		case HID_CODE_FOR_PROGRAM_DATA:
			status = program_saved_cache(buffer);
			memset(hid_in_buffer, 0, bufsize);
			hid_in_buffer[0] = (uint8_t) status;
			tud_hid_report(0, hid_in_buffer, bufsize);
		break;
		case HID_CODE_FOR_READ_CRC:
			// Grab the CRC from the application region
			crc_val = read_bl_crc();
			hid_in_buffer[3] = (uint8_t)(crc_val & 0xFF);
			hid_in_buffer[2] = (uint8_t)((crc_val >> 8) & 0xFF);
			hid_in_buffer[1] = (uint8_t)((crc_val >> 16) & 0xFF);
			hid_in_buffer[0] = (uint8_t)((crc_val >> 24) & 0xFF);
			tud_hid_report(0, hid_in_buffer, bufsize);
		break;
		case HID_CODE_FOR_CALC_CRC:
			crc_val = calc_bl_crc();
			memset(hid_in_buffer, 0, bufsize);
			hid_in_buffer[3] = (uint8_t)(crc_val & 0xFF);
			hid_in_buffer[2] = (uint8_t)((crc_val >> 8) & 0xFF);
			hid_in_buffer[1] = (uint8_t)((crc_val >> 16) & 0xFF);
			hid_in_buffer[0] = (uint8_t)((crc_val >> 24) & 0xFF);
			tud_hid_report(0, hid_in_buffer, bufsize);
		break;
		case HID_CODE_FOR_WRITE_SIG:
			status = cache_sig_data(buffer);
			memset(hid_in_buffer, 0, bufsize);
			hid_in_buffer[0] = (uint8_t) status;
			tud_hid_report(0, hid_in_buffer, bufsize);
		break;
		case HID_CODE_FOR_PROGRAM_SIG:
			status = program_cached_sig(buffer);
			memset(hid_in_buffer, 0, bufsize);
			hid_in_buffer[0] = (uint8_t) status;
			tud_hid_report(0, hid_in_buffer, bufsize);
		break;
		case HID_CODE_FOR_READ_SIG:
			status = flash_read_signature(sig_page_buf);
			if(status != Flash_No_Error) {
				memset(hid_in_buffer, 0, bufsize);
				hid_in_buffer[0] = (uint8_t) status;
				tud_hid_report(0, hid_in_buffer, bufsize);
				} else {
				// Start loading up 32-byte chunks to send back over hid reports
				// TODO: implement waiting on tud_hid_ready() and loop in main()
				hid_in_buffer[0] = Flash_No_Error;
				memcpy(&hid_in_buffer[1], sig_page_buf, 32);
				tud_hid_report(0, hid_in_buffer, bufsize);
			}
		break;
		case HID_CODE_FOR_ROM_BOOTLOADER:
			GPBR->SYS_GPBR[0] = BKUP_REG0_CODE_FOR_ROM_BOOTLOADER;
			GPBR->SYS_GPBR[1] = BKUP_REG1_MASK_FOR_ROM_BOOTLOADER + 1; // Start with 1 reset 
			// Trigger hardware reset
			RSTC->RSTC_CR = RSTC_CR_KEY_PASSWD | RSTC_CR_EXTRST_Msk;
		break;
		default:
			// Command error returned
			memset(hid_in_buffer, 0, bufsize);
			hid_in_buffer[0] = (uint8_t) Flash_Command_Error;
			tud_hid_report(0, buffer, bufsize);
		break;
	}
}

void usb3803_init(void) {
	I2C_Transaction_Request i2ctxn;
	uint8_t temp_byte = 0;
	i2ctxn.dev_addr = USB3803_ADDRESS_1;
	
	// I2C pins should already be configured
	h_i2c4.flexcom = FLEXCOM4;
	h_i2c4.SCL_Pin = PIO_PB11_IDX;
	h_i2c4.SDA_Pin = PIO_PB10_IDX;
	init_i2c(&h_i2c4, 100000);
	i2c_lockup_clear_sequence(&h_i2c4);
	
	// Enable USB hub clock
	HUB_CLK_EN_PORT->PIO_SODR = (1 << HUB_CLK_EN_PIN);
	Delay_Millis(10);
	// Clear hub reset
	HUB_RESET_PORT->PIO_SODR = (1 << HUB_RESET_PIN);
	Delay_Millis(10);
	// 10ms is safely past the startup time (4ms) and well before the end of connection delay (400ms)
	
	
	// Set the hub's ILOCK.config bit to pause startup
	i2ctxn.cb = NULL;
	i2ctxn.internal_addr = 0xE7;
	i2ctxn.data = &temp_byte;
	i2ctxn.len = 1;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {} // wait for completion
	
	// Set the bit
	temp_byte |= 0x01; // ILOCK.config_n
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
		
	// Now repeat for hub 2
	i2ctxn.dev_addr = USB3803_ADDRESS_2;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {} // wait for completion
	
	// Set the bit
	temp_byte |= 0x01; // ILOCK.config_n
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	
	// Now that both hubs are paused in their bootup state machine,
	// it's time to apply settings.
	// Back to hub 1
	i2ctxn.dev_addr = USB3803_ADDRESS_1;
	// Set the power mode. Self-powered, 0mA draw from bus
	i2ctxn.internal_addr = 0x06;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	
	temp_byte |= 0x80; // Self-powered
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	
	// Set max power to zero everywhere
	temp_byte = 0;
	i2ctxn.internal_addr = 0x0C;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	i2ctxn.internal_addr = 0x0D;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	i2ctxn.internal_addr = 0x0E;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	i2ctxn.internal_addr = 0x0F;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
		
	// Repeat the settings for hub 2
	i2ctxn.dev_addr = USB3803_ADDRESS_2;
	// Set the power mode. Self-powered, 0mA draw from bus
	i2ctxn.internal_addr = 0x06;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	
	temp_byte |= 0x80; // Self-powered
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	
	// Set max power to zero everywhere
	temp_byte = 0;
	i2ctxn.internal_addr = 0x0C;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	i2ctxn.internal_addr = 0x0D;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	i2ctxn.internal_addr = 0x0E;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	i2ctxn.internal_addr = 0x0F;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	
	// Release ILOCK.config_n for hub 1
	i2ctxn.dev_addr = USB3803_ADDRESS_1;
	i2ctxn.internal_addr = 0xE7;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	
	temp_byte &= ~(0x01); // clear bit 0x01
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
		
	// And for hub 2
	i2ctxn.dev_addr = USB3803_ADDRESS_2;
	i2ctxn.internal_addr = 0xE7;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
		
	temp_byte &= ~(0x01); // clear bit 0x01
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	
}

volatile uint32_t board_millis(void) {
	return systick_count;
}

static Flash_Error_T erase_flash_command(const uint8_t* hid_buf) {
	Flash_Erase_Size_T erasesize;
	uint32_t erase_start_addr, app_address, start_of_flash;
	uint8_t crc, mycrc;

	if(hid_buf[0] != HID_CODE_FOR_ERASE_FLASH) return Flash_Command_Error;

	erase_start_addr = ((uint32_t)hid_buf[2])
		+ (((uint32_t) hid_buf[3]) << 8)
		+ (((uint32_t) hid_buf[4]) << 16)
		+(((uint32_t) hid_buf[5]) << 24);

	start_of_flash = (uint32_t)(&__sflash);
	app_address = (uint32_t)(&__app_address);

	// Check address is within bootloader region of Flash
	if((erase_start_addr >= app_address) || (erase_start_addr < start_of_flash)) {
		return Flash_Address_Error;
	}
	// Check address is on correct alignment
	switch(hid_buf[1]) {
	case 0:
		erasesize = Flash_Erase_8kB;
		if((erase_start_addr & 0x1FFFu) != 0) {
			return Flash_Address_Error;
		}

	break;
	case 1:
		erasesize = Flash_Erase_16kB;
		if((erase_start_addr & 0x3FFFu) != 0) {
			return Flash_Address_Error;
		}
	break;
	case 2:
		erasesize = Flash_Erase_Sector;
		// This one is a little weird. Only certain values are allowed. Start of each sector.
		if( (erase_start_addr != start_of_flash)
			&& (erase_start_addr != start_of_flash + FLASH_LITTLE_SECTOR_SIZE)) {
				return Flash_Address_Error;
			}
	break;
	default:
		return Flash_Command_Error;
	break;
	}

	crc = hid_buf[6];
	mycrc = crc8(0x00, hid_buf, 6);
	if(crc != mycrc) {
		// Invalid crc - reject this command
		return Flash_CRC_Error;
	}

	// At this point all checks are good, erase some Flash.
	return flash_erase(erase_start_addr, erasesize);
}

static Flash_Error_T cache_app_data(const uint8_t* hid_buf) {
	uint8_t cmd, len, crc, my_crc;
	uint16_t flash_buffer_start;
	uint32_t addr, app_address, start_of_flash;
	cmd = hid_buf[0]; 
	if(cmd != HID_CODE_FOR_WRITE_DATA) {
		return Flash_Command_Error;
	}
	len = hid_buf[1];
	if((len < MIN_FLASH_WRITE_BYTES) || (len > MAX_FLASH_WRITE_BYTES)
		|| ((len % MIN_FLASH_WRITE_BYTES) != 0)) {
		// Length of bytes to write must be multiple
		// of 8 and maximum of 32
		return Flash_Size_Error;
	}
	addr = ((uint32_t)hid_buf[2])
		+ (((uint32_t) hid_buf[3]) << 8)
		+ (((uint32_t) hid_buf[4]) << 16)
		+(((uint32_t) hid_buf[5]) << 24);
	app_address = (uint32_t)(&__app_address);
	start_of_flash = (uint32_t)(&__sflash);
	if((addr < start_of_flash) || (addr >= app_address) ){
		// start address must be in valid bootloader space
		return Flash_Address_Error;
	}
	uint64_t addr_plus_len = ((uint64_t)addr) + ((uint64_t)len);
	
	if((addr_plus_len > UINT32_MAX) ||
		(addr_plus_len > app_address)) {
		// code written must be within bootloader space, too
		return Flash_Address_Error;
	}
	crc = hid_buf[6+len];
	my_crc = crc8(0x00, hid_buf, 6+len);
	if(crc != my_crc) {
		// Invalid crc - reject this command
		return Flash_CRC_Error;
	}
	
	// Everything is good, place this chunk of data within the flash programming buffer
	flash_buffer_start = (addr % FLASH_PAGE_SIZE)/sizeof(uint32_t);
	memcpy(&(flash_page_buf[flash_buffer_start]),  &(hid_buf[6]), len);
	
	return Flash_No_Error;
}

static Flash_Error_T program_saved_cache(const uint8_t* hid_buf) {
	uint8_t cmd, len, crc, my_crc;
	uint32_t addr, app_address, start_of_flash;
	Flash_Error_T retval;
	cmd = hid_buf[0];
	if(cmd != HID_CODE_FOR_PROGRAM_DATA) {
		return Flash_Command_Error;
	}
	len = hid_buf[1];
	if(len != 0) {
		// Length during program should always be zero. One full page is programmed.
		return Flash_Size_Error;
	}
	addr = ((uint32_t)hid_buf[2])
	+ (((uint32_t) hid_buf[3]) << 8)
	+ (((uint32_t) hid_buf[4]) << 16)
	+(((uint32_t) hid_buf[5]) << 24);
	app_address = (uint32_t)(&__app_address);
	start_of_flash = (uint32_t)(&__sflash);
	if((addr < start_of_flash) || (addr >= app_address) ){
		// start address must be in valid application space
		return Flash_Address_Error;
	}
	uint64_t addr_plus_len = ((uint64_t)addr) + ((uint64_t)len);
	if((addr_plus_len > UINT32_MAX) ||
		(addr_plus_len > app_address)) {
		// code written must be within application space, too
		return Flash_Address_Error;
	}
	crc = hid_buf[6+len];
	my_crc = crc8(0x00, hid_buf, 6+len);
	if(crc != my_crc) {
		// Invalid crc - reject this command
		return Flash_CRC_Error;
	}
	
	retval = flash_program(addr, FLASH_PAGE_SIZE, flash_page_buf);
	
	memset(flash_page_buf, 0xFF, FLASH_PAGE_SIZE);
	
	return retval;
}

static Flash_Error_T cache_sig_data(const uint8_t* hid_buf) {
	// Plot twist - exactly the same function as caching the application data
	// Uses a different buffer though.
	// That's a little safer, as the PC script doesn't have to be careful
	// not to program Flash after caching signature data, or vice versa.
	uint8_t cmd, len, crc, my_crc;
	uint16_t sig_buffer_start;
	uint32_t addr;
	cmd = hid_buf[0];
	if(cmd != HID_CODE_FOR_WRITE_SIG) {
		return Flash_Command_Error;
	}
	len = hid_buf[1];
	if((len < MIN_FLASH_WRITE_BYTES) || (len > MAX_FLASH_WRITE_BYTES))
	{
		return Flash_Size_Error;
	}
	addr = ((uint32_t)hid_buf[2])
	+ (((uint32_t) hid_buf[3]) << 8)
	+ (((uint32_t) hid_buf[4]) << 16)
	+(((uint32_t) hid_buf[5]) << 24);
	if((addr >= FLASH_PAGE_SIZE) ){
		// start address must be inside 1 page. We get 512 bytes (1 page) for user signature
		return Flash_Address_Error;
	}
	uint64_t addr_plus_len = ((uint64_t) addr) + ((uint64_t) len);
	if((addr_plus_len > UINT32_MAX) ||
		(addr_plus_len > FLASH_PAGE_SIZE)) {
		// data written must be within signature space, too
		return Flash_Address_Error;
	}
	crc = hid_buf[6+len];
	my_crc = crc8(0x00, hid_buf, 6+len);
	if(crc != my_crc) {
		// Invalid crc - reject this command
		return Flash_CRC_Error;
	}
	
	// Everything is good, place this chunk of data within the flash programming buffer
	sig_buffer_start = (addr)/sizeof(uint32_t);
	memcpy(&(sig_page_buf[sig_buffer_start]),  &(hid_buf[6]), len);
	
	return Flash_No_Error;
}

static Flash_Error_T program_cached_sig(const uint8_t* hid_buf) {
	Flash_Error_T retval;
	if(hid_buf[0] != HID_CODE_FOR_PROGRAM_SIG) {
		return Flash_Command_Error;
	}
	retval = flash_erase_signature();
	if(retval != Flash_No_Error) 
	{
		return retval;
	}
	retval = flash_program_signature(sig_page_buf);
	
	memset(sig_page_buf, 0xFF, FLASH_PAGE_SIZE);
	
	return retval;
}

static uint32_t read_bl_length(void) {
	// App length saved in a preset location "__app_size" defined in linker script
	uint32_t* p_app_len = ((uint32_t*)(&__bl_size)); //
	uint32_t app_len = *p_app_len;
	
	return app_len;
}

static uint32_t read_bl_crc(void) {
	// App CRC appended at the end of the app in Flash
	uint32_t bl_address = (uint32_t)(&__bl_address);
	uint32_t bl_len = read_bl_length() - 4; // offset by -4 as the length includes the CRC
	uint32_t bl_crc_stored = *(uint32_t*)(bl_address + bl_len); 
	
	return bl_crc_stored;
}

static uint32_t calc_bl_crc(void) {
	// Calculate the CRC32 of the bootloader in Flash
	// Uses the same CRC function as the built-in ROM bootloader
	
	uint8_t* bl_address = (uint8_t*)(&__bl_address);
	uint32_t bl_crc_local = calc_end_crc(bl_address, read_bl_length() - 4); // offset by -4 as the length includes the CRC
	
	return bl_crc_local;
}

void FLEXCOM4_Handler(void) {
	i2c_irq_handler(&h_i2c4);
}