Index: firmware/Makefile.dfu.local
===================================================================
--- firmware/Makefile.dfu.local	(nonexistent)
+++ firmware/Makefile.dfu.local	(revision 66923)
@@ -0,0 +1,575 @@
+# Hey Emacs, this is a -*- 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>
+#
+# based on the WinAVR makefile written by Eric B. Weddington, Jörg Wunsch, et al.
+# Released to the Public Domain
+# Please read the make user manual!
+#
+#
+# On command line:
+#
+# make all = Make software.
+#
+# make clean = Clean out built project files.
+#
+# make program = Download the hex file to the device
+#
+# (TODO: make filename.s = Just compile filename.c into the assembler code only)
+#
+# To rebuild project do "make clean" then "make all".
+#
+# Changelog:
+# - 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
+
+
+FLASH_TOOL = AT91FLASH
+#FLASH_TOOL = UVISION
+#FLASH_TOOL = OPENOCD
+
+# MCU name and submodel
+MCU      = arm7tdmi
+SUBMDL   = AT91SAM7S64
+#SUBMDL   = AT91SAM7S256
+
+USE_THUMB_MODE = NO
+#USE_THUMB_MODE = YES
+
+## Create ROM-Image (final)
+RUN_MODE=RUN_FROM_ROM
+## Create RAM-Image (debugging) - not used in this example
+#RUN_MODE=RUN_FROM_RAM
+
+## We want to produce a full-flash image, but including DFU 
+IMGTYPE=-sam7dfu-dfu
+
+# with / at end
+PATH_TO_LINKSCRIPTS=link/
+
+#### 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:=dfu
+
+USBSTRINGS=src/picc/usb_strings_dfu.h src/pcd/usb_strings_dfu.h src/simtrace/usb_strings_dfu.h
+
+# List C source files here. (C dependencies are automatically generated.)
+# use file-extension c for "c-only"-files
+SRC = 
+
+# List C source files here which must be compiled in ARM-Mode.
+# use file-extension c for "c-only"-files
+
+SRCARM  = src/start/Cstartup_SAM7.c lib/lib_AT91SAM7.c \
+	  src/dfu/dfu.c src/dfu/dbgu.c src/os/flash.c
+
+# List C++ source files here.
+# use file-extension cpp for C++-files (use extension .cpp)
+CPPSRC = 
+
+# List C++ source files here which must be compiled in ARM-Mode.
+# use file-extension cpp for C++-files (use extension .cpp)
+#CPPSRCARM = $(TARGET).cpp
+CPPSRCARM = 
+
+# List Assembler source files here.
+# Make them always end in a capital .S.  Files ending in a lowercase .s
+# will not be considered source files but generated files (assembler
+# output from the compiler), and will be deleted upon "make clean"!
+# Even though the DOS/Win* filesystem matches both .s and .S the same,
+# it will preserve the spelling of the filenames, and gcc itself does
+# care about how the name is spelled on its command-line.
+ASRC = 
+
+# List Assembler source files here which must be assembled in ARM-Mode..
+ASRCARM  = src/start/Cstartup.S
+
+ifeq ($(DEBUG),1)
+SRCARM 	+= lib/vsprintf.c lib/ctype.c lib/string.c
+ASRCARM += lib/div64.S
+endif
+
+## 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.
+# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
+OPT = s
+#OPT = 0
+
+# Debugging format.
+# Native formats for AVR-GCC's -g are stabs [default], or dwarf-2.
+# AVR (extended) COFF requires stabs, plus an avr-objcopy run.
+#DEBUGF = stabs
+DEBUGF = dwarf-2
+
+# List any extra directories to look for include files here.
+#     Each directory must be seperated by a space.
+#### FreeRTOS
+EXTRAINCDIRS =
+
+# 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
+# gnu89 - c89 plus GCC extensions
+# c99   - ISO C99 standard (not yet fully implemented)
+# gnu99 - c99 plus GCC extensions
+CSTANDARD = -std=gnu99
+
+# Place -D or -U options for C here
+CDEFS =  -D$(RUN_MODE) -D__MS_types__ -D__LIBRFID__ 
+
+ifdef DEBUG
+CDEFS += -DDEBUG 
+ADEFS += -DDEBUG 
+endif
+
+ifeq ($(BOARD),OLIMEX)
+CDEFS += -DOLIMEX
+ADEFS += -DOLIMEX
+CINCS  = -Isrc/simtrace
+endif
+
+ifeq ($(BOARD),PICC)
+CDEFS += -DPICC
+ADEFS += -DPICC
+CINCS  = -Isrc/picc
+endif
+
+ifeq ($(BOARD),PCD)
+SUBMDL = AT91SAM7S128
+CDEFS += -DPCD
+ADEFS += -DPCD
+CINCS  = -Isrc/pcd
+endif
+
+ifeq ($(BOARD),SIMTRACE)
+SUBMDL = AT91SAM7S128
+CDEFS += -DSIMTRACE
+ADEFS += -DSIMTRACE
+CINCS  = -Isrc/simtrace
+endif
+
+
+# Place -I options here
+CINCS += -Iinclude -Isrc
+
+# Place -D or -U options for ASM here
+ADEFS +=  -D$(RUN_MODE)
+
+ifdef VECTOR_LOCATION
+CDEFS += -D$(VECTOR_LOCATION)
+ADEFS += -D$(VECTOR_LOCATION)
+endif
+
+CDEFS += -D__$(SUBMDL)__
+ADEFS += -D__$(SUBMDL)__
+
+
+# Compiler flags.
+#  -g*:          generate debugging information
+#  -O*:          optimization level
+#  -f...:        tuning, see GCC manual and avr-libc documentation
+#  -Wall...:     warning level
+#  -Wa,...:      tell GCC to pass this to the assembler.
+#    -adhlns...: create assembler listing
+#
+# Flags for C and C++ (arm-none-eabi-gcc/arm-none-eabi-g++)
+CFLAGS += -g$(DEBUGF) -DBOARD=$(BOARD) 
+CFLAGS += $(CDEFS) $(CINCS)
+CFLAGS += -O$(OPT)
+CFLAGS += -Wall -Wextra -Wcast-align -Wimplicit -Wunused
+CFLAGS += -Wpointer-arith -Wswitch
+CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow 
+CFLAGS += -Wbad-function-cast -Wsign-compare -Waggregate-return 
+CFLAGS += -Wa,-adhlns=$(subst $(suffix $<),.lst,$<) 
+CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
+#CFLAGS += -ffunction-sections -fdata-sections       
+
+# flags only for C
+CONLYFLAGS += -Wnested-externs 
+CONLYFLAGS += $(CSTANDARD)
+
+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-none-eabi-g++)
+# CPPFLAGS = -fno-rtti -fno-exceptions
+CPPFLAGS = 
+
+# Assembler flags.
+#  -Wa,...:    tell GCC to pass this to the assembler.
+#  -ahlns:     create listing
+#  -g$(DEBUGF): have the assembler create line number information
+ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:.S=.lst),--g$(DEBUGF) -Iinclude/ -D__ASSEMBLY__
+
+
+#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
+
+MATH_LIB = #-lm
+
+# CPLUSPLUS_LIB = -lstdc++
+
+
+# Linker flags.
+#  -Wl,...:     tell GCC to pass this to linker.
+#    -Map:      create map file
+#    --cref:    add cross reference to  map file
+LDFLAGS = -nostartfiles -Wl,-Map=$(TARGET).map,--cref
+LDFLAGS += $(NEWLIBLPC) $(MATH_LIB)
+LDFLAGS += -lc -lgcc 
+LDFLAGS += $(CPLUSPLUS_LIB)
+LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
+LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
+#LDFLAGS += --gc-sections
+
+# 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$(IMGTYPE).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
+LPC21ISP_PORT = com1
+LPC21ISP_BAUD = 38400
+LPC21ISP_XTAL = 12000
+LPC21ISP_FLASHFILE = $(TARGET).hex
+# verbose output:
+#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/
+## DIRAVRUTILS = $(DIRAVR)/utils/bin/
+
+# Define programs and commands.
+SHELL = sh
+CC = arm-none-eabi-gcc
+CPP = arm-none-eabi-g++
+OBJCOPY = arm-none-eabi-objcopy
+OBJDUMP = arm-none-eabi-objdump
+SIZE = arm-none-eabi-size
+NM = arm-none-eabi-nm
+REMOVE = rm -f
+COPY = cp
+
+# Define Messages
+# English
+MSG_ERRORS_NONE = Errors: none
+MSG_BEGIN = "-------- begin (mode: $(RUN_MODE)) --------"
+MSG_END = --------  end  --------
+MSG_SIZE_BEFORE = Size before: 
+MSG_SIZE_AFTER = Size after:
+MSG_FLASH = Creating load file for Flash:
+MSG_EXTENDED_LISTING = Creating Extended Listing:
+MSG_SYMBOL_TABLE = Creating Symbol Table:
+MSG_LINKING = Linking:
+MSG_COMPILING = Compiling C:
+MSG_COMPILING_ARM = "Compiling C (ARM-only):"
+MSG_COMPILINGCPP = Compiling C++:
+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)
+COBJARM   = $(SRCARM:.c=.o)
+AOBJARM   = $(ASRCARM:.S=.o)
+CPPOBJ    = $(CPPSRC:.cpp=.o) 
+CPPOBJARM = $(CPPSRCARM:.cpp=.o)
+
+# Define all listing files.
+LST = $(ASRC:.S=.lst) $(ASRCARM:.S=.lst) $(SRC:.c=.lst) $(SRCARM:.c=.lst)
+LST += $(CPPSRC:.cpp=.lst) $(CPPSRCARM:.cpp=.lst)
+
+# Compiler flags to generate dependency files.
+### GENDEPFLAGS = -Wp,-M,-MP,-MT,$(*F).o,-MF,.dep/$(@F).d
+GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d
+
+# Combine all necessary flags and optional flags.
+# Add target processor to flags.
+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
+lss: $(TARGET).lss 
+sym: $(TARGET).sym
+
+# Eye candy.
+begin:
+	@echo
+	@echo $(MSG_BEGIN)
+
+finished:
+	@echo $(MSG_ERRORS_NONE)
+
+end:
+	@echo $(MSG_END)
+	@echo
+
+
+# Display size of file.
+HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex
+ELFSIZE = $(SIZE) -A $(TARGET).elf
+sizebefore:
+	@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
+
+sizeafter:
+	@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
+
+
+# Display compiler version information.
+gccversion : 
+	@$(CC) --version
+
+
+# Program the device.
+# Program the device by using our relais card robot over USB
+ifeq ($(FLASH_TOOL),AT91FLASH)
+program: $(TARGET).$(IMGEXT)
+	ls -l $(TARGET).$(IMGEXT)
+	at91flash $(TARGET).$(IMGEXT)
+else
+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 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).$(IMGEXT)
+	@echo
+	@echo $(MSG_LPC21_RESETREMINDER)
+	$(LPC21ISP) $(LPC21ISP_OPTIONS) $(LPC21ISP_DEBUG) $(LPC21ISP_FLASHFILE) $(LPC21ISP_PORT) $(LPC21ISP_BAUD) $(LPC21ISP_XTAL)
+endif
+endif
+endif
+
+
+# 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.
+# testing: option -C
+%.lss: %.elf
+	@echo
+	@echo $(MSG_EXTENDED_LISTING) $@
+	$(OBJDUMP) -h -S -C $< > $@
+
+
+# Create a symbol table from ELF output file.
+%.sym: %.elf
+	@echo
+	@echo $(MSG_SYMBOL_TABLE) $@
+	$(NM) -n $< > $@
+
+
+# Link: create ELF output file from object files.
+.SECONDARY : $(TARGET).elf
+.PRECIOUS : $(AOBJARM) $(AOBJ) $(COBJARM) $(COBJ) $(CPPOBJ) $(CPPOBJARM)
+%.elf:  $(AOBJARM) $(AOBJ) $(COBJARM) $(COBJ) $(CPPOBJ) $(CPPOBJARM)
+	@echo
+	@echo $(MSG_LINKING) $@
+	$(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 $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@ 
+
+# Compile: create object files from C source files. ARM-only
+$(COBJARM) : %.o : %.c include/compile.h $(USBSTRINGS)
+	@echo
+	@echo $(MSG_COMPILING_ARM) $<
+	$(CC) -c $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@ 
+
+# Compile: create object files from C++ source files. ARM/Thumb
+$(CPPOBJ) : %.o : %.cpp
+	@echo
+	@echo $(MSG_COMPILINGCPP) $<
+	$(CPP) -c $(ALL_CFLAGS) $(CPPFLAGS) $< -o $@ 
+
+# Compile: create object files from C++ source files. ARM-only
+$(CPPOBJARM) : %.o : %.cpp
+	@echo
+	@echo $(MSG_COMPILINGCPP_ARM) $<
+	$(CPP) -c $(ALL_CFLAGS) $(CPPFLAGS) $< -o $@ 
+
+
+# Compile: create assembler files from C source files. ARM/Thumb
+## does not work - TODO - hints welcome
+##$(COBJ) : %.s : %.c
+##	$(CC) $(THUMB) -S $(ALL_CFLAGS) $< -o $@
+
+
+# Assemble: create object files from assembler source files. ARM/Thumb
+$(AOBJ) : %.o : %.S
+	@echo
+	@echo $(MSG_ASSEMBLING) $<
+	$(CC) -c $(ALL_ASFLAGS) $< -o $@
+
+
+# Assemble: create object files from assembler source files. ARM-only
+$(AOBJARM) : %.o : %.S
+	@echo
+	@echo $(MSG_ASSEMBLING_ARM) $<
+	$(CC) -c $(ALL_ASFLAGS) $< -o $@
+
+
+# Target: clean project.
+clean: begin clean_list finished end
+
+
+clean_list :
+	@echo
+	@echo $(MSG_CLEANING)
+	$(REMOVE) $(TARGET).hex
+	$(REMOVE) $(TARGET).bin
+	$(REMOVE) $(TARGET).obj
+	$(REMOVE) $(TARGET).elf
+	$(REMOVE) $(TARGET).map
+	$(REMOVE) $(TARGET).obj
+	$(REMOVE) $(TARGET).a90
+	$(REMOVE) $(TARGET).sym
+	$(REMOVE) $(TARGET).lnk
+	$(REMOVE) $(TARGET).lss
+	$(REMOVE) $(COBJ)
+	$(REMOVE) $(CPPOBJ)
+	$(REMOVE) $(AOBJ)
+	$(REMOVE) $(COBJARM)
+	$(REMOVE) $(CPPOBJARM)
+	$(REMOVE) $(AOBJARM)
+	$(REMOVE) $(LST)
+	$(REMOVE) $(SRC:.c=.s)
+	$(REMOVE) $(SRC:.c=.d)
+	$(REMOVE) $(SRCARM:.c=.s)
+	$(REMOVE) $(SRCARM:.c=.d)
+	$(REMOVE) $(CPPSRC:.cpp=.s) 
+	$(REMOVE) $(CPPSRC:.cpp=.d)
+	$(REMOVE) $(CPPSRCARM:.cpp=.s) 
+	$(REMOVE) $(CPPSRCARM:.cpp=.d)
+	$(REMOVE) .dep/*
+	$(REMOVE) src/picc/usb_strings_dfu.h
+	$(REMOVE) src/dfu/usb_strings_dfu.h
+	$(REMOVE) scripts/usbstring
+
+.PHONY: include/compile.h
+include/compile.h: 
+	scripts/mkcompile_h > $@
+
+.PHONY:
+$(USBSTRINGS): %.h : %.txt ./scripts/usbstring
+	cat $< | ./scripts/usbstring > $@
+
+scripts/usbstring: scripts/usbstring.c
+	gcc $^ -o $@
+
+
+# Include the dependency files.
+-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*)
+
+
+# Listing of phony targets.
+.PHONY : all begin finish end sizebefore sizeafter gccversion \
+build elf hex bin lss sym clean clean_list program
+
Index: firmware/Makefile.local
===================================================================
--- firmware/Makefile.local	(nonexistent)
+++ firmware/Makefile.local	(revision 66923)
@@ -0,0 +1,622 @@
+# Hey Emacs, this is a -*- 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>
+#
+# based on the WinAVR makefile written by Eric B. Weddington, Jörg Wunsch, et al.
+# Released to the Public Domain
+# Please read the make user manual!
+#
+#
+# On command line:
+#
+# make all = Make software.
+#
+# make clean = Clean out built project files.
+#
+# make program = Download the hex file to the device
+#
+# (TODO: make filename.s = Just compile filename.c into the assembler code only)
+#
+# To rebuild project do "make clean" then "make all".
+#
+# Changelog:
+# - 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
+
+LIBRFID_DIR:=../../librfid/
+
+FLASH_TOOL = AT91FLASH
+#FLASH_TOOL = UVISION
+#FLASH_TOOL = OPENOCD
+
+# MCU name and submodel
+MCU      = arm7tdmi
+SUBMDL   = AT91SAM7S64
+#SUBMDL   = AT91SAM7S128
+
+USE_THUMB_MODE = NO
+#USE_THUMB_MODE = YES
+
+## Create ROM-Image (final)
+RUN_MODE:=RUN_FROM_ROM
+## Create RAM-Image (debugging) - not used in this example
+#RUN_MODE=RUN_FROM_RAM
+
+## We want to produce a full-flash image, but including DFU 
+IMGTYPE=-sam7dfu-app
+
+# with / at end
+PATH_TO_LINKSCRIPTS=link/
+
+#### 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_reqa
+
+USBSTRINGS=src/picc/usb_strings_app.h src/pcd/usb_strings_app.h src/simtrace/usb_strings_app.h
+
+# List C source files here. (C dependencies are automatically generated.)
+# use file-extension c for "c-only"-files
+SRC = 
+
+# List C source files here which must be compiled in ARM-Mode.
+# use file-extension c for "c-only"-files
+
+# First, build library ...
+SRCARM  = lib/lib_AT91SAM7.c 
+ifdef DEBUG
+SRCARM += lib/vsprintf.c lib/ctype.c lib/string.c
+endif
+
+# then, OS ...
+SRCARM += src/os/pcd_enumerate.c src/os/fifo.c src/os/dbgu.c \
+	  src/os/led.c src/os/req_ctx.c src/os/trigger.c \
+	  src/os/main.c src/os/syscalls.c src/os/usb_handler.c \
+	  src/os/usb_benchmark.c src/os/tc_cdiv.c src/os/pit.c \
+	  src/os/pwm.c src/os/pio_irq.c src/os/usbcmd_generic.c \
+	  src/os/wdt.c src/os/blinkcode.c src/os/system_irq.c \
+	  src/os/flash.c
+
+ifeq ($(BOARD), PCD)
+# PCD support code
+SRCARM += src/pcd/rc632.c src/pcd/rc632_highlevel.c
+# finally, the actual main application 
+SRCARM += src/pcd/$(TARGET).c 
+endif
+
+ifeq ($(BOARD), PICC)
+# PICC support code
+SRCARM += src/picc/tc_fdt.c src/picc/ssc_picc.c src/picc/adc.c \
+	  src/picc/decoder.c src/picc/decoder_miller.c \
+	  src/picc/load_modulation.c src/picc/tc_cdiv_sync.c \
+	  src/picc/decoder_nrzl.c src/picc/da.c src/picc/pll.c \
+	  src/picc/openpicc.c
+# finally, the actual main application 
+SRCARM += src/picc/$(TARGET).c 
+endif
+
+ifeq ($(BOARD), SIMTRACE)
+SUBMDL   = AT91SAM7S128
+TARGET := main_simtrace
+SRCARM += src/simtrace/iso7816_uart.c src/simtrace/tc_etu.c \
+	  src/simtrace/sim_switch.c src/simtrace/spi_flash.c
+SRCARM += src/simtrace/$(TARGET).c 
+endif
+
+
+# List C++ source files here.
+# use file-extension cpp for C++-files (use extension .cpp)
+CPPSRC = 
+
+# List C++ source files here which must be compiled in ARM-Mode.
+# use file-extension cpp for C++-files (use extension .cpp)
+#CPPSRCARM = $(TARGET).cpp
+CPPSRCARM = 
+
+# List Assembler source files here.
+# Make them always end in a capital .S.  Files ending in a lowercase .s
+# will not be considered source files but generated files (assembler
+# output from the compiler), and will be deleted upon "make clean"!
+# Even though the DOS/Win* filesystem matches both .s and .S the same,
+# it will preserve the spelling of the filenames, and gcc itself does
+# care about how the name is spelled on its command-line.
+ASRC = 
+
+ASRCLIB = lib/changebit.S lib/clearbit.S lib/setbit.S lib/testchangebit.S \
+	  lib/testclearbit.S lib/testsetbit.S 
+ifdef DEBUG
+#ASRCLIB += lib/memcpy.S lib/memset.S lib/lib1funcs.S lib/div64.S
+ASRCLIB += lib/lib1funcs.S lib/div64.S
+endif
+
+# List Assembler source files here which must be assembled in ARM-Mode..
+ASRCARM  = src/start/Cstartup_app.S $(ASRCLIB)
+
+## 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.
+# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
+OPT = 2
+#OPT = 0
+
+# Debugging format.
+# Native formats for AVR-GCC's -g are stabs [default], or dwarf-2.
+# AVR (extended) COFF requires stabs, plus an avr-objcopy run.
+#DEBUGF = stabs
+DEBUGF = dwarf-2
+
+# List any extra directories to look for include files here.
+#     Each directory must be seperated by a space.
+#### FreeRTOS
+EXTRAINCDIRS = $(LIBRFID_DIR)/include/
+
+# 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 = $(LIBRFID_DIR)/src/.libs/
+
+## 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
+# gnu89 - c89 plus GCC extensions
+# c99   - ISO C99 standard (not yet fully implemented)
+# gnu99 - c99 plus GCC extensions
+CSTANDARD = -std=gnu99
+
+# Place -D or -U options for C here
+CDEFS =  -D$(RUN_MODE) -D__MS_types__ -D__LIBRFID__ 
+
+ifdef DEBUG
+CDEFS += -DDEBUG 
+endif
+
+ifdef OLIMEX
+CDEFS += -DOLIMEX
+endif
+
+ifeq ($(BOARD),PICC)
+CDEFS += -DPICC
+CINCS = -Isrc/picc
+endif
+
+ifeq ($(BOARD),PCD)
+CDEFS += -DPCD
+CINCS = -Isrc/pcd
+endif
+
+ifeq ($(BOARD),SIMTRACE)
+CDEFS += -DSIMTRACE
+CINCS = -Isrc/simtrace
+endif
+
+
+# Place -I options here
+CINCS += -Iinclude -Isrc
+
+# Place -D or -U options for ASM here
+ADEFS =  -D$(RUN_MODE)
+
+ifdef VECTOR_LOCATION
+CDEFS += -D$(VECTOR_LOCATION)
+ADEFS += -D$(VECTOR_LOCATION)
+endif
+
+CDEFS += -D__$(SUBMDL)__
+ADEFS += -D__$(SUBMDL)__
+
+
+# Compiler flags.
+#  -g*:          generate debugging information
+#  -O*:          optimization level
+#  -f...:        tuning, see GCC manual and avr-libc documentation
+#  -Wall...:     warning level
+#  -Wa,...:      tell GCC to pass this to the assembler.
+#    -adhlns...: create assembler listing
+#
+# Flags for C and C++ (arm-none-eabi-gcc/arm-none-eabi-g++)
+CFLAGS = -g$(DEBUGF)
+CFLAGS += $(CDEFS) $(CINCS)
+CFLAGS += -O$(OPT)
+CFLAGS += -Wall -Wextra -Wcast-align -Wimplicit -Wunused
+CFLAGS += -Wpointer-arith -Wswitch
+CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow 
+CFLAGS += -Wbad-function-cast -Wsign-compare -Waggregate-return 
+CFLAGS += -Wa,-adhlns=$(subst $(suffix $<),.lst,$<) 
+CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
+CFLAGS += -ffunction-sections -fdata-sections
+
+# flags only for C
+CONLYFLAGS += -Wnested-externs 
+CONLYFLAGS += $(CSTANDARD)
+
+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-none-eabi-g++)
+# CPPFLAGS = -fno-rtti -fno-exceptions
+CPPFLAGS = 
+
+# Assembler flags.
+#  -Wa,...:    tell GCC to pass this to the assembler.
+#  -ahlns:     create listing
+#  -g$(DEBUGF): have the assembler create line number information
+ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:.S=.lst),--g$(DEBUGF) -Iinclude/ -D__ASSEMBLY__
+
+
+#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
+ifeq ($(TARGET),main_librfid)
+EXTRA_LIBS = rfid
+endif
+
+ifeq ($(TARGET),main_reqa)
+EXTRA_LIBS = rfid
+endif
+
+ifeq ($(TARGET),main_presence)
+EXTRA_LIBS = rfid
+endif
+
+ifeq ($(TARGET),main_mifare)
+EXTRA_LIBS = rfid
+endif
+
+#Support for newlibc-lpc (file: libnewlibc-lpc.a)
+#NEWLIBLPC = -lnewlib-lpc
+
+MATH_LIB = #-lm
+
+# CPLUSPLUS_LIB = -lstdc++
+
+
+# Linker flags.
+#  -Wl,...:     tell GCC to pass this to linker.
+#    -Map:      create map file
+#    --cref:    add cross reference to  map file
+LDFLAGS = -nostartfiles -Wl,-Map=$(TARGET).map,--cref
+LDFLAGS += $(NEWLIBLPC) $(MATH_LIB)
+LDFLAGS += -lc -lgcc 
+LDFLAGS += $(CPLUSPLUS_LIB)
+LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
+LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
+LDFLAGS += -Wl,--gc-sections -Wl,--entry=_startup
+LDFLAGS += -lstdc++_nano
+
+# Set Linker-Script Depending On Selected Memory and Controller
+ifeq ($(RUN_MODE),RUN_FROM_RAM)
+LDFLAGS +=-T$(PATH_TO_LINKSCRIPTS)$(SUBMDL)-RAM$(IMGTYPE).ld
+else 
+LDFLAGS +=-T$(PATH_TO_LINKSCRIPTS)$(SUBMDL)-ROM$(IMGTYPE).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
+LPC21ISP_PORT = com1
+LPC21ISP_BAUD = 38400
+LPC21ISP_XTAL = 12000
+LPC21ISP_FLASHFILE = $(TARGET).hex
+# verbose output:
+#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/
+## DIRAVRUTILS = $(DIRAVR)/utils/bin/
+
+# Define programs and commands.
+SHELL = sh
+CC = arm-none-eabi-gcc
+CPP = arm-none-eabi-g++
+OBJCOPY = arm-none-eabi-objcopy
+OBJDUMP = arm-none-eabi-objdump
+SIZE = arm-none-eabi-size
+NM = arm-none-eabi-nm
+REMOVE = rm -f
+COPY = cp
+
+# Define Messages
+# English
+MSG_ERRORS_NONE = Errors: none
+MSG_BEGIN = "-------- begin (mode: $(RUN_MODE)) --------"
+MSG_END = --------  end  --------
+MSG_SIZE_BEFORE = Size before: 
+MSG_SIZE_AFTER = Size after:
+MSG_FLASH = Creating load file for Flash:
+MSG_EXTENDED_LISTING = Creating Extended Listing:
+MSG_SYMBOL_TABLE = Creating Symbol Table:
+MSG_LINKING = Linking:
+MSG_COMPILING = Compiling C:
+MSG_COMPILING_ARM = "Compiling C (ARM-only):"
+MSG_COMPILINGCPP = Compiling C++:
+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)
+COBJARM   = $(SRCARM:.c=.o)
+AOBJARM   = $(ASRCARM:.S=.o)
+CPPOBJ    = $(CPPSRC:.cpp=.o) 
+CPPOBJARM = $(CPPSRCARM:.cpp=.o)
+
+# Define all listing files.
+LST = $(ASRC:.S=.lst) $(ASRCARM:.S=.lst) $(SRC:.c=.lst) $(SRCARM:.c=.lst)
+LST += $(CPPSRC:.cpp=.lst) $(CPPSRCARM:.cpp=.lst)
+
+# Compiler flags to generate dependency files.
+### GENDEPFLAGS = -Wp,-M,-MP,-MT,$(*F).o,-MF,.dep/$(@F).d
+GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d
+
+# Combine all necessary flags and optional flags.
+# Add target processor to flags.
+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
+lss: $(TARGET).lss 
+sym: $(TARGET).sym
+
+# Eye candy.
+begin:
+	@echo
+	@echo $(MSG_BEGIN)
+
+finished:
+	@echo $(MSG_ERRORS_NONE)
+
+end:
+	@echo $(MSG_END)
+	@echo
+
+
+# Display size of file.
+HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex
+ELFSIZE = $(SIZE) -A $(TARGET).elf
+sizebefore:
+	@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
+
+sizeafter:
+	@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
+
+
+# Display compiler version information.
+gccversion : 
+	@$(CC) --version
+
+
+# Program the device.
+# Program the device by using our relais card robot over USB
+ifeq ($(FLASH_TOOL),AT91FLASH)
+program: $(TARGET).$(IMGEXT)
+	ls -l $(TARGET).$(IMGEXT)
+	at91flash $(TARGET).$(IMGEXT)
+else
+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 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).$(IMGEXT)
+	@echo
+	@echo $(MSG_LPC21_RESETREMINDER)
+	$(LPC21ISP) $(LPC21ISP_OPTIONS) $(LPC21ISP_DEBUG) $(LPC21ISP_FLASHFILE) $(LPC21ISP_PORT) $(LPC21ISP_BAUD) $(LPC21ISP_XTAL)
+endif
+endif
+endif
+
+
+# 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.
+# testing: option -C
+%.lss: %.elf
+	@echo
+	@echo $(MSG_EXTENDED_LISTING) $@
+	$(OBJDUMP) -h -S -C $< > $@
+
+
+# Create a symbol table from ELF output file.
+%.sym: %.elf
+	@echo
+	@echo $(MSG_SYMBOL_TABLE) $@
+	$(NM) -n $< > $@
+
+
+# Link: create ELF output file from object files.
+.SECONDARY : $(TARGET).elf
+.PRECIOUS : $(AOBJARM) $(AOBJ) $(COBJARM) $(COBJ) $(CPPOBJ) $(CPPOBJARM)
+%.elf:  $(AOBJARM) $(AOBJ) $(COBJARM) $(COBJ) $(CPPOBJ) $(CPPOBJARM)
+	@echo
+	@echo $(MSG_LINKING) $@
+	$(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 $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@ 
+
+# Compile: create object files from C source files. ARM-only
+$(COBJARM) : %.o : %.c include/compile.h $(USBSTRINGS)
+	@echo
+	@echo $(MSG_COMPILING_ARM) $<
+	$(CC) -c $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@ 
+
+# Compile: create object files from C++ source files. ARM/Thumb
+$(CPPOBJ) : %.o : %.cpp
+	@echo
+	@echo $(MSG_COMPILINGCPP) $<
+	$(CPP) -c $(ALL_CFLAGS) $(CPPFLAGS) $< -o $@ 
+
+# Compile: create object files from C++ source files. ARM-only
+$(CPPOBJARM) : %.o : %.cpp
+	@echo
+	@echo $(MSG_COMPILINGCPP_ARM) $<
+	$(CPP) -c $(ALL_CFLAGS) $(CPPFLAGS) $< -o $@ 
+
+
+# Compile: create assembler files from C source files. ARM/Thumb
+## does not work - TODO - hints welcome
+##$(COBJ) : %.s : %.c
+##	$(CC) $(THUMB) -S $(ALL_CFLAGS) $< -o $@
+
+
+# Assemble: create object files from assembler source files. ARM/Thumb
+$(AOBJ) : %.o : %.S 
+	@echo
+	@echo $(MSG_ASSEMBLING) $<
+	$(CC) -c $(ALL_ASFLAGS) $< -o $@
+
+
+# Assemble: create object files from assembler source files. ARM-only
+$(AOBJARM) : %.o : %.S
+	@echo
+	@echo $(MSG_ASSEMBLING_ARM) $<
+	$(CC) -c $(ALL_ASFLAGS) $< -o $@
+
+
+# Target: clean project.
+clean: begin clean_list finished end
+
+
+clean_list :
+	@echo
+	@echo $(MSG_CLEANING)
+	$(REMOVE) $(TARGET).hex
+	$(REMOVE) $(TARGET).bin
+	$(REMOVE) $(TARGET).obj
+	$(REMOVE) $(TARGET).elf
+	$(REMOVE) $(TARGET).map
+	$(REMOVE) $(TARGET).obj
+	$(REMOVE) $(TARGET).a90
+	$(REMOVE) $(TARGET).sym
+	$(REMOVE) $(TARGET).lnk
+	$(REMOVE) $(TARGET).lss
+	$(REMOVE) $(COBJ)
+	$(REMOVE) $(CPPOBJ)
+	$(REMOVE) $(AOBJ)
+	$(REMOVE) $(COBJARM)
+	$(REMOVE) $(CPPOBJARM)
+	$(REMOVE) $(AOBJARM)
+	$(REMOVE) $(LST)
+	$(REMOVE) $(SRC:.c=.s)
+	$(REMOVE) $(SRC:.c=.d)
+	$(REMOVE) $(SRCARM:.c=.s)
+	$(REMOVE) $(SRCARM:.c=.d)
+	$(REMOVE) $(CPPSRC:.cpp=.s) 
+	$(REMOVE) $(CPPSRC:.cpp=.d)
+	$(REMOVE) $(CPPSRCARM:.cpp=.s) 
+	$(REMOVE) $(CPPSRCARM:.cpp=.d)
+	$(REMOVE) .dep/*
+	$(REMOVE) include/compile.h
+	$(REMOVE) src/picc/usb_strings_app.h
+	$(REMOVE) scripts/usbstring
+
+.PHONY: include/compile.h
+include/compile.h: 
+	scripts/mkcompile_h > $@
+
+$(USBSTRINGS): %.h : %.txt ./scripts/usbstring
+	cat $< | ./scripts/usbstring > $@
+
+scripts/usbstring: scripts/usbstring.c
+	gcc $^ -o $@
+
+# Include the dependency files.
+-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*)
+
+
+# Listing of phony targets.
+.PHONY : all begin finish end sizebefore sizeafter gccversion \
+build elf hex bin lss sym clean clean_list program
+
Index: firmware/include/simtrace_usb.h
===================================================================
--- firmware/include/simtrace_usb.h	(revision 66922)
+++ firmware/include/simtrace_usb.h	(revision 66923)
@@ -3,11 +3,16 @@
 
 //#include <stdint.h>
 
-/* this is kept compatible with OpenPCD protocol */
+/* this WAS compatible with OpenPCD protocol
+ * NOW modified to add apdu_offset and total_length
+ */
 struct simtrace_hdr {
 	u_int8_t cmd;
 	u_int8_t flags;
 	u_int8_t res[2];
+	u_int16_t seq_num;
+	u_int16_t offset;
+	u_int16_t tot_len;
 	u_int8_t data[0];
 } __attribute__ ((packed));
 
Index: firmware/link/AT91SAM7S128-ROM-sam7dfu-app.ld
===================================================================
--- firmware/link/AT91SAM7S128-ROM-sam7dfu-app.ld	(revision 66922)
+++ firmware/link/AT91SAM7S128-ROM-sam7dfu-app.ld	(revision 66923)
@@ -59,6 +59,12 @@
 
 	PROVIDE (end = .);
 
+/DISCARD/ :
+{
+    *(.ARM.exidx*)
+    *(.gnu.linkonce.armexidx.*)
+}
+
   /* Stabs debugging sections.  */
   .stab          0 : { *(.stab) }
   .stabstr       0 : { *(.stabstr) }
Index: firmware/make.sh
===================================================================
--- firmware/make.sh	(nonexistent)
+++ firmware/make.sh	(revision 66923)
@@ -0,0 +1,7 @@
+#! /bin/sh
+rm dfu.* main_simtrace.*
+make clean
+make -f Makefile.dfu.local BOARD=SIMTRACE
+make -f Makefile.local DEBUG=1 BOARD=SIMTRACE TARGET=main_simtrace
+cat dfu.bin  main_simtrace.bin > main_simtrace.samba
+
Index: firmware/src/os/dbgu.c
===================================================================
--- firmware/src/os/dbgu.c	(revision 66922)
+++ firmware/src/os/dbgu.c	(revision 66923)
@@ -45,18 +45,16 @@
 #include <asm/system.h>
 #include <compile.h>
 
-//#define DEBUG_UNBUFFERED
+// IN case we find that while (); is non interruptible, we may need to uncommend this line
+// #define ALLOW_INTERRUPT_LOOP asm("nop;nop;nop;nop;nop;nop;nop;nop;")
 
-#define USART_SYS_LEVEL 4
+#define ALLOW_INTERRUPT_LOOP
+
+// DEBUG_BUFFER_SIZE MUST BE A POWER OF 2
+#define DEBUG_BUFFER_SIZE     (1 << 10)
+#define DEBUG_BUFFER_MASK     (DEBUG_BUFFER_SIZE - 1)
+
 /*---------------------------- Global Variable ------------------------------*/
-//*--------------------------1--------------------------------------------------
-//* \fn    AT91F_DBGU_Printk
-//* \brief This function is used to send a string through the DBGU channel
-//*----------------------------------------------------------------------------
-void AT91F_DBGU_Ready(void)
-{
-	while (!(AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXEMPTY)) ;
-}
 
 //*----------------------------------------------------------------------------
 //* Function Name       : Send_reset
@@ -69,7 +67,7 @@
 	// Acknoledge the interrupt
 	// Mark the End of Interrupt on the AIC
 	AT91C_BASE_AIC->AIC_EOICR = 0;
-	AT91F_DBGU_Ready();
+	while (!(AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXEMPTY)) ;
 	// Jump in reset
 	pfct();
 }
@@ -92,26 +90,26 @@
 		break;
 	case '1':		//* info
 		udp_pullup_off();
-		AT91F_DBGU_Printk("Set Pull up\n\r");
+		AT91F_DBGU_Frame("Set Pull up\n\r");
 		// Reset Application
 		Send_reset();
 		break;
 	case '2':
-		AT91F_DBGU_Printk("Toggling LED 1\n\r");
+		AT91F_DBGU_Frame("Toggling LED 1\n\r");
 		led_toggle(1);
 		break;
 	case '3':
-		AT91F_DBGU_Printk("Toggling LED 2\n\r");
+		AT91F_DBGU_Frame("Toggling LED 2\n\r");
 		led_toggle(2);
 		break;
 	case '9':
-		AT91F_DBGU_Printk("Resetting SAM7\n\r");
+		AT91F_DBGU_Frame("Resetting SAM7\n\r");
 		AT91F_RSTSoftReset(AT91C_BASE_RSTC, AT91C_RSTC_PROCRST|
 				   AT91C_RSTC_PERRST|AT91C_RSTC_EXTRST);
 		break;
 	default:
 		if (_main_dbgu(value) < 0)
-			AT91F_DBGU_Printk("\n\r");
+			AT91F_DBGU_Frame("\n\r");
 		break;
 	}			// end switch
 }
@@ -148,17 +146,17 @@
 	//* open interrupt
 	sysirq_register(AT91SAM7_SYSIRQ_DBGU, &DBGU_irq_handler);
 
-	AT91F_DBGU_Printk("\n\r");
-	AT91F_DBGU_Printk("(C) 2006-2011 by Harald Welte <hwelte@hmw-consulting.de>\n\r"
+	DEBUGP("\n\r");
+	DEBUGP("(C) 2006-2011 by Harald Welte <hwelte@hmw-consulting.de>\n\r"
 			  "This software is FREE SOFTWARE licensed under GNU GPL\n\r");
-	AT91F_DBGU_Printk("Version " COMPILE_SVNREV
+	DEBUGP("Version " COMPILE_SVNREV
 			  " compiled " COMPILE_DATE
 			  " by " COMPILE_BY "\n\r\n\r");
-	AT91F_DBGU_Printk("\n\rDEBUG Interface:\n\r"
+	DEBUGP("\n\rDEBUG Interface:\n\r"
 			  "0) Set Pull-up 1) Clear Pull-up 2) Toggle LED1 3) "
 			  "Toggle LED2\r\n9) Reset\n\r");
 
-	debugp("RSTC_SR=0x%08x\n", rst_status);
+	DEBUGP("RSTC_SR=0x%08x\n\r", rst_status); 
 }
 
 /*
@@ -173,31 +171,36 @@
 }
 
 //*----------------------------------------------------------------------------
-//* \fn    AT91F_DBGU_Printk
+//* \fn    AT91F_DBGU_Frame
 //* \brief This function is used to send a string through the DBGU channel (Very low level debugging)
 //*----------------------------------------------------------------------------
-void AT91F_DBGU_Printk(char *buffer)
-{
-	while (*buffer != '\0') {
-		while (!AT91F_US_TxReady((AT91PS_USART) AT91C_BASE_DBGU)) ;
-		AT91F_US_PutChar((AT91PS_USART) AT91C_BASE_DBGU, *buffer++);
-	}
-}
-
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_DBGU_Frame
-//* \brief This function is used to send a string through the DBGU channel
-//*----------------------------------------------------------------------------
 void AT91F_DBGU_Frame(char *buffer)
 {
-	unsigned char len;
-
-	for (len = 0; buffer[len] != '\0'; len++) {
+	unsigned long intcFlags;
+	unsigned int len = 0;
+	while (buffer[len++]) ALLOW_INTERRUPT_LOOP;
+	len--;
+	local_irq_save(intcFlags);
+	AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer
+	if (AT91C_BASE_DBGU->DBGU_TNCR) {
+		AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+		local_irq_restore(intcFlags);
+		while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) ALLOW_INTERRUPT_LOOP;
+		local_irq_save(intcFlags);
+		AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer    
+		AT91C_BASE_DBGU->DBGU_TPR = (unsigned)buffer;
+		AT91C_BASE_DBGU->DBGU_TCR = len;
+	} else if (AT91C_BASE_DBGU->DBGU_TCR) {
+		AT91C_BASE_DBGU->DBGU_TNPR = (unsigned)buffer;
+		AT91C_BASE_DBGU->DBGU_TNCR = len;
+	} else {
+		AT91C_BASE_DBGU->DBGU_TPR = (unsigned)buffer;
+		AT91C_BASE_DBGU->DBGU_TCR = len;
 	}
-
-	AT91F_US_SendFrame((AT91PS_USART) AT91C_BASE_DBGU, 
-			   (unsigned char *)buffer, len, 0, 0);
-
+	AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+	local_irq_restore(intcFlags);
+	// Return ONLY after we complete Transfer
+	while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) ALLOW_INTERRUPT_LOOP;
 }
 
 //*----------------------------------------------------------------------------
@@ -239,105 +242,175 @@
 }
 
 struct dbgu {
-	char buf[4096];
-	char *next_inbyte;
-	char *next_outbyte;
+	char buf[DEBUG_BUFFER_SIZE];
+  // Since debugp appears to require to be re-entrant, we need a bit more state variables
+  
+  /* States: 
+     INUSE: when a position is filled and should be written out by dbgu_rb_flush
+     AVAIL: when a position is available to receive from dbgu_rb_append
+     in_head: Position where incoming *append* characters have finished copy to buffer
+     in_tail: Position where NEW incoming *append* characters should COPY to
+     out_head: Position where the LAST *possibly incomplete* dbgu_rb_flush started from
+     out_tail: Position where the NEXT dbug_rb_flush should start from
+
+     The position in the RING should be in this order:
+     --> out_tail --> in_head --> in_tail --> out_head -->     
+  */
+	volatile unsigned int in_head, in_tail, out_head, out_tail;
+
+  // flush_stack is to indicate rb_flush is being executed, NOT to execute again
+  // append_stack is to indicate the re-entrack stack order of the current dbgu_append call
+	volatile unsigned int flush_stack, append_stack;
 };
+
 static struct dbgu dbgu;
 
 void dbgu_rb_init(void)
 {
-	memset(dbgu.buf, 0, sizeof(dbgu.buf));
-	dbgu.next_inbyte = &dbgu.buf[0];
-	dbgu.next_outbyte = &dbgu.buf[0];
+	dbgu.in_head = dbgu.in_tail = dbgu.out_head = dbgu.out_tail = 0;
+	dbgu.flush_stack = dbgu.append_stack = 0;
 }
 
-/* pull one char out of debug ring buffer */
-static int dbgu_rb_pull(char *ret)
-{
-	unsigned long flags;
+/* flush pending data from debug ring buffer to serial port */
 
-	local_irq_save(flags);
-
-	if (dbgu.next_outbyte == dbgu.next_inbyte) {
-		local_irq_restore(flags);
-		return -1;
-	}
-
-	*ret = *dbgu.next_outbyte;
-
-	dbgu.next_outbyte++;
-	if (dbgu.next_outbyte == &dbgu.buf[0]+sizeof(dbgu.buf)) {
-		//AT91F_DBGU_Printk("WRAP DURING PULL\r\n");
-		dbgu.next_outbyte = &dbgu.buf[0];
-	} else if (dbgu.next_outbyte > &dbgu.buf[0]+sizeof(dbgu.buf)) {
-		//AT91F_DBGU_Printk("OUTBYTE > END_OF_BUF!!\r\n");
-		dbgu.next_outbyte -= sizeof(dbgu.buf);
-	}
-
-	local_irq_restore(flags);
-
-	return 0;
-}
-
-static void __rb_flush(void)
-{
-	char ch;
-	while (dbgu_rb_pull(&ch) >= 0) {
-		while (!AT91F_US_TxReady((AT91PS_USART) AT91C_BASE_DBGU)) ;
-		AT91F_US_PutChar((AT91PS_USART) AT91C_BASE_DBGU, ch);
-	}
-}
-
-/* flush pending data from debug ring buffer to serial port */
 void dbgu_rb_flush(void)
 {
-	__rb_flush();
-}
+	unsigned long intcFlags;
+	unsigned int flush_stack, start, len;
+	if (dbgu.in_head == dbgu.out_tail)
+		return;
 
-static void __dbgu_rb_append(char *data, int len)
-{
-	char *pos = dbgu.next_inbyte;
-
-	dbgu.next_inbyte += len;
-	if (dbgu.next_inbyte >= &dbgu.buf[0]+sizeof(dbgu.buf)) {
-		AT91F_DBGU_Printk("WRAP DURING APPEND\r\n");
-		dbgu.next_inbyte -= sizeof(dbgu.buf);
+	// Transmit can ONLY be disabled when Interrupt is disabled.  We don't want to be interrupted while  Transmit is disabled.
+	local_irq_save(intcFlags);
+	flush_stack = dbgu.flush_stack;
+	dbgu.flush_stack = 1;
+	if (flush_stack) {
+		local_irq_restore(intcFlags);
+		return;
 	}
-
-	memcpy(pos, data, len);
+	AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer
+	start = (unsigned)dbgu.buf + dbgu.out_tail;
+	len = (dbgu.in_head - dbgu.out_tail) & DEBUG_BUFFER_MASK;
+	if (dbgu.in_head > dbgu.out_tail || dbgu.in_head == 0) { // Just 1 fragmentf
+		if (AT91C_BASE_DBGU->DBGU_TNCR) {
+			if (AT91C_BASE_DBGU->DBGU_TNPR + AT91C_BASE_DBGU->DBGU_TNCR == start) {
+				AT91C_BASE_DBGU->DBGU_TNCR += len;
+			} else {
+				AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+				local_irq_restore(intcFlags);
+				while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) ALLOW_INTERRUPT_LOOP;
+				dbgu.out_head = dbgu.out_tail; // in case we are interrupted, they may need more space
+				/* Since the ONLY place where Transmit is started is dbgu_rb_flush and AT91F_DBGU_Frame and:
+				 * 1) AT91F_DBGU_Frame always leave the dbgu in a TX completed state
+				 * 2) dbgu_rb is non-reentrant by safeguard at the beginning of this routing
+				 * We can assume that after this INTERRUPTIBLE while loop, there are NO data to be transmitted
+				 */
+				local_irq_save(intcFlags);
+				AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer
+				AT91C_BASE_DBGU->DBGU_TPR = start;
+				AT91C_BASE_DBGU->DBGU_TCR = len;
+			}
+		} else if (AT91C_BASE_DBGU->DBGU_TCR) {
+		  if (AT91C_BASE_DBGU->DBGU_TPR + AT91C_BASE_DBGU->DBGU_TCR == start) {
+		    dbgu.out_head = AT91C_BASE_DBGU->DBGU_TPR - (unsigned)dbgu.buf;
+		    AT91C_BASE_DBGU->DBGU_TCR += len;
+		  } else {
+		    AT91C_BASE_DBGU->DBGU_TNPR = start;
+		    AT91C_BASE_DBGU->DBGU_TNCR = len;
+		  }
+		} else {
+		  AT91C_BASE_DBGU->DBGU_TPR = start;
+		  AT91C_BASE_DBGU->DBGU_TCR = len;
+		  dbgu.out_head = dbgu.out_tail;
+		}
+	} else { // 2 fragments
+		if ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) {
+			AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+			local_irq_restore(intcFlags);
+			while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) ALLOW_INTERRUPT_LOOP;
+			dbgu.out_head = dbgu.out_tail; // in case we are interrupted, they may need more space
+			local_irq_save(intcFlags);
+			AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer
+		}
+		AT91C_BASE_DBGU->DBGU_TPR = start;
+		AT91C_BASE_DBGU->DBGU_TCR = DEBUG_BUFFER_SIZE - dbgu.out_tail;
+		AT91C_BASE_DBGU->DBGU_TNPR = (unsigned)dbgu.buf;
+		AT91C_BASE_DBGU->DBGU_TNCR = dbgu.in_head;
+		dbgu.out_head = dbgu.out_tail;
+	}
+	AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+	dbgu.out_tail = dbgu.in_head;
+	dbgu.flush_stack = 0;
+	local_irq_restore(intcFlags);
 }
 
 void dbgu_rb_append(char *data, int len)
 {
-	unsigned long flags;
-	int bytes_left;
-	char *data_cur;
+	/* Rules:
+	 * 1) ONLY the LOWEST order of dbgu_rb_append CAN update in_head;
+	 * 2) WHEN updateing in_head, always set it to the current in_tail (since all higher order dbgu_rb_append have completed
+	 * 3) ONLY the LOWEST order of dbgu_rb_append MAY call dbgu_rb_flush
+	 */
+	unsigned long intcFlags;
+	unsigned int append_stack, avail, local_head;
+	if (len <= 0)
+		return;
 	
-	local_irq_save(flags);
-
-	bytes_left = &dbgu.buf[0]+sizeof(dbgu.buf)-dbgu.next_inbyte;
-	data_cur = data;
-
-	if (len > bytes_left) {
-		AT91F_DBGU_Printk("LEN > BYTES_LEFT\r\n");
-		__rb_flush();
-		__dbgu_rb_append(data_cur, bytes_left);
-		len -= bytes_left;
-		data_cur += bytes_left;
+	local_irq_save(intcFlags);
+	append_stack = dbgu.append_stack++;
+	if (AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE)
+		dbgu.out_head = dbgu.out_tail;
+	avail = (dbgu.out_head - 1 - dbgu.in_tail) & DEBUG_BUFFER_MASK;
+	local_head = (unsigned)len;
+	if (local_head > avail) {
+		local_irq_restore(intcFlags);
+		while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0);
+		local_irq_save(intcFlags);
+		while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0);
+		dbgu.out_head = dbgu.out_tail;
+		avail = (dbgu.out_head - 1 - dbgu.in_tail) & DEBUG_BUFFER_MASK;
+		if (local_head > avail) {
+			local_head -= avail;
+			AT91C_BASE_DBGU->DBGU_TPR = (unsigned)data;
+			AT91C_BASE_DBGU->DBGU_TCR = local_head;
+			data += local_head;
+			len = avail;
+		}
 	}
-	__dbgu_rb_append(data_cur, len);
-
-	local_irq_restore(flags);
+	local_head = dbgu.in_tail;
+	dbgu.in_tail += len;
+	dbgu.in_tail &= DEBUG_BUFFER_MASK;
+	local_irq_restore(intcFlags);
+	if (dbgu.out_head <= local_head) {
+		// We may have to wrap around: out_head won't change because NO call to flush will be made YET
+		avail = DEBUG_BUFFER_SIZE - local_head;
+		if (avail >= (unsigned)len) {
+			memcpy(dbgu.buf + local_head, data, (size_t)len);
+		} else {
+			memcpy(dbgu.buf + local_head, data, (size_t)avail);
+			memcpy(dbgu.buf, data + avail, (size_t)(len - avail));
+		}
+	} else {
+		memcpy(dbgu.buf + local_head, data, len);
+	}
+	local_irq_save(intcFlags);
+	dbgu.append_stack--;
+	if (!append_stack)
+		dbgu.in_head = dbgu.in_tail;
+	local_irq_restore(intcFlags);
+	if (!append_stack)
+		dbgu_rb_flush();
 }
 
 static char dbg_buf[256];
+static int line_num = 0;
 void debugp(const char *format, ...)
 {
 	va_list ap;
 
 	va_start(ap, format);
-	vsnprintf(dbg_buf, sizeof(dbg_buf)-1, format, ap);
+	sprintf(dbg_buf, "[%06X] ", line_num++);
+	vsnprintf(dbg_buf + 9, sizeof(dbg_buf)-10, format, ap);
 	va_end(ap);
 
 	dbg_buf[sizeof(dbg_buf)-1] = '\0';			
Index: firmware/src/os/dbgu.h
===================================================================
--- firmware/src/os/dbgu.h	(revision 66922)
+++ firmware/src/os/dbgu.h	(revision 66923)
@@ -17,15 +17,13 @@
 
 #define AT91C_DBGU_BAUD 115200
 
-//#define DEBUGP(x)	AT91F_DBGU_Printk(x)
-
 //* ----------------------- External Function Prototype -----------------------
 
 extern const char *hexdump(const void *data, unsigned int len);
 void AT91F_DBGU_Init(void);
 void AT91F_DBGU_Fini(void);
-void AT91F_DBGU_Printk(	char *buffer);
-void AT91F_DBGU_Frame(	char *buffer);
+void AT91F_DBGU_Frame(char *buffer);
+inline void AT91F_DBGU_Printk(char *buffer) { AT91F_DBGU_Frame(buffer); }
 int AT91F_DBGU_Get( char *val);
 void dbgu_rb_flush(void);
 #ifndef __WinARM__
Index: firmware/src/os/main.c
===================================================================
--- firmware/src/os/main.c	(revision 66922)
+++ firmware/src/os/main.c	(revision 66923)
@@ -43,8 +43,9 @@
 
 int main(void)
 {
+
 	/* initialize LED and debug unit */
-	led_init();
+  	led_init();
 	sysirq_init();
 	AT91F_DBGU_Init();
 
@@ -77,7 +78,6 @@
 	while (1) {
 		/* Call application specific main idle function */
 		_main_func();
-		dbgu_rb_flush();
 		
 		/* restart watchdog timer */
 		wdt_restart();
Index: firmware/src/os/pcd_enumerate.c
===================================================================
--- firmware/src/os/pcd_enumerate.c	(revision 66922)
+++ firmware/src/os/pcd_enumerate.c	(revision 66923)
@@ -99,8 +99,10 @@
 };
 
 static const struct epstate epstate[] = {
-	[0] =	{ .state_busy = RCTX_STATE_INVALID },
-	[1] =	{ .state_busy = RCTX_STATE_INVALID },
+ 	[0] =	{ .state_busy = RCTX_STATE_UDP_EP0_BUSY,
+		  .state_pending = RCTX_STATE_UDP_EP0_PENDING },
+	[1] =	{ .state_busy = RCTX_STATE_UDP_EP1_BUSY,
+		  .state_pending = RCTX_STATE_UDP_EP1_PENDING },
 	[2] =	{ .state_busy = RCTX_STATE_UDP_EP2_BUSY,
 		  .state_pending = RCTX_STATE_UDP_EP2_PENDING },
 	[3] =	{ .state_busy = RCTX_STATE_UDP_EP3_BUSY,
@@ -112,8 +114,6 @@
 	AT91PS_UDP pUDP = upcd.pUdp;
 	struct req_ctx *rctx;
 
-	//pUDP->UDP_CSR[ep] = AT91C_UDP_EPEDS;
-
 	atomic_set(&upcd.ep[ep].pkts_in_transit, 0);
 
 	/* free all currently transmitting contexts */
@@ -125,6 +125,7 @@
 
 	pUDP->UDP_RSTEP |= (1 << ep);
 	pUDP->UDP_RSTEP &= ~(1 << ep);
+	pUDP->UDP_CSR[ep] = AT91C_UDP_EPEDS;
 
 	upcd.ep[ep].incomplete.rctx = NULL;
 }
@@ -137,7 +138,7 @@
 	pUDP->UDP_IER = AT91C_UDP_EPINT1;
 }
 
-static int __udp_refill_ep(int ep)
+int udp_refill_ep(int ep)
 {
 	u_int16_t i;
 	AT91PS_UDP pUDP = upcd.pUdp;
@@ -149,13 +150,17 @@
 	if (!upcd.cur_config) {
 		return -ENXIO;
 	}
-	
+
 	/* If there are already two packets in transit, the DPR of
 	 * the SAM7 UDC doesn't have space for more data */
+	// if (atomic_read(&upcd.ep[ep].pkts_in_transit) == 2 || (pUDP->UDP_CSR[ep] & AT91C_UDP_TXPKTRDY)) {
+	// if (pUDP->UDP_CSR[ep] & AT91C_UDP_TXPKTRDY) {
 	if (atomic_read(&upcd.ep[ep].pkts_in_transit) == 2) {
 		return -EBUSY;
 	}
 
+	pUDP->UDP_IDR |= 1 << ep;
+
 	/* If we have an incompletely-transmitted req_ctx (>EP size),
 	 * we need to transmit the rest and finish the transaction */
 	if (upcd.ep[ep].incomplete.rctx) {
@@ -165,66 +170,55 @@
 		/* get pending rctx and start transmitting from zero */
 		rctx = req_ctx_find_get(0, epstate[ep].state_pending, 
 					epstate[ep].state_busy);
-		if (!rctx)
+		if (!rctx) {
+			pUDP->UDP_IER |= 1 << ep;
 			return 0;
+		}
+		if (rctx->tot_len == 0) {
+			pUDP->UDP_IER |= 1 << ep;
+			req_ctx_put(rctx);
+			return 0;
+		}
+
+		/*		DEBUGPCR("USBT(D=%08X, S=%02X%02X, L=%04u, P=%02u) H4/T4: %02X %02X %02X %02X / %02X %02X %02X %02X",
+			 rctx->data, rctx->data[5], rctx->data[4], rctx->tot_len, req_ctx_count(epstate[ep].state_pending),
+			 rctx->data[10], rctx->data[11], rctx->data[12], rctx->data[13],
+			 rctx->data[rctx->tot_len - 4], rctx->data[rctx->tot_len - 3], 
+			 rctx->data[rctx->tot_len - 2], rctx->data[rctx->tot_len - 1]);
+		*/
 		start = 0;
 
 		upcd.ep[ep].incomplete.bytes_sent = 0;
 	}
 
-	if (rctx->tot_len - start <= AT91C_EP_IN_SIZE)
+/*	pUDP->UDP_IER |= 1 << ep;
+	req_ctx_put(rctx);
+	return 0; */
+
+	if (rctx->tot_len - start <= AT91C_EP_IN_SIZE) {
 		end = rctx->tot_len;
-	else
+	} else {
 		end = start + AT91C_EP_IN_SIZE;
+	}
 
 	/* fill FIFO/DPR */
-	DEBUGII("RCTX_tx(ep=%u,ctx=%u):%u ", ep, req_ctx_num(rctx),
-		end - start);
 	for (i = start; i < end; i++) 
 		pUDP->UDP_FDR[ep] = rctx->data[i];
 
-	if (atomic_inc_return(&upcd.ep[ep].pkts_in_transit) == 1) {
-		/* not been transmitting before, start transmit */
+	if (atomic_inc_return(&upcd.ep[ep].pkts_in_transit) == 1)
 		pUDP->UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
-	}
-
-	if ((end - start < AT91C_EP_OUT_SIZE) ||
-	    (((end - start) == 0) && end && (rctx->tot_len % AT91C_EP_OUT_SIZE) == 0)) {
-		/* CASE 1: return context to pool, if
-		 * - packet transfer < AT91C_EP_OUT_SIZE
-		 * - after ZLP of transfer == AT91C_EP_OUT_SIZE
-		 * - after ZLP of transfer % AT91C_EP_OUT_SIZE == 0
-		 * - after last packet of transfer % AT91C_EP_OUT_SIZE != 0
-		 */
-		DEBUGII("RCTX(ep=%u,ctx=%u)_tx_done ", ep, req_ctx_num(rctx));
+	
+	if (end == rctx->tot_len) {
 		upcd.ep[ep].incomplete.rctx = NULL;
 		req_ctx_put(rctx);
 	} else {
-		/* CASE 2: mark transfer as incomplete, if
-		 * - after data of transfer == AT91C_EP_OUT_SIZE
-		 * - after data of transfer > AT91C_EP_OUT_SIZE
-		 * - after last packet of transfer % AT91C_EP_OUT_SIZE == 0
-		 */
 		upcd.ep[ep].incomplete.rctx = rctx;
 		upcd.ep[ep].incomplete.bytes_sent += end - start;
-		DEBUGII("RCTX(ep=%u)_tx_cont ", ep);
 	}
-
+	pUDP->UDP_IER |= 1 << ep;
 	return 1;
 }
 
-int udp_refill_ep(int ep)
-{
-	unsigned long flags;
-	int ret;
-
-	local_irq_save(flags);
-	ret = __udp_refill_ep(ep);
-	local_irq_restore(flags);
-
-	return ret;
-}
-
 static void udp_irq(void)
 {
 	u_int32_t csr;
@@ -238,19 +232,18 @@
 		DEBUGI("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;
+		reset_ep(0);
+		reset_ep(1);
+		reset_ep(2);
+		reset_ep(3);
+
 		/* Enable the function */
 		pUDP->UDP_FADDR = AT91C_UDP_FEN;
+
 		/* Configure endpoint 0 */
 		pUDP->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
 		upcd.cur_config = 0;
 		upcd.state = USB_STATE_DEFAULT;
-
-		reset_ep(1);
-		reset_ep(2);
-		reset_ep(3);
 		
 #ifdef CONFIG_DFU
 		if (*dfu->dfu_state == DFU_STATE_appDETACH) {
@@ -359,7 +352,7 @@
 			if (atomic_dec_return(&upcd.ep[2].pkts_in_transit) == 1)
 				pUDP->UDP_CSR[2] |= AT91C_UDP_TXPKTRDY;
 
-			__udp_refill_ep(2);
+			udp_refill_ep(2);
 		}
 	}
 	if (isr & AT91C_UDP_EPINT3) {
@@ -374,7 +367,7 @@
 			if (atomic_dec_return(&upcd.ep[3].pkts_in_transit) == 1)
 				pUDP->UDP_CSR[3] |= AT91C_UDP_TXPKTRDY;
 
-			__udp_refill_ep(3);
+			udp_refill_ep(3);
 		}
 	}
 	if (isr & AT91C_UDP_RXSUSP) {
Index: firmware/src/os/req_ctx.c
===================================================================
--- firmware/src/os/req_ctx.c	(revision 66922)
+++ firmware/src/os/req_ctx.c	(revision 66923)
@@ -29,77 +29,184 @@
 /* FIXME: locking, FIFO order processing */
 
 #if defined(__AT91SAM7S64__) || defined(RUN_FROM_RAM)
-#define NUM_RCTX_SMALL 16
-#define NUM_RCTX_LARGE 1
+#define NUM_RCTX_SMALL 0
+#define NUM_RCTX_LARGE 8
 #else
-#define NUM_RCTX_SMALL 8
-#define NUM_RCTX_LARGE 4
+#define NUM_RCTX_SMALL 0
+#define NUM_RCTX_LARGE 20
 #endif
 
 #define NUM_REQ_CTX	(NUM_RCTX_SMALL+NUM_RCTX_LARGE)
 
 static u_int8_t rctx_data[NUM_RCTX_SMALL][RCTX_SIZE_SMALL];
-static u_int8_t rctx_data_large[NUM_RCTX_LARGE][RCTX_SIZE_LARGE];
+static u_int8_t rctx_data_large[NUM_RCTX_LARGE][RCTX_SIZE_LARGE], rctx_data_TEST[RCTX_SIZE_LARGE];
 
 static struct req_ctx req_ctx[NUM_REQ_CTX];
 
+static struct req_ctx *req_ctx_queues[RCTX_STATE_COUNT], *req_ctx_tails[RCTX_STATE_COUNT];
+static unsigned req_counts[RCTX_STATE_COUNT];
+
 struct req_ctx __ramfunc *req_ctx_find_get(int large,
 				 unsigned long old_state, 
 				 unsigned long new_state)
 {
+        struct req_ctx *toReturn;
 	unsigned long flags;
-	u_int8_t i;
-	
-	if (large)
-		i = NUM_RCTX_SMALL;
-	else
-		i = 0;
 
-	for (; i < NUM_REQ_CTX; i++) {
-		local_irq_save(flags);
-		if (req_ctx[i].state == old_state) {
-			req_ctx[i].state = new_state;
-			local_irq_restore(flags);
-			return &req_ctx[i];
-		}
-		local_irq_restore(flags);
+	if (old_state >= RCTX_STATE_COUNT || new_state >= RCTX_STATE_COUNT) {
+		DEBUGPCR("Invalid parameters for req_ctx_find_get");
+		return NULL;
 	}
-
-	return NULL;
+	local_irq_save(flags);
+	toReturn = req_ctx_queues[old_state];
+	if (toReturn) {
+		if ((req_ctx_queues[old_state] = toReturn->next))
+			toReturn->next->prev = NULL;
+		else
+			req_ctx_tails[old_state] = NULL;
+		req_counts[old_state]--;
+		if ((toReturn->prev = req_ctx_tails[new_state]))
+	  		toReturn->prev->next = toReturn;
+		else
+			req_ctx_queues[new_state] = toReturn;
+		req_ctx_tails[new_state] = toReturn;
+		toReturn->state = new_state;
+		toReturn->next = NULL;
+		req_counts[new_state]++;
+	}
+	local_irq_restore(flags);
+	return toReturn;
 }
 
 u_int8_t req_ctx_num(struct req_ctx *ctx)
 {
-	return ((char *)ctx - (char *)&req_ctx[0])/sizeof(*ctx);
+	return ctx - req_ctx;
 }
 
 void req_ctx_set_state(struct req_ctx *ctx, unsigned long new_state)
 {
 	unsigned long flags;
+	unsigned old_state;
 
-	/* FIXME: do we need this kind of locking, we're UP! */
+	if (new_state >= RCTX_STATE_COUNT) {
+		DEBUGPCR("Invalid new_state for req_ctx_set_state");
+		return;
+	}
 	local_irq_save(flags);
+	old_state = ctx->state;
+	if (ctx->prev)
+		ctx->prev->next = ctx->next;
+	else 
+		req_ctx_queues[old_state] = ctx->next;
+	if (ctx->next)
+		ctx->next->prev = ctx->prev;
+	else
+		req_ctx_tails[old_state] = ctx->prev;
+	req_counts[old_state]--;
+	if ((ctx->prev = req_ctx_tails[new_state]))
+		ctx->prev->next = ctx;
+	else
+		req_ctx_queues[new_state] = ctx;
+	req_ctx_tails[new_state] = ctx;
 	ctx->state = new_state;
+	ctx->next = NULL;
+	req_counts[new_state]++;
 	local_irq_restore(flags);
 }
 
+#ifdef DEBUG_REQCTX
+void req_print(int state) {
+	int count = 0;
+	struct req_ctx *ctx, *last = NULL;
+	DEBUGP("State [%02i] start <==> ", state);
+	ctx = req_ctx_queues[state];
+	while (ctx) {
+		if (last != ctx->prev)
+			DEBUGP("*INV_PREV* ");
+		DEBUGP("%08X => ", ctx);
+		last = ctx;
+		ctx = ctx->next;
+		count++;
+		if (count > NUM_REQ_CTX) {
+		  DEBUGP("*WILD POINTER* => ");
+		  break;
+		}
+	}
+	DEBUGPCR("NULL");
+	if (!req_ctx_queues[state] && req_ctx_tails[state]) {
+		DEBUGPCR("NULL head, NON-NULL tail");
+	}
+	if (last != req_ctx_tails[state]) {
+		DEBUGPCR("Tail does not match last element");
+	}
+}
+#endif
+
 void req_ctx_put(struct req_ctx *ctx)
 {
-	req_ctx_set_state(ctx, RCTX_STATE_FREE);
+        unsigned long intcFlags;
+	unsigned old_state;
+	
+	local_irq_save(intcFlags);
+	old_state = ctx->state;
+	if (ctx->prev)
+		ctx->prev->next = ctx->next;
+	else 
+		req_ctx_queues[old_state] = ctx->next;
+	if (ctx->next)
+		ctx->next->prev = ctx->prev;
+	else
+		req_ctx_tails[old_state] = ctx->prev;
+	req_counts[old_state]--;
+	if ((ctx->prev = req_ctx_tails[RCTX_STATE_FREE]))
+		ctx->prev->next = ctx;
+	else
+		req_ctx_queues[RCTX_STATE_FREE] = ctx;
+	req_ctx_tails[RCTX_STATE_FREE] = ctx;
+	ctx->state = RCTX_STATE_FREE;
+	ctx->next = NULL;
+	req_counts[RCTX_STATE_FREE]++;
+	local_irq_restore(intcFlags);
 }
 
+unsigned req_ctx_count(unsigned long state)
+{
+	if (state >= RCTX_STATE_COUNT)
+		return 0;
+	return req_counts[state];
+}
+
 void req_ctx_init(void)
 {
 	int i;
-
 	for (i = 0; i < NUM_RCTX_SMALL; i++) {
+	        req_ctx[i].prev = req_ctx + i - 1;
+	        req_ctx[i].next = req_ctx + i + 1;
 		req_ctx[i].size = RCTX_SIZE_SMALL;
+		req_ctx[i].tot_len = 0;
 		req_ctx[i].data = rctx_data[i];
 		req_ctx[i].state = RCTX_STATE_FREE;
+		DEBUGPCR("SMALL req_ctx[%02i] initialized at %08X, Data: %08X => %08X", i, req_ctx + i, req_ctx[i].data, req_ctx[i].data + RCTX_SIZE_SMALL);
 	}
 
-	for (i = 0; i < NUM_RCTX_LARGE; i++) {
-		req_ctx[NUM_RCTX_SMALL+i].size = RCTX_SIZE_LARGE;
-		req_ctx[NUM_RCTX_SMALL+i].data = rctx_data_large[i];
+	for (; i < NUM_REQ_CTX; i++) {
+	        req_ctx[i].prev = req_ctx + i - 1;
+  	        req_ctx[i].next = req_ctx + i + 1;
+		req_ctx[i].size = RCTX_SIZE_LARGE;
+		req_ctx[i].tot_len = 0;
+		req_ctx[i].data = rctx_data_large[i];
+		req_ctx[i].state = RCTX_STATE_FREE;
+		DEBUGPCR("LARGE req_ctx[%02i] initialized at %08X, Data: %08X => %08X", i, req_ctx + i, req_ctx[i].data, req_ctx[i].data + RCTX_SIZE_LARGE);
 	}
+	req_ctx[0].prev = NULL;
+	req_ctx[NUM_REQ_CTX - 1].next = NULL;
+
+	req_ctx_queues[RCTX_STATE_FREE] = req_ctx;
+	req_ctx_tails[RCTX_STATE_FREE] = req_ctx + NUM_REQ_CTX - 1;
+	req_counts[RCTX_STATE_FREE] = NUM_REQ_CTX;
+
+	for (i = RCTX_STATE_FREE + 1; i < RCTX_STATE_COUNT; i++) {
+		req_ctx_queues[i] = req_ctx_tails[i] = NULL;
+		req_counts[i] = 0;
+	}
 }
Index: firmware/src/os/req_ctx.h
===================================================================
--- firmware/src/os/req_ctx.h	(revision 66922)
+++ firmware/src/os/req_ctx.h	(revision 66923)
@@ -1,11 +1,10 @@
 #ifndef _REQ_CTX_H
 #define _REQ_CTX_H
 
-#define RCTX_SIZE_LARGE	2048
-#define RCTX_SIZE_SMALL	128
+#define RCTX_SIZE_LARGE       960
+#define RCTX_SIZE_SMALL       320
 
 #define MAX_HDRSIZE	sizeof(struct openpcd_hdr)
-#define MAX_REQSIZE	(64-MAX_HDRSIZE)
 
 #define req_buf_payload(x)	(x->data[x->hdr_len])
 #define req_buf_hdr(x)		(x->data[0])
@@ -13,37 +12,42 @@
 #include <sys/types.h>
 #include <lib_AT91SAM7.h>
 
+typedef struct req_ctx *req_ctx_ptr;
+
 struct req_ctx {
-	volatile u_int32_t state;
-	u_int16_t size;
-	u_int16_t tot_len;
-	u_int8_t *data;
+  volatile u_int32_t state;
+  volatile req_ctx_ptr prev, next;
+  u_int16_t size;
+  u_int16_t tot_len;
+  u_int8_t *data;
 };
 
-#define RCTX_STATE_FREE			0xfe
-#define RCTX_STATE_UDP_RCV_BUSY		0x01
-#define RCTX_STATE_UDP_RCV_DONE		0x02
-#define RCTX_STATE_MAIN_PROCESSING	0x03
-#define RCTX_STATE_RC632IRQ_BUSY	0x04
+#define RCTX_STATE_FREE                 0
+#define RCTX_STATE_UDP_RCV_BUSY         1
+#define RCTX_STATE_UDP_RCV_DONE         2
+#define RCTX_STATE_MAIN_PROCESSING      3
+#define RCTX_STATE_RC632IRQ_BUSY        4
+#define RCTX_STATE_UDP_EP2_PENDING      5
+#define RCTX_STATE_UDP_EP2_BUSY         6
+#define RCTX_STATE_UDP_EP3_PENDING      7
+#define RCTX_STATE_UDP_EP3_BUSY         8
+#define RCTX_STATE_SSC_RX_BUSY          9
+#define RCTX_STATE_LIBRFID_BUSY        10
+#define RCTX_STATE_PIOIRQ_BUSY         11
+#define RCTX_STATE_INVALID             12
+// Nominally UNUSED states
+#define RCTX_STATE_UDP_EP0_PENDING     13
+#define RCTX_STATE_UDP_EP0_BUSY        14
+#define RCTX_STATE_UDP_EP1_PENDING     15
+#define RCTX_STATE_UDP_EP1_BUSY        16
+// Count of the number of STATES
+#define RCTX_STATE_COUNT               17
 
-#define RCTX_STATE_UDP_EP2_PENDING	0x10
-#define RCTX_STATE_UDP_EP2_BUSY		0x11
-
-#define RCTX_STATE_UDP_EP3_PENDING	0x12
-#define RCTX_STATE_UDP_EP3_BUSY		0x13
-
-#define RCTX_STATE_SSC_RX_BUSY		0x20
-
-#define RCTX_STATE_LIBRFID_BUSY		0x30
-
-#define RCTX_STATE_PIOIRQ_BUSY		0x80
-
-#define RCTX_STATE_INVALID		0xff
-
 extern struct req_ctx __ramfunc *req_ctx_find_get(int large, unsigned long old_state, unsigned long new_state);
 extern struct req_ctx *req_ctx_find_busy(void);
 extern void req_ctx_set_state(struct req_ctx *ctx, unsigned long new_state);
 extern void req_ctx_put(struct req_ctx *ctx);
 extern u_int8_t req_ctx_num(struct req_ctx *ctx);
+unsigned req_ctx_count(unsigned long state);
 
 #endif /* _REQ_CTX_H */
Index: firmware/src/os/system_irq.c
===================================================================
--- firmware/src/os/system_irq.c	(revision 66922)
+++ firmware/src/os/system_irq.c	(revision 66923)
@@ -19,6 +19,8 @@
 
 #undef DEBUG
 
+#include <stdio.h>
+
 #include <sys/types.h>
 #include <lib_AT91SAM7.h>
 #include <AT91SAM7.h>
@@ -25,15 +27,15 @@
 
 #include <os/system_irq.h>
 #include <os/dbgu.h>
+#include <string.h>
 
 #include "../openpcd.h"
 
 static sysirq_hdlr *sysirq_hdlrs[AT91SAM7_SYSIRQ_COUNT];
 
-static void sys_irq(void)
+void sys_irq(u_int32_t previous_pc)
 {
 	u_int32_t sr;
-	DEBUGP("sys_irq ");
 
 	/* Somehow Atmel decided to do really stupid interrupt sharing
 	 * for commonly-used interrupts such as the timer irq */
@@ -139,6 +141,10 @@
 	if (*AT91C_WDTC_WDMR & AT91C_WDTC_WDFIEN) {
 		sr = *AT91C_WDTC_WDSR;
 		if (sr) {
+			char dbg_buf[100];
+			sprintf(dbg_buf, "sys_irq [Old PC = %08X]\n\r", previous_pc);
+			AT91F_DBGU_Frame(dbg_buf);
+
 			DEBUGP("WDT(");
 			if (sysirq_hdlrs[AT91SAM7_SYSIRQ_WDT]) {
 				DEBUGP("handler ");
@@ -154,6 +160,13 @@
 	DEBUGPCR("END");
 }
 
+static void sysirq_entry() {
+  	// DO NOT MODIFY THIS SECTION AND Cstartup.S/IRQ_Handler_Entry.
+	register unsigned *previous_pc asm("r0");
+	asm("ADD R1, SP, #16; LDR R0, [R1]"); 
+	sys_irq((unsigned)previous_pc);
+}
+
 void sysirq_register(enum sysirqs irq, sysirq_hdlr *hdlr)
 {
 	if (irq >= AT91SAM7_SYSIRQ_COUNT)
@@ -167,6 +180,6 @@
 	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_SYS,
 			      OPENPCD_IRQ_PRIO_SYS,
 			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL,
-			      &sys_irq);
+			      &sysirq_entry);
 	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_SYS);
 }
Index: firmware/src/os/usbcmd_generic.c
===================================================================
--- firmware/src/os/usbcmd_generic.c	(revision 66922)
+++ firmware/src/os/usbcmd_generic.c	(revision 66923)
@@ -132,7 +132,7 @@
 
 void usbcmd_gen_init(void)
 {
-	DEBUGP("Inititalizing usbcmd_gen_init\n");
+	DEBUGP("Inititalizing usbcmd_gen_init\n\r");
 	/* setup FLASH write support for environment storage */
 	flash_init();
 	
Index: firmware/src/os/wdt.c
===================================================================
--- firmware/src/os/wdt.c	(revision 66922)
+++ firmware/src/os/wdt.c	(revision 66923)
@@ -30,12 +30,30 @@
 #undef  WDT_DEBUG
 #endif/*WDT_DEBUG*/
 
+#define WDT_DEBUG
+
 static void wdt_irq(u_int32_t sr)
 {
-	if (sr & 1)
-		DEBUGPCRF("================> WATCHDOG EXPIRED !!!!!");
-	if (sr & 2)
-		DEBUGPCRF("================> WATCHDOG ERROR !!!!!");
+  int loopX;
+  if (sr & 1)
+    AT91F_DBGU_Frame("================> WATCHDOG EXPIRED !!!!!\n\r");
+  if (sr & 2)
+    AT91F_DBGU_Frame("================> WATCHDOG ERROR !!!!!\n\r");
+  
+  while (1) {
+    led_switch(1, 0);
+    led_switch(2, 1);
+    loopX = 0;
+    while (loopX++ < 0x20000)
+      asm(" nop ");
+	  
+    led_switch(1, 1);
+    led_switch(2, 0);
+    loopX = 0;
+    while (loopX++ < 0x20000)
+      asm(" nop ");
+	  
+  }
 }
 
 void wdt_restart(void)
@@ -46,6 +64,7 @@
 void wdt_init(void)
 {
 	sysirq_register(AT91SAM7_SYSIRQ_WDT, &wdt_irq);
+
 #ifdef WDT_DEBUG
 	AT91F_WDTSetMode(AT91C_BASE_WDTC, (WDT_WDD << 16) |
 			 AT91C_WDTC_WDDBGHLT | AT91C_WDTC_WDIDLEHLT |
Index: firmware/src/simtrace/iso7816_uart.c
===================================================================
--- firmware/src/simtrace/iso7816_uart.c	(revision 66922)
+++ firmware/src/simtrace/iso7816_uart.c	(revision 66923)
@@ -23,6 +23,7 @@
 #include <AT91SAM7.h>
 #include <lib_AT91SAM7.h>
 #include <openpcd.h>
+#include <asm/system.h>
 
 #include <simtrace_usb.h>
 
@@ -32,6 +33,8 @@
 
 #include "../simtrace.h"
 #include "../openpcd.h"
+#include "iso7816_uart.h"
+#include "tc_etu.h"
 
 #define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
 
@@ -125,6 +128,20 @@
 	12, 20, 2, 4, 8, 16, 32, 64,
 };
 
+void iso_uart_report_errors(void) 
+{
+	static unsigned lastOverrun = 0, lastParity = 0, lastFrame = 0;
+	if (isoh.stats.overrun != lastOverrun) {
+		DEBUGPCR("UART overrun: %u", lastOverrun = isoh.stats.overrun);
+	}
+	if (isoh.stats.frame_err != lastFrame) {
+		DEBUGPCR("UART frame error: %u", lastFrame = isoh.stats.frame_err);
+	}
+	if (isoh.stats.parity_err != lastParity) {
+		DEBUGPCR("UART parity error: %u", lastParity = isoh.stats.parity_err);
+	}
+}
+
 void iso_uart_stats_dump(void)
 {
 	DEBUGPCRF("no_rctx: %u, rctx_sent: %u, rst: %u, pps: %u, bytes: %u, "
@@ -188,26 +205,33 @@
 
 static void send_rctx(struct iso7816_3_handle *ih)
 {
+	static u_int16_t seq_num = 0;
 	struct req_ctx *rctx = ih->rctx;
+	unsigned flags;
 
 	if (!rctx)
 		return;
 
+	local_irq_save(flags);
 	/* Put Fi and Di into res[2] array */
 	ih->sh.res[0] = ih->fi;
 	ih->sh.res[1] = ih->di;
 
+	ih->sh.seq_num = seq_num++;
+	ih->sh.offset = 0;
+	ih->sh.tot_len = rctx->tot_len;
+
 	/* copy the simtrace header */
 	memcpy(rctx->data, &ih->sh, sizeof(ih->sh));
+	ih->sh.flags = 0;
+	ih->rctx = NULL;
+	local_irq_restore(flags);
 
 	req_ctx_set_state(rctx, RCTX_STATE_UDP_EP2_PENDING);
-
-	memset(&ih->sh, 0, sizeof(ih->sh));
-	ih->rctx = NULL;
-
 	ih->stats.rctx_sent++;
 }
 
+struct iso_uart_atr_data static_atr;
 
 /* Update the ATR sub-state */
 static void set_atr_state(struct iso7816_3_handle *ih, enum atr_state new_atrs)
@@ -220,6 +244,10 @@
 		memset(ih->atr, 0, sizeof(ih->atr));
 	} else if (ih->atr_state == new_atrs)
 		return;
+	else if (new_atrs == ATR_S_DONE) {
+	  memcpy(static_atr.data, ih->atr, 64);
+	  static_atr.len = ih->atr_idx;
+	}
 
 	//DEBUGPCR("ATR state %u -> %u", ih->atr_state, new_atrs);
 	ih->atr_state = new_atrs;
@@ -234,7 +262,7 @@
 
 	rc = compute_fidi_ratio(ih->fi, ih->di);
 	if (rc > 0 && rc < 0x400) {
-		DEBUGPCR("computed Fi(%u) Di(%u) ratio: %d", ih->fi, ih->di, rc);
+	  //		DEBUGPCR("computed Fi(%u) Di(%u) ratio: %d", ih->fi, ih->di, rc);
 		/* make sure UART uses new F/D ratio */
 		usart->US_CR |= AT91C_US_RXDIS | AT91C_US_RSTRX;
 		usart->US_FIDI = rc & 0x3ff;
@@ -567,6 +595,23 @@
 	set_state(&isoh, ISO7816_S_WAIT_APDU);
 }
 
+void iso_uart_flush(void) {
+	send_rctx(&isoh);
+}
+
+void iso_uart_idleflush(void) {
+	static struct req_ctx *last_req = NULL;
+	static u_int16_t last_len = 0;
+	if (last_req == isoh.rctx &&
+        	last_len == isoh.rctx->tot_len &&
+		req_ctx_count(RCTX_STATE_UDP_EP2_PENDING) == 0 &&
+		(isoh.sh.flags & SIMTRACE_FLAG_ATR) == 0) {
+		send_rctx(&isoh);
+	}
+	last_req = isoh.rctx;
+	last_len = isoh.rctx->tot_len;
+}
+
 static __ramfunc void usart_irq(void)
 {
 	u_int32_t csr = usart->US_CSR;
@@ -586,9 +631,6 @@
 	}
 
 	if (csr & (AT91C_US_PARE|AT91C_US_FRAME|AT91C_US_OVRE)) {
-		u_int8_t nb_err = usart->US_NER;
-		/* FIXME: some error has occurrerd */
-		//DEBUGP("NER=%02x ", nb_err);
 		/* clear the status */
 		usart->US_CR |= AT91C_US_RSTSTA;
 
@@ -600,7 +642,7 @@
 			isoh.stats.overrun++;
 	}
 
-	if (csr & AT91C_US_INACK) {
+	if (csr & AT91C_US_NACK) {
 		/* we would have sent a NACK if INACK was not set */
 		usart->US_CR |= AT91C_US_RSTNACK;
 	}
@@ -609,11 +651,12 @@
 /* handler for the RST input pin state change */
 static void reset_pin_irq(u_int32_t pio)
 {
+	// When ever reset occured, we make sure to flush out any populated req_ctx
 	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, pio)) {
-		DEBUGPCR("nRST");
+		iso_uart_flush();
 		set_state(&isoh, ISO7816_S_RESET);
 	} else {
-		DEBUGPCR("RST");
+		iso_uart_flush();
 		set_state(&isoh, ISO7816_S_WAIT_ATR);
 		isoh.stats.rst++;
 	}
@@ -664,10 +707,10 @@
 				AT91C_US_CKLO | AT91C_US_INACK;
 		usart->US_BRGR = (0x0000 << 16) | 16;
 	} else {
-		usart->US_MR = AT91C_US_USMODE_ISO7816_0 | AT91C_US_CLKS_EXT |
+	  /*		usart->US_MR = AT91C_US_USMODE_ISO7816_0 | AT91C_US_CLKS_EXT |
 				AT91C_US_CHRL_8_BITS | AT91C_US_NBSTOP_1_BIT |
 				AT91C_US_CKLO | AT91C_US_INACK;
-		usart->US_BRGR = (0x0000 << 16) | 0x0001;
+				usart->US_BRGR = (0x0000 << 16) | 0x0001; */
 	}
 }
 
@@ -675,10 +718,12 @@
 {
 	DEBUGPCR("USART Initializing");
 
+	memset(&isoh, 0, sizeof(isoh));
+
 	refill_rctx(&isoh);
 
 	/* make sure we get clock from the power management controller */
-	AT91F_US0_CfgPMC();
+	//	AT91F_US0_CfgPMC();
 
 	/* configure all 3 signals as input */
 	AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, SIMTRACE_PIO_IO, SIMTRACE_PIO_CLK);
@@ -696,9 +741,17 @@
 	usart->US_CR = AT91C_US_RXDIS | AT91C_US_TXDIS;
 
 	/* ISO7816 T=0 mode with external clock input */
+	/*	       	usart->US_MR = AT91C_US_USMODE_ISO7816_0 | AT91C_US_CLKS_EXT | 
+	  AT91C_US_CHRL_8_BITS | AT91C_US_NBSTOP_15_BIT | AT91C_US_PAR_EVEN |
+	AT91C_US_CKLO | AT91C_US_INACK |
+	AT91C_US_OVER; */
+	
+
+	// OK
 	usart->US_MR = AT91C_US_USMODE_ISO7816_0 | AT91C_US_CLKS_EXT | 
-			AT91C_US_CHRL_8_BITS | AT91C_US_NBSTOP_1_BIT |
-			AT91C_US_CKLO | AT91C_US_INACK;
+	  AT91C_US_CHRL_8_BITS | AT91C_US_NBSTOP_15_BIT | AT91C_US_PAR_EVEN |
+	  AT91C_US_CKLO | AT91C_US_INACK;// |
+	  //	AT91C_US_OVER; 
 
 	/* Disable all interrupts */
 	usart->US_IDR = 0xff;
Index: firmware/src/simtrace/iso7816_uart.h
===================================================================
--- firmware/src/simtrace/iso7816_uart.h	(revision 66922)
+++ firmware/src/simtrace/iso7816_uart.h	(revision 66923)
@@ -1,5 +1,8 @@
+#ifndef SIMTRACE_ISO7816_UART_H
+#define SIMTRACE_ISO7816_UART_H
 
 struct simtrace_stats *iso_uart_stats_get(void);
+void iso_uart_report_errors(void);
 void iso_uart_stats_dump(void);
 void iso_uart_dump(void);
 void iso_uart_rst(unsigned int state);
@@ -6,3 +9,15 @@
 void iso_uart_rx_mode(void);
 void iso_uart_clk_master(unsigned int master);
 void iso_uart_init(void);
+void iso_uart_flush(void);
+void iso_uart_idleflush(void);
+void iso7816_wtime_expired(void);
+
+struct iso_uart_atr_data {
+  unsigned char len;
+  char data[64];
+};
+
+extern struct iso_uart_atr_data static_atr;
+
+#endif
Index: firmware/src/simtrace/main_simtrace.c
===================================================================
--- firmware/src/simtrace/main_simtrace.c	(revision 66922)
+++ firmware/src/simtrace/main_simtrace.c	(revision 66923)
@@ -17,7 +17,7 @@
  *
  */
 
-
+#include <string.h>
 #include <errno.h>
 #include <include/lib_AT91SAM7.h>
 #include <include/openpcd.h>
@@ -45,7 +45,8 @@
 		    SIMTRACE_PIO_CLK_PH |		\
 		    SIMTRACE_PIO_CLK_PH_T |		\
 		    SIMTRACE_PIO_IO_PH_RX |		\
-		    SIMTRACE_PIO_IO_PH_TX)
+		    SIMTRACE_PIO_IO_PH_TX |             \
+		    SIMTRACE_PIO_VCC_PHONE)
 
 #define UART0_PINS (SIMTRACE_PIO_nRST |			\
 		    SIMTRACE_PIO_CLK |			\
@@ -53,46 +54,6 @@
 		    SIMTRACE_PIO_IO |			\
 		    SIMTRACE_PIO_IO_T)
 
-enum simtrace_power_mode {
-	SIMTRACE_PWR_OFF,
-	SIMTRACE_PWR_PASS,
-	SIMTRACE_PWR_LDO
-};
-
-void simtrace_set_power(enum simtrace_power_mode mode)
-{
-	switch (mode) {
-	case SIMTRACE_PWR_PASS:
-		/* switch VCC_SIM pin into output mode, as in the first
-		 * generation prototype we use it directly to supply Vcc
-		 * to the SIM.  Pin unused in v1.0(p) and v1.1p */
-		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
-		/* v1.0/1.1: pass-throguh power from the reader/phone (FPC) */
-		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
-		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
-		break;
-	case SIMTRACE_PWR_LDO:
-		/* switch VCC_SIM pin into output mode, as in the first
-		 * generation prototype we use it directly to supply Vcc
-		 * to the SIM.  Pin unused in v1.0(p) and v1.1p */
-		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
-		/* v1.0/1.1: power from the internal 3.3V LDO */
-		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
-		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
-		break;
-	default:
-	case SIMTRACE_PWR_OFF:
-		/* switch VCC_SIM pin into output mode, as in the first
-		 * generation prototype we use it directly to supply Vcc
-		 * to the SIM.  Pin unused in v1.0(p) and v1.1p */
-		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
-		/* switch all power paths off */
-		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
-		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
-		break;
-	}
-}
-
 static void simtrace_set_mode(enum simtrace_md mode)
 {
 	switch (mode) {
@@ -99,9 +60,11 @@
 	case SIMTRACE_MD_SNIFFER:
 		DEBUGPCR("MODE: SNIFFER\n");
 
-		/* switch UART1 pins to input, no pull-up */
-		AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART1_PINS);
-		AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, UART1_PINS);
+		/* UART1 PINS -- PHONE SIDE SHOULD ALWAYS BE ON INPUT, NO CHANGE --
+		   DO NOT UPDATE IT
+		   switch UART1 pins to input, no pull-up */
+		//		AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART1_PINS);
+		//		AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, UART1_PINS);
 
 		/* switch UART0 pins to 'ISO7816 card mode' */
 		AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART0_PINS);
@@ -122,12 +85,12 @@
 {
 	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->data[0];
 	struct simtrace_stats *stats_in;
-	struct simtrace_stats *stats = (struct simtrace_stats *) poh->data;
+       	// struct simtrace_stats *stats = (struct simtrace_stats *) poh->data;
 
 	switch (poh->cmd) {
 	case SIMTRACE_MSGT_STATS:
 		stats_in = iso_uart_stats_get();
-		memcpy(stats, stats_in, sizeof(*stats));
+		memcpy(&poh->data[0], stats_in, sizeof(struct simtrace_stats));
 		req_ctx_set_state(rctx, RCTX_STATE_UDP_EP2_PENDING);
 		break;
 	default:
@@ -134,10 +97,41 @@
 		req_ctx_set_state(rctx, RCTX_STATE_FREE);
 		break;
 	}
+	return 0;
 }
 
+static volatile unsigned SPUIRQ_PC, SPUIRQ_COUNT = 0;
+
+void check_spurious_irq() {
+	static unsigned last_count = 0;
+	if (last_count != SPUIRQ_COUNT) {
+		DEBUGPCR("SPURRIOUS IRQ %i [Old PC = %08X]", SPUIRQ_COUNT, SPUIRQ_PC);
+		last_count = SPUIRQ_COUNT;
+	}
+}
+
+void custom_spurious_handler(unsigned previous_pc) {
+	unsigned flags;
+	local_irq_save(flags);
+	SPUIRQ_PC = previous_pc;
+	SPUIRQ_COUNT++;
+	local_irq_restore(flags);
+}
+
+void custom_spurious_entry(void) {
+	register unsigned *previous_pc asm("r0");
+	asm("ADD R1, SP, #16; LDR R0, [R1]");
+	custom_spurious_handler((unsigned)previous_pc);
+}
+
+// defined in os/usb_benchmark.c
+void usbtest_init(void);
+
 void _init_func(void)
 {
+        int loop = 0, loopX;
+	AT91C_BASE_AIC->AIC_SPU = (int)custom_spurious_entry;
+
 	/* low-level hardware initialization */
 	pio_irq_init();
 	iso_uart_init();
@@ -150,35 +144,176 @@
 	/* high-level protocol */
 	//opicc_usbapi_init();
 	led_switch(1, 0);
-	led_switch(2, 1);
+	led_switch(2, 0);
 
-	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
+	sim_switch_set_power(SIM_SWITCH_PWR_PASS);
+	// sim_switch_set_power(SIM_SWITCH_PWR_OFF);
+
+	//	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
 	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
 	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
-	/* default: use local LDO to supply power */
-	simtrace_set_power(SIMTRACE_PWR_LDO);
+	
+	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE)) {
+	  //	  sim_switch_set_power(SIM_SWITCH_PWR_OFF);
+	  led_switch(1, 0);
+	} else {  // USE LDO, otherwise, phone tries multiple Reset
+	  //	  sim_switch_set_power(SIM_SWITCH_PWR_LDO);
+	  led_switch(1, 1);
+	}
 
 	iso_uart_rx_mode();
 	simtrace_set_mode(SIMTRACE_MD_SNIFFER);
+
+	while (loop++ < 3) {
+	  led_switch(2, 1);
+	  loopX = 0;
+	  while (loopX++ < 0x20000)
+	    asm(" nop ");
+	  
+	  led_switch(2, 0);
+	  loopX = 0;
+	  while (loopX++ < 0x20000)
+	    asm(" nop ");
+	  
+	}
+	led_switch(2, 1);
 }
 
 
 static void help(void)
 {
-	DEBUGPCR("r: iso uart Rx mode\r\n"
-		 "c: toggle clock master/slave\r\n"
-		 "l: set nRST to low (active)\r\n"
-		 "h: set nRST to high (inactive)\r\n"
-		 "o: set nRST to input\r\n"
-		 "t: ISO UART statistics\r\n"
-		 "s: disconnect SIM bus switch\r\n"
-		 "S: connect SIM bus switch\r\n");
+  DEBUGPCR("r: iso uart Rx mode");
+  DEBUGPCR("c: toggle clock master/slave");
+  DEBUGPCR("l: set nRST to low (active)");
+  DEBUGPCR("h: set nRST to high (inactive)");
+  DEBUGPCR("o: set nRST to input");
+  DEBUGPCR("t: ISO UART statistics");
+  DEBUGPCR("s: disconnect SIM bus switch");
+  DEBUGPCR("S: connect SIM bus switch");
+  DEBUGPCR("v: turn off vcc_sim");
+  DEBUGPCR("V: turn on vcc_sim");
+  DEBUGPCR("p: read PA25/VCC_PHONE");
+  DEBUGPCR("b: toggle heartbeat");
+  DEBUGPCR("<: Previous PIN");
+  DEBUGPCR("P: PIN Information");
+  DEBUGPCR(">: Next PIN");
+  DEBUGPCR("D: Disable PULLUP");
+  DEBUGPCR("E: Enable PULLUP");
+  DEBUGPCR("I: Set PIN to Input");
+  DEBUGPCR("O: Set PIN to Output");
+  DEBUGPCR("+: Set PIN Output (1)");
+  DEBUGPCR("-: Clear PIN Output (0)");
+  //  DEBUGPCR("~: Set PIN to Opendrain\r\n");
 }
 
+unsigned currentPin = 0;
+
+const char* peripheral_a[] = {
+  "PWM0", // 0
+  "PWM1", // 1
+  "PWM2", // 2
+  "TWD", // 3
+  "TWCK", // 4
+  "RXD0", // 5
+  "TXD0", // 6
+  "RTS0", // 7
+  "CTS0", // 8
+  "DRXD", // 9
+  "DTXD", // 10
+  "NPCS0", // 11
+  "MISO", // 12
+  "MOSI", // 13
+  "SPCK", // 14
+  "TF", // 15
+  "TK", // 16
+  "AD0 - TD", // 17
+  "AD1 - RD", // 18
+  "AD2 - RK", // 19
+  "AD3 - RF", // 20
+  "RXD1", // 21
+  "TXD1", // 22
+  "SCK1", // 23
+  "RTS1", // 24
+  "CTS1", // 25
+  "DCD1", // 26
+  "DTR1", // 27
+  "DSR1", // 28
+  "RI1", // 29
+  "IRQ1", // 30
+  "NPCS1", // 31
+};
+
+const char* peripheral_b[] = {
+  "TIOA0", // 0
+  "TIOB0", // 1
+  "SCK0", // 2
+  "NPCS3", // 3
+  "TCLK0", // 4
+  "NPCS3", // 5
+  "PCK0", // 6
+  "PWM3", // 7
+  "ADTRG", // 8
+  "NPCS1", // 9
+  "NPCS2", // 10
+  "PWM0", // 11
+  "PWM1", // 12
+  "PWM2", // 13
+  "PWM3", // 14
+  "TIOA1", // 15
+  "TIOB1", // 16
+  "AD0 - PCK1", // 17
+  "AD1 - PCK2", // 18
+  "AD2 - FIQ", // 19
+  "AD3 - IRQ0", // 20
+  "PCK1", // 21
+  "NPCS3", // 22
+  "PWM0", // 23
+  "PWM1", // 24
+  "PWM2", // 25
+  "TIOA2", // 26
+  "TIOB2", // 27
+  "TCLK1", // 28
+  "TCLK2", // 29
+  "NPCS2", // 30
+  "PCK2", // 31
+};
+
+void print_pin() {
+  unsigned mask = 1 << currentPin;
+  if (currentPin > 31) {
+    DEBUGPCR("Invalid PIN #%u", currentPin);
+    return;
+  }
+  DEBUGP("PIN PA%u => %s: ", currentPin, (AT91C_BASE_PIOA->PIO_PSR & mask) ? "ENABLED" : "DISABLED");
+  if (AT91C_BASE_PIOA->PIO_ABSR & mask) {
+    DEBUGPCR("Peripheral B: %s", peripheral_b[currentPin]);
+  } else {
+    DEBUGPCR("Peripheral A: %s", peripheral_a[currentPin]);
+  }
+  if (AT91C_BASE_PIOA->PIO_MDSR & mask) {
+    DEBUGPCR("Multi-Drive/Opendrain enabled");
+  }
+  if (AT91C_BASE_PIOA->PIO_OWSR & mask) {
+    DEBUGPCR("Output-Write/DirectDrive enabled");
+  }
+  if (AT91C_BASE_PIOA->PIO_IFSR & mask) {
+    DEBUGPCR("Input filter enabled");
+  }
+  DEBUGP("IO / PULLUP / VALUE: ");
+  if (AT91C_BASE_PIOA->PIO_OSR & mask) {
+    DEBUGPCR("O %s %s", (AT91C_BASE_PIOA->PIO_PPUSR & mask) ? "D" : "E", (AT91C_BASE_PIOA->PIO_ODSR & mask) ? "1" : "0");
+  } else {
+    DEBUGPCR("I %s %s", (AT91C_BASE_PIOA->PIO_PPUSR & mask) ? "D" : "E", (AT91C_BASE_PIOA->PIO_PDSR & mask) ? "1" : "0");
+  }
+  DEBUGPCR("END PIN INFO\r\n");
+}
+
+unsigned do_heart_beat = 1;
+
 int _main_dbgu(char key)
 {
 	static int i = 0;
-	DEBUGPCRF("main_dbgu");
+	//	DEBUGPCRF("main_dbgu");
 
 	switch (key) {
 	case 's':
@@ -208,6 +343,62 @@
 	case 'd':
 		iso_uart_dump();
 		break;
+	case 'v':
+	  sim_switch_set_power(SIM_SWITCH_PWR_OFF);
+	  break;
+	case 'V':
+	  sim_switch_set_power(SIM_SWITCH_PWR_LDO);
+	  break;
+	case 'p':
+	  DEBUGPCR("PA25/VCC_PHONE = %i", (AT91F_PIO_GetInput(AT91C_BASE_PIOA) >> 25) & 1);
+	  break;
+	case 'b':
+	  do_heart_beat = !do_heart_beat;
+	  break;
+	case '<':
+	  if (currentPin == 0)
+	    currentPin = 32;
+	  currentPin--;
+	  print_pin();
+	  break;
+	case 'P':
+	  print_pin();
+	  break;
+	case '>':
+	  currentPin++;
+	  if (currentPin == 32)
+	    currentPin = 0;
+	  print_pin();
+	  break;
+	case 'D':
+	  AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, 1 << currentPin);
+	  print_pin();
+	  break;
+	case 'E':
+	  AT91F_PIO_CfgPullupEn(AT91C_BASE_PIOA, 1 << currentPin);
+	  print_pin();
+	  break;
+	case 'I':
+	  AT91F_PIO_CfgInput(AT91C_BASE_PIOA, 1 << currentPin);
+	  print_pin();
+	  break;
+	case 'O':
+	  AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, 1 << currentPin);
+	  print_pin();
+	  break;
+	case '+':
+	  AT91F_PIO_SetOutput(AT91C_BASE_PIOA, 1 << currentPin);
+	  print_pin();
+	  break;
+	case '-':
+	  AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, 1 << currentPin);
+	  print_pin();
+	  break;
+	  //	case '~':
+	  //	  print_pin();
+	  //	  AT91F_PIO_CfgOpendrain(AT91C_BASE_PIOA, 1 << currentPin);
+
+	  break;
 	case '?':
 		help();
 		break;
@@ -218,6 +409,10 @@
 
 void _main_func(void)
 {
+  	static unsigned loopLow = 0, loopHigh = 0;
+	char atr_buf[200], x;
+	unsigned char i, pos;
+
 	/* first we try to get rid of pending to-be-sent stuff */
 	usb_out_process();
 
@@ -225,4 +420,50 @@
 	usb_in_process();
 
 	udp_unthrottle();
+
+	if (static_atr.len) {
+	  atr_buf[0] = 'A';
+	  atr_buf[1] = 'T';
+	  atr_buf[2] = 'R';
+	  atr_buf[3] = ' ';
+	  atr_buf[4] = ' ';
+	  pos = 5;
+	  for (i = 0; i < static_atr.len; i++) {
+	    x = static_atr.data[i] >> 4;
+	    atr_buf[pos++] = (x < 10) ? '0' + x : 'A' + x - 10;
+	    x = static_atr.data[i] & 15;
+	    atr_buf[pos++] = (x < 10) ? '0' + x : 'A' + x - 10;
+	    atr_buf[pos++] = ' ';
+	  }
+	  atr_buf[pos - 1] = '\r';
+	  atr_buf[pos] = '\n';
+	  atr_buf[pos + 1] = 0;
+	  static_atr.len = 0;
+	  DEBUGP(atr_buf);
+	}
+       	if (do_heart_beat && (loopLow & 0xFFFF) == 0) {
+		DEBUGPCR("Heart beat %08X", loopHigh++);
+	}
+	if ((loopLow & 0x3F) == 0) {
+		iso_uart_idleflush();
+	}
+	loopLow++;
+
+	/*	if (loopHigh > 20) { // test WatchDog
+	  while (1) {
+	    asm(" NOP ");
+	    asm(" NOP ");
+	    asm(" NOP ");
+	    asm(" NOP ");
+	    asm(" NOP ");
+	    asm(" NOP ");
+	    asm(" NOP ");
+	    asm(" NOP ");
+	    asm(" NOP ");
+	    asm(" NOP ");
+	  }
+	  } */
+	
+	iso_uart_report_errors();
+	check_spurious_irq();
 }
Index: firmware/src/simtrace/sim_switch.c
===================================================================
--- firmware/src/simtrace/sim_switch.c	(revision 66922)
+++ firmware/src/simtrace/sim_switch.c	(revision 66923)
@@ -29,12 +29,50 @@
 #include <os/usb_handler.h>
 #include <os/dbgu.h>
 #include <os/pio_irq.h>
+#include <os/led.h>
 
+#include "sim_switch.h"
+#include "iso7816_uart.h"
 #include "../simtrace.h"
 #include "../openpcd.h"
 
 #define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
 
+void sim_switch_set_power(enum sim_switch_power_mode mode)
+{
+	switch (mode) {
+	case SIM_SWITCH_PWR_PASS:
+		/* switch VCC_SIM pin into output mode, as in the first
+		 * generation prototype we use it directly to supply Vcc
+		 * to the SIM.  Pin unused in v1.0(p) and v1.1p */
+	  //		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
+		/* v1.0/1.1: pass-throguh power from the reader/phone (FPC) */
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+		break;
+	case SIM_SWITCH_PWR_LDO:
+		/* switch VCC_SIM pin into output mode, as in the first
+		 * generation prototype we use it directly to supply Vcc
+		 * to the SIM.  Pin unused in v1.0(p) and v1.1p */
+	  //		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
+		/* v1.0/1.1: power from the internal 3.3V LDO */
+	        AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+	  //		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
+		break;
+	default:
+	case SIM_SWITCH_PWR_OFF:
+		/* switch VCC_SIM pin into output mode, as in the first
+		 * generation prototype we use it directly to supply Vcc
+		 * to the SIM.  Pin unused in v1.0(p) and v1.1p */
+	  //		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
+		/* switch all power paths off */
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+		break;
+	}
+}
+
 void sim_switch_mode(int connect_io, int connect_misc)
 {
 	if (connect_io)
@@ -59,10 +97,14 @@
 
 static void vcc_phone_irq(u_int32_t pio)
 {
-	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE))
-		DEBUGPCR("VCC_PHONE off");
-	else
-		DEBUGPCR("VCC_PHONE on");
+	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE)) {
+	  //	  sim_switch_set_power(SIM_SWITCH_PWR_OFF);
+		led_switch(1, 0);
+		iso_uart_flush();
+	} else { // USE LDO, otherwise, phone tries multiple Reset
+	  //	  sim_switch_set_power(SIM_SWITCH_PWR_LDO);
+		led_switch(1, 1);
+	}
 }
 
 void sim_switch_init(void)
@@ -81,6 +123,7 @@
 	AT91F_PIO_CfgInputFilter(AT91C_BASE_PIOA, SIMTRACE_PIO_SW_SIM);
 	pio_irq_register(SIMTRACE_PIO_SW_SIM, &sw_sim_irq);
 	pio_irq_enable(SIMTRACE_PIO_SW_SIM);
+
 	/* configure VCC_PHONE detection */
 	AT91F_PIO_CfgInput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE);
 	AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE);
Index: firmware/src/simtrace/sim_switch.h
===================================================================
--- firmware/src/simtrace/sim_switch.h	(revision 66922)
+++ firmware/src/simtrace/sim_switch.h	(revision 66923)
@@ -1,6 +1,13 @@
 #ifndef SIMTRACE_ISO_SW_H
 #define SIMTRACE_ISO_SW_H
 
+enum sim_switch_power_mode {
+	SIM_SWITCH_PWR_OFF,
+	SIM_SWITCH_PWR_PASS,
+	SIM_SWITCH_PWR_LDO
+};
+
+void sim_switch_set_power(enum sim_switch_power_mode mode);
 void sim_switch_mode(int connect_io, int connect_misc);
 void sim_switch_init(void);
 
Index: firmware/src/simtrace/tc_etu.c
===================================================================
--- firmware/src/simtrace/tc_etu.c	(revision 66922)
+++ firmware/src/simtrace/tc_etu.c	(revision 66923)
@@ -20,7 +20,7 @@
 #include <lib_AT91SAM7.h>
 #include <AT91SAM7.h>
 #include <os/dbgu.h>
-
+#include "iso7816_uart.h"
 #include "../openpcd.h"
 
 static AT91PS_TCB tcb;
Index: firmware/src/simtrace/tc_etu.h
===================================================================
--- firmware/src/simtrace/tc_etu.h	(revision 66922)
+++ firmware/src/simtrace/tc_etu.h	(revision 66923)
@@ -1,4 +1,8 @@
+#ifndef TC_ETU_H
+#define TC_ETU_H
 
 void tc_etu_set_wtime(u_int16_t wtime);
 void tc_etu_set_etu(u_int16_t etu);
 void tc_etu_init(void);
+
+#endif
Index: firmware/src/simtrace.h
===================================================================
--- firmware/src/simtrace.h	(revision 66922)
+++ firmware/src/simtrace.h	(revision 66923)
@@ -4,7 +4,7 @@
 #ifdef SIMTRACE
 #define OPENPCD_PIO_LED2	AT91C_PIO_PA17
 #define OPENPCD_PIO_LED1	AT91C_PIO_PA18
-#define OPENPCD_PIO_UDP_CNX	AT91C_PIO_PA29
+// #define OPENPCD_PIO_UDP_CNX	AT91C_PIO_PA29  // min.xu@min-info.net REDEF from openpcd.h, does not appear to be used
 #define OPENPCD_PIO_UDP_PUP	AT91C_PIO_PA16
 #define USB_PRODUCT_ID		SIMTRACE_PRODUCT_ID
 #else
Index: firmware/src/start/Cstartup_SAM7.c
===================================================================
--- firmware/src/start/Cstartup_SAM7.c	(revision 66922)
+++ firmware/src/start/Cstartup_SAM7.c	(revision 66923)
@@ -16,6 +16,7 @@
 
 // Include the board file description
 #include <board.h>
+#include <AT91SAM7.h>
 
 // The following functions must be write in ARM mode this function called directly
 // by exception vector
@@ -33,7 +34,20 @@
 AT91F_LowLevelInit (void)
 {
   volatile int i;
+ 
+#define UART1_PINS (AT91C_PA21_RXD1 | AT91C_PA22_TXD1 | AT91C_PA23_SCK1 | AT91C_PIO_PA24 | AT91C_PIO_PA25 | AT91C_PIO_PA28)
+#define UART0_PINS (AT91C_PIO_PA7 | AT91C_PIO_PA6 | AT91C_PIO_PA4 | AT91C_PIO_PA2 | AT91C_PIO_PA1)
+
+  AT91F_PIOA_CfgPMC();
+  AT91F_US0_CfgPMC();
+  AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART0_PINS | UART1_PINS);
+  AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, UART0_PINS | UART1_PINS);
+
+  AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, AT91C_PIO_PA5);
+  AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, AT91C_PIO_PA5);
+  AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, AT91C_PIO_PA5);
   
+  
   //* Debounce power supply
   for(i=0;i<1024;i++);
   
