fix usb device enumeration (IRQ based)

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

8 files changed, 757 insertions, 483 deletions

diff --git a/openpcd/firmware/src/Makefile b/openpcd/firmware/Makefile
index 1368a77..1882fe9 100644
--- a/openpcd/firmware/src/Makefile
+++ b/openpcd/firmware/Makefile
@@ -1,6 +1,8 @@
 # Hey Emacs, this is a -*- makefile -*-
 #
-# WinARM template makefile 
+# WinARM makefile for the FreeRTOS-demo on LPC2138
+# based in information from the FreeRTOS LPC2106 example
+#
 # by Martin Thomas, Kaiserslautern, Germany 
 # <eversmith@heizung-thomas.de>
 #
@@ -15,7 +17,7 @@
 #
 # make clean = Clean out built project files.
 #
-# make program = Download the hex file to the device, using lpc21isp
+# make program = Download the hex file to the device
 #
 # (TODO: make filename.s = Just compile filename.c into the assembler code only)
 #
@@ -25,55 +27,50 @@
 # - 17. Feb. 2005  - added thumb-interwork support (mth)
 # - 28. Apr. 2005  - added C++ support (mth)
 # - 29. Arp. 2005  - changed handling for lst-Filename (mth)
-#
-
+# -  1. Nov. 2005  - exception-vector placement options (mth)
+# - 15. Nov. 2005  - added library-search-path (EXTRA_LIB...) (mth)
+# -  2. Dec. 2005  - fixed ihex and binary file extensions (mth)
+# - 22. Feb. 2006  - added AT91LIBNOWARN setting (mth)
+# - 19. Apr. 2006  - option FLASH_TOOL (default lpc21isp); variable IMGEXT (mth)
+# - 19. Mai. 2006  - USE_THUMB_MODE switch, ROM_RUN->RUN_FROM_ROM RAM_RUN->RUN_FROM_RAM
 
-# project specific flashtool - uses Keil's uVision and ULINK
-# (www.keil.com), needs a configured uVision Workspace
-# for "batch programming"
-FLASH_TOOL = ULINK
 
+#FLASH_TOOL = UVISION
+#FLASH_TOOL = OPENOCD
 
 # MCU name and submodel
-MCU = arm7tdmi
-SUBMDL = AT91SAM7S64
-THUMB    = -mthumb
-THUMB_IW = -mthumb-interwork
+MCU      = arm7tdmi
+SUBMDL   = AT91SAM7S64
 
+USE_THUMB_MODE = NO
+#USE_THUMB_MODE = YES
 
 ## Create ROM-Image (final)
-RUN_MODE=ROM_RUN
-## Create RAM-Image (debugging)
-#RUN_MODE=RAM_RUN
+RUN_MODE=RUN_FROM_ROM
+## Create RAM-Image (debugging) - not used in this example
+#RUN_MODE=RUN_FROM_RAM
 
 # with / at end
-PATH_TO_LINKSCRIPTS=../compil/SrcWinARM/
-
-## TODO for this example - just a placeholder
-## interrupt vectors in ROM
-VECTOR_LOCATION=VECTORS_ROM
-## interrupt vectors in RAM
-#VECTOR_LOCATION=VECTORS_RAM
-
-
-# Output format. (can be srec, ihex, binary)
-FORMAT = ihex
+PATH_TO_LINKSCRIPTS=compil/SrcWinARM/
 
+#### not used in this example:
+## Exception-Vector placement only supported for "ROM_RUN"
+## (placement settings ignored when using "RAM_RUN")
+## - Exception vectors in ROM:
+#VECTOR_LOCATION=VECTORS_IN_ROM
+## - Exception vectors in RAM:
+#VECTOR_LOCATION=VECTORS_IN_RAM
 
 # Target file name (without extension).
 TARGET = main
 
-
 # List C source files here. (C dependencies are automatically generated.)
 # use file-extension c for "c-only"-files
-#SRC = $(TARGET).c pcd_enumerate.c dbgu.c 
-# only needed for the "dll-Target":
-#SRC += syscalls.c
-
+SRC = 
 
 # List C source files here which must be compiled in ARM-Mode.
 # use file-extension c for "c-only"-files
-SRCARM = $(TARGET).c pcd_enumerate.c fifo.c dbgu.c rc632.c ../compil/SrcWinARM/Cstartup_SAM7.c
+SRCARM  = lib/lib_AT91SAM7S64.c src/pcd_enumerate.c src/fifo.c src/dbgu.c src/rc632.c src/$(TARGET).c compil/SrcWinARM/Cstartup_SAM7.c 
 
 # List C++ source files here.
 # use file-extension cpp for C++-files (use extension .cpp)
@@ -94,7 +91,12 @@ CPPSRCARM =
 ASRC = 
 
 # List Assembler source files here which must be assembled in ARM-Mode..
-ASRCARM = ../compil/SrcWinARM/Cstartup.S 
+ASRCARM  = compil/SrcWinARM/Cstartup.S
+
+## Output format. (can be ihex or binary)
+## (binary i.e. for openocd and SAM-BA, hex i.e. for lpc21isp and uVision)
+#FORMAT = ihex
+FORMAT = binary
 
 # Optimization level, can be [0, 1, 2, 3, s]. 
 # 0 = turn off optimization. s = optimize for size.
@@ -110,8 +112,19 @@ DEBUG = dwarf-2
 
 # List any extra directories to look for include files here.
 #     Each directory must be seperated by a space.
-#EXTRAINCDIRS = ./include
-EXTRAINCDIRS = ../compil/SrcWinARM ../
+#### FreeRTOS
+EXTRAINCDIRS =  compil/SrcWinARM
+
+# List any extra directories to look for library files here.
+#     Each directory must be seperated by a space.
+#EXTRA_LIBDIRS = ../arm7_efsl_0_2_4
+EXTRA_LIBDIRS = 
+
+## Using the Atmel AT91_lib produces warning with
+## the default warning-levels. 
+## yes - disable these warnings; no - keep default settings
+AT91LIBNOWARN = yes
+#AT91LIBNOWARN = no
 
 # Compiler flag to set the C Standard level.
 # c89   - "ANSI" C
@@ -121,13 +134,21 @@ EXTRAINCDIRS = ../compil/SrcWinARM ../
 CSTANDARD = -std=gnu99
 
 # Place -D or -U options for C here
-CDEFS =  -D$(RUN_MODE) -D$(VECTOR_LOCATION) -D__WinARM__
+CDEFS =  -D$(RUN_MODE) 
 
 # Place -I options here
-CINCS =
+CINCS = -Ihelper -Isrc
 
 # Place -D or -U options for ASM here
-ADEFS =  -D$(RUN_MODE) -D$(VECTOR_LOCATION) -D__WinARM__
+ADEFS =  -D$(RUN_MODE)
+
+ifdef VECTOR_LOCATION
+CDEFS += -D$(VECTOR_LOCATION)
+ADEFS += -D$(VECTOR_LOCATION)
+endif
+
+CDEFS += -D__WinARM__
+ADEFS += -D__WinARM__
 
 
 # Compiler flags.
@@ -142,41 +163,45 @@ ADEFS =  -D$(RUN_MODE) -D$(VECTOR_LOCATION) -D__WinARM__
 CFLAGS = -g$(DEBUG)
 CFLAGS += $(CDEFS) $(CINCS)
 CFLAGS += -O$(OPT)
-CFLAGS += -Wall -Wcast-align -Wimplicit 
+CFLAGS += -Wall -Wextra -Wcast-align -Wimplicit -Wunused
 CFLAGS += -Wpointer-arith -Wswitch
-CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
+CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow 
+CFLAGS += -Wbad-function-cast -Wsign-compare -Waggregate-return 
 CFLAGS += -Wa,-adhlns=$(subst $(suffix $<),.lst,$<) 
 CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
-#AT91-lib warnings with:
-##CFLAGS += -Wcast-qual
-
 
 # flags only for C
 CONLYFLAGS += -Wnested-externs 
 CONLYFLAGS += $(CSTANDARD)
-#AT91-lib warnings with:
-##CONLYFLAGS += -Wmissing-prototypes 
-##CONLYFLAGS  = -Wstrict-prototypes
-##CONLYFLAGS += -Wmissing-declarations
-
 
+ifneq ($(AT91LIBNOWARN),yes)
+#AT91-lib warnings with:
+CFLAGS += -Wcast-qual
+CONLYFLAGS += -Wmissing-prototypes 
+CONLYFLAGS += -Wstrict-prototypes
+CONLYFLAGS += -Wmissing-declarations
+endif
 
 # flags only for C++ (arm-elf-g++)
 # CPPFLAGS = -fno-rtti -fno-exceptions
 CPPFLAGS = 
 
 # Assembler flags.
-#  -Wa,...:   tell GCC to pass this to the assembler.
-#  -ahlms:    create listing
-#  -gstabs:   have the assembler create line number information; note that
-#             for use in COFF files, additional information about filenames
-#             and function names needs to be present in the assembler source
-#             files -- see avr-libc docs [FIXME: not yet described there]
-##ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs 
-ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:.S=.lst),-g$(DEBUG)
+#  -Wa,...:    tell GCC to pass this to the assembler.
+#  -ahlns:     create listing
+#  -g$(DEBUG): have the assembler create line number information
+ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:.S=.lst),--g$(DEBUG)
+
 
 #Additional libraries.
 
+# Extra libraries
+#    Each library-name must be seperated by a space.
+#    To add libxyz.a, libabc.a and libefsl.a: 
+#    EXTRA_LIBS = xyz abc efsl
+#EXTRA_LIBS = efsl
+EXTRA_LIBS =
+
 #Support for newlibc-lpc (file: libnewlibc-lpc.a)
 #NEWLIBLPC = -lnewlib-lpc
 
@@ -184,6 +209,7 @@ MATH_LIB = -lm
 
 # CPLUSPLUS_LIB = -lstdc++
 
+
 # Linker flags.
 #  -Wl,...:     tell GCC to pass this to linker.
 #    -Map:      create map file
@@ -193,35 +219,36 @@ LDFLAGS += -lc
 LDFLAGS += $(NEWLIBLPC) $(MATH_LIB)
 LDFLAGS += -lc -lgcc 
 LDFLAGS += $(CPLUSPLUS_LIB)
+LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
+LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
 
-# Set Linker-Script Depending On Selected Memory
-ifeq ($(RUN_MODE),RAM_RUN)
+# Set Linker-Script Depending On Selected Memory and Controller
+ifeq ($(RUN_MODE),RUN_FROM_RAM)
 LDFLAGS +=-T$(PATH_TO_LINKSCRIPTS)$(SUBMDL)-RAM.ld
 else 
 LDFLAGS +=-T$(PATH_TO_LINKSCRIPTS)$(SUBMDL)-ROM.ld
 endif
 
 
-
 # ---------------------------------------------------------------------------
 # Flash-Programming support using lpc21isp by Martin Maurer 
-
+# only for Philips LPC and Analog ADuC ARMs
+#
 # Settings and variables:
 #LPC21ISP = lpc21isp
-LPC21ISP = lpc21isp_beta
+LPC21ISP = lpc21isp
 LPC21ISP_PORT = com1
-LPC21ISP_BAUD = 115200
-LPC21ISP_XTAL = 14746
+LPC21ISP_BAUD = 38400
+LPC21ISP_XTAL = 12000
 LPC21ISP_FLASHFILE = $(TARGET).hex
 # verbose output:
-## LPC21ISP_DEBUG = -debug
+#LPC21ISP_DEBUG = -debug
 # enter bootloader via RS232 DTR/RTS (only if hardware supports this
 # feature - see Philips AppNote):
 LPC21ISP_CONTROL = -control
-
-
 # ---------------------------------------------------------------------------
 
+
 # Define directories, if needed.
 ## DIRARM = c:/WinARM/
 ## DIRARMBIN = $(DIRAVR)/bin/
@@ -238,11 +265,10 @@ NM = arm-elf-nm
 REMOVE = rm -f
 COPY = cp
 
-
 # Define Messages
 # English
 MSG_ERRORS_NONE = Errors: none
-MSG_BEGIN = -------- begin --------
+MSG_BEGIN = "-------- begin (mode: $(RUN_MODE)) --------"
 MSG_END = --------  end  --------
 MSG_SIZE_BEFORE = Size before: 
 MSG_SIZE_AFTER = Size after:
@@ -257,9 +283,9 @@ MSG_COMPILINGCPP_ARM = "Compiling C++ (ARM-only):"
 MSG_ASSEMBLING = Assembling:
 MSG_ASSEMBLING_ARM = "Assembling (ARM-only):"
 MSG_CLEANING = Cleaning project:
+MSG_FORMATERROR = Can not handle output-format
 MSG_LPC21_RESETREMINDER = You may have to bring the target in bootloader-mode now.
 
-
 # Define all object files.
 COBJ      = $(SRC:.c=.o) 
 AOBJ      = $(ASRC:.S=.o)
@@ -278,17 +304,28 @@ GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d
 
 # Combine all necessary flags and optional flags.
 # Add target processor to flags.
-ALL_CFLAGS = -mcpu=$(MCU) $(THUMB_IW) -I. $(CFLAGS) $(GENDEPFLAGS)
-ALL_ASFLAGS = -mcpu=$(MCU) $(THUMB_IW) -I. -x assembler-with-cpp $(ASFLAGS)
+ALL_CFLAGS  = -mcpu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS)
+ALL_ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)
 
 
 # Default target.
 all: begin gccversion sizebefore build sizeafter finished end
 
+ifeq ($(FORMAT),ihex)
 build: elf hex lss sym
+hex: $(TARGET).hex
+IMGEXT=hex
+else 
+ifeq ($(FORMAT),binary)
+build: elf bin lss sym
+bin: $(TARGET).bin
+IMGEXT=bin
+else 
+$(error "$(MSG_FORMATERROR) $(FORMAT)")
+endif
+endif
 
 elf: $(TARGET).elf
-hex: $(TARGET).hex
 lss: $(TARGET).lss 
 sym: $(TARGET).sym
 
@@ -321,28 +358,40 @@ gccversion :
 
 
 # Program the device.
-ifeq ($(FLASH_TOOL),ULINK)
-# Program the device with Keil's ULINK (needs configured uVision-Workspace). 
-program: $(TARGET).hex
+ifeq ($(FLASH_TOOL),UVISION)
+# Program the device with Keil's uVision (needs configured uVision-Workspace). 
+program: $(TARGET).$(IMGEXT)
+	@echo
+	@echo "Programming with uVision"
+	C:\Keil\uv3\Uv3.exe -f uvisionflash.Uv2 -ouvisionflash.txt
+else
+ifeq ($(FLASH_TOOL),OPENOCD)
+# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
+program: $(TARGET).$(IMGEXT)
 	@echo
-	@echo "Programming with ULINK"
-	C:\Keil\uv3\Uv3.exe -f ulinkflash.Uv2 -oulinkflash.txt
+	@echo "Programming with OPENOCD"
+	C:\WinARM\utils\openocd\openocd_svn59\openocd.exe -f oocd_sam7_flash.cfg
 else
 # Program the device.  - lpc21isp will not work for SAM7
-program: $(TARGET).hex
+program: $(TARGET).$(IMGEXT)
 	@echo
 	@echo $(MSG_LPC21_RESETREMINDER)
-	$(LPC21ISP) $(LPC21ISP_CONTROL) $(LPC21ISP_DEBUG) $(LPC21ISP_FLASHFILE) $(LPC21ISP_PORT) $(LPC21ISP_BAUD) $(LPC21ISP_XTAL)
+	$(LPC21ISP) $(LPC21ISP_OPTIONS) $(LPC21ISP_DEBUG) $(LPC21ISP_FLASHFILE) $(LPC21ISP_PORT) $(LPC21ISP_BAUD) $(LPC21ISP_XTAL)
+endif
 endif
 
 
-
-# Create final output files (.hex, .eep) from ELF output file.
-# TODO: handling the .eeprom-section should be redundant
+# Create final output file (.hex) from ELF output file.
 %.hex: %.elf
 	@echo
 	@echo $(MSG_FLASH) $@
 	$(OBJCOPY) -O $(FORMAT) $< $@
+	
+# Create final output file (.bin) from ELF output file.
+%.bin: %.elf
+	@echo
+	@echo $(MSG_FLASH) $@
+	$(OBJCOPY) -O $(FORMAT) $< $@
 
 
 # Create extended listing file from ELF output file.
@@ -366,14 +415,13 @@ endif
 %.elf:  $(AOBJARM) $(AOBJ) $(COBJARM) $(COBJ) $(CPPOBJ) $(CPPOBJARM)
 	@echo
 	@echo $(MSG_LINKING) $@
-	$(CC) $(THUMB) $(ALL_CFLAGS) $(AOBJARM) $(AOBJ) $(COBJARM) $(COBJ) $(CPPOBJ) $(CPPOBJARM) --output $@ $(LDFLAGS)
-#	$(CPP) $(THUMB) $(ALL_CFLAGS) $(AOBJARM) $(AOBJ) $(COBJARM) $(COBJ) $(CPPOBJ) $(CPPOBJARM) --output $@ $(LDFLAGS)
+	$(CC) $(ALL_CFLAGS) $(AOBJARM) $(AOBJ) $(COBJARM) $(COBJ) $(CPPOBJ) $(CPPOBJARM) --output $@ $(LDFLAGS)
 
 # Compile: create object files from C source files. ARM/Thumb
 $(COBJ) : %.o : %.c
 	@echo
 	@echo $(MSG_COMPILING) $<
-	$(CC) -c $(THUMB) $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@ 
+	$(CC) -c $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@ 
 
 # Compile: create object files from C source files. ARM-only
 $(COBJARM) : %.o : %.c
@@ -385,7 +433,7 @@ $(COBJARM) : %.o : %.c
 $(CPPOBJ) : %.o : %.cpp
 	@echo
 	@echo $(MSG_COMPILINGCPP) $<
-	$(CPP) -c $(THUMB) $(ALL_CFLAGS) $(CPPFLAGS) $< -o $@ 
+	$(CPP) -c $(ALL_CFLAGS) $(CPPFLAGS) $< -o $@ 
 
 # Compile: create object files from C++ source files. ARM-only
 $(CPPOBJARM) : %.o : %.cpp
@@ -404,7 +452,7 @@ $(CPPOBJARM) : %.o : %.cpp
 $(AOBJ) : %.o : %.S
 	@echo
 	@echo $(MSG_ASSEMBLING) $<
-	$(CC) -c $(THUMB) $(ALL_ASFLAGS) $< -o $@
+	$(CC) -c $(ALL_ASFLAGS) $< -o $@
 
 
 # Assemble: create object files from assembler source files. ARM-only
@@ -421,7 +469,9 @@ clean: begin clean_list finished end
 clean_list :
 	@echo
 	@echo $(MSG_CLEANING)
+	$(REMOVE) ../Common_WinARM/$(TARGET).hex
 	$(REMOVE) $(TARGET).hex
+	$(REMOVE) $(TARGET).bin
 	$(REMOVE) $(TARGET).obj
 	$(REMOVE) $(TARGET).elf
 	$(REMOVE) $(TARGET).map
@@ -454,5 +504,5 @@ clean_list :
 
 # Listing of phony targets.
 .PHONY : all begin finish end sizebefore sizeafter gccversion \
-build elf hex lss sym clean clean_list program
+build elf hex bin lss sym clean clean_list program
 
diff --git a/openpcd/firmware/compil/SrcWinARM/Cstartup_SAM7.c b/openpcd/firmware/compil/SrcWinARM/Cstartup_SAM7.c
index 16fda01..94a2bbe 100644
--- a/openpcd/firmware/compil/SrcWinARM/Cstartup_SAM7.c
+++ b/openpcd/firmware/compil/SrcWinARM/Cstartup_SAM7.c
@@ -8,12 +8,10 @@
 //* intellectual property rights of others.
 //*----------------------------------------------------------------------------
 //* File Name           : Cstartup_SAM7.c
-//* Object              : Low level initializations written in C for Tools
+//* Object              : Low level initializations written in C for GCC Tools
 //* Creation            : 12/Jun/04
 //* 1.2   28/Feb/05 JPP : LIB change AT91C_WDTC_WDDIS & PLL
 //* 1.3   21/Mar/05 JPP : Change PLL Wait time
-//* 1.4   21/Aug/05 JPP : Change MC_FMR Setting
-//* 1.5   29/Aug/05 JPP : Change PLL error
 //*----------------------------------------------------------------------------
 
 // Include the board file description
@@ -21,9 +19,9 @@
 
 // The following functions must be write in ARM mode this function called directly
 // by exception vector
-extern void AT91F_Spurious_handler(void);
-extern void AT91F_Default_IRQ_handler(void);
-extern void AT91F_Default_FIQ_handler(void);
+extern void AT91F_Spurious_handler (void);
+extern void AT91F_Default_IRQ_handler (void);
+extern void AT91F_Default_FIQ_handler (void);
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_LowLevelInit
@@ -31,54 +29,55 @@ extern void AT91F_Default_FIQ_handler(void);
 //*        this function can be use a Stack, depending the compilation
 //*        optimization mode
 //*----------------------------------------------------------------------------
-void AT91F_LowLevelInit(void)
+void
+AT91F_LowLevelInit (void)
 {
-	int i;
-	AT91PS_PMC pPMC = AT91C_BASE_PMC;
-	//* Set Flash Waite sate
-	//  Single Cycle Access at Up to 30 MHz, or 40
-	AT91C_BASE_MC->MC_FMR = AT91C_MC_FWS_1FWS;
+  int i;
+  AT91PS_PMC pPMC = AT91C_BASE_PMC;
+  //* Set Flash Waite sate
+  //  Single Cycle Access at Up to 30 MHz, or 40
+  //  if MCK = 47923200 I have 50 Cycle for 1 usecond ( flied MC_FMR->FMCN
+  AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN) & (48 << 16)) | AT91C_MC_FWS_1FWS;
 
-	//* Watchdog Disable
-	AT91C_BASE_WDTC->WDTC_WDMR = AT91C_WDTC_WDDIS;
+  //* Watchdog Disable
+  AT91C_BASE_WDTC->WDTC_WDMR = AT91C_WDTC_WDDIS;
 
-	//* Set MCK at 47 923 200
-	// 1 Enabling the Main Oscillator:
-	// SCK = 1/32768 = 30.51 uSecond
-	// Start up time = 8 * 6 / SCK = 56 * 30.51 = 1,46484375 ms
-	//// mt pPMC->PMC_MOR = (( AT91C_CKGR_OSCOUNT & (0x06 <<8) | AT91C_CKGR_MOSCEN ));
-	pPMC->PMC_MOR =
-	    ((AT91C_CKGR_OSCOUNT & (0x06 << 8)) | AT91C_CKGR_MOSCEN);
-	// Wait the startup time
-	while (!(pPMC->PMC_SR & AT91C_PMC_MOSCS)) ;
-	// 2 Checking the Main Oscillator Frequency (Optional)
-	// 3 Setting PLL and divider:
-	// - div by 14 Fin = 1.3165 =(18,432 / 14)
-	// - Mul 72+1: Fout =   96.1097 =(3,6864 *73)
-	// for 96 MHz the erroe is 0.11%
-	// Field out NOT USED = 0
-	// PLLCOUNT pll startup time estimate at : 0.844 ms
-	// PLLCOUNT 28 = 0.000844 /(1/32768)
-	pPMC->PMC_PLLR = ((AT91C_CKGR_DIV & 14) |
-			  (AT91C_CKGR_PLLCOUNT & (28 << 8)) |
-			  (AT91C_CKGR_MUL & (72 << 16)));
+  //* Set MCK at 47 923 200
+  // 1 Enabling the Main Oscillator:
+  // SCK = 1/32768 = 30.51 uSecond
+  // Start up time = 8 * 6 / SCK = 56 * 30.51 = 1,46484375 ms
+  pPMC->PMC_MOR = ((AT91C_CKGR_OSCOUNT) & (0x06 << 8)) | AT91C_CKGR_MOSCEN;
+  // Wait the startup time
+  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)
+  // 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)
+  pPMC->PMC_PLLR = ((AT91C_CKGR_DIV & 0x05) |
+		    (AT91C_CKGR_PLLCOUNT & (28 << 8)) |
+		    (AT91C_CKGR_MUL & (25 << 16)));
 
-	// Wait the startup time
-	while (!(pPMC->PMC_SR & AT91C_PMC_LOCK)) ;
-	while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY)) ;
-	// 4. Selection of Master Clock and Processor Clock
-	// select the PLL clock divided by 2
-	pPMC->PMC_MCKR = AT91C_PMC_PRES_CLK_2;
-	while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY)) ;
+  // Wait the startup time
+  while (!(pPMC->PMC_SR & AT91C_PMC_LOCK));
+  while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
+  // 4. Selection of Master Clock and Processor Clock
+  // select the PLL clock divided by 2
+  pPMC->PMC_MCKR = AT91C_PMC_PRES_CLK_2;
+  while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
 
-	pPMC->PMC_MCKR |= AT91C_PMC_CSS_PLL_CLK;
-	while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY)) ;
+  pPMC->PMC_MCKR |= AT91C_PMC_CSS_PLL_CLK;
+  while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
 
-	// Set up the default interrupts handler vectors
-	AT91C_BASE_AIC->AIC_SVR[0] = (int)AT91F_Default_FIQ_handler;
-	for (i = 1; i < 31; i++) {
-		AT91C_BASE_AIC->AIC_SVR[i] = (int)AT91F_Default_IRQ_handler;
-	}
-	AT91C_BASE_AIC->AIC_SPU = (int)AT91F_Spurious_handler;
+  // Set up the default interrupts handler vectors
+  AT91C_BASE_AIC->AIC_SVR[0] = (int) AT91F_Default_FIQ_handler;
+  for (i = 1; i < 31; i++)
+    {
+      AT91C_BASE_AIC->AIC_SVR[i] = (int) AT91F_Default_IRQ_handler;
+    }
+  AT91C_BASE_AIC->AIC_SPU = (int) AT91F_Spurious_handler;
 
 }
diff --git a/openpcd/firmware/include/lib_AT91SAM7S64.h b/openpcd/firmware/include/lib_AT91SAM7S64.h
index a87919b..0f7c7df 100644
--- a/openpcd/firmware/include/lib_AT91SAM7S64.h
+++ b/openpcd/firmware/include/lib_AT91SAM7S64.h
@@ -51,30 +51,12 @@
 //* \fn    AT91F_AIC_ConfigureIt
 //* \brief Interrupt Handler Initialization
 //*----------------------------------------------------------------------------
-static inline unsigned int AT91F_AIC_ConfigureIt (
+extern unsigned int AT91F_AIC_ConfigureIt (
 	AT91PS_AIC pAic,  // \arg pointer to the AIC registers
 	unsigned int irq_id,     // \arg interrupt number to initialize
 	unsigned int priority,   // \arg priority to give to the interrupt
 	unsigned int src_type,   // \arg activation and sense of activation
-	void (*newHandler) () ) // \arg address of the interrupt handler
-{
-	unsigned int oldHandler;
-    unsigned int mask ;
-
-    oldHandler = pAic->AIC_SVR[irq_id];
-
-    mask = 0x1 << irq_id ;
-    //* Disable the interrupt on the interrupt controller
-    pAic->AIC_IDCR = mask ;
-    //* Save the interrupt handler routine pointer and the interrupt priority
-    pAic->AIC_SVR[irq_id] = (unsigned int) newHandler ;
-    //* Store the Source Mode Register
-    pAic->AIC_SMR[irq_id] = src_type | priority  ;
-    //* Clear the interrupt on the interrupt controller
-    pAic->AIC_ICCR = mask ;
-
-	return oldHandler;
-}
+	void (*newHandler) () ); // \arg address of the interrupt handler
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_AIC_EnableIt
@@ -129,19 +111,9 @@ static inline void AT91F_AIC_AcknowledgeIt (
 //* \fn    AT91F_AIC_SetExceptionVector
 //* \brief Configure vector handler
 //*----------------------------------------------------------------------------
-static inline unsigned int  AT91F_AIC_SetExceptionVector (
+extern unsigned int  AT91F_AIC_SetExceptionVector (
 	unsigned int *pVector, // \arg pointer to the AIC registers
-	void (*Handler) () )   // \arg Interrupt Handler
-{
-	unsigned int oldVector = *pVector;
-
-	if ((unsigned int) Handler == (unsigned int) AT91C_AIC_BRANCH_OPCODE)
-		*pVector = (unsigned int) AT91C_AIC_BRANCH_OPCODE;
-	else
-		*pVector = (((((unsigned int) Handler) - ((unsigned int) pVector) - 0x8) >> 2) & 0x00FFFFFF) | 0xEA000000;
-
-	return oldVector;
-}
+	void (*Handler) () );   // \arg Interrupt Handler
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_AIC_Trig
@@ -180,30 +152,14 @@ static inline unsigned int  AT91F_AIC_IsPending (
 //* \fn    AT91F_AIC_Open
 //* \brief Set exception vectors and AIC registers to default values
 //*----------------------------------------------------------------------------
-static inline void AT91F_AIC_Open(
+extern void AT91F_AIC_Open(
 	AT91PS_AIC pAic,        // \arg pointer to the AIC registers
 	void (*IrqHandler) (),  // \arg Default IRQ vector exception
 	void (*FiqHandler) (),  // \arg Default FIQ vector exception
 	void (*DefaultHandler)  (), // \arg Default Handler set in ISR
 	void (*SpuriousHandler) (), // \arg Default Spurious Handler
-	unsigned int protectMode)   // \arg Debug Control Register
-{
-	int i;
-
-	// Disable all interrupts and set IVR to the default handler
-	for (i = 0; i < 32; ++i) {
-		AT91F_AIC_DisableIt(pAic, i);
-		AT91F_AIC_ConfigureIt(pAic, i, AT91C_AIC_PRIOR_LOWEST, AT91C_AIC_SRCTYPE_HIGH_LEVEL, DefaultHandler);
-	}
-
-	// Set the IRQ exception vector
-	AT91F_AIC_SetExceptionVector((unsigned int *) 0x18, IrqHandler);
-	// Set the Fast Interrupt exception vector
-	AT91F_AIC_SetExceptionVector((unsigned int *) 0x1C, FiqHandler);
+	unsigned int protectMode);   // \arg Debug Control Register
 
-	pAic->AIC_SPU = (unsigned int) SpuriousHandler;
-	pAic->AIC_DCR = protectMode;
-}
 /* *****************************************************************************
                 SOFTWARE API FOR PDC
    ***************************************************************************** */
@@ -343,98 +299,36 @@ static inline int AT91F_PDC_IsNextRxEmpty ( // \return return 1 if transfer is c
 //* \fn    AT91F_PDC_Open
 //* \brief Open PDC: disable TX and RX reset transfer descriptors, re-enable RX and TX
 //*----------------------------------------------------------------------------
-static inline void AT91F_PDC_Open (
-	AT91PS_PDC pPDC)       // \arg pointer to a PDC controller
-{
-    //* Disable the RX and TX PDC transfer requests
-	AT91F_PDC_DisableRx(pPDC);
-	AT91F_PDC_DisableTx(pPDC);
-
-	//* Reset all Counter register Next buffer first
-	AT91F_PDC_SetNextTx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetNextRx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetTx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetRx(pPDC, (char *) 0, 0);
-
-    //* Enable the RX and TX PDC transfer requests
-	AT91F_PDC_EnableRx(pPDC);
-	AT91F_PDC_EnableTx(pPDC);
-}
+extern void AT91F_PDC_Open(AT91PS_PDC pPDC);      // \arg pointer to a PDC controller
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_PDC_Close
 //* \brief Close PDC: disable TX and RX reset transfer descriptors
 //*----------------------------------------------------------------------------
-static inline void AT91F_PDC_Close (
-	AT91PS_PDC pPDC)       // \arg pointer to a PDC controller
-{
-    //* Disable the RX and TX PDC transfer requests
-	AT91F_PDC_DisableRx(pPDC);
-	AT91F_PDC_DisableTx(pPDC);
-
-	//* Reset all Counter register Next buffer first
-	AT91F_PDC_SetNextTx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetNextRx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetTx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetRx(pPDC, (char *) 0, 0);
-
-}
+extern void AT91F_PDC_Close(AT91PS_PDC pPDC);       // \arg pointer to a PDC controller
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_PDC_SendFrame
 //* \brief Close PDC: disable TX and RX reset transfer descriptors
 //*----------------------------------------------------------------------------
-static inline unsigned int AT91F_PDC_SendFrame(
+extern unsigned int AT91F_PDC_SendFrame(
 	AT91PS_PDC pPDC,
 	char *pBuffer,
 	unsigned int szBuffer,
 	char *pNextBuffer,
-	unsigned int szNextBuffer )
-{
-	if (AT91F_PDC_IsTxEmpty(pPDC)) {
-		//* Buffer and next buffer can be initialized
-		AT91F_PDC_SetTx(pPDC, pBuffer, szBuffer);
-		AT91F_PDC_SetNextTx(pPDC, pNextBuffer, szNextBuffer);
-		return 2;
-	}
-	else if (AT91F_PDC_IsNextTxEmpty(pPDC)) {
-		//* Only one buffer can be initialized
-		AT91F_PDC_SetNextTx(pPDC, pBuffer, szBuffer);
-		return 1;
-	}
-	else {
-		//* All buffer are in use...
-		return 0;
-	}
-}
+	unsigned int szNextBuffer);
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_PDC_ReceiveFrame
 //* \brief Close PDC: disable TX and RX reset transfer descriptors
 //*----------------------------------------------------------------------------
-static inline unsigned int AT91F_PDC_ReceiveFrame (
+extern unsigned int AT91F_PDC_ReceiveFrame (
 	AT91PS_PDC pPDC,
 	char *pBuffer,
 	unsigned int szBuffer,
 	char *pNextBuffer,
-	unsigned int szNextBuffer )
-{
-	if (AT91F_PDC_IsRxEmpty(pPDC)) {
-		//* Buffer and next buffer can be initialized
-		AT91F_PDC_SetRx(pPDC, pBuffer, szBuffer);
-		AT91F_PDC_SetNextRx(pPDC, pNextBuffer, szNextBuffer);
-		return 2;
-	}
-	else if (AT91F_PDC_IsNextRxEmpty(pPDC)) {
-		//* Only one buffer can be initialized
-		AT91F_PDC_SetNextRx(pPDC, pBuffer, szBuffer);
-		return 1;
-	}
-	else {
-		//* All buffer are in use...
-		return 0;
-	}
-}
+	unsigned int szNextBuffer);
+
 /* *****************************************************************************
                 SOFTWARE API FOR DBGU
    ***************************************************************************** */
@@ -1167,28 +1061,10 @@ static inline unsigned int AT91F_PMC_GetMCKReg(
 //* \fn    AT91F_PMC_GetMasterClock
 //* \brief Return master clock in Hz which correponds to processor clock for ARM7
 //*------------------------------------------------------------------------------
-static inline unsigned int AT91F_PMC_GetMasterClock (
+extern unsigned int AT91F_PMC_GetMasterClock (
 	AT91PS_PMC pPMC, // \arg pointer to PMC controller
 	AT91PS_CKGR pCKGR, // \arg pointer to CKGR controller
-	unsigned int slowClock)  // \arg slowClock in Hz
-{
-	unsigned int reg = pPMC->PMC_MCKR;
-	unsigned int prescaler = (1 << ((reg & AT91C_PMC_PRES) >> 2));
-	unsigned int pllDivider, pllMultiplier;
-
-	switch (reg & AT91C_PMC_CSS) {
-		case AT91C_PMC_CSS_SLOW_CLK: // Slow clock selected
-			return slowClock / prescaler;
-		case AT91C_PMC_CSS_MAIN_CLK: // Main clock is selected
-			return AT91F_CKGR_GetMainClock(pCKGR, slowClock) / prescaler;
-		case AT91C_PMC_CSS_PLL_CLK: // PLLB clock is selected
-			reg = pCKGR->CKGR_PLLR;
-			pllDivider    = (reg  & AT91C_CKGR_DIV);
-			pllMultiplier = ((reg  & AT91C_CKGR_MUL) >> 16) + 1;
-			return AT91F_CKGR_GetMainClock(pCKGR, slowClock) / pllDivider * pllMultiplier / prescaler;
-	}
-	return 0;
-}
+	unsigned int slowClock);  // \arg slowClock in Hz
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_PMC_EnablePCK
@@ -1473,18 +1349,8 @@ static inline unsigned int AT91F_RTTGetStatus(
 //* \fn     AT91F_RTT_ReadValue()
 //* \brief  Read the RTT value
 //*--------------------------------------------------------------------------------------
-static inline unsigned int AT91F_RTTReadValue(
-        AT91PS_RTTC pRTTC)
-{
-        register volatile unsigned int val1,val2;
-	do
-	{
-		val1 = pRTTC->RTTC_RTVR;
-		val2 = pRTTC->RTTC_RTVR;
-	}	
-	while(val1 != val2);
-	return(val1);
-}
+extern unsigned int AT91F_RTTReadValue(AT91PS_RTTC pRTTC);
+
 /* *****************************************************************************
                 SOFTWARE API FOR PITC
    ***************************************************************************** */
@@ -1871,27 +1737,7 @@ static inline unsigned int AT91F_SPI_SendFrame(
 //* \fn    AT91F_SPI_Close
 //* \brief Close SPI: disable IT disable transfert, close PDC
 //*----------------------------------------------------------------------------
-static inline void AT91F_SPI_Close (
-	AT91PS_SPI pSPI)     // \arg pointer to a SPI controller
-{
-    //* Reset all the Chip Select register
-    pSPI->SPI_CSR[0] = 0 ;
-    pSPI->SPI_CSR[1] = 0 ;
-    pSPI->SPI_CSR[2] = 0 ;
-    pSPI->SPI_CSR[3] = 0 ;
-
-    //* Reset the SPI mode
-    pSPI->SPI_MR = 0  ;
-
-    //* Disable all interrupts
-    pSPI->SPI_IDR = 0xFFFFFFFF ;
-
-    //* Abort the Peripheral Data Transfers
-    AT91F_PDC_Close((AT91PS_PDC) &(pSPI->SPI_RPR));
-
-    //* Disable receiver and transmitter and stop any activity immediately
-    pSPI->SPI_CR = AT91C_SPI_SPIDIS;
-}
+extern void AT91F_SPI_Close(AT91PS_SPI pSPI);     // \arg pointer to a SPI controller
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_SPI_PutChar
@@ -2034,22 +1880,12 @@ static inline unsigned int AT91F_ADC_GetModeReg (
 //* \fn    AT91F_ADC_CfgTimings
 //* \brief Configure the different necessary timings of the ADC controller
 //*----------------------------------------------------------------------------
-static inline void AT91F_ADC_CfgTimings (
+extern void AT91F_ADC_CfgTimings (
 	AT91PS_ADC pADC, // pointer to a ADC controller
 	unsigned int mck_clock, // in MHz 
 	unsigned int adc_clock, // in MHz 
 	unsigned int startup_time, // in us 
-	unsigned int sample_and_hold_time)	// in ns  
-{
-	unsigned int prescal,startup,shtim;
-	
-	prescal = mck_clock/(2*adc_clock) - 1;
-	startup = adc_clock*startup_time/8 - 1;
-	shtim = adc_clock*sample_and_hold_time/1000 - 1;
-	
-	//* Write to the MR register
-	pADC->ADC_MR = ( (prescal<<8) & AT91C_ADC_PRESCAL) | ( (startup<<16) & AT91C_ADC_STARTUP) | ( (shtim<<24) & AT91C_ADC_SHTIM);
-}
+	unsigned int sample_and_hold_time);	// in ns  
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_ADC_EnableChannel
@@ -2240,66 +2076,23 @@ static inline unsigned int AT91F_ADC_GetConvertedDataCH7 (
 //* \fn    AT91F_SSC_SetBaudrate
 //* \brief Set the baudrate according to the CPU clock
 //*----------------------------------------------------------------------------
-static inline void AT91F_SSC_SetBaudrate (
+extern void AT91F_SSC_SetBaudrate (
         AT91PS_SSC pSSC,        // \arg pointer to a SSC controller
         unsigned int mainClock, // \arg peripheral clock
-        unsigned int speed)     // \arg SSC baudrate
-{
-        unsigned int baud_value;
-        //* Define the baud rate divisor register
-        if (speed == 0)
-           baud_value = 0;
-        else
-        {
-           baud_value = (unsigned int) (mainClock * 10)/(2*speed);
-           if ((baud_value % 10) >= 5)
-                  baud_value = (baud_value / 10) + 1;
-           else
-                  baud_value /= 10;
-        }
-
-        pSSC->SSC_CMR = baud_value;
-}
+        unsigned int speed);     // \arg SSC baudrate
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_SSC_Configure
 //* \brief Configure SSC
 //*----------------------------------------------------------------------------
-static inline void AT91F_SSC_Configure (
+extern void AT91F_SSC_Configure (
              AT91PS_SSC pSSC,          // \arg pointer to a SSC controller
              unsigned int syst_clock,  // \arg System Clock Frequency
              unsigned int baud_rate,   // \arg Expected Baud Rate Frequency
              unsigned int clock_rx,    // \arg Receiver Clock Parameters
              unsigned int mode_rx,     // \arg mode Register to be programmed
              unsigned int clock_tx,    // \arg Transmitter Clock Parameters
-             unsigned int mode_tx)     // \arg mode Register to be programmed
-{
-    //* Disable interrupts
-	pSSC->SSC_IDR = (unsigned int) -1;
-
-    //* Reset receiver and transmitter
-	pSSC->SSC_CR = AT91C_SSC_SWRST | AT91C_SSC_RXDIS | AT91C_SSC_TXDIS ;
-
-    //* Define the Clock Mode Register
-	AT91F_SSC_SetBaudrate(pSSC, syst_clock, baud_rate);
-
-     //* Write the Receive Clock Mode Register
-	pSSC->SSC_RCMR =  clock_rx;
-
-     //* Write the Transmit Clock Mode Register
-	pSSC->SSC_TCMR =  clock_tx;
-
-     //* Write the Receive Frame Mode Register
-	pSSC->SSC_RFMR =  mode_rx;
-
-     //* Write the Transmit Frame Mode Register
-	pSSC->SSC_TFMR =  mode_tx;
-
-    //* Clear Transmit and Receive Counters
-	AT91F_PDC_Open((AT91PS_PDC) &(pSSC->SSC_RPR));
-
-
-}
+             unsigned int mode_tx);    // \arg mode Register to be programmed
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_SSC_EnableRx
@@ -2544,32 +2337,12 @@ static inline void AT91F_US_DisableIt (
 //* \fn    AT91F_US_Configure
 //* \brief Configure USART
 //*----------------------------------------------------------------------------
-static inline void AT91F_US_Configure (
+extern void AT91F_US_Configure (
 	AT91PS_USART pUSART,     // \arg pointer to a USART controller
 	unsigned int mainClock,  // \arg peripheral clock
 	unsigned int mode ,      // \arg mode Register to be programmed
 	unsigned int baudRate ,  // \arg baudrate to be programmed
-	unsigned int timeguard ) // \arg timeguard to be programmed
-{
-    //* Disable interrupts
-    pUSART->US_IDR = (unsigned int) -1;
-
-    //* Reset receiver and transmitter
-    pUSART->US_CR = AT91C_US_RSTRX | AT91C_US_RSTTX | AT91C_US_RXDIS | AT91C_US_TXDIS ;
-
-	//* Define the baud rate divisor register
-	AT91F_US_SetBaudrate(pUSART, mainClock, baudRate);
-
-	//* Write the Timeguard Register
-	AT91F_US_SetTimeguard(pUSART, timeguard);
-
-    //* Clear Transmit and Receive Counters
-    AT91F_PDC_Open((AT91PS_PDC) &(pUSART->US_RPR));
-
-    //* Define the USART mode
-    pUSART->US_MR = mode  ;
-
-}
+	unsigned int timeguard ); // \arg timeguard to be programmed
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_US_EnableRx
@@ -2645,27 +2418,7 @@ static inline void AT91F_US_DisableTx (
 //* \fn    AT91F_US_Close
 //* \brief Close USART: disable IT disable receiver and transmitter, close PDC
 //*----------------------------------------------------------------------------
-static inline void AT91F_US_Close (
-	AT91PS_USART pUSART)     // \arg pointer to a USART controller
-{
-    //* Reset the baud rate divisor register
-    pUSART->US_BRGR = 0 ;
-
-    //* Reset the USART mode
-    pUSART->US_MR = 0  ;
-
-    //* Reset the Timeguard Register
-    pUSART->US_TTGR = 0;
-
-    //* Disable all interrupts
-    pUSART->US_IDR = 0xFFFFFFFF ;
-
-    //* Abort the Peripheral Data Transfers
-    AT91F_PDC_Close((AT91PS_PDC) &(pUSART->US_RPR));
-
-    //* Disable receiver and transmitter and stop any activity immediately
-    pUSART->US_CR = AT91C_US_TXDIS | AT91C_US_RXDIS | AT91C_US_RSTTX | AT91C_US_RSTRX ;
-}
+extern void AT91F_US_Close(AT91PS_USART pUSART);     // \arg pointer to a USART controller
 
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_US_TxReady
diff --git a/openpcd/firmware/include/usb_ch9.h b/openpcd/firmware/include/usb_ch9.h
index 40eeba7..b10c8ae 100644
--- a/openpcd/firmware/include/usb_ch9.h
+++ b/openpcd/firmware/include/usb_ch9.h
@@ -293,11 +293,6 @@ struct usb_endpoint_descriptor {
 	u_int8_t  bmAttributes;
 	u_int16_t wMaxPacketSize;
 	u_int8_t  bInterval;
-
-	/* NOTE:  these two are _only_ in audio endpoints. */
-	/* use USB_DT_ENDPOINT*_SIZE in bLength, not sizeof. */
-	u_int8_t  bRefresh;
-	u_int8_t  bSynchAddress;
 } __attribute__ ((packed));
 
 #define USB_DT_ENDPOINT_SIZE		7
diff --git a/openpcd/firmware/lib/lib_AT91SAM7S64.c b/openpcd/firmware/lib/lib_AT91SAM7S64.c
new file mode 100644
index 0000000..81a9ed0
--- /dev/null
+++ b/openpcd/firmware/lib/lib_AT91SAM7S64.c
@@ -0,0 +1,404 @@
+//* ----------------------------------------------------------------------------
+//*         ATMEL Microcontroller Software Support  -  ROUSSET  -
+//* ----------------------------------------------------------------------------
+//* DISCLAIMER:  THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
+//* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+//* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
+//* DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, INDIRECT,
+//* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+//* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+//* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+//* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+//* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+//* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//* ----------------------------------------------------------------------------
+//* File Name           : lib_AT91SAM7S64.h
+//* Object              : AT91SAM7S64 inlined functions
+//* Generated           : AT91 SW Application Group  08/30/2005 (15:52:59)
+//*
+
+#include <include/AT91SAM7S64.h>
+#include <include/lib_AT91SAM7S64.h>
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_ConfigureIt
+//* \brief Interrupt Handler Initialization
+//*----------------------------------------------------------------------------
+unsigned int AT91F_AIC_ConfigureIt (
+	AT91PS_AIC pAic,  // \arg pointer to the AIC registers
+	unsigned int irq_id,     // \arg interrupt number to initialize
+	unsigned int priority,   // \arg priority to give to the interrupt
+	unsigned int src_type,   // \arg activation and sense of activation
+	void (*newHandler) () ) // \arg address of the interrupt handler
+{
+	unsigned int oldHandler;
+    unsigned int mask ;
+
+    oldHandler = pAic->AIC_SVR[irq_id];
+
+    mask = 0x1 << irq_id ;
+    //* Disable the interrupt on the interrupt controller
+    pAic->AIC_IDCR = mask ;
+    //* Save the interrupt handler routine pointer and the interrupt priority
+    pAic->AIC_SVR[irq_id] = (unsigned int) newHandler ;
+    //* Store the Source Mode Register
+    pAic->AIC_SMR[irq_id] = src_type | priority  ;
+    //* Clear the interrupt on the interrupt controller
+    pAic->AIC_ICCR = mask ;
+
+	return oldHandler;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_SetExceptionVector
+//* \brief Configure vector handler
+//*----------------------------------------------------------------------------
+unsigned int  AT91F_AIC_SetExceptionVector (
+	unsigned int *pVector, // \arg pointer to the AIC registers
+	void (*Handler) () )   // \arg Interrupt Handler
+{
+	unsigned int oldVector = *pVector;
+
+	if ((unsigned int) Handler == (unsigned int) AT91C_AIC_BRANCH_OPCODE)
+		*pVector = (unsigned int) AT91C_AIC_BRANCH_OPCODE;
+	else
+		*pVector = (((((unsigned int) Handler) - ((unsigned int) pVector) - 0x8) >> 2) & 0x00FFFFFF) | 0xEA000000;
+
+	return oldVector;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_AIC_Open
+//* \brief Set exception vectors and AIC registers to default values
+//*----------------------------------------------------------------------------
+void AT91F_AIC_Open(
+	AT91PS_AIC pAic,        // \arg pointer to the AIC registers
+	void (*IrqHandler) (),  // \arg Default IRQ vector exception
+	void (*FiqHandler) (),  // \arg Default FIQ vector exception
+	void (*DefaultHandler)  (), // \arg Default Handler set in ISR
+	void (*SpuriousHandler) (), // \arg Default Spurious Handler
+	unsigned int protectMode)   // \arg Debug Control Register
+{
+	int i;
+
+	// Disable all interrupts and set IVR to the default handler
+	for (i = 0; i < 32; ++i) {
+		AT91F_AIC_DisableIt(pAic, i);
+		AT91F_AIC_ConfigureIt(pAic, i, AT91C_AIC_PRIOR_LOWEST, AT91C_AIC_SRCTYPE_HIGH_LEVEL, DefaultHandler);
+	}
+
+	// Set the IRQ exception vector
+	AT91F_AIC_SetExceptionVector((unsigned int *) 0x18, IrqHandler);
+	// Set the Fast Interrupt exception vector
+	AT91F_AIC_SetExceptionVector((unsigned int *) 0x1C, FiqHandler);
+
+	pAic->AIC_SPU = (unsigned int) SpuriousHandler;
+	pAic->AIC_DCR = protectMode;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_Open
+//* \brief Open PDC: disable TX and RX reset transfer descriptors, re-enable RX and TX
+//*----------------------------------------------------------------------------
+void AT91F_PDC_Open(AT91PS_PDC pPDC)       // \arg pointer to a PDC controller
+{
+    //* Disable the RX and TX PDC transfer requests
+	AT91F_PDC_DisableRx(pPDC);
+	AT91F_PDC_DisableTx(pPDC);
+
+	//* Reset all Counter register Next buffer first
+	AT91F_PDC_SetNextTx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetNextRx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetTx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetRx(pPDC, (char *) 0, 0);
+
+    //* Enable the RX and TX PDC transfer requests
+	AT91F_PDC_EnableRx(pPDC);
+	AT91F_PDC_EnableTx(pPDC);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_Close
+//* \brief Close PDC: disable TX and RX reset transfer descriptors
+//*----------------------------------------------------------------------------
+void AT91F_PDC_Close(AT91PS_PDC pPDC)       // \arg pointer to a PDC controller
+{
+    //* Disable the RX and TX PDC transfer requests
+	AT91F_PDC_DisableRx(pPDC);
+	AT91F_PDC_DisableTx(pPDC);
+
+	//* Reset all Counter register Next buffer first
+	AT91F_PDC_SetNextTx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetNextRx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetTx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetRx(pPDC, (char *) 0, 0);
+
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_SendFrame
+//* \brief Close PDC: disable TX and RX reset transfer descriptors
+//*----------------------------------------------------------------------------
+unsigned int AT91F_PDC_SendFrame(
+	AT91PS_PDC pPDC,
+	char *pBuffer,
+	unsigned int szBuffer,
+	char *pNextBuffer,
+	unsigned int szNextBuffer )
+{
+	if (AT91F_PDC_IsTxEmpty(pPDC)) {
+		//* Buffer and next buffer can be initialized
+		AT91F_PDC_SetTx(pPDC, pBuffer, szBuffer);
+		AT91F_PDC_SetNextTx(pPDC, pNextBuffer, szNextBuffer);
+		return 2;
+	}
+	else if (AT91F_PDC_IsNextTxEmpty(pPDC)) {
+		//* Only one buffer can be initialized
+		AT91F_PDC_SetNextTx(pPDC, pBuffer, szBuffer);
+		return 1;
+	}
+	else {
+		//* All buffer are in use...
+		return 0;
+	}
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_PDC_ReceiveFrame
+//* \brief Close PDC: disable TX and RX reset transfer descriptors
+//*----------------------------------------------------------------------------
+unsigned int AT91F_PDC_ReceiveFrame (
+	AT91PS_PDC pPDC,
+	char *pBuffer,
+	unsigned int szBuffer,
+	char *pNextBuffer,
+	unsigned int szNextBuffer )
+{
+	if (AT91F_PDC_IsRxEmpty(pPDC)) {
+		//* Buffer and next buffer can be initialized
+		AT91F_PDC_SetRx(pPDC, pBuffer, szBuffer);
+		AT91F_PDC_SetNextRx(pPDC, pNextBuffer, szNextBuffer);
+		return 2;
+	}
+	else if (AT91F_PDC_IsNextRxEmpty(pPDC)) {
+		//* Only one buffer can be initialized
+		AT91F_PDC_SetNextRx(pPDC, pBuffer, szBuffer);
+		return 1;
+	}
+	else {
+		//* All buffer are in use...
+		return 0;
+	}
+}
+
+//*------------------------------------------------------------------------------
+//* \fn    AT91F_PMC_GetMasterClock
+//* \brief Return master clock in Hz which correponds to processor clock for ARM7
+//*------------------------------------------------------------------------------
+unsigned int AT91F_PMC_GetMasterClock (
+	AT91PS_PMC pPMC, // \arg pointer to PMC controller
+	AT91PS_CKGR pCKGR, // \arg pointer to CKGR controller
+	unsigned int slowClock)  // \arg slowClock in Hz
+{
+	unsigned int reg = pPMC->PMC_MCKR;
+	unsigned int prescaler = (1 << ((reg & AT91C_PMC_PRES) >> 2));
+	unsigned int pllDivider, pllMultiplier;
+
+	switch (reg & AT91C_PMC_CSS) {
+		case AT91C_PMC_CSS_SLOW_CLK: // Slow clock selected
+			return slowClock / prescaler;
+		case AT91C_PMC_CSS_MAIN_CLK: // Main clock is selected
+			return AT91F_CKGR_GetMainClock(pCKGR, slowClock) / prescaler;
+		case AT91C_PMC_CSS_PLL_CLK: // PLLB clock is selected
+			reg = pCKGR->CKGR_PLLR;
+			pllDivider    = (reg  & AT91C_CKGR_DIV);
+			pllMultiplier = ((reg  & AT91C_CKGR_MUL) >> 16) + 1;
+			return AT91F_CKGR_GetMainClock(pCKGR, slowClock) / pllDivider * pllMultiplier / prescaler;
+	}
+	return 0;
+}
+
+//*--------------------------------------------------------------------------------------
+//* \fn     AT91F_RTT_ReadValue()
+//* \brief  Read the RTT value
+//*--------------------------------------------------------------------------------------
+unsigned int AT91F_RTTReadValue(AT91PS_RTTC pRTTC)
+{
+        register volatile unsigned int val1,val2;
+	do
+	{
+		val1 = pRTTC->RTTC_RTVR;
+		val2 = pRTTC->RTTC_RTVR;
+	}	
+	while(val1 != val2);
+	return(val1);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_SPI_Close
+//* \brief Close SPI: disable IT disable transfert, close PDC
+//*----------------------------------------------------------------------------
+void AT91F_SPI_Close(AT91PS_SPI pSPI)     // \arg pointer to a SPI controller
+{
+    //* Reset all the Chip Select register
+    pSPI->SPI_CSR[0] = 0 ;
+    pSPI->SPI_CSR[1] = 0 ;
+    pSPI->SPI_CSR[2] = 0 ;
+    pSPI->SPI_CSR[3] = 0 ;
+
+    //* Reset the SPI mode
+    pSPI->SPI_MR = 0  ;
+
+    //* Disable all interrupts
+    pSPI->SPI_IDR = 0xFFFFFFFF ;
+
+    //* Abort the Peripheral Data Transfers
+    AT91F_PDC_Close((AT91PS_PDC) &(pSPI->SPI_RPR));
+
+    //* Disable receiver and transmitter and stop any activity immediately
+    pSPI->SPI_CR = AT91C_SPI_SPIDIS;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_ADC_CfgTimings
+//* \brief Configure the different necessary timings of the ADC controller
+//*----------------------------------------------------------------------------
+void AT91F_ADC_CfgTimings (
+	AT91PS_ADC pADC, // pointer to a ADC controller
+	unsigned int mck_clock, // in MHz 
+	unsigned int adc_clock, // in MHz 
+	unsigned int startup_time, // in us 
+	unsigned int sample_and_hold_time)	// in ns  
+{
+	unsigned int prescal,startup,shtim;
+	
+	prescal = mck_clock/(2*adc_clock) - 1;
+	startup = adc_clock*startup_time/8 - 1;
+	shtim = adc_clock*sample_and_hold_time/1000 - 1;
+	
+	//* Write to the MR register
+	pADC->ADC_MR = ( (prescal<<8) & AT91C_ADC_PRESCAL) | ( (startup<<16) & AT91C_ADC_STARTUP) | ( (shtim<<24) & AT91C_ADC_SHTIM);
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_SSC_SetBaudrate
+//* \brief Set the baudrate according to the CPU clock
+//*----------------------------------------------------------------------------
+void AT91F_SSC_SetBaudrate (
+        AT91PS_SSC pSSC,        // \arg pointer to a SSC controller
+        unsigned int mainClock, // \arg peripheral clock
+        unsigned int speed)     // \arg SSC baudrate
+{
+        unsigned int baud_value;
+        //* Define the baud rate divisor register
+        if (speed == 0)
+           baud_value = 0;
+        else
+        {
+           baud_value = (unsigned int) (mainClock * 10)/(2*speed);
+           if ((baud_value % 10) >= 5)
+                  baud_value = (baud_value / 10) + 1;
+           else
+                  baud_value /= 10;
+        }
+
+        pSSC->SSC_CMR = baud_value;
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_SSC_Configure
+//* \brief Configure SSC
+//*----------------------------------------------------------------------------
+void AT91F_SSC_Configure (
+             AT91PS_SSC pSSC,          // \arg pointer to a SSC controller
+             unsigned int syst_clock,  // \arg System Clock Frequency
+             unsigned int baud_rate,   // \arg Expected Baud Rate Frequency
+             unsigned int clock_rx,    // \arg Receiver Clock Parameters
+             unsigned int mode_rx,     // \arg mode Register to be programmed
+             unsigned int clock_tx,    // \arg Transmitter Clock Parameters
+             unsigned int mode_tx)     // \arg mode Register to be programmed
+{
+    //* Disable interrupts
+	pSSC->SSC_IDR = (unsigned int) -1;
+
+    //* Reset receiver and transmitter
+	pSSC->SSC_CR = AT91C_SSC_SWRST | AT91C_SSC_RXDIS | AT91C_SSC_TXDIS ;
+
+    //* Define the Clock Mode Register
+	AT91F_SSC_SetBaudrate(pSSC, syst_clock, baud_rate);
+
+     //* Write the Receive Clock Mode Register
+	pSSC->SSC_RCMR =  clock_rx;
+
+     //* Write the Transmit Clock Mode Register
+	pSSC->SSC_TCMR =  clock_tx;
+
+     //* Write the Receive Frame Mode Register
+	pSSC->SSC_RFMR =  mode_rx;
+
+     //* Write the Transmit Frame Mode Register
+	pSSC->SSC_TFMR =  mode_tx;
+
+    //* Clear Transmit and Receive Counters
+	AT91F_PDC_Open((AT91PS_PDC) &(pSSC->SSC_RPR));
+
+
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_US_Configure
+//* \brief Configure USART
+//*----------------------------------------------------------------------------
+void AT91F_US_Configure (
+	AT91PS_USART pUSART,     // \arg pointer to a USART controller
+	unsigned int mainClock,  // \arg peripheral clock
+	unsigned int mode ,      // \arg mode Register to be programmed
+	unsigned int baudRate ,  // \arg baudrate to be programmed
+	unsigned int timeguard ) // \arg timeguard to be programmed
+{
+    //* Disable interrupts
+    pUSART->US_IDR = (unsigned int) -1;
+
+    //* Reset receiver and transmitter
+    pUSART->US_CR = AT91C_US_RSTRX | AT91C_US_RSTTX | AT91C_US_RXDIS | AT91C_US_TXDIS ;
+
+	//* Define the baud rate divisor register
+	AT91F_US_SetBaudrate(pUSART, mainClock, baudRate);
+
+	//* Write the Timeguard Register
+	AT91F_US_SetTimeguard(pUSART, timeguard);
+
+    //* Clear Transmit and Receive Counters
+    AT91F_PDC_Open((AT91PS_PDC) &(pUSART->US_RPR));
+
+    //* Define the USART mode
+    pUSART->US_MR = mode  ;
+
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_US_Close
+//* \brief Close USART: disable IT disable receiver and transmitter, close PDC
+//*----------------------------------------------------------------------------
+void AT91F_US_Close(AT91PS_USART pUSART)     // \arg pointer to a USART controller
+{
+    //* Reset the baud rate divisor register
+    pUSART->US_BRGR = 0 ;
+
+    //* Reset the USART mode
+    pUSART->US_MR = 0  ;
+
+    //* Reset the Timeguard Register
+    pUSART->US_TTGR = 0;
+
+    //* Disable all interrupts
+    pUSART->US_IDR = 0xFFFFFFFF ;
+
+    //* Abort the Peripheral Data Transfers
+    AT91F_PDC_Close((AT91PS_PDC) &(pUSART->US_RPR));
+
+    //* Disable receiver and transmitter and stop any activity immediately
+    pUSART->US_CR = AT91C_US_TXDIS | AT91C_US_RXDIS | AT91C_US_RSTTX | AT91C_US_RSTRX ;
+}
+
+
diff --git a/openpcd/firmware/src/main.c b/openpcd/firmware/src/main.c
index 375c665..2a569d4 100644
--- a/openpcd/firmware/src/main.c
+++ b/openpcd/firmware/src/main.c
@@ -30,17 +30,8 @@
 #endif
 #endif
 
-//* external function
-
-extern void Usart_init(void);
-extern void AT91F_US_Put(char *buffer);	// \arg pointer to a string ending by \0
-extern void Trace_Toggel_LED(unsigned int led);
-
 //*----------------------------------------------------------------------------
-//* \fn    AT91F_USB_Open
-//* \brief This function Open the USB device
-//*----------------------------------------------------------------------------
-void AT91F_USB_Open(void)
+static void AT91F_USB_Open(void)
 {
 	// Set the PLL USB Divider
 	AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
@@ -75,6 +66,8 @@ int main(void)
 	AT91F_DBGU_Printk
 	    ("\n\r-I- Basic USB loop back\n\r 0) Set Pull-UP 1) Clear Pull UP\n\r");
 
+	//printf("test 0x%02x\n\r", 123);
+
 	// Enable User Reset and set its minimal assertion to 960 us
 	AT91C_BASE_RSTC->RSTC_RMR =
 	    AT91C_RSTC_URSTEN | (0x4 << 8) | (unsigned int)(0xA5 << 24);
diff --git a/openpcd/firmware/src/pcd_enumerate.c b/openpcd/firmware/src/pcd_enumerate.c
index 7997e32..46b2763 100644
--- a/openpcd/firmware/src/pcd_enumerate.c
+++ b/openpcd/firmware/src/pcd_enumerate.c
@@ -25,6 +25,8 @@
 #include "pcd_enumerate.h"
 #include "dbgu.h"
 
+static char dbg_buf[256];
+
 static struct _AT91S_CDC pCDC;
 static AT91PS_CDC pCdc = &pCDC;
 
@@ -50,7 +52,7 @@ struct usb_device_descriptor devDescriptor = {
 struct _desc {
 	struct usb_config_descriptor ucfg;
 	struct usb_interface_descriptor uif;
-	    struct usb_endpoint_descriptor ep[3];
+	struct usb_endpoint_descriptor ep[3];
 };
 
 const struct _desc cfgDescriptor = {
@@ -76,30 +78,30 @@ const struct _desc cfgDescriptor = {
 		.bInterfaceProtocol = 0xff,
 		.iInterface = 0,
 		},
-	.ep[0] = {
-		  .bLength = USB_DT_ENDPOINT_SIZE,
-		  .bDescriptorType = USB_DT_ENDPOINT,
-		  .bEndpointAddress = 0x01,
-		  .bmAttributes = USB_ENDPOINT_XFER_BULK,
-		  .wMaxPacketSize = 64,
-		  .bInterval = 0x10,	/* FIXME */
-		  },
-	.ep[1] = {
-		  .bLength = USB_DT_ENDPOINT_SIZE,
-		  .bDescriptorType = USB_DT_ENDPOINT,
-		  .bEndpointAddress = 0x81,
-		  .bmAttributes = USB_ENDPOINT_XFER_BULK,
-		  .wMaxPacketSize = 64,
-		  .bInterval = 0x10,	/* FIXME */
-		  },
-	.ep[2] = {
-		  .bLength = USB_DT_ENDPOINT_SIZE,
-		  .bDescriptorType = USB_DT_ENDPOINT,
-		  .bEndpointAddress = 0x82,
-		  .bmAttributes = USB_ENDPOINT_XFER_INT,
-		  .wMaxPacketSize = 64,
-		  .bInterval = 0x10,	/* FIXME */
+	.ep= {
+		{
+			  .bLength = USB_DT_ENDPOINT_SIZE,
+			  .bDescriptorType = USB_DT_ENDPOINT,
+			  .bEndpointAddress = 0x01,
+			  .bmAttributes = USB_ENDPOINT_XFER_BULK,
+			  .wMaxPacketSize = 64,
+			  .bInterval = 0x10,	/* FIXME */
+		  }, {
+			  .bLength = USB_DT_ENDPOINT_SIZE,
+			  .bDescriptorType = USB_DT_ENDPOINT,
+			  .bEndpointAddress = 0x81,
+			  .bmAttributes = USB_ENDPOINT_XFER_BULK,
+			  .wMaxPacketSize = 64,
+			  .bInterval = 0x10,	/* FIXME */
+		  }, {
+			  .bLength = USB_DT_ENDPOINT_SIZE,
+			  .bDescriptorType = USB_DT_ENDPOINT,
+			  .bEndpointAddress = 0x82,
+			  .bmAttributes = USB_ENDPOINT_XFER_INT,
+			  .wMaxPacketSize = 64,
+			  .bInterval = 0x10,	/* FIXME */
 		  },
+	},
 };
 
 /* USB standard request code */
@@ -130,16 +132,25 @@ 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];
+
 static void udp_irq(void)
 {
+	u_int32_t csr;
 	AT91PS_UDP pUDP = pCDC.pUdp;
 	AT91_REG isr = pUDP->UDP_ISR;
 
-	DEBUGP("udp_irq: ");
+	sprintf(dbg_buf, "udp_irq(imr=0x%04x, isr=0x%04x): ", pUDP->UDP_IMR, isr);
+	DEBUGP(dbg_buf);
+
+	//DEBUGP("udp_irq: ");
+
+	AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_UDP);
 
 	if (isr & AT91C_UDP_ENDBUSRES) {
 		DEBUGP("ENDBUSRES ");
 		pUDP->UDP_ICR = AT91C_UDP_ENDBUSRES;
+		pUDP->UDP_IER = AT91C_UDP_EPINT0;
 		// reset all endpoints
 		pUDP->UDP_RSTEP = (unsigned int)-1;
 		pUDP->UDP_RSTEP = 0;
@@ -151,17 +162,65 @@ static void udp_irq(void)
 	}
 
 	if (isr & AT91C_UDP_EPINT0) {
-		DEBUGP("EP0INT ");
 		pUDP->UDP_ICR = AT91C_UDP_EPINT0;
+		DEBUGP("EP0INT(Control) ");
 		AT91F_CDC_Enumerate();
+		DEBUGP("Enumerate finish\n");
 	}
 	if (isr & AT91C_UDP_EPINT1) {
-		DEBUGP("EP1INT ");
+		u_int32_t cur_rcv_bank = pCDC.currentRcvBank;
+		pUDP->UDP_ICR = AT91C_UDP_EPINT1;
+		csr = pUDP->UDP_CSR[1];
+		DEBUGP("EP1INT(Out) ");
+
+		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;
+		}
+
+		rcv_data[63] = '\0';
+		DEBUGP(rcv_data);
 	}
 	if (isr & AT91C_UDP_EPINT2) {
-		DEBUGP("EP2INT ");
+		csr = pUDP->UDP_CSR[2];
+		pUDP->UDP_ICR = AT91C_UDP_EPINT2;
+		DEBUGP("EP2INT(In) ");
 	}
-	DEBUGP("\n");
+	if (isr & AT91C_UDP_EPINT3) {
+		csr = pUDP->UDP_CSR[3];
+		pUDP->UDP_ICR = AT91C_UDP_EPINT3;
+		DEBUGP("EP3INT(Interrupt) ");
+	}
+
+	if (isr & AT91C_UDP_RXSUSP) {
+		pUDP->UDP_ICR = AT91C_UDP_RXSUSP;
+		DEBUGP("RXSUSP ");
+	}
+	if (isr & AT91C_UDP_RXRSM) {
+		pUDP->UDP_ICR = AT91C_UDP_RXRSM;
+		DEBUGP("RXRSM ");
+	}
+	if (isr & AT91C_UDP_EXTRSM) {
+		pUDP->UDP_ICR = AT91C_UDP_EXTRSM;
+		DEBUGP("EXTRSM ");
+	}
+	if (isr & AT91C_UDP_SOFINT) {
+		pUDP->UDP_ICR = AT91C_UDP_SOFINT;
+		DEBUGP("SOFINT ");
+	}
+	if (isr & AT91C_UDP_WAKEUP) {
+		pUDP->UDP_ICR = AT91C_UDP_WAKEUP;
+		DEBUGP("WAKEUP ");
+	}
+
+	DEBUGP("END\r\n");
 }
 
 //*----------------------------------------------------------------------------
@@ -176,11 +235,13 @@ AT91PS_CDC AT91F_CDC_Open(AT91PS_UDP pUdp)
 	pCdc->currentRcvBank = AT91C_UDP_RX_DATA_BK0;
 
 	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_UDP, AT91C_AIC_PRIOR_LOWEST, 
-			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &udp_irq);
+			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, udp_irq);
+			      //AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE, udp_irq);
 	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);
+	pCdc->pUdp->UDP_IER = (AT91C_UDP_EPINT0|AT91C_UDP_EPINT1|
+			       AT91C_UDP_EPINT2|AT91C_UDP_EPINT3);
 
 	return pCdc;
 }
@@ -191,25 +252,6 @@ AT91PS_CDC AT91F_CDC_Open(AT91PS_UDP pUdp)
 //*----------------------------------------------------------------------------
 u_int8_t AT91F_UDP_IsConfigured(void)
 {
-#if 0
-	AT91PS_UDP pUDP = pCdc->pUdp;
-	AT91_REG isr = pUDP->UDP_ISR;
-
-	if (isr & AT91C_UDP_ENDBUSRES) {
-		pUDP->UDP_ICR = AT91C_UDP_ENDBUSRES;
-		// reset all endpoints
-		pUDP->UDP_RSTEP = (unsigned int)-1;
-		pUDP->UDP_RSTEP = 0;
-		// Enable the function
-		pUDP->UDP_FADDR = AT91C_UDP_FEN;
-		// Configure endpoint 0
-		pUDP->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
-		pCdc->currentConfiguration = 0;	/* +++ */
-	} else if (isr & AT91C_UDP_EPINT0) {
-		pUDP->UDP_ICR = AT91C_UDP_EPINT0;
-		AT91F_CDC_Enumerate(pCdc);
-	}
-#endif
 	return pCdc->currentConfiguration;
 }
 
@@ -300,6 +342,9 @@ static void AT91F_USB_SendData(AT91PS_UDP pUdp, const char *pData, u_int32_t len
 	u_int32_t cpt = 0;
 	AT91_REG csr;
 
+	sprintf(dbg_buf, "send_data: %u bytes ", length);
+	DEBUGP(dbg_buf);
+
 	do {
 		cpt = MIN(length, 8);
 		length -= cpt;
@@ -319,6 +364,7 @@ static void AT91F_USB_SendData(AT91PS_UDP pUdp, const char *pData, u_int32_t len
 			// 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);
+				DEBUGP("stopped by status out ");
 				return;
 			}
 		} while (!(csr & AT91C_UDP_TXCOMP));
@@ -365,8 +411,10 @@ static void AT91F_CDC_Enumerate(void)
 	u_int8_t bmRequestType, bRequest;
 	u_int16_t wValue, wIndex, wLength, wStatus;
 
-	if (!(pUDP->UDP_CSR[0] & AT91C_UDP_RXSETUP))
+	if (!(pUDP->UDP_CSR[0] & AT91C_UDP_RXSETUP)) {
+		DEBUGP("no setup packet ");
 		return;
+	}
 
 	bmRequestType = pUDP->UDP_FDR[0];
 	bRequest = pUDP->UDP_FDR[0];
@@ -387,6 +435,7 @@ static void AT91F_CDC_Enumerate(void)
 	// Handle supported standard device request Cf Table 9-3 in USB specification Rev 1.1
 	switch ((bRequest << 8) | bmRequestType) {
 	case STD_GET_DESCRIPTOR:
+		DEBUGP("GET_DESCRIPTOR ");
 		if (wValue == 0x100)	// Return Device Descriptor
 			AT91F_USB_SendData(pUDP, (const char *) &devDescriptor,
 					   MIN(sizeof(devDescriptor), wLength));
@@ -397,13 +446,19 @@ static void AT91F_CDC_Enumerate(void)
 			AT91F_USB_SendStall(pUDP);
 		break;
 	case STD_SET_ADDRESS:
+		DEBUGP("SET_ADDRESS ");
 		AT91F_USB_SendZlp(pUDP);
 		pUDP->UDP_FADDR = (AT91C_UDP_FEN | wValue);
 		pUDP->UDP_GLBSTATE = (wValue) ? AT91C_UDP_FADDEN : 0;
 		break;
 	case STD_SET_CONFIGURATION:
+		DEBUGP("SET_CONFIG ");
+		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] =
@@ -412,21 +467,28 @@ static void AT91F_CDC_Enumerate(void)
 		pUDP->UDP_CSR[2] =
 		    (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN) : 0;
 		pUDP->UDP_CSR[3] =
-		    (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_ISO_IN) : 0;
+		    (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 ");
 		AT91F_USB_SendData(pUDP, (char *)&(pCdc->currentConfiguration),
 				   sizeof(pCdc->currentConfiguration));
 		break;
 	case STD_GET_STATUS_ZERO:
+		DEBUGP("GET_STATUS_ZERO ");
 		wStatus = 0;
 		AT91F_USB_SendData(pUDP, (char *)&wStatus, sizeof(wStatus));
 		break;
 	case STD_GET_STATUS_INTERFACE:
+		DEBUGP("GET_STATUS_INTERFACE ");
 		wStatus = 0;
 		AT91F_USB_SendData(pUDP, (char *)&wStatus, sizeof(wStatus));
 		break;
 	case STD_GET_STATUS_ENDPOINT:
+		DEBUGP("GET_STATUS_ENDPOINT ");
 		wStatus = 0;
 		wIndex &= 0x0F;
 		if ((pUDP->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex <= 3)) {
@@ -444,12 +506,16 @@ static void AT91F_CDC_Enumerate(void)
 			AT91F_USB_SendStall(pUDP);
 		break;
 	case STD_SET_FEATURE_ZERO:
+		DEBUGP("SET_FEATURE_ZERO ");
 		AT91F_USB_SendStall(pUDP);
 		break;
 	case STD_SET_FEATURE_INTERFACE:
+		DEBUGP("SET_FEATURE_INTERFACE ");
 		AT91F_USB_SendZlp(pUDP);
 		break;
 	case STD_SET_FEATURE_ENDPOINT:
+		DEBUGP("SET_FEATURE_ENDPOINT ");
+		AT91F_USB_SendZlp(pUDP);
 		wIndex &= 0x0F;
 		if ((wValue == 0) && wIndex && (wIndex <= 3)) {
 			pUDP->UDP_CSR[wIndex] = 0;
@@ -458,12 +524,15 @@ static void AT91F_CDC_Enumerate(void)
 			AT91F_USB_SendStall(pUDP);
 		break;
 	case STD_CLEAR_FEATURE_ZERO:
+		DEBUGP("CLEAR_FEATURE_ZERO ");
 		AT91F_USB_SendStall(pUDP);
 		break;
 	case STD_CLEAR_FEATURE_INTERFACE:
+		DEBUGP("CLEAR_FEATURE_INTERFACE ");
 		AT91F_USB_SendZlp(pUDP);
 		break;
 	case STD_CLEAR_FEATURE_ENDPOINT:
+		DEBUGP("CLEAR_FEATURE_ENDPOINT ");
 		wIndex &= 0x0F;
 		if ((wValue == 0) && wIndex && (wIndex <= 3)) {
 			if (wIndex == 1)
@@ -476,12 +545,14 @@ static void AT91F_CDC_Enumerate(void)
 				     AT91C_UDP_EPTYPE_BULK_IN);
 			else if (wIndex == 3)
 				pUDP->UDP_CSR[3] =
-				    (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_ISO_IN);
+				    (AT91C_UDP_EPEDS |
+				     AT91C_UDP_EPTYPE_INT_IN);
 			AT91F_USB_SendZlp(pUDP);
 		} else
 			AT91F_USB_SendStall(pUDP);
 		break;
 	default:
+		DEBUGP("DEFAULT ");
 		AT91F_USB_SendStall(pUDP);
 		break;
 	}
diff --git a/openpcd/firmware/src/rc632.c b/openpcd/firmware/src/rc632.c
index b3f63e2..0de7765 100644
--- a/openpcd/firmware/src/rc632.c
+++ b/openpcd/firmware/src/rc632.c
@@ -13,6 +13,7 @@
 
 static AT91PS_SPI pSPI = AT91C_BASE_SPI;
 
+/* SPI irq handler */
 static void spi_irq(void)
 {
 	u_int32_t status = pSPI->SPI_SR;
@@ -63,10 +64,8 @@ struct rc632 {
 	struct fifo fifo;
 };
 #define RC632_F_FIFO_TX		0x0001
-
 static struct rc632 rc632;
 
-
 /* RC632 access primitives */
 
 void rc632_reg_write(u_int8_t addr, u_int8_t data)
@@ -165,9 +164,18 @@ static void rc632_irq(void)
 	DEBUGP("\n");
 }
 
+void rc632_power(u_int8_t up)
+{
+	if (up)
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_RC632_RESET);
+	else
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_RC632_RESET);
+}
+
 void rc632_reset(void)
 {
-	/* FIXME */
+	rc632_power(0);
+	rc632_power(1);
 }
 
 void rc632_init(void)
@@ -195,7 +203,8 @@ void rc632_init(void)
 	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_IRQ1);
 
 	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_RC632_RESET);
-	/* FIXME: get RC632 out of reset */
+
+	rc632_reset();
 };
 
 void rc632_exit(void)