Completely separate DFU from application program, we now have a real bootloader.

DFU occupies 0x00100000 - 0x00100fff in flash, application starts at 0x00101000.
DFU also occupies the first couple of bytes in SRAM, application starts at 0x00200024.

In order to produce a samba-flashable image, first build dfu.bin by typing 
'make -f Makefule.dfu', succeeded by 'make TARGET=... DEBUG=... BOARD=...' and then 
concatenating the two files together, e.g. cat dfu.bin main_reqa.bin > flash.bin.

Actual flashing via DFU is still not operational, but will be implemented next


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

13 files changed, 1056 insertions, 203 deletions

diff --git a/firmware/Makefile b/firmware/Makefile
index 3d8c42d..a916ae6 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -53,7 +53,7 @@ RUN_MODE=RUN_FROM_ROM
 #RUN_MODE=RUN_FROM_RAM
 
 ## We want to produce a full-flash image, but including DFU 
-IMGTYPE=-dfu-fullimage
+IMGTYPE=-sam7dfu-app
 
 # with / at end
 PATH_TO_LINKSCRIPTS=link/
@@ -86,8 +86,8 @@ endif
 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/dfu.c src/start/Cstartup_SAM7.c \
-	  src/os/tc_cdiv.c src/os/pit.c src/os/pwm.c src/os/pio_irq.c
+	  src/os/usb_benchmark.c src/os/tc_cdiv.c src/os/pit.c \
+	  src/os/pwm.c src/os/pio_irq.c
 
 ifeq ($(BOARD), PCD)
 # PCD support code
@@ -134,7 +134,7 @@ ASRCLIB += lib/lib1funcs.S lib/div64.S
 endif
 
 # List Assembler source files here which must be assembled in ARM-Mode..
-ASRCARM  = src/start/Cstartup.S $(ASRCLIB)
+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)
diff --git a/firmware/Makefile.dfu b/firmware/Makefile.dfu
new file mode 100644
index 0000000..5fcb4d3
--- /dev/null
+++ b/firmware/Makefile.dfu
@@ -0,0 +1,537 @@
+# 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
+
+# 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 src/dfu/dfu.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
+
+## 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 
+endif
+
+ifdef OLIMEX
+CDEFS += -DOLIMEX
+endif
+
+ifeq ($(BOARD),PICC)
+CDEFS += -DPICC
+endif
+
+ifeq ($(BOARD),PCD)
+CDEFS += -DPCD
+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-elf-gcc/arm-elf-g++)
+CFLAGS = -g$(DEBUGF) -DBOARD=PCD -DPCD
+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-elf-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-elf-gcc
+CPP = arm-elf-g++
+OBJCOPY = arm-elf-objcopy
+OBJDUMP = arm-elf-objdump
+SIZE = arm-elf-size
+NM = arm-elf-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
+	@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/*
+
+
+# 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
+
diff --git a/firmware/TODO b/firmware/TODO
index eea1974..c63fb89 100644
--- a/firmware/TODO
+++ b/firmware/TODO
@@ -12,16 +12,12 @@ USB:
 - Add DFU descriptor to host
 
 DFU:
-- implement DFU protocol for memory and flash
+- implement DFU protocol for bootloader flash partition
 - add capability to copy and execute DFU from RAM
+- Get rid of lots of constant values and replace them by proper defines
 
 Generic:
 -
 
-Later, for PICCsim:
-- Implement SSC code for sampling subcarrier
-- Implement SSC code for generating subcarrier
-- Implement Manchester coding
-
 Way Later, for librfid:
 - Implement parts (or all of) 14443 in firmware as alternative configuration
diff --git a/firmware/doc/dfu.txt b/firmware/doc/dfu.txt
index 9b40c3c..78fe5a6 100644
--- a/firmware/doc/dfu.txt
+++ b/firmware/doc/dfu.txt
@@ -86,4 +86,46 @@ dfu_api				x	x	flash (const anyway)
 dfu_state			x	x	RAM
 
 
+preconditions:
+	- dfu code assumes to be loaded to address zero to make it work from both ram and rom
+
+startup:
+	- exception vectors (in flash)
+	- call lowlevel_init
+	- setup user/supervisor/FIQ/IRQ stack
+	- relocate dfu_state
+	- if DFU switch is pressed
+		- jump to dfu_main in flash
+			- relocate all of DFU .text/.data into ram (including exception vectors)
+			- initialize DFU .bss
+			- remap RAM to address zero
+
+	- if DFU switch is not pressed jump to entry address of app
+		- Cstartup_app.S
+			- relocate application .data
+			- initialize application .bss
+			- jump to appliction main 
+			- application uses dfu_api in flash, pointing to in-flash data
+			- application calls dfu_switch()
+				- disable all interupts but USB
+				- relocate all of DFU .text/.data into ram (including exception vectors)
+				- remap RAM to address zero
+				- check whether app has already remapped RAM before !?!
+
+memory map:
+
+load_addr	run_Addr
+0x00100000	0x00000000	exception vectors (DFU Cstartup)
+				Cstartup_SAM7
+
+		0x00200000	
+
+
+DFU constants:
+_etext:
+_data:	0x200000
+_edata:
+
+APP contants:
+_data: _edata_dfu
 
diff --git a/firmware/link/AT91SAM7S256-ROM-sam7dfu-app.ld b/firmware/link/AT91SAM7S256-ROM-sam7dfu-app.ld
new file mode 100644
index 0000000..32bd787
--- /dev/null
+++ b/firmware/link/AT91SAM7S256-ROM-sam7dfu-app.ld
@@ -0,0 +1,94 @@
+/* Memory Definitions */
+
+MEMORY
+{
+  FLASH (rx) : ORIGIN = 0x00101000, LENGTH = (0x00040000 - 0x1000)
+  DATA (rw)  : ORIGIN = 0x00200024, LENGTH = (0x00010000 - 0x24)
+  STACK (rw) : ORIGIN = 0x00210000, LENGTH = 0x00000000
+}
+
+
+/* Section Definitions */
+
+SECTIONS
+{
+	. = 0x00000000;
+	/* first section is .text which is used for code */
+	.text 0x00101000: AT ( 0x00000000 ) { 
+  		src/start/Cstartup_app.o (.text)
+		* (.text)
+		* (.rodata*)
+  		. = ALIGN(4);
+  	} >FLASH
+
+  	_etext = . ;
+	PROVIDE (etext = .);
+
+	/* 0x00200000 ... */
+	.data 0x00200024: AT ( ADDR(.text) + SIZEOF(.text) - ADDR(.text) ) {
+  		_data = . ;
+  		* (.data)
+  		. = ALIGN(4);
+  	} >DATA
+
+  	_edata = . ;
+	PROVIDE (edata = .);
+	
+	/* .bss section which is used for uninitialized data */
+	.bss : {
+		__bss_start = . ;
+		__bss_start__ = . ;
+		*(.bss)
+		*(COMMON)
+	} >DATA
+
+	. = ALIGN(4);
+	__bss_end__ = . ;
+	__bss_end__ = . ;
+
+	PROVIDE (main = .);
+
+	_end = . ;
+
+	. = ALIGN(4);
+	.int_data : { 
+		*(.internal_ram_top) 
+   	} >STACK 
+
+	PROVIDE (end = .);
+
+  /* Stabs debugging sections.  */
+  .stab          0 : { *(.stab) }
+  .stabstr       0 : { *(.stabstr) }
+  .stab.excl     0 : { *(.stab.excl) }
+  .stab.exclstr  0 : { *(.stab.exclstr) }
+  .stab.index    0 : { *(.stab.index) }
+  .stab.indexstr 0 : { *(.stab.indexstr) }
+  .comment       0 : { *(.comment) }
+  /* DWARF debug sections.
+     Symbols in the DWARF debugging sections are relative to the beginning
+     of the section so we begin them at 0.  */
+  /* DWARF 1 */
+  .debug          0 : { *(.debug) }
+  .line           0 : { *(.line) }
+  /* GNU DWARF 1 extensions */
+  .debug_srcinfo  0 : { *(.debug_srcinfo) }
+  .debug_sfnames  0 : { *(.debug_sfnames) }
+  /* DWARF 1.1 and DWARF 2 */
+  .debug_aranges  0 : { *(.debug_aranges) }
+  .debug_pubnames 0 : { *(.debug_pubnames) }
+  /* DWARF 2 */
+  .debug_info     0 : { *(.debug_info .gnu.linkonce.wi.*) }
+  .debug_abbrev   0 : { *(.debug_abbrev) }
+  .debug_line     0 : { *(.debug_line) }
+  .debug_frame    0 : { *(.debug_frame) }
+  .debug_str      0 : { *(.debug_str) }
+  .debug_loc      0 : { *(.debug_loc) }
+  .debug_macinfo  0 : { *(.debug_macinfo) }
+  /* SGI/MIPS DWARF 2 extensions */
+  .debug_weaknames 0 : { *(.debug_weaknames) }
+  .debug_funcnames 0 : { *(.debug_funcnames) }
+  .debug_typenames 0 : { *(.debug_typenames) }
+  .debug_varnames  0 : { *(.debug_varnames) }
+
+}
diff --git a/firmware/link/AT91SAM7S256-ROM-sam7dfu-dfu.ld b/firmware/link/AT91SAM7S256-ROM-sam7dfu-dfu.ld
new file mode 100644
index 0000000..03cd6e0
--- /dev/null
+++ b/firmware/link/AT91SAM7S256-ROM-sam7dfu-dfu.ld
@@ -0,0 +1,109 @@
+/* Memory Definitions */
+
+MEMORY
+{
+  FLASH (rx) : ORIGIN = 0x00000000, LENGTH = 0x00040000
+  DATA (rw)  : ORIGIN = 0x00200000, LENGTH = 0x00010000
+  STACK (rw) : ORIGIN = 0x00210000, LENGTH = 0x00000000
+}
+
+
+/* Section Definitions */
+
+SECTIONS
+{
+	. = 0x00000000;
+	/* first section is .text which is used for code */
+	.text 0x00000000: AT ( 0x00000000 ) { 
+		* (.reset)
+		* (.text)
+		* (.rodata)
+  		. = ALIGN(4);
+  	} >FLASH
+
+  	_etext = . ;
+	PROVIDE (etext = .);
+
+	.data.shared (0x00200020): AT (LOADADDR(.text) + SIZEOF(.text)) {
+		* (.data.shared)
+		. = ALIGN(4);
+	} >DATA
+
+	/* 0x00200000 ... */
+	.data (0x00200000 + SIZEOF(.text)): AT ( LOADADDR(.text) + SIZEOF(.text) + SIZEOF(.data.shared)) {
+  		_data = . ;
+		* (.vectram)
+  		* (.data)
+  		. = ALIGN(4);
+  	} >DATA
+
+  	_edata = . ;
+	PROVIDE (edata = .);
+	
+	/* dfu.functab addresses are valid as long as DFU is in flash, i.e. only while
+	 * application calls those functions.  Once executing in DFU mode, we are
+	 * re-located to ram */
+	.dfu.functab 0x00000fd0: AT ( ADDR(.dfu.functab) - ADDR(.text) ) {
+  		*(.dfu.functab)
+  		. = ALIGN(4096);
+  	} >FLASH
+
+
+	/* .bss section which is used for uninitialized data */
+	.bss : {
+		__bss_start = . ;
+		__bss_start__ = . ;
+		*(.bss)
+		*(COMMON)
+	} >DATA
+
+	. = ALIGN(4);
+	__bss_end__ = . ;
+	__bss_end__ = . ;
+
+	PROVIDE (main = .);
+
+	_end = . ;
+
+	. = ALIGN(4);
+	.int_data : { 
+		*(.internal_ram_top) 
+   	} >STACK 
+
+	PROVIDE (end = .);
+
+  /* Stabs debugging sections.  */
+  .stab          0 : { *(.stab) }
+  .stabstr       0 : { *(.stabstr) }
+  .stab.excl     0 : { *(.stab.excl) }
+  .stab.exclstr  0 : { *(.stab.exclstr) }
+  .stab.index    0 : { *(.stab.index) }
+  .stab.indexstr 0 : { *(.stab.indexstr) }
+  .comment       0 : { *(.comment) }
+  /* DWARF debug sections.
+     Symbols in the DWARF debugging sections are relative to the beginning
+     of the section so we begin them at 0.  */
+  /* DWARF 1 */
+  .debug          0 : { *(.debug) }
+  .line           0 : { *(.line) }
+  /* GNU DWARF 1 extensions */
+  .debug_srcinfo  0 : { *(.debug_srcinfo) }
+  .debug_sfnames  0 : { *(.debug_sfnames) }
+  /* DWARF 1.1 and DWARF 2 */
+  .debug_aranges  0 : { *(.debug_aranges) }
+  .debug_pubnames 0 : { *(.debug_pubnames) }
+  /* DWARF 2 */
+  .debug_info     0 : { *(.debug_info .gnu.linkonce.wi.*) }
+  .debug_abbrev   0 : { *(.debug_abbrev) }
+  .debug_line     0 : { *(.debug_line) }
+  .debug_frame    0 : { *(.debug_frame) }
+  .debug_str      0 : { *(.debug_str) }
+  .debug_loc      0 : { *(.debug_loc) }
+  .debug_macinfo  0 : { *(.debug_macinfo) }
+  /* SGI/MIPS DWARF 2 extensions */
+  .debug_weaknames 0 : { *(.debug_weaknames) }
+  .debug_funcnames 0 : { *(.debug_funcnames) }
+  .debug_typenames 0 : { *(.debug_typenames) }
+  .debug_varnames  0 : { *(.debug_varnames) }
+
+}
diff --git a/firmware/src/os/dfu.c b/firmware/src/dfu/dfu.c
index f2bc0ca..f50b564 100644
--- a/firmware/src/os/dfu.c
+++ b/firmware/src/dfu/dfu.c
@@ -11,9 +11,8 @@
 #include <usb_dfu.h>
 #include <lib_AT91SAM7.h>
 
-#include <os/dfu.h>
+#include <dfu/dfu.h>
 #include <os/pcd_enumerate.h>
-#include <os/req_ctx.h>
 #include "../openpcd.h"
 
 /* If debug is enabled, we need to access debug functions from flash
@@ -29,7 +28,7 @@
 #endif
 
 
-void __dfufunc udp_init(void)
+static void __dfufunc udp_init(void)
 {
 	/* Set the PLL USB Divider */
 	AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
@@ -107,7 +106,7 @@ static void __dfufunc udp_ep0_send_stall(void)
 
 static u_int8_t status;
 static u_int8_t *ptr;
-static u_int8_t dfu_state;
+static __dfudata u_int32_t dfu_state;
 
 static int __dfufunc handle_dnload(u_int16_t val, u_int16_t len)
 {
@@ -149,7 +148,7 @@ static __dfufunc int handle_upload(u_int16_t val, u_int16_t len)
 {
 	DEBUGE("upload ");
 	if (len > AT91C_IFLASH_PAGE_SIZE
-	    || ptr > AT91C_IFLASH_SIZE) {
+	    || ptr > AT91C_IFLASH_SIZE - 0x1000) {
 		/* Too big */
 		dfu_state = DFU_STATE_dfuERROR;
 		status = DFU_STATUS_errADDRESS;
@@ -226,7 +225,7 @@ int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
 			handle_dnload(val, len);
 			break;
 		case USB_REQ_DFU_UPLOAD:
-			ptr = 0;
+			ptr = 0x00101000; /* Flash base address for app */
 			dfu_state = DFU_STATE_dfuUPLOAD_IDLE;
 			handle_upload(val, len);
 			break;
@@ -667,6 +666,7 @@ void __dfufunc dfu_main(void)
 }
 
 const struct dfuapi __dfufunctab dfu_api = {
+	.udp_init		= &udp_init,
 	.ep0_send_data		= &udp_ep0_send_data,
 	.ep0_send_zlp		= &udp_ep0_send_zlp,
 	.ep0_send_stall		= &udp_ep0_send_stall,
diff --git a/firmware/src/os/dfu.h b/firmware/src/dfu/dfu.h
index 8786044..992a4f2 100644
--- a/firmware/src/os/dfu.h
+++ b/firmware/src/dfu/dfu.h
@@ -13,8 +13,6 @@
 #include <usb_ch9.h>
 #include <usb_dfu.h>
 
-#include "dbgu.h"
-
 /* USB DFU functional descriptor */
 #define DFU_FUNC_DESC  {						\
 	.bLength		= USB_DT_DFU_SIZE,			\
@@ -38,9 +36,10 @@
 	.iInterface		= 1,					\
 }
 
-#define __dfufunc __attribute__ ((long_call, section (".dfu.func")))
-#define __dfustruct __attribute__ ((section (".dfu.struct"))) const
 #define __dfufunctab  __attribute__ ((section (".dfu.functab")))
+#define __dfudata __attribute__ ((section (".data.shared")))
+#define __dfufunc 
+#define __dfustruct const
 	
 #if 0
 extern void __dfufunc udp_ep0_send_data(const char *data, u_int32_t length);
@@ -55,9 +54,6 @@ extern static u_int8_t dfu_state;
 struct udp_pcd;
 #endif
 
-
-extern void __dfufunc udp_init(void);
-
 struct _dfu_desc {
 	struct usb_config_descriptor ucfg;
 	struct usb_interface_descriptor uif[2];
@@ -65,6 +61,7 @@ struct _dfu_desc {
 };
 
 struct dfuapi {
+	void (*udp_init)(void);
 	void (*ep0_send_data)(const char *data, u_int32_t len);
 	void (*ep0_send_zlp)(void);
 	void (*ep0_send_stall)(void);
diff --git a/firmware/src/os/main.c b/firmware/src/os/main.c
index 4b7a7a2..878fdbf 100644
--- a/firmware/src/os/main.c
+++ b/firmware/src/os/main.c
@@ -3,7 +3,6 @@
 #include <include/lib_AT91SAM7.h>
 #include <os/dbgu.h>
 #include <os/led.h>
-#include <os/dfu.h>
 #include <os/main.h>
 #include <os/power.h>
 #include <os/pcd_enumerate.h>
@@ -20,7 +19,6 @@ int main(void)
 	_init_func();
 
 	/* initialize USB */
-	udp_init();
 	udp_open();
 
 	// Enable User Reset and set its minimal assertion to 960 us
diff --git a/firmware/src/os/pcd_enumerate.c b/firmware/src/os/pcd_enumerate.c
index cded8c8..40bd88e 100644
--- a/firmware/src/os/pcd_enumerate.c
+++ b/firmware/src/os/pcd_enumerate.c
@@ -23,7 +23,7 @@
 
 #include <os/pcd_enumerate.h>
 #include <os/req_ctx.h>
-#include <os/dfu.h>
+#include <dfu/dfu.h>
 #include "../openpcd.h"
 #include <os/dbgu.h>
 
@@ -41,6 +41,7 @@
 #ifdef CONFIG_DFU
 #define DFU_API_LOCATION	((const struct dfuapi *) 0x00102100)
 static const struct dfuapi *dfu = DFU_API_LOCATION;
+#define udp_init		dfu->udp_init
 #define udp_ep0_send_data	dfu->ep0_send_data
 #define udp_ep0_send_zlp	dfu->ep0_send_zlp
 #define udp_ep0_send_stall	dfu->ep0_send_stall
@@ -328,6 +329,7 @@ static void udp_irq(void)
 void udp_open(void)
 {
 	DEBUGPCRF("entering");
+	udp_init();
 	upcd.pUdp = AT91C_BASE_UDP;
 	upcd.cur_config = 0;
 	upcd.cur_rcv_bank = AT91C_UDP_RX_DATA_BK0;
@@ -340,7 +342,6 @@ void udp_open(void)
 	/* End-of-Bus-Reset is always enabled */
 
 	/* Set the Pull up resistor */
-	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
 	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
 }
 
diff --git a/firmware/src/os/pcd_enumerate.h b/firmware/src/os/pcd_enumerate.h
index 57ff88c..813243e 100644
--- a/firmware/src/os/pcd_enumerate.h
+++ b/firmware/src/os/pcd_enumerate.h
@@ -5,7 +5,7 @@
 #include <sys/types.h>
 #include <asm/atomic.h>
 #include "openpcd.h"
-#include "dfu.h"
+#include <dfu/dfu.h>
 
 struct req_ctx;
 
diff --git a/firmware/src/start/Cstartup.S b/firmware/src/start/Cstartup.S
index e91fd86..e7d5714 100644
--- a/firmware/src/start/Cstartup.S
+++ b/firmware/src/start/Cstartup.S
@@ -76,165 +76,22 @@
 	.text
 	.arm
 	.section	.vectram, "ax"
-resetvecR:	B	resetvecR
-undefvecR:	B	undefvecR
-swivecR:	B	swivecR
-pabtvecR:	B	pabtvecR
-dabtvecR:	B	dabtvecR
-rsvdvecR:	B	rsvdvecR
-irqvecR:	B	IRQ_Handler_EntryR
-fiqvecR:	
 
-FIQ_Handler_EntryR:
-
-/*- Switch in SVC/User Mode to allow User Stack access for C code 	*/
-/* because the FIQ is not yet acknowledged*/
-
-/*- Save and r0 in FIQ_Register */
-            mov         r9,r0
-	        ldr         r0 , [r8, #AIC_FVR]
-            msr         CPSR_c,#I_BIT | F_BIT | ARM_MODE_SVC
-
-/*- Save scratch/used registers and LR in User Stack */
-            stmfd       sp!, { r1-r3, r12, lr}
-
-/*- Branch to the routine pointed by the AIC_FVR */
-            mov         r14, pc
-            bx          r0
-
-/*- Restore scratch/used registers and LR from User Stack */
-            ldmia       sp!, { r1-r3, r12, lr}
-
-/*- Leave Interrupts disabled and switch back in FIQ mode */
-            msr         CPSR_c, #I_BIT | F_BIT | ARM_MODE_FIQ
-
-/*- Restore the R0 ARM_MODE_SVC register */
-            mov         r0,r9
-
-/*- Restore the Program Counter using the LR_fiq directly in the PC */
-            subs        pc,lr,#4
-
-        .global IRQ_Handler_EntryR
-        .func   IRQ_Handler_EntryR
-
-IRQ_Handler_EntryR:
-
-/*- Manage Exception Entry  */
-/*- Adjust and save LR_irq in IRQ stack  */
-            sub         lr, lr, #4
-            stmfd       sp!, {lr}
-
-/*- Save SPSR need to be saved for nested interrupt */
-            mrs         r14, SPSR
-            stmfd       sp!, {r14}
-
-/*- Save and r0 in IRQ stack  */
-            stmfd       sp!, {r0}
-
-/*- Write in the IVR to support Protect Mode  */
-/*- No effect in Normal Mode  */
-/*- De-assert the NIRQ and clear the source in Protect Mode */
-            ldr         r14, =AT91C_BASE_AIC
-	    ldr         r0 , [r14, #AIC_IVR]
-	    str         r14, [r14, #AIC_IVR]
-
-/*- Enable Interrupt and Switch in Supervisor Mode */
-            msr         CPSR_c, #ARM_MODE_SVC
-
-/*- Save scratch/used registers and LR in User Stack */
-            stmfd       sp!, { r1-r3, r12, r14}
-
-/*- Branch to the routine pointed by the AIC_IVR  */
-            mov         r14, pc
-            bx          r0
-/*- Restore scratch/used registers and LR from User Stack*/
-            ldmia       sp!, { r1-r3, r12, r14}
-
-/*- Disable Interrupt and switch back in IRQ mode */
-            msr         CPSR_c, #I_BIT | ARM_MODE_IRQ
-
-/*- Mark the End of Interrupt on the AIC */
-            ldr         r14, =AT91C_BASE_AIC
-            str         r14, [r14, #AIC_EOICR]
-
-/*- Restore SPSR_irq and r0 from IRQ stack */
-            ldmia       sp!, {r0}
-
-/*- Restore SPSR_irq and r0 from IRQ stack */
-            ldmia       sp!, {r14}
-            msr         SPSR_cxsf, r14
-
-/*- Restore adjusted  LR_irq from IRQ stack directly in the PC */
-            ldmia       sp!, {pc}^
-	
-        .size   IRQ_Handler_EntryR, . - IRQ_Handler_EntryR
-        .endfunc
-
-	.global remap
-	.func	remap
-_remap:
-#		led1on
-# Remap RAM to 0x00000000 for DFU 
+	.global _remap_call_dfu
+	.func	_remap_call_dfu
+_remap_call_dfu:
+		led1on
+		/* Remap RAM to 0x00000000 for DFU */
 		ldr	r1, =AT91C_BASE_AIC
 		mov	r2, #0x01
 		str	r2, [r1, #AIC_MCR_RCR]
+		
+		ldr	r4, =dfu_main
+		bx	r4
 
-		/* prepare c function call to main */
-		mov	r0, #0	/* argc = 0 */
-		ldr	lr,=exit
-		ldr	r10,=main
-
-#ifdef CONFIG_DFU_SWITCH
-		/* check whether bootloader button is pressed */
-		ldr	r1, =AT91C_PMC_PCER
-		mov	r2, #(1 << AT91C_ID_PIOA)
-		str	r2, [r1] 
-
-		ldr 	r1, =AT91C_BASE_PIOA
-		ldr	r2, [r1, #PIOA_PDSR]
-		tst	r2, #PIO_BOOTLDR
-		ldrne	r10,=dfu_main
-#endif
-
-		bx	r10
-	.size	remap, . - remap
+	.size	_remap_call_dfu, . - _remap_call_dfu
 	.endfunc
 
-/* "exit" dummy added by mthomas to avoid sbrk write read etc. needed
-   by the newlib default "exit" */
-        .global exit
-        .func   exit
-exit:
-        b    .
-		.size   exit, . - exit
-        .endfunc
-
-/*---------------------------------------------------------------
-//* ?EXEPTION_VECTOR
-//* This module is only linked if needed for closing files.
-//*---------------------------------------------------------------*/
-        .global AT91F_Default_FIQ_handler
-        .func   AT91F_Default_FIQ_handler
-AT91F_Default_FIQ_handler:
-            b     AT91F_Default_FIQ_handler
-        .size   AT91F_Default_FIQ_handler, . - AT91F_Default_FIQ_handler
-        .endfunc
-
-        .global AT91F_Default_IRQ_handler
-        .func   AT91F_Default_IRQ_handler
-AT91F_Default_IRQ_handler:
-            b     AT91F_Default_IRQ_handler
-        .size   AT91F_Default_IRQ_handler, . - AT91F_Default_IRQ_handler
-        .endfunc
-
-        .global AT91F_Spurious_handler
-        .func   AT91F_Spurious_handler
-AT91F_Spurious_handler:
-            b     AT91F_Spurious_handler
-        .size   AT91F_Spurious_handler, . - AT91F_Spurious_handler
-        .endfunc
-
-
 
 #;------------------------------------------------------------------------------
 #;- Section Definition
@@ -255,9 +112,6 @@ Top_Stack:
 * .text is used instead of .section .text so it works with arm-aout too.  */
         .section 	.reset
         .text
-        .global _startup
-        .func   _startup
-_startup:
 reset: 
 /*------------------------------------------------------------------------------
 //*- Exception vectors 
@@ -277,17 +131,108 @@ swivec:
 pabtvec:
                 B           pabtvec             /* 0x0C Prefetch Abort */
 dabtvec:
-                B           dabtvec             /* 0x10 Data Abort */
+		b	dabtvec             /* 0x10 Data Abort */
 rsvdvec:
-                B           rsvdvec             /* 0x14 reserved  */
+		b	rsvdvec             /* 0x14 reserved  */
 irqvec:
-                B           irqvec		/* 0x18 IRQ	 */
-fiqvec:               		  
-		B	    fiqvec          	/* 0x1c FIQ	*/
+		b	IRQ_Handler_Entry	/* 0x18 IRQ */
+fiqvec:
+		b	FIQ_Handler_Entry
+						/* 0x1c FIQ */
+dfu_state_dummy:
+		.word	0
+		
+FIQ_Handler_Entry:
+
+/*- Switch in SVC/User Mode to allow User Stack access for C code 	*/
+/* because the FIQ is not yet acknowledged*/
+
+/*- Save and r0 in FIQ_Register */
+		mov	r9, r0
+		ldr	r0, [r8, #AIC_FVR]
+		msr	CPSR_c, #I_BIT | F_BIT | ARM_MODE_SVC
+
+		/*- Save scratch/used registers and LR in User Stack */
+		stmfd	sp!, { r1-r3, r12, lr}
+
+		/*- Branch to the routine pointed by the AIC_FVR */
+		mov	r14, pc
+		bx	r0
+
+		/*- Restore scratch/used registers and LR from User Stack */
+		ldmia	sp!, { r1-r3, r12, lr}
+
+		/*- Leave Interrupts disabled and switch back in FIQ mode */
+		msr	CPSR_c, #I_BIT | F_BIT | ARM_MODE_FIQ
+
+		/*- Restore the R0 ARM_MODE_SVC register */
+		mov	r0,r9
+
+		/*- Restore the Program Counter using the LR_fiq directly in the PC */
+		subs        pc, lr, #4
+
+	.global IRQ_Handler_Entry
+	.func   IRQ_Handler_Entry
+
+IRQ_Handler_Entry:
+
+		/*- Manage Exception Entry  */
+		/*- Adjust and save LR_irq in IRQ stack  */
+		sub         lr, lr, #4
+		stmfd       sp!, {lr}
+
+		/*- Save SPSR need to be saved for nested interrupt */
+		mrs	r14, SPSR
+		stmfd	sp!, {r14}
+
+		/*- Save and r0 in IRQ stack  */
+		stmfd	sp!, {r0}
+
+		/*- Write in the IVR to support Protect Mode  */
+		/*- No effect in Normal Mode  */
+		/*- De-assert the NIRQ and clear the source in Protect Mode */
+		ldr	r14, =AT91C_BASE_AIC
+		ldr	r0 , [r14, #AIC_IVR]
+		str	r14, [r14, #AIC_IVR]
+
+		/*- Enable Interrupt and Switch in Supervisor Mode */
+		msr	CPSR_c, #ARM_MODE_SVC
+
+		/*- Save scratch/used registers and LR in User Stack */
+		stmfd	sp!, { r1-r3, r12, r14}
+
+		/*- Branch to the routine pointed by the AIC_IVR  */
+		mov	r14, pc
+		bx	r0
+
+		/*- Restore scratch/used registers and LR from User Stack*/
+		ldmia	sp!, { r1-r3, r12, r14}
+
+		/*- Disable Interrupt and switch back in IRQ mode */
+		msr	CPSR_c, #I_BIT | ARM_MODE_IRQ
+
+		/*- Mark the End of Interrupt on the AIC */
+		ldr	r14, =AT91C_BASE_AIC
+		str	r14, [r14, #AIC_EOICR]
+
+		/*- Restore SPSR_irq and r0 from IRQ stack */
+		ldmia	sp!, {r0}
+
+		/*- Restore SPSR_irq and r0 from IRQ stack */
+		ldmia	sp!, {r14}
+		msr	SPSR_cxsf, r14
+
+		/*- Restore adjusted  LR_irq from IRQ stack directly in the PC */
+		ldmia	sp!, {pc}^
+
+	.size   IRQ_Handler_Entry, . - IRQ_Handler_Entry
+	.endfunc
 	.align 0
 .RAM_TOP:
 	.word	Top_Stack
 
+	.global _startup
+	.func _startup
 InitReset:
 /*------------------------------------------------------------------------------
 /*- Low level Init (PMC, AIC, ? ....) by C function AT91F_LowLevelInit
@@ -348,16 +293,7 @@ InitReset:
                 msr     CPSR_c, #ARM_MODE_SVC
                 mov     r13, r0                     /* Init stack Sup */
 
-/*- Relocate DFU .data section (Copy from ROM to RAM)*/
-		ldr	r1, =_etext_dfu
-		ldr	r2, =_data_dfu
-		ldr	r3, =_edata_dfu
-LoopRelDFU:	cmp	r2, r3
-		ldrlo	r0, [r1], #4
-		strlo	r0, [r2], #4
-                blo	LoopRelDFU
-
-# Relocate .data section (Copy from ROM to RAM)
+# Relocate DFU .data section (Copy from ROM to RAM)
                 LDR     R1, =_etext
                 LDR     R2, =_data
                 LDR     R3, =_edata
@@ -366,7 +302,7 @@ LoopRel:        CMP     R2, R3
                 STRLO   R0, [R2], #4
                 BLO     LoopRel
 
-# Clear .bss section (Zero init)
+# Clear DFU .bss section (Zero init)
                 MOV     R0, #0
                 LDR     R1, =__bss_start__
                 LDR     R2, =__bss_end__
@@ -374,13 +310,75 @@ LoopZI:         CMP     R1, R2
                 STRLO   R0, [R1], #4
                 BLO     LoopZI
 
-		led1on
+		/* prepare c function call to main */
+		mov	r0, #0	/* argc = 0 */
+		ldr	lr, =exit
+		ldr	r10, =0x00101000
 
-		ldr	r0, =_remap
-		bx	r0
+#ifdef CONFIG_DFU_SWITCH
+		/* check whether bootloader button is pressed */
+		ldr	r1, =AT91C_PMC_PCER
+		mov	r2, #(1 << AT91C_ID_PIOA)
+		str	r2, [r1] 
 
+		ldr 	r1, =AT91C_BASE_PIOA
+		ldr	r2, [r1, #PIOA_PDSR]
+		tst	r2, #PIO_BOOTLDR
+		bne	_reloc_dfu_text
+#endif
+
+		bx	r10
+
+_reloc_dfu_text:
+
+		ldr	r1, =0x00100000
+		ldr	r2, =0x00200000
+		ldr	r3, =_etext
+		add	r3, r3, r2
+loop_rel_t:	cmp	r2, r3
+		ldrlo	r4, [r1], #4
+		strlo	r4, [r2], #4
+		blo	loop_rel_t
+		ldr	r4, =_remap_call_dfu
+		bx	r4
+		
         .size   _startup, . - _startup
         .endfunc
-		
+
+/* "exit" dummy to avoid sbrk write read etc. needed by the newlib default "exit" */
+        .global exit
+        .func   exit
+exit:
+        b    .
+		.size   exit, . - exit
+        .endfunc
+
+/*---------------------------------------------------------------
+//* ?EXEPTION_VECTOR
+//* This module is only linked if needed for closing files.
+//*---------------------------------------------------------------*/
+        .global AT91F_Default_FIQ_handler
+        .func   AT91F_Default_FIQ_handler
+AT91F_Default_FIQ_handler:
+            b     AT91F_Default_FIQ_handler
+        .size   AT91F_Default_FIQ_handler, . - AT91F_Default_FIQ_handler
+        .endfunc
+
+        .global AT91F_Default_IRQ_handler
+        .func   AT91F_Default_IRQ_handler
+AT91F_Default_IRQ_handler:
+            b     AT91F_Default_IRQ_handler
+        .size   AT91F_Default_IRQ_handler, . - AT91F_Default_IRQ_handler
+        .endfunc
+
+        .global AT91F_Spurious_handler
+        .func   AT91F_Spurious_handler
+AT91F_Spurious_handler:
+            b     AT91F_Spurious_handler
+        .size   AT91F_Spurious_handler, . - AT91F_Spurious_handler
+        .endfunc
+
+
+
         .end
 
diff --git a/firmware/src/start/Cstartup_app.S b/firmware/src/start/Cstartup_app.S
new file mode 100644
index 0000000..4a2187a
--- /dev/null
+++ b/firmware/src/start/Cstartup_app.S
@@ -0,0 +1,81 @@
+/* Cstartup header for the application to be started by at91dfu 
+ * (C) 2006 by Harald Welte <hwelte@hmw-consulting.de> */
+
+//#define DEBUG_LL
+
+#ifdef DEBUG_LL
+/* Debugging macros for switching on/off LED1 (green) */
+#define PIOA_PER	0xFFFFF400
+#define PIOA_OER	0xFFFFF410
+#define PIOA_SODR	0xFFFFF430
+#define PIOA_CODR	0xFFFFF434
+#define LED1		25		/* this only works on OpenPICC, not Olimex */
+	.macro	led1on
+		ldr	r2, =PIOA_CODR
+		mov	r1, #(1 << LED1)
+		str	r1, [r2]
+	.endm
+	.macro	led1off
+		ldr	r2, =PIOA_SODR
+		mov	r1, #(1 << LED1)
+		str	r1, [r2]
+	.endm
+	.macro	ledinit
+		ldr	r2, =PIOA_PER
+		mov	r1, #(1 << LED1)
+		str	r1, [r2]
+		ldr	r2, =PIOA_OER
+		str	r1, [r2]
+		led1off
+	.endm
+#else
+	.macro ledinit
+	.endm
+	.macro led1on
+	.endm
+	.macro led1off
+	.endm
+#endif
+
+	.global	_startup
+	.func _startup
+_startup:
+	/* Relocate .data section (copy from Flash to RAM) */
+	ldr	r1, =_etext
+	ldr	r2, =_data
+	ldr	r3, =_edata
+loop_r:	cmp	r2, r3
+	ldrlo	r0, [r1], #4
+	strlo	r0, [r2], #4
+	blo	loop_r
+
+	/* Clear .bss section (Zero init) */
+	mov	r0, #0
+	ldr	r1, =__bss_start__
+	ldr	r2, =__bss_end__
+loop_z:	cmp	r1, r2
+	strlo	r0, [r1], #4
+	blo	loop_z
+
+	led1on
+
+	/* prepare C function call to main */
+	mov	r0, #0	/* argc = 0 */
+	ldr	lr, =exit
+	ldr	r10, =main
+
+	bx	r10
+
+        .size   _startup, . - _startup
+        .endfunc
+		
+/* "exit" dummy to avoid sbrk write read etc. needed by the newlib default "exit" */
+        .global exit
+        .func   exit
+exit:
+        b    .
+		.size   exit, . - exit
+        .endfunc
+
+        .end
+