===================================================================
RCS file: /cvs/funnyos/arch/sam7s64/config.c,v
retrieving revision 1.2
retrieving revision 1.9
diff -u -r1.2 -r1.9
--- funnyos/arch/sam7s64/config.c	2007/11/09 16:10:07	1.2
+++ funnyos/arch/sam7s64/config.c	2007/11/24 10:12:44	1.9
@@ -1,9 +1,12 @@
 /*
- * $Id: config.c,v 1.2 2007/11/09 16:10:07 init Exp $
+ * $Id: config.c,v 1.9 2007/11/24 10:12:44 nbrk Exp $
  */
 #include <sys/types.h>
 #include <sys/device.h>
 
+#include <dev/cpuvar.h>
+#include <arch/sam7s64/dev/at91sam7.h>
+
 /*
  * Configuration file for platform (AT91SAM7S64).
  */
@@ -11,13 +14,16 @@
 /* device drivers */
 extern struct driver root_dr;
 extern struct driver cpu_dr;
+extern struct driver saapbus_dr;
+extern struct driver sapio_dr;
+extern struct driver gpioled_dr;
 
 
-extern void(*putchar)(char);
-void 	sauart_early_putc(char ch);
+extern void 	(*putchar)(char);
+extern void 	sausart_0_putchar(char ch);
 
 /* amount of physical memory, in Bytes */
-uint32_t physmem = 16384;
+uint32_t physmem = 16384 /* 16KB :) */;
 
 /*
  * Where to attach each device.
@@ -25,6 +31,10 @@
 struct attachinfo config_attachinfo[] = {
 	/* child,   parent, pminor, loc, 	intrno, flags */
 	{ "cpu" , 	"root", 	0, 0, 			0,	0 },
+	{ "saapbus","root", 	0, 0, 			0,	0 },
+	{ "sapio",  "saapbus", 	0, 0, 			0,  0 },
+	{ "gpioled","sapio", 	0, 17, 			0, 	0 },
+	{ "gpioled","sapio", 	0, 18, 			0, 	0 },
 	{ NULL, 	NULL, 		0, 0, 			0,  0 }
 };
 
@@ -36,6 +46,9 @@
 /* name, driverp, ninstances (should be -1) */
 	{ "root", &root_dr, -1 },
 	{ "cpu" , &cpu_dr, -1 },
+	{ "saapbus" , &saapbus_dr, -1 },
+	{ "sapio", &sapio_dr, -1 },
+	{ "gpioled", &gpioled_dr, -1 },
 	{ NULL, NULL, 0 }
 };
 
@@ -45,12 +58,99 @@
  */
 
 void
-config_consinit(void)
+config_machineinit(void)
 {
 	/*
-	 * Configure putchar, so we can printf messages to the console without fcons.
+	 * Initialize critical devices at startup.
 	 */
-	/* putchar = tauart_early_putc; */
+	__cpu_disable_irq();
+
+	/* disable watchdog */
+	*AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS;
+
+	/* set FLASH to high-speed */
+	*AT91C_MC_FMR = AT91C_MC_FWS_0FWS;
+
+	/*
+	 * Initialize oscillators.
+	 * Taken from Atmel's examples.
+	 */
+	/* Set MCK at 48 054 850 */
+
+	/* 1 Enabling the Main Oscillator */
+    /* SCK = 1/32768 = 30.51 uSecond
+     * Start up time = 8 * 6 / SCK = 56 * 30.51 = 1,46484375 ms
+	 */
+	*AT91C_PMC_MOR = ( (AT91C_CKGR_OSCOUNT & (0x06 <<8)) | AT91C_CKGR_MOSCEN);
+
+	/* Wait the startup time */
+	while(!(*AT91C_PMC_SR & AT91C_PMC_MOSCS))
+		;
+
+	/* 2 Checking the Main Oscillator Frequency (Optional) */
+	/* TODO */
+
+	/* 3 Setting PLL and divider: */
+	/* - div by 14 Fin = 1.3165 =(18,432 / 14)
+	 * - Mul 72+1: Fout =   96.1097 =(3,6864 *73)
+	 * for 96 MHz the erroe is 0.11%
+	 * Field out NOT USED = 0
+	 * PLLCOUNT pll startup time estimate at : 0.844 ms
+	 * PLLCOUNT 28 = 0.000844 /(1/32768)
+	 */
+	*AT91C_PMC_PLLR = ( (AT91C_CKGR_DIV & 14 ) 		|
+ 					(AT91C_CKGR_PLLCOUNT & (28<<8)) |
+ 					(AT91C_CKGR_MUL & (72<<16)) );
+
+	/* Wait the startup time */
+	while(!(*AT91C_PMC_SR & AT91C_PMC_LOCK))
+		;
+	while(!(*AT91C_PMC_SR & AT91C_PMC_MCKRDY))
+		;
+
+	/* 4. Selection of Master Clock and Processor Clock */
+	/* select the PLL clock divided by 2: */
+	*AT91C_PMC_MCKR = AT91C_PMC_PRES_CLK_2;
+	while(!(*AT91C_PMC_SR & AT91C_PMC_MCKRDY))
+		;
+
+	*AT91C_PMC_MCKR |= AT91C_PMC_CSS_PLL_CLK;
+	while(!(*AT91C_PMC_SR & AT91C_PMC_MCKRDY))
+		;
+
+	/* enable clock to PIO and USART0 */
+	*AT91C_PMC_PCER = AT91C_ID_PIOA | AT91C_ID_US0 AT91C_ID_PDC;
+	 
+	*AT91C_PIOA_PER = (1 << 17 | 1 << 18);
+	*AT91C_PIOA_OER = (1 << 17 | 1 << 18);
+	*AT91C_PIOA_CODR= (1 << 17 | 1 << 18);
+
+	/* initialize USART0 (we clock it in PMC above) */
+
+  *AT91C_PIOA_PDR = AT91C_PA5_RXD0 |        /* Enable RxD0 Pin */
+                    AT91C_PA6_TXD0;         /* Enalbe TxD0 Pin */
+
+  *AT91C_US0_CR = AT91C_US_RSTRX |          /* Reset Receiver      */
+                  AT91C_US_RSTTX |          /* Reset Transmitter   */
+                  AT91C_US_RXDIS |          /* Receiver Disable    */
+                  AT91C_US_TXDIS;           /* Transmitter Disable */
+
+  *AT91C_US0_MR = AT91C_US_USMODE_NORMAL |  /* Normal Mode */
+                  AT91C_US_CLKS_CLOCK    |  /* Clock = MCK */
+                  AT91C_US_CHRL_8_BITS   |  /* 8-bit Data  */
+                  AT91C_US_PAR_NONE      |  /* No Parity   */
+                  AT91C_US_NBSTOP_1_BIT;    /* 1 Stop Bit  */
+
+  *AT91C_US0_BRGR = 48054857 / 16 / 9600;                    /* Baud Rate Divisor */
+
+  *AT91C_US0_CR = AT91C_US_RXEN  |          /* Receiver Enable     */
+                  AT91C_US_TXEN;            /* Transmitter Enable  */
+
+	/* redefine putchar */
+	putchar = sausart_0_putchar;
+//	putchar(0);
+//	putchar(0x46);
+//	putchar(0x46);
 }