#include "clocks.h"
#include "conf_core.h"

#if CONF_CORE_CLK_XOSC0_FREQUENCY > 24000000
	#define CONF_CORE_CLK_XOSC0_IPTAT		(3)
	#define CONF_CORE_CLK_XOSC0_IMULT		(6)
	#define CONF_CORE_CLK_XOSC0_CFDPRESC	(0x0)
#elif CONF_CORE_CLK_XOSC0_FREQUENCY > 16000000
	#define CONF_CORE_CLK_XOSC0_IPTAT		(3)
	#define CONF_CORE_CLK_XOSC0_IMULT		(5)
	#define CONF_CORE_CLK_XOSC0_CFDPRESC	(0x1)
#elif CONF_CORE_CLK_XOSC0_FREQUENCY > 8000000
	#define CONF_CORE_CLK_XOSC0_IPTAT		(3)
	#define CONF_CORE_CLK_XOSC0_IMULT		(4)
	#define CONF_CORE_CLK_XOSC0_CFDPRESC	(0x2)
#else
	#define CONF_CORE_CLK_XOSC0_IPTAT		(2)
	#define CONF_CORE_CLK_XOSC0_IMULT		(3)
	#define CONF_CORE_CLK_XOSC0_CFDPRESC	(0x3)
#endif

#if CONF_CORE_CLK_XOSC1_FREQUENCY > 24000000
	#define CONF_CORE_CLK_XOSC1_IPTAT		(3)
	#define CONF_CORE_CLK_XOSC1_IMULT		(6)
	#define CONF_CORE_CLK_XOSC1_CFDPRESC	(0x0)
#elif CONF_CORE_CLK_XOSC1_FREQUENCY > 16000000
	#define CONF_CORE_CLK_XOSC1_IPTAT		(3)
	#define CONF_CORE_CLK_XOSC1_IMULT		(5)
	#define CONF_CORE_CLK_XOSC1_CFDPRESC	(0x1)
#elif CONF_CORE_CLK_XOSC1_FREQUENCY > 8000000
	#define CONF_CORE_CLK_XOSC1_IPTAT		(3)
	#define CONF_CORE_CLK_XOSC1_IMULT		(4)
	#define CONF_CORE_CLK_XOSC1_CFDPRESC	(0x2)
#else
	#define CONF_CORE_CLK_XOSC1_IPTAT		(2)
	#define CONF_CORE_CLK_XOSC1_IMULT		(3)
	#define CONF_CORE_CLK_XOSC1_CFDPRESC	(0x3)
#endif

void clock_osc32k_init(void)
{
#if CONF_CORE_CLK_XOSC32K_ENABLE == 1
	OSC32KCTRL->XOSC32K.bit.CGM =		CONF_CORE_CLK_XOSC32K_CGM;
	OSC32KCTRL->XOSC32K.bit.WRTLOCK =	CONF_CORE_CLK_XOSC32K_WRTLOCK;
	OSC32KCTRL->XOSC32K.bit.STARTUP =	CONF_CORE_CLK_XOSC32K_STARTUP_TIME;
	OSC32KCTRL->XOSC32K.bit.RUNSTDBY =	CONF_CORE_CLK_XOSC32K_RUNSTDBY;
	OSC32KCTRL->XOSC32K.bit.ONDEMAND =	CONF_CORE_CLK_XOSC32K_ONDEMAND;
	OSC32KCTRL->XOSC32K.bit.EN1K =		CONF_CORE_CLK_XOSC32K_EN1K;
	OSC32KCTRL->XOSC32K.bit.EN32K =	CONF_CORE_CLK_XOSC32K_EN32K;
	OSC32KCTRL->XOSC32K.bit.XTALEN =	CONF_CORE_CLK_XOSC32K_XTALEN;
	OSC32KCTRL->CFDCTRL.bit.CFDPRESC =	CONF_CORE_CLK_XOSC32K_CFDPRESC;
	OSC32KCTRL->CFDCTRL.bit.SWBACK =	CONF_CORE_CLK_XOSC32K_SWBACK;
	OSC32KCTRL->CFDCTRL.bit.CFDEN =	CONF_CORE_CLK_XOSC32K_CFDEN;

#if CONF_CORE_CLK_XOSC32K_ENABLE == 1 && CONF_CORE_CLK_XOSC32K_ONDEMAND == 0
	while(OSC32KCTRL->STATUS.bit.XOSC32KRDY == 0);
#endif

#endif
}
void clock_osc_init(void)
{
#if CONF_CORE_CLK_XOSC0_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	OSCCTRL->XOSCCTRL[0].bit.XTALEN =			CONF_CORE_CLK_XOSC0_XTALEN;
	OSCCTRL->XOSCCTRL[0].bit.RUNSTDBY =		CONF_CORE_CLK_XOSC0_RUNSTDBY;
	OSCCTRL->XOSCCTRL[0].bit.ONDEMAND =		CONF_CORE_CLK_XOSC0_ONDEMAND;
	OSCCTRL->XOSCCTRL[0].bit.LOWBUFGAIN =		CONF_CORE_CLK_XOSC0_LOWBUFGAIN;
	OSCCTRL->XOSCCTRL[0].bit.IPTAT =			CONF_CORE_CLK_XOSC0_IPTAT;
	OSCCTRL->XOSCCTRL[0].bit.IMULT =			CONF_CORE_CLK_XOSC0_IMULT;
	OSCCTRL->XOSCCTRL[0].bit.ENALC =			CONF_CORE_CLK_XOSC0_ENALC;
	OSCCTRL->XOSCCTRL[0].bit.CFDEN =			CONF_CORE_CLK_XOSC0_CFDEN;
	OSCCTRL->XOSCCTRL[0].bit.CFDPRESC =		CONF_CORE_CLK_XOSC0_CFDPRESC;
	OSCCTRL->XOSCCTRL[0].bit.SWBEN =			CONF_CORE_CLK_XOSC0_SWBEN;
	OSCCTRL->XOSCCTRL[0].bit.STARTUP =			CONF_CORE_CLK_XOSC0_STARTUP_TIME;
	OSCCTRL->XOSCCTRL[0].bit.ENABLE =			CONF_CORE_CLK_XOSC0_ENABLE;
	CRITICAL_SECTION_LEAVE();
	while(0 == OSCCTRL->STATUS.bit.XOSCRDY0);
#endif

#if CONF_CORE_CLK_XOSC1_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	OSCCTRL->XOSCCTRL[1].bit.XTALEN =			CONF_CORE_CLK_XOSC1_XTALEN;
	OSCCTRL->XOSCCTRL[1].bit.RUNSTDBY =		CONF_CORE_CLK_XOSC1_RUNSTDBY;
	OSCCTRL->XOSCCTRL[1].bit.LOWBUFGAIN =		CONF_CORE_CLK_XOSC1_LOWBUFGAIN;
	OSCCTRL->XOSCCTRL[1].bit.IPTAT =			CONF_CORE_CLK_XOSC1_IPTAT;
	OSCCTRL->XOSCCTRL[1].bit.IMULT =			CONF_CORE_CLK_XOSC1_IMULT;
	OSCCTRL->XOSCCTRL[1].bit.CFDEN =			CONF_CORE_CLK_XOSC1_CFDEN;
	OSCCTRL->XOSCCTRL[1].bit.CFDPRESC =		CONF_CORE_CLK_XOSC1_CFDPRESC;
	OSCCTRL->XOSCCTRL[1].bit.SWBEN =			CONF_CORE_CLK_XOSC1_SWBEN;
	OSCCTRL->XOSCCTRL[1].bit.STARTUP =			CONF_CORE_CLK_XOSC1_STARTUP_TIME;
	OSCCTRL->XOSCCTRL[1].bit.ENABLE =			CONF_CORE_CLK_XOSC1_ENABLE;
	CRITICAL_SECTION_LEAVE();
	while(0 == OSCCTRL->STATUS.bit.XOSCRDY1);
#endif

#if CONF_CORE_CLK_XOSC0_ENABLE == 1
	CRITICAL_SECTION_ENTER();
#if CONF_CORE_CLK_XOSC0_ENALC == 1
	OSCCTRL->XOSCCTRL[0].bit.ENALC =			CONF_CORE_CLK_XOSC0_ENALC;
#endif

#if CONF_CORE_CLK_XOSC0_ONDEMAND == 1
	OSCCTRL->XOSCCTRL[0].bit.ONDEMAND =		CONF_CORE_CLK_XOSC0_ONDEMAND;
#endif
	CRITICAL_SECTION_LEAVE();
#endif

#if CONF_CORE_CLK_XOSC1_ENABLE == 1
	CRITICAL_SECTION_ENTER();
#if CONF_CORE_CLK_XOSC1_ENALC == 1
	OSCCTRL->XOSCCTRL[1].bit.ENALC =			CONF_CORE_CLK_XOSC1_ENALC;
#endif

#if CONF_CORE_CLK_XOSC1_ONDEMAND == 1
	OSCCTRL->XOSCCTRL[1].bit.ONDEMAND =		CONF_CORE_CLK_XOSC1_ONDEMAND;
#endif
	CRITICAL_SECTION_LEAVE();
#endif
}

void clock_mclk_init(void)
{
	CRITICAL_SECTION_ENTER();
	MCLK->CPUDIV.reg = CONF_CORE_MCLK_CPUDIV;
	CRITICAL_SECTION_LEAVE();
}

void clock_gclk_init(void)
{
#if CONF_CORE_GCLK_0_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[0].bit.DIV = CONF_CORE_GCLK_0_DIV_VAL;
	GCLK->GENCTRL[0].bit.DIVSEL = CONF_CORE_GCLK_0_DIVSEL;
	GCLK->GENCTRL[0].bit.RUNSTDBY = CONF_CORE_GCLK_0_RUN_IN_STANDBY;
	GCLK->GENCTRL[0].bit.OE = CONF_CORE_GCLK_0_OUTPUT_ENABLE;
	GCLK->GENCTRL[0].bit.OOV = CONF_CORE_GCLK_0_DIVSEL;
	GCLK->GENCTRL[0].bit.IDC = CONF_CORE_GCLK_0_IDC;
	GCLK->GENCTRL[0].bit.GENEN = CONF_CORE_GCLK_0_ENABLE;
	GCLK->GENCTRL[0].bit.SRC = CONF_CORE_GCLK_0_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_1_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[1].bit.DIV = CONF_CORE_GCLK_1_DIV_VAL;
	GCLK->GENCTRL[1].bit.DIVSEL = CONF_CORE_GCLK_1_DIVSEL;
	GCLK->GENCTRL[1].bit.RUNSTDBY = CONF_CORE_GCLK_1_RUN_IN_STANDBY;
	GCLK->GENCTRL[1].bit.OE = CONF_CORE_GCLK_1_OUTPUT_ENABLE;
	GCLK->GENCTRL[1].bit.OOV = CONF_CORE_GCLK_1_DIVSEL;
	GCLK->GENCTRL[1].bit.IDC = CONF_CORE_GCLK_1_IDC;
	GCLK->GENCTRL[1].bit.GENEN = CONF_CORE_GCLK_1_ENABLE;
	GCLK->GENCTRL[1].bit.SRC = CONF_CORE_GCLK_1_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_2_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[2].bit.DIV = CONF_CORE_GCLK_2_DIV_VAL;
	GCLK->GENCTRL[2].bit.DIVSEL = CONF_CORE_GCLK_2_DIVSEL;
	GCLK->GENCTRL[2].bit.RUNSTDBY = CONF_CORE_GCLK_2_RUN_IN_STANDBY;
	GCLK->GENCTRL[2].bit.OE = CONF_CORE_GCLK_2_OUTPUT_ENABLE;
	GCLK->GENCTRL[2].bit.OOV = CONF_CORE_GCLK_2_DIVSEL;
	GCLK->GENCTRL[2].bit.IDC = CONF_CORE_GCLK_2_IDC;
	GCLK->GENCTRL[2].bit.GENEN = CONF_CORE_GCLK_2_ENABLE;
	GCLK->GENCTRL[2].bit.SRC = CONF_CORE_GCLK_2_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_3_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[3].bit.DIV = CONF_CORE_GCLK_3_DIV_VAL;
	GCLK->GENCTRL[3].bit.DIVSEL = CONF_CORE_GCLK_3_DIVSEL;
	GCLK->GENCTRL[3].bit.RUNSTDBY = CONF_CORE_GCLK_3_RUN_IN_STANDBY;
	GCLK->GENCTRL[3].bit.OE = CONF_CORE_GCLK_3_OUTPUT_ENABLE;
	GCLK->GENCTRL[3].bit.OOV = CONF_CORE_GCLK_3_DIVSEL;
	GCLK->GENCTRL[3].bit.IDC = CONF_CORE_GCLK_3_IDC;
	GCLK->GENCTRL[3].bit.GENEN = CONF_CORE_GCLK_3_ENABLE;
	GCLK->GENCTRL[3].bit.SRC = CONF_CORE_GCLK_3_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_4_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[4].bit.DIV = CONF_CORE_GCLK_4_DIV_VAL;
	GCLK->GENCTRL[4].bit.DIVSEL = CONF_CORE_GCLK_4_DIVSEL;
	GCLK->GENCTRL[4].bit.RUNSTDBY = CONF_CORE_GCLK_4_RUN_IN_STANDBY;
	GCLK->GENCTRL[4].bit.OE = CONF_CORE_GCLK_4_OUTPUT_ENABLE;
	GCLK->GENCTRL[4].bit.OOV = CONF_CORE_GCLK_4_DIVSEL;
	GCLK->GENCTRL[4].bit.IDC = CONF_CORE_GCLK_4_IDC;
	GCLK->GENCTRL[4].bit.GENEN = CONF_CORE_GCLK_4_ENABLE;
	GCLK->GENCTRL[4].bit.SRC = CONF_CORE_GCLK_4_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_5_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[5].bit.DIV = CONF_CORE_GCLK_5_DIV_VAL;
	GCLK->GENCTRL[5].bit.DIVSEL = CONF_CORE_GCLK_5_DIVSEL;
	GCLK->GENCTRL[5].bit.RUNSTDBY = CONF_CORE_GCLK_5_RUN_IN_STANDBY;
	GCLK->GENCTRL[5].bit.OE = CONF_CORE_GCLK_5_OUTPUT_ENABLE;
	GCLK->GENCTRL[5].bit.OOV = CONF_CORE_GCLK_5_DIVSEL;
	GCLK->GENCTRL[5].bit.IDC = CONF_CORE_GCLK_5_IDC;
	GCLK->GENCTRL[5].bit.GENEN = CONF_CORE_GCLK_5_ENABLE;
	GCLK->GENCTRL[5].bit.SRC = CONF_CORE_GCLK_5_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_6_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[6].bit.DIV = CONF_CORE_GCLK_6_DIV_VAL;
	GCLK->GENCTRL[6].bit.DIVSEL = CONF_CORE_GCLK_6_DIVSEL;
	GCLK->GENCTRL[6].bit.RUNSTDBY = CONF_CORE_GCLK_6_RUN_IN_STANDBY;
	GCLK->GENCTRL[6].bit.OE = CONF_CORE_GCLK_6_OUTPUT_ENABLE;
	GCLK->GENCTRL[6].bit.OOV = CONF_CORE_GCLK_6_DIVSEL;
	GCLK->GENCTRL[6].bit.IDC = CONF_CORE_GCLK_6_IDC;
	GCLK->GENCTRL[6].bit.GENEN = CONF_CORE_GCLK_6_ENABLE;
	GCLK->GENCTRL[6].bit.SRC = CONF_CORE_GCLK_6_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_7_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[7].bit.DIV = CONF_CORE_GCLK_7_DIV_VAL;
	GCLK->GENCTRL[7].bit.DIVSEL = CONF_CORE_GCLK_7_DIVSEL;
	GCLK->GENCTRL[7].bit.RUNSTDBY = CONF_CORE_GCLK_7_RUN_IN_STANDBY;
	GCLK->GENCTRL[7].bit.OE = CONF_CORE_GCLK_7_OUTPUT_ENABLE;
	GCLK->GENCTRL[7].bit.OOV = CONF_CORE_GCLK_7_DIVSEL;
	GCLK->GENCTRL[7].bit.IDC = CONF_CORE_GCLK_7_IDC;
	GCLK->GENCTRL[7].bit.GENEN = CONF_CORE_GCLK_7_ENABLE;
	GCLK->GENCTRL[7].bit.SRC = CONF_CORE_GCLK_7_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_8_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[8].bit.DIV = CONF_CORE_GCLK_8_DIV_VAL;
	GCLK->GENCTRL[8].bit.DIVSEL = CONF_CORE_GCLK_8_DIVSEL;
	GCLK->GENCTRL[8].bit.RUNSTDBY = CONF_CORE_GCLK_8_RUN_IN_STANDBY;
	GCLK->GENCTRL[8].bit.OE = CONF_CORE_GCLK_8_OUTPUT_ENABLE;
	GCLK->GENCTRL[8].bit.OOV = CONF_CORE_GCLK_8_DIVSEL;
	GCLK->GENCTRL[8].bit.IDC = CONF_CORE_GCLK_8_IDC;
	GCLK->GENCTRL[8].bit.GENEN = CONF_CORE_GCLK_8_ENABLE;
	GCLK->GENCTRL[8].bit.SRC = CONF_CORE_GCLK_8_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_9_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[9].bit.DIV = CONF_CORE_GCLK_9_DIV_VAL;
	GCLK->GENCTRL[9].bit.DIVSEL = CONF_CORE_GCLK_9_DIVSEL;
	GCLK->GENCTRL[9].bit.RUNSTDBY = CONF_CORE_GCLK_9_RUN_IN_STANDBY;
	GCLK->GENCTRL[9].bit.OE = CONF_CORE_GCLK_9_OUTPUT_ENABLE;
	GCLK->GENCTRL[9].bit.OOV = CONF_CORE_GCLK_9_DIVSEL;
	GCLK->GENCTRL[9].bit.IDC = CONF_CORE_GCLK_9_IDC;
	GCLK->GENCTRL[9].bit.GENEN = CONF_CORE_GCLK_9_ENABLE;
	GCLK->GENCTRL[9].bit.SRC = CONF_CORE_GCLK_9_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_10_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[10].bit.DIV = CONF_CORE_GCLK_10_DIV_VAL;
	GCLK->GENCTRL[10].bit.DIVSEL = CONF_CORE_GCLK_10_DIVSEL;
	GCLK->GENCTRL[10].bit.RUNSTDBY = CONF_CORE_GCLK_10_RUN_IN_STANDBY;
	GCLK->GENCTRL[10].bit.OE = CONF_CORE_GCLK_10_OUTPUT_ENABLE;
	GCLK->GENCTRL[10].bit.OOV = CONF_CORE_GCLK_10_DIVSEL;
	GCLK->GENCTRL[10].bit.IDC = CONF_CORE_GCLK_10_IDC;
	GCLK->GENCTRL[10].bit.GENEN = CONF_CORE_GCLK_10_ENABLE;
	GCLK->GENCTRL[10].bit.SRC = CONF_CORE_GCLK_10_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
#if CONF_CORE_GCLK_11_ENABLE == 1
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[11].bit.DIV = CONF_CORE_GCLK_11_DIV_VAL;
	GCLK->GENCTRL[11].bit.DIVSEL = CONF_CORE_GCLK_11_DIVSEL;
	GCLK->GENCTRL[11].bit.RUNSTDBY = CONF_CORE_GCLK_11_RUN_IN_STANDBY;
	GCLK->GENCTRL[11].bit.OE = CONF_CORE_GCLK_11_OUTPUT_ENABLE;
	GCLK->GENCTRL[11].bit.OOV = CONF_CORE_GCLK_11_DIVSEL;
	GCLK->GENCTRL[11].bit.IDC = CONF_CORE_GCLK_11_IDC;
	GCLK->GENCTRL[11].bit.GENEN = CONF_CORE_GCLK_11_ENABLE;
	GCLK->GENCTRL[11].bit.SRC = CONF_CORE_GCLK_11_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
}

void clock_dpll_init(void)
{
#if CONF_CORE_CLK_DPLL0_ENABLE == 1
#if CONF_CORE_CLK_DPLL0_REFCLK == 0
	CRITICAL_SECTION_ENTER();
	GCLK->PCHCTRL[OSCCTRL_GCLK_ID_FDPLL0].bit.GEN =
		GCLK_PCHCTRL_GEN(CONF_CORE_CLK_DPLL0_GCLK_SRC);
	GCLK->PCHCTRL[OSCCTRL_GCLK_ID_FDPLL0].bit.CHEN =
		CONF_CORE_CLK_DPLL0_ENABLE;
	CRITICAL_SECTION_LEAVE();
#endif

	CRITICAL_SECTION_ENTER();
	// write dpll ratio
	OSCCTRL->Dpll[0].DPLLRATIO.bit.LDRFRAC = CONF_CORE_CLK_DPLL0_LDRFRAC_VAL;
	OSCCTRL->Dpll[0].DPLLRATIO.bit.LDR = CONF_CORE_CLK_DPLL0_LDR_VAL;

	// write dpll ctrlb
	OSCCTRL->Dpll[0].DPLLCTRLB.bit.DIV = CONF_CORE_CLK_DPLL0_DIV_VAL;
	OSCCTRL->Dpll[0].DPLLCTRLB.bit.DCOEN = CONF_CORE_CLK_DPLL0_DCOEN;
	OSCCTRL->Dpll[0].DPLLCTRLB.bit.DCOFILTER = CONF_CORE_CLK_DPLL0_DCOFILTER;
	OSCCTRL->Dpll[0].DPLLCTRLB.bit.LBYPASS = CONF_CORE_CLK_DPLL0_LBYPASS;
	OSCCTRL->Dpll[0].DPLLCTRLB.bit.LTIME = CONF_CORE_CLK_DPLL0_LTIME;
	OSCCTRL->Dpll[0].DPLLCTRLB.bit.REFCLK = CONF_CORE_CLK_DPLL0_REFCLK;
	OSCCTRL->Dpll[0].DPLLCTRLB.bit.WUF = CONF_CORE_CLK_DPLL0_WUF;
	OSCCTRL->Dpll[0].DPLLCTRLB.bit.FILTER = CONF_CORE_CLK_DPLL0_FILTER;
	CRITICAL_SECTION_LEAVE();

	CRITICAL_SECTION_ENTER();
	OSCCTRL->Dpll[0].DPLLCTRLA.reg = 0;
	OSCCTRL->Dpll[0].DPLLCTRLA.bit.RUNSTDBY = CONF_CORE_CLK_DPLL0_RUNSTDBY;
	OSCCTRL->Dpll[0].DPLLCTRLA.bit.ENABLE = CONF_CORE_CLK_DPLL0_ENABLE;
	CRITICAL_SECTION_LEAVE();
#endif

#if CONF_CORE_CLK_DPLL1_ENABLE == 1
#if CONF_CORE_CLK_DPLL0_REFCLK == 0
	CRITICAL_SECTION_ENTER();
	GCLK->PCHCTRL[OSCCTRL_GCLK_ID_FDPLL1].bit.GEN =
		GCLK_PCHCTRL_GEN(CONF_CORE_CLK_DPLL1_GCLK_SRC);
	GCLK->PCHCTRL[OSCCTRL_GCLK_ID_FDPLL1].bit.CHEN =
		CONF_CORE_CLK_DPLL1_ENABLE;
	CRITICAL_SECTION_LEAVE();
#endif

	CRITICAL_SECTION_ENTER();
	// write dpll ratio
	OSCCTRL->Dpll[1].DPLLRATIO.bit.LDRFRAC = CONF_CORE_CLK_DPLL1_LDRFRAC_VAL;
	OSCCTRL->Dpll[1].DPLLRATIO.bit.LDR = CONF_CORE_CLK_DPLL1_LDR_VAL;
	// write dpll ctrlb
	OSCCTRL->Dpll[1].DPLLCTRLB.bit.DIV =		CONF_CORE_CLK_DPLL1_DIV_VAL;
	OSCCTRL->Dpll[1].DPLLCTRLB.bit.DCOEN =		CONF_CORE_CLK_DPLL1_DCOEN;
	OSCCTRL->Dpll[1].DPLLCTRLB.bit.DCOFILTER =	CONF_CORE_CLK_DPLL1_DCOFILTER;
	OSCCTRL->Dpll[1].DPLLCTRLB.bit.LBYPASS =	CONF_CORE_CLK_DPLL1_LBYPASS;
	OSCCTRL->Dpll[1].DPLLCTRLB.bit.LTIME =		CONF_CORE_CLK_DPLL1_LTIME;
	OSCCTRL->Dpll[1].DPLLCTRLB.bit.REFCLK =	CONF_CORE_CLK_DPLL1_REFCLK;
	OSCCTRL->Dpll[1].DPLLCTRLB.bit.WUF =		CONF_CORE_CLK_DPLL1_WUF;
	OSCCTRL->Dpll[1].DPLLCTRLB.bit.FILTER =	CONF_CORE_CLK_DPLL1_FILTER;
	CRITICAL_SECTION_LEAVE();

	CRITICAL_SECTION_ENTER();
	OSCCTRL->Dpll[1].DPLLCTRLA.reg = 0;
	OSCCTRL->Dpll[1].DPLLCTRLA.bit.RUNSTDBY =	CONF_CORE_CLK_DPLL1_RUNSTDBY;
	OSCCTRL->Dpll[1].DPLLCTRLA.bit.ENABLE =	CONF_CORE_CLK_DPLL1_ENABLE;
	CRITICAL_SECTION_LEAVE();
#endif

#if CONF_CORE_CLK_DPLL0_ENABLE == 1
	while(!(OSCCTRL->Dpll[0].DPLLSTATUS.bit.LOCK || OSCCTRL->Dpll[0].DPLLSTATUS.bit.CLKRDY));
	CRITICAL_SECTION_ENTER();
#if CONF_CORE_CLK_DPLL0_ONDEMAND == 1
	OSCCTRL->Dpll[0].DPLLCTRLA.bit.ONDEMAND = CONF_CORE_CLK_DPLL0_ONDEMAND;
#endif
	CRITICAL_SECTION_LEAVE();
#endif

#if CONF_CORE_CLK_DPLL1_ENABLE == 1
	while(!(OSCCTRL->Dpll[1].DPLLSTATUS.bit.LOCK || OSCCTRL->Dpll[1].DPLLSTATUS.bit.CLKRDY));
	CRITICAL_SECTION_ENTER();
#if CONF_CORE_CLK_DPLL1_ONDEMAND == 1
	OSCCTRL->Dpll[1].DPLLCTRLA.bit.ONDEMAND = CONF_CORE_CLK_DPLL1_ONDEMAND;
#endif
	CRITICAL_SECTION_LEAVE();
#endif
}

void clock_dfll_init(void)
{
#if CONF_CORE_CLK_DFLL_ENABLE
	CRITICAL_SECTION_ENTER();
	GCLK->GENCTRL[0].bit.SRC = GCLK_GENCTRL_SRC_OSCULP32K;
	CRITICAL_SECTION_LEAVE();
	while(GCLK->SYNCBUSY.bit.GENCTRL0);

	CRITICAL_SECTION_ENTER();
	// reset stuff
	OSCCTRL->DFLLCTRLA.reg = 0;
	CRITICAL_SECTION_LEAVE();
#if CONF_CORE_CLK_DFLL_USBCRM != 1 && CONF_CORE_CLK_DFLL_MODE != 0
	CRITICAL_SECTION_ENTER();
	// reset stuff
	GCLK->PCHCTRL[OSCCTRL_GCLK_ID_DFLL48].bit.GEN =
		GCLK_PCHCTRL_GEN(CONF_CORE_CLK_DFLL_GCLK_SRC);
	GCLK->PCHCTRL[OSCCTRL_GCLK_ID_DFLL48].bit.EN = CONF_CORE_CLK_DFLL_ENABLE;
	CRITICAL_SECTION_LEAVE();
#endif

	CRITICAL_SECTION_ENTER();
	// configure clock multiplier stuff
	OSCCTRL->DFLLMUL.bit.MUL = CONF_CORE_CLK_DFLL_MUL_VAL;
	OSCCTRL->DFLLMUL.bit.CSTEP = CONF_CORE_CLK_DFLL_CSTEP_VAL;
	OSCCTRL->DFLLMUL.bit.FSTEP = CONF_CORE_CLK_DFLL_FSTEP_VAL;
	CRITICAL_SECTION_LEAVE();
	while(OSCCTRL->DFLLSYNC.bit.DFLLMUL);

	CRITICAL_SECTION_ENTER();
	// reset dfll ctrlb
	OSCCTRL->DFLLCTRLB.reg = 0;
	CRITICAL_SECTION_LEAVE();
	// wait for ctrlb sync
	while(OSCCTRL->DFLLSYNC.bit.DFLLCTRLB);

	CRITICAL_SECTION_ENTER();
	OSCCTRL->DFLLCTRLA.bit.RUNSTDBY = CONF_CORE_CLK_DFLL_RUNSTDBY;
	OSCCTRL->DFLLCTRLA.bit.ENABLE = CONF_CORE_CLK_DFLL_ENABLE;
	CRITICAL_SECTION_LEAVE();
	while(OSCCTRL->DFLLSYNC.bit.ENABLE);
#if CONF_CORE_CLK_DFLL_OVERWRITE_CAL == 1
	CRITICAL_SECTION_ENTER();
	// set calib for dfll48m
	OSCCTRL->DFLLVAL.bit.COARSE = CONF_CORE_CLK_DFLL_COARSE_VAL;
	OSCCTRL->DFLLVAL.bit.FINE = CONF_CORE_CLK_DFLL_FINE_VAL;
	OSCCTRL->DFLLVAL.bit.DIFF = CONF_CORE_CLK_DFLL_DIFF_VAL;
	CRITICAL_SECTION_LEAVE();
#endif

	CRITICAL_SECTION_ENTER();
	// write dfll val
	// rewriting this for some reason?
	OSCCTRL->DFLLVAL.reg = OSCCTRL->DFLLVAL.reg;
	CRITICAL_SECTION_LEAVE();
	// let dfll val sync
	while(OSCCTRL->DFLLSYNC.bit.DFLLVAL);

	CRITICAL_SECTION_ENTER();
	// write dfll ctrlb
	OSCCTRL->DFLLCTRLB.bit.WAITLOCK = CONF_CORE_CLK_DFLL_WAITLOCK;
	OSCCTRL->DFLLCTRLB.bit.BPLCKC = CONF_CORE_CLK_DFLL_BPLKC;
	OSCCTRL->DFLLCTRLB.bit.QLDIS = CONF_CORE_CLK_DFLL_QLDIS;
	OSCCTRL->DFLLCTRLB.bit.CCDIS = CONF_CORE_CLK_DFLL_CCDIS;
	OSCCTRL->DFLLCTRLB.bit.USBCRM = CONF_CORE_CLK_DFLL_USBCRM;
	OSCCTRL->DFLLCTRLB.bit.LLAW = CONF_CORE_CLK_DFLL_LLAW;
	OSCCTRL->DFLLCTRLB.bit.STABLE = CONF_CORE_CLK_DFLL_STABLE_FCALIB;
	OSCCTRL->DFLLCTRLB.bit.MODE = CONF_CORE_CLK_DFLL_MODE;
	CRITICAL_SECTION_LEAVE();
	while(OSCCTRL->DFLLSYNC.bit.DFLLCTRLB);

	if (OSCCTRL->DFLLCTRLB.bit.MODE)
	{
		volatile uint32_t status_mask = OSCCTRL_STATUS_DFLLRDY | OSCCTRL_STATUS_DFLLLCKC;
		while((OSCCTRL->STATUS.reg & status_mask) != status_mask);
	}
	else
	{
		while(!OSCCTRL->STATUS.bit.DFLLRDY);
	}

#if CONF_CORE_CLK_DFLL_ONDEMAND == 1
	CRITICAL_SECTION_ENTER();
	OSCCTRL->DFLLCTRLA.bit.ONDEMAND = CONF_CORE_CLK_DFLL_ONDEMAND;
	CRITICAL_SECTION_LEAVE();
#endif

	while(GCLK->SYNCBUSY.reg);
	CRITICAL_SECTION_ENTER();
	// reset gclk 0
	GCLK->GENCTRL[0].bit.SRC = CONF_CORE_GCLK_0_CLOCK_SOURCE;
	CRITICAL_SECTION_LEAVE();
#endif
}
