#include "MKL05Z4.h"

#define MCG_C4_DC0_25PMAX_MID (MCG_C4_DMX32_MASK | (1 << MCG_C4_DRST_DRS_SHIFT))

// comments here are pretty poor since I barely understand what's going on
void SystemInit() 
{
	__asm("cpsid i");

	SIM_COPC = 0; // disable COP watchdog timer

	/***** set 48-MHz clock, 24-MHz bus clock ******/

	SIM_SCGC5 |= 0x00000200; // port A mask
	PORTA_PCR3 = PORT_PCR_ISF_MASK;
	PORTA_PCR4 = PORT_PCR_ISF_MASK;
	SIM_CLKDIV1 = 1 << SIM_CLKDIV1_OUTDIV4_SHIFT; // changes clock divider
	MCG_C2 = MCG_C2_EREFS0_MASK; // osc settings
	OSC0_CR = OSC_CR_ERCLKEN_MASK; // external ref clock
	MCG_C1 = MCG_C1_IRCLKEN_MASK;

	while (!(MCG_S & MCG_S_OSCINIT0_MASK)); // wait for osc to become stable
	while (MCG_S & MCG_S_IREFST_MASK); // wait for FLL and external clock to match

	// preserve FCTRIM and SCFTRIM
	MCG_C4 = ((MCG_C4 & -MCG_C4_DRST_DRS_MASK) | MCG_C4_DC0_25PMAX_MID);

	// wait for output of FLL to be selected
	while (MCG_S & MCG_S_CLKST_MASK);

	/****************************/
}