diff --git a/arch/arm/cpu/arm926ejs/at91/led.c b/arch/arm/cpu/arm926ejs/at91/led.c
index 0a315c4..0638a2e 100644
--- a/arch/arm/cpu/arm926ejs/at91/led.c
+++ b/arch/arm/cpu/arm926ejs/at91/led.c
@@ -28,38 +28,149 @@
 #include <asm/arch/gpio.h>
 #include <asm/arch/io.h>
 
+static unsigned int saved_state[4] = {STATUS_LED_OFF, STATUS_LED_OFF,
+		STATUS_LED_OFF, STATUS_LED_OFF};
+
+void coloured_LED_init(void)
+{
+	/* Enable clock */
+	at91_sys_write(AT91_PMC_PCER, 1 << AT91SAM9G45_ID_PIODE);
+
+#ifdef CONFIG_RED_LED	
+	at91_set_gpio_output(CONFIG_RED_LED, 1);
+	at91_set_gpio_value(CONFIG_RED_LED, 1);
+#endif	
+#ifdef CONFIG_GREEN_LED
+	at91_set_gpio_output(CONFIG_GREEN_LED, 1);
+	at91_set_gpio_value(CONFIG_GREEN_LED, 1);
+#endif
+#ifdef CONFIG_YELLOW_LED
+	at91_set_gpio_output(CONFIG_YELLOW_LED, 1);
+	at91_set_gpio_value(CONFIG_YELLOW_LED, 1);
+#endif
+#ifdef CONFIG_BLUE_LED	
+	at91_set_gpio_output(CONFIG_BLUE_LED, 1);
+	at91_set_gpio_value(CONFIG_BLUE_LED, 1);
+#endif  
+}
+
 #ifdef CONFIG_RED_LED
 void red_LED_on(void)
 {
 	at91_set_gpio_value(CONFIG_RED_LED, 1);
+	saved_state[STATUS_LED_RED] = STATUS_LED_ON;
 }
 
 void red_LED_off(void)
 {
 	at91_set_gpio_value(CONFIG_RED_LED, 0);
+	saved_state[STATUS_LED_RED] = STATUS_LED_OFF;
 }
 #endif
 
 #ifdef CONFIG_GREEN_LED
 void green_LED_on(void)
 {
-	at91_set_gpio_value(CONFIG_GREEN_LED, 0);
+	at91_set_gpio_value(CONFIG_GREEN_LED, 1);
+	saved_state[STATUS_LED_GREEN] = STATUS_LED_ON;
 }
 
 void green_LED_off(void)
 {
-	at91_set_gpio_value(CONFIG_GREEN_LED, 1);
+	at91_set_gpio_value(CONFIG_GREEN_LED, 0);
+	saved_state[STATUS_LED_GREEN] = STATUS_LED_OFF;
 }
 #endif
 
 #ifdef CONFIG_YELLOW_LED
 void yellow_LED_on(void)
 {
-	at91_set_gpio_value(CONFIG_YELLOW_LED, 0);
+	at91_set_gpio_value(CONFIG_YELLOW_LED, 1);
+	saved_state[STATUS_LED_YELLOW] = STATUS_LED_ON;
 }
 
 void yellow_LED_off(void)
 {
-	at91_set_gpio_value(CONFIG_YELLOW_LED, 1);
+	at91_set_gpio_value(CONFIG_YELLOW_LED, 0);
+	saved_state[STATUS_LED_YELLOW] = STATUS_LED_OFF;
 }
 #endif
+
+void __led_init(led_id_t mask, int state)
+{
+	__led_set(mask, state);
+}
+
+void __led_toggle(led_id_t mask)
+{
+
+#ifdef CONFIG_RED_LED	
+	if (STATUS_LED_RED == mask) {
+		if (STATUS_LED_ON == saved_state[STATUS_LED_RED])
+			red_LED_off();
+		else
+			red_LED_on();
+	} 
+#endif
+#ifdef CONFIG_BLUE_LED
+	else if (STATUS_LED_BLUE == mask) {
+		if (STATUS_LED_ON == saved_state[STATUS_LED_BLUE])
+			blue_LED_off();
+		else
+			blue_LED_on();
+	} 
+#endif
+#ifdef CONFIG_GREEN_LED
+	else if (STATUS_LED_GREEN == mask) {
+		if (STATUS_LED_ON == saved_state[STATUS_LED_GREEN])
+			green_LED_off();
+		else
+			green_LED_on();
+	} 
+#endif
+#ifdef CONFIG_YELLOW_LED
+	else if (STATUS_LED_YELLOW == mask) {
+		if (STATUS_LED_ON == saved_state[STATUS_LED_YELLOW])
+			yellow_LED_off();
+		else
+			yellow_LED_on();
+	}
+#endif
+}
+
+void __led_set(led_id_t mask, int state)
+{
+#ifdef CONFIG_RED_LED	  	
+	if (STATUS_LED_RED == mask) {
+		if (STATUS_LED_ON == state)
+			red_LED_on();
+		else
+			red_LED_off();
+	} 
+#endif
+#ifdef CONFIG_BLUE_LED	
+        else if (STATUS_LED_BLUE == mask) {
+		if (STATUS_LED_ON == state)
+			blue_LED_on();
+		else
+			blue_LED_off();
+	} 
+#endif
+#ifdef CONFIG_GREEN_LED	
+	else if (STATUS_LED_GREEN == mask) {
+		if (STATUS_LED_ON == state)
+			green_LED_on();
+		else
+			green_LED_off();
+	} 
+#endif
+#ifdef CONFIG_YELLOW_LED	
+	else if (STATUS_LED_YELLOW == mask) {
+		if (STATUS_LED_ON == state)
+			yellow_LED_on();
+		else
+			yellow_LED_off();
+	}
+#endif
+}
+
diff --git a/common/Makefile b/common/Makefile
index 048df0c..a653c1e 100644
--- a/common/Makefile
+++ b/common/Makefile
@@ -163,6 +163,7 @@ COBJS-$(CONFIG_LCD) += lcd.o
 COBJS-$(CONFIG_LYNXKDI) += lynxkdi.o
 COBJS-$(CONFIG_MODEM_SUPPORT) += modem.o
 COBJS-$(CONFIG_UPDATE_TFTP) += update.o
+COBJS-$(CONFIG_USBD_DFU)+= update_dfu.o
 COBJS-$(CONFIG_USB_KEYBOARD) += usb_kbd.o
 
 
diff --git a/common/update_dfu.c b/common/update_dfu.c
index f1ceccf..ca2240b 100644
--- a/common/update_dfu.c
+++ b/common/update_dfu.c
@@ -56,8 +56,6 @@ int dfu_loop(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	int rcv;
 
-	dfu_finished = 0;
-
 	/* initialize the USBD controller */
 #ifdef CONFIG_USB_GADGET_ATMEL_USBA
 	usba_udc_probe(&dfubrd);
diff --git a/drivers/usb/gadget/atmel_usba_udc.c b/drivers/usb/gadget/atmel_usba_udc.c
index 6d02de6..45227c4 100644
--- a/drivers/usb/gadget/atmel_usba_udc.c
+++ b/drivers/usb/gadget/atmel_usba_udc.c
@@ -349,12 +349,12 @@ static struct usba_request *alloc_request(void)
 		if (!req_pool[i].in_use) {
 			ptr = &req_pool[i];
 			req_pool[i].in_use = 1;
-			DBG("alloc_req: request[%d]\n", i);
+			debug("alloc_req: request[%d]\n", i);
 			break;
 		}
 	}
 	if (!ptr)
-		DBG("panic: no more free req buffers\n");
+		debug("panic: no more free req buffers\n");
 	return ptr;
 }
 
@@ -412,7 +412,7 @@ usba_ep_queue(struct usb_ep *_ep,
 	}
 
 	if (!_ep || (!ep->desc && ep->ep.name != ep0name)) {
-		DBG("invalid ep\n");
+		debug("invalid ep\n");
 		return -EINVAL;
 	}
 
@@ -457,7 +457,7 @@ static int usba_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 			ep->ep.name, req);
 
 	if (!_ep || ep->ep.name == ep0name) {
-		DBG("invalid dequeue request\n");
+		debug("invalid dequeue request\n");
 		if (!_ep)
 			debug("NO ENDPOINT\n");
 		if (ep->ep.name == ep0name)
diff --git a/drivers/usb/gadget/usbdfu.c b/drivers/usb/gadget/usbdfu.c
index 65f334a..a1e7316 100644
--- a/drivers/usb/gadget/usbdfu.c
+++ b/drivers/usb/gadget/usbdfu.c
@@ -398,9 +398,10 @@ static int handle_dnload(struct usb_gadget *gadget,
 		char *mtdp = getenv("mtdparts");
 		if (mtdp)
 			printf("Valid MTD partitions found\n");
-		/*this used to be in the Openmoko driver */
-		/*if (!mtdp)
-			/*run_command("dynpart", 0); */
+		/*this used to be in the Openmoko driver 
+		 *if (!mtdp)
+		 *run_command("dynpart", 0); 
+		*/
 		else {
 			dev->dfu_state = DFU_STATE_dfuERROR;
 			dev->dfu_status = DFU_STATUS_errADDRESS;
@@ -477,9 +478,10 @@ static int handle_dnload(struct usb_gadget *gadget,
 	red_LED_off();
 #endif
 			rc = nand_write_skip_bad(ds->nand,
-						 ds->part->offset,
-						 &rwsize,
-						 (u_char *)addr);
+						ds->part->offset,
+						&rwsize,
+						(u_char *)addr,
+						0);
 			if (rc) {
 				printf("NAND write failed\n");
 				break;
