* Update for AT91RM9200DK and CMC_PU2 boards:
  - Enable booting directly from flash
  - fix CMC_PU2 flash driver

* Fix mkimage usage message
diff --git a/cpu/at91rm9200/at91rm9200_ether.c b/cpu/at91rm9200/at91rm9200_ether.c
index 85afba7..b6247c9 100644
--- a/cpu/at91rm9200/at91rm9200_ether.c
+++ b/cpu/at91rm9200/at91rm9200_ether.c
@@ -169,6 +169,7 @@
 			  AT91C_PA10_ETX1 | AT91C_PA9_ETX0 | AT91C_PA8_ETXEN |
 			  AT91C_PA7_ETXCK_EREFCK;
 
+#if defined(CONFIG_AT91C_USE_RMII) && !defined(CONFIG_CMC_PU2)
 	*AT91C_PIOB_PDR = AT91C_PB25_EF100 |
 			  AT91C_PB19_ERXCK | AT91C_PB18_ECOL | AT91C_PB17_ERXDV |
 			  AT91C_PB16_ERX3 | AT91C_PB15_ERX2 | AT91C_PB14_ETXER |
@@ -178,6 +179,7 @@
 	*AT91C_PIOB_BSR = AT91C_PB25_EF100 | AT91C_PB19_ERXCK | AT91C_PB18_ECOL |
 			  AT91C_PB17_ERXDV | AT91C_PB16_ERX3 | AT91C_PB15_ERX2 |
 			  AT91C_PB14_ETXER | AT91C_PB13_ETX3 | AT91C_PB12_ETX2;
+#endif
 
 	*AT91C_PMC_PCER = 1 << AT91C_ID_EMAC;	/* Peripheral Clock Enable Register */
 
diff --git a/cpu/at91rm9200/i2c.c b/cpu/at91rm9200/i2c.c
index 515792c..c12d2dd 100644
--- a/cpu/at91rm9200/i2c.c
+++ b/cpu/at91rm9200/i2c.c
@@ -42,7 +42,7 @@
 static short at91_poll_status(AT91PS_TWI twi, unsigned long bit) {
 	int loop_cntr = 10000;
 	do {
-		udelay(100);
+		udelay(10);
 	} while (!(twi->TWI_SR & bit) && (--loop_cntr > 0));
 
 	return (loop_cntr > 0);
diff --git a/cpu/at91rm9200/serial.c b/cpu/at91rm9200/serial.c
index fb03851..a9693bf 100644
--- a/cpu/at91rm9200/serial.c
+++ b/cpu/at91rm9200/serial.c
@@ -55,17 +55,11 @@
 
 	if ((baudrate = gd->baudrate) <= 0)
 		baudrate = CONFIG_BAUDRATE;
-	if (baudrate == CONFIG_BAUDRATE) {
+	if (baudrate == 0 || baudrate == CONFIG_BAUDRATE)
 		us->US_BRGR = CFG_AT91C_BRGR_DIVISOR;	/* hardcode so no __divsi3 */
-	} else {
-#if 0
-		/* 33 -> 115200 */
-		us->US_BRGR = 33 * (115200/baudrate);
-#else
+	else
 		/* MASTER_CLOCK/(16 * baudrate) */
 		us->US_BRGR = (AT91C_MASTER_CLOCK >> 4)/baudrate;
-#endif
-	}
 }
 
 int serial_init (void)
diff --git a/cpu/at91rm9200/start.S b/cpu/at91rm9200/start.S
index d73af20..4f1d9ae 100644
--- a/cpu/at91rm9200/start.S
+++ b/cpu/at91rm9200/start.S
@@ -112,9 +112,53 @@
 	 */
 	mrs     r0,cpsr
 	bic     r0,r0,#0x1f
-	orr     r0,r0,#0x13
+	orr     r0,r0,#0xd3 /* was 13 */
 	msr     cpsr,r0
 
+#ifdef CONFIG_BOOTBINFUNC
+/* code based on entry.S from ATMEL */
+#define AT91C_BASE_CKGR 0xFFFFFC20
+#define CKGR_MOR 0
+	/* Get the CKGR Base Address */
+	ldr     r1, =AT91C_BASE_CKGR
+
+/* Main oscillator Enable register	APMC_MOR : Enable main oscillator , OSCOUNT = 0xFF */
+/*	ldr 	r0, = AT91C_CKGR_MOSCEN:OR:AT91C_CKGR_OSCOUNT */
+	ldr 	r0, =0x0000FF01
+	str     r0, [r1, #CKGR_MOR]
+	/* Add loop to compensate Main Oscillator startup time */
+	ldr 	r0, =0x00000010
+LoopOsc:
+	subs    r0, r0, #1
+	bhi     LoopOsc
+
+	/* scratch stack */
+	ldr 	r1, =0x00204000
+	/* Insure word alignment */
+	bic     r1, r1, #3
+	/* Init stack SYS	 */
+	mov     sp, r1
+	/*
+	 * This does a lot more than just set up the memory, which
+	 * is why it's called lowlevelinit
+	 */
+	bl	lowlevelinit /* in memsetup.S */
+	bl	icache_enable;
+	/*------------------------------------
+	  Read/modify/write CP15 control register
+	 -------------------------------------
+	  read cp15 control register (cp15 r1) in r0
+	  ------------------------------------*/
+	mrc     p15, 0, r0, c1, c0, 0
+	/* Reset bit :Little Endian end fast bus mode */
+	ldr     r3, =0xC0000080
+	/* Set bit :Asynchronous clock mode, Not Fast Bus */
+	ldr     r4, =0xC0000000
+	bic     r0, r0, r3
+	orr     r0, r0, r4
+	/* write r0 in cp15 control register (cp15 r1) */
+	mcr     p15, 0, r0, c1, c0, 0
+#endif /* CONFIG_BOOTBINFUNC */
 	/*
 	 * relocate exeception table
 	 */
@@ -135,6 +179,25 @@
 	bl      cpu_init_crit
 #endif
 
+#ifdef CONFIG_BOOTBINFUNC
+relocate:				/* relocate U-Boot to RAM	    */
+	adr	r0, _start		/* r0 <- current position of code   */
+	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            */
+	add	r2, r0, r2		/* r2 <- source end address         */
+
+copy_loop:
+	ldmia	r0!, {r3-r10}		/* copy from source address [r0]    */
+	stmia	r1!, {r3-r10}		/* copy to   target address [r1]    */
+	cmp	r0, r2			/* until source end addreee [r2]    */
+	ble	copy_loop
+#endif /* CONFIG_BOOTBINFUNC */
+
 	/* Set up the stack						    */
 stack_setup:
 	ldr	r0, _TEXT_BASE		/* upper 128 KiB: relocated uboot   */
@@ -168,7 +231,7 @@
  */
 
 cpu_init_crit:
-	# actually do nothing for now!
+	/* do nothing for now */
 	mov	pc, lr