/*
 * clock_config.c
 *
 * Created: 12/20/2021 12:19:02 PM
 *  Author: david
 */ 

#include "sam.h"
#include "main.h"

/* Global system clock variable */
uint32_t SystemClockFrequency;

static void set_flash_waitstates(uint32_t ul_clk);

void ClockConfig(void) {
	
	uint32_t unique_id[32];
	EFC->EEFC_FMR = EEFC_FMR_FWS(5)|EEFC_FMR_CLOE;// set max flash wait states
	// Set SCLK to external crystal
	
#if   defined(__SAMG55G19__) || defined(__ATSAMG55G19__)
	SUPC->SUPC_MR |= SUPC_MR_KEY_PASSWD | SUPC_MR_OSCBYPASS; // bypass mode
#endif

	if((SUPC->SUPC_SR & SUPC_SR_OSCSEL) == SUPC_SR_OSCSEL_RC) {
		// Was set to RC oscillator, switch to xtal
		SUPC->SUPC_CR = SUPC_CR_KEY_PASSWD | SUPC_CR_XTALSEL;
		// Wait until it's ready
		// check crystal status in supply control and power management
		while(!((SUPC->SUPC_SR & SUPC_SR_OSCSEL)
			&& (PMC->PMC_SR & PMC_SR_OSCSELS))) {
			// TODO: implement timeout
		}
	}
	
	// Initialize PLLs
	
	// PLLA 
	CKGR_PLLAR_Type pllaconfig = {
		.bit.PLLAEN = 1,
		.bit.PLLACOUNT = 0x3fU,
		.bit.MULA = 0, // always stop PLL first
		.bit.ZERO = 0 
	};
	PMC->CKGR_PLLAR = pllaconfig.reg; 
	pllaconfig.bit.MULA = CONFIG_CLOCK_PLLAMULT - 1;
	PMC->CKGR_PLLAR = pllaconfig.reg;
	
	// Wait for PLLA ready
	while(! (PMC->PMC_SR & PMC_SR_LOCKA)) {
		// TODO: implement timeout
	}
	
	//PLLB
	CKGR_PLLBR_Type pllbconfig = {
		.bit.PLLBEN = 1,
		.bit.PLLBCOUNT = 0x3fU,
		.bit.MULB = 0, // always stop PLL first
		.bit.ZERO = 0
	};
	PMC->CKGR_PLLBR = pllbconfig.reg;
	pllbconfig.bit.MULB = CONFIG_CLOCK_PLLBMULT - 1;
	PMC->CKGR_PLLBR = pllbconfig.reg;
	
	// Wait for PLLB ready
	while(!(PMC->PMC_SR & PMC_SR_LOCKB)) {
		// TODO: implement timeout
	}
	
	// switch main clock to PLLA
	PMC->PMC_MCKR = PMC_MCKR_CSS_PLLA_CLK;
	while(!(PMC->PMC_SR & PMC_SR_MCKRDY)) {
		// TODO: implement timeout
	}
	
	// Update system clock frequency
	SystemClockFrequency = CONFIG_CLOCK_PLLAMULT * FREQ_XTAL_32K;
	
	// Set flash wait states
	set_flash_waitstates(SystemClockFrequency);
	
	/* Set the trim value when system run near 120M */

	if ((SystemClockFrequency <= (CHIP_FREQ_CPU_MAX + (CHIP_FREQ_CPU_MAX >> 3))) &&
	(SystemClockFrequency >= (CHIP_FREQ_CPU_MAX - (CHIP_FREQ_CPU_MAX >> 3)))) {
		
		efc_read_unique_id(unique_id, 32);

		uint32_t trim_value = unique_id[16] & 0x0000FFFF;
		
		// set regulator trim value
		uint32_t ul_pwmr = SUPC->SUPC_PWMR & (~(0xFu << 9));
		SUPC->SUPC_PWMR = SUPC_PWMR_KEY_PASSWD | ul_pwmr | SUPC_PWMR_ECPWRS
			| ((trim_value & 0xFu) << 9);
	}

}

static void set_flash_waitstates(uint32_t ul_clk)
{
	/* Set FWS for embedded Flash access according to operating frequency */
	if (ul_clk < CHIP_FREQ_FWS_0) {
		EFC->EEFC_FMR = EEFC_FMR_FWS(0)|EEFC_FMR_CLOE;
	} else if (ul_clk < CHIP_FREQ_FWS_1) {
		EFC->EEFC_FMR = EEFC_FMR_FWS(1)|EEFC_FMR_CLOE;
	} else if (ul_clk < CHIP_FREQ_FWS_2) {
		EFC->EEFC_FMR = EEFC_FMR_FWS(2)|EEFC_FMR_CLOE;
	} else if (ul_clk < CHIP_FREQ_FWS_3) {
		EFC->EEFC_FMR = EEFC_FMR_FWS(3)|EEFC_FMR_CLOE;
	} else if (ul_clk < CHIP_FREQ_FWS_4) {
		EFC->EEFC_FMR = EEFC_FMR_FWS(4)|EEFC_FMR_CLOE;
	} else {
		EFC->EEFC_FMR = EEFC_FMR_FWS(5)|EEFC_FMR_CLOE;
	}
}


__attribute__((__noinline__))
__attribute__ ((section(".ramfunc")))
void efc_read_unique_id(uint32_t *p_ul_buf, uint32_t ul_size) {
	
	uint32_t *p_ul_data = (uint32_t *) IFLASH_ADDR;
	volatile uint32_t ul_status;
	EFC->EEFC_FMR |= (0x1u << 16); // disable sequential code optimization 
	EFC->EEFC_FCR = EEFC_FCR_FKEY_PASSWD | EEFC_FCR_FCMD_STUI; // start operation: read ID
	
	// Wait for completion (frdy falling edge)
	do {
		ul_status = EFC->EEFC_FSR;
	} while ((ul_status & EEFC_FSR_FRDY) == EEFC_FSR_FRDY);
	
	/* The data is located in the first address of the Flash
	 * memory mapping.
	 */
	for (uint32_t ul_cnt = 0; ul_cnt < ul_size; ul_cnt++) {
		p_ul_buf[ul_cnt] = p_ul_data[ul_cnt];
	}
	// stop the read mode
	EFC->EEFC_FCR = EEFC_FCR_FKEY_PASSWD | EEFC_FCR_FCMD_SPUI;
	
	// wait for FRDY rising edge
	do {
		ul_status = EFC->EEFC_FSR;
	} while ((ul_status & EEFC_FSR_FRDY) != EEFC_FSR_FRDY);
	
	EFC->EEFC_FMR &= ~(0x1u << 16); //enable sequential code optimization
}