5 files changed, 34 insertions, 6 deletions
diff --git a/openpcd/firmware/src/openpcd.h b/openpcd/firmware/src/openpcd.h
index bd3cfb4..3099373 100644
--- a/openpcd/firmware/src/openpcd.h
+++ b/openpcd/firmware/src/openpcd.h
@@ -20,11 +20,13 @@
 #define OPENPCD_PIO_UDP_PUP	AT91C_PIO_PA22
 #define OPENPCD_PIO_LED1	AT91C_PIO_PA25
 #define OPENPCD_PIO_LED2	AT91C_PIO_PA26
+#define PIO_BOOTLDR		AT91C_PIO_PA27
 #elif defined(PICC)
 #define OPENPCD_PIO_UDP_CNX	NO_UDP_CNX
 #define OPENPCD_PIO_UDP_PUP	AT91C_PIO_PA22
 #define OPENPCD_PIO_LED1	AT91C_PIO_PA25
 #define OPENPCD_PIO_LED2	AT91C_PIO_PA12
+#define PIO_BOOTLDR		AT91C_PIO_PA6
 #else
 #error "unknown PCB"
 #endif
diff --git a/openpcd/firmware/src/os/dfu.c b/openpcd/firmware/src/os/dfu.c
index 25040db..f2bc0ca 100644
--- a/openpcd/firmware/src/os/dfu.c
+++ b/openpcd/firmware/src/os/dfu.c
@@ -28,8 +28,8 @@
 #define DEBUGI(x, args ...) do { } while (0)
 #endif
 
-/* this is only called once before DFU mode, no __dfufunc required */
-void udp_init(void)
+
+void __dfufunc udp_init(void)
 {
 	/* Set the PLL USB Divider */
 	AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
@@ -641,12 +641,13 @@ static void dfu_switch(void)
 
 void __dfufunc dfu_main(void)
 {
-	/*
+	udp_init();
+
+	/* This implements 
 	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_UDP,
 			      OPENPCD_IRQ_PRIO_UDP,
 			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, dfu_udp_irq);
 	*/
-
 	AT91PS_AIC pAic = AT91C_BASE_AIC;
 	pAic->AIC_IDCR = 1 << AT91C_ID_UDP;
 	pAic->AIC_SVR[AT91C_ID_UDP] = (unsigned int) &dfu_udp_irq;
@@ -659,7 +660,7 @@ void __dfufunc dfu_main(void)
 	/* End-of-Bus-Reset is always enabled */
 
 	/* Clear for set the Pull up resistor */
-	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
 
 	/* do nothing, since all of DFU is interrupt driven */
 	while (1) ;
@@ -676,4 +677,5 @@ const struct dfuapi __dfufunctab dfu_api = {
 	.dfu_cfg_descriptor	= &dfu_cfg_descriptor,
 };
 
+/* just for testing */
 int foo = 12345;
diff --git a/openpcd/firmware/src/os/dfu.h b/openpcd/firmware/src/os/dfu.h
index e68f62d..8786044 100644
--- a/openpcd/firmware/src/os/dfu.h
+++ b/openpcd/firmware/src/os/dfu.h
@@ -56,7 +56,7 @@ struct udp_pcd;
 #endif
 
 
-extern void udp_init(void);
+extern void __dfufunc udp_init(void);
 
 struct _dfu_desc {
 	struct usb_config_descriptor ucfg;
diff --git a/openpcd/firmware/src/os/main.c b/openpcd/firmware/src/os/main.c
index 66cac8b..4b7a7a2 100644
--- a/openpcd/firmware/src/os/main.c
+++ b/openpcd/firmware/src/os/main.c
@@ -15,6 +15,7 @@ int main(void)
 	led_init();
 	AT91F_DBGU_Init();
 
+	AT91F_PIOA_CfgPMC();
 	/* call application specific init function */
 	_init_func();
 
diff --git a/openpcd/firmware/src/start/Cstartup.S b/openpcd/firmware/src/start/Cstartup.S
index 1c9f1db..e91fd86 100644
--- a/openpcd/firmware/src/start/Cstartup.S
+++ b/openpcd/firmware/src/start/Cstartup.S
@@ -16,6 +16,8 @@
 //*- 1.1 01/Apr/05 JPP    : save SPSR 
 //*-----------------------------------------------------------------------------*/
 
+#define CONFIG_DFU_SWITCH
+
 //#define DEBUG_LL
 
 #ifdef DEBUG_LL
@@ -59,6 +61,13 @@
 	.equ AIC_EOICR,       (304)
 	.equ AIC_MCR_RCR,     (0xf00)
 	.equ AT91C_BASE_AIC,  (0xFFFFF000)
+	.equ AT91C_PMC_PCER,  (0xFFFFFC10)
+	.equ AT91C_BASE_PIOA, (0xFFFFF400)
+	.equ AT91C_ID_PIOA,   (2)
+	.equ PIOA_PDSR,	      (0x3c)
+	.equ PIO_BOOTLDR,     (1 << 27)
+	#.equ PIO_BOOTLDR,     (1 << 6)
+
 
 /* #include "AT91SAM7S64_inc.h"	  */
 
@@ -170,9 +179,23 @@ _remap:
 		mov	r2, #0x01
 		str	r2, [r1, #AIC_MCR_RCR]
 
+		/* prepare c function call to main */
 		mov	r0, #0	/* argc = 0 */
 		ldr	lr,=exit
 		ldr	r10,=main
+
+#ifdef CONFIG_DFU_SWITCH
+		/* check whether bootloader button is pressed */
+		ldr	r1, =AT91C_PMC_PCER
+		mov	r2, #(1 << AT91C_ID_PIOA)
+		str	r2, [r1] 
+
+		ldr 	r1, =AT91C_BASE_PIOA
+		ldr	r2, [r1, #PIOA_PDSR]
+		tst	r2, #PIO_BOOTLDR
+		ldrne	r10,=dfu_main
+#endif
+
 		bx	r10
 	.size	remap, . - remap
 	.endfunc