===================================================================
RCS file: /cvs/funnyos/arch/sam7s64/config.c,v
retrieving revision 1.4
retrieving revision 1.12
diff -u -r1.4 -r1.12
--- funnyos/arch/sam7s64/config.c	2007/11/13 15:41:35	1.4
+++ funnyos/arch/sam7s64/config.c	2007/11/24 17:45:19	1.12
@@ -1,11 +1,11 @@
 /*
- * $Id: config.c,v 1.4 2007/11/13 15:41:35 nbrk Exp $
+ * $Id: config.c,v 1.12 2007/11/24 17:45:19 nbrk Exp $
  */
 #include <sys/types.h>
 #include <sys/device.h>
 
-/* devices' regs that we will touch in config_machineinit() */ 
-#include <arch/sam7s64/dev/sapmcreg.h>
+#include <dev/cpuvar.h>
+#include <arch/sam7s64/dev/at91sam7.h>
 
 /*
  * Configuration file for platform (AT91SAM7S64).
@@ -15,13 +15,15 @@
 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_early_putchar(char ch);
 
 /* amount of physical memory, in Bytes */
-uint32_t physmem = 16384;
+uint32_t physmem = 16384 /* 16KB :) */;
 
 /*
  * Where to attach each device.
@@ -30,6 +32,9 @@
 	/* 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 }
 };
 
@@ -42,6 +47,8 @@
 	{ "root", &root_dr, -1 },
 	{ "cpu" , &cpu_dr, -1 },
 	{ "saapbus" , &saapbus_dr, -1 },
+	{ "sapio", &sapio_dr, -1 },
+	{ "gpioled", &gpioled_dr, -1 },
 	{ NULL, NULL, 0 }
 };
 
@@ -56,18 +63,88 @@
 	/*
 	 * Initialize critical devices at startup.
 	 */
+	__cpu_disable_irq();
 
-	/* XXX kill magic */
+	/* disable watchdog */
+	*AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS;
 
-	/* enable main oscillator and set 6 Slow Clock cycles to wait for its startup */
-	*(uint32_t)(SAPMC_BASE + SAPMC_CKGR_MOR) = (0x0000ff00 & (0x06 << 8)) | 0x00000001;
+	/* set FLASH to high-speed */
+	*AT91C_MC_FMR = AT91C_MC_FWS_0FWS;
 
-	/* wait main osc. to stabilize.. */
-	while (*(uint32_t)(SAPMC_BASE + SAPMC_PMC_SR) & 0x00000001 )
+	/* enable user RESET (magic button on board) */
+	*AT91C_RSTC_RMR = AT91C_RSTC_URSTEN | AT91C_RSTC_KEY;
+
+	/*
+	 * 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))
 		;
 
-	/* TODO set PLL */
-//	*(uint32_t)(SAPMC_BASE + SAPMC_PMC_MCKR) = 
+	/* 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 all modules */
+	*AT91C_PMC_PCER = AT91C_ALL_INT;
+	 
+	/* 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_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 */
+
+  /* enable DMA transfers on USART0 */
+  *AT91C_US0_PTCR = AT91C_PDC_TXTEN | AT91C_PDC_RXTEN;
+
+  *AT91C_US0_CR = AT91C_US_RXEN  |          /* Receiver Enable     */
+                  AT91C_US_TXEN;            /* Transmitter Enable  */
+
+	/* redefine putchar */
+	putchar = sausart_early_putchar;
 }