Init commit

1 files changed, 90 insertions, 0 deletions

diff --git a/setup.c b/setup.c
new file mode 100644
index 0000000..ad29e40
--- /dev/null
+++ b/setup.c
@@ -0,0 +1,90 @@
+#include <reg.h>
+#include <stddef.h>
+#include <string.h>
+
+#include "asm.h"
+#include "usb/usb.h"
+
+extern char _sflashdata[], _sdata[], _edata[];
+extern char _sbss[], _ebss[];
+
+#define RANGE0_VAL 2 /* Very High Frequency */
+#define HGO0_VAL 0 /* High Gain Mode Disabled */
+#define FRDIV_VAL 4 /* Divide 16 MHz by 512 to produce 31.25 kHz FLL Reference */
+#define PRDIV0_VAL 7 /* Divide 16 MHz by 8 to produce 2 MHz PLL Reference */
+#define VDIV0_VAL 12 /* Multiply 2 MHz by 36 to produce 72 MHz resulting clock */
+
+/* Option 2 Clock Configuration */
+#define OUTDIV1_VAL 0 /* 72 MHz / 1 = 72 MHz Core / system clocks */
+#define OUTDIV2_VAL 1 /* 72 MHz / 2 = 36 MHz Bus clock */
+#define OUTDIV4_VAL 2 /* 72 MHz / 3 = 24 MHz Flash clock */
+
+void setup(void);
+void setup(void)
+{
+	/* Disable interrupts for setup */
+	CLI();
+
+	/* Disable Watchdog Timer */
+	WDOG_UNLOCK = WDOG_UNLOCK_S1;
+	WDOG_UNLOCK = WDOG_UNLOCK_S2;
+	UNSET_BIT(WDOG_STCTRLH, STCTRLH_WDOGEN);
+
+	/* Copy data and clear bss */
+	memcpy(_sdata, _sflashdata, _edata - _sdata);
+	memset(_sbss, 0, _ebss - _sdata);
+
+	/* Enable port clocks */
+	SIM_SCGC5 |= BV(SCGC5_PORTE) | BV(SCGC5_PORTD) | BV(SCGC5_PORTC)
+		| BV(SCGC5_PORTB) | BV(SCGC5_PORTA);
+
+	/* Change Clock Dividers */
+	SIM_CLKDIV1 = OUTDIV1_VAL << CLKDIV1_OUTDIV1
+		   | OUTDIV2_VAL << CLKDIV1_OUTDIV2
+		   | OUTDIV4_VAL << CLKDIV1_OUTDIV4;
+
+	/* Clear latched peripherals and I/O */
+	if (IS_BIT_SET(PMC_REGSC, REGSC_ACKISO))
+		SET_BIT(PMC_REGSC, REGSC_ACKISO);
+
+	/*
+	 * CLOCKING SETUP
+	 */
+
+	/* FEI to FBE */
+	SET_MASKED(MCG_C2, C2_RANGE0_M | BV(C2_HGO0) | BV(C2_EREFS0),
+		   RANGE0_VAL << C2_RANGE0 | BV(C2_EREFS0));
+
+	SET_MASKED(MCG_C1, C1_CLKS_M | C1_FRDIV_M | BV(C1_IREFS),
+		   2 << C1_CLKS | FRDIV_VAL << C1_FRDIV);
+
+	WAIT_BIT_SET(MCG_S, S_OSCINIT0);
+	WAIT_BIT_UNSET(MCG_S, S_IREFST);
+	do { } while (GET_BITS(MCG_S, S_CLKST) != 2);
+
+	/* Enable Clock Monitor */
+	SET_BIT(MCG_C6, C6_CME0);
+
+
+	/* FBE to PBE */
+	SET_MASKED(MCG_C5, C5_PRDIV0_M, PRDIV0_VAL << C5_PRDIV0);
+	SET_MASKED(MCG_C6, BV(C6_PLLS) | C6_VDIV0_M, BV(C6_PLLS) | VDIV0_VAL << C6_VDIV0);
+
+	WAIT_BIT_SET(MCG_S, S_PLLST);
+	WAIT_BIT_SET(MCG_S, S_LOCK0);
+
+	/* PBE to PEE */
+	SET_MASKED(MCG_C1, C1_CLKS_M, 0);
+
+	do { } while (GET_BITS(MCG_S, S_CLKST) != 3);
+
+	/* Set PLL as the clock to use for things which can take PLL or FLL */
+	SET_BIT(SIM_SOPT2, SOPT2_PLLFLLSEL);
+	SET_MASKED(SIM_SOPT2, SOPT2_CLKOUTSEL_M, 4 << SOPT2_CLKOUTSEL);
+
+
+	usb_setup();
+
+	/* Enable interrupts for setup */
+	STI();
+}