- add S256 linker script

- add USB transmit routines
- connect usb OUT endpoint with parser / spi


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

17 files changed, 468 insertions, 85 deletions

diff --git a/openpcd/firmware/Makefile b/openpcd/firmware/Makefile
index f9f81fc..a3968a4 100644
--- a/openpcd/firmware/Makefile
+++ b/openpcd/firmware/Makefile
@@ -40,7 +40,8 @@
 
 # MCU name and submodel
 MCU      = arm7tdmi
-SUBMDL   = AT91SAM7S64
+#SUBMDL   = AT91SAM7S64
+SUBMDL   = AT91SAM7S256
 
 USE_THUMB_MODE = NO
 #USE_THUMB_MODE = YES
@@ -70,7 +71,7 @@ SRC =
 
 # List C source files here which must be compiled in ARM-Mode.
 # use file-extension c for "c-only"-files
-SRCARM  = lib/lib_AT91SAM7S64.c src/pcd_enumerate.c src/fifo.c src/dbgu.c src/led.c src/rc632.c src/rc632_debug.c src/$(TARGET).c compil/SrcWinARM/Cstartup_SAM7.c 
+SRCARM  = lib/lib_AT91SAM7S64.c src/pcd_enumerate.c src/fifo.c src/dbgu.c src/led.c src/rc632.c src/rc632_debug.c src/req_ctx.c src/$(TARGET).c compil/SrcWinARM/Cstartup_SAM7.c 
 
 # List C++ source files here.
 # use file-extension cpp for C++-files (use extension .cpp)
@@ -134,7 +135,7 @@ AT91LIBNOWARN = yes
 CSTANDARD = -std=gnu99
 
 # Place -D or -U options for C here
-CDEFS =  -D$(RUN_MODE) -DDEBUG -DOLIMEX
+CDEFS =  -D$(RUN_MODE) -DOLIMEX -DDEBUG 
 
 # Place -I options here
 CINCS = -Ihelper -Isrc
diff --git a/openpcd/firmware/compil/SrcWinARM/AT91SAM7S256-RAM.ld b/openpcd/firmware/compil/SrcWinARM/AT91SAM7S256-RAM.ld
new file mode 100644
index 0000000..5eab239
--- /dev/null
+++ b/openpcd/firmware/compil/SrcWinARM/AT91SAM7S256-RAM.ld
@@ -0,0 +1,131 @@
+/*---------------------------------------------------------------------------*/

+/*-         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_RAM.ld                                        */

+/*- Object               : Linker Script File for RAM 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 .

+   - modified the region where the .text section is loaded to DATA

+   - tested succesfully with AT91SAM7 GPIO Example and PEEDI debugger

+     on AT91SAM7S-EK equipped with AT91SAM7S256 

+*/

+

+/* Memory Definitions */

+

+MEMORY

+{

+  FLASH (rx) : ORIGIN = 0x00100000, LENGTH = 0x00040000

+  DATA (rw)  : ORIGIN = 0x00200000, LENGTH = 0x00010000

+  STACK (rw) : ORIGIN = 0x00210000, LENGTH = 0x00000000

+}

+

+

+/* Section Definitions */

+

+SECTIONS

+{

+  /* first section is .text which is used for code */

+  . = 0x0000000;

+  .text : { *cstartup.o (.text) }>DATA =0

+  .text :

+  {

+    *(.text)                   /* remaining code */

+

+    *(.glue_7t) *(.glue_7)

+

+  } >DATA

+  . = ALIGN(4);

+

+  /* .rodata section which is used for read-only data (constants) */

+

+  .rodata :

+  {

+    *(.rodata)

+  } >DATA

+

+  . = ALIGN(4);

+

+  _etext = . ;

+  PROVIDE (etext = .);

+

+  /* .data section which is used for initialized data */

+

+  .data : AT (_etext)

+  {

+    _data = . ;

+    *(.data)

+    SORT(CONSTRUCTORS)

+  } >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/compil/SrcWinARM/AT91SAM7S256-ROM.ld b/openpcd/firmware/compil/SrcWinARM/AT91SAM7S256-ROM.ld
new file mode 100644
index 0000000..e1dfd64
--- /dev/null
+++ b/openpcd/firmware/compil/SrcWinARM/AT91SAM7S256-ROM.ld
@@ -0,0 +1,131 @@
+/*---------------------------------------------------------------------------*/

+/*-         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 

+*/

+

+/* Memory Definitions */

+

+MEMORY

+{

+  FLASH (rx) : ORIGIN = 0x00100000, LENGTH = 0x00040000

+  DATA (rw)  : ORIGIN = 0x00200000, LENGTH = 0x00010000

+  STACK (rw) : ORIGIN = 0x00210000, LENGTH = 0x00000000

+}

+

+

+/* Section Definitions */

+

+SECTIONS

+{

+  /* first section is .text which is used for code */

+  . = 0x0000000;

+  .text : { *cstartup.o (.text) }>FLASH =0

+  .text :

+  {

+    *(.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)

+  } >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/compil/SrcWinARM/Cstartup_SAM7.c b/openpcd/firmware/compil/SrcWinARM/Cstartup_SAM7.c
index 94a2bbe..b5a6dc5 100644
--- a/openpcd/firmware/compil/SrcWinARM/Cstartup_SAM7.c
+++ b/openpcd/firmware/compil/SrcWinARM/Cstartup_SAM7.c
@@ -51,15 +51,21 @@ AT91F_LowLevelInit (void)
   while (!(pPMC->PMC_SR & AT91C_PMC_MOSCS));
   // 2 Checking the Main Oscillator Frequency (Optional)
   // 3 Setting PLL and divider:
-  // - div by 5 Fin = 3,6864 =(18,432 / 5)
-  // - Mul 25+1: Fout =   95,8464 =(3,6864 *26)
+  // - div by 24 Fin = 0,7680 =(18,432 / 24)
+  // - Mul 125: Fout =   96,0000 =(0,7680 *125)
   // for 96 MHz the erroe is 0.16%
   // Field out NOT USED = 0
   // PLLCOUNT pll startup time estimate at : 0.844 ms
   // PLLCOUNT 28 = 0.000844 /(1/32768)
+#if 0
   pPMC->PMC_PLLR = ((AT91C_CKGR_DIV & 0x05) |
 		    (AT91C_CKGR_PLLCOUNT & (28 << 8)) |
 		    (AT91C_CKGR_MUL & (25 << 16)));
+#else
+  pPMC->PMC_PLLR = ((AT91C_CKGR_DIV & 24) |
+		    (AT91C_CKGR_PLLCOUNT & (28 << 8)) |
+		    (AT91C_CKGR_MUL & (125 << 16)));
+#endif
 
   // Wait the startup time
   while (!(pPMC->PMC_SR & AT91C_PMC_LOCK));
diff --git a/openpcd/firmware/include/AT91SAM7S64.h b/openpcd/firmware/include/AT91SAM7S64.h
index 1b12210..4788c56 100644
--- a/openpcd/firmware/include/AT91SAM7S64.h
+++ b/openpcd/firmware/include/AT91SAM7S64.h
@@ -1332,7 +1332,7 @@ typedef struct _AT91S_UDP {
 #define AT91C_UDP_FEN         ((unsigned int) 0x1 <<  8) // (UDP) Function Enable
 // -------- UDP_IER : (UDP Offset: 0x10) USB Interrupt Enable Register -------- 
 #define AT91C_UDP_EPINT0      ((unsigned int) 0x1 <<  0) // (UDP) Endpoint 0 Interrupt
-#define AT91C_UDP_EPINT1      ((unsigned int) 0x1 <<  1) // (UDP) Endpoint 0 Interrupt
+#define AT91C_UDP_EPINT1      ((unsigned int) 0x1 <<  1) // (UDP) Endpoint 1 Interrupt
 #define AT91C_UDP_EPINT2      ((unsigned int) 0x1 <<  2) // (UDP) Endpoint 2 Interrupt
 #define AT91C_UDP_EPINT3      ((unsigned int) 0x1 <<  3) // (UDP) Endpoint 3 Interrupt
 #define AT91C_UDP_RXSUSP      ((unsigned int) 0x1 <<  8) // (UDP) USB Suspend Interrupt
diff --git a/openpcd/firmware/include/lib_AT91SAM7S64.h b/openpcd/firmware/include/lib_AT91SAM7S64.h
index 0f7c7df..4c43c25 100644
--- a/openpcd/firmware/include/lib_AT91SAM7S64.h
+++ b/openpcd/firmware/include/lib_AT91SAM7S64.h
@@ -499,8 +499,8 @@ static inline int AT91F_PIO_IsInputSet(
 //* \brief Set to 1 output PIO
 //*----------------------------------------------------------------------------
 static inline void AT91F_PIO_SetOutput(
-	AT91PS_PIO pPio,   // \arg  pointer to a PIO controller
-	unsigned int flag) // \arg  output to be set
+	const AT91PS_PIO pPio,   // \arg  pointer to a PIO controller
+	const unsigned int flag) // \arg  output to be set
 {
 	pPio->PIO_SODR = flag;
 }
diff --git a/openpcd/firmware/include/openpcd.h b/openpcd/firmware/include/openpcd.h
index cd85af2..e7183cc 100644
--- a/openpcd/firmware/include/openpcd.h
+++ b/openpcd/firmware/include/openpcd.h
@@ -1,5 +1,5 @@
-#ifndef _OPENPCD_H
-#define _OPENPCD_H
+#ifndef _OPENPCD_PROTO_H
+#define _OPENPCD_PROTO_H
 
 #include <include/types.h>
 #include <include/AT91SAM7S64.h>
@@ -10,6 +10,7 @@ struct openpcd_hdr {
 	u_int8_t reg;		/* register */
 	u_int8_t val;		/* value (in case of write *) */
 	u_int16_t len;
+	u_int16_t res;
 	u_int8_t data[0];
 } __attribute__ ((packed));
 
diff --git a/openpcd/firmware/include/types.h b/openpcd/firmware/include/types.h
index c5c036c..f17ffe5 100644
--- a/openpcd/firmware/include/types.h
+++ b/openpcd/firmware/include/types.h
@@ -6,4 +6,9 @@ typedef unsigned short	u_int16_t;
 typedef unsigned int	u_int32_t;
 typedef unsigned long long u_int64_t;
 
+typedef	signed char	int8_t;
+typedef signed short	int16_t;
+typedef signed int	int32_t;
+typedef signed long long int64_t;
+
 #endif
diff --git a/openpcd/firmware/src/dbgu.c b/openpcd/firmware/src/dbgu.c
index 7c94141..fd7c765 100644
--- a/openpcd/firmware/src/dbgu.c
+++ b/openpcd/firmware/src/dbgu.c
@@ -20,6 +20,7 @@
 #include "Board.h"
 #include "dbgu.h"
 #include "rc632_debug.h"
+#include "openpcd.h"
 #define USART_SYS_LEVEL 4
 /*---------------------------- Global Variable ------------------------------*/
 //*--------------------------1--------------------------------------------------
@@ -61,10 +62,10 @@ static void DBGU_irq_handler(void)
 	case '0':		//* info
 		AT91F_DBGU_Frame("Set Pull up\n\r");
 		// Set
-		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, AT91C_PIO_PA16);
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, AT91C_PIO_UDP_PUP);
 		break;
 	case '1':		//* info
-		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, AT91C_PIO_PA16);
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, AT91C_PIO_UDP_PUP);
 		AT91F_DBGU_Printk("Clear Pull up\n\r");
 		// Reset Application
 		Send_reset();
diff --git a/openpcd/firmware/src/dbgu.h b/openpcd/firmware/src/dbgu.h
index 8f7f0a4..c38fd63 100644
--- a/openpcd/firmware/src/dbgu.h
+++ b/openpcd/firmware/src/dbgu.h
@@ -33,7 +33,7 @@ void AT91F_DBGU_scanf(char * type,unsigned int * val);
 extern void debugp(const char *format, ...);

 #define DEBUGP(x, args ...) debugp(x, ## args)

 #else

-#define	DEBUGP(x, args ...)

+#define	DEBUGP(x, args ...) do {} while(0)

 #endif

 

 #endif /* dbgu_h */

diff --git a/openpcd/firmware/src/led.c b/openpcd/firmware/src/led.c
index 7f5db72..88e1026 100644
--- a/openpcd/firmware/src/led.c
+++ b/openpcd/firmware/src/led.c
@@ -43,13 +43,10 @@ int led_toggle(int led)
 	if (on == -1)
 		return -1;
 
-	if (on) {
-		DEBUGP("led%d on, switching off\r\n", led);
+	if (on)
 		led_switch(led, 0);
-	} else {
-		DEBUGP("led%d off, switching on\r\n", led);
+	else
 		led_switch(led, 1);
-	}
 }
 
 void led_init(void)
diff --git a/openpcd/firmware/src/main.c b/openpcd/firmware/src/main.c
index a7e3171..dd57303 100644
--- a/openpcd/firmware/src/main.c
+++ b/openpcd/firmware/src/main.c
@@ -25,6 +25,7 @@
 #include "rc632.h"
 #include "led.h"
 #include "pcd_enumerate.h"
+#include "openpcd.h"
 
 #define MSG_SIZE 				1000
 #if 0
@@ -48,19 +49,21 @@ static void AT91F_USB_Open(void)
 
 	// 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, AT91C_PIO_PA16);
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, AT91C_PIO_UDP_PUP);
 	// Clear for set the Pul up resistor
-	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, AT91C_PIO_PA16);
+	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, AT91C_PIO_UDP_PUP);
 
 	// CDC Open by structure initialization
 	AT91F_CDC_Open(AT91C_BASE_UDP);
 }
 
-static int usb_in(int len)
+static int usb_in(struct req_ctx *rctx)
 {
-	struct openpcd_hdr *poh;
-	struct openpcd_hdr *pih;
-	u_int16_t data_len;
+	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
+	struct openpcd_hdr *pih = (struct openpcd_hdr *) &rctx->tx.data[0];
+	u_int16_t len = rctx->rx.tot_len;
+
+	DEBUGP("usb_in ");
 
 	if (len < sizeof(*poh))
 		return -EINVAL;
@@ -71,48 +74,53 @@ static int usb_in(int len)
 
 	switch (poh->cmd) {
 	case OPENPCD_CMD_WRITE_REG:
-		DEBUGP("WRITE_REG ");
+		DEBUGP("WRITE_REG(0x%02x, 0x%02x) ", poh->reg, poh->val);
 		rc632_reg_write(poh->reg, poh->val);
 		break;
 	case OPENPCD_CMD_WRITE_FIFO:
-		DEBUGP("WRITE FIFO ");
-		if (len - sizeof(*poh) < data_len)
+		DEBUGP("WRITE FIFO(len=%u) ", poh->len);
+		if (len - sizeof(*poh) < poh->len)
 			return -EINVAL;
-		rc632_fifo_write(data_len, poh->data);
+		rc632_fifo_write(poh->len, poh->data);
 		break;
 	case OPENPCD_CMD_WRITE_VFIFO:
 		DEBUGP("WRITE VFIFO ");
 		break;
 	case OPENPCD_CMD_READ_REG:
-		DEBUGP("READ REG ");
+		DEBUGP("READ REG(0x%02x) ", poh->reg);
 		pih->val = rc632_reg_read(poh->reg);
 		break;
 	case OPENPCD_CMD_READ_FIFO:
-		DEBUGP("READ FIFO ");
+		DEBUGP("READ FIFO(len=%u) ", poh->len);
 		pih->len = rc632_fifo_read(poh->len, pih->data);
 		break;
 	case OPENPCD_CMD_READ_VFIFO:
 		DEBUGP("READ VFIFO ");
 		break;
 	case OPENPCD_CMD_SET_LED:
-		DEBUGP("SET LED ");
+		DEBUGP("SET LED(%u,%u) ", poh->reg, poh->val);
 		led_switch(poh->reg, poh->val);
 		break;
 	default:
 		return -EINVAL;
 	}
+	AT91F_UDP_Write(0, &rctx->tx.data[0], rctx->tx.tot_len);
 }
 
+//#define DEBUG_TOGGLE_LED
+
 int main(void)
 {
-	int i, state = 0;
+	int i;
 
+	led_init();
 	// Init trace DBGU
 	AT91F_DBGU_Init();
+#if 1
 	AT91F_DBGU_Printk
-	    ("\n\r-I- OpenPCD USB loop back\n\r 0) Set Pull-UP 1) Clear Pull UP\n\r");
+	    ("\n\r-I- OpenPCD test mode\n\r 0) Set Pull-up 1) Clear Pull-up "
+	     "2) Toggle LED1 3) Toggle LED2 4) Test RC632\n\r");
 
-	led_init();
 	rc632_init();
 	
 	//printf("test 0x%02x\n\r", 123);
@@ -124,21 +132,41 @@ int main(void)
 	// Init USB device
 	AT91F_USB_Open();
 
+#ifdef DEBUG_CLOCK_PA6
+	AT91F_PMC_EnablePCK(AT91C_BASE_PMC, 0, AT91C_PMC_CSS_PLL_CLK);
+	AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, 0, AT91C_PA6_PCK0);
+#endif
+
+#ifdef DEBUG_LOOP_LED
+	while (1) {
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_LED1);
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_LED1);
+	}
+
+#endif
+
+#endif
+	led_switch(1, 1);
+
 	// Init USB device
 	while (1) {
-		// Check enumeration
-		if (AT91F_UDP_IsConfigured()) {
+		struct req_ctx *rctx;
 
+#ifdef DEBUG_TOGGLE_LED
+		/* toggle LEDs */
+		led_toggle(1);
+		led_toggle(2);
+#endif
+
+		for (rctx = req_ctx_find_busy(); rctx; 
+		     rctx = req_ctx_find_busy()) {
+		     	DEBUGP("found used ctx %u: len=%u\r\n", 
+				req_ctx_num(rctx), rctx->rx.tot_len);
+			usb_in(rctx);
+			req_ctx_put(rctx);
 		}
-		if (state == 0) {
-			led_switch(1, 1);
-			led_switch(2, 0);
-			state = 1;
-		} else {
-			led_switch(1, 0);
-			led_switch(2, 1);
-			state = 0;
-		}
+
+		/* busy-wait for led toggle */
 		for (i = 0; i < 0x7fffff; i++) {}
 	}
 }
diff --git a/openpcd/firmware/src/openpcd.h b/openpcd/firmware/src/openpcd.h
index 40cc44a..cf67467 100644
--- a/openpcd/firmware/src/openpcd.h
+++ b/openpcd/firmware/src/openpcd.h
@@ -4,9 +4,12 @@
 #ifdef OLIMEX
 #define OPENPCD_LED1	AT91C_PIO_PA18
 #define OPENPCD_LED2	AT91C_PIO_PA17
+//#define AT91C_PIO_UDP_PUP	AT91C_PIO_PA25
+#define AT91C_PIO_UDP_PUP	AT91C_PIO_PA16
 #else
 #define OPENPCD_LED1	AT91C_PIO_PA25
 #define OPENPCD_LED2	AT91C_PIO_PA26
+#define AT91C_PIO_UDP_PUP	AT91C_PIO_PA16
 #endif
 
 #define OPENPCD_RC632_IRQ	AT91C_ID_IRQ1
@@ -27,7 +30,7 @@ struct req_buf {
 };
 
 struct req_ctx {
-	u_int16_t seq;		/* request sequence number */
+	//u_int16_t seq;		/* request sequence number */
 
 	u_int32_t flags;
 
@@ -35,4 +38,9 @@ struct req_ctx {
 	struct req_buf tx;
 };
 
+#define NUM_REQ_CTX	8
+extern struct req_ctx *req_ctx_find_get(void);
+extern void req_ctx_put(struct req_ctx *ctx);
+extern u_int8_t req_ctx_num(struct req_ctx *ctx);
+
 #endif /* _OPENPCD_H */
diff --git a/openpcd/firmware/src/pcd_enumerate.c b/openpcd/firmware/src/pcd_enumerate.c
index a8f55e3..b83ee2e 100644
--- a/openpcd/firmware/src/pcd_enumerate.c
+++ b/openpcd/firmware/src/pcd_enumerate.c
@@ -23,6 +23,7 @@
 #include <include/types.h>
 #include <include/lib_AT91SAM7S64.h>
 #include "pcd_enumerate.h"
+#include "openpcd.h"
 #include "dbgu.h"
 
 static struct _AT91S_CDC pCDC;
@@ -103,6 +104,7 @@ const struct _desc cfgDescriptor = {
 };
 
 /* USB standard request code */
+
 #define STD_GET_STATUS_ZERO           0x0080
 #define STD_GET_STATUS_INTERFACE      0x0081
 #define STD_GET_STATUS_ENDPOINT       0x0082
@@ -127,7 +129,6 @@ const struct _desc cfgDescriptor = {
 /// mt u_int32_t currentReceiveBank = AT91C_UDP_RX_DATA_BK0;
 
 static u_int32_t AT91F_UDP_Read(char *pData, u_int32_t length);
-static u_int32_t AT91F_UDP_Write(const char *pData, u_int32_t length);
 static void AT91F_CDC_Enumerate(void);
 
 static char rcv_data[64];
@@ -157,40 +158,56 @@ static void udp_irq(void)
 	}
 
 	if (isr & AT91C_UDP_EPINT0) {
-		pUDP->UDP_ICR = AT91C_UDP_EPINT0;
 		DEBUGP("EP0INT(Control) ");
 		AT91F_CDC_Enumerate();
-		DEBUGP("Enumerate finish\n");
 	}
 	if (isr & AT91C_UDP_EPINT1) {
 		u_int32_t cur_rcv_bank = pCDC.currentRcvBank;
-		pUDP->UDP_ICR = AT91C_UDP_EPINT1;
 		csr = pUDP->UDP_CSR[1];
-		DEBUGP("EP1INT(Out) ");
+		DEBUGP("EP1INT(Out, CSR=0x%08x) ", csr);
+		if (cur_rcv_bank == AT91C_UDP_RX_DATA_BK1)
+			DEBUGP("cur_bank=1 ");
+		else if (cur_rcv_bank == AT91C_UDP_RX_DATA_BK0)
+			DEBUGP("cur_bank=0 ");
+		else
+			DEBUGP("cur_bank INVALID ");
+
+		if (csr & AT91C_UDP_RX_DATA_BK1)
+			DEBUGP("BANK1 ");
+		if (csr & AT91C_UDP_RX_DATA_BK0)
+			DEBUGP("BANK0 ");
 
 		if (csr & cur_rcv_bank) {
-			u_int32_t pkt_recv = 0;
-			u_int32_t pkt_size = csr >> 16;
-			while (pkt_size--)
-				rcv_data[pkt_recv++] = pUDP->UDP_FDR[1];
-			pUDP->UDP_CSR[2] &= ~cur_rcv_bank;
-			if (cur_rcv_bank == AT91C_UDP_RX_DATA_BK0)
-				cur_rcv_bank = AT91C_UDP_RX_DATA_BK1;
-			else
-				cur_rcv_bank = AT91C_UDP_RX_DATA_BK0;
+			u_int16_t pkt_recv = 0;
+			u_int16_t pkt_size = csr >> 16;
+			struct req_ctx *rctx = req_ctx_find_get();
+
+			if (rctx) {
+				rctx->rx.tot_len = pkt_size;
+				while (pkt_size--)
+					rctx->rx.data[pkt_recv++] = pUDP->UDP_FDR[1];
+				pUDP->UDP_CSR[1] &= ~cur_rcv_bank;
+				if (cur_rcv_bank == AT91C_UDP_RX_DATA_BK0)
+					cur_rcv_bank = AT91C_UDP_RX_DATA_BK1;
+				else
+					cur_rcv_bank = AT91C_UDP_RX_DATA_BK0;
+#if 0
+				rctx->rx.data[MAX_REQSIZE+MAX_HDRSIZE-1] = '\0';
+				DEBUGP(rctx->rx.data);
+#endif
+				pCDC.currentRcvBank = cur_rcv_bank;
+			} else
+				DEBUGP("NO RCTX AVAIL! ");
 		}
-
-		rcv_data[63] = '\0';
-		DEBUGP(rcv_data);
 	}
 	if (isr & AT91C_UDP_EPINT2) {
 		csr = pUDP->UDP_CSR[2];
-		pUDP->UDP_ICR = AT91C_UDP_EPINT2;
+		//pUDP->UDP_ICR = AT91C_UDP_EPINT2;
 		DEBUGP("EP2INT(In) ");
 	}
 	if (isr & AT91C_UDP_EPINT3) {
 		csr = pUDP->UDP_CSR[3];
-		pUDP->UDP_ICR = AT91C_UDP_EPINT3;
+		//pUDP->UDP_ICR = AT91C_UDP_EPINT3;
 		DEBUGP("EP3INT(Interrupt) ");
 	}
 
@@ -235,8 +252,7 @@ AT91PS_CDC AT91F_CDC_Open(AT91PS_UDP pUdp)
 	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_UDP);
 
 	/* End-of-Bus-Reset is always enabled */
-	pCdc->pUdp->UDP_IER = (AT91C_UDP_EPINT0|AT91C_UDP_EPINT1|
-			       AT91C_UDP_EPINT2|AT91C_UDP_EPINT3);
+	//pCdc->pUdp->UDP_IER = (AT91C_UDP_EPINT0|AT91C_UDP_EPINT1| AT91C_UDP_EPINT2|AT91C_UDP_EPINT3);
 
 	return pCdc;
 }
@@ -289,38 +305,44 @@ static u_int32_t AT91F_UDP_Read(char *pData, u_int32_t length)
 //* \fn    AT91F_CDC_Write
 //* \brief Send through endpoint 2
 //*----------------------------------------------------------------------------
-static u_int32_t AT91F_UDP_Write(const char *pData, u_int32_t length)
+u_int32_t AT91F_UDP_Write(u_int8_t irq, const char *pData, u_int32_t length)
 {
 	AT91PS_UDP pUdp = pCdc->pUdp;
 	u_int32_t cpt = 0;
+	u_int8_t ep;
+
+	if (irq)
+		ep = 3;
+	else
+		ep = 2;
 
 	// Send the first packet
-	cpt = MIN(length, AT91C_EP_IN_SIZE);
+	cpt = MIN(length, 64);
 	length -= cpt;
 	while (cpt--)
-		pUdp->UDP_FDR[AT91C_EP_IN] = *pData++;
-	pUdp->UDP_CSR[AT91C_EP_IN] |= AT91C_UDP_TXPKTRDY;
+		pUdp->UDP_FDR[ep] = *pData++;
+	pUdp->UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
 
 	while (length) {
 		// Fill the second bank
-		cpt = MIN(length, AT91C_EP_IN_SIZE);
+		cpt = MIN(length, 64);
 		length -= cpt;
 		while (cpt--)
-			pUdp->UDP_FDR[AT91C_EP_IN] = *pData++;
+			pUdp->UDP_FDR[ep] = *pData++;
 		// Wait for the the first bank to be sent
-		while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP))
+		while (!(pUdp->UDP_CSR[ep] & AT91C_UDP_TXCOMP))
 			if (!AT91F_UDP_IsConfigured())
 				return length;
-		pUdp->UDP_CSR[AT91C_EP_IN] &= ~(AT91C_UDP_TXCOMP);
-		while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP) ;
-		pUdp->UDP_CSR[AT91C_EP_IN] |= AT91C_UDP_TXPKTRDY;
+		pUdp->UDP_CSR[ep] &= ~(AT91C_UDP_TXCOMP);
+		while (pUdp->UDP_CSR[ep] & AT91C_UDP_TXCOMP) ;
+		pUdp->UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
 	}
 	// Wait for the end of transfer
-	while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP))
+	while (!(pUdp->UDP_CSR[ep] & AT91C_UDP_TXCOMP))
 		if (!AT91F_UDP_IsConfigured())
 			return length;
-	pUdp->UDP_CSR[AT91C_EP_IN] &= ~(AT91C_UDP_TXCOMP);
-	while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP) ;
+	pUdp->UDP_CSR[ep] &= ~(AT91C_UDP_TXCOMP);
+	while (pUdp->UDP_CSR[ep] & AT91C_UDP_TXCOMP) ;
 
 	return length;
 }
@@ -450,9 +472,7 @@ static void AT91F_CDC_Enumerate(void)
 		if (wValue)
 			DEBUGP("VALUE!=0 ");
 		pCdc->currentConfiguration = wValue;
-		DEBUGP("SET_CONFIG_BEFORE_ZLP ");
 		AT91F_USB_SendZlp(pUDP);
-		DEBUGP("SET_CONFIG_AFTER_ZLP ");
 		pUDP->UDP_GLBSTATE =
 		    (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN;
 		pUDP->UDP_CSR[1] =
@@ -464,7 +484,6 @@ static void AT91F_CDC_Enumerate(void)
 		    (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN) : 0;
 		pUDP->UDP_IER = (AT91C_UDP_EPINT0|AT91C_UDP_EPINT1|
 				  AT91C_UDP_EPINT2|AT91C_UDP_EPINT3);
-		DEBUGP("SET_CONFIG_END ");
 		break;
 	case STD_GET_CONFIGURATION:
 		DEBUGP("GET_CONFIG ");
@@ -545,8 +564,12 @@ static void AT91F_CDC_Enumerate(void)
 		} else
 			AT91F_USB_SendStall(pUDP);
 		break;
+	case STD_SET_INTERFACE:
+		DEBUGP("SET INTERFACE ");
+		AT91F_USB_SendStall(pUDP);
+		break;
 	default:
-		DEBUGP("DEFAULT ");
+		DEBUGP("DEFAULT(req=0x%02x, type=0x%02x) ", bRequest, bmRequestType);
 		AT91F_USB_SendStall(pUDP);
 		break;
 	}
diff --git a/openpcd/firmware/src/pcd_enumerate.h b/openpcd/firmware/src/pcd_enumerate.h
index 872ae34..af0d00c 100644
--- a/openpcd/firmware/src/pcd_enumerate.h
+++ b/openpcd/firmware/src/pcd_enumerate.h
@@ -37,6 +37,7 @@ typedef struct _AT91S_CDC
 
 AT91PS_CDC AT91F_CDC_Open(AT91PS_UDP pUdp);
 u_int8_t AT91F_UDP_IsConfigured(void);
+u_int32_t AT91F_UDP_Write(u_int8_t irq, const char *pData, u_int32_t length);
 
 #endif // CDC_ENUMERATE_H
 
diff --git a/openpcd/firmware/src/rc632_debug.c b/openpcd/firmware/src/rc632_debug.c
index fae9b2b..f3e36b7 100644
--- a/openpcd/firmware/src/rc632_debug.c
+++ b/openpcd/firmware/src/rc632_debug.c
@@ -26,5 +26,7 @@ int rc632_test(void)
 
 	return 0;
 }
-
+#else
+int rc632_test(void)
+{}
 #endif
diff --git a/openpcd/firmware/src/req_ctx.c b/openpcd/firmware/src/req_ctx.c
new file mode 100644
index 0000000..41298e6
--- /dev/null
+++ b/openpcd/firmware/src/req_ctx.c
@@ -0,0 +1,48 @@
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <include/types.h>
+#include "openpcd.h"
+#include "dbgu.h"
+
+/* FIXME: locking, FIFO order processing */
+
+static struct req_ctx req_ctx[8];
+static u_int8_t req_ctx_busy;		/* bitmask of used request contexts */
+
+struct req_ctx *req_ctx_find_get(void)
+{
+	u_int8_t i;
+	for (i = 0; i < NUM_REQ_CTX; i++) {
+		if (!(req_ctx_busy & (1 << i))) {
+			req_ctx_busy |= (1 << i);
+			return &req_ctx[i];
+		}
+	}
+
+	return NULL;
+}
+
+struct req_ctx *req_ctx_find_busy(void)
+{
+	u_int8_t i;
+	for (i = 0; i < NUM_REQ_CTX; i++) {
+		if (req_ctx_busy & (1 << i))
+			return &req_ctx[i];
+	}
+}
+
+
+u_int8_t req_ctx_num(struct req_ctx *ctx)
+{
+	return ((void *)ctx - (void *)&req_ctx[0])/sizeof(*ctx);
+}
+
+void req_ctx_put(struct req_ctx *ctx)
+{
+	int offset = req_ctx_num(ctx);
+	if (offset > NUM_REQ_CTX)
+		DEBUGP("Error in offset calculation req_ctx_put\n");
+
+	req_ctx_busy &= ~(1 << offset);
+}