/*
 * 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;

void USBInit(void);
void PinsInit(void);
uint8_t usb3803_init(void);
uint8_t usb3803_set_ilock(uint8_t i2caddr);
uint8_t usb3803_clear_ilock(uint8_t i2caddr);
uint8_t usb3803_set_registers(uint8_t i2caddr);
// uint8_t usb2517_init(void);
// void usb2517_mem_write(I2C_Handle* hi2c, uint8_t reg_addr, uint8_t reg_val);

void _app_exec(uint32_t app_address);
static bool check_app(uint32_t app_address);
static uint32_t read_app_length(void);
static uint32_t read_app_crc(void);
static uint32_t calc_app_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
#define USB3803_ADDRESS_3	(0x52u) // 3rd hub address

/*
#define USB2517_ADDRESS		(0x58u) // For use with UNSHIFTED address type, 0b0101.100W, where W is the read/write bit
#define USB2517_REG_CFG1            (0x06u) // Config Byte 1
#define USB2517_REG_CFG2            (0x07u) // Config Byte 2
#define USB2517_REG_CFG3            (0x08u) // Config Byte 3
#define USB2517_REG_NONREMOVDEV     (0x09u) // Non removable devices
#define USB2517_REG_PORTDIS_SELF    (0x0Au) // Port disable in self-powered configuration
#define USB2517_REG_PORTDIS_BUS     (0x0Bu) // Port disable in bus-powered
#define USB2517_REG_MAX_POWER_SELF  (0x0Cu) // Maximum power (mA) in self-powered
#define USB2517_REG_MAX_POWER_BUS   (0x0Du) // Max power in bus-powered
#define USB2517_REG_MAX_CURR_SELF   (0x0Eu) // Hub controller max current (mA) in self-powered
#define USB2517_REG_MAX_CURR_BUS    (0x0Fu) // Hub controller max current in bus-powered
#define USB2517_REG_BOOST_UP        (0xF6u) // Signal drive boost strength for upstream port
#define USB2517_REG_STATUS_CMD      (0xFFu) // Power down SMBUS, register reset, and USB attach (which locks 0x00-0xFE)

#define USB2517_CFG1_CURRENT_SENS_DISABLE   (0x06u) // Port current sensing disabled
#define USB2517_CFG1_MTT_ENABLE     (0x10u) // 0-single TT for all ports, 1-multi TT, one per port
#define USB2517_CFG1_SELF_BUS_PWR   (0x80u) // 0-Bus-powered, 1-Self-powered

#define USB2517_CFG2_COMPOUND       (0x08u) // 0-not a compound device, 1-compound device, non-removable must be defined too

#define USB2517_BOOST_DRIVE_NORMAL  (0x00u) // No boost
#define USB2517_BOOST_DRIVE_LOW     (0x01u) // Approx 4% boost
#define USB2517_BOOST_DRIVE_MEDIUM  (0x02u) // 8% boost
#define USB2517_BOOST_DRIVE_HIGH    (0x03u) // 12% boost
#define USB2517_BOOST_UP(boost_amt)     ((boost_amt) << 0u) // Reg 0xF6, bits 1:0

#define USB2517_STATUS_USB_ATTACH   (0x01u) // After setting 1, hub will attach to an upstream host and lock registers 0x00 - 0xFE
#define USB2517_STATUS_RESET        (0x02u) // 1 will reset all registers to default state
#define USB2517_STATUS_INF_PW_DN    (0x04u) // 1 will disable the SMbus interface after the ACK completes for this write cycle
*/

// 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;

board_ver_t board_ver = BOARD_VER_UNKNOWN;

int main(void)
{
	uint8_t reset_count;
    /* Pause watchdog timer */
    WDT->WDT_MR = WDT_MR_WDDIS;
	__disable_irq();
    /* Initialize the SAM system */
    SystemInit();
    /* Configure clock as customized */
    ClockConfig();
	if((GPBR->SYS_GPBR[0] == BKUP_REG0_CODE_FOR_ROM_BOOTLOADER)
	&& ((GPBR->SYS_GPBR[1] & BKUP_REG1_MASK_FOR_ROM_BOOTLOADER) == BKUP_REG1_MASK_FOR_ROM_BOOTLOADER))
	{
		// Count how many resets
		reset_count = GPBR->SYS_GPBR[1] & 0xFF;
		if(reset_count >= 9) {
			GPBR->SYS_GPBR[0] = 0;
			GPBR->SYS_GPBR[1] = 0;
		}
		else
		{
			GPBR->SYS_GPBR[1] = BKUP_REG1_MASK_FOR_ROM_BOOTLOADER + (reset_count + 1);
		}
		// Trigger hardware (user) reset by asserting the reset pin
		RSTC->RSTC_CR = RSTC_CR_KEY_PASSWD | RSTC_CR_EXTRST_Msk;
	}
	if(check_app((uint32_t)(&__app_address))) {
		// Valid app exists
		
		// Check if forced entry to USB bootloader (skip app check) is enabled
		if((GPBR->SYS_GPBR[0] == BKUP_REG0_CODE_FOR_USB_BOOTLOADER)
		 && (GPBR->SYS_GPBR[1] == BKUP_REG1_CODE_FOR_USB_BOOTLOADER)) {
			GPBR->SYS_GPBR[0] = 0;
			GPBR->SYS_GPBR[1] = 0;
		}
		// Check if forced entry to ROM bootloader is enabled
		else
		{		
			// Nothing special was enabled, go straight to application
			if(boot_on_check) {
				_app_exec((uint32_t)(&__app_address));
			}
		}
	}
	// 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();
	//if(pdFAIL == usb3803_init()) {
		//usb2517_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_BOOT_APP:
			// load the booted app
			//_app_exec((uint32_t)(&__app_address));
			// Just reboot the processor. If app is valid, it will get booted into
			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_APP:
			// Clear the entire app region
			// Warning: this will delete variables saved in the emulated EEPROM,
			// as that EEPROM is simply the last two 8kB sections of Flash
			status = flash_erase_app_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_app_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_app_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;
	}
}

uint8_t usb3803_init(void) {
	
	// 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
	if(pdFAIL == usb3803_set_ilock(USB3803_ADDRESS_1)) {
		return pdFAIL;
	}

	// Now repeat for hub 2 and 3
	if(pdFAIL == usb3803_set_ilock(USB3803_ADDRESS_2)) {
		return pdFAIL;
	}
	if(pdFAIL == usb3803_set_ilock(USB3803_ADDRESS_3)) {
		// don't fail out here, only BS2 has three hubs. BS1 has 2.
		board_ver = BOARD_VER_BS1;
		// Re-init I2C to clear failures
		init_i2c(&h_i2c4, 100000);
		i2c_lockup_clear_sequence(&h_i2c4);
	} else {
		board_ver = BOARD_VER_BS2;
	}
	
	// Now that hubs are paused in their bootup state machine,
	// it's time to apply settings.
	if(pdFAIL == usb3803_set_registers(USB3803_ADDRESS_1)) {
		return pdFAIL;
	}
	if(pdFAIL == usb3803_set_registers(USB3803_ADDRESS_2)) {
		return pdFAIL;
	}	
	if(board_ver == BOARD_VER_BS2) {
		if(pdFAIL == usb3803_set_registers(USB3803_ADDRESS_3)) {
			return pdFAIL;
		}
	}
	
	// Clear ilock bit so hubs start up
	if(pdFAIL == usb3803_clear_ilock(USB3803_ADDRESS_1)) {
		return pdFAIL;
	}
	if(pdFAIL == usb3803_clear_ilock(USB3803_ADDRESS_2)) {
		return pdFAIL;
	}
	if(board_ver == BOARD_VER_BS2) {
		if(pdFAIL == usb3803_clear_ilock(USB3803_ADDRESS_3)) {
			return pdFAIL;
		}
	}
	
	return pdPASS;
}

uint8_t usb3803_set_ilock(uint8_t i2caddr) {
	I2C_Transaction_Request i2ctxn;
	uint8_t temp_byte = 0;
	i2ctxn.dev_addr = i2caddr;
	// Set the hub's ILOCK.config bit to pause startup
	i2ctxn.cb = NULL;
	i2ctxn.internal_addr = 0xE7; // "serial port interlock control" (SP_ILOCK)
	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
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
	temp_byte |= 0x01; // ILOCK.config_n
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
	return pdPASS;
}

uint8_t usb3803_clear_ilock(uint8_t i2caddr) {
	I2C_Transaction_Request i2ctxn;
	uint8_t temp_byte = 0;
	i2ctxn.dev_addr = i2caddr;
	// Set the hub's ILOCK.config bit to pause startup
	i2ctxn.cb = NULL;
	i2ctxn.internal_addr = 0xE7; // "serial port interlock control" (SP_ILOCK)
	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
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
	
	temp_byte &= (~0x01); // ILOCK.config_n
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
	return pdPASS;
}

uint8_t usb3803_set_registers(uint8_t i2caddr) {
	I2C_Transaction_Request i2ctxn;
	uint8_t temp_byte = 0;
	i2ctxn.cb = NULL;
	i2ctxn.data = &temp_byte;
	i2ctxn.dev_addr = i2caddr;
	i2ctxn.shift = I2C_Unshifted;
	
	// Set the power mode. Self-powered, 0mA draw from bus
	i2ctxn.internal_addr = 0x06;
	i2ctxn.len = 1;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
	
	temp_byte |= 0x80; // Self-powered
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
	
	// Set max power to zero everywhere
	temp_byte = 0;
	i2ctxn.internal_addr = 0x0C;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
	
	i2ctxn.internal_addr = 0x0D;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
		
	i2ctxn.internal_addr = 0x0E;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
	
	i2ctxn.internal_addr = 0x0F;
	i2c_transact(&h_i2c4, &i2ctxn);
	while(i2c_busy(&h_i2c4)) {}
		
	if(i2c_status(&h_i2c4) != I2C_SUCCESS) {
		return pdFAIL;
	}
	return pdPASS;
}

/*
uint8_t usb2517_init(void) {
		
	//// 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); // performs a software reset too
	//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);
	
	// Set hub reset
	HUB_RESET_PORT->PIO_CODR = (1 << HUB_RESET_PIN);
	Delay_Millis(10);
	i2c_lockup_clear_sequence(&h_i2c4);
	Delay_Millis(10);
	// Clear hub reset
	HUB_RESET_PORT->PIO_SODR = (1 << HUB_RESET_PIN);
	Delay_Millis(10);
	//
	// Disable current sense, enable multi-transaction translator, set self-powered
	usb2517_mem_write(&h_i2c4, USB2517_REG_CFG1, USB2517_CFG1_CURRENT_SENS_DISABLE | USB2517_CFG1_MTT_ENABLE | USB2517_CFG1_SELF_BUS_PWR);
		
	// check if the USB hub was not present.
	if(I2C_SUCCESS != h_i2c4.status) {
		return pdFAIL;
	}
	
	// set this as a compound device (permanently attached devices)
	usb2517_mem_write(&h_i2c4, USB2517_REG_CFG2, USB2517_CFG2_COMPOUND);
		
	// all devices except aux USB (port 1) are non removeable. port 7 is unused. bit 0 is reserved.
	
	usb2517_mem_write(&h_i2c4, USB2517_REG_NONREMOVDEV, 0x7C); // 0x7C = 0b01111100
		
	// disable port 7
	usb2517_mem_write(&h_i2c4, USB2517_REG_PORTDIS_SELF, 0x80);
	usb2517_mem_write(&h_i2c4, USB2517_REG_PORTDIS_BUS, 0x80);
		
	// set all max power to minimum
	
	usb2517_mem_write(&h_i2c4, USB2517_REG_MAX_POWER_SELF, 0x00);
	usb2517_mem_write(&h_i2c4, USB2517_REG_MAX_POWER_BUS, 0x00);
	usb2517_mem_write(&h_i2c4, USB2517_REG_MAX_CURR_SELF, 0x00);
	usb2517_mem_write(&h_i2c4, USB2517_REG_MAX_CURR_BUS, 0x00);
	
	// set upstream boost
	usb2517_mem_write(&h_i2c4, USB2517_REG_BOOST_UP, USB2517_BOOST_UP(USB2517_BOOST_DRIVE_MEDIUM));
	
	// set the hub attach
	usb2517_mem_write(&h_i2c4, USB2517_REG_STATUS_CMD, USB2517_STATUS_USB_ATTACH);
	
	return pdPASS;
}

void usb2517_mem_write(I2C_Handle* hi2c, uint8_t reg_addr, uint8_t reg_val) {
	I2C_Transaction_Request i2ctxn;
	uint8_t temp_bytes[2];
	i2ctxn.dev_addr = USB2517_ADDRESS;
	i2ctxn.cb = NULL;
	i2ctxn.data = temp_bytes;
	i2ctxn.len = 2;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2ctxn.internal_addr = reg_addr;
	temp_bytes[0] = 1; // length of bytes to write
	temp_bytes[1] = reg_val;
	i2c_transact(hi2c, &i2ctxn);
	while(i2c_busy(hi2c)) {} // wait for completion
}
*/

volatile uint32_t board_millis(void) {
	return systick_count;
}

void _app_exec(uint32_t app_address) {
	uint8_t i;
	// Disable IRQs
	__DMB();
	__ISB();
	__disable_irq();
	// Clear pending IRQs
	for(i = 0; i < 8; i++) {
		NVIC->ICER[i] = 0xFFFFFFFFu;
		NVIC->ICPR[i] = 0xFFFFFFFFu;
	}
	// Disable Systick, USB
	SysTick->CTRL = 0;
	// disable both pullup and transceiver
	UDP->UDP_TXVC = UDP_TXVC_TXVDIS_Msk;
	// Modify vector table location 
	__DSB();
	__ISB();
	SCB->VTOR = ((uint32_t)app_address & SCB_VTOR_TBLOFF_Msk);
	// Set new stack pointer and jump to app
	void (*pApp)(void) = (void(*)(void)) *((uint32_t *)(app_address + 4));
	__DMB();
	__ISB();
	__set_MSP(*((uint32_t *)(app_address)));
	pApp();
}

static Flash_Error_T erase_flash_command(const uint8_t* hid_buf) {
	Flash_Erase_Size_T erasesize;
	uint32_t erase_start_addr, app_address, end_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);

	app_address = (uint32_t)(&__app_address);
	end_of_flash = (uint32_t)(&__eflash);

	// Check address is within app region of Flash
	if((erase_start_addr >= end_of_flash) || (erase_start_addr < app_address)) {
		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 != 0x00404000)
			&& (erase_start_addr != 0x00420000)
			&& (erase_start_addr != 0x00440000)
			&& (erase_start_addr != 0x00460000) ) {
				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, end_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);
	end_of_flash = (uint32_t)(&__eflash);
	if((addr < app_address) || (addr >= end_of_flash) ){
		// 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 > (uint32_t)(&__eflash))) {
		// 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;
	}
	
	// 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, end_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);
	end_of_flash = (uint32_t)(&__eflash);
	if((addr < app_address) || (addr >= end_of_flash) ){
		// 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 > (uint32_t)(&__eflash))) {
		// 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 + 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;
}

// Looks at the application Flash area and checks if there is a valid
// app living there.
// Simply checks the top of stack and reset vectors to make sure they
// are in a valid range
static bool check_app(uint32_t app_address) {
	// Grab the beginning of the app's vector table
	// First 4 bytes are initial stack pointer, second set of 4 bytes are the reset vector jump address
	// Default for Arm Cortex processors
	uint32_t start_of_flash = (uint32_t)(&__sflash);
	uint32_t end_of_flash = (uint32_t)(&__eflash);
	uint32_t start_of_ram = (uint32_t)(&__sram);
	uint32_t end_of_ram = (uint32_t)(&__eram);
	
	uint32_t top_of_stack = (uint32_t)(*((uint32_t *)(app_address))); // turn app_address into a pointer, dereference it
	uint32_t app_reset_vector = (uint32_t)(*((uint32_t *)(app_address + 4))); // same but the next 4 bytes (reset vector)
	
	// top of stack in valid region (within ram)
	if((top_of_stack < start_of_ram) || (top_of_stack >= end_of_ram)) 
	{
		return false;
	}
	
	// reset vector in valid region (in flash)
	if((app_reset_vector < start_of_flash) || (app_reset_vector >= end_of_flash))
	{
			return false;
	}
	
	uint32_t app_len = read_app_length();
	uint64_t addr_plus_len = ((uint64_t)app_address) + ((uint64_t)app_len);
	// Check validity of app_length. Anything between the size of a vector table and the rest of the memory is okay
	if(addr_plus_len > UINT32_MAX) {
		return false;
	}
	
	if((app_len < sizeof(DeviceVectors)) || (addr_plus_len > (uint32_t)(&__eflash))) {
		return false;
	}
	
	// Now feed the entire app segment into the CRC calculator.
	// from the beginning (its vector table) all the way to the end 
	// defined by this app_len we just found, and including the value of "app_len", too!
	// Invert the bits to match the BZIP2 CRC32 calculation...
	// Initial value = 0xFFFF_FFFF
	// Polynomial = 0x04C1_1DB7
	// Final xor value = 0xFFFF_FFFF (equivalent to "NOT"ing the whole thing)
	// Don't reverse input data, don't reverse output data
	
	uint32_t app_crc_local = calc_app_crc();
	
	// Now grab the app's saved CRC, which is right after the section we just calculated CRC on
	uint32_t app_crc_stored = read_app_crc();
	
	// and compare
	return (app_crc_local == app_crc_stored);
}

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

static uint32_t read_app_crc(void) {
	// App CRC appended at the end of the app in Flash
	uint32_t app_address = (uint32_t)(&__app_address);
	uint32_t app_len = read_app_length();
	uint32_t app_crc_stored = *(uint32_t*)(app_address + app_len);
	
	return app_crc_stored;
}

static uint32_t calc_app_crc(void) {
	// Calculate the CRC32 of the application in Flash using hardware CRC unit
	// CRC32 is computed on the entire Flash region for the application, using the
	// size field to determine the total length to compute over. 
	// CRC32 uses "BZIP2" format for the calculation, which needs to be 
	// inverted after calculation... thus the "~" operator on the crc32_hw() fcn call
	
	uint8_t* app_address = (uint8_t*)(&__app_address);
	uint32_t app_crc_local = ~(crc32_hw(app_address, read_app_length()));
	
	return app_crc_local;
}

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