Merge commit 'wd/master'

13 files changed, 303 insertions, 248 deletions

diff --git a/cpu/at32ap/Makefile b/cpu/at32ap/Makefile
index f69b1f3..d16c58b 100644
--- a/cpu/at32ap/Makefile
+++ b/cpu/at32ap/Makefile
@@ -27,13 +27,19 @@ include $(TOPDIR)/config.mk
 
 LIB	:= $(obj)lib$(CPU).a
 
-START	:= start.o
-SOBJS	:= entry.o
-COBJS	:= cpu.o hsdramc.o exception.o cache.o
-COBJS	+= interrupts.o pio.o atmel_mci.o
-SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
-OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
-START	:= $(addprefix $(obj),$(START))
+START-y			+= start.o
+
+COBJS-y			+= cpu.o
+COBJS-y			+= hsdramc.o
+COBJS-y			+= exception.o
+COBJS-y			+= cache.o
+COBJS-y			+= interrupts.o
+COBJS-y			+= pio.o
+COBJS-$(CONFIG_MMC)	+= atmel_mci.o
+
+SRCS	:= $(START-y:.o=.S) $(SOBJS-y:.o=.S) $(COBJS-y:.o=.c)
+OBJS	:= $(addprefix $(obj),$(SOBJS-y) $(COBJS-y))
+START	:= $(addprefix $(obj),$(START-y))
 
 all: $(obj).depend $(START) $(LIB)
 
diff --git a/cpu/at32ap/at32ap700x/Makefile b/cpu/at32ap/at32ap700x/Makefile
index d276712..7404235 100644
--- a/cpu/at32ap/at32ap700x/Makefile
+++ b/cpu/at32ap/at32ap700x/Makefile
@@ -24,7 +24,7 @@ include $(TOPDIR)/config.mk
 
 LIB	:= $(obj)lib$(SOC).a
 
-COBJS	:= gpio.o
+COBJS	:= gpio.o clk.o
 SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
 
diff --git a/cpu/at32ap/at32ap700x/clk.c b/cpu/at32ap/at32ap700x/clk.c
new file mode 100644
index 0000000..b3aa034
--- /dev/null
+++ b/cpu/at32ap/at32ap700x/clk.c
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2005-2008 Atmel Corporation
+ *
+ * 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/clk.h>
+#include <asm/arch/memory-map.h>
+
+#include "sm.h"
+
+void clk_init(void)
+{
+	uint32_t cksel;
+
+	/* in case of soft resets, disable watchdog */
+	sm_writel(WDT_CTRL, SM_BF(KEY, 0x55));
+	sm_writel(WDT_CTRL, SM_BF(KEY, 0xaa));
+
+#ifdef CONFIG_PLL
+	/* Initialize the PLL */
+	sm_writel(PM_PLL0, (SM_BF(PLLCOUNT, CFG_PLL0_SUPPRESS_CYCLES)
+			    | SM_BF(PLLMUL, CFG_PLL0_MUL - 1)
+			    | SM_BF(PLLDIV, CFG_PLL0_DIV - 1)
+			    | SM_BF(PLLOPT, CFG_PLL0_OPT)
+			    | SM_BF(PLLOSC, 0)
+			    | SM_BIT(PLLEN)));
+
+	/* Wait for lock */
+	while (!(sm_readl(PM_ISR) & SM_BIT(LOCK0))) ;
+#endif
+
+	/* Set up clocks for the CPU and all peripheral buses */
+	cksel = 0;
+	if (CFG_CLKDIV_CPU)
+		cksel |= SM_BIT(CPUDIV) | SM_BF(CPUSEL, CFG_CLKDIV_CPU - 1);
+	if (CFG_CLKDIV_HSB)
+		cksel |= SM_BIT(HSBDIV) | SM_BF(HSBSEL, CFG_CLKDIV_HSB - 1);
+	if (CFG_CLKDIV_PBA)
+		cksel |= SM_BIT(PBADIV) | SM_BF(PBASEL, CFG_CLKDIV_PBA - 1);
+	if (CFG_CLKDIV_PBB)
+		cksel |= SM_BIT(PBBDIV) | SM_BF(PBBSEL, CFG_CLKDIV_PBB - 1);
+	sm_writel(PM_CKSEL, cksel);
+
+#ifdef CONFIG_PLL
+	/* Use PLL0 as main clock */
+	sm_writel(PM_MCCTRL, SM_BIT(PLLSEL));
+#endif
+}
diff --git a/cpu/at32ap/at32ap700x/gpio.c b/cpu/at32ap/at32ap700x/gpio.c
index 859124a..3da35d4 100644
--- a/cpu/at32ap/at32ap700x/gpio.c
+++ b/cpu/at32ap/at32ap700x/gpio.c
@@ -21,8 +21,11 @@
  */
 #include <common.h>
 
+#include <asm/io.h>
+
 #include <asm/arch/chip-features.h>
 #include <asm/arch/gpio.h>
+#include <asm/arch/memory-map.h>
 
 /*
  * Lots of small functions here. We depend on --gc-sections getting
@@ -142,3 +145,43 @@ void gpio_enable_mmci(void)
 	gpio_select_periph_A(GPIO_PIN_PA15, 0);	/* DATA3 */
 }
 #endif
+
+#ifdef AT32AP700x_CHIP_HAS_SPI
+void gpio_enable_spi0(unsigned long cs_mask)
+{
+	u32 pa_mask = 0;
+
+	gpio_select_periph_A(GPIO_PIN_PA0,  0);	/* MISO	*/
+	gpio_select_periph_A(GPIO_PIN_PA1,  0);	/* MOSI	*/
+	gpio_select_periph_A(GPIO_PIN_PA2,  0);	/* SCK	*/
+
+	if (cs_mask & (1 << 0))
+		pa_mask |= 1 << 3;	/* NPCS0 */
+	if (cs_mask & (1 << 1))
+		pa_mask |= 1 << 4;	/* NPCS1 */
+	if (cs_mask & (1 << 2))
+		pa_mask |= 1 << 5;	/* NPCS2 */
+	if (cs_mask & (1 << 3))
+		pa_mask |= 1 << 20;	/* NPCS3 */
+
+	__raw_writel(pa_mask, PIOA_BASE + 0x00);
+	__raw_writel(pa_mask, PIOA_BASE + 0x30);
+	__raw_writel(pa_mask, PIOA_BASE + 0x10);
+}
+
+void gpio_enable_spi1(unsigned long cs_mask)
+{
+	gpio_select_periph_B(GPIO_PIN_PA0,  0);	/* MISO	*/
+	gpio_select_periph_B(GPIO_PIN_PB1,  0);	/* MOSI	*/
+	gpio_select_periph_B(GPIO_PIN_PB5,  0);	/* SCK	*/
+
+	if (cs_mask & (1 << 0))
+		gpio_select_periph_B(GPIO_PIN_PB2,  0);	/* NPCS0 */
+	if (cs_mask & (1 << 1))
+		gpio_select_periph_B(GPIO_PIN_PB3,  0);	/* NPCS1 */
+	if (cs_mask & (1 << 2))
+		gpio_select_periph_B(GPIO_PIN_PB4,  0);	/* NPCS2 */
+	if (cs_mask & (1 << 3))
+		gpio_select_periph_A(GPIO_PIN_PA27, 0);	/* NPCS3 */
+}
+#endif
diff --git a/cpu/at32ap/sm.h b/cpu/at32ap/at32ap700x/sm.h
index 6492c8e..6492c8e 100644
--- a/cpu/at32ap/sm.h
+++ b/cpu/at32ap/at32ap700x/sm.h
diff --git a/cpu/at32ap/atmel_mci.c b/cpu/at32ap/atmel_mci.c
index f59dfb5..3795add 100644
--- a/cpu/at32ap/atmel_mci.c
+++ b/cpu/at32ap/atmel_mci.c
@@ -21,8 +21,6 @@
  */
 #include <common.h>
 
-#ifdef CONFIG_MMC
-
 #include <part.h>
 #include <mmc.h>
 
@@ -139,7 +137,7 @@ mmc_cmd(unsigned long cmd, unsigned long arg,
 
 	pr_debug("mmc: status 0x%08lx\n", status);
 
-	if (status & ERROR_FLAGS) {
+	if (status & error_flags) {
 		printf("mmc: command %lu failed (status: 0x%08lx)\n",
 		       cmd, status);
 		return -EIO;
@@ -182,12 +180,13 @@ static int mmc_acmd(unsigned long cmd, unsigned long arg,
 
 static unsigned long
 mmc_bread(int dev, unsigned long start, lbaint_t blkcnt,
-	  unsigned long *buffer)
+	  void *buffer)
 {
 	int ret, i = 0;
 	unsigned long resp[4];
 	unsigned long card_status, data;
 	unsigned long wordcount;
+	u32 *p = buffer;
 	u32 status;
 
 	if (blkcnt == 0)
@@ -225,7 +224,7 @@ mmc_bread(int dev, unsigned long start, lbaint_t blkcnt,
 			if (status & MMCI_BIT(RXRDY)) {
 				data = mmci_readl(RDR);
 				/* pr_debug("%x\n", data); */
-				*buffer++ = data;
+				*p++ = data;
 				wordcount++;
 			}
 		} while(wordcount < (mmc_blkdev.blksz / 4));
@@ -443,6 +442,7 @@ static void mci_set_data_timeout(struct mmc_csd *csd)
 
 	dtocyc = timeout_clks;
 	dtomul = 0;
+	shift = 0;
 	while (dtocyc > 15 && dtomul < 8) {
 		dtomul++;
 		shift = dtomul_to_shift[dtomul];
@@ -546,5 +546,3 @@ int mmc2info(ulong addr)
 {
 	return 0;
 }
-
-#endif /* CONFIG_MMC */
diff --git a/cpu/at32ap/cpu.c b/cpu/at32ap/cpu.c
index 311466b..0ba8361 100644
--- a/cpu/at32ap/cpu.c
+++ b/cpu/at32ap/cpu.c
@@ -30,7 +30,6 @@
 #include <asm/arch/memory-map.h>
 
 #include "hsmc3.h"
-#include "sm.h"
 
 /* Sanity checks */
 #if (CFG_CLKDIV_CPU > CFG_CLKDIV_HSB)		\
@@ -44,47 +43,9 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
-static void pm_init(void)
-{
-	uint32_t cksel;
-
-#ifdef CONFIG_PLL
-	/* Initialize the PLL */
-	sm_writel(PM_PLL0, (SM_BF(PLLCOUNT, CFG_PLL0_SUPPRESS_CYCLES)
-			    | SM_BF(PLLMUL, CFG_PLL0_MUL - 1)
-			    | SM_BF(PLLDIV, CFG_PLL0_DIV - 1)
-			    | SM_BF(PLLOPT, CFG_PLL0_OPT)
-			    | SM_BF(PLLOSC, 0)
-			    | SM_BIT(PLLEN)));
-
-	/* Wait for lock */
-	while (!(sm_readl(PM_ISR) & SM_BIT(LOCK0))) ;
-#endif
-
-	/* Set up clocks for the CPU and all peripheral buses */
-	cksel = 0;
-	if (CFG_CLKDIV_CPU)
-		cksel |= SM_BIT(CPUDIV) | SM_BF(CPUSEL, CFG_CLKDIV_CPU - 1);
-	if (CFG_CLKDIV_HSB)
-		cksel |= SM_BIT(HSBDIV) | SM_BF(HSBSEL, CFG_CLKDIV_HSB - 1);
-	if (CFG_CLKDIV_PBA)
-		cksel |= SM_BIT(PBADIV) | SM_BF(PBASEL, CFG_CLKDIV_PBA - 1);
-	if (CFG_CLKDIV_PBB)
-		cksel |= SM_BIT(PBBDIV) | SM_BF(PBBSEL, CFG_CLKDIV_PBB - 1);
-	sm_writel(PM_CKSEL, cksel);
-
-	gd->cpu_hz = get_cpu_clk_rate();
-
-#ifdef CONFIG_PLL
-	/* Use PLL0 as main clock */
-	sm_writel(PM_MCCTRL, SM_BIT(PLLSEL));
-#endif
-}
-
 int cpu_init(void)
 {
 	extern void _evba(void);
-	char *p;
 
 	gd->cpu_hz = CFG_OSC0_HZ;
 
@@ -95,16 +56,15 @@ int cpu_init(void)
 	hsmc3_writel(PULSE0, 0x0b0a0906);
 	hsmc3_writel(SETUP0, 0x00010002);
 
-	pm_init();
+	clk_init();
 
+	/* Update the CPU speed according to the PLL configuration */
+	gd->cpu_hz = get_cpu_clk_rate();
+
+	/* Set up the exception handler table and enable exceptions */
 	sysreg_write(EVBA, (unsigned long)&_evba);
 	asm volatile("csrf	%0" : : "i"(SYSREG_EM_OFFSET));
 
-	/* Lock everything that mess with the flash in the icache */
-	for (p = __flashprog_start; p <= (__flashprog_end + CFG_ICACHE_LINESZ);
-	     p += CFG_ICACHE_LINESZ)
-		asm volatile("cache %0, 0x02" : "=m"(*p) :: "memory");
-
 	return 0;
 }
 
diff --git a/cpu/at32ap/entry.S b/cpu/at32ap/entry.S
deleted file mode 100644
index a6fc688..0000000
--- a/cpu/at32ap/entry.S
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * Copyright (C) 2004-2006 Atmel Corporation
- *
- * 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 <asm/sysreg.h>
-#include <asm/ptrace.h>
-
-	.section .text.exception,"ax"
-	.global	_evba
-	.type	_evba,@function
-	.align	10
-_evba:
-	.irp	x,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16
-	.align	2
-	rjmp	unknown_exception
-	.endr
-
-	.global	timer_interrupt_handler
-	.type	timer_interrupt_handler,@function
-	.align	2
-timer_interrupt_handler:
-	/*
-	 * Increment timer_overflow and re-write COMPARE with 0xffffffff.
-	 *
-	 * We're running at interrupt level 3, so we don't need to save
-	 * r8-r12 or lr to the stack.
-	 */
-	lda.w	r8, timer_overflow
-	ld.w	r9, r8[0]
-	mov	r10, -1
-	mtsr	SYSREG_COMPARE, r10
-	sub	r9, -1
-	st.w	r8[0], r9
-	rete
-
-	.type	unknown_exception, @function
-unknown_exception:
-	pushm	r0-r12
-	sub	r8, sp, REG_R12 - REG_R0 - 4
-	mov	r9, lr
-	mfsr	r10, SYSREG_RAR_EX
-	mfsr	r11, SYSREG_RSR_EX
-	pushm	r8-r11
-	mfsr	r12, SYSREG_ECR
-	mov	r11, sp
-	rcall	do_unknown_exception
-1:	rjmp	1b
diff --git a/cpu/at32ap/exception.c b/cpu/at32ap/exception.c
index 0672685..dc9c300 100644
--- a/cpu/at32ap/exception.c
+++ b/cpu/at32ap/exception.c
@@ -111,7 +111,8 @@ void do_unknown_exception(unsigned int ecr, struct pt_regs *regs)
 	printf("CPU Mode: %s\n", cpu_modes[mode]);
 
 	/* Avoid exception loops */
-	if (regs->sp < CFG_SDRAM_BASE || regs->sp >= gd->stack_end)
+	if (regs->sp < (gd->stack_end - CONFIG_STACKSIZE)
+			|| regs->sp >= gd->stack_end)
 		printf("\nStack pointer seems bogus, won't do stack dump\n");
 	else
 		dump_mem("\nStack: ", regs->sp, gd->stack_end);
diff --git a/cpu/at32ap/hsdramc.c b/cpu/at32ap/hsdramc.c
index 1fcfe75..992612b 100644
--- a/cpu/at32ap/hsdramc.c
+++ b/cpu/at32ap/hsdramc.c
@@ -30,39 +30,32 @@
 
 #include "hsdramc1.h"
 
-unsigned long sdram_init(const struct sdram_info *info)
+unsigned long sdram_init(void *sdram_base, const struct sdram_config *config)
 {
-	unsigned long *sdram = (unsigned long *)uncached(info->phys_addr);
 	unsigned long sdram_size;
-	unsigned long tmp;
-	unsigned long bus_hz;
+	uint32_t cfgreg;
 	unsigned int i;
 
-	if (!info->refresh_period)
-		panic("ERROR: SDRAM refresh period == 0. "
-				"Please update the board code\n");
-
-	tmp = (HSDRAMC1_BF(NC, info->col_bits - 8)
-	       | HSDRAMC1_BF(NR, info->row_bits - 11)
-	       | HSDRAMC1_BF(NB, info->bank_bits - 1)
-	       | HSDRAMC1_BF(CAS, info->cas)
-	       | HSDRAMC1_BF(TWR, info->twr)
-	       | HSDRAMC1_BF(TRC, info->trc)
-	       | HSDRAMC1_BF(TRP, info->trp)
-	       | HSDRAMC1_BF(TRCD, info->trcd)
-	       | HSDRAMC1_BF(TRAS, info->tras)
-	       | HSDRAMC1_BF(TXSR, info->txsr));
-
-#ifdef CFG_SDRAM_16BIT
-	tmp |= HSDRAMC1_BIT(DBW);
-	sdram_size = 1 << (info->row_bits + info->col_bits
-			   + info->bank_bits + 1);
-#else
-	sdram_size = 1 << (info->row_bits + info->col_bits
-			   + info->bank_bits + 2);
-#endif
-
-	hsdramc1_writel(CR, tmp);
+	cfgreg = (HSDRAMC1_BF(NC, config->col_bits - 8)
+		       | HSDRAMC1_BF(NR, config->row_bits - 11)
+		       | HSDRAMC1_BF(NB, config->bank_bits - 1)
+		       | HSDRAMC1_BF(CAS, config->cas)
+		       | HSDRAMC1_BF(TWR, config->twr)
+		       | HSDRAMC1_BF(TRC, config->trc)
+		       | HSDRAMC1_BF(TRP, config->trp)
+		       | HSDRAMC1_BF(TRCD, config->trcd)
+		       | HSDRAMC1_BF(TRAS, config->tras)
+		       | HSDRAMC1_BF(TXSR, config->txsr));
+
+	if (config->data_bits == SDRAM_DATA_16BIT)
+		cfgreg |= HSDRAMC1_BIT(DBW);
+
+	hsdramc1_writel(CR, cfgreg);
+
+	/* Send a NOP to turn on the clock (necessary on some chips) */
+	hsdramc1_writel(MR, HSDRAMC1_MODE_NOP);
+	hsdramc1_readl(MR);
+	writel(0, sdram_base);
 
 	/*
 	 * Initialization sequence for SDRAM, from the data sheet:
@@ -77,7 +70,7 @@ unsigned long sdram_init(const struct sdram_info *info)
 	 */
 	hsdramc1_writel(MR, HSDRAMC1_MODE_BANKS_PRECHARGE);
 	hsdramc1_readl(MR);
-	writel(0, sdram);
+	writel(0, sdram_base);
 
 	/*
 	 * 3. Eight auto-refresh (CBR) cycles are provided
@@ -85,58 +78,41 @@ unsigned long sdram_init(const struct sdram_info *info)
 	hsdramc1_writel(MR, HSDRAMC1_MODE_AUTO_REFRESH);
 	hsdramc1_readl(MR);
 	for (i = 0; i < 8; i++)
-		writel(0, sdram);
+		writel(0, sdram_base);
 
 	/*
 	 * 4. A mode register set (MRS) cycle is issued to program
 	 *    SDRAM parameters, in particular CAS latency and burst
 	 *    length.
 	 *
-	 * CAS from info struct, burst length 1, serial burst type
+	 * The address will be chosen by the SDRAMC automatically; we
+	 * just have to make sure BA[1:0] are set to 0.
 	 */
 	hsdramc1_writel(MR, HSDRAMC1_MODE_LOAD_MODE);
 	hsdramc1_readl(MR);
-	writel(0, sdram + (info->cas << 4));
+	writel(0, sdram_base);
 
 	/*
-	 * 5. A Normal Mode command is provided, 3 clocks after tMRD
-	 *    is met.
-	 *
-	 * From the timing diagram, it looks like tMRD is 3
-	 * cycles...try a dummy read from the peripheral bus.
+	 * 5. The application must go into Normal Mode, setting Mode
+	 *    to 0 in the Mode Register and performing a write access
+	 *    at any location in the SDRAM.
 	 */
-	hsdramc1_readl(MR);
 	hsdramc1_writel(MR, HSDRAMC1_MODE_NORMAL);
 	hsdramc1_readl(MR);
-	writel(0, sdram);
+	writel(0, sdram_base);
 
 	/*
 	 * 6. Write refresh rate into SDRAMC refresh timer count
 	 *    register (refresh rate = timing between refresh cycles).
-	 *
-	 * 15.6 us is a typical value for a burst of length one
 	 */
-	bus_hz = get_sdram_clk_rate();
-	hsdramc1_writel(TR, info->refresh_period);
-
-	printf("SDRAM: %u MB at address 0x%08lx\n",
-	       sdram_size >> 20, info->phys_addr);
-
-	printf("Testing SDRAM...");
-	for (i = 0; i < sdram_size / 4; i++)
-		sdram[i] = i;
-
-	for (i = 0; i < sdram_size / 4; i++) {
-		tmp = sdram[i];
-		if (tmp != i) {
-			printf("FAILED at address 0x%08lx\n",
-			       info->phys_addr + i * 4);
-			printf("SDRAM: read 0x%lx, expected 0x%lx\n", tmp, i);
-			return 0;
-		}
-	}
-
-	puts("OK\n");
+	hsdramc1_writel(TR, config->refresh_period);
+
+	if (config->data_bits == SDRAM_DATA_16BIT)
+		sdram_size = 1 << (config->row_bits + config->col_bits
+				   + config->bank_bits + 1);
+	else
+		sdram_size = 1 << (config->row_bits + config->col_bits
+				   + config->bank_bits + 2);
 
 	return sdram_size;
 }
diff --git a/cpu/at32ap/interrupts.c b/cpu/at32ap/interrupts.c
index bef1f30..160838e 100644
--- a/cpu/at32ap/interrupts.c
+++ b/cpu/at32ap/interrupts.c
@@ -98,18 +98,16 @@ void set_timer(unsigned long t)
  */
 void udelay(unsigned long usec)
 {
-	unsigned long now, end;
+	unsigned long cycles;
+	unsigned long base;
+	unsigned long now;
 
-	now = sysreg_read(COUNT);
+	base = sysreg_read(COUNT);
+	cycles = ((usec * (get_tbclk() / 10000)) + 50) / 100;
 
-	end = ((usec * (get_tbclk() / 10000)) + 50) / 100;
-	end += now;
-
-	while (now > end)
-		now = sysreg_read(COUNT);
-
-	while (now < end)
+	do {
 		now = sysreg_read(COUNT);
+	} while ((now - base) < cycles);
 }
 
 static int set_interrupt_handler(unsigned int nr, void (*handler)(void),
diff --git a/cpu/at32ap/pm.c b/cpu/at32ap/pm.c
deleted file mode 100644
index c78d547..0000000
--- a/cpu/at32ap/pm.c
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright (C) 2006 Atmel Corporation
- *
- * 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>
-
-#ifdef CFG_POWER_MANAGER
-#include <asm/errno.h>
-#include <asm/io.h>
-
-#include <asm/arch/memory-map.h>
-
-#include "sm.h"
-
-
-#ifdef CONFIG_PLL
-#define MAIN_CLK_RATE ((CFG_OSC0_HZ / CFG_PLL0_DIV) * CFG_PLL0_MUL)
-#else
-#define MAIN_CLK_RATE (CFG_OSC0_HZ)
-#endif
-
-DECLARE_GLOBAL_DATA_PTR;
-
-
-#endif /* CFG_POWER_MANAGER */
diff --git a/cpu/at32ap/start.S b/cpu/at32ap/start.S
index ab8c2b7..907e9b1 100644
--- a/cpu/at32ap/start.S
+++ b/cpu/at32ap/start.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005-2006 Atmel Corporation
+ * Copyright (C) 2005-2008 Atmel Corporation
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -20,12 +20,9 @@
  * MA 02111-1307 USA
  */
 #include <config.h>
+#include <asm/ptrace.h>
 #include <asm/sysreg.h>
 
-#ifndef PART_SPECIFIC_BOOTSTRAP
-# define PART_SPECIFIC_BOOTSTRAP
-#endif
-
 #define SYSREG_MMUCR_I_OFFSET	2
 #define SYSREG_MMUCR_S_OFFSET	4
 
@@ -34,11 +31,115 @@
 		    | SYSREG_BIT(FE) | SYSREG_BIT(RE)		\
 		    | SYSREG_BIT(IBE) | SYSREG_BIT(IEE))
 
-	.text
+	/*
+	 * To save some space, we use the same entry point for
+	 * exceptions and reset. This avoids lots of alignment padding
+	 * since the reset vector is always suitably aligned.
+	 */
+	.section .exception.text, "ax", @progbits
 	.global	_start
+	.global	_evba
+	.type	_start, @function
+	.type	_evba, @function
 _start:
-	PART_SPECIFIC_BOOTSTRAP
+	.size	_start, 0
+_evba:
+	.org	0x00
+	rjmp	unknown_exception	/* Unrecoverable exception */
+	.org	0x04
+	rjmp	unknown_exception	/* TLB multiple hit */
+	.org	0x08
+	rjmp	unknown_exception	/* Bus error data fetch */
+	.org	0x0c
+	rjmp	unknown_exception	/* Bus error instruction fetch */
+	.org	0x10
+	rjmp	unknown_exception	/* NMI */
+	.org	0x14
+	rjmp	unknown_exception	/* Instruction address */
+	.org	0x18
+	rjmp	unknown_exception	/* ITLB protection */
+	.org	0x1c
+	rjmp	unknown_exception	/* Breakpoint */
+	.org	0x20
+	rjmp	unknown_exception	/* Illegal opcode */
+	.org	0x24
+	rjmp	unknown_exception	/* Unimplemented instruction */
+	.org	0x28
+	rjmp	unknown_exception	/* Privilege violation */
+	.org	0x2c
+	rjmp	unknown_exception	/* Floating-point */
+	.org	0x30
+	rjmp	unknown_exception	/* Coprocessor absent */
+	.org	0x34
+	rjmp	unknown_exception	/* Data Address (read) */
+	.org	0x38
+	rjmp	unknown_exception	/* Data Address (write) */
+	.org	0x3c
+	rjmp	unknown_exception	/* DTLB Protection (read) */
+	.org	0x40
+	rjmp	unknown_exception	/* DTLB Protection (write) */
+	.org	0x44
+	rjmp	unknown_exception	/* DTLB Modified */
+
+	.org	0x50
+	rjmp	unknown_exception	/* ITLB Miss */
+	.org	0x60
+	rjmp	unknown_exception	/* DTLB Miss (read) */
+	.org	0x70
+	rjmp	unknown_exception	/* DTLB Miss (write) */
+
+	.size	_evba, . - _evba
+
+	.align	2
+	.type	unknown_exception, @function
+unknown_exception:
+	/* Figure out whether we're handling an exception (Exception
+	 * mode) or just booting (Supervisor mode). */
+	csrfcz	SYSREG_M1_OFFSET
+	brcc	at32ap_cpu_bootstrap
+
+	/* This is an exception. Complain. */
+	pushm	r0-r12
+	sub	r8, sp, REG_R12 - REG_R0 - 4
+	mov	r9, lr
+	mfsr	r10, SYSREG_RAR_EX
+	mfsr	r11, SYSREG_RSR_EX
+	pushm	r8-r11
+	mfsr	r12, SYSREG_ECR
+	mov	r11, sp
+	rcall	do_unknown_exception
+1:	rjmp	1b
+
+	/* The COUNT/COMPARE timer interrupt handler */
+	.global	timer_interrupt_handler
+	.type	timer_interrupt_handler,@function
+	.align	2
+timer_interrupt_handler:
+	/*
+	 * Increment timer_overflow and re-write COMPARE with 0xffffffff.
+	 *
+	 * We're running at interrupt level 3, so we don't need to save
+	 * r8-r12 or lr to the stack.
+	 */
+	lda.w	r8, timer_overflow
+	ld.w	r9, r8[0]
+	mov	r10, -1
+	mtsr	SYSREG_COMPARE, r10
+	sub	r9, -1
+	st.w	r8[0], r9
+	rete
 
+	/*
+	 * CPU bootstrap after reset is handled here. SoC code may
+	 * override this in case they need to initialize oscillators,
+	 * etc.
+	 */
+	.section .text.at32ap_cpu_bootstrap, "ax", @progbits
+	.global	at32ap_cpu_bootstrap
+	.weak	at32ap_cpu_bootstrap
+	.type	at32ap_cpu_bootstrap, @function
+	.align	2
+at32ap_cpu_bootstrap:
 	/* Reset the Status Register */
 	mov	r0, lo(SR_INIT)
 	orh	r0, hi(SR_INIT)
@@ -66,9 +167,16 @@ _start:
 	lddpc	pc, 1f
 
 	.align	2
-1:	.long	2f
+1:	.long	at32ap_low_level_init
+	.size	_start, . - _start
 
-2:	lddpc	sp, sp_init
+	/* Common CPU bootstrap code after oscillator/cache/etc. init */
+	.section .text.avr32ap_low_level_init, "ax", @progbits
+	.global	at32ap_low_level_init
+	.type	at32ap_low_level_init, @function
+	.align	2
+at32ap_low_level_init:
+	lddpc	sp, sp_init
 
 	/* Initialize the GOT pointer */
 	lddpc	r6, got_init
@@ -90,6 +198,7 @@ got_init:
 	 * Relocate the u-boot image into RAM and continue from there.
 	 * Does not return.
 	 */
+	.section .text.relocate_code,"ax",@progbits
 	.global	relocate_code
 	.type	relocate_code,@function
 relocate_code:
@@ -162,3 +271,5 @@ in_ram:
 	.align	2
 got_init_reloc:
 	.long	3b - _GLOBAL_OFFSET_TABLE_
+
+	.size	relocate_code, . - relocate_code