/*
 * flash_prog.c
 *
 * Created: 12/20/2021 2:57:11 PM
 *  Author: david
 */ 

#include "sam.h"
#include "flash_prog.h"
#include <string.h>

extern uint32_t __sflash;

extern Flash_Error_T perform_flash_func(uint32_t flashcmd);
extern Flash_Error_T perform_flash_func_with_result(uint32_t flashcmd, uint32_t* result);

__attribute__((__noinline__)) __attribute__ ((section(".ramfunc")))
Flash_Error_T perform_flash_func(uint32_t flashcmd) {
	volatile uint32_t ul_status;

	EFC->EEFC_FCR = flashcmd;
	do {
		ul_status = EFC->EEFC_FSR;
	} while ((ul_status & EEFC_FSR_FRDY) != EEFC_FSR_FRDY);

	if((ul_status & EEFC_FSR_FCMDE) != 0) return Flash_Command_Error;
	if((ul_status & EEFC_FSR_FLERR) != 0) return Flash_Verify_Error;
	if((ul_status & EEFC_FSR_FLOCKE) != 0) return Flash_Lock_Error;
	
	return Flash_No_Error;
}

__attribute__((__noinline__)) __attribute__ ((section(".ramfunc")))
Flash_Error_T perform_flash_func_with_result(uint32_t flashcmd, uint32_t* result) {
	volatile uint32_t ul_status;

	EFC->EEFC_FCR = flashcmd;
	do {
		ul_status = EFC->EEFC_FSR;
	} while ((ul_status & EEFC_FSR_FRDY) != EEFC_FSR_FRDY);

	if((ul_status & EEFC_FSR_FCMDE) != 0) return Flash_Command_Error;
	if((ul_status & EEFC_FSR_FLERR) != 0) return Flash_Verify_Error;
	if((ul_status & EEFC_FSR_FLOCKE) != 0) return Flash_Lock_Error;
	
	*result = EFC->EEFC_FRR;
	
	return Flash_No_Error;
}

__attribute__((__noinline__)) __attribute__((section(".ramfunc")))
Flash_Error_T read_flash_region(uint32_t flashcmd_1, uint32_t flashcmd_2, uint32_t* data, uint32_t datalen) {
	volatile uint32_t ul_status;
	uint32_t* data_region;
	data_region = (uint32_t*)(&__sflash);
	
	EFC->EEFC_FCR = flashcmd_1;
	do {
		ul_status = EFC->EEFC_FSR;
		
	}while ((ul_status & EEFC_FSR_FRDY) != 0);
	
	if((ul_status & EEFC_FSR_FCMDE) != 0) return Flash_Command_Error;
	if((ul_status & EEFC_FSR_FLERR) != 0) return Flash_Verify_Error;
	if((ul_status & EEFC_FSR_FLOCKE) != 0) return Flash_Lock_Error;
	for(uint32_t i = 0; i < datalen; i++) {
		data[i] = data_region[i];
	}
	
	EFC->EEFC_FCR = flashcmd_2;
	do {
		ul_status = EFC->EEFC_FSR;
		
	}while ((ul_status & EEFC_FSR_FRDY) != EEFC_FSR_FRDY);
	if((ul_status & EEFC_FSR_FCMDE) != 0) return Flash_Command_Error;
	if((ul_status & EEFC_FSR_FLERR) != 0) return Flash_Verify_Error;
	if((ul_status & EEFC_FSR_FLOCKE) != 0) return Flash_Lock_Error;
	
	return Flash_No_Error;
}


Flash_Error_T flash_erase(uint32_t start_addr, Flash_Erase_Size_T erase_size) {

	// Operation format depends on the erase size.
	// the FARG register region is formatted as follows:
	//	- erasing 8kB (16 pages)
	//		FARG[15:4] = start page number to be erased divided by 16
	//		FARG[3:2] = 0
	//		FARG[1:0] = 2
	//  - erasing 16kB (32 pages)
	//		FARG[15:5] = start page number to be erased divided by 32
	//		FARG[4:2] = 0
	//		FARG[1:0] = 3		
	//	- erase sector (112kB or 128kB, depending on the sector selected)
	//		FARG[15:0] = any page within that sector

	Flash_Error_T status;
	uint32_t farg = 0;
	uint32_t fcmd;
	uint32_t start_page = (start_addr - IFLASH_ADDR) / IFLASH_PAGE_SIZE;

	switch(erase_size) {
	case Flash_Erase_8kB:
		farg |= 2; // Command to erase 8kB / 16 pages
		farg |= ((start_page/16) << 4);
		fcmd = EEFC_FCR_FKEY_PASSWD | EEFC_FCR_FARG(farg) | Flash_Command_ErasePages;
	break;

	case Flash_Erase_16kB:
		farg |= 3; // Command to erase 16kB / 32 pages
		farg |= ((start_page/32) << 5);
		fcmd = EEFC_FCR_FKEY_PASSWD | EEFC_FCR_FARG(farg) | Flash_Command_ErasePages;
	break;

	case Flash_Erase_Sector:
		farg = start_page;
		fcmd = EEFC_FCR_FKEY_PASSWD | EEFC_FCR_FARG(farg) | Flash_Command_EraseSector;
	break;

	default:
		return Flash_Command_Error;
	break;
	}

	uint32_t cur_int_status = __get_PRIMASK();
	if(cur_int_status == 0 ){
		__disable_irq();
	}

	status = perform_flash_func(fcmd);

	if(cur_int_status == 0 ){
		__enable_irq();
	}

	return status;
}

Flash_Error_T flash_erase_bl_region(void) {
	uint32_t start_addr;
	Flash_Error_T status;
	
	// Erase the two smaller sections of sector 0 (the two little 8kB sub-sectors)
	start_addr = IFLASH_ADDR; 
	status = flash_erase(start_addr, Flash_Erase_Sector);

	if(status != Flash_No_Error) return status;
	
	start_addr = IFLASH_ADDR + FLASH_LITTLE_SECTOR_SIZE;
	status = flash_erase(start_addr, Flash_Erase_Sector);
	
	return status;
}

Flash_Error_T flash_program(uint32_t start_addr, uint32_t len_bytes, uint32_t* data) {
	uint32_t cmd;
	uint32_t* dest_addr, *page_start_addr;
	uint32_t prefill, postfill;
	uint16_t i, page;
	Flash_Error_T status;
	
	uint32_t cur_int_status = __get_PRIMASK();
	if(cur_int_status == 0 ){
		__disable_irq();
	}
	// Flash programming must be performed in multiples of 64-bit address aligned words
	// The entire 64 bit word must already have been erased (pages erase or sector erase command)
	// Programming is performed by writing to the location in flash, then executing the Write Page command
	// Writing to flash actually writes to a page buffer. This buffer could be already filled with junk
	// which then just clobbers the flash memory except for the specific bytes written to. To 
	// prevent this from happening, we need to write an entire flash page's worth of data with 
	// 0xFFFFFFFF to all locations that we aren't programming.
	// Also the flash page buffer must be written sequentially.
	
	page = (start_addr - IFLASH_ADDR) / IFLASH_PAGE_SIZE;
	page_start_addr = (uint32_t*)(IFLASH_ADDR + (IFLASH_PAGE_SIZE * page));
	prefill = ((uint32_t)start_addr) - ((uint32_t)page_start_addr);
	postfill = ((uint32_t)page_start_addr) + IFLASH_PAGE_SIZE - (((uint32_t)start_addr) + len_bytes);
	 
	// Copy empty bytes to prefill area
	for(i = 0; i < (prefill / sizeof(uint32_t)); i++) {
		page_start_addr[i] = 0xFFFFFFFFUL;
	}
	
	// Copy bytes to the destination flash page buffer
	// Must be performed in 32 bit writes. 8 or 16 bit writes could result in weird stuff
	dest_addr = (uint32_t*) start_addr;
	for(i = 0; i < (len_bytes / 4); i++) {
		dest_addr[i] = data[i];
	}
	
	// Copy empty bytes after our destination area
	page_start_addr = (uint32_t*)((uint32_t)start_addr + len_bytes);
	for(i = 0; i < (postfill / sizeof(uint32_t)); i++) {
		page_start_addr[i] = 0xFFFFFFFFUL;
	}
	
	// Trigger the flash page write
	
	cmd = EEFC_FCR_FKEY_PASSWD | EEFC_FCR_FARG(page) | Flash_Command_WritePage;
	status = perform_flash_func(cmd);
	if(status != Flash_No_Error) {
		if(cur_int_status == 0 ){
			__enable_irq();	
		}
		return (uint8_t) status;
	}
	// Verify
	uint32_t miss_count = 0;
	for(i = 0; i < len_bytes; i+=4) {
		if(data[i/4] != *((uint32_t*)(start_addr + i)))
			miss_count++;
	}
	if(cur_int_status == 0 ){
		__enable_irq();
	}
	if(miss_count != 0) return Flash_Verify_Error;
	
	return Flash_No_Error;
}

Flash_Error_T flash_change_NVM_bits(uint16_t bitmask, Flash_Config_Set_T set_or_clear) {
	uint32_t cmd;
	Flash_Error_T retval;
	//uint8_t irq_was_enabled;
	
	uint32_t cur_int_status = __get_PRIMASK();
	if(cur_int_status == 0 ){
		__disable_irq();
	}
	// Clear or set nvm bits are done one at a time
	// there's only one NVM bit we care about on the ATSAMG55, bit #1 which is the bootloader disable
	// so bitmask should only ever be 0x01
	if(bitmask != 0x01) return Flash_Command_Error;
	
	switch(set_or_clear) {
		case Flash_Config_Clear:
			cmd = EEFC_FCR_FKEY_PASSWD | EEFC_FCR_FARG(bitmask) | Flash_Command_ClearGPNVM;
			retval = perform_flash_func(cmd);
			break;
		case Flash_Config_Set:
			cmd = EEFC_FCR_FKEY_PASSWD | EEFC_FCR_FARG(bitmask) | Flash_Command_SetGPNVM;
			retval = perform_flash_func(cmd);
			break;
		default:
			retval = Flash_Command_Error;
	}
	if(cur_int_status == 0) {
		__enable_irq();
	}
	return retval;
}

Flash_Error_T flash_read_NVM_bits(uint32_t* config_bits) {
	
	uint32_t cur_int_status = __get_PRIMASK();
	if(cur_int_status == 0 ){
		__disable_irq();
	}
		// NVM bits are simple commands that don't interfere with flash memory
		// But I don't trust the datasheet so we'll do it in ram anyway
	Flash_Error_T retval = perform_flash_func_with_result(EEFC_FCR_FKEY_PASSWD | Flash_Command_GetGPNVM, config_bits);
	
	if(cur_int_status == 0) {
		__enable_irq();
	}
	return retval;
}

Flash_Error_T flash_erase_signature(void) 
{
	uint32_t cur_int_status = __get_PRIMASK();
	if(cur_int_status == 0 ){
		__disable_irq();
	}

	Flash_Error_T retval = perform_flash_func(EEFC_FCR_FKEY_PASSWD | Flash_Command_EraseUserSig);
	
	if(cur_int_status == 0) {
		__enable_irq();
	}
	return retval;
	
}
Flash_Error_T flash_program_signature(uint32_t* data)
{
	uint32_t* first_flash_page; // can program to any valid page within Flash
	uint32_t cur_int_status = __get_PRIMASK();
	if(cur_int_status == 0 ){
		__disable_irq();
	}
	
	first_flash_page = (uint32_t*)(IFLASH_ADDR);

	// Copy bytes to the destination flash page buffer
	// Must be performed in 32 bit writes. 8 or 16 bit writes could result in weird stuff
	// Also, writes must be continuous (contiguous?) from lower to higher address.
	// So we can't use memcpy. Probably does random weird stuff in write sequence.
	
	// Making a (possibly stupid) assumption that data is at least one page (512 bytes) long
	for(uint16_t i = 0; i < (LEN_USER_SIG_BYTES / sizeof(uint32_t)); i++) {
		first_flash_page[i] = data[i];
	}
	
	// Perform program function
	Flash_Error_T retval = perform_flash_func(EEFC_FCR_FKEY_PASSWD | Flash_Command_WriteUserSig);
	
	if(cur_int_status == 0) {
		__enable_irq();
	}
		
	return retval;
}

Flash_Error_T flash_read_signature(uint32_t* data)
{
	Flash_Error_T retval;
	uint32_t cur_int_status = __get_PRIMASK();
	if(cur_int_status == 0 ){
		__disable_irq();
	}
	
	retval = read_flash_region(EEFC_FCR_FKEY_PASSWD | Flash_Command_StartReadUserSig, EEFC_FCR_FKEY_PASSWD | Flash_Command_StopReadUserSig, data, 512/sizeof(uint32_t));
	
	if(cur_int_status == 0) {
		__enable_irq();
	}
	
	return retval;
	
}