diff --git a/board/atmel/at91cap9adk/at91cap9adk.c b/board/atmel/at91cap9adk/at91cap9adk.c
index 52e62de..24861ba 100644
--- a/board/atmel/at91cap9adk/at91cap9adk.c
+++ b/board/atmel/at91cap9adk/at91cap9adk.c
@@ -23,7 +23,13 @@
  */
 
 #include <common.h>
-#include <asm/arch/AT91CAP9.h>
+#include <asm/arch/at91cap9.h>
+#include <asm/arch/at91cap9_matrix.h>
+#include <asm/arch/at91sam926x_mc.h>
+#include <asm/arch/at91_pmc.h>
+#include <asm/arch/at91_rstc.h>
+#include <asm/arch/gpio.h>
+#include <asm/arch/io.h>
 #if defined(CONFIG_RESET_PHY_R) && defined(CONFIG_MACB)
 #include <net.h>
 #endif
@@ -40,126 +46,106 @@
 static void at91cap9_serial_hw_init(void)
 {
 #ifdef CONFIG_USART0
-	AT91C_BASE_PIOA->PIO_PDR = AT91C_PA22_TXD0 | AT91C_PA23_RXD0;
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_US0;
+	at91_set_A_periph(AT91_PIN_PA22, 1);		/* TXD0 */
+	at91_set_A_periph(AT91_PIN_PA23, 0);		/* RXD0 */
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_US0);
 #endif
 
 #ifdef CONFIG_USART1
-	AT91C_BASE_PIOD->PIO_PDR = AT91C_PD0_TXD1 | AT91C_PD1_RXD1;
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_US1;
+	at91_set_A_periph(AT91_PIN_PD0, 1);		/* TXD1 */
+	at91_set_A_periph(AT91_PIN_PD1, 0);		/* RXD1 */
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_US1);
 #endif
 
 #ifdef CONFIG_USART2
-	AT91C_BASE_PIOD->PIO_PDR = AT91C_PD2_TXD2 | AT91C_PD3_RXD2;
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_US2;
+	at91_set_A_periph(AT91_PIN_PD2, 1);		/* TXD2 */
+	at91_set_A_periph(AT91_PIN_PD3, 0);		/* RXD2 */
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_US2);
 #endif
 
 #ifdef CONFIG_USART3	/* DBGU */
-	AT91C_BASE_PIOC->PIO_PDR = AT91C_PC31_DTXD | AT91C_PC30_DRXD;
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_SYS;
+	at91_set_A_periph(AT91_PIN_PC30, 0);		/* DRXD */
+	at91_set_A_periph(AT91_PIN_PC31, 1);		/* DTXD */
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_SYS);
 #endif
-
-
 }
 
 static void at91cap9_nor_hw_init(void)
 {
+	unsigned long csa;
+
 	/* Ensure EBI supply is 3.3V */
-	AT91C_BASE_CCFG->CCFG_EBICSA |= AT91C_EBI_SUP_3V3;
-
+	csa = at91_sys_read(AT91_MATRIX_EBICSA);
+	at91_sys_write(AT91_MATRIX_EBICSA,
+		       csa | AT91_MATRIX_EBI_VDDIOMSEL_3_3V);
 	/* Configure SMC CS0 for parallel flash */
-	AT91C_BASE_SMC->SMC_SETUP0 = AT91C_FLASH_NWE_SETUP |
-				     AT91C_FLASH_NCS_WR_SETUP |
-				     AT91C_FLASH_NRD_SETUP |
-				     AT91C_FLASH_NCS_RD_SETUP;
-
-	AT91C_BASE_SMC->SMC_PULSE0 = AT91C_FLASH_NWE_PULSE |
-				     AT91C_FLASH_NCS_WR_PULSE |
-				     AT91C_FLASH_NRD_PULSE |
-				     AT91C_FLASH_NCS_RD_PULSE;
-
-	AT91C_BASE_SMC->SMC_CYCLE0 = AT91C_FLASH_NWE_CYCLE |
-				     AT91C_FLASH_NRD_CYCLE;
-
-	AT91C_BASE_SMC->SMC_CTRL0 =  AT91C_SMC_READMODE |
-				     AT91C_SMC_WRITEMODE |
-				     AT91C_SMC_NWAITM_NWAIT_DISABLE |
-				     AT91C_SMC_BAT_BYTE_WRITE |
-				     AT91C_SMC_DBW_WIDTH_SIXTEEN_BITS |
-				     (AT91C_SMC_TDF & (1 << 16));
+	at91_sys_write(AT91_SMC_SETUP(0),
+		       AT91_SMC_NWESETUP_(4) | AT91_SMC_NCS_WRSETUP_(2) |
+		       AT91_SMC_NRDSETUP_(4) | AT91_SMC_NCS_RDSETUP_(2));
+	at91_sys_write(AT91_SMC_PULSE(0),
+		       AT91_SMC_NWEPULSE_(8) | AT91_SMC_NCS_WRPULSE_(10) |
+		       AT91_SMC_NRDPULSE_(8) | AT91_SMC_NCS_RDPULSE_(10));
+	at91_sys_write(AT91_SMC_CYCLE(0),
+		       AT91_SMC_NWECYCLE_(16) | AT91_SMC_NRDCYCLE_(16));
+	at91_sys_write(AT91_SMC_MODE(0),
+		       AT91_SMC_READMODE | AT91_SMC_WRITEMODE |
+		       AT91_SMC_EXNWMODE_DISABLE | AT91_SMC_BAT_WRITE |
+		       AT91_SMC_DBW_16 | AT91_SMC_TDF_(1));
 }
 
 #ifdef CONFIG_CMD_NAND
 static void at91cap9_nand_hw_init(void)
 {
+	unsigned long csa;
+
 	/* Enable CS3 */
-	AT91C_BASE_CCFG->CCFG_EBICSA |= AT91C_EBI_CS3A_SM | AT91C_EBI_SUP_3V3;
+	csa = at91_sys_read(AT91_MATRIX_EBICSA);
+	at91_sys_write(AT91_MATRIX_EBICSA,
+		       csa | AT91_MATRIX_EBI_CS3A_SMC_SMARTMEDIA |
+		       AT91_MATRIX_EBI_VDDIOMSEL_3_3V);
 
 	/* Configure SMC CS3 for NAND/SmartMedia */
-	AT91C_BASE_SMC->SMC_SETUP3 = AT91C_SM_NWE_SETUP |
-				     AT91C_SM_NCS_WR_SETUP |
-				     AT91C_SM_NRD_SETUP |
-				     AT91C_SM_NCS_RD_SETUP;
+	at91_sys_write(AT91_SMC_SETUP(3),
+		       AT91_SMC_NWESETUP_(2) | AT91_SMC_NCS_WRSETUP_(1) |
+		       AT91_SMC_NRDSETUP_(2) | AT91_SMC_NCS_RDSETUP_(1));
+	at91_sys_write(AT91_SMC_PULSE(3),
+		       AT91_SMC_NWEPULSE_(4) | AT91_SMC_NCS_WRPULSE_(6) |
+		       AT91_SMC_NRDPULSE_(4) | AT91_SMC_NCS_RDPULSE_(6));
+	at91_sys_write(AT91_SMC_CYCLE(3),
+		       AT91_SMC_NWECYCLE_(8) | AT91_SMC_NRDCYCLE_(8));
+	at91_sys_write(AT91_SMC_MODE(3),
+		       AT91_SMC_READMODE | AT91_SMC_WRITEMODE |
+		       AT91_SMC_EXNWMODE_DISABLE |
+		       AT91_SMC_DBW_8 | AT91_SMC_TDF_(1));
 
-	AT91C_BASE_SMC->SMC_PULSE3 = AT91C_SM_NWE_PULSE |
-				     AT91C_SM_NCS_WR_PULSE |
-				     AT91C_SM_NRD_PULSE |
-				     AT91C_SM_NCS_RD_PULSE;
-
-	AT91C_BASE_SMC->SMC_CYCLE3 = AT91C_SM_NWE_CYCLE |
-				     AT91C_SM_NRD_CYCLE;
-
-	AT91C_BASE_SMC->SMC_CTRL3 =  AT91C_SMC_READMODE |
-				     AT91C_SMC_WRITEMODE |
-				     AT91C_SMC_NWAITM_NWAIT_DISABLE |
-				     AT91C_SMC_DBW_WIDTH_EIGTH_BITS |
-				     AT91C_SM_TDF;
-
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_PIOABCD;
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91CAP9_ID_PIOABCD);
 
 	/* RDY/BSY is not connected */
 
 	/* Enable NandFlash */
-	AT91C_BASE_PIOD->PIO_PER = AT91C_PIO_PD15;
-	AT91C_BASE_PIOD->PIO_OER = AT91C_PIO_PD15;
+	at91_set_gpio_output(AT91_PIN_PD15, 1);
 }
 #endif
 
 #ifdef CONFIG_HAS_DATAFLASH
 static void at91cap9_spi_hw_init(void)
 {
-	AT91C_BASE_PIOD->PIO_BSR = AT91C_PD0_SPI0_NPCS2D |
-				   AT91C_PD1_SPI0_NPCS3D;
-	AT91C_BASE_PIOD->PIO_PDR = AT91C_PD0_SPI0_NPCS2D |
-				   AT91C_PD1_SPI0_NPCS3D;
+	at91_set_B_periph(AT91_PIN_PA5, 0);	/* SPI0_NPCS0 */
 
-	AT91C_BASE_PIOA->PIO_ASR = AT91C_PA28_SPI0_NPCS3A;
-	AT91C_BASE_PIOA->PIO_BSR = AT91C_PA4_SPI0_NPCS2A |
-				   AT91C_PA1_SPI0_MOSI |
-				   AT91C_PA0_SPI0_MISO |
-				   AT91C_PA3_SPI0_NPCS1 |
-				   AT91C_PA5_SPI0_NPCS0 |
-				   AT91C_PA2_SPI0_SPCK;
-	AT91C_BASE_PIOA->PIO_PDR = AT91C_PA28_SPI0_NPCS3A |
-				   AT91C_PA4_SPI0_NPCS2A |
-				   AT91C_PA1_SPI0_MOSI |
-				   AT91C_PA0_SPI0_MISO |
-				   AT91C_PA3_SPI0_NPCS1 |
-				   AT91C_PA5_SPI0_NPCS0 |
-				   AT91C_PA2_SPI0_SPCK;
+	at91_set_B_periph(AT91_PIN_PA0, 0);	/* SPI0_MISO */
+	at91_set_B_periph(AT91_PIN_PA1, 0);	/* SPI0_MOSI */
+	at91_set_B_periph(AT91_PIN_PA2, 0);	/* SPI0_SPCK */
 
-	/* Enable Clock */
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_SPI0;
+	/* Enable clock */
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91CAP9_ID_SPI0);
 }
 #endif
 
 #ifdef CONFIG_MACB
 static void at91cap9_macb_hw_init(void)
 {
-	unsigned int gpio;
-
 	/* Enable clock */
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_EMAC;
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91CAP9_ID_EMAC);
 
 	/*
 	 * Disable pull-up on:
@@ -169,54 +155,59 @@
 	 *
 	 * PHY has internal pull-down
 	 */
-	AT91C_BASE_PIOB->PIO_PPUDR = AT91C_PB22_E_RXDV |
-				     AT91C_PB25_E_RX0 |
-				     AT91C_PB26_E_RX1;
+	writel(pin_to_mask(AT91_PIN_PB22) |
+	       pin_to_mask(AT91_PIN_PB25) |
+	       pin_to_mask(AT91_PIN_PB26),
+	       pin_to_controller(AT91_PIN_PA0) + PIO_PUDR);
 
 	/* Need to reset PHY -> 500ms reset */
-	AT91C_BASE_RSTC->RSTC_RMR = (AT91C_RSTC_KEY & (0xA5 << 24)) |
-				    (AT91C_RSTC_ERSTL & (0x0D << 8)) |
-				    AT91C_RSTC_URSTEN;
-	AT91C_BASE_RSTC->RSTC_RCR = (AT91C_RSTC_KEY & (0xA5 << 24)) |
-				    AT91C_RSTC_EXTRST;
+	at91_sys_write(AT91_RSTC_MR, AT91_RSTC_KEY |
+				     AT91_RSTC_ERSTL | (0x0D << 8) |
+				     AT91_RSTC_URSTEN);
+
+	at91_sys_write(AT91_RSTC_CR, AT91_RSTC_KEY | AT91_RSTC_EXTRST);
 
 	/* Wait for end hardware reset */
-	while (!(AT91C_BASE_RSTC->RSTC_RSR & AT91C_RSTC_NRSTL));
+	while (!(at91_sys_read(AT91_RSTC_SR) & AT91_RSTC_NRSTL));
 
 	/* Re-enable pull-up */
-	AT91C_BASE_PIOB->PIO_PPUER = AT91C_PB22_E_RXDV |
-				     AT91C_PB25_E_RX0 |
-				     AT91C_PB26_E_RX1;
+	writel(pin_to_mask(AT91_PIN_PB22) |
+	       pin_to_mask(AT91_PIN_PB25) |
+	       pin_to_mask(AT91_PIN_PB26),
+	       pin_to_controller(AT91_PIN_PA0) + PIO_PUER);
 
-#ifdef CONFIG_RMII
-	gpio =	AT91C_PB30_E_MDIO |
-		AT91C_PB29_E_MDC  |
-		AT91C_PB21_E_TXCK |
-		AT91C_PB27_E_RXER |
-		AT91C_PB25_E_RX0  |
-		AT91C_PB22_E_RXDV |
-		AT91C_PB26_E_RX1  |
-		AT91C_PB28_E_TXEN |
-		AT91C_PB23_E_TX0  |
-		AT91C_PB24_E_TX1;
-	AT91C_BASE_PIOB->PIO_ASR = gpio;
-	AT91C_BASE_PIOB->PIO_BSR = 0;
-	AT91C_BASE_PIOB->PIO_PDR = gpio;
-#else
-#error AT91CAP9A-DK works only in RMII mode
+	at91_set_A_periph(AT91_PIN_PB21, 0);	/* ETXCK_EREFCK */
+	at91_set_A_periph(AT91_PIN_PB22, 0);	/* ERXDV */
+	at91_set_A_periph(AT91_PIN_PB25, 0);	/* ERX0 */
+	at91_set_A_periph(AT91_PIN_PB26, 0);	/* ERX1 */
+	at91_set_A_periph(AT91_PIN_PB27, 0);	/* ERXER */
+	at91_set_A_periph(AT91_PIN_PB28, 0);	/* ETXEN */
+	at91_set_A_periph(AT91_PIN_PB23, 0);	/* ETX0 */
+	at91_set_A_periph(AT91_PIN_PB24, 0);	/* ETX1 */
+	at91_set_A_periph(AT91_PIN_PB30, 0);	/* EMDIO */
+	at91_set_A_periph(AT91_PIN_PB29, 0);	/* EMDC */
+
+#ifndef CONFIG_RMII
+	at91_set_B_periph(AT91_PIN_PC25, 0);	/* ECRS */
+	at91_set_B_periph(AT91_PIN_PC26, 0);	/* ECOL */
+	at91_set_B_periph(AT91_PIN_PC22, 0);	/* ERX2 */
+	at91_set_B_periph(AT91_PIN_PC23, 0);	/* ERX3 */
+	at91_set_B_periph(AT91_PIN_PC27, 0);	/* ERXCK */
+	at91_set_B_periph(AT91_PIN_PC20, 0);	/* ETX2 */
+	at91_set_B_periph(AT91_PIN_PC21, 0);	/* ETX3 */
+	at91_set_B_periph(AT91_PIN_PC24, 0);	/* ETXER */
 #endif
-
 	/* Unlock EMAC, 3 0 2 1 sequence */
 #define MP_MAC_KEY0	0x5969cb2a
 #define MP_MAC_KEY1	0xb4a1872e
 #define MP_MAC_KEY2	0x05683fbc
 #define MP_MAC_KEY3	0x3634fba4
 #define UNLOCK_MAC	0x00000008
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x3c)) = MP_MAC_KEY3;
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x30)) = MP_MAC_KEY0;
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x38)) = MP_MAC_KEY2;
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x34)) = MP_MAC_KEY1;
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x40)) = UNLOCK_MAC;
+	writel(MP_MAC_KEY3, MP_BLOCK_3_BASE + 0x3c);
+	writel(MP_MAC_KEY0, MP_BLOCK_3_BASE + 0x30);
+	writel(MP_MAC_KEY2, MP_BLOCK_3_BASE + 0x38);
+	writel(MP_MAC_KEY1, MP_BLOCK_3_BASE + 0x34);
+	writel(UNLOCK_MAC, MP_BLOCK_3_BASE + 0x40);
 }
 #endif
 
@@ -229,11 +220,11 @@
 #define MP_OHCI_KEY2	0x4823efbc
 #define MP_OHCI_KEY3	0x8651aae4
 #define UNLOCK_OHCI	0x00000010
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x3c)) = MP_OHCI_KEY3;
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x38)) = MP_OHCI_KEY2;
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x30)) = MP_OHCI_KEY0;
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x34)) = MP_OHCI_KEY1;
-	*((AT91_REG *)((AT91_REG) MP_BLOCK_3_BASE + 0x40)) = UNLOCK_OHCI;
+	writel(MP_OHCI_KEY3, MP_BLOCK_3_BASE + 0x3c);
+	writel(MP_OHCI_KEY2, MP_BLOCK_3_BASE + 0x38);
+	writel(MP_OHCI_KEY0, MP_BLOCK_3_BASE + 0x30);
+	writel(MP_OHCI_KEY1, MP_BLOCK_3_BASE + 0x34);
+	writel(UNLOCK_OHCI, MP_BLOCK_3_BASE + 0x40);
 }
 #endif
 
diff --git a/board/atmel/at91cap9adk/led.c b/board/atmel/at91cap9adk/led.c
index 8588a91..04de139 100644
--- a/board/atmel/at91cap9adk/led.c
+++ b/board/atmel/at91cap9adk/led.c
@@ -23,58 +23,55 @@
  */
 
 #include <common.h>
-#include <asm/arch/AT91CAP9.h>
+#include <asm/arch/at91cap9.h>
+#include <asm/arch/at91_pmc.h>
+#include <asm/arch/gpio.h>
+#include <asm/arch/io.h>
 
-#define	RED_LED		AT91C_PIO_PC29	/* this is the power led */
-#define	GREEN_LED	AT91C_PIO_PA10	/* this is the user1 led */
-#define	YELLOW_LED	AT91C_PIO_PA11	/* this is the user1 led */
+#define	RED_LED		AT91_PIN_PC29	/* this is the power led */
+#define	GREEN_LED	AT91_PIN_PA10	/* this is the user1 led */
+#define	YELLOW_LED	AT91_PIN_PA11	/* this is the user1 led */
 
 void red_LED_on(void)
 {
-	AT91C_BASE_PIOC->PIO_SODR = RED_LED;
+	at91_set_gpio_value(RED_LED, 1);
 }
 
 void red_LED_off(void)
 {
-	AT91C_BASE_PIOC->PIO_CODR = RED_LED;
+	at91_set_gpio_value(RED_LED, 0);
 }
 
 void green_LED_on(void)
 {
-	AT91C_BASE_PIOA->PIO_CODR = GREEN_LED;
+	at91_set_gpio_value(GREEN_LED, 0);
 }
 
 void green_LED_off(void)
 {
-	AT91C_BASE_PIOA->PIO_SODR = GREEN_LED;
+	at91_set_gpio_value(GREEN_LED, 1);
 }
 
 void yellow_LED_on(void)
 {
-	AT91C_BASE_PIOA->PIO_CODR = YELLOW_LED;
+	at91_set_gpio_value(YELLOW_LED, 0);
 }
 
 void yellow_LED_off(void)
 {
-	AT91C_BASE_PIOA->PIO_SODR = YELLOW_LED;
+	at91_set_gpio_value(YELLOW_LED, 1);
 }
 
 void coloured_LED_init(void)
 {
 	/* Enable clock */
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_PIOABCD;
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91CAP9_ID_PIOABCD);
 
-	/* Disable peripherals on LEDs */
-	AT91C_BASE_PIOA->PIO_PER = GREEN_LED | YELLOW_LED;
-	/* Enable pins as outputs */
-	AT91C_BASE_PIOA->PIO_OER = GREEN_LED | YELLOW_LED;
-	/* Turn all LEDs OFF */
-	AT91C_BASE_PIOA->PIO_SODR = GREEN_LED | YELLOW_LED;
+	at91_set_gpio_output(RED_LED, 1);
+	at91_set_gpio_output(GREEN_LED, 1);
+	at91_set_gpio_output(YELLOW_LED, 1);
 
-	/* Disable peripherals on LEDs */
-	AT91C_BASE_PIOC->PIO_PER = RED_LED;
-	/* Enable pins as outputs */
-	AT91C_BASE_PIOC->PIO_OER = RED_LED;
-	/* Turn all LEDs OFF */
-	AT91C_BASE_PIOC->PIO_CODR = RED_LED;
+	at91_set_gpio_output(RED_LED, 0);
+	at91_set_gpio_output(GREEN_LED, 1);
+	at91_set_gpio_output(YELLOW_LED, 1);
 }
diff --git a/board/atmel/at91cap9adk/nand.c b/board/atmel/at91cap9adk/nand.c
index 2f02126..1277973 100644
--- a/board/atmel/at91cap9adk/nand.c
+++ b/board/atmel/at91cap9adk/nand.c
@@ -25,7 +25,9 @@
  */
 
 #include <common.h>
-#include <asm/arch/hardware.h>
+#include <asm/arch/at91cap9.h>
+#include <asm/arch/gpio.h>
+#include <asm/arch/at91_pio.h>
 
 #ifdef CONFIG_CMD_NAND
 
@@ -51,10 +53,10 @@
 		IO_ADDR_W |= MASK_ALE;
 		break;
 	case NAND_CTL_CLRNCE:
-		AT91C_BASE_PIOD->PIO_SODR = AT91C_PIO_PD15;
+		at91_set_gpio_value(AT91_PIN_PD15, 1);
 		break;
 	case NAND_CTL_SETNCE:
-		AT91C_BASE_PIOD->PIO_CODR = AT91C_PIO_PD15;
+		at91_set_gpio_value(AT91_PIN_PD15, 0);
 		break;
 	}
 	this->IO_ADDR_W = (void *) IO_ADDR_W;
diff --git a/cpu/arm926ejs/at91sam9/ether.c b/cpu/arm926ejs/at91sam9/ether.c
index 8de1bb7..e4f5601 100644
--- a/cpu/arm926ejs/at91sam9/ether.c
+++ b/cpu/arm926ejs/at91sam9/ether.c
@@ -30,6 +30,6 @@
 #if defined(CONFIG_MACB) && defined(CONFIG_CMD_NET)
 void at91sam9_eth_initialize(bd_t *bi)
 {
-	macb_eth_initialize(0, (void *)AT91C_BASE_MACB, 0x00);
+	macb_eth_initialize(0, (void *)AT91_BASE_EMAC, 0x00);
 }
 #endif
diff --git a/cpu/arm926ejs/at91sam9/spi.c b/cpu/arm926ejs/at91sam9/spi.c
index 0953820..c819608 100644
--- a/cpu/arm926ejs/at91sam9/spi.c
+++ b/cpu/arm926ejs/at91sam9/spi.c
@@ -19,94 +19,135 @@
  *
  */
 
-#include <config.h>
 #include <common.h>
-#include <asm/hardware.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/gpio.h>
+#include <asm/arch/io.h>
+#include <asm/arch/at91_pio.h>
+#include <asm/arch/at91_spi.h>
 
 #ifdef CONFIG_HAS_DATAFLASH
 #include <dataflash.h>
 
-/* Max Value = 10MHz to be compliant to the Continuous Array Read function */
-#define AT91C_SPI_CLK	10000000
-
-/* AC Characteristics: DLYBS = tCSS = 250ns min and DLYBCT = tCSH = 250ns */
-#define DATAFLASH_TCSS	(0xFA << 16)
-#define DATAFLASH_TCHS	(0x8 << 24)
-
-#define AT91C_TIMEOUT_WRDY		200000
-#define AT91C_SPI_PCS0_DATAFLASH_CARD	0xE	/* Chip Select 0: NPCS0%1110 */
-#define AT91C_SPI_PCS3_DATAFLASH_CARD	0x7	/* Chip Select 3: NPCS3%0111 */
+#define AT91_SPI_PCS0_DATAFLASH_CARD	0xE	/* Chip Select 0: NPCS0%1110 */
+#define AT91_SPI_PCS1_DATAFLASH_CARD	0xD	/* Chip Select 0: NPCS0%1101 */
+#define AT91_SPI_PCS3_DATAFLASH_CARD	0x7	/* Chip Select 3: NPCS3%0111 */
 
 void AT91F_SpiInit(void)
 {
 	/* Reset the SPI */
-	AT91C_BASE_SPI0->SPI_CR = AT91C_SPI_SWRST;
+	writel(AT91_SPI_SWRST, AT91_BASE_SPI + AT91_SPI_CR);
 
 	/* Configure SPI in Master Mode with No CS selected !!! */
-	AT91C_BASE_SPI0->SPI_MR =
-		AT91C_SPI_MSTR | AT91C_SPI_MODFDIS | AT91C_SPI_PCS;
+	writel(AT91_SPI_MSTR | AT91_SPI_MODFDIS | AT91_SPI_PCS,
+	       AT91_BASE_SPI + AT91_SPI_MR);
 
 	/* Configure CS0 */
-	AT91C_BASE_SPI0->SPI_CSR[0] =
-		AT91C_SPI_CPOL |
-		(AT91C_SPI_DLYBS & DATAFLASH_TCSS) |
-		(AT91C_SPI_DLYBCT & DATAFLASH_TCHS) |
-		((AT91C_MASTER_CLOCK / (2*AT91C_SPI_CLK)) << 8);
+	writel(AT91_SPI_NCPHA |
+	       (AT91_SPI_DLYBS & DATAFLASH_TCSS) |
+	       (AT91_SPI_DLYBCT & DATAFLASH_TCHS) |
+	       ((AT91_MASTER_CLOCK / AT91_SPI_CLK) << 8),
+	       AT91_BASE_SPI + AT91_SPI_CSR(0));
+
+#ifdef CFG_DATAFLASH_LOGIC_ADDR_CS1
+	/* Configure CS1 */
+	writel(AT91_SPI_NCPHA |
+	       (AT91_SPI_DLYBS & DATAFLASH_TCSS) |
+	       (AT91_SPI_DLYBCT & DATAFLASH_TCHS) |
+	       ((AT91_MASTER_CLOCK / AT91_SPI_CLK) << 8),
+	       AT91_BASE_SPI + AT91_SPI_CSR(1));
+#endif
+
+#ifdef CFG_DATAFLASH_LOGIC_ADDR_CS3
+	/* Configure CS3 */
+	writel(AT91_SPI_NCPHA |
+	       (AT91_SPI_DLYBS & DATAFLASH_TCSS) |
+	       (AT91_SPI_DLYBCT & DATAFLASH_TCHS) |
+	       ((AT91_MASTER_CLOCK / AT91_SPI_CLK) << 8),
+	       AT91_BASE_SPI + AT91_SPI_CSR(3));
+#endif
+
+	/* SPI_Enable */
+	writel(AT91_SPI_SPIEN, AT91_BASE_SPI + AT91_SPI_CR);
+
+	while (!(readl(AT91_BASE_SPI + AT91_SPI_SR) & AT91_SPI_SPIENS));
+
+	/*
+	 * Add tempo to get SPI in a safe state.
+	 * Should not be needed for new silicon (Rev B)
+	 */
+	udelay(500000);
+	readl(AT91_BASE_SPI + AT91_SPI_SR);
+	readl(AT91_BASE_SPI + AT91_SPI_RDR);
+
 }
 
 void AT91F_SpiEnable(int cs)
 {
+	unsigned long mode;
 	switch (cs) {
 	case 0:	/* Configure SPI CS0 for Serial DataFlash AT45DBxx */
-		AT91C_BASE_SPI0->SPI_MR &= 0xFFF0FFFF;
-		AT91C_BASE_SPI0->SPI_MR |=
-			((AT91C_SPI_PCS0_DATAFLASH_CARD<<16) & AT91C_SPI_PCS);
+		mode = readl(AT91_BASE_SPI + AT91_SPI_MR);
+		mode &= 0xFFF0FFFF;
+		writel(mode | ((AT91_SPI_PCS0_DATAFLASH_CARD<<16) & AT91_SPI_PCS),
+		       AT91_BASE_SPI + AT91_SPI_MR);
+		break;
+	case 1:	/* Configure SPI CS1 for Serial DataFlash AT45DBxx */
+		mode = readl(AT91_BASE_SPI + AT91_SPI_MR);
+		mode &= 0xFFF0FFFF;
+		writel(mode | ((AT91_SPI_PCS1_DATAFLASH_CARD<<16) & AT91_SPI_PCS),
+		       AT91_BASE_SPI + AT91_SPI_MR);
 		break;
 	case 3:
-		AT91C_BASE_SPI0->SPI_MR &= 0xFFF0FFFF;
-		AT91C_BASE_SPI0->SPI_MR |=
-			((AT91C_SPI_PCS3_DATAFLASH_CARD<<16) & AT91C_SPI_PCS);
+		mode = readl(AT91_BASE_SPI + AT91_SPI_MR);
+		mode &= 0xFFF0FFFF;
+		writel(mode | ((AT91_SPI_PCS3_DATAFLASH_CARD<<16) & AT91_SPI_PCS),
+		       AT91_BASE_SPI + AT91_SPI_MR);
 		break;
 	}
 
 	/* SPI_Enable */
-	AT91C_BASE_SPI0->SPI_CR = AT91C_SPI_SPIEN;
+	writel(AT91_SPI_SPIEN, AT91_BASE_SPI + AT91_SPI_CR);
 }
 
+unsigned int AT91F_SpiWrite1(AT91PS_DataflashDesc pDesc);
+
 unsigned int AT91F_SpiWrite(AT91PS_DataflashDesc pDesc)
 {
 	unsigned int timeout;
 
+
 	pDesc->state = BUSY;
 
-	AT91C_BASE_SPI0->SPI_PTCR = AT91C_PDC_TXTDIS + AT91C_PDC_RXTDIS;
+	writel(AT91_SPI_TXTDIS + AT91_SPI_RXTDIS, AT91_BASE_SPI + AT91_SPI_PTCR);
+
 
 	/* Initialize the Transmit and Receive Pointer */
-	AT91C_BASE_SPI0->SPI_RPR = (unsigned int)pDesc->rx_cmd_pt;
-	AT91C_BASE_SPI0->SPI_TPR = (unsigned int)pDesc->tx_cmd_pt;
+	writel((unsigned int)pDesc->rx_cmd_pt, AT91_BASE_SPI + AT91_SPI_RPR);
+	writel((unsigned int)pDesc->tx_cmd_pt, AT91_BASE_SPI + AT91_SPI_TPR);
 
 	/* Intialize the Transmit and Receive Counters */
-	AT91C_BASE_SPI0->SPI_RCR = pDesc->rx_cmd_size;
-	AT91C_BASE_SPI0->SPI_TCR = pDesc->tx_cmd_size;
+	writel(pDesc->rx_cmd_size, AT91_BASE_SPI + AT91_SPI_RCR);
+	writel(pDesc->tx_cmd_size, AT91_BASE_SPI + AT91_SPI_TCR);
 
 	if (pDesc->tx_data_size != 0) {
 		/* Initialize the Next Transmit and Next Receive Pointer */
-		AT91C_BASE_SPI0->SPI_RNPR = (unsigned int)pDesc->rx_data_pt;
-		AT91C_BASE_SPI0->SPI_TNPR = (unsigned int)pDesc->tx_data_pt;
+		writel((unsigned int)pDesc->rx_data_pt, AT91_BASE_SPI + AT91_SPI_RNPR);
+		writel((unsigned int)pDesc->tx_data_pt, AT91_BASE_SPI + AT91_SPI_TNPR);
 
 		/* Intialize the Next Transmit and Next Receive Counters */
-		AT91C_BASE_SPI0->SPI_RNCR = pDesc->rx_data_size;
-		AT91C_BASE_SPI0->SPI_TNCR = pDesc->tx_data_size;
+		writel(pDesc->rx_data_size, AT91_BASE_SPI + AT91_SPI_RNCR);
+		writel(pDesc->tx_data_size, AT91_BASE_SPI + AT91_SPI_TNCR);
 	}
 
 	/* arm simple, non interrupt dependent timer */
 	reset_timer_masked();
 	timeout = 0;
 
-	AT91C_BASE_SPI0->SPI_PTCR = AT91C_PDC_TXTEN + AT91C_PDC_RXTEN;
-	while (!(AT91C_BASE_SPI0->SPI_SR & AT91C_SPI_RXBUFF) &&
+	writel(AT91_SPI_TXTEN + AT91_SPI_RXTEN, AT91_BASE_SPI + AT91_SPI_PTCR);
+	while (!(readl(AT91_BASE_SPI + AT91_SPI_SR) & AT91_SPI_RXBUFF) &&
 		((timeout = get_timer_masked()) < CFG_SPI_WRITE_TOUT));
-	AT91C_BASE_SPI0->SPI_PTCR = AT91C_PDC_TXTDIS + AT91C_PDC_RXTDIS;
+	writel(AT91_SPI_TXTDIS + AT91_SPI_RXTDIS, AT91_BASE_SPI + AT91_SPI_PTCR);
 	pDesc->state = IDLE;
 
 	if (timeout >= CFG_SPI_WRITE_TOUT) {
diff --git a/cpu/arm926ejs/at91sam9/timer.c b/cpu/arm926ejs/at91sam9/timer.c
index 79c135f..4e79466 100644
--- a/cpu/arm926ejs/at91sam9/timer.c
+++ b/cpu/arm926ejs/at91sam9/timer.c
@@ -24,22 +24,24 @@
 
 #include <common.h>
 #include <asm/arch/hardware.h>
+#include <asm/arch/at91_pit.h>
+#include <asm/arch/at91_pmc.h>
+#include <asm/arch/at91_rstc.h>
+#include <asm/arch/io.h>
 
 /*
  * We're using the AT91CAP9/SAM9 PITC in 32 bit mode, by
  * setting the 20 bit counter period to its maximum (0xfffff).
  */
 #define TIMER_LOAD_VAL	0xfffff
-#define READ_RESET_TIMER (AT91C_BASE_PITC->PITC_PIVR)
-#define READ_TIMER (AT91C_BASE_PITC->PITC_PIIR)
+#define READ_RESET_TIMER at91_sys_read(AT91_PIT_PIVR)
+#define READ_TIMER at91_sys_read(AT91_PIT_PIIR)
 #define TIMER_FREQ (AT91C_MASTER_CLOCK << 4)
 #define TICKS_TO_USEC(ticks) ((ticks) / 6)
 
 ulong get_timer_masked(void);
 ulong resettime;
 
-AT91PS_PITC p_pitc;
-
 /* nothing really to do with interrupts, just starts up a counter. */
 int timer_init(void)
 {
@@ -47,13 +49,10 @@
 	 * Enable PITC Clock
 	 * The clock is already enabled for system controller in boot
 	 */
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_SYS;
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_SYS);
 
 	/* Enable PITC */
-	AT91C_BASE_PITC->PITC_PIMR = AT91C_PITC_PITEN;
-
-	/* Load PITC_PIMR with the right timer value */
-	AT91C_BASE_PITC->PITC_PIMR |= TIMER_LOAD_VAL;
+	at91_sys_write(AT91_PIT_MR, TIMER_LOAD_VAL | AT91_PIT_PITEN);
 
 	reset_timer_masked();
 
@@ -67,6 +66,7 @@
 static inline ulong get_timer_raw(void)
 {
 	ulong now = READ_TIMER;
+
 	if (now >= resettime)
 		return now - resettime;
 	else
@@ -129,6 +129,7 @@
 ulong get_tbclk(void)
 {
 	ulong tbclk;
+
 	tbclk = CFG_HZ;
 	return tbclk;
 }
@@ -139,9 +140,9 @@
 void reset_cpu(ulong ignored)
 {
 	/* this is the way Linux does it */
-	AT91C_BASE_RSTC->RSTC_RCR = (0xA5 << 24) |
-				    AT91C_RSTC_PROCRST |
-				    AT91C_RSTC_PERRST;
+	at91_sys_write(AT91_RSTC_CR, AT91_RSTC_KEY |
+				     AT91_RSTC_PROCRST |
+				     AT91_RSTC_PERRST);
 
 	while (1);
 	/* Never reached */
diff --git a/cpu/arm926ejs/at91sam9/usb.c b/cpu/arm926ejs/at91sam9/usb.c
index 09292a6..d678897 100644
--- a/cpu/arm926ejs/at91sam9/usb.c
+++ b/cpu/arm926ejs/at91sam9/usb.c
@@ -26,12 +26,14 @@
 #if defined(CONFIG_USB_OHCI_NEW) && defined(CFG_USB_OHCI_CPU_INIT)
 
 #include <asm/arch/hardware.h>
+#include <asm/arch/io.h>
+#include <asm/arch/at91_pmc.h>
 
 int usb_cpu_init(void)
 {
 	/* Enable USB host clock. */
-	AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_UHP;
-	AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_UHP;
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_UHP);
+	at91_sys_write(AT91_PMC_SCER, AT91_PMC_UHP);
 
 	return 0;
 }
@@ -39,8 +41,8 @@
 int usb_cpu_stop(void)
 {
 	/* Disable USB host clock. */
-	AT91C_BASE_PMC->PMC_PCDR = 1 << AT91C_ID_UHP;
-	AT91C_BASE_PMC->PMC_SCDR = AT91C_PMC_UHP;
+	at91_sys_write(AT91_PMC_PCDR, 1 << AT91_ID_UHP);
+	at91_sys_write(AT91_PMC_SCDR, AT91_PMC_UHP);
 	return 0;
 }
 
diff --git a/include/configs/at91cap9adk.h b/include/configs/at91cap9adk.h
index f0dfd71..dab21d0 100644
--- a/include/configs/at91cap9adk.h
+++ b/include/configs/at91cap9adk.h
@@ -1,5 +1,5 @@
 /*
- * (C) Copyright 2007
+ * (C) Copyright 2007-2008
  * Stelian Pop <stelian.pop <at> leadtechdesign.com>
  * Lead Tech Design <www.leadtechdesign.com>
  *
@@ -28,8 +28,8 @@
 #define __CONFIG_H
 
 /* ARM asynchronous clock */
-#define AT91C_MAIN_CLOCK	200000000	/* from 12 MHz crystal */
-#define AT91C_MASTER_CLOCK	100000000	/* peripheral = main / 2 */
+#define AT91_MAIN_CLOCK		200000000	/* from 12 MHz crystal */
+#define AT91_MASTER_CLOCK	100000000	/* peripheral = main / 2 */
 #define CFG_HZ			1000000		/* 1us resolution */
 
 #define AT91_SLOW_CLOCK		32768	/* slow clock */
@@ -46,19 +46,9 @@
 #define CONFIG_SKIP_LOWLEVEL_INIT
 #define CONFIG_SKIP_RELOCATE_UBOOT
 
-#define ROUND(A, B)		(((A) + (B)) & ~((B) - 1))
-/*
- * Size of malloc() pool
- */
-#define CFG_MALLOC_LEN		ROUND(CFG_ENV_SIZE + 128*1024, 0x1000)
-#define CFG_GBL_DATA_SIZE	128	/* 128 bytes for initial data */
-
-#define CONFIG_BAUDRATE		115200
-
 /*
  * Hardware drivers
  */
-
 #define CONFIG_ATMEL_USART	1
 #undef CONFIG_USART0
 #undef CONFIG_USART1
@@ -104,7 +94,9 @@
 #define CFG_SPI_WRITE_TOUT		(5*CFG_HZ)
 #define CFG_MAX_DATAFLASH_BANKS		1
 #define CFG_DATAFLASH_LOGIC_ADDR_CS0	0xC0000000	/* CS0 */
-#define CONFIG_NEW_PARTITION		1
+#define AT91_SPI_CLK			20000000
+#define DATAFLASH_TCSS			(0xFA << 16)
+#define DATAFLASH_TCHS			(0x8 << 24)
 
 /* NOR flash */
 #define CFG_FLASH_CFI			1
@@ -114,39 +106,11 @@
 #define CFG_MAX_FLASH_SECT		256
 #define CFG_MAX_FLASH_BANKS		1
 
-#define AT91C_FLASH_NWE_SETUP		(4 << 0)
-#define AT91C_FLASH_NCS_WR_SETUP	(2 << 8)
-#define AT91C_FLASH_NRD_SETUP		(4 << 16)
-#define AT91C_FLASH_NCS_RD_SETUP	(2 << 24)
-
-#define AT91C_FLASH_NWE_PULSE		(8 << 0)
-#define AT91C_FLASH_NCS_WR_PULSE	(10 << 8)
-#define AT91C_FLASH_NRD_PULSE		(8 << 16)
-#define AT91C_FLASH_NCS_RD_PULSE	(10 << 24)
-
-#define AT91C_FLASH_NWE_CYCLE		(16 << 0)
-#define AT91C_FLASH_NRD_CYCLE		(16 << 16)
-
 /* NAND flash */
 #define NAND_MAX_CHIPS			1
 #define CFG_MAX_NAND_DEVICE		1
 #define CFG_NAND_BASE			0x40000000
 
-#define AT91C_SM_NWE_SETUP		(2 << 0)
-#define AT91C_SM_NCS_WR_SETUP		(1 << 8)
-#define AT91C_SM_NRD_SETUP		(2 << 16)
-#define AT91C_SM_NCS_RD_SETUP		(1 << 24)
-
-#define AT91C_SM_NWE_PULSE		(4 << 0)
-#define AT91C_SM_NCS_WR_PULSE		(6 << 8)
-#define AT91C_SM_NRD_PULSE		(4 << 16)
-#define AT91C_SM_NCS_RD_PULSE		(6 << 24)
-
-#define AT91C_SM_NWE_CYCLE		(8 << 0)
-#define AT91C_SM_NRD_CYCLE		(8 << 16)
-
-#define AT91C_SM_TDF			(1 << 16)
-
 /* Ethernet */
 #define CONFIG_MACB			1
 #define CONFIG_RMII			1
@@ -159,15 +123,14 @@
 #define LITTLEENDIAN			1
 #define CONFIG_DOS_PARTITION		1
 #define CFG_USB_OHCI_CPU_INIT		1
-#define CFG_USB_OHCI_REGS_BASE		0x00700000	/* AT91C_BASE_UHP */
+#define CFG_USB_OHCI_REGS_BASE		0x00700000	/* AT91_BASE_UHP */
 #define CFG_USB_OHCI_SLOT_NAME		"at91cap9"
 #define CFG_USB_OHCI_MAX_ROOT_PORTS	2
 
-
 #define CFG_LOAD_ADDR			0x72000000	/* load address */
 
 #define CFG_MEMTEST_START		PHYS_SDRAM
-#define CFG_MEMTEST_END			0x73000000
+#define CFG_MEMTEST_END			0x73e00000
 
 #define CFG_USE_DATAFLASH		1
 #undef CFG_USE_NORFLASH
@@ -194,6 +157,7 @@
 
 #endif
 
+#define CONFIG_BAUDRATE		115200
 #define CFG_BAUDRATE_TABLE	{115200 , 19200, 38400, 57600, 9600 }
 
 #define CFG_PROMPT		"U-Boot> "
@@ -203,6 +167,13 @@
 #define CFG_LONGHELP		1
 #define CONFIG_CMDLINE_EDITING	1
 
+#define ROUND(A, B)		(((A) + (B)) & ~((B) - 1))
+/*
+ * Size of malloc() pool
+ */
+#define CFG_MALLOC_LEN		ROUND(CFG_ENV_SIZE + 128*1024, 0x1000)
+#define CFG_GBL_DATA_SIZE	128	/* 128 bytes for initial data */
+
 #define CONFIG_STACKSIZE	(32*1024)	/* regular stack */
 
 #ifdef CONFIG_USE_IRQ