Add DFU support (unfinished, doesn't actually flash yet)

git-svn-id: https://svn.openpcd.org:2342/trunk@141 6dc7ffe9-61d6-0310-9af1-9938baff3ed1

9 files changed, 417 insertions, 142 deletions

diff --git a/openpcd/firmware/Makefile b/openpcd/firmware/Makefile
index edbf63c..2999093 100644
--- a/openpcd/firmware/Makefile
+++ b/openpcd/firmware/Makefile
@@ -52,6 +52,9 @@ RUN_MODE=RUN_FROM_ROM
 ## Create RAM-Image (debugging) - not used in this example
 #RUN_MODE=RUN_FROM_RAM
 
+## We want to produce a full-flash image, but including DFU 
+IMGTYPE=-dfu-fullimage
+
 # with / at end
 PATH_TO_LINKSCRIPTS=link/
 
@@ -254,7 +257,7 @@ LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
 ifeq ($(RUN_MODE),RUN_FROM_RAM)
 LDFLAGS +=-T$(PATH_TO_LINKSCRIPTS)$(SUBMDL)-RAM.ld
 else 
-LDFLAGS +=-T$(PATH_TO_LINKSCRIPTS)$(SUBMDL)-ROM.ld
+LDFLAGS +=-T$(PATH_TO_LINKSCRIPTS)$(SUBMDL)-ROM$(IMGTYPE).ld
 endif
 
 
diff --git a/openpcd/firmware/link/AT91SAM7S256-ROM-dfu-fullimage.ld b/openpcd/firmware/link/AT91SAM7S256-ROM-dfu-fullimage.ld
new file mode 100644
index 0000000..9bb5163
--- /dev/null
+++ b/openpcd/firmware/link/AT91SAM7S256-ROM-dfu-fullimage.ld
@@ -0,0 +1,155 @@
+/*---------------------------------------------------------------------------*/
+/*-         ATMEL Microcontroller Software Support  -  ROUSSET  -            */
+/*---------------------------------------------------------------------------*/
+/* The software is delivered "AS IS" without warranty or condition of any    */
+/* kind, either express, implied or statutory. This includes without	     */
+/* limitation any warranty or condition with respect to merchantability or   */
+/* fitness for any particular purpose, or against the infringements of	     */
+/* intellectual property rights of others.				     */
+/*---------------------------------------------------------------------------*/
+/*- File source          : GCC_FLASH.ld                                      */
+/*- Object               : Linker Script File for Flash Workspace            */
+/*- Compilation flag     : None                                              */
+/*-                                                                          */
+/*- 1.0 11/Mar/05 JPP    : Creation S256                                     */
+/*---------------------------------------------------------------------------*/
+
+/* 
+   24-02-2006  Ewout Boks. Adapted from AT91SAM7S64-RAM.ld script by M. Thomas. 
+   - Changed the memory sections to model the AT91SAM7S256 .
+   - tested succesfully with AT91SAM7 GPIO Example and PEEDI debugger
+     on AT91SAM7S-EK equipped with AT91SAM7S256 
+   20-08-2006 Harald Welte
+   - Add support for USB DFU handling
+   	- Partition FLASH into two sections
+	  idx 0x00000 ... 0x01fff: dfu loader
+	  idx 0x02000 ... 0x020ff: dfu_api function pointers
+	  idx 0x02100 ... 0x40000: application program
+*/
+
+/* Memory Definitions */
+
+MEMORY
+{
+  FLASH (rx) : ORIGIN = 0x00100000, LENGTH = 0x00040000
+  DATA (rw)  : ORIGIN = 0x00200000, LENGTH = 0x00010000
+  STACK (rw) : ORIGIN = 0x00210000, LENGTH = 0x00000000
+}
+
+
+/* Section Definitions */
+
+SECTIONS
+{
+  . = 0x00000000;
+  /* first section is .text which is used for code */
+  .text : { 
+  	src/start/Cstartup*.o (.text) 
+	*(.dfu.func)
+	*(.dfu.struct)
+  } >FLASH
+  _etext_dfu = . ;
+
+  .data : AT (_etext_dfu) {
+  	_data_dfu = . ;
+    	KEEP(*(.vectram))
+  	src/dfu.o (.data)
+  	_edata_dfu = . ;
+  } >DATA
+	
+  .text : {
+  	. = 0x00002000;
+  	*(.dfu.functab)
+
+    	. = 0x00002100;
+	*(.text)                   /* remaining code */
+
+    *(.glue_7t) *(.glue_7)
+
+  } >FLASH
+
+  . = ALIGN(4);
+
+  /* .rodata section which is used for read-only data (constants) */
+
+  .rodata :
+  {
+    *(.rodata)
+  } >FLASH
+
+  . = ALIGN(4);
+
+  _etext = . ;
+  PROVIDE (etext = .);
+
+  /* .data section which is used for initialized data */
+
+  .data : AT (_etext)
+  {
+    _data = . ;
+    *(.data)
+    SORT(CONSTRUCTORS)
+    . = ALIGN(4);
+    *(.fastrun)
+  } >DATA
+  . = ALIGN(4);
+
+  _edata = . ;
+   PROVIDE (edata = .);
+
+  /* .bss section which is used for uninitialized data */
+
+  .bss :
+  {
+    __bss_start = . ;
+    __bss_start__ = . ;
+    *(.bss)
+    *(COMMON)
+  } 
+  . = ALIGN(4);
+  __bss_end__ = . ;
+  __bss_end__ = . ;
+  _end = .;
+    . = ALIGN(4);
+   .int_data :  
+   { 
+   *(.internal_ram_top) 
+   }> STACK 
+
+  PROVIDE (end = .);
+
+  /* Stabs debugging sections.  */
+  .stab          0 : { *(.stab) }
+  .stabstr       0 : { *(.stabstr) }
+  .stab.excl     0 : { *(.stab.excl) }
+  .stab.exclstr  0 : { *(.stab.exclstr) }
+  .stab.index    0 : { *(.stab.index) }
+  .stab.indexstr 0 : { *(.stab.indexstr) }
+  .comment       0 : { *(.comment) }
+  /* DWARF debug sections.
+     Symbols in the DWARF debugging sections are relative to the beginning
+     of the section so we begin them at 0.  */
+  /* DWARF 1 */
+  .debug          0 : { *(.debug) }
+  .line           0 : { *(.line) }
+  /* GNU DWARF 1 extensions */
+  .debug_srcinfo  0 : { *(.debug_srcinfo) }
+  .debug_sfnames  0 : { *(.debug_sfnames) }
+  /* DWARF 1.1 and DWARF 2 */
+  .debug_aranges  0 : { *(.debug_aranges) }
+  .debug_pubnames 0 : { *(.debug_pubnames) }
+  /* DWARF 2 */
+  .debug_info     0 : { *(.debug_info .gnu.linkonce.wi.*) }
+  .debug_abbrev   0 : { *(.debug_abbrev) }
+  .debug_line     0 : { *(.debug_line) }
+  .debug_frame    0 : { *(.debug_frame) }
+  .debug_str      0 : { *(.debug_str) }
+  .debug_loc      0 : { *(.debug_loc) }
+  .debug_macinfo  0 : { *(.debug_macinfo) }
+  /* SGI/MIPS DWARF 2 extensions */
+  .debug_weaknames 0 : { *(.debug_weaknames) }
+  .debug_funcnames 0 : { *(.debug_funcnames) }
+  .debug_typenames 0 : { *(.debug_typenames) }
+  .debug_varnames  0 : { *(.debug_varnames) }
+
+}
diff --git a/openpcd/firmware/src/dfu.c b/openpcd/firmware/src/dfu.c
index a1624e9..a56e538 100644
--- a/openpcd/firmware/src/dfu.c
+++ b/openpcd/firmware/src/dfu.c
@@ -27,11 +27,88 @@
 #define DEBUGI(x, args ...)
 #endif
 
+/* this is only called once before DFU mode, no __dfufunc required */
+void udp_init(void)
+{
+	/* Set the PLL USB Divider */
+	AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
+
+	/* Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock */
+	AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_UDP;
+	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_UDP);
+
+	/* Enable UDP PullUp (USB_DP_PUP) : enable & Clear of the corresponding PIO
+	 * Set in PIO mode and Configure in Output */
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+}
+
+/* Send Data through the control endpoint */
+static void __dfufunc udp_ep0_send_data(const char *pData, u_int32_t length)
+{
+	AT91PS_UDP pUdp = AT91C_BASE_UDP;
+	u_int32_t cpt = 0;
+	AT91_REG csr;
+
+	DEBUGE("send_data: %u bytes ", length);
+
+	do {
+		cpt = MIN(length, 8);
+		length -= cpt;
+
+		while (cpt--)
+			pUdp->UDP_FDR[0] = *pData++;
+
+		if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
+			pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+			while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+		}
+
+		pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
+		do {
+			csr = pUdp->UDP_CSR[0];
+
+			/* Data IN stage has been stopped by a status OUT */
+			if (csr & AT91C_UDP_RX_DATA_BK0) {
+				pUdp->UDP_CSR[0] &= ~(AT91C_UDP_RX_DATA_BK0);
+				DEBUGE("stopped by status out ");
+				return;
+			}
+		} while (!(csr & AT91C_UDP_TXCOMP));
+
+	} while (length);
+
+	if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
+		pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+		while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+	}
+}
+
+/* Send zero length packet through the control endpoint */
+static void __dfufunc udp_ep0_send_zlp(void)
+{
+	AT91PS_UDP pUdp = AT91C_BASE_UDP;
+	pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
+	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP)) ;
+	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+	while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+}
+
+/* Stall the control endpoint */
+static void __dfufunc udp_ep0_send_stall(void)
+{
+	AT91PS_UDP pUdp = AT91C_BASE_UDP;
+	pUdp->UDP_CSR[0] |= AT91C_UDP_FORCESTALL;
+	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_ISOERROR)) ;
+	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR);
+	while (pUdp->UDP_CSR[0] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR)) ;
+}
+
+
 static u_int8_t status;
 static u_int8_t *ptr;
-enum dfu_state dfu_state;
+static u_int8_t dfu_state;
 
-static int __ramfunc handle_dnload(u_int16_t val, u_int16_t len)
+static int __dfufunc handle_dnload(u_int16_t val, u_int16_t len)
 {
 	volatile u_int32_t *p = (volatile u_int32_t *)ptr;
 
@@ -67,7 +144,7 @@ static int __ramfunc handle_dnload(u_int16_t val, u_int16_t len)
 	return 0;
 }
 
-static __ramfunc int handle_upload(u_int16_t val, u_int16_t len)
+static __dfufunc int handle_upload(u_int16_t val, u_int16_t len)
 {
 	DEBUGE("upload ");
 	if (len > AT91C_IFLASH_PAGE_SIZE
@@ -88,7 +165,7 @@ static __ramfunc int handle_upload(u_int16_t val, u_int16_t len)
 	return len;
 }
 
-static __ramfunc void handle_getstatus(void)
+static __dfufunc void handle_getstatus(void)
 {
 	struct dfu_status dstat;
 
@@ -101,7 +178,7 @@ static __ramfunc void handle_getstatus(void)
 	udp_ep0_send_data(&dstat, sizeof(dstat));
 }
 
-static void __ramfunc handle_getstate(void)
+static void __dfufunc handle_getstate(void)
 {
 	u_int8_t u8 = dfu_state;
 	DEBUGE("getstate ");
@@ -109,7 +186,7 @@ static void __ramfunc handle_getstate(void)
 }
 
 /* callback function for DFU requests */
-int __ramfunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
+int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 		    u_int16_t val, u_int16_t len)
 {
 	int rc;
@@ -290,7 +367,7 @@ send_zlp:
 static u_int8_t cur_config;
 
 /* USB DFU Device descriptor in DFU mode */
-struct usb_device_descriptor dfu_dev_descriptor = {
+__dfustruct struct usb_device_descriptor dfu_dev_descriptor = {
 	.bLength		= USB_DT_DEVICE_SIZE,
 	.bDescriptorType	= USB_DT_DEVICE,
 	.bcdUSB			= 0x0100,
@@ -308,7 +385,7 @@ struct usb_device_descriptor dfu_dev_descriptor = {
 };
 
 /* USB DFU Config descriptor in DFU mode */
-struct _dfu_desc dfu_cfg_descriptor = {
+__dfustruct struct _dfu_desc dfu_cfg_descriptor = {
 	.ucfg = {
 		.bLength = USB_DT_CONFIG_SIZE,
 		.bDescriptorType = USB_DT_CONFIG,
@@ -349,7 +426,7 @@ struct _dfu_desc dfu_cfg_descriptor = {
 
 
 /* minimal USB EP0 handler in DFU mode */
-static __ramfunc void dfu_udp_ep0_handler(void)
+static __dfufunc void dfu_udp_ep0_handler(void)
 {
 	AT91PS_UDP pUDP = AT91C_BASE_UDP;
 	u_int8_t bmRequestType, bRequest;
@@ -524,7 +601,7 @@ static __ramfunc void dfu_udp_ep0_handler(void)
 }
 
 /* minimal USB IRQ handler in DFU mode */
-static __ramfunc void dfu_udp_irq(void)
+static __dfufunc void dfu_udp_irq(void)
 {
 	AT91PS_UDP pUDP = AT91C_BASE_UDP;
 	AT91_REG isr = pUDP->UDP_ISR;
@@ -550,7 +627,8 @@ static __ramfunc void dfu_udp_irq(void)
 	AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_UDP);
 }
 
-void dfu_switch(void)
+/* this is only called once before DFU mode, no __dfufunc required */
+static void dfu_switch(void)
 {
 	AT91PS_AIC pAic = AT91C_BASE_AIC;
 
@@ -559,3 +637,42 @@ void dfu_switch(void)
 	pAic->AIC_SVR[AT91C_ID_UDP] = (unsigned int) &dfu_udp_irq;
 	dfu_state = DFU_STATE_dfuIDLE;
 }
+
+void __dfufunc dfu_main(void)
+{
+	/*
+	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;
+	pAic->AIC_SMR[AT91C_ID_UDP] = AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL | 
+					OPENPCD_IRQ_PRIO_UDP;
+	pAic->AIC_ICCR = 1 << AT91C_ID_UDP;
+
+	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_UDP);
+
+	/* End-of-Bus-Reset is always enabled */
+
+	/* Clear for set the Pull up resistor */
+	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+
+	/* do nothing, since all of DFU is interrupt driven */
+	while (1) ;
+}
+
+struct dfuapi __dfufunctab dfu_api = {
+	.ep0_send_data		= &udp_ep0_send_data,
+	.ep0_send_zlp		= &udp_ep0_send_zlp,
+	.ep0_send_stall		= &udp_ep0_send_stall,
+	.dfu_ep0_handler	= &dfu_ep0_handler,
+	.dfu_switch		= &dfu_switch,
+	.dfu_state		= &dfu_state,
+	.dfu_dev_descriptor	= &dfu_dev_descriptor,
+	.dfu_cfg_descriptor	= &dfu_cfg_descriptor,
+};
+
+int foo = 12345;
diff --git a/openpcd/firmware/src/dfu.h b/openpcd/firmware/src/dfu.h
index 4372043..a663688 100644
--- a/openpcd/firmware/src/dfu.h
+++ b/openpcd/firmware/src/dfu.h
@@ -38,9 +38,25 @@
 	.iInterface		= 1,					\
 }
 
+#define __dfufunc __attribute__ ((long_call, section (".dfu.func")))
+#define __dfustruct __attribute__ ((section (".dfu.struct"))) const
+#define __dfufunctab  __attribute__ ((section (".dfu.functab")))
+	
+#if 0
+extern void __dfufunc udp_ep0_send_data(const char *data, u_int32_t length);
+extern void __dfufunc udp_ep0_send_zlp(void);
+extern void __dfufunc udp_ep0_send_stall(void);
+extern __dfustruct struct usb_device_descriptor dfu_dev_descriptor;
+extern __dfustruct struct _dfu_desc dfu_cfg_descriptor;
+extern void dfu_switch(void);
+extern int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
+				     u_int16_t val, u_int16_t len);
+extern static u_int8_t dfu_state;
 struct udp_pcd;
+#endif
 
-extern struct usb_device_descriptor dfu_dev_descriptor;
+
+extern void udp_init(void);
 
 struct _dfu_desc {
 	struct usb_config_descriptor ucfg;
@@ -48,12 +64,17 @@ struct _dfu_desc {
 	struct usb_dfu_func_descriptor func_dfu;
 };
 
-extern struct _dfu_desc dfu_cfg_descriptor;
-
-extern void dfu_switch(void);
-extern int __ramfunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
+struct dfuapi {
+	void (*ep0_send_data)(const char *data, u_int32_t len);
+	void (*ep0_send_zlp)(void);
+	void (*ep0_send_stall)(void);
+	int  (*dfu_ep0_handler)(u_int8_t req_type, u_int8_t req,
 				     u_int16_t val, u_int16_t len);
+	void (*dfu_switch)(void);
+	u_int8_t *dfu_state;
+	struct usb_device_descriptor *dfu_dev_descriptor;
+	struct _dfu_desc *dfu_cfg_descriptor;
+};
+
 
-extern enum dfu_state dfu_state;
-	
 #endif /* _DFU_H */
diff --git a/openpcd/firmware/src/flash.c b/openpcd/firmware/src/flash.c
index cca7fb7..2aaf760 100644
--- a/openpcd/firmware/src/flash.c
+++ b/openpcd/firmware/src/flash.c
@@ -9,3 +9,37 @@
 #define EFCS_CMD_CLEAR_NVM_BIT		0x0d
 #define EFCS_CMD_SET_SECURITY_BIT	0x0f
 
+
+int unlock_page(u_int16_t page)
+{
+	AT91C_MC_FCMD_UNLOCK | AT91C_MC_CORRECT_KEY | 
+
+}
+
+int flash_sector(unsigned int sector, const u_int8_t *data, unsigned int len)
+{
+	volatile u_int32_t *p = (volatile u_int32_t *)0;
+	u_int32_t *src32 = (u_int32_t *)data;
+	int i;
+
+	/* hand-code memcpy because we need to make sure only 32bit accesses
+	 * are used */
+	for (i = 0; i < len/4; i++)
+		p[i] = src32[i];
+
+	AT91F_MC_EFC_PerformCmd(pmc , AT91C_MC_FCMD_START_PROG|
+				AT91C_MC_CORRECT_KEY | );
+}
+
+
+void flash_init(void)
+{
+	unsigned int fmcn = AT91F_MC_EFC_ComputerFMCN(48000000);
+
+	AT91F_MC_EFC_CfgModeReg(ff, fmcn << 16 | AT91C_MC_FWS_3FWS |
+				AT91C_MC_FRDY | AT91C_MC_LOCKE | 
+				AT91C_MC_PROGE);
+
+	AT91F_AIC_EnableIt();
+
+}
diff --git a/openpcd/firmware/src/openpcd.h b/openpcd/firmware/src/openpcd.h
index 51f3592..ec577d3 100644
--- a/openpcd/firmware/src/openpcd.h
+++ b/openpcd/firmware/src/openpcd.h
@@ -28,6 +28,22 @@
 #define OPENPCD_PIO_TRIGGER		AT91C_PIO_PA31
 #define OPENPCD_PIO_CARRIER_IN		AT91C_PA28_TCLK1
 
+#ifdef CONFIG_PICCSIM
+#define OPENPICC_PIO_PLL_INHIBIT	AT91C_PIO_PA24
+#define OPENPICC_PIO_SS1_GAIN		AT91C_PIO_PA11
+#define OPENPICC_PIO_SS2_DT_THRESH	AT91C_PIO_PA8
+#define OPENPICC_PIO_BOOTLDR		AT91C_PIO_PA6
+#define OPENPICC_PIO_SLAVE_RESET	AT91C_PIO_PA5
+#define OPENPICC_PIO_LOAD1		AT91C_PIO_PA2
+#define OPENPICC_PIO_LOAD2		AT91C_PIO_PA3
+#define OPENPICC_PIO_AB_DETECT		AT91C_PIO_PA20
+
+#define OPENPICC_ADC_FIELD_STRENGTH	AT91C_ADC_CH4
+#define OPENPICC_ADC_PLL_DEM		AT91C_ADC_CH5
+#define OPENPICC_ADC_AN1		AT91C_ADC_CH6
+#define OPENPICC_ADC_AN2		AT91C_ADC_CH7
+#endif
+
 #define OPENPCD_IRQ_PRIO_SPI	AT91C_AIC_PRIOR_HIGHEST
 #define OPENPCD_IRQ_PRIO_SSC	(AT91C_AIC_PRIOR_HIGHEST-1)
 #define OPENPCD_IRQ_PRIO_UDP	(AT91C_AIC_PRIOR_LOWEST+1)
diff --git a/openpcd/firmware/src/pcd_enumerate.c b/openpcd/firmware/src/pcd_enumerate.c
index 75c0279..4d89e87 100644
--- a/openpcd/firmware/src/pcd_enumerate.c
+++ b/openpcd/firmware/src/pcd_enumerate.c
@@ -37,6 +37,15 @@
 #define AT91C_EP_IN_SIZE 0x40
 #define AT91C_EP_INT  3
 
+#ifdef CONFIG_DFU
+#define DFU_API_LOCATION	((const struct dfuapi *) 0x1f00)
+static const struct dfuapi *dfu = DFU_API_LOCATION;
+#define udp_ep0_send_data	dfu->ep0_send_data
+#define udp_ep0_send_zlp	dfu->ep0_send_zlp
+#define udp_ep0_send_stall	dfu->ep0_send_stall
+#else
+#error non-DFU builds currently not supported (yet) again
+#endif
 
 static struct udp_pcd upcd;
 
@@ -128,7 +137,7 @@ const struct _desc cfg_descriptor = {
 #endif
 };
 
-static struct usb_string_descriptor string0 = {
+static const struct usb_string_descriptor string0 = {
 	.bLength = sizeof(string0),
 	.bDescriptorType = USB_DT_STRING,
 	.wData[0] = 0x0409,	/* English */
@@ -202,10 +211,12 @@ static void udp_irq(void)
 		pUDP->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
 		upcd.cur_config = 0;
 		
-		if (dfu_state == DFU_STATE_appDETACH) {
+#ifdef CONFIG_DFU
+		if (*dfu->dfu_state == DFU_STATE_appDETACH) {
 			/* now we need to switch to DFU mode */
-			dfu_switch();
+			dfu->dfu_switch();
 		}
+#endif
 	}
 
 	if (isr & AT91C_UDP_EPINT0) {
@@ -313,13 +324,13 @@ static void udp_irq(void)
 }
 
 /* Open USB Device Port  */
-static int udp_open(AT91PS_UDP pUdp)
+void udp_open(void)
 {
 	DEBUGPCRF("entering");
-	upcd.pUdp = pUdp;
+	upcd.pUdp = AT91C_BASE_UDP;
 	upcd.cur_config = 0;
 	upcd.cur_rcv_bank = AT91C_UDP_RX_DATA_BK0;
-	
+
 	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_UDP,
 			      OPENPCD_IRQ_PRIO_UDP,
 			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, udp_irq);
@@ -327,26 +338,8 @@ static int udp_open(AT91PS_UDP pUdp)
 
 	/* End-of-Bus-Reset is always enabled */
 
-	return 0;
-}
-
-void udp_init(void)
-{
-	/* Set the PLL USB Divider */
-	AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
-
-	/* Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock */
-	AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_UDP;
-	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_UDP);
-
-	/* Enable UDP PullUp (USB_DP_PUP) : enable & Clear of the corresponding PIO
-	 * Set in PIO mode and Configure in Output */
-	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
-	/* Clear for set the Pul up resistor */
+	/* Clear for set the Pull up resistor */
 	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
-
-	/* CDC Open by structure initialization */
-	udp_open(AT91C_BASE_UDP);
 }
 
 void udp_reset(void)
@@ -365,67 +358,6 @@ void udp_reset(void)
 #define DEBUGE(x, args ...)	do { } while (0)
 #endif
 
-/* Send Data through the control endpoint */
-void __ramfunc udp_ep0_send_data(const char *pData, u_int32_t length)
-{
-	AT91PS_UDP pUdp = AT91C_BASE_UDP;
-	u_int32_t cpt = 0;
-	AT91_REG csr;
-
-	DEBUGE("send_data: %u bytes ", length);
-
-	do {
-		cpt = MIN(length, 8);
-		length -= cpt;
-
-		while (cpt--)
-			pUdp->UDP_FDR[0] = *pData++;
-
-		if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
-			pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
-			while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
-		}
-
-		pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
-		do {
-			csr = pUdp->UDP_CSR[0];
-
-			/* Data IN stage has been stopped by a status OUT */
-			if (csr & AT91C_UDP_RX_DATA_BK0) {
-				pUdp->UDP_CSR[0] &= ~(AT91C_UDP_RX_DATA_BK0);
-				DEBUGE("stopped by status out ");
-				return;
-			}
-		} while (!(csr & AT91C_UDP_TXCOMP));
-
-	} while (length);
-
-	if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
-		pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
-		while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
-	}
-}
-
-/* Send zero length packet through the control endpoint */
-void __ramfunc udp_ep0_send_zlp(void)
-{
-	AT91PS_UDP pUdp = AT91C_BASE_UDP;
-	pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
-	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP)) ;
-	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
-	while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
-}
-
-/* Stall the control endpoint */
-void __ramfunc udp_ep0_send_stall(void)
-{
-	AT91PS_UDP pUdp = AT91C_BASE_UDP;
-	pUdp->UDP_CSR[0] |= AT91C_UDP_FORCESTALL;
-	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_ISOERROR)) ;
-	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR);
-	while (pUdp->UDP_CSR[0] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR)) ;
-}
-
 /* Handle requests on the USB Control Endpoint */
 static void udp_ep0_handler(void)
 {
@@ -484,10 +416,10 @@ static void udp_ep0_handler(void)
 		if (wValue == 0x100) {
 			/* Return Device Descriptor */
 #ifdef CONFIG_DFU
-			if (dfu_state != DFU_STATE_appIDLE)
+			if (*dfu->dfu_state != DFU_STATE_appIDLE)
 				udp_ep0_send_data((const char *) 
-						  &dfu_dev_descriptor,
-						  MIN(sizeof(dfu_dev_descriptor),
+						  dfu->dfu_dev_descriptor,
+						  MIN(dfu->dfu_dev_descriptor->bLength,
 						      wLength));
 			else
 #endif
@@ -496,10 +428,10 @@ static void udp_ep0_handler(void)
 		} else if (wValue == 0x200) {
 			/* Return Configuration Descriptor */
 #ifdef CONFIG_DFU
-			if (dfu_state != DFU_STATE_appIDLE)
+			if (*dfu->dfu_state != DFU_STATE_appIDLE)
 				udp_ep0_send_data((const char *)
-						  &dfu_cfg_descriptor,
-						  MIN(sizeof(dfu_cfg_descriptor),
+						  dfu->dfu_cfg_descriptor,
+						  MIN(dfu->dfu_cfg_descriptor->ucfg.wTotalLength,
 						      wLength));
 			else
 #endif
@@ -667,7 +599,7 @@ static void udp_ep0_handler(void)
 		DEBUGE("DEFAULT(req=0x%02x, type=0x%02x) ", bRequest, bmRequestType);
 #ifdef CONFIG_DFU
 		if ((bmRequestType & 0x3f) == USB_TYPE_DFU) {
-			dfu_ep0_handler(bmRequestType, bRequest, wValue, wLength);
+			dfu->dfu_ep0_handler(bmRequestType, bRequest, wValue, wLength);
 		} else
 #endif
 		udp_ep0_send_stall();
diff --git a/openpcd/firmware/src/pcd_enumerate.h b/openpcd/firmware/src/pcd_enumerate.h
index e4ad57d..57ff88c 100644
--- a/openpcd/firmware/src/pcd_enumerate.h
+++ b/openpcd/firmware/src/pcd_enumerate.h
@@ -9,11 +9,7 @@
 
 struct req_ctx;
 
-extern void __ramfunc udp_ep0_send_data(const char *data, u_int32_t length);
-extern void __ramfunc udp_ep0_send_zlp(void);
-extern void __ramfunc udp_ep0_send_stall(void);
-
-extern void udp_init(void);
+extern void udp_open(void);
 extern int udp_refill_ep(int ep, struct req_ctx *rctx);
 extern void udp_unthrottle(void);
 extern void udp_reset(void);
diff --git a/openpcd/firmware/src/start/Cstartup.S b/openpcd/firmware/src/start/Cstartup.S
index 12bc4cd..68e1a07 100644
--- a/openpcd/firmware/src/start/Cstartup.S
+++ b/openpcd/firmware/src/start/Cstartup.S
@@ -149,6 +149,32 @@ exit:
 		.size   exit, . - exit
         .endfunc
 
+/*---------------------------------------------------------------
+//* ?EXEPTION_VECTOR
+//* This module is only linked if needed for closing files.
+//*---------------------------------------------------------------*/
+        .global AT91F_Default_FIQ_handler
+        .func   AT91F_Default_FIQ_handler
+AT91F_Default_FIQ_handler:
+            b     AT91F_Default_FIQ_handler
+        .size   AT91F_Default_FIQ_handler, . - AT91F_Default_FIQ_handler
+        .endfunc
+
+        .global AT91F_Default_IRQ_handler
+        .func   AT91F_Default_IRQ_handler
+AT91F_Default_IRQ_handler:
+            b     AT91F_Default_IRQ_handler
+        .size   AT91F_Default_IRQ_handler, . - AT91F_Default_IRQ_handler
+        .endfunc
+
+        .global AT91F_Spurious_handler
+        .func   AT91F_Spurious_handler
+AT91F_Spurious_handler:
+            b     AT91F_Spurious_handler
+        .size   AT91F_Spurious_handler, . - AT91F_Spurious_handler
+        .endfunc
+
+
 
 #;------------------------------------------------------------------------------
 #;- Section Definition
@@ -284,30 +310,5 @@ LoopZI:         CMP     R1, R2
         .size   _startup, . - _startup
         .endfunc
 		
-/*---------------------------------------------------------------
-//* ?EXEPTION_VECTOR
-//* This module is only linked if needed for closing files.
-//*---------------------------------------------------------------*/
-        .global AT91F_Default_FIQ_handler
-        .func   AT91F_Default_FIQ_handler
-AT91F_Default_FIQ_handler:
-            b     AT91F_Default_FIQ_handler
-        .size   AT91F_Default_FIQ_handler, . - AT91F_Default_FIQ_handler
-        .endfunc
-
-        .global AT91F_Default_IRQ_handler
-        .func   AT91F_Default_IRQ_handler
-AT91F_Default_IRQ_handler:
-            b     AT91F_Default_IRQ_handler
-        .size   AT91F_Default_IRQ_handler, . - AT91F_Default_IRQ_handler
-        .endfunc
-
-        .global AT91F_Spurious_handler
-        .func   AT91F_Spurious_handler
-AT91F_Spurious_handler:
-            b     AT91F_Spurious_handler
-        .size   AT91F_Spurious_handler, . - AT91F_Spurious_handler
-        .endfunc
-
         .end