1 files changed, 33 insertions, 0 deletions
diff --git a/boards/at91sam3u-ek/board_cstartup_gnu.c b/boards/at91sam3u-ek/board_cstartup_gnu.c
index 66a433b..8a8e942 100644
--- a/boards/at91sam3u-ek/board_cstartup_gnu.c
+++ b/boards/at91sam3u-ek/board_cstartup_gnu.c
@@ -33,6 +33,7 @@
 #include "board.h"
 #include "exceptions.h"
 #include "board_lowlevel.h"
+#include <core_cm3.h>
 
 //------------------------------------------------------------------------------
 //         External Variables
@@ -121,6 +122,18 @@ IntFunc exception_table[] = {
     IrqHandlerNotUsed   // 30 not used
 };
 
+#if defined(BOARD_USB_DFU) && !defined(dfu)
+static void BootIntoApp(void)
+{
+    unsigned int *pSrc;
+    void (*appReset)(void);
+
+    pSrc = (unsigned int *) ((unsigned char *)AT91C_IFLASH0 + BOARD_DFU_BOOT_SIZE);
+    AT91C_BASE_NVIC->NVIC_VTOFFR = ((unsigned int)(pSrc)) | (0x0 << 7);
+    appReset = pSrc[1];
+    appReset();
+}
+#endif
 
 //------------------------------------------------------------------------------
 /// This is the code that gets called on processor reset. To initialize the
@@ -131,6 +144,24 @@ void ResetException(void)
     unsigned int *pSrc, *pDest;
 
     LowLevelInit();
+
+#if defined(BOARD_USB_DFU) && !defined(dfu)
+#ifdef BOARD_DFU_BTN_PIOA
+    /* There is a PIO button that can be used to enter DFU */
+    AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_PIOA;
+    AT91C_BASE_PIOA->PIO_PPUER = (1 << BOARD_DFU_BTN_PIOA);
+    AT91C_BASE_PIOA->PIO_ODR = (1 << BOARD_DFU_BTN_PIOA);
+    AT91C_BASE_PIOA->PIO_PER = (1 << BOARD_DFU_BTN_PIOA);
+
+    if (AT91C_BASE_PIOA->PIO_PDSR & (1 << BOARD_DFU_BTN_PIOA) &&
+#else /* BOARD_DFU_BTN_PIOA */
+    if (1 &&
+#endif /* BOARD_DFU_BTN_PIOA */
+	(unsigned long *)AT91C_IRAM != 0xDFDFDFDF) {
+	BootIntoApp();
+    }
+#endif
+
 #if defined(psram)
     pDest = &_vect_start;
     pSrc = &_svectorrelocate;
@@ -162,6 +193,8 @@ void ResetException(void)
 #endif        
     
     AT91C_BASE_NVIC->NVIC_VTOFFR = ((unsigned int)(pSrc)) | (0x0 << 7);
+    /* APP should have disabled interrupts during the transition */
+    __enable_irq();
 
     main();
 }