Merge branch 'master' of git://git.denx.de/u-boot-arm

Conflicts:

	board/davinci/da830evm/da830evm.c
	board/edb93xx/sdram_cfg.c
	board/esd/otc570/otc570.c
	board/netstar/eeprom.c
	board/netstar/eeprom_start.S
	cpu/arm920t/ep93xx/timer.c
	include/configs/netstar.h
	include/configs/otc570.h

Signed-off-by: Minkyu Kang <mk7.kang@samsung.com>

53 files changed, 2803 insertions, 466 deletions

diff --git a/cpu/74xx_7xx/traps.c b/cpu/74xx_7xx/traps.c
index 24e28e2..1d5dd01 100644
--- a/cpu/74xx_7xx/traps.c
+++ b/cpu/74xx_7xx/traps.c
@@ -34,6 +34,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 
 #ifdef CONFIG_AMIGAONEG3SE
diff --git a/cpu/arm920t/at91/Makefile b/cpu/arm920t/at91/Makefile
new file mode 100644
index 0000000..d8a4383
--- /dev/null
+++ b/cpu/arm920t/at91/Makefile
@@ -0,0 +1,47 @@
+#
+# (C) Copyright 2000-2006
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# See file CREDITS for list of people who contributed to this
+# project.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of
+# the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+# MA 02111-1307 USA
+#
+
+include $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(SOC).a
+
+SOBJS	+= lowlevel_init.o
+COBJS	+= reset.o
+COBJS	+= timer.o
+
+SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
+
+all:	$(obj).depend $(LIB)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/arm920t/at91/lowlevel_init.S b/cpu/arm920t/at91/lowlevel_init.S
new file mode 100644
index 0000000..22fc86c
--- /dev/null
+++ b/cpu/arm920t/at91/lowlevel_init.S
@@ -0,0 +1,164 @@
+/*
+ * Copyright (C) 1999 2000 2001 Erik Mouw (J.A.K.Mouw@its.tudelft.nl) and
+ *		       Jan-Derk Bakker (J.D.Bakker@its.tudelft.nl)
+ *
+ * Modified for the at91rm9200dk board by
+ * (C) Copyright 2004
+ * Gary Jennejohn, DENX Software Engineering, <garyj@denx.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <config.h>
+
+#ifndef CONFIG_SKIP_LOWLEVEL_INIT
+
+#include <asm/arch/hardware.h>
+#include <asm/arch/at91_mc.h>
+#include <asm/arch/at91_pmc.h>
+#include <asm/arch/at91_pio.h>
+
+#define ARM920T_CONTROL	0xC0000000	/* @ set bit 31 (iA) and 30 (nF) */
+
+_MTEXT_BASE:
+#undef START_FROM_MEM
+#ifdef START_FROM_MEM
+	.word	TEXT_BASE-PHYS_FLASH_1
+#else
+	.word	TEXT_BASE
+#endif
+
+.globl lowlevel_init
+lowlevel_init:
+	ldr     r1, =AT91_ASM_PMC_MOR
+	/* Main oscillator Enable register */
+#ifdef CONFIG_SYS_USE_MAIN_OSCILLATOR
+	ldr     r0, =0x0000FF01		/* Enable main oscillator */
+#else
+	ldr     r0, =0x0000FF00		/* Disable main oscillator */
+#endif
+	str     r0, [r1] /*AT91C_CKGR_MOR] */
+	/* Add loop to compensate Main Oscillator startup time */
+	ldr     r0, =0x00000010
+LoopOsc:
+	subs    r0, r0, #1
+	bhi     LoopOsc
+
+	/* memory control configuration */
+	/* this isn't very elegant, but	 what the heck */
+	ldr	r0, =SMRDATA
+	ldr	r1, _MTEXT_BASE
+	sub	r0, r0, r1
+	add	r2, r0, #80
+pllloop:
+	/* the address */
+	ldr	r1, [r0], #4
+	/* the value */
+	ldr	r3, [r0], #4
+	str	r3, [r1]
+	cmp	r2, r0
+	bne	pllloop
+	/* delay - this is all done by guess */
+	ldr	r0, =0x00010000
+	/* (vs reading PMC_SR for LOCKA, LOCKB ... or MOSCS earlier) */
+lock:
+	subs	r0, r0, #1
+	bhi	lock
+	ldr	r0, =SMRDATA1
+	ldr	r1, _MTEXT_BASE
+	sub	r0, r0, r1
+	add	r2, r0, #176
+sdinit:
+	/* the address */
+	ldr	r1, [r0], #4
+	/* the value */
+	ldr	r3, [r0], #4
+	str	r3, [r1]
+	cmp	r2, r0
+	bne	sdinit
+
+	/* switch from FastBus to Asynchronous clock mode */
+	mrc	p15, 0, r0, c1, c0, 0
+	orr	r0, r0, #ARM920T_CONTROL
+	mcr	p15, 0, r0, c1, c0, 0
+
+	/* everything is fine now */
+	mov	pc, lr
+
+	.ltorg
+
+SMRDATA:
+	.word AT91_ASM_MC_EBI_CFG
+	.word CONFIG_SYS_EBI_CFGR_VAL
+	.word AT91_ASM_MC_SMC_CSR0
+	.word CONFIG_SYS_SMC_CSR0_VAL
+	.word AT91_ASM_PMC_PLLAR
+	.word CONFIG_SYS_PLLAR_VAL
+	.word AT91_ASM_PMC_PLLBR
+	.word CONFIG_SYS_PLLBR_VAL
+	.word AT91_ASM_PMC_MCKR
+	.word CONFIG_SYS_MCKR_VAL
+	/* here there's a delay */
+SMRDATA1:
+	.word AT91_ASM_PIOC_ASR
+	.word CONFIG_SYS_PIOC_ASR_VAL
+	.word AT91_ASM_PIOC_BSR
+	.word CONFIG_SYS_PIOC_BSR_VAL
+	.word AT91_ASM_PIOC_PDR
+	.word CONFIG_SYS_PIOC_PDR_VAL
+	.word AT91_ASM_MC_EBI_CSA
+	.word CONFIG_SYS_EBI_CSA_VAL
+	.word AT91_ASM_MC_SDRAMC_CR
+	.word CONFIG_SYS_SDRC_CR_VAL
+	.word AT91_ASM_MC_SDRAMC_MR
+	.word CONFIG_SYS_SDRC_MR_VAL
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word AT91_ASM_MC_SDRAMC_MR
+	.word CONFIG_SYS_SDRC_MR_VAL1
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word AT91_ASM_MC_SDRAMC_MR
+	.word CONFIG_SYS_SDRC_MR_VAL2
+	.word CONFIG_SYS_SDRAM1
+	.word CONFIG_SYS_SDRAM_VAL
+	.word AT91_ASM_MC_SDRAMC_TR
+	.word CONFIG_SYS_SDRC_TR_VAL
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	.word AT91_ASM_MC_SDRAMC_MR
+	.word CONFIG_SYS_SDRC_MR_VAL3
+	.word CONFIG_SYS_SDRAM
+	.word CONFIG_SYS_SDRAM_VAL
+	/* SMRDATA1 is 176 bytes long */
+#endif /* CONFIG_SKIP_LOWLEVEL_INIT */
diff --git a/cpu/arm920t/at91/reset.c b/cpu/arm920t/at91/reset.c
new file mode 100644
index 0000000..ce9c156
--- /dev/null
+++ b/cpu/arm920t/at91/reset.c
@@ -0,0 +1,59 @@
+/*
+ * (C) Copyright 2002
+ * Lineo, Inc. <www.lineo.com>
+ * Bernhard Kuhn <bkuhn@lineo.com>
+ *
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Marius Groeger <mgroeger@sysgo.de>
+ *
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Alex Zuepke <azu@sysgo.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/at91_st.h>
+
+void board_reset(void) __attribute__((__weak__));
+
+void reset_cpu(ulong ignored)
+{
+	at91_st_t *st = (at91_st_t *) AT91_ST_BASE;
+#if defined(CONFIG_AT91RM9200_USART)
+	/*shutdown the console to avoid strange chars during reset */
+	serial_exit();
+#endif
+
+	if (board_reset)
+		board_reset();
+
+	/* Reset the cpu by setting up the watchdog timer */
+	writel(AT91_ST_WDMR_RSTEN | AT91_ST_WDMR_EXTEN | AT91_ST_WDMR_WDV(2),
+		&st->wdmr);
+	writel(AT91_ST_CR_WDRST, &st->cr);
+	/* and let it timeout */
+	while (1)
+		;
+	/* Never reached */
+}
diff --git a/cpu/arm920t/at91/timer.c b/cpu/arm920t/at91/timer.c
new file mode 100644
index 0000000..91377d4
--- /dev/null
+++ b/cpu/arm920t/at91/timer.c
@@ -0,0 +1,163 @@
+/*
+ * (C) Copyright 2002
+ * Lineo, Inc. <www.lineo.com>
+ * Bernhard Kuhn <bkuhn@lineo.com>
+ *
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Marius Groeger <mgroeger@sysgo.de>
+ *
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Alex Zuepke <azu@sysgo.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+
+#include <asm/io.h>
+#include <asm/hardware.h>
+#include <asm/arch/at91_tc.h>
+#include <asm/arch/at91_pmc.h>
+
+/* the number of clocks per CONFIG_SYS_HZ */
+#define TIMER_LOAD_VAL (CONFIG_SYS_HZ_CLOCK/CONFIG_SYS_HZ)
+
+static u32 timestamp;
+static u32 lastinc;
+
+int timer_init(void)
+{
+	at91_tc_t *tc = (at91_tc_t *) AT91_TC_BASE;
+	at91_pmc_t *pmc = (at91_pmc_t *) AT91_PMC_BASE;
+
+	/* enables TC1.0 clock */
+	writel(1 << AT91_ID_TC0, &pmc->pcer);	/* enable clock */
+
+	writel(0, &tc->bcr);
+	writel(AT91_TC_BMR_TC0XC0S_NONE | AT91_TC_BMR_TC1XC1S_NONE |
+		AT91_TC_BMR_TC2XC2S_NONE , &tc->bmr);
+
+	writel(AT91_TC_CCR_CLKDIS, &tc->tc[0].ccr);
+	/* set to MCLK/2 and restart the timer
+	when the value in TC_RC is reached */
+	writel(AT91_TC_CMR_TCCLKS_CLOCK1 | AT91_TC_CMR_CPCTRG, &tc->tc[0].cmr);
+
+	writel(0xFFFFFFFF, &tc->tc[0].idr); /* disable interupts */
+	writel(TIMER_LOAD_VAL, &tc->tc[0].rc);
+
+	writel(AT91_TC_CCR_SWTRG | AT91_TC_CCR_CLKEN, &tc->tc[0].ccr);
+	lastinc = 0;
+	timestamp = 0;
+
+	return 0;
+}
+
+/*
+ * timer without interrupts
+ */
+
+void reset_timer(void)
+{
+	reset_timer_masked();
+}
+
+ulong get_timer(ulong base)
+{
+	return get_timer_masked() - base;
+}
+
+void set_timer(ulong t)
+{
+	timestamp = t;
+}
+
+void __udelay(unsigned long usec)
+{
+	udelay_masked(usec);
+}
+
+void reset_timer_masked(void)
+{
+	/* reset time */
+	at91_tc_t *tc = (at91_tc_t *) AT91_TC_BASE;
+	lastinc = readl(&tc->tc[0].cv) & 0x0000ffff;
+	timestamp = 0;
+}
+
+ulong get_timer_raw(void)
+{
+	at91_tc_t *tc = (at91_tc_t *) AT91_TC_BASE;
+	u32 now;
+
+	now = readl(&tc->tc[0].cv) & 0x0000ffff;
+
+	if (now >= lastinc) {
+		/* normal mode */
+		timestamp += now - lastinc;
+	} else {
+		/* we have an overflow ... */
+		timestamp += now + TIMER_LOAD_VAL - lastinc;
+	}
+	lastinc = now;
+
+	return timestamp;
+}
+
+ulong get_timer_masked(void)
+{
+	return get_timer_raw()/TIMER_LOAD_VAL;
+}
+
+void udelay_masked(unsigned long usec)
+{
+	u32 tmo;
+	u32 endtime;
+	signed long diff;
+
+	tmo = CONFIG_SYS_HZ_CLOCK / 1000;
+	tmo *= usec;
+	tmo /= 1000;
+
+	endtime = get_timer_raw() + tmo;
+
+	do {
+		u32 now = get_timer_raw();
+		diff = endtime - now;
+	} while (diff >= 0);
+}
+
+/*
+ * This function is derived from PowerPC code (read timebase as long long).
+ * On ARM it just returns the timer value.
+ */
+unsigned long long get_ticks(void)
+{
+	return get_timer(0);
+}
+
+/*
+ * This function is derived from PowerPC code (timebase clock frequency).
+ * On ARM it returns the number of timer ticks per second.
+ */
+ulong get_tbclk(void)
+{
+	return CONFIG_SYS_HZ;
+}
diff --git a/cpu/arm920t/at91rm9200/bcm5221.c b/cpu/arm920t/at91rm9200/bcm5221.c
index b52c615..8de3cba 100644
--- a/cpu/arm920t/at91rm9200/bcm5221.c
+++ b/cpu/arm920t/at91rm9200/bcm5221.c
@@ -28,10 +28,10 @@
 
 #include <at91rm9200_net.h>
 #include <net.h>
-#include <bcm5221.h>
-
 #ifdef CONFIG_DRIVER_ETHER
 
+#include <bcm5221.h>
+
 #if defined(CONFIG_CMD_NET)
 
 /*
diff --git a/cpu/arm920t/at91rm9200/dm9161.c b/cpu/arm920t/at91rm9200/dm9161.c
index 1beb6e8..6d4384f 100644
--- a/cpu/arm920t/at91rm9200/dm9161.c
+++ b/cpu/arm920t/at91rm9200/dm9161.c
@@ -23,9 +23,8 @@
 
 #include <at91rm9200_net.h>
 #include <net.h>
-#include <dm9161.h>
-
 #ifdef CONFIG_DRIVER_ETHER
+#include <dm9161.h>
 
 #if defined(CONFIG_CMD_NET)
 
diff --git a/cpu/arm920t/cpu.c b/cpu/arm920t/cpu.c
index 34adb11..be82c87 100644
--- a/cpu/arm920t/cpu.c
+++ b/cpu/arm920t/cpu.c
@@ -33,6 +33,10 @@
 #include <command.h>
 #include <asm/system.h>
 
+#ifdef CONFIG_AT91_LEGACY
+#warning Your board is using legacy AT91RM9200 SoC access. Please update!
+#endif
+
 static void cache_flush(void);
 
 int cleanup_before_linux (void)
diff --git a/cpu/arm920t/ep93xx/timer.c b/cpu/arm920t/ep93xx/timer.c
index 6d969d9..4a0ce4d 100644
--- a/cpu/arm920t/ep93xx/timer.c
+++ b/cpu/arm920t/ep93xx/timer.c
@@ -1,8 +1,7 @@
 /*
  * Cirrus Logic EP93xx timer support.
  *
- * Copyright (C) 2009, 2010
- * Matthias Kaehlcke <matthias@kaehlcke.net>
+ * Copyright (C) 2009, 2010 Matthias Kaehlcke <matthias@kaehlcke.net>
  *
  * Copyright (C) 2004, 2005
  * Cory T. Tusar, Videon Central, Inc., <ctusar@videon-central.com>
@@ -31,70 +30,60 @@
 #include <linux/types.h>
 #include <asm/arch/ep93xx.h>
 #include <asm/io.h>
+#include <div64.h>
 
 #define TIMER_CLKSEL	(1 << 3)
-#define TIMER_MODE	(1 << 6)
 #define TIMER_ENABLE	(1 << 7)
 
-#define TIMER_FREQ	508469
-#define TIMER_LOAD_VAL	(TIMER_FREQ / CONFIG_SYS_HZ)
+#define TIMER_FREQ			508469		/* ticks / second */
+#define TIMER_MAX_VAL			0xFFFFFFFF
 
-static ulong timestamp;
-static ulong lastdec;
-
-static inline unsigned long clk_to_systicks(unsigned long clk_ticks)
+static struct ep93xx_timer
 {
-	unsigned long sys_ticks = (clk_ticks * CONFIG_SYS_HZ) / TIMER_FREQ;
-
-	return sys_ticks;
-}
+	unsigned long long ticks;
+	unsigned long last_read;
+} timer;
 
-static inline unsigned long usecs_to_ticks(unsigned long usecs)
+static inline unsigned long long usecs_to_ticks(unsigned long usecs)
 {
-	unsigned long ticks;
-
-	if (usecs >= 1000) {
-		ticks = usecs / 1000;
-		ticks *= (TIMER_LOAD_VAL * CONFIG_SYS_HZ);
-		ticks /= 1000;
-	} else {
-		ticks = usecs * TIMER_LOAD_VAL * CONFIG_SYS_HZ;
-		ticks /= (1000 * 1000);
-	}
+	unsigned long long ticks = (unsigned long long)usecs * TIMER_FREQ;
+	do_div(ticks, 1000 * 1000);
 
 	return ticks;
 }
 
-static inline unsigned long read_timer(void)
+static inline void read_timer(void)
 {
-	struct timer_regs *timer = (struct timer_regs *)TIMER_BASE;
+	struct timer_regs *timer_regs = (struct timer_regs *)TIMER_BASE;
+	const unsigned long now = TIMER_MAX_VAL - readl(&timer_regs->timer3.value);
+
+	if (now >= timer.last_read)
+		timer.ticks += now - timer.last_read;
+	else
+		/* an overflow occurred */
+		timer.ticks += TIMER_MAX_VAL - timer.last_read + now;
 
-	return readl(&timer->timer3.value);
+	timer.last_read = now;
 }
 
 /*
- * timer without interrupts
+ * Get the number of ticks (in CONFIG_SYS_HZ resolution)
  */
 unsigned long long get_ticks(void)
 {
-	const unsigned long now = read_timer();
+	unsigned long long sys_ticks;
 
-	if (lastdec >= now) {
-		/* normal mode */
-		timestamp += lastdec - now;
-	} else {
-		/* we have an overflow ... */
-		timestamp += lastdec + TIMER_LOAD_VAL - now;
-	}
+	read_timer();
 
-	lastdec = now;
+	sys_ticks = timer.ticks * CONFIG_SYS_HZ;
+	do_div(sys_ticks, TIMER_FREQ);
 
-	return timestamp;
+	return sys_ticks;
 }
 
 unsigned long get_timer_masked(void)
 {
-	return clk_to_systicks(get_ticks());
+	return get_ticks();
 }
 
 unsigned long get_timer(unsigned long base)
@@ -104,8 +93,8 @@ unsigned long get_timer(unsigned long base)
 
 void reset_timer_masked(void)
 {
-	lastdec = read_timer();
-	timestamp = 0;
+	read_timer();
+	timer.ticks = 0;
 }
 
 void reset_timer(void)
@@ -113,45 +102,31 @@ void reset_timer(void)
 	reset_timer_masked();
 }
 
-void set_timer(unsigned long t)
-{
-	timestamp = t;
-}
-
 void __udelay(unsigned long usec)
 {
-	const unsigned long ticks = usecs_to_ticks(usec);
-	const unsigned long target = clk_to_systicks(ticks) + get_timer(0);
+	unsigned long long target;
 
-	while (get_timer_masked() < target)
-		/* noop */;
-}
+	read_timer();
 
-void udelay_masked(unsigned long usec)
-{
-	const unsigned long ticks = usecs_to_ticks(usec);
-	const unsigned long target = clk_to_systicks(ticks) + get_timer(0);
-
-	reset_timer_masked();
+	target = timer.ticks + usecs_to_ticks(usec);
 
-	while (get_timer_masked() < target)
-		/* noop */;
+	while (timer.ticks < target)
+		read_timer();
 }
 
 int timer_init(void)
 {
-	struct timer_regs *timer = (struct timer_regs *)TIMER_BASE;
+	struct timer_regs *timer_regs = (struct timer_regs *)TIMER_BASE;
 
-	/* use timer 3 with 508KHz and free running */
-	writel(TIMER_CLKSEL, &timer->timer3.control);
+	/* use timer 3 with 508KHz and free running, not enabled now */
+	writel(TIMER_CLKSEL, &timer_regs->timer3.control);
 
-	/* auto load, manual update of Timer 3 */
-	lastdec = TIMER_LOAD_VAL;
-	writel(TIMER_LOAD_VAL, &timer->timer3.load);
+	/* set initial timer value */
+	writel(TIMER_MAX_VAL, &timer_regs->timer3.load);
 
-	/* Enable the timer and periodic mode */
-	writel(TIMER_ENABLE | TIMER_MODE | TIMER_CLKSEL,
-		&timer->timer3.control);
+	/* Enable the timer */
+	writel(TIMER_ENABLE | TIMER_CLKSEL,
+		&timer_regs->timer3.control);
 
 	reset_timer_masked();
 
diff --git a/cpu/arm926ejs/at91/at91cap9_devices.c b/cpu/arm926ejs/at91/at91cap9_devices.c
index 39e405f..2d878fd 100644
--- a/cpu/arm926ejs/at91/at91cap9_devices.c
+++ b/cpu/arm926ejs/at91/at91cap9_devices.c
@@ -34,30 +34,38 @@
 
 void at91_serial0_hw_init(void)
 {
-	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 << AT91CAP9_ID_US0);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 22, 1);		/* TXD0 */
+	at91_set_a_periph(AT91_PIO_PORTA, 23, 0);		/* RXD0 */
+	writel(1 << AT91CAP9_ID_US0, &pmc->pcer);
 }
 
 void at91_serial1_hw_init(void)
 {
-	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 << AT91CAP9_ID_US1);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTD, 0, 1);		/* TXD1 */
+	at91_set_a_periph(AT91_PIO_PORTD, 1, 0);		/* RXD1 */
+	writel(1 << AT91CAP9_ID_US1, &pmc->pcer);
 }
 
 void at91_serial2_hw_init(void)
 {
-	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 << AT91CAP9_ID_US2);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTD, 2, 1);		/* TXD2 */
+	at91_set_a_periph(AT91_PIO_PORTD, 3, 0);		/* RXD2 */
+	writel(1 << AT91CAP9_ID_US2, &pmc->pcer);
 }
 
 void at91_serial3_hw_init(void)
 {
-	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);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTC, 30, 0);		/* DRXD */
+	at91_set_a_periph(AT91_PIO_PORTC, 31, 1);		/* DTXD */
+	writel(1 << AT91_ID_SYS, &pmc->pcer);
 }
 
 void at91_serial_hw_init(void)
@@ -82,71 +90,75 @@ void at91_serial_hw_init(void)
 #ifdef CONFIG_HAS_DATAFLASH
 void at91_spi0_hw_init(unsigned long cs_mask)
 {
-	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 */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_b_periph(AT91_PIO_PORTA, 0, 0);	/* SPI0_MISO */
+	at91_set_b_periph(AT91_PIO_PORTA, 1, 0);	/* SPI0_MOSI */
+	at91_set_b_periph(AT91_PIO_PORTA, 2, 0);	/* SPI0_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91CAP9_ID_SPI0);
+	writel(1 << AT91CAP9_ID_SPI0, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_B_periph(AT91_PIN_PA5, 1);
+		at91_set_b_periph(AT91_PIO_PORTA, 5, 1);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_B_periph(AT91_PIN_PA3, 1);
+		at91_set_b_periph(AT91_PIO_PORTA, 3, 1);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_B_periph(AT91_PIN_PD0, 1);
+		at91_set_b_periph(AT91_PIO_PORTD, 0, 1);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_B_periph(AT91_PIN_PD1, 1);
+		at91_set_b_periph(AT91_PIO_PORTD, 1, 1);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PA5, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 5, 1);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PA3, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 3, 1);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PD0, 1);
+		at91_set_pio_output(AT91_PIO_PORTD, 0, 1);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PD1, 1);
+		at91_set_pio_output(AT91_PIO_PORTD, 1, 1);
 	}
 }
 
 void at91_spi1_hw_init(unsigned long cs_mask)
 {
-	at91_set_A_periph(AT91_PIN_PB12, 0);	/* SPI1_MISO */
-	at91_set_A_periph(AT91_PIN_PB13, 0);	/* SPI1_MOSI */
-	at91_set_A_periph(AT91_PIN_PB14, 0);	/* SPI1_SPCK */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 12, 0);	/* SPI1_MISO */
+	at91_set_a_periph(AT91_PIO_PORTB, 13, 0);	/* SPI1_MOSI */
+	at91_set_a_periph(AT91_PIO_PORTB, 14, 0);	/* SPI1_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91CAP9_ID_SPI1);
+	writel(1 << AT91CAP9_ID_SPI1, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_A_periph(AT91_PIN_PB15, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 15, 1);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_A_periph(AT91_PIN_PB16, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 16, 1);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_A_periph(AT91_PIN_PB17, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 17, 1);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_A_periph(AT91_PIN_PB18, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 18, 1);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PB15, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 15, 1);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PB16, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 16, 1);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PB17, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 17, 1);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PB18, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 18, 1);
 	}
 
 }
@@ -155,26 +167,26 @@ void at91_spi1_hw_init(unsigned long cs_mask)
 #ifdef CONFIG_MACB
 void at91_macb_hw_init(void)
 {
-	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 */
+	at91_set_a_periph(AT91_PIO_PORTB, 21, 0);	/* ETXCK_EREFCK */
+	at91_set_a_periph(AT91_PIO_PORTB, 22, 0);	/* ERXDV */
+	at91_set_a_periph(AT91_PIO_PORTB, 25, 0);	/* ERX0 */
+	at91_set_a_periph(AT91_PIO_PORTB, 26, 0);	/* ERX1 */
+	at91_set_a_periph(AT91_PIO_PORTB, 27, 0);	/* ERXER */
+	at91_set_a_periph(AT91_PIO_PORTB, 28, 0);	/* ETXEN */
+	at91_set_a_periph(AT91_PIO_PORTB, 23, 0);	/* ETX0 */
+	at91_set_a_periph(AT91_PIO_PORTB, 24, 0);	/* ETX1 */
+	at91_set_a_periph(AT91_PIO_PORTB, 30, 0);	/* EMDIO */
+	at91_set_a_periph(AT91_PIO_PORTB, 29, 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 */
+	at91_set_b_periph(AT91_PIO_PORTC, 25, 0);	/* ECRS */
+	at91_set_b_periph(AT91_PIO_PORTC, 26, 0);	/* ECOL */
+	at91_set_b_periph(AT91_PIO_PORTC, 22, 0);	/* ERX2 */
+	at91_set_b_periph(AT91_PIO_PORTC, 23, 0);	/* ERX3 */
+	at91_set_b_periph(AT91_PIO_PORTC, 27, 0);	/* ERXCK */
+	at91_set_b_periph(AT91_PIO_PORTC, 20, 0);	/* ETX2 */
+	at91_set_b_periph(AT91_PIO_PORTC, 21, 0);	/* ETX3 */
+	at91_set_b_periph(AT91_PIO_PORTC, 24, 0);	/* ETXER */
 #endif
 }
 #endif
@@ -182,10 +194,12 @@ void at91_macb_hw_init(void)
 #ifdef CONFIG_AT91_CAN
 void at91_can_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA12, 0);	/* CAN_TX */
-	at91_set_A_periph(AT91_PIN_PA13, 1);	/* CAN_RX */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 12, 0);	/* CAN_TX */
+	at91_set_a_periph(AT91_PIO_PORTA, 13, 1);	/* CAN_RX */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91CAP9_ID_CAN);
+	writel(1 << AT91CAP9_ID_CAN, &pmc->pcer);
 }
 #endif
diff --git a/cpu/arm926ejs/at91/at91sam9260_devices.c b/cpu/arm926ejs/at91/at91sam9260_devices.c
index f86cb99..77d49ab 100644
--- a/cpu/arm926ejs/at91/at91sam9260_devices.c
+++ b/cpu/arm926ejs/at91/at91sam9260_devices.c
@@ -30,30 +30,38 @@
 
 void at91_serial0_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PB4, 1);		/* TXD0 */
-	at91_set_A_periph(AT91_PIN_PB5, 0);		/* RXD0 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9260_ID_US0);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 4, 1);		/* TXD0 */
+	at91_set_a_periph(AT91_PIO_PORTB, 5, 0);		/* RXD0 */
+	writel(1 << AT91SAM9260_ID_US0, &pmc->pcer);
 }
 
 void at91_serial1_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PB6, 1);		/* TXD1 */
-	at91_set_A_periph(AT91_PIN_PB7, 0);		/* RXD1 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9260_ID_US1);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 6, 1);		/* TXD1 */
+	at91_set_a_periph(AT91_PIO_PORTB, 7, 0);		/* RXD1 */
+	writel(1 << AT91SAM9260_ID_US1, &pmc->pcer);
 }
 
 void at91_serial2_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PB8, 1);		/* TXD2 */
-	at91_set_A_periph(AT91_PIN_PB9, 0);		/* RXD2 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9260_ID_US2);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 8, 1);		/* TXD2 */
+	at91_set_a_periph(AT91_PIO_PORTB, 9, 0);		/* RXD2 */
+	writel(1 << AT91SAM9260_ID_US2, &pmc->pcer);
 }
 
 void at91_serial3_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PB14, 0);		/* DRXD */
-	at91_set_A_periph(AT91_PIN_PB15, 1);		/* DTXD */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_SYS);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 14, 0);		/* DRXD */
+	at91_set_a_periph(AT91_PIO_PORTB, 15, 1);		/* DTXD */
+	writel(1 << AT91_ID_SYS, &pmc->pcer);
 }
 
 void at91_serial_hw_init(void)
@@ -78,71 +86,75 @@ void at91_serial_hw_init(void)
 #if defined(CONFIG_HAS_DATAFLASH) || defined(CONFIG_ATMEL_SPI)
 void at91_spi0_hw_init(unsigned long cs_mask)
 {
-	at91_set_A_periph(AT91_PIN_PA0, 0);	/* SPI0_MISO */
-	at91_set_A_periph(AT91_PIN_PA1, 0);	/* SPI0_MOSI */
-	at91_set_A_periph(AT91_PIN_PA2, 0);	/* SPI0_SPCK */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 0, 0);	/* SPI0_MISO */
+	at91_set_a_periph(AT91_PIO_PORTA, 1, 0);	/* SPI0_MOSI */
+	at91_set_a_periph(AT91_PIO_PORTA, 2, 0);	/* SPI0_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9260_ID_SPI0);
+	writel(1 << AT91SAM9260_ID_SPI0, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_A_periph(AT91_PIN_PA3, 1);
+		at91_set_a_periph(AT91_PIO_PORTA, 3, 1);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_B_periph(AT91_PIN_PC11, 1);
+		at91_set_b_periph(AT91_PIO_PORTC, 11, 1);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_B_periph(AT91_PIN_PC16, 1);
+		at91_set_b_periph(AT91_PIO_PORTC, 16, 1);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_B_periph(AT91_PIN_PC17, 1);
+		at91_set_b_periph(AT91_PIO_PORTC, 17, 1);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PA3, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 3, 1);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PC11, 1);
+		at91_set_pio_output(AT91_PIO_PORTC, 11, 1);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PC16, 1);
+		at91_set_pio_output(AT91_PIO_PORTC, 16, 1);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PC17, 1);
+		at91_set_pio_output(AT91_PIO_PORTC, 17, 1);
 	}
 }
 
 void at91_spi1_hw_init(unsigned long cs_mask)
 {
-	at91_set_A_periph(AT91_PIN_PB0, 0);	/* SPI1_MISO */
-	at91_set_A_periph(AT91_PIN_PB1, 0);	/* SPI1_MOSI */
-	at91_set_A_periph(AT91_PIN_PB2, 0);	/* SPI1_SPCK */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 0, 0);	/* SPI1_MISO */
+	at91_set_a_periph(AT91_PIO_PORTB, 1, 0);	/* SPI1_MOSI */
+	at91_set_a_periph(AT91_PIO_PORTB, 2, 0);	/* SPI1_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9260_ID_SPI1);
+	writel(1 << AT91SAM9260_ID_SPI1, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_A_periph(AT91_PIN_PB3, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 3, 1);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_B_periph(AT91_PIN_PC5, 1);
+		at91_set_b_periph(AT91_PIO_PORTC, 5, 1);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_B_periph(AT91_PIN_PC4, 1);
+		at91_set_b_periph(AT91_PIO_PORTC, 4, 1);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_gpio_output(AT91_PIN_PC3, 1);
+		at91_set_pio_output(AT91_PIO_PORTC, 3, 1);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PB3, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 3, 1);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PC5, 1);
+		at91_set_pio_output(AT91_PIO_PORTC, 5, 1);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PC4, 1);
+		at91_set_pio_output(AT91_PIO_PORTC, 4, 1);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PC3, 1);
+		at91_set_pio_output(AT91_PIO_PORTC, 3, 1);
 	}
 }
 #endif
@@ -150,35 +162,35 @@ void at91_spi1_hw_init(unsigned long cs_mask)
 #ifdef CONFIG_MACB
 void at91_macb_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA19, 0);	/* ETXCK_EREFCK */
-	at91_set_A_periph(AT91_PIN_PA17, 0);	/* ERXDV */
-	at91_set_A_periph(AT91_PIN_PA14, 0);	/* ERX0 */
-	at91_set_A_periph(AT91_PIN_PA15, 0);	/* ERX1 */
-	at91_set_A_periph(AT91_PIN_PA18, 0);	/* ERXER */
-	at91_set_A_periph(AT91_PIN_PA16, 0);	/* ETXEN */
-	at91_set_A_periph(AT91_PIN_PA12, 0);	/* ETX0 */
-	at91_set_A_periph(AT91_PIN_PA13, 0);	/* ETX1 */
-	at91_set_A_periph(AT91_PIN_PA21, 0);	/* EMDIO */
-	at91_set_A_periph(AT91_PIN_PA20, 0);	/* EMDC */
+	at91_set_a_periph(AT91_PIO_PORTA, 19, 0);	/* ETXCK_EREFCK */
+	at91_set_a_periph(AT91_PIO_PORTA, 17, 0);	/* ERXDV */
+	at91_set_a_periph(AT91_PIO_PORTA, 14, 0);	/* ERX0 */
+	at91_set_a_periph(AT91_PIO_PORTA, 15, 0);	/* ERX1 */
+	at91_set_a_periph(AT91_PIO_PORTA, 18, 0);	/* ERXER */
+	at91_set_a_periph(AT91_PIO_PORTA, 16, 0);	/* ETXEN */
+	at91_set_a_periph(AT91_PIO_PORTA, 12, 0);	/* ETX0 */
+	at91_set_a_periph(AT91_PIO_PORTA, 13, 0);	/* ETX1 */
+	at91_set_a_periph(AT91_PIO_PORTA, 21, 0);	/* EMDIO */
+	at91_set_a_periph(AT91_PIO_PORTA, 20, 0);	/* EMDC */
 
 #ifndef CONFIG_RMII
-	at91_set_B_periph(AT91_PIN_PA28, 0);	/* ECRS */
-	at91_set_B_periph(AT91_PIN_PA29, 0);	/* ECOL */
-	at91_set_B_periph(AT91_PIN_PA25, 0);	/* ERX2 */
-	at91_set_B_periph(AT91_PIN_PA26, 0);	/* ERX3 */
-	at91_set_B_periph(AT91_PIN_PA27, 0);	/* ERXCK */
+	at91_set_b_periph(AT91_PIO_PORTA, 28, 0);	/* ECRS */
+	at91_set_b_periph(AT91_PIO_PORTA, 29, 0);	/* ECOL */
+	at91_set_b_periph(AT91_PIO_PORTA, 25, 0);	/* ERX2 */
+	at91_set_b_periph(AT91_PIO_PORTA, 26, 0);	/* ERX3 */
+	at91_set_b_periph(AT91_PIO_PORTA, 27, 0);	/* ERXCK */
 #if defined(CONFIG_AT91SAM9260EK) || defined(CONFIG_AFEB9260)
 	/*
 	 * use PA10, PA11 for ETX2, ETX3.
 	 * PA23 and PA24 are for TWI EEPROM
 	 */
-	at91_set_B_periph(AT91_PIN_PA10, 0);	/* ETX2 */
-	at91_set_B_periph(AT91_PIN_PA11, 0);	/* ETX3 */
+	at91_set_b_periph(AT91_PIO_PORTA, 10, 0);	/* ETX2 */
+	at91_set_b_periph(AT91_PIO_PORTA, 11, 0);	/* ETX3 */
 #else
-	at91_set_B_periph(AT91_PIN_PA23, 0);	/* ETX2 */
-	at91_set_B_periph(AT91_PIN_PA24, 0);	/* ETX3 */
+	at91_set_b_periph(AT91_PIO_PORTA, 23, 0);	/* ETX2 */
+	at91_set_b_periph(AT91_PIO_PORTA, 24, 0);	/* ETX3 */
 #endif
-	at91_set_B_periph(AT91_PIN_PA22, 0);	/* ETXER */
+	at91_set_b_periph(AT91_PIO_PORTA, 22, 0);	/* ETXER */
 #endif
 }
 #endif
diff --git a/cpu/arm926ejs/at91/at91sam9261_devices.c b/cpu/arm926ejs/at91/at91sam9261_devices.c
index 16d411f..b4353ef 100644
--- a/cpu/arm926ejs/at91/at91sam9261_devices.c
+++ b/cpu/arm926ejs/at91/at91sam9261_devices.c
@@ -30,30 +30,38 @@
 
 void at91_serial0_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PC8, 1);		/* TXD0 */
-	at91_set_A_periph(AT91_PIN_PC9, 0);		/* RXD0 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9261_ID_US0);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTC, 8, 1);		/* TXD0 */
+	at91_set_a_periph(AT91_PIO_PORTC, 9, 0);		/* RXD0 */
+	writel(1 << AT91SAM9261_ID_US0, &pmc->pcer);
 }
 
 void at91_serial1_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PC12, 1);		/* TXD1 */
-	at91_set_A_periph(AT91_PIN_PC13, 0);		/* RXD1 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9261_ID_US1);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTC, 12, 1);		/* TXD1 */
+	at91_set_a_periph(AT91_PIO_PORTC, 13, 0);		/* RXD1 */
+	writel(1 << AT91SAM9261_ID_US1, &pmc->pcer);
 }
 
 void at91_serial2_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PC14, 1);		/* TXD2 */
-	at91_set_A_periph(AT91_PIN_PC15, 0);		/* RXD2 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9261_ID_US2);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTC, 14, 1);		/* TXD2 */
+	at91_set_a_periph(AT91_PIO_PORTC, 15, 0);		/* RXD2 */
+	writel(1 << AT91SAM9261_ID_US2, &pmc->pcer);
 }
 
 void at91_serial3_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA9, 0);		/* DRXD */
-	at91_set_A_periph(AT91_PIN_PA10, 1);		/* DTXD */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_SYS);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 9, 0);		/* DRXD */
+	at91_set_a_periph(AT91_PIO_PORTA, 10, 1);		/* DTXD */
+	writel(1 << AT91_ID_SYS, &pmc->pcer);
 }
 
 void at91_serial_hw_init(void)
@@ -78,71 +86,75 @@ void at91_serial_hw_init(void)
 #ifdef CONFIG_HAS_DATAFLASH
 void at91_spi0_hw_init(unsigned long cs_mask)
 {
-	at91_set_A_periph(AT91_PIN_PA0, 0);	/* SPI0_MISO */
-	at91_set_A_periph(AT91_PIN_PA1, 0);	/* SPI0_MOSI */
-	at91_set_A_periph(AT91_PIN_PA2, 0);	/* SPI0_SPCK */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 0, 0);	/* SPI0_MISO */
+	at91_set_a_periph(AT91_PIO_PORTA, 1, 0);	/* SPI0_MOSI */
+	at91_set_a_periph(AT91_PIO_PORTA, 2, 0);	/* SPI0_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9261_ID_SPI0);
+	writel(1 << AT91SAM9261_ID_SPI0, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_A_periph(AT91_PIN_PA3, 1);
+		at91_set_a_periph(AT91_PIO_PORTA, 3, 1);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_A_periph(AT91_PIN_PA4, 1);
+		at91_set_a_periph(AT91_PIO_PORTA, 4, 1);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_A_periph(AT91_PIN_PA5, 1);
+		at91_set_a_periph(AT91_PIO_PORTA, 5, 1);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_A_periph(AT91_PIN_PA6, 1);
+		at91_set_a_periph(AT91_PIO_PORTA, 6, 1);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PA3, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 3, 1);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PA4, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 4, 1);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PA5, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 5, 1);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PA6, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 6, 1);
 	}
 }
 
 void at91_spi1_hw_init(unsigned long cs_mask)
 {
-	at91_set_A_periph(AT91_PIN_PB30, 0);	/* SPI1_MISO */
-	at91_set_A_periph(AT91_PIN_PB31, 0);	/* SPI1_MOSI */
-	at91_set_A_periph(AT91_PIN_PB29, 0);	/* SPI1_SPCK */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 30, 0);	/* SPI1_MISO */
+	at91_set_a_periph(AT91_PIO_PORTB, 31, 0);	/* SPI1_MOSI */
+	at91_set_a_periph(AT91_PIO_PORTB, 29, 0);	/* SPI1_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9261_ID_SPI1);
+	writel(1 << AT91SAM9261_ID_SPI1, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_A_periph(AT91_PIN_PB28, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 28, 1);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_B_periph(AT91_PIN_PA24, 1);
+		at91_set_b_periph(AT91_PIO_PORTA, 24, 1);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_B_periph(AT91_PIN_PA25, 1);
+		at91_set_b_periph(AT91_PIO_PORTA, 25, 1);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_A_periph(AT91_PIN_PA26, 1);
+		at91_set_a_periph(AT91_PIO_PORTA, 26, 1);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PB28, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 28, 1);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PA24, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 24, 1);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PA25, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 25, 1);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PA26, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 26, 1);
 	}
 }
 #endif
diff --git a/cpu/arm926ejs/at91/at91sam9263_devices.c b/cpu/arm926ejs/at91/at91sam9263_devices.c
index f72efdf..deda3e5 100644
--- a/cpu/arm926ejs/at91/at91sam9263_devices.c
+++ b/cpu/arm926ejs/at91/at91sam9263_devices.c
@@ -27,37 +27,46 @@
  */
 
 #include <common.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/io.h>
 #include <asm/arch/at91_common.h>
 #include <asm/arch/at91_pmc.h>
-#include <asm/arch/gpio.h>
-#include <asm/arch/io.h>
+#include <asm/arch/at91_pio.h>
 
 void at91_serial0_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA26, 1);		/* TXD0 */
-	at91_set_A_periph(AT91_PIN_PA27, 0);		/* RXD0 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9263_ID_US0);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 26, 1);		/* TXD0 */
+	at91_set_a_periph(AT91_PIO_PORTA, 27, 0);		/* RXD0 */
+	writel(1 << AT91SAM9263_ID_US0, &pmc->pcer);
 }
 
 void at91_serial1_hw_init(void)
 {
-	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 << AT91SAM9263_ID_US1);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTD, 0, 1);		/* TXD1 */
+	at91_set_a_periph(AT91_PIO_PORTD, 1, 0);		/* RXD1 */
+	writel(1 << AT91SAM9263_ID_US1, &pmc->pcer);
 }
 
 void at91_serial2_hw_init(void)
 {
-	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 << AT91SAM9263_ID_US2);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTD, 2, 1);		/* TXD2 */
+	at91_set_a_periph(AT91_PIO_PORTD, 3, 0);		/* RXD2 */
+	writel(1 << AT91SAM9263_ID_US2, &pmc->pcer);
 }
 
 void at91_serial3_hw_init(void)
 {
-	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);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTC, 30, 0);		/* DRXD */
+	at91_set_a_periph(AT91_PIO_PORTC, 31, 1);		/* DTXD */
+	writel(1 << AT91_ID_SYS, &pmc->pcer);
 }
 
 void at91_serial_hw_init(void)
@@ -82,71 +91,75 @@ void at91_serial_hw_init(void)
 #ifdef CONFIG_HAS_DATAFLASH
 void at91_spi0_hw_init(unsigned long cs_mask)
 {
-	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 */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_b_periph(AT91_PIO_PORTA, 0, 0);	/* SPI0_MISO */
+	at91_set_b_periph(AT91_PIO_PORTA, 1, 0);	/* SPI0_MOSI */
+	at91_set_b_periph(AT91_PIO_PORTA, 2, 0);	/* SPI0_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9263_ID_SPI0);
+	writel(1 << AT91SAM9263_ID_SPI0, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_B_periph(AT91_PIN_PA5, 1);
+		at91_set_b_periph(AT91_PIO_PORTA, 5, 1);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_B_periph(AT91_PIN_PA3, 1);
+		at91_set_b_periph(AT91_PIO_PORTA, 3, 1);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_B_periph(AT91_PIN_PA4, 1);
+		at91_set_b_periph(AT91_PIO_PORTA, 4, 1);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_B_periph(AT91_PIN_PB11, 1);
+		at91_set_b_periph(AT91_PIO_PORTB, 11, 1);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PA5, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 5, 1);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PA3, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 3, 1);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PA4, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 4, 1);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PB11, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 11, 1);
 	}
 }
 
 void at91_spi1_hw_init(unsigned long cs_mask)
 {
-	at91_set_A_periph(AT91_PIN_PB12, 0);	/* SPI1_MISO */
-	at91_set_A_periph(AT91_PIN_PB13, 0);	/* SPI1_MOSI */
-	at91_set_A_periph(AT91_PIN_PB14, 0);	/* SPI1_SPCK */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 12, 0);	/* SPI1_MISO */
+	at91_set_a_periph(AT91_PIO_PORTB, 13, 0);	/* SPI1_MOSI */
+	at91_set_a_periph(AT91_PIO_PORTB, 14, 0);	/* SPI1_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9263_ID_SPI1);
+	writel(1 << AT91SAM9263_ID_SPI1, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_A_periph(AT91_PIN_PB15, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 15, 1);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_A_periph(AT91_PIN_PB16, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 16, 1);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_A_periph(AT91_PIN_PB17, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 17, 1);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_A_periph(AT91_PIN_PB18, 1);
+		at91_set_a_periph(AT91_PIO_PORTB, 18, 1);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PB15, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 15, 1);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PB16, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 16, 1);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PB17, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 17, 1);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PB18, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 18, 1);
 	}
 }
 #endif
@@ -154,26 +167,26 @@ void at91_spi1_hw_init(unsigned long cs_mask)
 #ifdef CONFIG_MACB
 void at91_macb_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PE21, 0);	/* ETXCK_EREFCK */
-	at91_set_B_periph(AT91_PIN_PC25, 0);	/* ERXDV */
-	at91_set_A_periph(AT91_PIN_PE25, 0);	/* ERX0 */
-	at91_set_A_periph(AT91_PIN_PE26, 0);	/* ERX1 */
-	at91_set_A_periph(AT91_PIN_PE27, 0);	/* ERXER */
-	at91_set_A_periph(AT91_PIN_PE28, 0);	/* ETXEN */
-	at91_set_A_periph(AT91_PIN_PE23, 0);	/* ETX0 */
-	at91_set_A_periph(AT91_PIN_PE24, 0);	/* ETX1 */
-	at91_set_A_periph(AT91_PIN_PE30, 0);	/* EMDIO */
-	at91_set_A_periph(AT91_PIN_PE29, 0);	/* EMDC */
+	at91_set_a_periph(AT91_PIO_PORTE, 21, 0);	/* ETXCK_EREFCK */
+	at91_set_b_periph(AT91_PIO_PORTC, 25, 0);	/* ERXDV */
+	at91_set_a_periph(AT91_PIO_PORTE, 25, 0);	/* ERX0 */
+	at91_set_a_periph(AT91_PIO_PORTE, 26, 0);	/* ERX1 */
+	at91_set_a_periph(AT91_PIO_PORTE, 27, 0);	/* ERXER */
+	at91_set_a_periph(AT91_PIO_PORTE, 28, 0);	/* ETXEN */
+	at91_set_a_periph(AT91_PIO_PORTE, 23, 0);	/* ETX0 */
+	at91_set_a_periph(AT91_PIO_PORTE, 24, 0);	/* ETX1 */
+	at91_set_a_periph(AT91_PIO_PORTE, 30, 0);	/* EMDIO */
+	at91_set_a_periph(AT91_PIO_PORTE, 29, 0);	/* EMDC */
 
 #ifndef CONFIG_RMII
-	at91_set_A_periph(AT91_PIN_PE22, 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 */
+	at91_set_a_periph(AT91_PIO_PORTE, 22, 0);	/* ECRS */
+	at91_set_b_periph(AT91_PIO_PORTC, 26, 0);	/* ECOL */
+	at91_set_b_periph(AT91_PIO_PORTC, 22, 0);	/* ERX2 */
+	at91_set_b_periph(AT91_PIO_PORTC, 23, 0);	/* ERX3 */
+	at91_set_b_periph(AT91_PIO_PORTC, 27, 0);	/* ERXCK */
+	at91_set_b_periph(AT91_PIO_PORTC, 20, 0);	/* ETX2 */
+	at91_set_b_periph(AT91_PIO_PORTC, 21, 0);	/* ETX3 */
+	at91_set_b_periph(AT91_PIO_PORTC, 24, 0);	/* ETXER */
 #endif
 }
 #endif
@@ -182,18 +195,20 @@ void at91_macb_hw_init(void)
 void at91_uhp_hw_init(void)
 {
 	/* Enable VBus on UHP ports */
-	at91_set_gpio_output(AT91_PIN_PA21, 0);
-	at91_set_gpio_output(AT91_PIN_PA24, 0);
+	at91_set_pio_output(AT91_PIO_PORTA, 21, 0);
+	at91_set_pio_output(AT91_PIO_PORTA, 24, 0);
 }
 #endif
 
 #ifdef CONFIG_AT91_CAN
 void at91_can_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA13, 0);	/* CAN_TX */
-	at91_set_A_periph(AT91_PIN_PA14, 1);	/* CAN_RX */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 13, 0);	/* CAN_TX */
+	at91_set_a_periph(AT91_PIO_PORTA, 14, 1);	/* CAN_RX */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9263_ID_CAN);
+	writel(1 << AT91SAM9263_ID_CAN, &pmc->pcer);
 }
 #endif
diff --git a/cpu/arm926ejs/at91/at91sam9m10g45_devices.c b/cpu/arm926ejs/at91/at91sam9m10g45_devices.c
index 98d90f2..4ad9b1f 100644
--- a/cpu/arm926ejs/at91/at91sam9m10g45_devices.c
+++ b/cpu/arm926ejs/at91/at91sam9m10g45_devices.c
@@ -30,30 +30,38 @@
 
 void at91_serial0_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PB19, 1);	/* TXD0 */
-	at91_set_A_periph(AT91_PIN_PB18, 0);	/* RXD0 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9G45_ID_US0);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 19, 1);	/* TXD0 */
+	at91_set_a_periph(AT91_PIO_PORTB, 18, 0);	/* RXD0 */
+	writel(1 << AT91SAM9G45_ID_US0, &pmc->pcer);
 }
 
 void at91_serial1_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PB4, 1);		/* TXD1 */
-	at91_set_A_periph(AT91_PIN_PB5, 0);		/* RXD1 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9G45_ID_US1);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 4, 1);		/* TXD1 */
+	at91_set_a_periph(AT91_PIO_PORTB, 5, 0);		/* RXD1 */
+	writel(1 << AT91SAM9G45_ID_US1, &pmc->pcer);
 }
 
 void at91_serial2_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PD6, 1);		/* TXD2 */
-	at91_set_A_periph(AT91_PIN_PD7, 0);		/* RXD2 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9G45_ID_US2);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTD, 6, 1);		/* TXD2 */
+	at91_set_a_periph(AT91_PIO_PORTD, 7, 0);		/* RXD2 */
+	writel(1 << AT91SAM9G45_ID_US2, &pmc->pcer);
 }
 
 void at91_serial3_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PB12, 0);	/* DRXD */
-	at91_set_A_periph(AT91_PIN_PB13, 1);	/* DTXD */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_SYS);;
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 12, 0);	/* DRXD */
+	at91_set_a_periph(AT91_PIO_PORTB, 13, 1);	/* DTXD */
+	writel(1 << AT91_ID_SYS, &pmc->pcer);
 }
 
 void at91_serial_hw_init(void)
@@ -78,71 +86,75 @@ void at91_serial_hw_init(void)
 #ifdef CONFIG_ATMEL_SPI
 void at91_spi0_hw_init(unsigned long cs_mask)
 {
-	at91_set_A_periph(AT91_PIN_PB0, 0);	/* SPI0_MISO */
-	at91_set_A_periph(AT91_PIN_PB1, 0);	/* SPI0_MOSI */
-	at91_set_A_periph(AT91_PIN_PB2, 0);	/* SPI0_SPCK */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 0, 0);	/* SPI0_MISO */
+	at91_set_a_periph(AT91_PIO_PORTB, 1, 0);	/* SPI0_MOSI */
+	at91_set_a_periph(AT91_PIO_PORTB, 2, 0);	/* SPI0_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9G45_ID_SPI0);
+	writel(1 << AT91SAM9G45_ID_SPI0, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_A_periph(AT91_PIN_PB3, 0);
+		at91_set_a_periph(AT91_PIO_PORTB, 3, 0);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_B_periph(AT91_PIN_PB18, 0);
+		at91_set_b_periph(AT91_PIO_PORTB, 18, 0);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_B_periph(AT91_PIN_PB19, 0);
+		at91_set_b_periph(AT91_PIO_PORTB, 19, 0);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_B_periph(AT91_PIN_PD27, 0);
+		at91_set_b_periph(AT91_PIO_PORTD, 27, 0);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PB3, 0);
+		at91_set_pio_output(AT91_PIO_PORTB, 3, 0);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PB18, 0);
+		at91_set_pio_output(AT91_PIO_PORTB, 18, 0);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PB19, 0);
+		at91_set_pio_output(AT91_PIO_PORTB, 19, 0);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PD27, 0);
+		at91_set_pio_output(AT91_PIO_PORTD, 27, 0);
 	}
 }
 
 void at91_spi1_hw_init(unsigned long cs_mask)
 {
-	at91_set_A_periph(AT91_PIN_PB14, 0);	/* SPI1_MISO */
-	at91_set_A_periph(AT91_PIN_PB15, 0);	/* SPI1_MOSI */
-	at91_set_A_periph(AT91_PIN_PB16, 0);	/* SPI1_SPCK */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTB, 14, 0);	/* SPI1_MISO */
+	at91_set_a_periph(AT91_PIO_PORTB, 15, 0);	/* SPI1_MOSI */
+	at91_set_a_periph(AT91_PIO_PORTB, 16, 0);	/* SPI1_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9G45_ID_SPI1);
+	writel(1 << AT91SAM9G45_ID_SPI1, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_A_periph(AT91_PIN_PB17, 0);
+		at91_set_a_periph(AT91_PIO_PORTB, 17, 0);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_B_periph(AT91_PIN_PD28, 0);
+		at91_set_b_periph(AT91_PIO_PORTD, 28, 0);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_A_periph(AT91_PIN_PD18, 0);
+		at91_set_a_periph(AT91_PIO_PORTD, 18, 0);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_A_periph(AT91_PIN_PD19, 0);
+		at91_set_a_periph(AT91_PIO_PORTD, 19, 0);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PB17, 0);
+		at91_set_pio_output(AT91_PIO_PORTB, 17, 0);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PD28, 0);
+		at91_set_pio_output(AT91_PIO_PORTD, 28, 0);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PD18, 0);
+		at91_set_pio_output(AT91_PIO_PORTD, 18, 0);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PD19, 0);
+		at91_set_pio_output(AT91_PIO_PORTD, 19, 0);
 	}
 
 }
@@ -151,25 +163,25 @@ void at91_spi1_hw_init(unsigned long cs_mask)
 #ifdef CONFIG_MACB
 void at91_macb_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA17, 0);	/* ETXCK_EREFCK */
-	at91_set_A_periph(AT91_PIN_PA15, 0);	/* ERXDV */
-	at91_set_A_periph(AT91_PIN_PA12, 0);	/* ERX0 */
-	at91_set_A_periph(AT91_PIN_PA13, 0);	/* ERX1 */
-	at91_set_A_periph(AT91_PIN_PA16, 0);	/* ERXER */
-	at91_set_A_periph(AT91_PIN_PA14, 0);	/* ETXEN */
-	at91_set_A_periph(AT91_PIN_PA10, 0);	/* ETX0 */
-	at91_set_A_periph(AT91_PIN_PA11, 0);	/* ETX1 */
-	at91_set_A_periph(AT91_PIN_PA19, 0);	/* EMDIO */
-	at91_set_A_periph(AT91_PIN_PA18, 0);	/* EMDC */
+	at91_set_a_periph(AT91_PIO_PORTA, 17, 0);	/* ETXCK_EREFCK */
+	at91_set_a_periph(AT91_PIO_PORTA, 15, 0);	/* ERXDV */
+	at91_set_a_periph(AT91_PIO_PORTA, 12, 0);	/* ERX0 */
+	at91_set_a_periph(AT91_PIO_PORTA, 13, 0);	/* ERX1 */
+	at91_set_a_periph(AT91_PIO_PORTA, 16, 0);	/* ERXER */
+	at91_set_a_periph(AT91_PIO_PORTA, 14, 0);	/* ETXEN */
+	at91_set_a_periph(AT91_PIO_PORTA, 10, 0);	/* ETX0 */
+	at91_set_a_periph(AT91_PIO_PORTA, 11, 0);	/* ETX1 */
+	at91_set_a_periph(AT91_PIO_PORTA, 19, 0);	/* EMDIO */
+	at91_set_a_periph(AT91_PIO_PORTA, 18, 0);	/* EMDC */
 #ifndef CONFIG_RMII
-	at91_set_B_periph(AT91_PIN_PA29, 0);	/* ECRS */
-	at91_set_B_periph(AT91_PIN_PA30, 0);	/* ECOL */
-	at91_set_B_periph(AT91_PIN_PA8,  0);	/* ERX2 */
-	at91_set_B_periph(AT91_PIN_PA9,  0);	/* ERX3 */
-	at91_set_B_periph(AT91_PIN_PA28, 0);	/* ERXCK */
-	at91_set_B_periph(AT91_PIN_PA6,  0);	/* ETX2 */
-	at91_set_B_periph(AT91_PIN_PA7,  0);	/* ETX3 */
-	at91_set_B_periph(AT91_PIN_PA27, 0);	/* ETXER */
+	at91_set_b_periph(AT91_PIO_PORTA, 29, 0);	/* ECRS */
+	at91_set_b_periph(AT91_PIO_PORTA, 30, 0);	/* ECOL */
+	at91_set_b_periph(AT91_PIO_PORTA, 8,  0);	/* ERX2 */
+	at91_set_b_periph(AT91_PIO_PORTA, 9,  0);	/* ERX3 */
+	at91_set_b_periph(AT91_PIO_PORTA, 28, 0);	/* ERXCK */
+	at91_set_b_periph(AT91_PIO_PORTA, 6,  0);	/* ETX2 */
+	at91_set_b_periph(AT91_PIO_PORTA, 7,  0);	/* ETX3 */
+	at91_set_b_periph(AT91_PIO_PORTA, 27, 0);	/* ETXER */
 #endif
 }
 #endif
diff --git a/cpu/arm926ejs/at91/at91sam9rl_devices.c b/cpu/arm926ejs/at91/at91sam9rl_devices.c
index ebed193..4f570f4 100644
--- a/cpu/arm926ejs/at91/at91sam9rl_devices.c
+++ b/cpu/arm926ejs/at91/at91sam9rl_devices.c
@@ -30,30 +30,38 @@
 
 void at91_serial0_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA6, 1);		/* TXD0 */
-	at91_set_A_periph(AT91_PIN_PA7, 0);		/* RXD0 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9RL_ID_US0);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 6, 1);		/* TXD0 */
+	at91_set_a_periph(AT91_PIO_PORTA, 7, 0);		/* RXD0 */
+	writel(1 << AT91SAM9RL_ID_US0, &pmc->pcer);
 }
 
 void at91_serial1_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA11, 1);		/* TXD1 */
-	at91_set_A_periph(AT91_PIN_PA12, 0);		/* RXD1 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9RL_ID_US1);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 11, 1);		/* TXD1 */
+	at91_set_a_periph(AT91_PIO_PORTA, 12, 0);		/* RXD1 */
+	writel(1 << AT91SAM9RL_ID_US1, &pmc->pcer);
 }
 
 void at91_serial2_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA13, 1);		/* TXD2 */
-	at91_set_A_periph(AT91_PIN_PA14, 0);		/* RXD2 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9RL_ID_US2);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 13, 1);		/* TXD2 */
+	at91_set_a_periph(AT91_PIO_PORTA, 14, 0);		/* RXD2 */
+	writel(1 << AT91SAM9RL_ID_US2, &pmc->pcer);
 }
 
 void at91_serial3_hw_init(void)
 {
-	at91_set_A_periph(AT91_PIN_PA21, 0);		/* DRXD */
-	at91_set_A_periph(AT91_PIN_PA22, 1);		/* DTXD */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_SYS);
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 21, 0);		/* DRXD */
+	at91_set_a_periph(AT91_PIO_PORTA, 22, 1);		/* DTXD */
+	writel(1 << AT91_ID_SYS, &pmc->pcer);
 }
 
 void at91_serial_hw_init(void)
@@ -78,36 +86,38 @@ void at91_serial_hw_init(void)
 #ifdef CONFIG_HAS_DATAFLASH
 void at91_spi0_hw_init(unsigned long cs_mask)
 {
-	at91_set_A_periph(AT91_PIN_PA25, 0);	/* SPI0_MISO */
-	at91_set_A_periph(AT91_PIN_PA26, 0);	/* SPI0_MOSI */
-	at91_set_A_periph(AT91_PIN_PA27, 0);	/* SPI0_SPCK */
+	at91_pmc_t	*pmc	= (at91_pmc_t *) AT91_PMC_BASE;
+
+	at91_set_a_periph(AT91_PIO_PORTA, 25, 0);	/* SPI0_MISO */
+	at91_set_a_periph(AT91_PIO_PORTA, 26, 0);	/* SPI0_MOSI */
+	at91_set_a_periph(AT91_PIO_PORTA, 27, 0);	/* SPI0_SPCK */
 
 	/* Enable clock */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9RL_ID_SPI);
+	writel(1 << AT91SAM9RL_ID_SPI, &pmc->pcer);
 
 	if (cs_mask & (1 << 0)) {
-		at91_set_A_periph(AT91_PIN_PA28, 1);
+		at91_set_a_periph(AT91_PIO_PORTA, 28, 1);
 	}
 	if (cs_mask & (1 << 1)) {
-		at91_set_B_periph(AT91_PIN_PB7, 1);
+		at91_set_b_periph(AT91_PIO_PORTB, 7, 1);
 	}
 	if (cs_mask & (1 << 2)) {
-		at91_set_A_periph(AT91_PIN_PD8, 1);
+		at91_set_a_periph(AT91_PIO_PORTD, 8, 1);
 	}
 	if (cs_mask & (1 << 3)) {
-		at91_set_B_periph(AT91_PIN_PD9, 1);
+		at91_set_b_periph(AT91_PIO_PORTD, 9, 1);
 	}
 	if (cs_mask & (1 << 4)) {
-		at91_set_gpio_output(AT91_PIN_PA28, 1);
+		at91_set_pio_output(AT91_PIO_PORTA, 28, 1);
 	}
 	if (cs_mask & (1 << 5)) {
-		at91_set_gpio_output(AT91_PIN_PB7, 1);
+		at91_set_pio_output(AT91_PIO_PORTB, 7, 1);
 	}
 	if (cs_mask & (1 << 6)) {
-		at91_set_gpio_output(AT91_PIN_PD8, 1);
+		at91_set_pio_output(AT91_PIO_PORTD, 8, 1);
 	}
 	if (cs_mask & (1 << 7)) {
-		at91_set_gpio_output(AT91_PIN_PD9, 1);
+		at91_set_pio_output(AT91_PIO_PORTD, 9, 1);
 	}
 }
 #endif
diff --git a/cpu/arm926ejs/at91/clock.c b/cpu/arm926ejs/at91/clock.c
index 574f488..b06d760 100644
--- a/cpu/arm926ejs/at91/clock.c
+++ b/cpu/arm926ejs/at91/clock.c
@@ -13,9 +13,9 @@
 
 #include <config.h>
 #include <asm/arch/hardware.h>
+#include <asm/arch/io.h>
 #include <asm/arch/at91_pmc.h>
 #include <asm/arch/clk.h>
-#include <asm/arch/io.h>
 
 static unsigned long cpu_clk_rate_hz;
 static unsigned long main_clk_rate_hz;
@@ -57,14 +57,14 @@ u32 get_pllb_init(void)
 static unsigned long at91_css_to_rate(unsigned long css)
 {
 	switch (css) {
-		case AT91_PMC_CSS_SLOW:
-			return AT91_SLOW_CLOCK;
-		case AT91_PMC_CSS_MAIN:
-			return main_clk_rate_hz;
-		case AT91_PMC_CSS_PLLA:
-			return plla_rate_hz;
-		case AT91_PMC_CSS_PLLB:
-			return pllb_rate_hz;
+	case AT91_PMC_MCKR_CSS_SLOW:
+		return AT91_SLOW_CLOCK;
+	case AT91_PMC_MCKR_CSS_MAIN:
+		return main_clk_rate_hz;
+	case AT91_PMC_MCKR_CSS_PLLA:
+		return plla_rate_hz;
+	case AT91_PMC_MCKR_CSS_PLLB:
+		return pllb_rate_hz;
 	}
 
 	return 0;
@@ -146,7 +146,8 @@ static u32 at91_pll_rate(u32 freq, u32 reg)
 int at91_clock_init(unsigned long main_clock)
 {
 	unsigned freq, mckr;
-#ifndef AT91_MAIN_CLOCK
+	at91_pmc_t *pmc = (at91_pmc_t *) AT91_PMC_BASE;
+#ifndef CONFIG_SYS_AT91_MAIN_CLOCK
 	unsigned tmp;
 	/*
 	 * When the bootloader initialized the main oscillator correctly,
@@ -156,15 +157,16 @@ int at91_clock_init(unsigned long main_clock)
 	 */
 	if (!main_clock) {
 		do {
-			tmp = at91_sys_read(AT91_CKGR_MCFR);
-		} while (!(tmp & AT91_PMC_MAINRDY));
-		main_clock = (tmp & AT91_PMC_MAINF) * (AT91_SLOW_CLOCK / 16);
+			tmp = readl(&pmc->mcfr);
+		} while (!(tmp & AT91_PMC_MCFR_MAINRDY));
+		tmp &= AT91_PMC_MCFR_MAINF_MASK;
+		main_clock = tmp * (AT91_SLOW_CLOCK / 16);
 	}
 #endif
 	main_clk_rate_hz = main_clock;
 
 	/* report if PLLA is more than mildly overclocked */
-	plla_rate_hz = at91_pll_rate(main_clock, at91_sys_read(AT91_CKGR_PLLAR));
+	plla_rate_hz = at91_pll_rate(main_clock, readl(&pmc->pllar));
 
 #ifdef CONFIG_USB_ATMEL
 	/*
@@ -174,7 +176,7 @@ int at91_clock_init(unsigned long main_clock)
 	 * REVISIT:  assumes MCK doesn't derive from PLLB!
 	 */
 	at91_pllb_usb_init = at91_pll_calc(main_clock, 48000000 * 2) |
-			     AT91_PMC_USB96M;
+			     AT91_PMC_PLLBR_USBDIV_2;
 	pllb_rate_hz = at91_pll_rate(main_clock, at91_pllb_usb_init);
 #endif
 
@@ -182,28 +184,32 @@ int at91_clock_init(unsigned long main_clock)
 	 * MCK and CPU derive from one of those primary clocks.
 	 * For now, assume this parentage won't change.
 	 */
-	mckr = at91_sys_read(AT91_PMC_MCKR);
+	mckr = readl(&pmc->mckr);
 #if defined(CONFIG_AT91SAM9G45) || defined(CONFIG_AT91SAM9M10G45)
 	/* plla divisor by 2 */
 	plla_rate_hz /= (1 << ((mckr & 1 << 12) >> 12));
 #endif
-	freq = mck_rate_hz = at91_css_to_rate(mckr & AT91_PMC_CSS);
+	mck_rate_hz = at91_css_to_rate(mckr & AT91_PMC_MCKR_CSS_MASK);
+	freq = mck_rate_hz;
 
-	freq /= (1 << ((mckr & AT91_PMC_PRES) >> 2));			/* prescale */
+	freq /= (1 << ((mckr & AT91_PMC_MCKR_PRES_MASK) >> 2));	/* prescale */
 #if defined(CONFIG_AT91RM9200)
-	mck_rate_hz = freq / (1 + ((mckr & AT91_PMC_MDIV) >> 8));	/* mdiv */
+	/* mdiv */
+	mck_rate_hz = freq / (1 + ((mckr & AT91_PMC_MCKR_MDIV_MASK) >> 8));
 #elif defined(CONFIG_AT91SAM9G20)
-	mck_rate_hz = (mckr & AT91_PMC_MDIV) ?
-		freq / ((mckr & AT91_PMC_MDIV) >> 7) : freq;		/* mdiv ; (x >> 7) = ((x >> 8) * 2) */
-	if (mckr & AT91_PMC_PDIV)
-		freq /= 2;						/* processor clock division */
+	/* mdiv ; (x >> 7) = ((x >> 8) * 2) */
+	mck_rate_hz = (mckr & AT91_PMC_MCKR_MDIV_MASK) ?
+		freq / ((mckr & AT91_PMC_MCKR_MDIV_MASK) >> 7) : freq;
+	if (mckr & AT91_PMC_MCKR_MDIV_MASK)
+		freq /= 2;			/* processor clock division */
 #elif defined(CONFIG_AT91SAM9G45) || defined(CONFIG_AT91SAM9M10G45)
-	mck_rate_hz = (mckr & AT91_PMC_MDIV) == AT91SAM9_PMC_MDIV_3 ?
-		freq / 3 : freq / (1 << ((mckr & AT91_PMC_MDIV) >> 8));	/* mdiv */
+	mck_rate_hz = (mckr & AT91_PMC_MCKR_MDIV_MASK) == AT91SAM9_PMC_MDIV_3
+		? freq / 3
+		: freq / (1 << ((mckr & AT91_PMC_MCKR_MDIV_MASK) >> 8));
 #else
-	mck_rate_hz = freq / (1 << ((mckr & AT91_PMC_MDIV) >> 8));	/* mdiv */
+	mck_rate_hz = freq / (1 << ((mckr & AT91_PMC_MCKR_MDIV_MASK) >> 8));
 #endif
 	cpu_clk_rate_hz = freq;
 
-		return 0;
+	return 0;
 }
diff --git a/cpu/arm926ejs/at91/cpu.c b/cpu/arm926ejs/at91/cpu.c
index f2f7b62..141a7d1 100644
--- a/cpu/arm926ejs/at91/cpu.c
+++ b/cpu/arm926ejs/at91/cpu.c
@@ -22,18 +22,29 @@
  */
 
 #include <common.h>
+#ifdef CONFIG_AT91_LEGACY
+#warning Your board is using legacy SoC access. Please update!
+#endif
+
 #include <asm/arch/hardware.h>
 #include <asm/arch/at91_pmc.h>
 #include <asm/arch/clk.h>
 #include <asm/arch/io.h>
 
-#ifndef AT91_MAIN_CLOCK
-#define AT91_MAIN_CLOCK 0
+#ifndef CONFIG_SYS_AT91_MAIN_CLOCK
+#define CONFIG_SYS_AT91_MAIN_CLOCK 0
 #endif
 
+/*
+ * The at91sam9260 has 4 GPBR (0-3), we'll use the last one, nr 3,
+ * to keep track of the bootcount.
+ */
+#define AT91_GPBR_BOOTCOUNT_REGISTER 3
+#define AT91_BOOTCOUNT_ADDRESS (AT91_GPBR + 4*AT91_GPBR_BOOTCOUNT_REGISTER)
+
 int arch_cpu_init(void)
 {
-	return at91_clock_init(AT91_MAIN_CLOCK);
+	return at91_clock_init(CONFIG_SYS_AT91_MAIN_CLOCK);
 }
 
 #if defined(CONFIG_DISPLAY_CPUINFO)
@@ -41,7 +52,7 @@ int print_cpuinfo(void)
 {
 	char buf[32];
 
-	printf("CPU: %s\n", AT91_CPU_NAME);
+	printf("CPU: %s\n", CONFIG_SYS_AT91_CPU_NAME);
 	printf("Crystal frequency: %8s MHz\n",
 					strmhz(buf, get_main_clk_rate()));
 	printf("CPU clock        : %8s MHz\n",
@@ -52,3 +63,30 @@ int print_cpuinfo(void)
 	return 0;
 }
 #endif
+
+#ifdef CONFIG_BOOTCOUNT_LIMIT
+/*
+ * Just as the mpc5xxx, we combine the BOOTCOUNT_MAGIC and boocount
+ * in one 32-bit register. This is done, as the AT91SAM9260 only has
+ * 4 GPBR.
+ */
+void bootcount_store (ulong a)
+{
+	volatile ulong *save_addr =
+		(volatile ulong *)(AT91_BASE_SYS + AT91_BOOTCOUNT_ADDRESS);
+
+	*save_addr = (BOOTCOUNT_MAGIC & 0xffff0000) | (a & 0x0000ffff);
+}
+
+ulong bootcount_load (void)
+{
+	volatile ulong *save_addr =
+		(volatile ulong *)(AT91_BASE_SYS + AT91_BOOTCOUNT_ADDRESS);
+
+	if ((*save_addr & 0xffff0000) != (BOOTCOUNT_MAGIC & 0xffff0000))
+		return 0;
+	else
+		return (*save_addr & 0x0000ffff);
+}
+
+#endif /* CONFIG_BOOTCOUNT_LIMIT */
diff --git a/cpu/arm926ejs/at91/led.c b/cpu/arm926ejs/at91/led.c
index be68f59..0a315c4 100644
--- a/cpu/arm926ejs/at91/led.c
+++ b/cpu/arm926ejs/at91/led.c
@@ -24,6 +24,7 @@
 
 #include <common.h>
 #include <asm/arch/at91_pmc.h>
+#include <asm/arch/at91_pio.h>
 #include <asm/arch/gpio.h>
 #include <asm/arch/io.h>
 
diff --git a/cpu/arm926ejs/at91/lowlevel_init.S b/cpu/arm926ejs/at91/lowlevel_init.S
index f11ebc6..559c35c 100644
--- a/cpu/arm926ejs/at91/lowlevel_init.S
+++ b/cpu/arm926ejs/at91/lowlevel_init.S
@@ -27,15 +27,20 @@
  */
 
 #include <config.h>
-#include <version.h>
 #include <asm/arch/hardware.h>
 #include <asm/arch/at91_pmc.h>
-#include <asm/arch/at91_pio.h>
-#include <asm/arch/at91_rstc.h>
 #include <asm/arch/at91_wdt.h>
-#include <asm/arch/at91sam9_matrix.h>
+#include <asm/arch/at91_pio.h>
+#include <asm/arch/at91_matrix.h>
 #include <asm/arch/at91sam9_sdramc.h>
 #include <asm/arch/at91sam9_smc.h>
+#include <asm/arch/at91_rstc.h>
+#ifdef CONFIG_AT91_LEGACY
+#include <asm/arch/at91sam9_matrix.h>
+#endif
+#ifndef CONFIG_SYS_MATRIX_EBICSA_VAL
+#define CONFIG_SYS_MATRIX_EBICSA_VAL CONFIG_SYS_MATRIX_EBI0CSA_VAL
+#endif
 
 _TEXT_BASE:
 	.word	TEXT_BASE
@@ -75,7 +80,7 @@ POS1:
  * - Check if the PLL is already initialized
  * ----------------------------------------------------------------------------
  */
-	ldr	r1, =(AT91_BASE_SYS + AT91_PMC_MCKR)
+	ldr	r1, =(AT91_ASM_PMC_MCKR)
 	ldr	r0, [r1]
 	and	r0, r0, #3
 	cmp	r0, #0
@@ -85,18 +90,18 @@ POS1:
  * - Enable the Main Oscillator
  * ---------------------------------------------------------------------------
  */
-	ldr	r1, =(AT91_BASE_SYS + AT91_CKGR_MOR)
-	ldr	r2, =(AT91_BASE_SYS + AT91_PMC_SR)
+	ldr	r1, =(AT91_ASM_PMC_MOR)
+	ldr	r2, =(AT91_ASM_PMC_SR)
 	/* Main oscillator Enable register PMC_MOR: */
 	ldr	r0, =CONFIG_SYS_MOR_VAL
 	str	r0, [r1]
 
 	/* Reading the PMC Status to detect when the Main Oscillator is enabled */
-	mov	r4, #AT91_PMC_MOSCS
+	mov	r4, #AT91_PMC_IXR_MOSCS
 MOSCS_Loop:
 	ldr	r3, [r2]
 	and	r3, r4, r3
-	cmp	r3, #AT91_PMC_MOSCS
+	cmp	r3, #AT91_PMC_IXR_MOSCS
 	bne	MOSCS_Loop
 
 /* ----------------------------------------------------------------------------
@@ -105,16 +110,16 @@ MOSCS_Loop:
  * Setup PLLA
  * ----------------------------------------------------------------------------
  */
-	ldr	r1, =(AT91_BASE_SYS + AT91_CKGR_PLLAR)
+	ldr	r1, =(AT91_ASM_PMC_PLLAR)
 	ldr	r0, =CONFIG_SYS_PLLAR_VAL
 	str	r0, [r1]
 
 	/* Reading the PMC Status register to detect when the PLLA is locked */
-	mov	r4, #AT91_PMC_LOCKA
+	mov	r4, #AT91_PMC_IXR_LOCKA
 MOSCS_Loop1:
 	ldr	r3, [r2]
 	and	r3, r4, r3
-	cmp	r3, #AT91_PMC_LOCKA
+	cmp	r3, #AT91_PMC_IXR_LOCKA
 	bne	MOSCS_Loop1
 
 /* ----------------------------------------------------------------------------
@@ -123,38 +128,37 @@ MOSCS_Loop1:
  * - Switch on the Main Oscillator
  * ----------------------------------------------------------------------------
  */
-	ldr	r1, =(AT91_BASE_SYS + AT91_PMC_MCKR)
+	ldr	r1, =(AT91_ASM_PMC_MCKR)
 
 	/* -Master Clock Controller register PMC_MCKR */
 	ldr	r0, =CONFIG_SYS_MCKR1_VAL
 	str	r0, [r1]
 
 	/* Reading the PMC Status to detect when the Master clock is ready */
-	mov	r4, #AT91_PMC_MCKRDY
+	mov	r4, #AT91_PMC_IXR_MCKRDY
 MCKRDY_Loop:
 	ldr	r3, [r2]
 	and	r3, r4, r3
-	cmp	r3, #AT91_PMC_MCKRDY
+	cmp	r3, #AT91_PMC_IXR_MCKRDY
 	bne	MCKRDY_Loop
 
 	ldr	r0, =CONFIG_SYS_MCKR2_VAL
 	str	r0, [r1]
 
 	/* Reading the PMC Status to detect when the Master clock is ready */
-	mov	r4, #AT91_PMC_MCKRDY
+	mov	r4, #AT91_PMC_IXR_MCKRDY
 MCKRDY_Loop1:
 	ldr	r3, [r2]
 	and	r3, r4, r3
-	cmp	r3, #AT91_PMC_MCKRDY
+	cmp	r3, #AT91_PMC_IXR_MCKRDY
 	bne	MCKRDY_Loop1
-
 PLL_setup_end:
 
 /* ----------------------------------------------------------------------------
  * - memory control configuration 2
  * ----------------------------------------------------------------------------
  */
-	ldr	r0, =(AT91_BASE_SYS + AT91_SDRAMC_TR)
+	ldr	r0, =(AT91_ASM_SDRAMC_TR)
 	ldr	r1, [r0]
 	cmp	r1, #0
 	bne	SDRAM_setup_end
@@ -166,7 +170,6 @@ PLL_setup_end:
 	sub	r2, r2, r1
 	add	r0, r0, r5
 	add	r2, r2, r5
-
 2:
 	/* the address */
 	ldr	r1, [r0], #4
@@ -183,60 +186,53 @@ SDRAM_setup_end:
 	.ltorg
 
 SMRDATA:
-	.word (AT91_BASE_SYS + AT91_WDT_MR)
+	.word AT91_ASM_WDT_MR
 	.word CONFIG_SYS_WDTC_WDMR_VAL
-
 	/* configure PIOx as EBI0 D[16-31] */
 #if defined(CONFIG_AT91SAM9263)
-	.word (AT91_BASE_SYS + AT91_PIOD + PIO_PDR)
+	.word AT91_ASM_PIOD_PDR
 	.word CONFIG_SYS_PIOD_PDR_VAL1
-	.word (AT91_BASE_SYS + AT91_PIOD + PIO_PUDR)
+	.word AT91_ASM_PIOD_PUDR
 	.word CONFIG_SYS_PIOD_PPUDR_VAL
-	.word (AT91_BASE_SYS + AT91_PIOD + PIO_ASR)
+	.word AT91_ASM_PIOD_ASR
 	.word CONFIG_SYS_PIOD_PPUDR_VAL
 #elif defined(CONFIG_AT91SAM9260) || defined(CONFIG_AT91SAM9261) \
 	|| defined(CONFIG_AT91SAM9G20)
-	.word (AT91_BASE_SYS + AT91_PIOC + PIO_PDR)
+	.word AT91_ASM_PIOC_PDR
 	.word CONFIG_SYS_PIOC_PDR_VAL1
-	.word (AT91_BASE_SYS + AT91_PIOC + PIO_PUDR)
+	.word AT91_ASM_PIOC_PUDR
 	.word CONFIG_SYS_PIOC_PPUDR_VAL
 #endif
-
-#if defined(AT91_MATRIX_EBI0CSA)
-	.word (AT91_BASE_SYS + AT91_MATRIX_EBI0CSA)
-	.word CONFIG_SYS_MATRIX_EBI0CSA_VAL
-#else /* AT91_MATRIX_EBICSA */
-	.word (AT91_BASE_SYS + AT91_MATRIX_EBICSA)
+	.word AT91_ASM_MATRIX_CSA0
 	.word CONFIG_SYS_MATRIX_EBICSA_VAL
-#endif
 
 	/* flash */
-	.word (AT91_BASE_SYS + AT91_SMC_MODE(0))
+	.word AT91_ASM_SMC_MODE0
 	.word CONFIG_SYS_SMC0_MODE0_VAL
 
-	.word (AT91_BASE_SYS + AT91_SMC_CYCLE(0))
+	.word AT91_ASM_SMC_CYCLE0
 	.word CONFIG_SYS_SMC0_CYCLE0_VAL
 
-	.word (AT91_BASE_SYS + AT91_SMC_PULSE(0))
+	.word AT91_ASM_SMC_PULSE0
 	.word CONFIG_SYS_SMC0_PULSE0_VAL
 
-	.word (AT91_BASE_SYS + AT91_SMC_SETUP(0))
+	.word AT91_ASM_SMC_SETUP0
 	.word CONFIG_SYS_SMC0_SETUP0_VAL
 
 SMRDATA1:
-	.word (AT91_BASE_SYS + AT91_SDRAMC_MR)
+	.word AT91_ASM_SDRAMC_MR
 	.word CONFIG_SYS_SDRC_MR_VAL1
-	.word (AT91_BASE_SYS + AT91_SDRAMC_TR)
+	.word AT91_ASM_SDRAMC_TR
 	.word CONFIG_SYS_SDRC_TR_VAL1
-	.word (AT91_BASE_SYS + AT91_SDRAMC_CR)
+	.word AT91_ASM_SDRAMC_CR
 	.word CONFIG_SYS_SDRC_CR_VAL
-	.word (AT91_BASE_SYS + AT91_SDRAMC_MDR)
+	.word AT91_ASM_SDRAMC_MDR
 	.word CONFIG_SYS_SDRC_MDR_VAL
-	.word (AT91_BASE_SYS + AT91_SDRAMC_MR)
+	.word AT91_ASM_SDRAMC_MR
 	.word CONFIG_SYS_SDRC_MR_VAL2
 	.word AT91_SDRAM_BASE
 	.word CONFIG_SYS_SDRAM_VAL1
-	.word (AT91_BASE_SYS + AT91_SDRAMC_MR)
+	.word AT91_ASM_SDRAMC_MR
 	.word CONFIG_SYS_SDRC_MR_VAL3
 	.word AT91_SDRAM_BASE
 	.word CONFIG_SYS_SDRAM_VAL2
@@ -254,26 +250,25 @@ SMRDATA1:
 	.word CONFIG_SYS_SDRAM_VAL8
 	.word AT91_SDRAM_BASE
 	.word CONFIG_SYS_SDRAM_VAL9
-	.word (AT91_BASE_SYS + AT91_SDRAMC_MR)
+	.word AT91_ASM_SDRAMC_MR
 	.word CONFIG_SYS_SDRC_MR_VAL4
 	.word AT91_SDRAM_BASE
 	.word CONFIG_SYS_SDRAM_VAL10
-	.word (AT91_BASE_SYS + AT91_SDRAMC_MR)
+	.word AT91_ASM_SDRAMC_MR
 	.word CONFIG_SYS_SDRC_MR_VAL5
 	.word AT91_SDRAM_BASE
 	.word CONFIG_SYS_SDRAM_VAL11
-	.word (AT91_BASE_SYS + AT91_SDRAMC_TR)
+	.word AT91_ASM_SDRAMC_TR
 	.word CONFIG_SYS_SDRC_TR_VAL2
 	.word AT91_SDRAM_BASE
 	.word CONFIG_SYS_SDRAM_VAL12
 	/* User reset enable*/
-	.word (AT91_BASE_SYS + AT91_RSTC_MR)
+	.word AT91_ASM_RSTC_MR
 	.word CONFIG_SYS_RSTC_RMR_VAL
 #ifdef CONFIG_SYS_MATRIX_MCFG_REMAP
 	/* MATRIX_MCFG - REMAP all masters */
-	.word (AT91_BASE_SYS + AT91_MATRIX_MCFG0)
+	.word AT91_ASM_MATRIX_MCFG
 	.word 0x1FF
 #endif
-
 SMRDATA2:
 	.word 0
diff --git a/cpu/arm926ejs/at91/reset.c b/cpu/arm926ejs/at91/reset.c
index f963e14..1b67e77 100644
--- a/cpu/arm926ejs/at91/reset.c
+++ b/cpu/arm926ejs/at91/reset.c
@@ -32,10 +32,12 @@
  */
 void reset_cpu(ulong ignored)
 {
+	at91_rstc_t *rstc = (at91_rstc_t *) AT91_RSTC_BASE;
+
 	/* this is the way Linux does it */
-	at91_sys_write(AT91_RSTC_CR, AT91_RSTC_KEY |
-				     AT91_RSTC_PROCRST |
-				     AT91_RSTC_PERRST);
+
+	writel(AT91_RSTC_KEY | AT91_RSTC_CR_PROCRST | AT91_RSTC_CR_PERRST,
+		&rstc->cr);
 
 	while (1);
 	/* Never reached */
diff --git a/cpu/arm926ejs/at91/timer.c b/cpu/arm926ejs/at91/timer.c
index 7352b5c..d21eebf 100644
--- a/cpu/arm926ejs/at91/timer.c
+++ b/cpu/arm926ejs/at91/timer.c
@@ -35,8 +35,6 @@
  * setting the 20 bit counter period to its maximum (0xfffff).
  */
 #define TIMER_LOAD_VAL	0xfffff
-#define READ_RESET_TIMER at91_sys_read(AT91_PIT_PIVR)
-#define READ_TIMER at91_sys_read(AT91_PIT_PIIR)
 
 static ulong timestamp;
 static ulong lastinc;
@@ -61,14 +59,16 @@ static inline unsigned long long usec_to_tick(unsigned long long usec)
 /* nothing really to do with interrupts, just starts up a counter. */
 int timer_init(void)
 {
+	at91_pmc_t *pmc = (at91_pmc_t *) AT91_PMC_BASE;
+	at91_pit_t *pit = (at91_pit_t *) AT91_PIT_BASE;
 	/*
 	 * Enable PITC Clock
 	 * The clock is already enabled for system controller in boot
 	 */
-	at91_sys_write(AT91_PMC_PCER, 1 << AT91_ID_SYS);
+	writel(1 << AT91_ID_SYS, &pmc->pcer);
 
 	/* Enable PITC */
-	at91_sys_write(AT91_PIT_MR, TIMER_LOAD_VAL | AT91_PIT_PITEN);
+	writel(TIMER_LOAD_VAL | AT91_PIT_MR_EN , &pit->mr);
 
 	reset_timer_masked();
 
@@ -82,7 +82,9 @@ int timer_init(void)
  */
 unsigned long long get_ticks(void)
 {
-	ulong now = READ_TIMER;
+	at91_pit_t *pit = (at91_pit_t *) AT91_PIT_BASE;
+
+	ulong now = readl(&pit->piir);
 
 	if (now >= lastinc)	/* normal mode (non roll) */
 		/* move stamp forward with absolut diff ticks */
@@ -96,7 +98,10 @@ unsigned long long get_ticks(void)
 void reset_timer_masked(void)
 {
 	/* reset time */
-	lastinc = READ_TIMER; /* capture current incrementer value time */
+	at91_pit_t *pit = (at91_pit_t *) AT91_PIT_BASE;
+
+	/* capture current incrementer value time */
+	lastinc = readl(&pit->piir);
 	timestamp = 0; /* start "advancing" time stamp from 0 */
 }
 
diff --git a/cpu/arm926ejs/mx25/Makefile b/cpu/arm926ejs/mx25/Makefile
new file mode 100644
index 0000000..55c1e89
--- /dev/null
+++ b/cpu/arm926ejs/mx25/Makefile
@@ -0,0 +1,46 @@
+#
+# (C) Copyright 2000-2006
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# See file CREDITS for list of people who contributed to this
+# project.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of
+# the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+# MA 02111-1307 USA
+
+include $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(SOC).a
+
+COBJS	= generic.o timer.o
+MX27OBJS = reset.o
+
+SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
+SRCS	+= $(addprefix $(SRCTREE)/cpu/arm926ejs/mx27/,$(MX27OBJS:.o=.c))
+OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS) $(MX27OBJS))
+
+all:	$(obj).depend $(LIB)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/arm926ejs/mx25/generic.c b/cpu/arm926ejs/mx25/generic.c
new file mode 100644
index 0000000..694841d
--- /dev/null
+++ b/cpu/arm926ejs/mx25/generic.c
@@ -0,0 +1,263 @@
+/*
+ * (C) Copyright 2009 DENX Software Engineering
+ * Author: John Rigby <jrigby@gmail.com>
+ *
+ * Based on mx27/generic.c:
+ *  Copyright (c) 2008 Eric Jarrige <eric.jarrige@armadeus.org>
+ *  Copyright (c) 2009 Ilya Yanok <yanok@emcraft.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <div64.h>
+#include <netdev.h>
+#include <asm/io.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/imx25-pinmux.h>
+#ifdef CONFIG_MXC_MMC
+#include <asm/arch/mxcmmc.h>
+#endif
+
+/*
+ *  get the system pll clock in Hz
+ *
+ *                  mfi + mfn / (mfd +1)
+ *  f = 2 * f_ref * --------------------
+ *                        pd + 1
+ */
+static unsigned int imx_decode_pll (unsigned int pll, unsigned int f_ref)
+{
+	unsigned int mfi = (pll >> CCM_PLL_MFI_SHIFT)
+	    & CCM_PLL_MFI_MASK;
+	unsigned int mfn = (pll >> CCM_PLL_MFN_SHIFT)
+	    & CCM_PLL_MFN_MASK;
+	unsigned int mfd = (pll >> CCM_PLL_MFD_SHIFT)
+	    & CCM_PLL_MFD_MASK;
+	unsigned int pd = (pll >> CCM_PLL_PD_SHIFT)
+	    & CCM_PLL_PD_MASK;
+
+	mfi = mfi <= 5 ? 5 : mfi;
+
+	return lldiv (2 * (u64) f_ref * (mfi * (mfd + 1) + mfn),
+		      (mfd + 1) * (pd + 1));
+}
+
+static ulong imx_get_mpllclk (void)
+{
+	struct ccm_regs *ccm = (struct ccm_regs *)IMX_CCM_BASE;
+	ulong fref = 24000000;
+
+	return imx_decode_pll (readl (&ccm->mpctl), fref);
+}
+
+ulong imx_get_armclk (void)
+{
+	struct ccm_regs *ccm = (struct ccm_regs *)IMX_CCM_BASE;
+	ulong cctl = readl (&ccm->cctl);
+	ulong fref = imx_get_mpllclk ();
+	ulong div;
+
+	if (cctl & CCM_CCTL_ARM_SRC)
+		fref = lldiv ((fref * 3), 4);
+
+	div = ((cctl >> CCM_CCTL_ARM_DIV_SHIFT)
+	       & CCM_CCTL_ARM_DIV_MASK) + 1;
+
+	return lldiv (fref, div);
+}
+
+ulong imx_get_ahbclk (void)
+{
+	struct ccm_regs *ccm = (struct ccm_regs *)IMX_CCM_BASE;
+	ulong cctl = readl (&ccm->cctl);
+	ulong fref = imx_get_armclk ();
+	ulong div;
+
+	div = ((cctl >> CCM_CCTL_AHB_DIV_SHIFT)
+	       & CCM_CCTL_AHB_DIV_MASK) + 1;
+
+	return lldiv (fref, div);
+}
+
+ulong imx_get_perclk (int clk)
+{
+	struct ccm_regs *ccm = (struct ccm_regs *)IMX_CCM_BASE;
+	ulong fref = imx_get_ahbclk ();
+	ulong div;
+
+	div = readl (&ccm->pcdr[CCM_PERCLK_REG (clk)]);
+	div = ((div >> CCM_PERCLK_SHIFT (clk)) & CCM_PERCLK_MASK) + 1;
+
+	return lldiv (fref, div);
+}
+
+#if defined(CONFIG_DISPLAY_CPUINFO)
+int print_cpuinfo (void)
+{
+	char buf[32];
+
+	printf ("CPU:   Freescale i.MX25 at %s MHz\n\n",
+		strmhz (buf, imx_get_mpllclk ()));
+	return 0;
+}
+#endif
+
+int cpu_eth_init (bd_t * bis)
+{
+#if defined(CONFIG_FEC_MXC)
+	struct ccm_regs *ccm = (struct ccm_regs *)IMX_CCM_BASE;
+	ulong val;
+
+	val = readl (&ccm->cgr0);
+	val |= (1 << 23);
+	writel (val, &ccm->cgr0);
+	return fecmxc_initialize (bis);
+#else
+	return 0;
+#endif
+}
+
+/*
+ * Initializes on-chip MMC controllers.
+ * to override, implement board_mmc_init()
+ */
+int cpu_mmc_init (bd_t * bis)
+{
+#ifdef CONFIG_MXC_MMC
+	return mxc_mmc_init (bis);
+#else
+	return 0;
+#endif
+}
+
+#ifdef CONFIG_MXC_UART
+void mx25_uart_init_pins (void)
+{
+	struct iomuxc_mux_ctl *muxctl;
+	struct iomuxc_pad_ctl *padctl;
+	u32 inpadctl;
+	u32 outpadctl;
+	u32 muxmode0;
+
+	muxctl = (struct iomuxc_mux_ctl *)IMX_IOPADMUX_BASE;
+	padctl = (struct iomuxc_pad_ctl *)IMX_IOPADCTL_BASE;
+	muxmode0 = MX25_PIN_MUX_MODE (0);
+	/*
+	 * set up input pins with hysteresis and 100K pull-ups
+	 */
+	inpadctl = MX25_PIN_PAD_CTL_HYS
+	    | MX25_PIN_PAD_CTL_PKE
+	    | MX25_PIN_PAD_CTL_PUE | MX25_PIN_PAD_CTL_100K_PU;
+
+	/*
+	 * set up output pins with 100K pull-downs
+	 * FIXME: need to revisit this
+	 *      PUE is ignored if PKE is not set
+	 *      so the right value here is likely
+	 *        0x0 for no pull up/down
+	 *      or
+	 *        0xc0 for 100k pull down
+	 */
+	outpadctl = MX25_PIN_PAD_CTL_PUE | MX25_PIN_PAD_CTL_100K_PD;
+
+	/* UART1 */
+	/* rxd */
+	writel (muxmode0, &muxctl->pad_uart1_rxd);
+	writel (inpadctl, &padctl->pad_uart1_rxd);
+
+	/* txd */
+	writel (muxmode0, &muxctl->pad_uart1_txd);
+	writel (outpadctl, &padctl->pad_uart1_txd);
+
+	/* rts */
+	writel (muxmode0, &muxctl->pad_uart1_rts);
+	writel (outpadctl, &padctl->pad_uart1_rts);
+
+	/* cts */
+	writel (muxmode0, &muxctl->pad_uart1_cts);
+	writel (inpadctl, &padctl->pad_uart1_cts);
+}
+#endif /* CONFIG_MXC_UART */
+
+#ifdef CONFIG_FEC_MXC
+void mx25_fec_init_pins (void)
+{
+	struct iomuxc_mux_ctl *muxctl;
+	struct iomuxc_pad_ctl *padctl;
+	u32 inpadctl_100kpd;
+	u32 inpadctl_22kpu;
+	u32 outpadctl;
+	u32 muxmode0;
+
+	muxctl = (struct iomuxc_mux_ctl *)IMX_IOPADMUX_BASE;
+	padctl = (struct iomuxc_pad_ctl *)IMX_IOPADCTL_BASE;
+	muxmode0 = MX25_PIN_MUX_MODE (0);
+	inpadctl_100kpd = MX25_PIN_PAD_CTL_HYS
+	    | MX25_PIN_PAD_CTL_PKE
+	    | MX25_PIN_PAD_CTL_PUE | MX25_PIN_PAD_CTL_100K_PD;
+	inpadctl_22kpu = MX25_PIN_PAD_CTL_HYS
+	    | MX25_PIN_PAD_CTL_PKE
+	    | MX25_PIN_PAD_CTL_PUE | MX25_PIN_PAD_CTL_22K_PU;
+	/*
+	 * set up output pins with 100K pull-downs
+	 * FIXME: need to revisit this
+	 *      PUE is ignored if PKE is not set
+	 *      so the right value here is likely
+	 *        0x0 for no pull
+	 *      or
+	 *        0xc0 for 100k pull down
+	 */
+	outpadctl = MX25_PIN_PAD_CTL_PUE | MX25_PIN_PAD_CTL_100K_PD;
+
+	/* FEC_TX_CLK */
+	writel (muxmode0, &muxctl->pad_fec_tx_clk);
+	writel (inpadctl_100kpd, &padctl->pad_fec_tx_clk);
+
+	/* FEC_RX_DV */
+	writel (muxmode0, &muxctl->pad_fec_rx_dv);
+	writel (inpadctl_100kpd, &padctl->pad_fec_rx_dv);
+
+	/* FEC_RDATA0 */
+	writel (muxmode0, &muxctl->pad_fec_rdata0);
+	writel (inpadctl_100kpd, &padctl->pad_fec_rdata0);
+
+	/* FEC_TDATA0 */
+	writel (muxmode0, &muxctl->pad_fec_tdata0);
+	writel (outpadctl, &padctl->pad_fec_tdata0);
+
+	/* FEC_TX_EN */
+	writel (muxmode0, &muxctl->pad_fec_tx_en);
+	writel (outpadctl, &padctl->pad_fec_tx_en);
+
+	/* FEC_MDC */
+	writel (muxmode0, &muxctl->pad_fec_mdc);
+	writel (outpadctl, &padctl->pad_fec_mdc);
+
+	/* FEC_MDIO */
+	writel (muxmode0, &muxctl->pad_fec_mdio);
+	writel (inpadctl_22kpu, &padctl->pad_fec_mdio);
+
+	/* FEC_RDATA1 */
+	writel (muxmode0, &muxctl->pad_fec_rdata1);
+	writel (inpadctl_100kpd, &padctl->pad_fec_rdata1);
+
+	/* FEC_TDATA1 */
+	writel (muxmode0, &muxctl->pad_fec_tdata1);
+	writel (outpadctl, &padctl->pad_fec_tdata1);
+
+}
+#endif /* CONFIG_FEC_MXC */
diff --git a/cpu/arm926ejs/mx25/reset.c b/cpu/arm926ejs/mx25/reset.c
new file mode 100644
index 0000000..1e33150
--- /dev/null
+++ b/cpu/arm926ejs/mx25/reset.c
@@ -0,0 +1,56 @@
+/*
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Marius Groeger <mgroeger@sysgo.de>
+ *
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Alex Zuepke <azu@sysgo.de>
+ *
+ * (C) Copyright 2002
+ * Gary Jennejohn, DENX Software Engineering, <gj@denx.de>
+ *
+ * (C) Copyright 2009
+ * Ilya Yanok, Emcraft Systems Ltd, <yanok@emcraft.com>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/imx-regs.h>
+
+/*
+ * Reset the cpu by setting up the watchdog timer and let it time out
+ */
+void reset_cpu (ulong ignored)
+{
+	struct wdog_regs *regs = (struct wdog_regs *)IMX_WDT_BASE;
+	/* Disable watchdog and set Time-Out field to 0 */
+	writel (0x00000000, &regs->wcr);
+
+	/* Write Service Sequence */
+	writel (0x00005555, &regs->wsr);
+	writel (0x0000AAAA, &regs->wsr);
+
+	/* Enable watchdog */
+	writel (WCR_WDE, &regs->wcr);
+
+	while (1) ;
+}
diff --git a/cpu/arm926ejs/mx25/timer.c b/cpu/arm926ejs/mx25/timer.c
new file mode 100644
index 0000000..11d41a8
--- /dev/null
+++ b/cpu/arm926ejs/mx25/timer.c
@@ -0,0 +1,187 @@
+/*
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Marius Groeger <mgroeger@sysgo.de>
+ *
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Alex Zuepke <azu@sysgo.de>
+ *
+ * (C) Copyright 2002
+ * Gary Jennejohn, DENX Software Engineering, <gj@denx.de>
+ *
+ * (C) Copyright 2009
+ * Ilya Yanok, Emcraft Systems Ltd, <yanok@emcraft.com>
+ *
+ * (C) Copyright 2009 DENX Software Engineering
+ * Author: John Rigby <jrigby@gmail.com>
+ * 	Add support for MX25
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <div64.h>
+#include <asm/io.h>
+#include <asm/arch/imx-regs.h>
+
+static ulong timestamp;
+static ulong lastinc;
+
+/*
+ * "time" is measured in 1 / CONFIG_SYS_HZ seconds,
+ * "tick" is internal timer period
+ */
+#ifdef CONFIG_MX25_TIMER_HIGH_PRECISION
+/* ~0.4% error - measured with stop-watch on 100s boot-delay */
+static inline unsigned long long tick_to_time(unsigned long long tick)
+{
+	tick *= CONFIG_SYS_HZ;
+	do_div(tick, CONFIG_MX25_CLK32);
+	return tick;
+}
+
+static inline unsigned long long time_to_tick(unsigned long long time)
+{
+	time *= CONFIG_MX25_CLK32;
+	do_div(time, CONFIG_SYS_HZ);
+	return time;
+}
+
+static inline unsigned long long us_to_tick(unsigned long long us)
+{
+	us = us * CONFIG_MX25_CLK32 + 999999;
+	do_div(us, 1000000);
+	return us;
+}
+#else
+/* ~2% error */
+#define TICK_PER_TIME	((CONFIG_MX25_CLK32 + CONFIG_SYS_HZ / 2) / \
+		CONFIG_SYS_HZ)
+#define US_PER_TICK	(1000000 / CONFIG_MX25_CLK32)
+
+static inline unsigned long long tick_to_time(unsigned long long tick)
+{
+	do_div(tick, TICK_PER_TIME);
+	return tick;
+}
+
+static inline unsigned long long time_to_tick(unsigned long long time)
+{
+	return time * TICK_PER_TIME;
+}
+
+static inline unsigned long long us_to_tick(unsigned long long us)
+{
+	us += US_PER_TICK - 1;
+	do_div(us, US_PER_TICK);
+	return us;
+}
+#endif
+
+/* nothing really to do with interrupts, just starts up a counter. */
+/* The 32KHz 32-bit timer overruns in 134217 seconds */
+int timer_init(void)
+{
+	int i;
+	struct gpt_regs *gpt = (struct gpt_regs *)IMX_GPT1_BASE;
+	struct ccm_regs *ccm = (struct ccm_regs *)IMX_CCM_BASE;
+
+	/* setup GP Timer 1 */
+	writel(GPT_CTRL_SWR, &gpt->ctrl);
+
+	writel(readl(&ccm->cgr1) | CCM_CGR1_GPT1, &ccm->cgr1);
+
+	for (i = 0; i < 100; i++)
+		writel(0, &gpt->ctrl); /* We have no udelay by now */
+	writel(0, &gpt->pre); /* prescaler = 1 */
+	/* Freerun Mode, 32KHz input */
+	writel(readl(&gpt->ctrl) | GPT_CTRL_CLKSOURCE_32 | GPT_CTRL_FRR,
+			&gpt->ctrl);
+	writel(readl(&gpt->ctrl) | GPT_CTRL_TEN, &gpt->ctrl);
+
+	return 0;
+}
+
+void reset_timer_masked(void)
+{
+	struct gpt_regs *gpt = (struct gpt_regs *)IMX_GPT1_BASE;
+	/* reset time */
+	/* capture current incrementer value time */
+	lastinc = readl(&gpt->counter);
+	timestamp = 0; /* start "advancing" time stamp from 0 */
+}
+
+void reset_timer(void)
+{
+	reset_timer_masked();
+}
+
+unsigned long long get_ticks (void)
+{
+	struct gpt_regs *gpt = (struct gpt_regs *)IMX_GPT1_BASE;
+	ulong now = readl(&gpt->counter); /* current tick value */
+
+	if (now >= lastinc) {
+		/*
+		 * normal mode (non roll)
+		 * move stamp forward with absolut diff ticks
+		 */
+		timestamp += (now - lastinc);
+	} else {
+		/* we have rollover of incrementer */
+		timestamp += (0xFFFFFFFF - lastinc) + now;
+	}
+	lastinc = now;
+	return timestamp;
+}
+
+ulong get_timer_masked (void)
+{
+	/*
+	 * get_ticks() returns a long long (64 bit), it wraps in
+	 * 2^64 / CONFIG_MX25_CLK32 = 2^64 / 2^15 = 2^49 ~ 5 * 10^14 (s) ~
+	 * 5 * 10^9 days... and get_ticks() * CONFIG_SYS_HZ wraps in
+	 * 5 * 10^6 days - long enough.
+	 */
+	return tick_to_time(get_ticks());
+}
+
+ulong get_timer (ulong base)
+{
+	return get_timer_masked () - base;
+}
+
+void set_timer (ulong t)
+{
+	timestamp = time_to_tick(t);
+}
+
+/* delay x useconds AND preserve advance timstamp value */
+void __udelay (unsigned long usec)
+{
+	unsigned long long tmp;
+	ulong tmo;
+
+	tmo = us_to_tick(usec);
+	tmp = get_ticks() + tmo;	/* get current timestamp */
+
+	while (get_ticks() < tmp)	/* loop till event */
+		 /*NOP*/;
+}
diff --git a/cpu/arm926ejs/mx27/generic.c b/cpu/arm926ejs/mx27/generic.c
index da05c55..30cf544 100644
--- a/cpu/arm926ejs/mx27/generic.c
+++ b/cpu/arm926ejs/mx27/generic.c
@@ -166,6 +166,11 @@ int print_cpuinfo (void)
 int cpu_eth_init(bd_t *bis)
 {
 #if defined(CONFIG_FEC_MXC)
+	struct pll_regs *pll = (struct pll_regs *)IMX_PLL_BASE;
+
+	/* enable FEC clock */
+	writel(readl(&pll->pccr1) | PCCR1_HCLK_FEC, &pll->pccr1);
+	writel(readl(&pll->pccr0) | PCCR0_FEC_EN, &pll->pccr0);
 	return fecmxc_initialize(bis);
 #else
 	return 0;
diff --git a/cpu/arm926ejs/start.S b/cpu/arm926ejs/start.S
index 4421b6a..3b81151 100644
--- a/cpu/arm926ejs/start.S
+++ b/cpu/arm926ejs/start.S
@@ -53,6 +53,27 @@
 .globl _start
 _start:
 	b	reset
+#ifdef CONFIG_PRELOADER
+/* No exception handlers in preloader */
+	ldr	pc, _hang
+	ldr	pc, _hang
+	ldr	pc, _hang
+	ldr	pc, _hang
+	ldr	pc, _hang
+	ldr	pc, _hang
+	ldr	pc, _hang
+
+_hang:
+	.word	do_hang
+/* pad to 64 byte boundary */
+	.word	0x12345678
+	.word	0x12345678
+	.word	0x12345678
+	.word	0x12345678
+	.word	0x12345678
+	.word	0x12345678
+	.word	0x12345678
+#else
 	ldr	pc, _undefined_instruction
 	ldr	pc, _software_interrupt
 	ldr	pc, _prefetch_abort
@@ -76,6 +97,7 @@ _irq:
 _fiq:
 	.word fiq
 
+#endif	/* CONFIG_PRELOADER */
 	.balignl 16,0xdeadbeef
 
 
@@ -150,7 +172,6 @@ relocate:				/* relocate U-Boot to RAM	    */
 	ldr	r1, _TEXT_BASE		/* test if we run from flash or RAM */
 	cmp     r0, r1                  /* don't reloc during debug         */
 	beq     stack_setup
-
 	ldr	r2, _armboot_start
 	ldr	r3, _bss_start
 	sub	r2, r3, r2		/* r2 <- size of armboot            */
@@ -166,11 +187,14 @@ copy_loop:
 	/* Set up the stack						    */
 stack_setup:
 	ldr	r0, _TEXT_BASE		/* upper 128 KiB: relocated uboot   */
+	sub	sp, r0, #128		/* leave 32 words for abort-stack   */
+#ifndef CONFIG_PRELOADER
 	sub	r0, r0, #CONFIG_SYS_MALLOC_LEN	/* malloc area                      */
 	sub	r0, r0, #CONFIG_SYS_GBL_DATA_SIZE /* bdinfo                        */
 #ifdef CONFIG_USE_IRQ
 	sub	r0, r0, #(CONFIG_STACKSIZE_IRQ+CONFIG_STACKSIZE_FIQ)
 #endif
+#endif /* CONFIG_PRELOADER */
 	sub	sp, r0, #12		/* leave 3 words for abort-stack    */
 	bic	sp, r0, #7		/* 8-byte align stack for ABI compliance */
 
@@ -179,6 +203,7 @@ clear_bss:
 	ldr	r1, _bss_end		/* stop here                        */
 	mov	r2, #0x00000000		/* clear                            */
 
+#ifndef CONFIG_PRELOADER
 clbss_l:str	r2, [r0]		/* clear loop...                    */
 	add	r0, r0, #4
 	cmp	r0, r1
@@ -186,11 +211,16 @@ clbss_l:str	r2, [r0]		/* clear loop...                    */
 
 	bl coloured_LED_init
 	bl red_LED_on
+#endif /* CONFIG_PRELOADER */
 
 	ldr	pc, _start_armboot
 
 _start_armboot:
+#ifdef CONFIG_NAND_SPL
+	.word nand_boot
+#else
 	.word start_armboot
+#endif /* CONFIG_NAND_SPL */
 
 
 /*
@@ -231,6 +261,7 @@ cpu_init_crit:
 	mov	pc, lr		/* back to my caller */
 #endif /* CONFIG_SKIP_LOWLEVEL_INIT */
 
+#ifndef CONFIG_PRELOADER
 /*
  *************************************************************************
  *
@@ -332,10 +363,18 @@ cpu_init_crit:
 	.macro get_fiq_stack			@ setup FIQ stack
 	ldr	sp, FIQ_STACK_START
 	.endm
+#endif	/* CONFIG_PRELOADER */
 
 /*
  * exception handlers
  */
+#ifdef CONFIG_PRELOADER
+	.align	5
+do_hang:
+	ldr	sp, _TEXT_BASE			/* switch to abort stack */
+1:
+	bl	1b				/* hang and never return */
+#else	/* !CONFIG_PRELOADER */
 	.align  5
 undefined_instruction:
 	get_bad_stack
@@ -398,3 +437,4 @@ fiq:
 	bl	do_fiq
 
 #endif
+#endif	/* CONFIG_PRELOADER */
diff --git a/cpu/arm_cortexa8/mx51/Makefile b/cpu/arm_cortexa8/mx51/Makefile
new file mode 100644
index 0000000..7cfaa2c
--- /dev/null
+++ b/cpu/arm_cortexa8/mx51/Makefile
@@ -0,0 +1,48 @@
+#
+# (C) Copyright 2000-2006
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# (C) Copyright 2009 Freescale Semiconductor, Inc.
+#
+# See file CREDITS for list of people who contributed to this
+# project.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of
+# the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+# MA 02111-1307 USA
+#
+
+include $(TOPDIR)/config.mk
+
+LIB	= $(obj)lib$(SOC).a
+
+COBJS	= soc.o clock.o iomux.o timer.o speed.o
+SOBJS = lowlevel_init.o
+
+SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
+
+all:	$(obj).depend $(LIB)
+
+$(LIB):	$(OBJS)
+	$(AR) $(ARFLAGS) $@ $(OBJS)
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+#########################################################################
diff --git a/cpu/arm_cortexa8/mx51/clock.c b/cpu/arm_cortexa8/mx51/clock.c
new file mode 100644
index 0000000..38480ac
--- /dev/null
+++ b/cpu/arm_cortexa8/mx51/clock.c
@@ -0,0 +1,294 @@
+/*
+ * (C) Copyright 2007
+ * Sascha Hauer, Pengutronix
+ *
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/crm_regs.h>
+#include <asm/arch/clock.h>
+
+enum pll_clocks {
+	PLL1_CLOCK = 0,
+	PLL2_CLOCK,
+	PLL3_CLOCK,
+	PLL_CLOCKS,
+};
+
+struct mxc_pll_reg *mxc_plls[PLL_CLOCKS] = {
+	[PLL1_CLOCK] = (struct mxc_pll_reg *)PLL1_BASE_ADDR,
+	[PLL2_CLOCK] = (struct mxc_pll_reg *)PLL2_BASE_ADDR,
+	[PLL3_CLOCK] = (struct mxc_pll_reg *)PLL3_BASE_ADDR,
+};
+
+struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)MXC_CCM_BASE;
+
+/*
+ * Calculate the frequency of this pll.
+ */
+static u32 decode_pll(struct mxc_pll_reg *pll, u32 infreq)
+{
+	u32 mfi, mfn, mfd, pd;
+
+	mfn = __raw_readl(&pll->mfn);
+	mfd = __raw_readl(&pll->mfd) + 1;
+	mfi = __raw_readl(&pll->op);
+	pd = (mfi  & 0xF) + 1;
+	mfi = (mfi >> 4) & 0xF;
+	mfi = (mfi >= 5) ? mfi : 5;
+
+	return ((4 * (infreq / 1000) * (mfi * mfd + mfn)) / (mfd * pd)) * 1000;
+}
+
+/*
+ * Get mcu main rate
+ */
+u32 get_mcu_main_clk(void)
+{
+	u32 reg, freq;
+
+	reg = (__raw_readl(&mxc_ccm->cacrr) & MXC_CCM_CACRR_ARM_PODF_MASK) >>
+		MXC_CCM_CACRR_ARM_PODF_OFFSET;
+	freq = decode_pll(mxc_plls[PLL1_CLOCK], CONFIG_MX51_HCLK_FREQ);
+	return freq / (reg + 1);
+}
+
+/*
+ * Get the rate of peripheral's root clock.
+ */
+static u32 get_periph_clk(void)
+{
+	u32 reg;
+
+	reg = __raw_readl(&mxc_ccm->cbcdr);
+	if (!(reg & MXC_CCM_CBCDR_PERIPH_CLK_SEL))
+		return decode_pll(mxc_plls[PLL2_CLOCK], CONFIG_MX51_HCLK_FREQ);
+	reg = __raw_readl(&mxc_ccm->cbcmr);
+	switch ((reg & MXC_CCM_CBCMR_PERIPH_CLK_SEL_MASK) >>
+		MXC_CCM_CBCMR_PERIPH_CLK_SEL_OFFSET) {
+	case 0:
+		return decode_pll(mxc_plls[PLL1_CLOCK], CONFIG_MX51_HCLK_FREQ);
+	case 1:
+		return decode_pll(mxc_plls[PLL3_CLOCK], CONFIG_MX51_HCLK_FREQ);
+	default:
+		return 0;
+	}
+	/* NOTREACHED */
+}
+
+/*
+ * Get the rate of ipg clock.
+ */
+static u32 get_ipg_clk(void)
+{
+	u32 ahb_podf, ipg_podf;
+
+	ahb_podf = __raw_readl(&mxc_ccm->cbcdr);
+	ipg_podf = (ahb_podf & MXC_CCM_CBCDR_IPG_PODF_MASK) >>
+			MXC_CCM_CBCDR_IPG_PODF_OFFSET;
+	ahb_podf = (ahb_podf & MXC_CCM_CBCDR_AHB_PODF_MASK) >>
+			MXC_CCM_CBCDR_AHB_PODF_OFFSET;
+	return get_periph_clk() / ((ahb_podf + 1) * (ipg_podf + 1));
+}
+
+/*
+ * Get the rate of ipg_per clock.
+ */
+static u32 get_ipg_per_clk(void)
+{
+	u32 pred1, pred2, podf;
+
+	if (__raw_readl(&mxc_ccm->cbcmr) & MXC_CCM_CBCMR_PERCLK_IPG_CLK_SEL)
+		return get_ipg_clk();
+	/* Fixme: not handle what about lpm*/
+	podf = __raw_readl(&mxc_ccm->cbcdr);
+	pred1 = (podf & MXC_CCM_CBCDR_PERCLK_PRED1_MASK) >>
+		MXC_CCM_CBCDR_PERCLK_PRED1_OFFSET;
+	pred2 = (podf & MXC_CCM_CBCDR_PERCLK_PRED2_MASK) >>
+		MXC_CCM_CBCDR_PERCLK_PRED2_OFFSET;
+	podf = (podf & MXC_CCM_CBCDR_PERCLK_PODF_MASK) >>
+		MXC_CCM_CBCDR_PERCLK_PODF_OFFSET;
+
+	return get_periph_clk() / ((pred1 + 1) * (pred2 + 1) * (podf + 1));
+}
+
+/*
+ * Get the rate of uart clk.
+ */
+static u32 get_uart_clk(void)
+{
+	unsigned int freq, reg, pred, podf;
+
+	reg = __raw_readl(&mxc_ccm->cscmr1);
+	switch ((reg & MXC_CCM_CSCMR1_UART_CLK_SEL_MASK) >>
+		MXC_CCM_CSCMR1_UART_CLK_SEL_OFFSET) {
+	case 0x0:
+		freq = decode_pll(mxc_plls[PLL1_CLOCK],
+				    CONFIG_MX51_HCLK_FREQ);
+		break;
+	case 0x1:
+		freq = decode_pll(mxc_plls[PLL2_CLOCK],
+				    CONFIG_MX51_HCLK_FREQ);
+		break;
+	case 0x2:
+		freq = decode_pll(mxc_plls[PLL3_CLOCK],
+				    CONFIG_MX51_HCLK_FREQ);
+		break;
+	default:
+		return 66500000;
+	}
+
+	reg = __raw_readl(&mxc_ccm->cscdr1);
+
+	pred = (reg & MXC_CCM_CSCDR1_UART_CLK_PRED_MASK) >>
+		MXC_CCM_CSCDR1_UART_CLK_PRED_OFFSET;
+
+	podf = (reg & MXC_CCM_CSCDR1_UART_CLK_PODF_MASK) >>
+		MXC_CCM_CSCDR1_UART_CLK_PODF_OFFSET;
+	freq /= (pred + 1) * (podf + 1);
+
+	return freq;
+}
+
+/*
+ * This function returns the low power audio clock.
+ */
+u32 get_lp_apm(void)
+{
+	u32 ret_val = 0;
+	u32 ccsr = __raw_readl(&mxc_ccm->ccsr);
+
+	if (((ccsr >> 9) & 1) == 0)
+		ret_val = CONFIG_MX51_HCLK_FREQ;
+	else
+		ret_val = ((32768 * 1024));
+
+	return ret_val;
+}
+
+/*
+ * get cspi clock rate.
+ */
+u32 imx_get_cspiclk(void)
+{
+	u32 ret_val = 0, pdf, pre_pdf, clk_sel;
+	u32 cscmr1 = __raw_readl(&mxc_ccm->cscmr1);
+	u32 cscdr2 = __raw_readl(&mxc_ccm->cscdr2);
+
+	pre_pdf = (cscdr2 & MXC_CCM_CSCDR2_CSPI_CLK_PRED_MASK) \
+			>> MXC_CCM_CSCDR2_CSPI_CLK_PRED_OFFSET;
+	pdf = (cscdr2 & MXC_CCM_CSCDR2_CSPI_CLK_PODF_MASK) \
+			>> MXC_CCM_CSCDR2_CSPI_CLK_PODF_OFFSET;
+	clk_sel = (cscmr1 & MXC_CCM_CSCMR1_CSPI_CLK_SEL_MASK) \
+			>> MXC_CCM_CSCMR1_CSPI_CLK_SEL_OFFSET;
+
+	switch (clk_sel) {
+	case 0:
+		ret_val = decode_pll(mxc_plls[PLL1_CLOCK],
+					CONFIG_MX51_HCLK_FREQ) /
+					((pre_pdf + 1) * (pdf + 1));
+		break;
+	case 1:
+		ret_val = decode_pll(mxc_plls[PLL2_CLOCK],
+					CONFIG_MX51_HCLK_FREQ) /
+					((pre_pdf + 1) * (pdf + 1));
+		break;
+	case 2:
+		ret_val = decode_pll(mxc_plls[PLL3_CLOCK],
+					CONFIG_MX51_HCLK_FREQ) /
+					((pre_pdf + 1) * (pdf + 1));
+		break;
+	default:
+		ret_val = get_lp_apm() / ((pre_pdf + 1) * (pdf + 1));
+		break;
+	}
+
+	return ret_val;
+}
+
+/*
+ * The API of get mxc clockes.
+ */
+unsigned int mxc_get_clock(enum mxc_clock clk)
+{
+	switch (clk) {
+	case MXC_ARM_CLK:
+		return get_mcu_main_clk();
+	case MXC_AHB_CLK:
+		break;
+	case MXC_IPG_CLK:
+		return get_ipg_clk();
+	case MXC_IPG_PERCLK:
+		return get_ipg_per_clk();
+	case MXC_UART_CLK:
+		return get_uart_clk();
+	case MXC_CSPI_CLK:
+		return imx_get_cspiclk();
+	case MXC_FEC_CLK:
+		return decode_pll(mxc_plls[PLL1_CLOCK],
+				    CONFIG_MX51_HCLK_FREQ);
+	default:
+		break;
+	}
+	return -1;
+}
+
+u32 imx_get_uartclk(void)
+{
+	return get_uart_clk();
+}
+
+
+u32 imx_get_fecclk(void)
+{
+	return mxc_get_clock(MXC_IPG_CLK);
+}
+
+/*
+ * Dump some core clockes.
+ */
+int do_mx51_showclocks(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
+{
+	u32 freq;
+
+	freq = decode_pll(mxc_plls[PLL1_CLOCK], CONFIG_MX51_HCLK_FREQ);
+	printf("mx51 pll1: %dMHz\n", freq / 1000000);
+	freq = decode_pll(mxc_plls[PLL2_CLOCK], CONFIG_MX51_HCLK_FREQ);
+	printf("mx51 pll2: %dMHz\n", freq / 1000000);
+	freq = decode_pll(mxc_plls[PLL3_CLOCK], CONFIG_MX51_HCLK_FREQ);
+	printf("mx51 pll3: %dMHz\n", freq / 1000000);
+	printf("ipg clock     : %dHz\n", mxc_get_clock(MXC_IPG_CLK));
+	printf("ipg per clock : %dHz\n", mxc_get_clock(MXC_IPG_PERCLK));
+
+	return 0;
+}
+
+/***************************************************/
+
+U_BOOT_CMD(
+	clockinfo,	CONFIG_SYS_MAXARGS,	1,	do_mx51_showclocks,
+	"display mx51 clocks\n",
+	""
+);
diff --git a/cpu/arm_cortexa8/mx51/iomux.c b/cpu/arm_cortexa8/mx51/iomux.c
new file mode 100644
index 0000000..62b2954
--- /dev/null
+++ b/cpu/arm_cortexa8/mx51/iomux.c
@@ -0,0 +1,166 @@
+/*
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/mx51_pins.h>
+#include <asm/arch/iomux.h>
+#include <asm/arch/sys_proto.h>
+
+/* IOMUX register (base) addresses */
+enum iomux_reg_addr {
+	IOMUXGPR0 = IOMUXC_BASE_ADDR,
+	IOMUXGPR1 = IOMUXC_BASE_ADDR + 0x004,
+	IOMUXSW_MUX_CTL = IOMUXC_BASE_ADDR,
+	IOMUXSW_MUX_END = IOMUXC_BASE_ADDR + MUX_I_END,
+	IOMUXSW_PAD_CTL = IOMUXC_BASE_ADDR + PAD_I_START,
+	IOMUXSW_INPUT_CTL = IOMUXC_BASE_ADDR,
+};
+
+#define MUX_PIN_NUM_MAX (((MUX_I_END - MUX_I_START) >> 2) + 1)
+
+/* Get the iomux register address of this pin */
+static inline u32 get_mux_reg(iomux_pin_name_t pin)
+{
+	u32 mux_reg = PIN_TO_IOMUX_MUX(pin);
+
+	if (is_soc_rev(CHIP_REV_2_0) < 0) {
+		/*
+		 * Fixup register address:
+		 *	i.MX51 TO1 has offset with the register
+		 * 	which is define as TO2.
+		 */
+		if ((pin == MX51_PIN_NANDF_RB5) ||
+			(pin == MX51_PIN_NANDF_RB6) ||
+			(pin == MX51_PIN_NANDF_RB7))
+			; /* Do nothing */
+		else if (mux_reg >= 0x2FC)
+			mux_reg += 8;
+		else if (mux_reg >= 0x130)
+			mux_reg += 0xC;
+	}
+	mux_reg += IOMUXSW_MUX_CTL;
+	return mux_reg;
+}
+
+/* Get the pad register address of this pin */
+static inline u32 get_pad_reg(iomux_pin_name_t pin)
+{
+	u32 pad_reg = PIN_TO_IOMUX_PAD(pin);
+
+	if (is_soc_rev(CHIP_REV_2_0) < 0) {
+		/*
+		 * Fixup register address:
+		 *	i.MX51 TO1 has offset with the register
+		 * 	which is define as TO2.
+		 */
+		if ((pin == MX51_PIN_NANDF_RB5) ||
+			(pin == MX51_PIN_NANDF_RB6) ||
+			(pin == MX51_PIN_NANDF_RB7))
+			; /* Do nothing */
+		else if (pad_reg == 0x4D0 - PAD_I_START)
+			pad_reg += 0x4C;
+		else if (pad_reg == 0x860 - PAD_I_START)
+			pad_reg += 0x9C;
+		else if (pad_reg >= 0x804 - PAD_I_START)
+			pad_reg += 0xB0;
+		else if (pad_reg >= 0x7FC - PAD_I_START)
+			pad_reg += 0xB4;
+		else if (pad_reg >= 0x4E4 - PAD_I_START)
+			pad_reg += 0xCC;
+		else
+			pad_reg += 8;
+	}
+	pad_reg += IOMUXSW_PAD_CTL;
+	return pad_reg;
+}
+
+/* Get the last iomux register address */
+static inline u32 get_mux_end(void)
+{
+	if (is_soc_rev(CHIP_REV_2_0) < 0)
+		return IOMUXC_BASE_ADDR + (0x3F8 - 4);
+	else
+		return IOMUXC_BASE_ADDR + (0x3F0 - 4);
+}
+
+/*
+ * This function is used to configure a pin through the IOMUX module.
+ * @param  pin		a pin number as defined in iomux_pin_name_t
+ * @param  cfg		an output function as defined in iomux_pin_cfg_t
+ *
+ * @return 		0 if successful; Non-zero otherwise
+ */
+static void iomux_config_mux(iomux_pin_name_t pin, iomux_pin_cfg_t cfg)
+{
+	u32 mux_reg = get_mux_reg(pin);
+
+	if ((mux_reg > get_mux_end()) || (mux_reg < IOMUXSW_MUX_CTL))
+		return ;
+	if (cfg == IOMUX_CONFIG_GPIO)
+		writel(PIN_TO_ALT_GPIO(pin), mux_reg);
+	else
+		writel(cfg, mux_reg);
+}
+
+/*
+ * Request ownership for an IO pin. This function has to be the first one
+ * being called before that pin is used. The caller has to check the
+ * return value to make sure it returns 0.
+ *
+ * @param  pin		a name defined by iomux_pin_name_t
+ * @param  cfg		an input function as defined in iomux_pin_cfg_t
+ *
+ */
+void mxc_request_iomux(iomux_pin_name_t pin, iomux_pin_cfg_t cfg)
+{
+	iomux_config_mux(pin, cfg);
+}
+
+/*
+ * Release ownership for an IO pin
+ *
+ * @param  pin		a name defined by iomux_pin_name_t
+ * @param  cfg		an input function as defined in iomux_pin_cfg_t
+ */
+void mxc_free_iomux(iomux_pin_name_t pin, iomux_pin_cfg_t cfg)
+{
+}
+
+/*
+ * This function configures the pad value for a IOMUX pin.
+ *
+ * @param  pin     a pin number as defined in iomux_pin_name_t
+ * @param  config  the ORed value of elements defined in iomux_pad_config_t
+ */
+void mxc_iomux_set_pad(iomux_pin_name_t pin, u32 config)
+{
+	u32 pad_reg = get_pad_reg(pin);
+	writel(config, pad_reg);
+}
+
+unsigned int mxc_iomux_get_pad(iomux_pin_name_t pin)
+{
+	u32 pad_reg = get_pad_reg(pin);
+	return readl(pad_reg);
+}
diff --git a/cpu/arm_cortexa8/mx51/lowlevel_init.S b/cpu/arm_cortexa8/mx51/lowlevel_init.S
new file mode 100644
index 0000000..700506e
--- /dev/null
+++ b/cpu/arm_cortexa8/mx51/lowlevel_init.S
@@ -0,0 +1,288 @@
+/*
+ * Copyright (C) 2007, Guennadi Liakhovetski <lg@denx.de>
+ *
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <config.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/asm-offsets.h>
+
+/*
+ * L2CC Cache setup/invalidation/disable
+ */
+.macro init_l2cc
+	/* explicitly disable L2 cache */
+	mrc 15, 0, r0, c1, c0, 1
+	bic r0, r0, #0x2
+	mcr 15, 0, r0, c1, c0, 1
+
+	/* reconfigure L2 cache aux control reg */
+	mov r0, #0xC0                   /* tag RAM */
+	add r0, r0, #0x4                /* data RAM */
+	orr r0, r0, #(1 << 24)          /* disable write allocate delay */
+	orr r0, r0, #(1 << 23)          /* disable write allocate combine */
+	orr r0, r0, #(1 << 22)          /* disable write allocate */
+
+	cmp r3, #0x10    /* r3 contains the silicon rev */
+
+	/* disable write combine for TO 2 and lower revs */
+	orrls r0, r0, #(1 << 25)
+
+	mcr 15, 1, r0, c9, c0, 2
+.endm /* init_l2cc */
+
+/* AIPS setup - Only setup MPROTx registers.
+ * The PACR default values are good.*/
+.macro init_aips
+	/*
+	 * Set all MPROTx to be non-bufferable, trusted for R/W,
+	 * not forced to user-mode.
+	 */
+	ldr r0, =AIPS1_BASE_ADDR
+	ldr r1, =0x77777777
+	str r1, [r0, #0x0]
+	str r1, [r0, #0x4]
+	ldr r0, =AIPS2_BASE_ADDR
+	str r1, [r0, #0x0]
+	str r1, [r0, #0x4]
+	/*
+	 * Clear the on and off peripheral modules Supervisor Protect bit
+	 * for SDMA to access them. Did not change the AIPS control registers
+	 * (offset 0x20) access type
+	 */
+.endm /* init_aips */
+
+/* M4IF setup */
+.macro init_m4if
+	/* VPU and IPU given higher priority (0x4)
+	 * IPU accesses with ID=0x1 given highest priority (=0xA)
+	 */
+	ldr r0, =M4IF_BASE_ADDR
+
+	ldr r1, =0x00000203
+	str r1, [r0, #0x40]
+
+	ldr r1, =0x0
+	str r1, [r0, #0x44]
+
+	ldr r1, =0x00120125
+	str r1, [r0, #0x9C]
+
+	ldr r1, =0x001901A3
+	str r1, [r0, #0x48]
+
+.endm /* init_m4if */
+
+.macro setup_pll pll, freq
+	ldr r2, =\pll
+	ldr r1, =0x00001232
+	str r1, [r2, #PLL_DP_CTL] /* Set DPLL ON (set UPEN bit): BRMO=1 */
+	mov r1, #0x2
+	str r1, [r2, #PLL_DP_CONFIG] /* Enable auto-restart AREN bit */
+
+	str r3, [r2, #PLL_DP_OP]
+	str r3, [r2, #PLL_DP_HFS_OP]
+
+	str r4, [r2, #PLL_DP_MFD]
+	str r4, [r2, #PLL_DP_HFS_MFD]
+
+	str r5, [r2, #PLL_DP_MFN]
+	str r5, [r2, #PLL_DP_HFS_MFN]
+
+	ldr r1, =0x00001232
+	str r1, [r2, #PLL_DP_CTL]
+1:	ldr r1, [r2, #PLL_DP_CTL]
+	ands r1, r1, #0x1
+	beq 1b
+.endm
+
+.macro init_clock
+	ldr r0, =CCM_BASE_ADDR
+
+	/* Gate of clocks to the peripherals first */
+	ldr r1, =0x3FFFFFFF
+	str r1, [r0, #CLKCTL_CCGR0]
+	ldr r1, =0x0
+	str r1, [r0, #CLKCTL_CCGR1]
+	str r1, [r0, #CLKCTL_CCGR2]
+	str r1, [r0, #CLKCTL_CCGR3]
+
+	ldr r1, =0x00030000
+	str r1, [r0, #CLKCTL_CCGR4]
+	ldr r1, =0x00FFF030
+	str r1, [r0, #CLKCTL_CCGR5]
+	ldr r1, =0x00000300
+	str r1, [r0, #CLKCTL_CCGR6]
+
+	/* Disable IPU and HSC dividers */
+	mov r1, #0x60000
+	str r1, [r0, #CLKCTL_CCDR]
+
+	/* Make sure to switch the DDR away from PLL 1 */
+	ldr r1, =0x19239145
+	str r1, [r0, #CLKCTL_CBCDR]
+	/* make sure divider effective */
+1:	ldr r1, [r0, #CLKCTL_CDHIPR]
+	cmp r1, #0x0
+	bne 1b
+
+	/* Switch ARM to step clock */
+	mov r1, #0x4
+	str r1, [r0, #CLKCTL_CCSR]
+	mov r3, #DP_OP_800
+	mov r4, #DP_MFD_800
+	mov r5, #DP_MFN_800
+	setup_pll PLL1_BASE_ADDR
+
+	mov r3, #DP_OP_665
+	mov r4, #DP_MFD_665
+	mov r5, #DP_MFN_665
+	setup_pll PLL3_BASE_ADDR
+
+	/* Switch peripheral to PLL 3 */
+	ldr r0, =CCM_BASE_ADDR
+        ldr r1, =0x000010C0
+	str r1, [r0, #CLKCTL_CBCMR]
+	ldr r1, =0x13239145
+	str r1, [r0, #CLKCTL_CBCDR]
+	mov r3, #DP_OP_665
+	mov r4, #DP_MFD_665
+	mov r5, #DP_MFN_665
+	setup_pll PLL2_BASE_ADDR
+
+	/* Switch peripheral to PLL2 */
+	ldr r0, =CCM_BASE_ADDR
+	ldr r1, =0x19239145
+	str r1, [r0, #CLKCTL_CBCDR]
+	ldr r1, =0x000020C0
+	str r1, [r0, #CLKCTL_CBCMR]
+
+	mov r3, #DP_OP_216
+	mov r4, #DP_MFD_216
+	mov r5, #DP_MFN_216
+	setup_pll PLL3_BASE_ADDR
+
+
+	/* Set the platform clock dividers */
+	ldr r0, =ARM_BASE_ADDR
+	ldr r1, =0x00000725
+	str r1, [r0, #0x14]
+
+	ldr r0, =CCM_BASE_ADDR
+
+	/* Run 3.0 at Full speed, for other TO's wait till we increase VDDGP */
+	ldr r1, =0x0
+	ldr r3, [r1, #ROM_SI_REV]
+	cmp r3, #0x10
+	movls r1, #0x1
+	movhi r1, #0
+	str r1, [r0, #CLKCTL_CACRR]
+
+	/* Switch ARM back to PLL 1 */
+	mov r1, #0
+	str r1, [r0, #CLKCTL_CCSR]
+
+	/* setup the rest */
+	/* Use lp_apm (24MHz) source for perclk */
+	ldr r1, =0x000020C2
+	str r1, [r0, #CLKCTL_CBCMR]
+	/* ddr clock from PLL 1, all perclk dividers are 1 since using 24MHz */
+	ldr r1, =0x59E35100
+	str r1, [r0, #CLKCTL_CBCDR]
+
+	/* Restore the default values in the Gate registers */
+	ldr r1, =0xFFFFFFFF
+	str r1, [r0, #CLKCTL_CCGR0]
+	str r1, [r0, #CLKCTL_CCGR1]
+	str r1, [r0, #CLKCTL_CCGR2]
+	str r1, [r0, #CLKCTL_CCGR3]
+	str r1, [r0, #CLKCTL_CCGR4]
+	str r1, [r0, #CLKCTL_CCGR5]
+	str r1, [r0, #CLKCTL_CCGR6]
+
+	/* Use PLL 2 for UART's, get 66.5MHz from it */
+	ldr r1, =0xA5A2A020
+	str r1, [r0, #CLKCTL_CSCMR1]
+	ldr r1, =0x00C30321
+	str r1, [r0, #CLKCTL_CSCDR1]
+
+	/* make sure divider effective */
+1:	ldr r1, [r0, #CLKCTL_CDHIPR]
+	cmp r1, #0x0
+	bne 1b
+
+	mov r1, #0x0
+	str r1, [r0, #CLKCTL_CCDR]
+
+	/* for cko - for ARM div by 8 */
+	mov r1, #0x000A0000
+	add r1, r1, #0x00000F0
+	str r1, [r0, #CLKCTL_CCOSR]
+.endm
+
+.macro setup_wdog
+	ldr r0, =WDOG1_BASE_ADDR
+	mov r1, #0x30
+	strh r1, [r0]
+.endm
+
+.section ".text.init", "x"
+
+.globl lowlevel_init
+lowlevel_init:
+	ldr r0, =GPIO1_BASE_ADDR
+	ldr r1, [r0, #0x0]
+	orr r1, r1, #(1 << 23)
+	str r1, [r0, #0x0]
+	ldr r1, [r0, #0x4]
+	orr r1, r1, #(1 << 23)
+	str r1, [r0, #0x4]
+
+#ifdef ENABLE_IMPRECISE_ABORT
+	mrs r1, spsr            /* save old spsr */
+	mrs r0, cpsr            /* read out the cpsr */
+	bic r0, r0, #0x100      /* clear the A bit */
+	msr spsr, r0            /* update spsr */
+	add lr, pc, #0x8        /* update lr */
+	movs pc, lr             /* update cpsr */
+	nop
+	nop
+	nop
+	nop
+	msr spsr, r1            /* restore old spsr */
+#endif
+
+	init_l2cc
+
+	init_aips
+
+	init_m4if
+
+	init_clock
+
+	/* r12 saved upper lr*/
+	mov pc,lr
+
+/* Board level setting value */
+DDR_PERCHARGE_CMD:	.word 0x04008008
+DDR_REFRESH_CMD:	.word 0x00008010
+DDR_LMR1_W:		.word 0x00338018
+DDR_LMR_CMD:		.word 0xB2220000
+DDR_TIMING_W:		.word 0xB02567A9
+DDR_MISC_W:		.word 0x000A0104
diff --git a/cpu/arm_cortexa8/mx51/soc.c b/cpu/arm_cortexa8/mx51/soc.c
new file mode 100644
index 0000000..2a139b2
--- /dev/null
+++ b/cpu/arm_cortexa8/mx51/soc.c
@@ -0,0 +1,114 @@
+/*
+ * (C) Copyright 2007
+ * Sascha Hauer, Pengutronix
+ *
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/clock.h>
+#include <asm/errno.h>
+#include <asm/io.h>
+
+#ifdef CONFIG_FSL_ESDHC
+#include <fsl_esdhc.h>
+#endif
+
+u32 get_cpu_rev(void)
+{
+	int reg;
+	int system_rev;
+
+	reg = __raw_readl(ROM_SI_REV);
+	switch (reg) {
+	case 0x02:
+		system_rev = 0x51000 | CHIP_REV_1_1;
+		break;
+	case 0x10:
+		if ((__raw_readl(GPIO1_BASE_ADDR + 0x0) & (0x1 << 22)) == 0)
+			system_rev = 0x51000 | CHIP_REV_2_5;
+		else
+			system_rev = 0x51000 | CHIP_REV_2_0;
+		break;
+	case 0x20:
+		system_rev = 0x51000 | CHIP_REV_3_0;
+		break;
+	return system_rev;
+	default:
+		system_rev = 0x51000 | CHIP_REV_1_0;
+		break;
+	}
+	return system_rev;
+}
+
+
+#if defined(CONFIG_DISPLAY_CPUINFO)
+int print_cpuinfo(void)
+{
+	u32 cpurev;
+
+	cpurev = get_cpu_rev();
+	printf("CPU:   Freescale i.MX51 family %d.%dV at %d MHz\n",
+		(cpurev & 0xF0) >> 4,
+		(cpurev & 0x0F) >> 4,
+		mxc_get_clock(MXC_ARM_CLK) / 1000000);
+	return 0;
+}
+#endif
+
+/*
+ * Initializes on-chip ethernet controllers.
+ * to override, implement board_eth_init()
+ */
+#if defined(CONFIG_FEC_MXC)
+extern int fecmxc_initialize(bd_t *bis);
+#endif
+
+int cpu_eth_init(bd_t *bis)
+{
+	int rc = -ENODEV;
+
+#if defined(CONFIG_FEC_MXC)
+	rc = fecmxc_initialize(bis);
+#endif
+
+	return rc;
+}
+
+/*
+ * Initializes on-chip MMC controllers.
+ * to override, implement board_mmc_init()
+ */
+int cpu_mmc_init(bd_t *bis)
+{
+#ifdef CONFIG_FSL_ESDHC
+	return fsl_esdhc_mmc_init(bis);
+#else
+	return 0;
+#endif
+}
+
+
+void reset_cpu(ulong addr)
+{
+	__raw_writew(4, WDOG1_BASE_ADDR);
+}
diff --git a/cpu/arm_cortexa8/mx51/speed.c b/cpu/arm_cortexa8/mx51/speed.c
new file mode 100644
index 0000000..a444def
--- /dev/null
+++ b/cpu/arm_cortexa8/mx51/speed.c
@@ -0,0 +1,39 @@
+/*
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 2004-2007 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/clock.h>
+
+int get_clocks(void)
+{
+	DECLARE_GLOBAL_DATA_PTR;
+
+#ifdef CONFIG_FSL_ESDHC
+	gd->sdhc_clk = mxc_get_clock(MXC_IPG_PERCLK);
+#endif
+	return 0;
+}
diff --git a/cpu/arm_cortexa8/mx51/timer.c b/cpu/arm_cortexa8/mx51/timer.c
new file mode 100644
index 0000000..8ecfec6
--- /dev/null
+++ b/cpu/arm_cortexa8/mx51/timer.c
@@ -0,0 +1,119 @@
+/*
+ * (C) Copyright 2007
+ * Sascha Hauer, Pengutronix
+ *
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/imx-regs.h>
+
+/* General purpose timers registers */
+struct mxc_gpt {
+	unsigned int control;
+	unsigned int prescaler;
+	unsigned int status;
+	unsigned int nouse[6];
+	unsigned int counter;
+};
+
+static struct mxc_gpt *cur_gpt = (struct mxc_gpt *)GPT1_BASE_ADDR;
+
+/* General purpose timers bitfields */
+#define GPTCR_SWR       (1<<15)	/* Software reset */
+#define GPTCR_FRR       (1<<9)	/* Freerun / restart */
+#define GPTCR_CLKSOURCE_32 (4<<6)	/* Clock source */
+#define GPTCR_TEN       (1)	/* Timer enable */
+
+static ulong timestamp;
+static ulong lastinc;
+
+int timer_init(void)
+{
+	int i;
+
+	/* setup GP Timer 1 */
+	__raw_writel(GPTCR_SWR, &cur_gpt->control);
+
+	/* We have no udelay by now */
+	for (i = 0; i < 100; i++)
+		__raw_writel(0, &cur_gpt->control);
+
+	__raw_writel(0, &cur_gpt->prescaler); /* 32Khz */
+
+	/* Freerun Mode, PERCLK1 input */
+	i = __raw_readl(&cur_gpt->control);
+	__raw_writel(i | GPTCR_CLKSOURCE_32 | GPTCR_TEN, &cur_gpt->control);
+	reset_timer_masked();
+	return 0;
+}
+
+void reset_timer(void)
+{
+	reset_timer_masked();
+}
+
+void reset_timer_masked(void)
+{
+	ulong val = __raw_readl(&cur_gpt->counter);
+	lastinc = val / (CONFIG_MX51_CLK32 / CONFIG_SYS_HZ);
+	timestamp = 0;
+}
+
+ulong get_timer_masked(void)
+{
+	ulong val = __raw_readl(&cur_gpt->counter);
+	val /= (CONFIG_MX51_CLK32 / CONFIG_SYS_HZ);
+	if (val >= lastinc)
+		timestamp += (val - lastinc);
+	else
+		timestamp += ((0xFFFFFFFF / (CONFIG_MX51_CLK32 / CONFIG_SYS_HZ))
+				- lastinc) + val;
+	lastinc = val;
+	return val;
+}
+
+ulong get_timer(ulong base)
+{
+	return get_timer_masked() - base;
+}
+
+void set_timer(ulong t)
+{
+	timestamp = t;
+}
+
+/* delay x useconds AND perserve advance timstamp value */
+void __udelay(unsigned long usec)
+{
+	unsigned long now, start, tmo;
+	tmo = usec * (CONFIG_MX51_CLK32 / 1000) / 1000;
+
+	if (!tmo)
+		tmo = 1;
+
+	now = start = readl(&cur_gpt->counter);
+
+	while ((now - start) < tmo)
+		now = readl(&cur_gpt->counter);
+
+}
diff --git a/cpu/arm_cortexa8/mx51/u-boot.lds b/cpu/arm_cortexa8/mx51/u-boot.lds
new file mode 100644
index 0000000..84c173a
--- /dev/null
+++ b/cpu/arm_cortexa8/mx51/u-boot.lds
@@ -0,0 +1,61 @@
+/*
+ * January 2004 - Changed to support H4 device
+ * Copyright (c) 2004 Texas Instruments
+ *
+ * (C) Copyright 2002
+ * Gary Jennejohn, DENX Software Engineering, <gj@denx.de>
+ *
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+OUTPUT_FORMAT("elf32-littlearm", "elf32-littlearm", "elf32-littlearm")
+OUTPUT_ARCH(arm)
+ENTRY(_start)
+SECTIONS
+{
+	. = 0x00000000;
+
+	. = ALIGN(4);
+	.text	   :
+	{
+	  cpu/arm_cortexa8/start.o
+	  *(.text)
+	}
+
+	. = ALIGN(4);
+	.rodata : { *(.rodata) }
+
+	. = ALIGN(4);
+	.data : { *(.data) }
+
+	. = ALIGN(4);
+	.got : { *(.got) }
+
+	. = .;
+	__u_boot_cmd_start = .;
+	.u_boot_cmd : { *(.u_boot_cmd) }
+	__u_boot_cmd_end = .;
+
+	. = ALIGN(4);
+	__bss_start = .;
+	.bss : { *(.bss) }
+	_end = .;
+}
diff --git a/cpu/arm_cortexa8/omap3/board.c b/cpu/arm_cortexa8/omap3/board.c
index 2aa69b3..7b78fa4 100644
--- a/cpu/arm_cortexa8/omap3/board.c
+++ b/cpu/arm_cortexa8/omap3/board.c
@@ -146,6 +146,12 @@ void setup_auxcr()
 	__asm__ __volatile__("orr r0, r0, #1 << 5");
 	/* SMI instruction to call ROM Code API */
 	__asm__ __volatile__(".word 0xE1600070");
+	/* Set PLD_FWD bit in L2AUXCR (Cortex-A8 erratum 725233 workaround) */
+	__asm__ __volatile__("mov r12, #0x2");
+	__asm__ __volatile__("mrc p15, 1, r0, c9, c0, 2");
+	__asm__ __volatile__("orr r0, r0, #1 << 27");
+	/* SMI instruction to call ROM Code API */
+	__asm__ __volatile__(".word 0xE1600070");
 	__asm__ __volatile__("mov r0, %0":"=r"(i));
 	__asm__ __volatile__("mov r12, %0":"=r"(j));
 }
diff --git a/cpu/arm_cortexa8/omap3/clock.c b/cpu/arm_cortexa8/omap3/clock.c
index 174c453..6330c9e 100644
--- a/cpu/arm_cortexa8/omap3/clock.c
+++ b/cpu/arm_cortexa8/omap3/clock.c
@@ -40,7 +40,7 @@
  *****************************************************************************/
 u32 get_osc_clk_speed(void)
 {
-	u32 start, cstart, cend, cdiff, val;
+	u32 start, cstart, cend, cdiff, cdiv, val;
 	struct prcm *prcm_base = (struct prcm *)PRCM_BASE;
 	struct prm *prm_base = (struct prm *)PRM_BASE;
 	struct gptimer *gpt1_base = (struct gptimer *)OMAP34XX_GPT1;
@@ -48,9 +48,15 @@ u32 get_osc_clk_speed(void)
 
 	val = readl(&prm_base->clksrc_ctrl);
 
-	/* If SYS_CLK is being divided by 2, remove for now */
-	val = (val & (~SYSCLKDIV_2)) | SYSCLKDIV_1;
-	writel(val, &prm_base->clksrc_ctrl);
+	if (val & SYSCLKDIV_2)
+		cdiv = 2;
+	else if (val & SYSCLKDIV_1)
+		cdiv = 1;
+	else
+		/*
+		 * Should never reach here! (Assume divider as 1)
+		 */
+		cdiv = 1;
 
 	/* enable timer2 */
 	val = readl(&prcm_base->clksel_wkup) | CLKSEL_GPT1;
@@ -61,6 +67,7 @@ u32 get_osc_clk_speed(void)
 	/* Enable I and F Clocks for GPT1 */
 	val = readl(&prcm_base->iclken_wkup) | EN_GPT1 | EN_32KSYNC;
 	writel(val, &prcm_base->iclken_wkup);
+
 	val = readl(&prcm_base->fclken_wkup) | EN_GPT1;
 	writel(val, &prcm_base->fclken_wkup);
 
@@ -83,6 +90,11 @@ u32 get_osc_clk_speed(void)
 	cend = readl(&gpt1_base->tcrr);		/* get end sys_clk count */
 	cdiff = cend - cstart;			/* get elapsed ticks */
 
+	if (cdiv == 2)
+	{
+		cdiff *= 2;
+	}
+
 	/* based on number of ticks assign speed */
 	if (cdiff > 19000)
 		return S38_4M;
diff --git a/cpu/mcf52x2/cpu.c b/cpu/mcf52x2/cpu.c
index 2cfe631..c4c5d50 100644
--- a/cpu/mcf52x2/cpu.c
+++ b/cpu/mcf52x2/cpu.c
@@ -33,6 +33,7 @@
 #include <command.h>
 #include <asm/immap.h>
 #include <netdev.h>
+#include "cpu.h"
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -143,6 +144,11 @@ int checkcpu(void)
 
 int do_reset(cmd_tbl_t * cmdtp, bd_t * bd, int flag, int argc, char *argv[])
 {
+	/* Call the board specific reset actions first. */
+	if(board_reset) {
+		board_reset();
+	}
+
 	mbar_writeByte(MCF_RCM_RCR,
 		       MCF_RCM_RCR_SOFTRST | MCF_RCM_RCR_FRCRSTOUT);
 	return 0;
diff --git a/cpu/mcf52x2/cpu.h b/cpu/mcf52x2/cpu.h
new file mode 100644
index 0000000..c1227eb
--- /dev/null
+++ b/cpu/mcf52x2/cpu.h
@@ -0,0 +1,33 @@
+/*
+ *  cpu.h
+ *
+ *  Copyright (c) 2009 Freescale Semiconductor, Inc.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ * MA 02110-1301 USA
+ */
+
+#ifndef _CPU_H_
+#define _CPU_H_
+
+#include <command.h>
+
+/* Use this to create board specific reset functions */
+void board_reset(void) __attribute__((__weak__));
+
+#endif /* _CPU_H_ */
diff --git a/cpu/mcf532x/speed.c b/cpu/mcf532x/speed.c
index 0d378e6..67f08c7 100644
--- a/cpu/mcf532x/speed.c
+++ b/cpu/mcf532x/speed.c
@@ -204,6 +204,8 @@ int clock_pll(int fsys, int flags)
 	fout = ((fref * mfd) / (BUSDIV * 4));
 #endif
 
+/* must not tamper with SDRAMC if running from SDRAM */
+#if !defined(CONFIG_MONITOR_IS_IN_RAM)
 	/*
 	 * Check to see if the SDRAM has already been initialized.
 	 * If it has then the SDRAM needs to be put into self refresh
@@ -254,6 +256,7 @@ int clock_pll(int fsys, int flags)
 
 	/* wait for DQS logic to relock */
 	for (i = 0; i < 0x200; i++) ;
+#endif /* !defined(CONFIG_MONITOR_IS_IN_RAM) */
 
 	return fout;
 }
diff --git a/cpu/mcf532x/start.S b/cpu/mcf532x/start.S
index a46c47a..5b134aa 100644
--- a/cpu/mcf532x/start.S
+++ b/cpu/mcf532x/start.S
@@ -45,6 +45,7 @@
 	addl	#60,%sp;		/* space for 15 regs */ \
 	rte;
 
+#if !defined(CONFIG_MONITOR_IS_IN_RAM)
 .text
 /*
  *	Vector table. This is used for initial platform startup.
@@ -121,6 +122,7 @@ vector192_255:
 .long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
 .long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
 .long	_FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT, _FAULT
+#endif /* !defined(CONFIG_MONITOR_IS_IN_RAM) */
 
 	.text
 
@@ -130,9 +132,11 @@ _start:
 	nop
 	move.w #0x2700,%sr	/* Mask off Interrupt */
 
+#if !defined(CONFIG_MONITOR_IS_IN_RAM)
 	/* Set vector base register at the beginning of the Flash */
 	move.l	#CONFIG_SYS_FLASH_BASE, %d0
 	movec	%d0, %VBR
+#endif
 
 	move.l	#(CONFIG_SYS_INIT_RAM_ADDR + CONFIG_SYS_INIT_RAM_CTRL), %d0
 	movec	%d0, %RAMBAR1
@@ -280,7 +284,7 @@ _int_handler:
 icache_enable:
 	move.l	#0x01000000, %d0		/* Invalidate cache cmd */
 	movec	%d0, %CACR			/* Invalidate cache */
-	move.l	#(CONFIG_SYS_SDRAM_BASE + 0x1c000), %d0
+	move.l  #(CONFIG_SYS_SDRAM_BASE + 0xc000 + ((CONFIG_SYS_SDRAM_SIZE & 0x1fe0) << 11)), %d0
 	movec	%d0, %ACR0			/* Enable cache */
 
 	move.l	#0x80000200, %d0		/* Setup cache mask */
diff --git a/cpu/mpc512x/traps.c b/cpu/mpc512x/traps.c
index 8000fab..786f4a5 100644
--- a/cpu/mpc512x/traps.c
+++ b/cpu/mpc512x/traps.c
@@ -28,6 +28,7 @@
  */
 
 #include <common.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 
 DECLARE_GLOBAL_DATA_PTR;
diff --git a/cpu/mpc5xx/traps.c b/cpu/mpc5xx/traps.c
index cc8e091..6882c21 100644
--- a/cpu/mpc5xx/traps.c
+++ b/cpu/mpc5xx/traps.c
@@ -34,6 +34,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 
 #if defined(CONFIG_CMD_BEDBUG)
diff --git a/cpu/mpc5xxx/cpu_init.c b/cpu/mpc5xxx/cpu_init.c
index b151464..560c9b3 100644
--- a/cpu/mpc5xxx/cpu_init.c
+++ b/cpu/mpc5xxx/cpu_init.c
@@ -40,15 +40,20 @@ void cpu_init_f (void)
 		(struct mpc5xxx_mmap_ctl *) CONFIG_SYS_MBAR;
 	volatile struct mpc5xxx_lpb *lpb =
 		(struct mpc5xxx_lpb *) MPC5XXX_LPB;
-	volatile struct mpc5xxx_cdm *cdm =
-		(struct mpc5xxx_cdm *) MPC5XXX_CDM;
 	volatile struct mpc5xxx_gpio *gpio =
 		(struct mpc5xxx_gpio *) MPC5XXX_GPIO;
 	volatile struct mpc5xxx_xlb *xlb =
 		(struct mpc5xxx_xlb *) MPC5XXX_XLBARB;
+#if defined(CONFIG_SYS_IPBCLK_EQUALS_XLBCLK)
+	volatile struct mpc5xxx_cdm *cdm =
+		(struct mpc5xxx_cdm *) MPC5XXX_CDM;
+#endif	/* CONFIG_SYS_IPBCLK_EQUALS_XLBCLK */
+#if defined(CONFIG_WATCHDOG)
 	volatile struct mpc5xxx_gpt *gpt0 =
 		(struct mpc5xxx_gpt *) MPC5XXX_GPT;
+#endif /* CONFIG_WATCHDOG */
 	unsigned long addecr = (1 << 25); /* Boot_CS */
+
 #if defined(CONFIG_SYS_RAMBOOT) && defined(CONFIG_MGT5100)
 	addecr |= (1 << 22); /* SDRAM enable */
 #endif
@@ -184,11 +189,11 @@ void cpu_init_f (void)
 
 # if defined(CONFIG_SYS_IPBCLK_EQUALS_XLBCLK)
 	/* Motorola reports IPB should better run at 133 MHz. */
-#if defined(CONFIG_MGT5100)
+#  if defined(CONFIG_MGT5100)
 	setbits_be32(&mm->addecr, 1);
-#elif defined(CONFIG_MPC5200)
+#  elif defined(CONFIG_MPC5200)
 	setbits_be32(&mm->ipbi_ws_ctrl, 1);
-#endif
+#  endif
 	/* pci_clk_sel = 0x02, ipb_clk_sel = 0x00; */
 	addecr = in_be32(&cdm->cfg);
 	addecr &= ~0x103;
diff --git a/cpu/mpc5xxx/traps.c b/cpu/mpc5xxx/traps.c
index 2a09153..934a2f2 100644
--- a/cpu/mpc5xxx/traps.c
+++ b/cpu/mpc5xxx/traps.c
@@ -35,6 +35,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 
 /* Returns 0 if exception not found and fixup otherwise.  */
diff --git a/cpu/mpc8220/traps.c b/cpu/mpc8220/traps.c
index f98d40f..4565780 100644
--- a/cpu/mpc8220/traps.c
+++ b/cpu/mpc8220/traps.c
@@ -35,6 +35,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 
 /* Returns 0 if exception not found and fixup otherwise.  */
diff --git a/cpu/mpc8260/traps.c b/cpu/mpc8260/traps.c
index f9f4dea..d216f71 100644
--- a/cpu/mpc8260/traps.c
+++ b/cpu/mpc8260/traps.c
@@ -34,6 +34,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 #include <asm/m8260_pci.h>
 
diff --git a/cpu/mpc83xx/cpu.c b/cpu/mpc83xx/cpu.c
index e38a372..51180d6 100644
--- a/cpu/mpc83xx/cpu.c
+++ b/cpu/mpc83xx/cpu.c
@@ -199,7 +199,10 @@ do_reset (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
 
 	volatile immap_t *immap = (immap_t *) CONFIG_SYS_IMMR;
 
+	puts("Resetting the board.\n");
+
 #ifdef MPC83xx_RESET
+
 	/* Interrupts and MMU off */
 	__asm__ __volatile__ ("mfmsr    %0":"=r" (msr):);
 
@@ -214,9 +217,6 @@ do_reset (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
 	/* confirm Reset Control Reg is enabled */
 	while(!((immap->reset.rcer) & RCER_CRE));
 
-	printf("Resetting the board.");
-	printf("\n");
-
 	udelay(200);
 
 	/* perform reset, only one bit */
@@ -238,8 +238,6 @@ do_reset (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
 	 */
 	addr = CONFIG_SYS_RESET_ADDRESS;
 
-	printf("resetting the board.");
-	printf("\n");
 	((void (*)(void)) addr) ();
 #endif	/* MPC83xx_RESET */
 
diff --git a/cpu/mpc83xx/traps.c b/cpu/mpc83xx/traps.c
index 3b09a62..9d71b8b 100644
--- a/cpu/mpc83xx/traps.c
+++ b/cpu/mpc83xx/traps.c
@@ -27,6 +27,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 #include <asm/mpc8349_pci.h>
 
diff --git a/cpu/mpc85xx/traps.c b/cpu/mpc85xx/traps.c
index 241ebd5..10108d8 100644
--- a/cpu/mpc85xx/traps.c
+++ b/cpu/mpc85xx/traps.c
@@ -38,6 +38,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 
 DECLARE_GLOBAL_DATA_PTR;
diff --git a/cpu/mpc86xx/traps.c b/cpu/mpc86xx/traps.c
index ad005c3..406403e 100644
--- a/cpu/mpc86xx/traps.c
+++ b/cpu/mpc86xx/traps.c
@@ -32,6 +32,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 
 DECLARE_GLOBAL_DATA_PTR;
diff --git a/cpu/mpc8xx/traps.c b/cpu/mpc8xx/traps.c
index f357c8d..6efe8e0 100644
--- a/cpu/mpc8xx/traps.c
+++ b/cpu/mpc8xx/traps.c
@@ -34,6 +34,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 
 #if defined(CONFIG_CMD_BEDBUG)
diff --git a/cpu/ppc4xx/traps.c b/cpu/ppc4xx/traps.c
index cb35faf..42e4221 100644
--- a/cpu/ppc4xx/traps.c
+++ b/cpu/ppc4xx/traps.c
@@ -34,6 +34,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <kgdb.h>
 #include <asm/processor.h>
 
 DECLARE_GLOBAL_DATA_PTR;