Init oscillatorss (somewhat) properly

arch/at91sam3x8e/startup.c

@@ -29,6 +29,7 @@
 #include <stdint.h>
 #include <arch/at91sam3x8e/interrupt.h>
 #include <ardix/string.h>
+#include <arch/at91sam3x8e/hardware.h>
 #include <toolchain.h>
 
 /* from flash.ld */
@@ -45,6 +46,88 @@ extern uint32_t _estack;	/* stack end */
 /* implementation in init/main.c */
 void do_bootstrap(void);
 
+/**
+ * Keep the CPU busy by continuously checking the same
+ * expression over and over again until it is true.
+ *
+ * @param expr The expression.
+ */
+#define mom_are_we_there_yet(expr) while (!(expr))
+
+/** Prepare the system for bootstrapping the Kernel. */
+void sys_init(void)
+{
+	/*
+	 * This method is basically an implementation of chapter 28.12 in the
+	 * Atmel SAM3X8E Datasheet, combined with the startup code from libsam.
+	 */
+
+	/* # of wait states as per hardware spec (stolen from SAM SysInit) */
+	REG_EEFC0_FMR = REG_EEFC_FWS_VAL(4);
+	REG_EEFC1_FMR = REG_EEFC_FWS_VAL(4);
+
+	/* disable osc write protection */
+	REG_PMC_WPMR = REG_PMC_WPMR_WPKEY_VAL(REG_PMC_WPMR_WPKEY_MAGIC)
+		     & ~REG_PMC_WPMR_WPEN_BIT;
+
+	/*
+	 * 1. Enabling the Main Oscillator
+	 */
+
+	/* initialize main osc */
+	if (!(REG_CKGR_MOR & REG_CKGR_MOR_MOSCSEL_BIT)) {
+		REG_CKGR_MOR = REG_CKGR_MOR_KEY_VAL(REG_CKGR_MOR_KEY_MAGIC)
+			     | REG_CKGR_MOR_MOSCXTST_VAL(8)
+			     | REG_CKGR_MOR_MOSCRCEN_BIT
+			     | REG_CKGR_MOR_MOSCXTEN_BIT;
+		mom_are_we_there_yet(REG_PMC_SR & REG_PMC_SR_MOSCXTS_BIT);
+	}
+
+	/* switch to Xtal osc */
+	REG_CKGR_MOR = REG_CKGR_MOR_KEY_VAL(REG_CKGR_MOR_KEY_MAGIC)
+		     | REG_CKGR_MOR_MOSCXTST_VAL(8)
+		     | REG_CKGR_MOR_MOSCRCEN_BIT
+		     | REG_CKGR_MOR_MOSCXTEN_BIT
+		     | REG_CKGR_MOR_MOSCSEL_BIT;
+	mom_are_we_there_yet(REG_PMC_SR & REG_PMC_SR_MOSCXTS_BIT);
+
+	REG_PMC_MCKR = (REG_PMC_MCKR & ~REG_PMC_MCKR_CSS_MASK)
+		     | REG_PMC_MCKR_CSS_VAL(1 /* = main clock */);
+	mom_are_we_there_yet(REG_PMC_SR & REG_PMC_SR_MCKRDY_BIT);
+
+	/*
+	 * 2. Checking the Main Oscillator Frequency (Optional)
+	 */
+
+	/* I repeat: **(Optional)** */
+
+	/*
+	 * 3. Setting PLL and Divider
+	 */
+
+	REG_CKGR_PLLAR = REG_CKGR_PLLAR_ONE_BIT
+		       | REG_CKGR_PLLAR_MULA_VAL(0xD)
+		       | REG_CKGR_PLLAR_PLLACOUNT_VAL(0x3F /* maximum value */)
+		       | REG_CKGR_PLLAR_DIVA_VAL(0x1);
+	mom_are_we_there_yet(REG_PMC_SR & REG_PMC_SR_LOCKA_BIT);
+
+	/*
+	 * 4. Selection of Master Clock and Processor Clock
+	 */
+
+	REG_PMC_MCKR = REG_PMC_MCKR_PRES_VAL(1 /* = as fast as it gets */)
+		     | REG_PMC_MCKR_CSS_VAL(1 /* = main clock */);
+	mom_are_we_there_yet(REG_PMC_SR & REG_PMC_SR_MCKRDY_BIT);
+
+	/* PMC_MCKR must not be configured within one clock cycle */
+	REG_PMC_MCKR = REG_PMC_MCKR_PRES_VAL(1 /* = as fast as it gets */)
+		     | REG_PMC_MCKR_CSS_VAL(2 /* = PLLA clock */);
+	mom_are_we_there_yet(REG_PMC_SR & REG_PMC_SR_MCKRDY_BIT);
+
+	/* turn osc write protection on again */
+	REG_PMC_WPMR |= REG_PMC_WPMR_WPEN_BIT;
+}
+
 void isr_reset(void)
 {
 	memmove(
@@ -58,6 +141,8 @@ void isr_reset(void)
 		(size_t)(&_ezero) - (size_t)(&_szero)
 	);
 
+	sys_init();
+
 	/* start the Kernel */
 	do_bootstrap();
 

init/main.c

@@ -50,16 +50,17 @@ void do_bootstrap(void)
 	REG_PIOB_OER |= state;
 	REG_PIOB_PER |= state;
 
-	while (true)
-	{
+	/* we'll only let the LED flash for now */
+
+	while (true) {
 		if (count++ != 100000)
 			continue;
 
 		if (on) {
-			REG_PIOB_SODR |= (1 << 27);
+			REG_PIOB_SODR |= state;
 			on = false;
 		} else {
-			REG_PIOB_CODR |= (1 << 27);
+			REG_PIOB_CODR |= state;
 			on = true;
 		}
 		count = 0;