major rework

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

35 files changed, 1679 insertions, 355 deletions

diff --git a/openpcd/firmware/Makefile b/openpcd/firmware/Makefile
index 48f247e..78c9716 100644
--- a/openpcd/firmware/Makefile
+++ b/openpcd/firmware/Makefile
@@ -63,7 +63,7 @@ PATH_TO_LINKSCRIPTS=compil/SrcWinARM/
 #VECTOR_LOCATION=VECTORS_IN_RAM
 
 # Target file name (without extension).
-TARGET = main
+TARGET:= main_dumbreader
 
 # List C source files here. (C dependencies are automatically generated.)
 # use file-extension c for "c-only"-files
@@ -71,7 +71,9 @@ SRC =
 
 # List C source files here which must be compiled in ARM-Mode.
 # use file-extension c for "c-only"-files
-SRCARM  = lib/lib_AT91SAM7.c src/pcd_enumerate.c src/fifo.c src/dbgu.c src/led.c src/rc632.c  src/req_ctx.c src/$(TARGET).c compil/SrcWinARM/Cstartup_SAM7.c 
+SRCARM  = lib/lib_AT91SAM7.c src/pcd_enumerate.c src/fifo.c src/dbgu.c \
+	  src/led.c src/rc632.c  src/req_ctx.c src/trigger.c src/main.c \
+	  src/$(TARGET).c compil/SrcWinARM/Cstartup_SAM7.c 
 
 # List C++ source files here.
 # use file-extension cpp for C++-files (use extension .cpp)
@@ -91,8 +93,10 @@ CPPSRCARM =
 # 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
+
 # List Assembler source files here which must be assembled in ARM-Mode..
-ASRCARM  = compil/SrcWinARM/Cstartup.S
+ASRCARM  = compil/SrcWinARM/Cstartup.S $(ASRCLIB)
 
 ## Output format. (can be ihex or binary)
 ## (binary i.e. for openocd and SAM-BA, hex i.e. for lpc21isp and uVision)
@@ -135,11 +139,11 @@ AT91LIBNOWARN = yes
 CSTANDARD = -std=gnu99
 
 # Place -D or -U options for C here
-CDEFS =  -D$(RUN_MODE) -DDEBUG 
-#CDEFS += -DOLIMEX
+CDEFS =  -D$(RUN_MODE) -D__MS_types__ -DDEBUG 
+CDEFS += -DOLIMEX
 
 # Place -I options here
-CINCS = -Ihelper -Isrc
+CINCS = -Iinclude -Isrc
 
 # Place -D or -U options for ASM here
 ADEFS =  -D$(RUN_MODE)
@@ -192,7 +196,7 @@ CPPFLAGS =
 #  -Wa,...:    tell GCC to pass this to the assembler.
 #  -ahlns:     create listing
 #  -g$(DEBUG): have the assembler create line number information
-ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:.S=.lst),--g$(DEBUG)
+ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:.S=.lst),--g$(DEBUG) -Iinclude/ -D__ASSEMBLY__
 
 
 #Additional libraries.
diff --git a/openpcd/firmware/README b/openpcd/firmware/README
new file mode 100644
index 0000000..15d3573
--- /dev/null
+++ b/openpcd/firmware/README
@@ -0,0 +1,6 @@
+Different Targets can be built by implementing a _init_func() and _main_func()_
+function in src/main_foo.c
+
+The resulting binary main_foo.bin can be built by issuing
+	make TARGET=main_foo
+
diff --git a/openpcd/firmware/include/asm/assembler.h b/openpcd/firmware/include/asm/assembler.h
new file mode 100644
index 0000000..b43f9d1
--- /dev/null
+++ b/openpcd/firmware/include/asm/assembler.h
@@ -0,0 +1,97 @@
+/*
+ *  linux/include/asm-arm/assembler.h
+ *
+ *  Copyright (C) 1996-2000 Russell King
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ *  This file contains arm architecture specific defines
+ *  for the different processors.
+ *
+ *  Do not include any C declarations in this file - it is included by
+ *  assembler source.
+ */
+#ifndef __ASSEMBLY__
+#error "Only include this from assembly code"
+#endif
+
+#include <asm/ptrace.h>
+
+#define pull            lsl
+#define push            lsr
+#define get_byte_0	lsr #24
+#define get_byte_1	lsr #16
+#define get_byte_2	lsr #8
+#define get_byte_3      lsl #0
+#define put_byte_0	lsl #24
+#define put_byte_1	lsl #16
+#define put_byte_2	lsl #8
+#define put_byte_3      lsl #0
+
+#define PLD(code...)
+
+#define MODE_USR	USR_MODE
+#define MODE_FIQ	FIQ_MODE
+#define MODE_IRQ	IRQ_MODE
+#define MODE_SVC	SVC_MODE
+
+#define DEFAULT_FIQ	MODE_FIQ
+
+/*
+ * LOADREGS - ldm with PC in register list (eg, ldmfd sp!, {pc})
+ */
+#ifdef __STDC__
+#define LOADREGS(cond, base, reglist...)\
+	ldm##cond	base,reglist
+#else
+#define LOADREGS(cond, base, reglist...)\
+	ldm/**/cond	base,reglist
+#endif
+
+/*
+ * Build a return instruction for this processor type.
+ */
+#define RETINSTR(instr, regs...)\
+	instr	regs
+
+/*
+ * Enable and disable interrupts
+ */
+	.macro	disable_irq
+	msr	cpsr_c, #PSR_I_BIT | SVC_MODE
+	.endm
+
+	.macro	enable_irq
+	msr	cpsr_c, #SVC_MODE
+	.endm
+
+/*
+ * Save the current IRQ state and disable IRQs.  Note that this macro
+ * assumes FIQs are enabled, and that the processor is in SVC mode.
+ */
+	.macro	save_and_disable_irqs, oldcpsr
+	mrs	\oldcpsr, cpsr
+	disable_irq
+	.endm
+
+/*
+ * Restore interrupt state previously stored in a register.  We don't
+ * guarantee that this will preserve the flags.
+ */
+	.macro	restore_irqs, oldcpsr
+	msr	cpsr_c, \oldcpsr
+	.endm
+
+/*
+ * These two are used to save LR/restore PC over a user-based access.
+ * The old 26-bit architecture requires that we do.  On 32-bit
+ * architecture, we can safely ignore this requirement.
+ */
+	.macro	save_lr
+	.endm
+
+	.macro	restore_pc
+	mov	pc, lr
+	.endm
diff --git a/openpcd/firmware/include/asm/atomic.h b/openpcd/firmware/include/asm/atomic.h
new file mode 100644
index 0000000..bb5a2b1
--- /dev/null
+++ b/openpcd/firmware/include/asm/atomic.h
@@ -0,0 +1,105 @@
+/*
+ *  linux/include/asm-arm/atomic.h
+ *
+ *  Copyright (C) 1996 Russell King.
+ *  Copyright (C) 2002 Deep Blue Solutions Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef __ASM_ARM_ATOMIC_H
+#define __ASM_ARM_ATOMIC_H
+
+typedef struct { volatile int counter; } atomic_t;
+
+#define ATOMIC_INIT(i)	{ (i) }
+
+#define atomic_read(v)	((v)->counter)
+
+#include <asm/system.h>
+
+#define atomic_set(v,i)	(((v)->counter) = (i))
+
+static inline int atomic_add_return(int i, atomic_t *v)
+{
+	unsigned long flags;
+	int val;
+
+	local_irq_save(flags);
+	val = v->counter;
+	v->counter = val += i;
+	local_irq_restore(flags);
+
+	return val;
+}
+
+static inline int atomic_sub_return(int i, atomic_t *v)
+{
+	unsigned long flags;
+	int val;
+
+	local_irq_save(flags);
+	val = v->counter;
+	v->counter = val -= i;
+	local_irq_restore(flags);
+
+	return val;
+}
+
+static inline int atomic_cmpxchg(atomic_t *v, int old, int new)
+{
+	int ret;
+	unsigned long flags;
+
+	local_irq_save(flags);
+	ret = v->counter;
+	if (likely(ret == old))
+		v->counter = new;
+	local_irq_restore(flags);
+
+	return ret;
+}
+
+static inline void atomic_clear_mask(unsigned long mask, unsigned long *addr)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+	*addr &= ~mask;
+	local_irq_restore(flags);
+}
+
+#define atomic_xchg(v, new) (xchg(&((v)->counter), new))
+
+static inline int atomic_add_unless(atomic_t *v, int a, int u)
+{
+	int c, old;
+
+	c = atomic_read(v);
+	while (c != u && (old = atomic_cmpxchg((v), c, c + a)) != c)
+		c = old;
+	return c != u;
+}
+#define atomic_inc_not_zero(v) atomic_add_unless((v), 1, 0)
+
+#define atomic_add(i, v)	(void) atomic_add_return(i, v)
+#define atomic_inc(v)		(void) atomic_add_return(1, v)
+#define atomic_sub(i, v)	(void) atomic_sub_return(i, v)
+#define atomic_dec(v)		(void) atomic_sub_return(1, v)
+
+#define atomic_inc_and_test(v)	(atomic_add_return(1, v) == 0)
+#define atomic_dec_and_test(v)	(atomic_sub_return(1, v) == 0)
+#define atomic_inc_return(v)    (atomic_add_return(1, v))
+#define atomic_dec_return(v)    (atomic_sub_return(1, v))
+#define atomic_sub_and_test(i, v) (atomic_sub_return(i, v) == 0)
+
+#define atomic_add_negative(i,v) (atomic_add_return(i, v) < 0)
+
+/* Atomic operations are already serializing on ARM */
+#define smp_mb__before_atomic_dec()	barrier()
+#define smp_mb__after_atomic_dec()	barrier()
+#define smp_mb__before_atomic_inc()	barrier()
+#define smp_mb__after_atomic_inc()	barrier()
+
+#endif
diff --git a/openpcd/firmware/include/asm/bitops.h b/openpcd/firmware/include/asm/bitops.h
new file mode 100644
index 0000000..337d800
--- /dev/null
+++ b/openpcd/firmware/include/asm/bitops.h
@@ -0,0 +1,225 @@
+/*
+ * Copyright 1995, Russell King.
+ * Various bits and pieces copyrights include:
+ *  Linus Torvalds (test_bit).
+ * Big endian support: Copyright 2001, Nicolas Pitre
+ *  reworked by rmk.
+ *
+ * bit 0 is the LSB of an "unsigned long" quantity.
+ *
+ * Please note that the code in this file should never be included
+ * from user space.  Many of these are not implemented in assembler
+ * since they would be too costly.  Also, they require privileged
+ * instructions (which are not available from user mode) to ensure
+ * that they are atomic.
+ */
+
+#ifndef __ASM_ARM_BITOPS_H
+#define __ASM_ARM_BITOPS_H
+
+#include <asm/system.h>
+
+#define smp_mb__before_clear_bit()	mb()
+#define smp_mb__after_clear_bit()	mb()
+
+/*
+ * These functions are the basis of our bit ops.
+ *
+ * First, the atomic bitops. These use native endian.
+ */
+static inline void ____atomic_set_bit(unsigned int bit, volatile unsigned long *p)
+{
+	unsigned long flags;
+	unsigned long mask = 1UL << (bit & 31);
+
+	p += bit >> 5;
+
+	local_irq_save(flags);
+	*p |= mask;
+	local_irq_restore(flags);
+}
+
+static inline void ____atomic_clear_bit(unsigned int bit, volatile unsigned long *p)
+{
+	unsigned long flags;
+	unsigned long mask = 1UL << (bit & 31);
+
+	p += bit >> 5;
+
+	local_irq_save(flags);
+	*p &= ~mask;
+	local_irq_restore(flags);
+}
+
+static inline void ____atomic_change_bit(unsigned int bit, volatile unsigned long *p)
+{
+	unsigned long flags;
+	unsigned long mask = 1UL << (bit & 31);
+
+	p += bit >> 5;
+
+	local_irq_save(flags);
+	*p ^= mask;
+	local_irq_restore(flags);
+}
+
+static inline int
+____atomic_test_and_set_bit(unsigned int bit, volatile unsigned long *p)
+{
+	unsigned long flags;
+	unsigned int res;
+	unsigned long mask = 1UL << (bit & 31);
+
+	p += bit >> 5;
+
+	local_irq_save(flags);
+	res = *p;
+	*p = res | mask;
+	local_irq_restore(flags);
+
+	return res & mask;
+}
+
+static inline int
+____atomic_test_and_clear_bit(unsigned int bit, volatile unsigned long *p)
+{
+	unsigned long flags;
+	unsigned int res;
+	unsigned long mask = 1UL << (bit & 31);
+
+	p += bit >> 5;
+
+	local_irq_save(flags);
+	res = *p;
+	*p = res & ~mask;
+	local_irq_restore(flags);
+
+	return res & mask;
+}
+
+static inline int
+____atomic_test_and_change_bit(unsigned int bit, volatile unsigned long *p)
+{
+	unsigned long flags;
+	unsigned int res;
+	unsigned long mask = 1UL << (bit & 31);
+
+	p += bit >> 5;
+
+	local_irq_save(flags);
+	res = *p;
+	*p = res ^ mask;
+	local_irq_restore(flags);
+
+	return res & mask;
+}
+
+//#include <asm-generic/bitops/non-atomic.h>
+
+/*
+ *  A note about Endian-ness.
+ *  -------------------------
+ *
+ * When the ARM is put into big endian mode via CR15, the processor
+ * merely swaps the order of bytes within words, thus:
+ *
+ *          ------------ physical data bus bits -----------
+ *          D31 ... D24  D23 ... D16  D15 ... D8  D7 ... D0
+ * little     byte 3       byte 2       byte 1      byte 0
+ * big        byte 0       byte 1       byte 2      byte 3
+ *
+ * This means that reading a 32-bit word at address 0 returns the same
+ * value irrespective of the endian mode bit.
+ *
+ * Peripheral devices should be connected with the data bus reversed in
+ * "Big Endian" mode.  ARM Application Note 61 is applicable, and is
+ * available from http://www.arm.com/.
+ *
+ * The following assumes that the data bus connectivity for big endian
+ * mode has been followed.
+ *
+ * Note that bit 0 is defined to be 32-bit word bit 0, not byte 0 bit 0.
+ */
+
+/*
+ * Little endian assembly bitops.  nr = 0 -> byte 0 bit 0.
+ */
+extern void _set_bit_le(int nr, volatile unsigned long * p);
+extern void _clear_bit_le(int nr, volatile unsigned long * p);
+extern void _change_bit_le(int nr, volatile unsigned long * p);
+extern int _test_and_set_bit_le(int nr, volatile unsigned long * p);
+extern int _test_and_clear_bit_le(int nr, volatile unsigned long * p);
+extern int _test_and_change_bit_le(int nr, volatile unsigned long * p);
+extern int _find_first_zero_bit_le(const void * p, unsigned size);
+extern int _find_next_zero_bit_le(const void * p, int size, int offset);
+extern int _find_first_bit_le(const unsigned long *p, unsigned size);
+extern int _find_next_bit_le(const unsigned long *p, int size, int offset);
+
+/*
+ * Big endian assembly bitops.  nr = 0 -> byte 3 bit 0.
+ */
+extern void _set_bit_be(int nr, volatile unsigned long * p);
+extern void _clear_bit_be(int nr, volatile unsigned long * p);
+extern void _change_bit_be(int nr, volatile unsigned long * p);
+extern int _test_and_set_bit_be(int nr, volatile unsigned long * p);
+extern int _test_and_clear_bit_be(int nr, volatile unsigned long * p);
+extern int _test_and_change_bit_be(int nr, volatile unsigned long * p);
+extern int _find_first_zero_bit_be(const void * p, unsigned size);
+extern int _find_next_zero_bit_be(const void * p, int size, int offset);
+extern int _find_first_bit_be(const unsigned long *p, unsigned size);
+extern int _find_next_bit_be(const unsigned long *p, int size, int offset);
+
+/*
+ * The __* form of bitops are non-atomic and may be reordered.
+ */
+#define	ATOMIC_BITOP_LE(name,nr,p)		\
+	(__builtin_constant_p(nr) ?		\
+	 ____atomic_##name(nr, p) :		\
+	 _##name##_le(nr,p))
+
+#define	ATOMIC_BITOP_BE(name,nr,p)		\
+	(__builtin_constant_p(nr) ?		\
+	 ____atomic_##name(nr, p) :		\
+	 _##name##_be(nr,p))
+
+#define NONATOMIC_BITOP(name,nr,p)		\
+	(____nonatomic_##name(nr, p))
+
+/*
+ * These are the little endian, atomic definitions.
+ */
+#define set_bit(nr,p)			ATOMIC_BITOP_LE(set_bit,nr,p)
+#define clear_bit(nr,p)			ATOMIC_BITOP_LE(clear_bit,nr,p)
+#define change_bit(nr,p)		ATOMIC_BITOP_LE(change_bit,nr,p)
+#define test_and_set_bit(nr,p)		ATOMIC_BITOP_LE(test_and_set_bit,nr,p)
+#define test_and_clear_bit(nr,p)	ATOMIC_BITOP_LE(test_and_clear_bit,nr,p)
+#define test_and_change_bit(nr,p)	ATOMIC_BITOP_LE(test_and_change_bit,nr,p)
+#define find_first_zero_bit(p,sz)	_find_first_zero_bit_le(p,sz)
+#define find_next_zero_bit(p,sz,off)	_find_next_zero_bit_le(p,sz,off)
+#define find_first_bit(p,sz)		_find_first_bit_le(p,sz)
+#define find_next_bit(p,sz,off)		_find_next_bit_le(p,sz,off)
+
+#define WORD_BITOFF_TO_LE(x)		((x))
+
+#if 0
+#include <asm-generic/bitops/ffz.h>
+#include <asm-generic/bitops/__ffs.h>
+#include <asm-generic/bitops/fls.h>
+#include <asm-generic/bitops/ffs.h>
+
+#include <asm-generic/bitops/fls64.h>
+
+#include <asm-generic/bitops/sched.h>
+#include <asm-generic/bitops/hweight.h>
+#endif
+
+#define BITS_PER_LONG		32
+#define BITOP_MASK(nr)          (1UL << ((nr) % BITS_PER_LONG))
+#define BITOP_WORD(nr)          ((nr) / BITS_PER_LONG)
+
+static inline int test_bit(int nr, const volatile unsigned long *addr)
+{
+	return 1UL & (addr[BITOP_WORD(nr)] >> (nr & (BITS_PER_LONG-1)));
+}
+
+#endif /* _ARM_BITOPS_H */
diff --git a/openpcd/firmware/include/asm/linkage.h b/openpcd/firmware/include/asm/linkage.h
new file mode 100644
index 0000000..ac1c900
--- /dev/null
+++ b/openpcd/firmware/include/asm/linkage.h
@@ -0,0 +1,18 @@
+#ifndef __ASM_LINKAGE_H
+#define __ASM_LINKAGE_H
+
+/* asm-arm/linkage.h */
+
+#define __ALIGN .align 0
+#define __ALIGN_STR ".align 0"
+
+/* linux/linkage.h */
+
+#define ALIGN __ALIGN
+
+#define ENTRY(name) \
+  .globl name; \
+  ALIGN; \
+  name:
+
+#endif
diff --git a/openpcd/firmware/include/asm/ptrace.h b/openpcd/firmware/include/asm/ptrace.h
new file mode 100644
index 0000000..f3a654e
--- /dev/null
+++ b/openpcd/firmware/include/asm/ptrace.h
@@ -0,0 +1,128 @@
+/*
+ *  linux/include/asm-arm/ptrace.h
+ *
+ *  Copyright (C) 1996-2003 Russell King
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef __ASM_ARM_PTRACE_H
+#define __ASM_ARM_PTRACE_H
+
+/*
+ * PSR bits
+ */
+#define USR26_MODE	0x00000000
+#define FIQ26_MODE	0x00000001
+#define IRQ26_MODE	0x00000002
+#define SVC26_MODE	0x00000003
+#define USR_MODE	0x00000010
+#define FIQ_MODE	0x00000011
+#define IRQ_MODE	0x00000012
+#define SVC_MODE	0x00000013
+#define ABT_MODE	0x00000017
+#define UND_MODE	0x0000001b
+#define SYSTEM_MODE	0x0000001f
+#define MODE32_BIT	0x00000010
+#define MODE_MASK	0x0000001f
+#define PSR_T_BIT	0x00000020
+#define PSR_F_BIT	0x00000040
+#define PSR_I_BIT	0x00000080
+#define PSR_J_BIT	0x01000000
+#define PSR_Q_BIT	0x08000000
+#define PSR_V_BIT	0x10000000
+#define PSR_C_BIT	0x20000000
+#define PSR_Z_BIT	0x40000000
+#define PSR_N_BIT	0x80000000
+#define PCMASK		0
+
+/*
+ * Groups of PSR bits
+ */
+#define PSR_f		0xff000000	/* Flags		*/
+#define PSR_s		0x00ff0000	/* Status		*/
+#define PSR_x		0x0000ff00	/* Extension		*/
+#define PSR_c		0x000000ff	/* Control		*/
+
+#ifndef __ASSEMBLY__
+
+/*
+ * This struct defines the way the registers are stored on the
+ * stack during a system call.  Note that sizeof(struct pt_regs)
+ * has to be a multiple of 8.
+ */
+struct pt_regs {
+	long uregs[18];
+};
+
+#define ARM_cpsr	uregs[16]
+#define ARM_pc		uregs[15]
+#define ARM_lr		uregs[14]
+#define ARM_sp		uregs[13]
+#define ARM_ip		uregs[12]
+#define ARM_fp		uregs[11]
+#define ARM_r10		uregs[10]
+#define ARM_r9		uregs[9]
+#define ARM_r8		uregs[8]
+#define ARM_r7		uregs[7]
+#define ARM_r6		uregs[6]
+#define ARM_r5		uregs[5]
+#define ARM_r4		uregs[4]
+#define ARM_r3		uregs[3]
+#define ARM_r2		uregs[2]
+#define ARM_r1		uregs[1]
+#define ARM_r0		uregs[0]
+#define ARM_ORIG_r0	uregs[17]
+
+#define user_mode(regs)	\
+	(((regs)->ARM_cpsr & 0xf) == 0)
+
+#ifdef CONFIG_ARM_THUMB
+#define thumb_mode(regs) \
+	(((regs)->ARM_cpsr & PSR_T_BIT))
+#else
+#define thumb_mode(regs) (0)
+#endif
+
+#define processor_mode(regs) \
+	((regs)->ARM_cpsr & MODE_MASK)
+
+#define interrupts_enabled(regs) \
+	(!((regs)->ARM_cpsr & PSR_I_BIT))
+
+#define fast_interrupts_enabled(regs) \
+	(!((regs)->ARM_cpsr & PSR_F_BIT))
+
+#define condition_codes(regs) \
+	((regs)->ARM_cpsr & (PSR_V_BIT|PSR_C_BIT|PSR_Z_BIT|PSR_N_BIT))
+	
+/* Are the current registers suitable for user mode?
+ * (used to maintain security in signal handlers)
+ */
+static inline int valid_user_regs(struct pt_regs *regs)
+{
+	if (user_mode(regs) &&
+	    (regs->ARM_cpsr & (PSR_F_BIT|PSR_I_BIT)) == 0)
+		return 1;
+
+	/*
+	 * Force CPSR to something logical...
+	 */
+	regs->ARM_cpsr &= PSR_f | PSR_s | PSR_x | PSR_T_BIT | MODE32_BIT;
+
+	return 0;
+}
+
+#define pc_pointer(v) \
+	((v) & ~PCMASK)
+
+#define instruction_pointer(regs) \
+	(pc_pointer((regs)->ARM_pc))
+
+#define profile_pc(regs) instruction_pointer(regs)
+
+#endif /* __ASSEMBLY__ */
+
+#endif
+
diff --git a/openpcd/firmware/include/asm/system.h b/openpcd/firmware/include/asm/system.h
new file mode 100644
index 0000000..07d4151
--- /dev/null
+++ b/openpcd/firmware/include/asm/system.h
@@ -0,0 +1,108 @@
+#ifndef __ASM_ARM_SYSTEM_H
+#define __ASM_ARM_SYSTEM_H
+
+/* Generic ARM7TDMI (ARMv4T) synchronisation primitives, mostly
+ * taken from Linux kernel source, licensed under GPL */
+
+#define local_irq_save(x)					\
+	({							\
+		unsigned long temp;				\
+		(void) (&temp == &x);				\
+	__asm__ __volatile__(					\
+	"mrs	%0, cpsr		@ local_irq_save\n"	\
+"	orr	%1, %0, #128\n"					\
+"	msr	cpsr_c, %1"					\
+	: "=r" (x), "=r" (temp)					\
+	:							\
+	: "memory", "cc");					\
+	})
+	
+/*
+ * Enable IRQs
+ */
+#define local_irq_enable()					\
+	({							\
+		unsigned long temp;				\
+	__asm__ __volatile__(					\
+	"mrs	%0, cpsr		@ local_irq_enable\n"	\
+"	bic	%0, %0, #128\n"					\
+"	msr	cpsr_c, %0"					\
+	: "=r" (temp)						\
+	:							\
+	: "memory", "cc");					\
+	})
+
+/*
+ * Disable IRQs
+ */
+#define local_irq_disable()					\
+	({							\
+		unsigned long temp;				\
+	__asm__ __volatile__(					\
+	"mrs	%0, cpsr		@ local_irq_disable\n"	\
+"	orr	%0, %0, #128\n"					\
+"	msr	cpsr_c, %0"					\
+	: "=r" (temp)						\
+	:							\
+	: "memory", "cc");					\
+	})
+
+/*
+ * Enable FIQs
+ */
+#define local_fiq_enable()					\
+	({							\
+		unsigned long temp;				\
+	__asm__ __volatile__(					\
+	"mrs	%0, cpsr		@ stf\n"		\
+"	bic	%0, %0, #64\n"					\
+"	msr	cpsr_c, %0"					\
+	: "=r" (temp)						\
+	:							\
+	: "memory", "cc");					\
+	})
+
+/*
+ * Disable FIQs
+ */
+#define local_fiq_disable()					\
+	({							\
+		unsigned long temp;				\
+	__asm__ __volatile__(					\
+	"mrs	%0, cpsr		@ clf\n"		\
+"	orr	%0, %0, #64\n"					\
+"	msr	cpsr_c, %0"					\
+	: "=r" (temp)						\
+	:							\
+	: "memory", "cc");					\
+	})
+
+/*
+ * Save the current interrupt enable state.
+ */
+#define local_save_flags(x)					\
+	({							\
+	__asm__ __volatile__(					\
+	"mrs	%0, cpsr		@ local_save_flags"	\
+	: "=r" (x) : : "memory", "cc");				\
+	})
+
+/*
+ * restore saved IRQ & FIQ state
+ */
+#define local_irq_restore(x)					\
+	__asm__ __volatile__(					\
+	"msr	cpsr_c, %0		@ local_irq_restore\n"	\
+	:							\
+	: "r" (x)						\
+	: "memory", "cc")
+
+#define irqs_disabled()			\
+({					\
+	unsigned long flags;		\
+	local_save_flags(flags);	\
+	(int)(flags & PSR_I_BIT);	\
+})
+
+
+#endif
diff --git a/openpcd/firmware/include/lib_AT91SAM7.h b/openpcd/firmware/include/lib_AT91SAM7.h
index 49f00bd..f43aaec 100644
--- a/openpcd/firmware/include/lib_AT91SAM7.h
+++ b/openpcd/firmware/include/lib_AT91SAM7.h
@@ -169,7 +169,7 @@ extern void AT91F_AIC_Open(
 //*----------------------------------------------------------------------------
 static inline void AT91F_PDC_SetNextRx (
 	AT91PS_PDC pPDC,     // \arg pointer to a PDC controller
-	char *address,       // \arg address to the next bloc to be received
+	unsigned char *address,// \arg address to the next bloc to be received
 	unsigned int bytes)  // \arg number of bytes to be received
 {
 	pPDC->PDC_RNPR = (unsigned int) address;
@@ -182,7 +182,7 @@ static inline void AT91F_PDC_SetNextRx (
 //*----------------------------------------------------------------------------
 static inline void AT91F_PDC_SetNextTx (
 	AT91PS_PDC pPDC,       // \arg pointer to a PDC controller
-	char *address,         // \arg address to the next bloc to be transmitted
+	const unsigned char *address,// \arg address to the next bloc to be transmitted
 	unsigned int bytes)    // \arg number of bytes to be transmitted
 {
 	pPDC->PDC_TNPR = (unsigned int) address;
@@ -195,7 +195,7 @@ static inline void AT91F_PDC_SetNextTx (
 //*----------------------------------------------------------------------------
 static inline void AT91F_PDC_SetRx (
 	AT91PS_PDC pPDC,       // \arg pointer to a PDC controller
-	char *address,         // \arg address to the next bloc to be received
+	unsigned char *address,// \arg address to the next bloc to be received
 	unsigned int bytes)    // \arg number of bytes to be received
 {
 	pPDC->PDC_RPR = (unsigned int) address;
@@ -208,7 +208,7 @@ static inline void AT91F_PDC_SetRx (
 //*----------------------------------------------------------------------------
 static inline void AT91F_PDC_SetTx (
 	AT91PS_PDC pPDC,       // \arg pointer to a PDC controller
-	char *address,         // \arg address to the next bloc to be transmitted
+	const unsigned char *address,// \arg address to the next bloc to be transmitted
 	unsigned int bytes)    // \arg number of bytes to be transmitted
 {
 	pPDC->PDC_TPR = (unsigned int) address;
@@ -313,9 +313,9 @@ extern void AT91F_PDC_Close(AT91PS_PDC pPDC);       // \arg pointer to a PDC con
 //*----------------------------------------------------------------------------
 extern unsigned int AT91F_PDC_SendFrame(
 	AT91PS_PDC pPDC,
-	char *pBuffer,
+	const unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	const unsigned char *pNextBuffer,
 	unsigned int szNextBuffer);
 
 //*----------------------------------------------------------------------------
@@ -324,9 +324,9 @@ extern unsigned int AT91F_PDC_SendFrame(
 //*----------------------------------------------------------------------------
 extern unsigned int AT91F_PDC_ReceiveFrame (
 	AT91PS_PDC pPDC,
-	char *pBuffer,
+	unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	unsigned char *pNextBuffer,
 	unsigned int szNextBuffer);
 
 /* *****************************************************************************
@@ -1701,9 +1701,9 @@ static inline void AT91F_SPI_CfgPCS (
 //*----------------------------------------------------------------------------
 static inline unsigned int AT91F_SPI_ReceiveFrame (
 	AT91PS_SPI pSPI,
-	char *pBuffer,
+	unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	unsigned char *pNextBuffer,
 	unsigned int szNextBuffer )
 {
 	return AT91F_PDC_ReceiveFrame(
@@ -1720,9 +1720,9 @@ static inline unsigned int AT91F_SPI_ReceiveFrame (
 //*----------------------------------------------------------------------------
 static inline unsigned int AT91F_SPI_SendFrame(
 	AT91PS_SPI pSPI,
-	char *pBuffer,
+	const unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	const unsigned char *pNextBuffer,
 	unsigned int szNextBuffer )
 {
 	return AT91F_PDC_SendFrame(
@@ -2168,9 +2168,9 @@ static inline void AT91F_SSC_DisableIt (
 //*----------------------------------------------------------------------------
 static inline unsigned int AT91F_SSC_ReceiveFrame (
 	AT91PS_SSC pSSC,
-	char *pBuffer,
+	unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	unsigned char *pNextBuffer,
 	unsigned int szNextBuffer )
 {
 	return AT91F_PDC_ReceiveFrame(
@@ -2187,9 +2187,9 @@ static inline unsigned int AT91F_SSC_ReceiveFrame (
 //*----------------------------------------------------------------------------
 static inline unsigned int AT91F_SSC_SendFrame(
 	AT91PS_SSC pSSC,
-	char *pBuffer,
+	const unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	const unsigned char *pNextBuffer,
 	unsigned int szNextBuffer )
 {
 	return AT91F_PDC_SendFrame(
@@ -2480,9 +2480,9 @@ static inline int AT91F_US_GetChar (
 //*----------------------------------------------------------------------------
 static inline unsigned int AT91F_US_SendFrame(
 	AT91PS_USART pUSART,
-	char *pBuffer,
+	const unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	const unsigned char *pNextBuffer,
 	unsigned int szNextBuffer )
 {
 	return AT91F_PDC_SendFrame(
@@ -2499,9 +2499,9 @@ static inline unsigned int AT91F_US_SendFrame(
 //*----------------------------------------------------------------------------
 static inline unsigned int AT91F_US_ReceiveFrame (
 	AT91PS_USART pUSART,
-	char *pBuffer,
+	unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	unsigned char *pNextBuffer,
 	unsigned int szNextBuffer )
 {
 	return AT91F_PDC_ReceiveFrame(
diff --git a/openpcd/firmware/include/openpcd.h b/openpcd/firmware/include/openpcd.h
index 81d2ad1..6596717 100644
--- a/openpcd/firmware/include/openpcd.h
+++ b/openpcd/firmware/include/openpcd.h
@@ -3,7 +3,7 @@
 
 /* This header file describes the USB protocol of the OpenPCD RFID reader */
 
-#include <include/types.h>
+#include <sys/types.h>
 
 struct openpcd_hdr {
 	u_int8_t cmd;		/* command */
diff --git a/openpcd/firmware/include/types.h b/openpcd/firmware/include/sys/types.h
index f17ffe5..f17ffe5 100644
--- a/openpcd/firmware/include/types.h
+++ b/openpcd/firmware/include/sys/types.h
diff --git a/openpcd/firmware/include/usb_ch9.h b/openpcd/firmware/include/usb_ch9.h
index b10c8ae..82edf61 100644
--- a/openpcd/firmware/include/usb_ch9.h
+++ b/openpcd/firmware/include/usb_ch9.h
@@ -19,7 +19,7 @@
 #ifndef __LINUX_USB_CH9_H
 #define __LINUX_USB_CH9_H
 
-#include <include/types.h>
+#include <sys/types.h>
 
 /*-------------------------------------------------------------------------*/
 
diff --git a/openpcd/firmware/lib/changebit.S b/openpcd/firmware/lib/changebit.S
new file mode 100644
index 0000000..7c709fb
--- /dev/null
+++ b/openpcd/firmware/lib/changebit.S
@@ -0,0 +1,21 @@
+/*
+ *  linux/arch/arm/lib/changebit.S
+ *
+ *  Copyright (C) 1995-1996 Russell King
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <asm/linkage.h>
+#include <asm/assembler.h>
+#include "bitops.h"
+                .text
+
+/* Purpose  : Function to change a bit
+ * Prototype: int change_bit(int bit, void *addr)
+ */
+ENTRY(_change_bit_be)
+		eor	r0, r0, #0x18		@ big endian byte ordering
+ENTRY(_change_bit_le)
+	bitop	eor
diff --git a/openpcd/firmware/lib/clearbit.S b/openpcd/firmware/lib/clearbit.S
new file mode 100644
index 0000000..cb48f7a
--- /dev/null
+++ b/openpcd/firmware/lib/clearbit.S
@@ -0,0 +1,22 @@
+/*
+ *  linux/arch/arm/lib/clearbit.S
+ *
+ *  Copyright (C) 1995-1996 Russell King
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <asm/linkage.h>
+#include <asm/assembler.h>
+#include "bitops.h"
+                .text
+
+/*
+ * Purpose  : Function to clear a bit
+ * Prototype: int clear_bit(int bit, void *addr)
+ */
+ENTRY(_clear_bit_be)
+		eor	r0, r0, #0x18		@ big endian byte ordering
+ENTRY(_clear_bit_le)
+	bitop	bic
diff --git a/openpcd/firmware/lib/lib_AT91SAM7.c b/openpcd/firmware/lib/lib_AT91SAM7.c
index f3e49d4..950761f 100644
--- a/openpcd/firmware/lib/lib_AT91SAM7.c
+++ b/openpcd/firmware/lib/lib_AT91SAM7.c
@@ -17,6 +17,7 @@
 //* Generated           : AT91 SW Application Group  08/30/2005 (15:52:59)
 //*
 
+#include <sys/types.h>
 #include <include/AT91SAM7.h>
 #include <include/lib_AT91SAM7.h>
 
@@ -107,10 +108,10 @@ void AT91F_PDC_Open(AT91PS_PDC pPDC)       // \arg pointer to a PDC controller
 	AT91F_PDC_DisableTx(pPDC);
 
 	//* Reset all Counter register Next buffer first
-	AT91F_PDC_SetNextTx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetNextRx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetTx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetRx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetNextTx(pPDC, NULL, 0);
+	AT91F_PDC_SetNextRx(pPDC, NULL, 0);
+	AT91F_PDC_SetTx(pPDC, NULL, 0);
+	AT91F_PDC_SetRx(pPDC, NULL, 0);
 
     //* Enable the RX and TX PDC transfer requests
 	AT91F_PDC_EnableRx(pPDC);
@@ -128,10 +129,10 @@ void AT91F_PDC_Close(AT91PS_PDC pPDC)       // \arg pointer to a PDC controller
 	AT91F_PDC_DisableTx(pPDC);
 
 	//* Reset all Counter register Next buffer first
-	AT91F_PDC_SetNextTx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetNextRx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetTx(pPDC, (char *) 0, 0);
-	AT91F_PDC_SetRx(pPDC, (char *) 0, 0);
+	AT91F_PDC_SetNextTx(pPDC, NULL, 0);
+	AT91F_PDC_SetNextRx(pPDC, NULL, 0);
+	AT91F_PDC_SetTx(pPDC, NULL, 0);
+	AT91F_PDC_SetRx(pPDC, NULL, 0);
 
 }
 
@@ -141,9 +142,9 @@ void AT91F_PDC_Close(AT91PS_PDC pPDC)       // \arg pointer to a PDC controller
 //*----------------------------------------------------------------------------
 unsigned int AT91F_PDC_SendFrame(
 	AT91PS_PDC pPDC,
-	char *pBuffer,
+	const unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	const unsigned char *pNextBuffer,
 	unsigned int szNextBuffer )
 {
 	if (AT91F_PDC_IsTxEmpty(pPDC)) {
@@ -169,9 +170,9 @@ unsigned int AT91F_PDC_SendFrame(
 //*----------------------------------------------------------------------------
 unsigned int AT91F_PDC_ReceiveFrame (
 	AT91PS_PDC pPDC,
-	char *pBuffer,
+	unsigned char *pBuffer,
 	unsigned int szBuffer,
-	char *pNextBuffer,
+	unsigned char *pNextBuffer,
 	unsigned int szNextBuffer )
 {
 	if (AT91F_PDC_IsRxEmpty(pPDC)) {
diff --git a/openpcd/firmware/lib/setbit.S b/openpcd/firmware/lib/setbit.S
new file mode 100644
index 0000000..9009bc1
--- /dev/null
+++ b/openpcd/firmware/lib/setbit.S
@@ -0,0 +1,22 @@
+/*
+ *  linux/arch/arm/lib/setbit.S
+ *
+ *  Copyright (C) 1995-1996 Russell King
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <asm/linkage.h>
+#include <asm/assembler.h>
+#include "bitops.h"
+		.text
+
+/*
+ * Purpose  : Function to set a bit
+ * Prototype: int set_bit(int bit, void *addr)
+ */
+ENTRY(_set_bit_be)
+		eor	r0, r0, #0x18		@ big endian byte ordering
+ENTRY(_set_bit_le)
+	bitop	orr
diff --git a/openpcd/firmware/lib/testchangebit.S b/openpcd/firmware/lib/testchangebit.S
new file mode 100644
index 0000000..37c303e
--- /dev/null
+++ b/openpcd/firmware/lib/testchangebit.S
@@ -0,0 +1,18 @@
+/*
+ *  linux/arch/arm/lib/testchangebit.S
+ *
+ *  Copyright (C) 1995-1996 Russell King
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <asm/linkage.h>
+#include <asm/assembler.h>
+#include "bitops.h"
+                .text
+
+ENTRY(_test_and_change_bit_be)
+		eor	r0, r0, #0x18		@ big endian byte ordering
+ENTRY(_test_and_change_bit_le)
+	testop	eor, strb
diff --git a/openpcd/firmware/lib/testclearbit.S b/openpcd/firmware/lib/testclearbit.S
new file mode 100644
index 0000000..985c399
--- /dev/null
+++ b/openpcd/firmware/lib/testclearbit.S
@@ -0,0 +1,18 @@
+/*
+ *  linux/arch/arm/lib/testclearbit.S
+ *
+ *  Copyright (C) 1995-1996 Russell King
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <asm/linkage.h>
+#include <asm/assembler.h>
+#include "bitops.h"
+                .text
+
+ENTRY(_test_and_clear_bit_be)
+		eor	r0, r0, #0x18		@ big endian byte ordering
+ENTRY(_test_and_clear_bit_le)
+	testop	bicne, strneb
diff --git a/openpcd/firmware/lib/testsetbit.S b/openpcd/firmware/lib/testsetbit.S
new file mode 100644
index 0000000..4a8a164
--- /dev/null
+++ b/openpcd/firmware/lib/testsetbit.S
@@ -0,0 +1,18 @@
+/*
+ *  linux/arch/arm/lib/testsetbit.S
+ *
+ *  Copyright (C) 1995-1996 Russell King
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <asm/linkage.h>
+#include <asm/assembler.h>
+#include "bitops.h"
+                .text
+
+ENTRY(_test_and_set_bit_be)
+		eor	r0, r0, #0x18		@ big endian byte ordering
+ENTRY(_test_and_set_bit_le)
+	testop	orreq, streqb
diff --git a/openpcd/firmware/src/dbgu.c b/openpcd/firmware/src/dbgu.c
index 1624795..1d54b41 100644
--- a/openpcd/firmware/src/dbgu.c
+++ b/openpcd/firmware/src/dbgu.c
@@ -1,26 +1,24 @@
-//*----------------------------------------------------------------------------
-//*         ATMEL Microcontroller Software Support  -  ROUSSET  -
-//*----------------------------------------------------------------------------
-//* The software is delivered "AS IS" without warranty or condition of any
-//* kind, either express, implied or statutory. This includes without
-//* limitation any warranty or condition with respect to merchantability or
-//* fitness for any particular purpose, or against the infringements of
-//* intellectual property rights of others.
-//*----------------------------------------------------------------------------
-//* File Name           : Debug.c
-//* Object              : Debug menu
-//* Creation            : JPP   14/Sep/2004
-//* 1.1 29/Aug/05 JPP   : Update AIC definion
-//*----------------------------------------------------------------------------
-
-// modified for WinARM example (remove scanf-function) 
-// by Martin Thomas
+/*----------------------------------------------------------------------------
+ *         ATMEL Microcontroller Software Support  -  ROUSSET  -
+ *----------------------------------------------------------------------------
+ * The software is delivered "AS IS" without warranty or condition of any
+ * kind, either express, implied or statutory. This includes without
+ * limitation any warranty or condition with respect to merchantability or
+ * fitness for any particular purpose, or against the infringements of
+ * intellectual property rights of others.
+ *----------------------------------------------------------------------------
+ * File Name           : Debug.c
+ * Object              : Debug menu
+ * Creation            : JPP   14/Sep/2004
+ * 1.1 29/Aug/05 JPP   : Update AIC definion
+ *----------------------------------------------------------------------------*/
 
 // Include Standard files
 #include "Board.h"
 #include "dbgu.h"
 #include "rc632.h"
 #include "openpcd.h"
+#include "led.h"
 #define USART_SYS_LEVEL 4
 /*---------------------------- Global Variable ------------------------------*/
 //*--------------------------1--------------------------------------------------
@@ -62,10 +60,10 @@ static void DBGU_irq_handler(void)
 	case '0':		//* info
 		AT91F_DBGU_Frame("Set Pull up\n\r");
 		// Set
-		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, AT91C_PIO_UDP_PUP);
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
 		break;
 	case '1':		//* info
-		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, AT91C_PIO_UDP_PUP);
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
 		AT91F_DBGU_Printk("Clear Pull up\n\r");
 		// Reset Application
 		Send_reset();
@@ -92,8 +90,8 @@ static void DBGU_irq_handler(void)
 
 		break;
 	case '6':
-		DEBUGPCR("Writing RC632 Reg RxWait: 0x%02x",
-			  rc632_reg_write(RC632_REG_RX_WAIT, 0x55));
+		DEBUGPCR("Writing RC632 Reg RxWait: 0x55");
+		rc632_reg_write(RC632_REG_RX_WAIT, 0x55);
 		break;
 	case '7':
 		rc632_dump();
@@ -162,7 +160,8 @@ void AT91F_DBGU_Frame(char *buffer)
 
 	for (len = 0; buffer[len] != '\0'; len++) {
 	}
-	AT91F_US_SendFrame((AT91PS_USART) AT91C_BASE_DBGU, buffer, len, 0, 0);
+	AT91F_US_SendFrame((AT91PS_USART) AT91C_BASE_DBGU, 
+			   (unsigned char *)buffer, len, 0, 0);
 
 }
 
@@ -233,8 +232,9 @@ void debugp(const char *format, ...)
 	vsnprintf(dbg_buf, sizeof(dbg_buf)-1, format, ap);
 	va_end(ap);
 
-	dbg_buf[sizeof(dbg_buf)-1] = '\0';				\
-	AT91F_DBGU_Printk(dbg_buf);					\
+	dbg_buf[sizeof(dbg_buf)-1] = '\0';			
+	//AT91F_DBGU_Frame(dbg_buf);
+	AT91F_DBGU_Printk(dbg_buf);
 }
 #endif
 
diff --git a/openpcd/firmware/src/fifo.c b/openpcd/firmware/src/fifo.c
index 5bb0015..b0ec152 100644
--- a/openpcd/firmware/src/fifo.c
+++ b/openpcd/firmware/src/fifo.c
@@ -42,8 +42,6 @@ void fifo_check_raise_int(struct fifo *fifo)
 
 u_int16_t fifo_data_put(struct fifo *fifo, u_int16_t len, u_int8_t *data)
 {
-	u_int16_t old_producer = fifo->producer;
-
 	if (len > fifo_available(fifo)) {
 		len = fifo_available(fifo);
 		fifo->irq |= FIFO_IRQ_OFLOW;
@@ -51,16 +49,16 @@ u_int16_t fifo_data_put(struct fifo *fifo, u_int16_t len, u_int8_t *data)
 
 	if (len + fifo->producer <= fifo->size) {
 		/* easy case */
-		memcpy(fifo->data[fifo->producer], data, len);
+		memcpy(&fifo->data[fifo->producer], data, len);
 		fifo->producer += len;
 	} else {
 		/* difficult: wrap around */
 		u_int16_t chunk_len;
 
 		chunk_len = fifo->size - fifo->producer;
-		memcpy(fifo->data[fifo->producer], data, chunk_len);
+		memcpy(&fifo->data[fifo->producer], data, chunk_len);
 
-		memcpy(fifo->data[0], data + chunk_len, len - chunk_len);
+		memcpy(&fifo->data[0], data + chunk_len, len - chunk_len);
 		fifo->producer = len - chunk_len;
 	}
 
@@ -79,12 +77,12 @@ u_int16_t fifo_data_get(struct fifo *fifo, u_int16_t len, u_int8_t *data)
 
 	if (fifo->producer > fifo->consumer) {
 		/* easy case */
-		memcpy(data, fifo->data[fifo->consumer], len);
+		memcpy(data, &fifo->data[fifo->consumer], len);
 	} else {
 		/* difficult case: wrap */
 		u_int16_t chunk_len = fifo->size - fifo->consumer;
-		memcpy(data, fifo->data[fifo->consumer], chunk_len);
-		memcpy(data+chunk_len, fifo->data[0], len - chunk_len);
+		memcpy(data, &fifo->data[fifo->consumer], chunk_len);
+		memcpy(data+chunk_len, &fifo->data[0], len - chunk_len);
 	}
 
 	fifo_check_water(fifo);
diff --git a/openpcd/firmware/src/fifo.h b/openpcd/firmware/src/fifo.h
index 324dad6..d91c6c2 100644
--- a/openpcd/firmware/src/fifo.h
+++ b/openpcd/firmware/src/fifo.h
@@ -1,7 +1,7 @@
 #ifndef _FIFO_H
 #define _FIFO_H
 
-#include <include/types.h>
+#include <sys/types.h>
 
 #define FIFO_SIZE	1024
 
diff --git a/openpcd/firmware/src/led.c b/openpcd/firmware/src/led.c
index 83e7ce7..b3dcfc7 100644
--- a/openpcd/firmware/src/led.c
+++ b/openpcd/firmware/src/led.c
@@ -1,5 +1,5 @@
 
-#include <include/types.h>
+#include <sys/types.h>
 #include <include/lib_AT91SAM7.h>
 #include "openpcd.h"
 #include "dbgu.h"
@@ -7,11 +7,11 @@
 static int led2port(int led)
 {
 	if (led == 1)
-		return OPENPCD_LED1;
+		return OPENPCD_PIO_LED1;
 	else if (led == 2)
-		return OPENPCD_LED2;
+		return OPENPCD_PIO_LED2;
 	else
-		return -1;
+		return 0;
 }
 
 void led_switch(int led, int on)
@@ -47,12 +47,14 @@ int led_toggle(int led)
 		led_switch(led, 0);
 	else
 		led_switch(led, 1);
+	
+	return !on;
 }
 
 void led_init(void)
 {
-	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_LED1);
-	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_LED2);
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_LED1);
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_LED2);
 	led_switch(1, 0);
 	led_switch(2, 0);
 }
diff --git a/openpcd/firmware/src/main.c b/openpcd/firmware/src/main.c
index 9d53ed1..384bad9 100644
--- a/openpcd/firmware/src/main.c
+++ b/openpcd/firmware/src/main.c
@@ -1,170 +1,42 @@
-//*--------------------------------------------------------------------------------------
-//*      ATMEL Microcontroller Software Support  -  ROUSSET  -
-//*--------------------------------------------------------------------------------------
-//* The software is delivered "AS IS" without warranty or condition of any
-//* kind, either express, implied or statutory. This includes without
-//* limitation any warranty or condition with respect to merchantability or
-//* fitness for any particular purpose, or against the infringements of
-//* intellectual property rights of others.
-//*--------------------------------------------------------------------------------------
-//* File Name           : main.c
-//* Object              : Testr USB device
-//* Translator          :
-//* 1.0 19/03/01 HI     : Creation
-//* 1.1 02/10/02 FB     : Add on Svc DataFlash
-//* 1.2 13/Sep/04 JPP   : Add DBGU
-//* 1.3 16/Dec/04 JPP   : Add USART and enable reset
-//* 1.4 27/Apr/05 JPP   : Unset the USART_COM and suppress displaying data
-//*--------------------------------------------------------------------------------------
-
 #include <errno.h>
 #include <string.h>
 #include <include/lib_AT91SAM7.h>
-#include <include/openpcd.h>
 #include "dbgu.h"
-#include "rc632.h"
 #include "led.h"
-#include "pcd_enumerate.h"
 #include "openpcd.h"
 
-#define MSG_SIZE 				1000
-#if 0
-#define USART_COM
-
-#if defined(__WinARM__) && !defined(UART_COM)
-#warning "make sure syscalls.c is added to the source-file list (see makefile)"
-#endif
-#endif
-
-//*----------------------------------------------------------------------------
-static void AT91F_USB_Open(void)
-{
-	// Set the PLL USB Divider
-	AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
-
-	// Specific Chip USB Initialisation
-	// Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock
-	AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_UDP;
-	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_UDP);
-
-	// Enable UDP PullUp (USB_DP_PUP) : enable & Clear of the corresponding PIO
-	// Set in PIO mode and Configure in Output
-	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, AT91C_PIO_UDP_PUP);
-	// Clear for set the Pul up resistor
-	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, AT91C_PIO_UDP_PUP);
-
-	// CDC Open by structure initialization
-	AT91F_CDC_Open(AT91C_BASE_UDP);
-}
-
-static int usb_in(struct req_ctx *rctx)
-{
-	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
-	struct openpcd_hdr *pih = (struct openpcd_hdr *) &rctx->tx.data[0];
-	u_int16_t len = rctx->rx.tot_len;
-
-	DEBUGP("usb_in ");
-
-	if (len < sizeof(*poh))
-		return -EINVAL;
-
-	//data_len = ntohs(poh->len);
-
-	memcpy(pih, poh, sizeof(*poh));
-	rctx->tx.tot_len = sizeof(*poh);
-
-	switch (poh->cmd) {
-	case OPENPCD_CMD_READ_REG:
-		DEBUGP("READ REG(0x%02x) ", poh->reg);
-		pih->val = rc632_reg_read(poh->reg);
-		break;
-	case OPENPCD_CMD_READ_FIFO:
-		DEBUGP("READ FIFO(len=%u) ", poh->val);
-		pih->len = rc632_fifo_read(poh->val, pih->data);
-		rctx->tx.tot_len += pih->len;
-		break;
-	case OPENPCD_CMD_WRITE_REG:
-		DEBUGP("WRITE_REG(0x%02x, 0x%02x) ", poh->reg, poh->val);
-		rc632_reg_write(poh->reg, poh->val);
-		break;
-	case OPENPCD_CMD_WRITE_FIFO:
-		DEBUGP("WRITE FIFO(len=%u) ", poh->len);
-		if (len - sizeof(*poh) < poh->len)
-			return -EINVAL;
-		rc632_fifo_write(poh->len, poh->data);
-		break;
-	case OPENPCD_CMD_READ_VFIFO:
-		DEBUGP("READ VFIFO ");
-		DEBUGP("NOT IMPLEMENTED YET ");
-		break;
-	case OPENPCD_CMD_WRITE_VFIFO:
-		DEBUGP("WRITE VFIFO ");
-		DEBUGP("NOT IMPLEMENTED YET ");
-		break;
-	case OPENPCD_CMD_REG_BITS_CLEAR:
-		DEBUGP("CLEAR BITS ");
-		pih->val = rc632_clear_bits(poh->reg, poh->val);
-		break;
-	case OPENPCD_CMD_REG_BITS_SET:
-		DEBUGP("SET BITS ");
-		pih->val = rc632_set_bits(poh->reg, poh->val);
-		break;
-	case OPENPCD_CMD_SET_LED:
-		DEBUGP("SET LED(%u,%u) ", poh->reg, poh->val);
-		led_switch(poh->reg, poh->val);
-		break;
-	default:
-		return -EINVAL;
-	}
-	DEBUGPCRF("calling UDP_Write");
-	AT91F_UDP_Write(0, &rctx->tx.data[0], rctx->tx.tot_len);
-	DEBUGPCRF("usb_in: returning to main");
-}
-
 #define DEBUG_TOGGLE_LED
 
 int main(void)
 {
-	int i;
-
+	/* initialize LED and debug unit */
 	led_init();
-	// Init trace DBGU
 	AT91F_DBGU_Init();
-	rc632_init();
-	
+
+	/* call application specific init function */
+	_init_func();
+
 	// Enable User Reset and set its minimal assertion to 960 us
 	AT91C_BASE_RSTC->RSTC_RMR =
 	    AT91C_RSTC_URSTEN | (0x4 << 8) | (unsigned int)(0xA5 << 24);
 
-	// Init USB device
-	AT91F_USB_Open();
-
 #ifdef DEBUG_CLOCK_PA6
 	AT91F_PMC_EnablePCK(AT91C_BASE_PMC, 0, AT91C_PMC_CSS_PLL_CLK);
 	AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, 0, AT91C_PA6_PCK0);
 #endif
 
+	/* switch on first led */
 	led_switch(1, 1);
 
-	// Init USB device
+	DEBUGPCRF("entering main (idle) loop");
 	while (1) {
-		struct req_ctx *rctx;
-
 #ifdef DEBUG_TOGGLE_LED
 		/* toggle LEDs */
 		led_toggle(1);
 		led_toggle(2);
 #endif
 
-		for (rctx = req_ctx_find_busy(); rctx; 
-		     rctx = req_ctx_find_busy()) {
-		     	DEBUGPCRF("found used ctx %u: len=%u", 
-				req_ctx_num(rctx), rctx->rx.tot_len);
-			usb_in(rctx);
-			req_ctx_put(rctx);
-		}
-
-		/* busy-wait for led toggle */
-		for (i = 0; i < 0x2fffff; i++) {}
+		/* Call application specific main idle function */
+		_main_func();
 	}
 }
diff --git a/openpcd/firmware/src/main_dumbreader.c b/openpcd/firmware/src/main_dumbreader.c
new file mode 100644
index 0000000..43801f6
--- /dev/null
+++ b/openpcd/firmware/src/main_dumbreader.c
@@ -0,0 +1,94 @@
+#include <errno.h>
+#include <string.h>
+#include <include/lib_AT91SAM7.h>
+#include <include/openpcd.h>
+#include "dbgu.h"
+#include "rc632.h"
+#include "led.h"
+#include "pcd_enumerate.h"
+#include "openpcd.h"
+
+static int usb_in(struct req_ctx *rctx)
+{
+	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
+	struct openpcd_hdr *pih = (struct openpcd_hdr *) &rctx->tx.data[0];
+	u_int16_t len = rctx->rx.tot_len;
+
+	DEBUGP("usb_in ");
+
+	if (len < sizeof(*poh))
+		return -EINVAL;
+
+	//data_len = ntohs(poh->len);
+
+	memcpy(pih, poh, sizeof(*poh));
+	rctx->tx.tot_len = sizeof(*poh);
+
+	switch (poh->cmd) {
+	case OPENPCD_CMD_READ_REG:
+		DEBUGP("READ REG(0x%02x) ", poh->reg);
+		pih->val = rc632_reg_read(poh->reg);
+		break;
+	case OPENPCD_CMD_READ_FIFO:
+		DEBUGP("READ FIFO(len=%u) ", poh->val);
+		pih->len = rc632_fifo_read(poh->val, pih->data);
+		rctx->tx.tot_len += pih->len;
+		break;
+	case OPENPCD_CMD_WRITE_REG:
+		DEBUGP("WRITE_REG(0x%02x, 0x%02x) ", poh->reg, poh->val);
+		rc632_reg_write(poh->reg, poh->val);
+		break;
+	case OPENPCD_CMD_WRITE_FIFO:
+		DEBUGP("WRITE FIFO(len=%u) ", poh->len);
+		if (len - sizeof(*poh) < poh->len)
+			return -EINVAL;
+		rc632_fifo_write(poh->len, poh->data);
+		break;
+	case OPENPCD_CMD_READ_VFIFO:
+		DEBUGP("READ VFIFO ");
+		DEBUGP("NOT IMPLEMENTED YET ");
+		break;
+	case OPENPCD_CMD_WRITE_VFIFO:
+		DEBUGP("WRITE VFIFO ");
+		DEBUGP("NOT IMPLEMENTED YET ");
+		break;
+	case OPENPCD_CMD_REG_BITS_CLEAR:
+		DEBUGP("CLEAR BITS ");
+		pih->val = rc632_clear_bits(poh->reg, poh->val);
+		break;
+	case OPENPCD_CMD_REG_BITS_SET:
+		DEBUGP("SET BITS ");
+		pih->val = rc632_set_bits(poh->reg, poh->val);
+		break;
+	case OPENPCD_CMD_SET_LED:
+		DEBUGP("SET LED(%u,%u) ", poh->reg, poh->val);
+		led_switch(poh->reg, poh->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	DEBUGPCRF("calling UDP_Write");
+	AT91F_UDP_Write(0, &rctx->tx.data[0], rctx->tx.tot_len);
+	DEBUGPCRF("usb_in: returning to main");
+	
+	return 0;
+}
+
+void _init_func(void)
+{
+	rc632_init();
+	udp_init();
+}
+
+void _main_func(void)
+{
+	struct req_ctx *rctx;
+
+	for (rctx = req_ctx_find_busy(); rctx; 
+	     rctx = req_ctx_find_busy()) {
+	     	DEBUGPCRF("found used ctx %u: len=%u", 
+			req_ctx_num(rctx), rctx->rx.tot_len);
+		usb_in(rctx);
+		req_ctx_put(rctx);
+	}
+}
diff --git a/openpcd/firmware/src/main_reqa.c b/openpcd/firmware/src/main_reqa.c
new file mode 100644
index 0000000..8d0b170
--- /dev/null
+++ b/openpcd/firmware/src/main_reqa.c
@@ -0,0 +1,25 @@
+#include "rc632_highlevel.h"
+#include "rc632.h"
+#include "dbgu.h"
+#include "trigger.h"
+
+void _init_func(void)
+{
+	rc632_init();
+	udp_init();
+	trigger_init();
+}
+
+
+void _main_func(void)
+{
+	struct iso14443a_atqa atqa;
+
+	memset(&atqa, 0, sizeof(atqua));
+
+	trigger_pulse();
+
+	if (rc632_iso14443a_transceive_sf(ISO14443A_SF_CMD_WUPA, &atqa)	< 0)
+		DEBUGPCRF("error during transceive_sf");
+	
+}
diff --git a/openpcd/firmware/src/openpcd.h b/openpcd/firmware/src/openpcd.h
index ddd3370..3ba2477 100644
--- a/openpcd/firmware/src/openpcd.h
+++ b/openpcd/firmware/src/openpcd.h
@@ -2,18 +2,22 @@
 #define _OPENPCD_H
 
 #ifdef OLIMEX
-#define OPENPCD_LED1	AT91C_PIO_PA18
-#define OPENPCD_LED2	AT91C_PIO_PA17
-//#define AT91C_PIO_UDP_PUP	AT91C_PIO_PA25
-#define AT91C_PIO_UDP_PUP	AT91C_PIO_PA16
+#define OPENPCD_PIO_LED2	AT91C_PIO_PA17
+#define OPENPCD_PIO_LED1	AT91C_PIO_PA18
+#define OPENPCD_PIO_UDP_PUP	AT91C_PIO_PA25
 #else
-#define OPENPCD_LED1	AT91C_PIO_PA25
-#define OPENPCD_LED2	AT91C_PIO_PA26
-#define AT91C_PIO_UDP_PUP	AT91C_PIO_PA16
+#define OPENPCD_PIO_UDP_CNX	AT91C_PIO_PA15
+#define OPENPCD_PIO_UDP_PUP	AT91C_PIO_PA16
+#define OPENPCD_PIO_LED1	AT91C_PIO_PA25
+#define OPENPCD_PIO_LED2	AT91C_PIO_PA26
 #endif
 
-#define OPENPCD_RC632_IRQ	AT91C_ID_IRQ1
-#define OPENPCD_RC632_RESET	AT91C_PIO_PA29
+#define OPENPCD_IRQ_RC632	AT91C_ID_IRQ1
+
+#define OPENPCD_PIO_MFIN	AT91C_PIO_PA9
+#define OPENPCD_PIO_MFOUT	AT91C_PIO_PA10
+#define OPENPCD_PIO_RC632_RESET	AT91C_PIO_PA29
+#define OPENPCD_PIO_TRIGGER	AT91C_PIO_PA31
 
 #define OPENPCD_IRQ_PRIO_SPI	AT91C_AIC_PRIOR_HIGHEST
 #define OPENPCD_IRQ_PRIO_UDP	(AT91C_AIC_PRIOR_LOWEST+1)
@@ -25,7 +29,7 @@
 #define req_buf_payload(x)	(x->data[x->hdr_len])
 #define req_buf_hdr(x)		(x->data[0])
 
-#include <include/types.h>
+#include <sys/types.h>
 
 struct req_buf {
 	u_int16_t hdr_len;
@@ -44,7 +48,11 @@ struct req_ctx {
 
 #define NUM_REQ_CTX	8
 extern struct req_ctx *req_ctx_find_get(void);
+extern struct req_ctx *req_ctx_find_busy(void);
 extern void req_ctx_put(struct req_ctx *ctx);
 extern u_int8_t req_ctx_num(struct req_ctx *ctx);
 
+extern void _init_func(void);
+extern void _main_func(void);
+
 #endif /* _OPENPCD_H */
diff --git a/openpcd/firmware/src/pcd_enumerate.c b/openpcd/firmware/src/pcd_enumerate.c
index 64c915c..ad18e29 100644
--- a/openpcd/firmware/src/pcd_enumerate.c
+++ b/openpcd/firmware/src/pcd_enumerate.c
@@ -18,7 +18,7 @@
 // 12. Apr. 2006: added modification found in the mikrocontroller.net gcc-Forum 
 //                additional line marked with /* +++ */
 
-#include <include/types.h>
+#include <sys/types.h>
 #include <include/usb_ch9.h>
 #include <include/lib_AT91SAM7.h>
 #include <include/openpcd.h>
@@ -127,12 +127,7 @@ const struct _desc cfgDescriptor = {
 #define STD_SET_INTERFACE             0x0B01
 #define STD_SYNCH_FRAME               0x0C82
 
-/// mt u_int32_t currentReceiveBank = AT91C_UDP_RX_DATA_BK0;
-
-static u_int32_t AT91F_UDP_Read(char *pData, u_int32_t length);
-static void AT91F_CDC_Enumerate(void);
-
-static char rcv_data[64];
+static void udp_ep0_handler(void);
 
 static void udp_irq(void)
 {
@@ -160,7 +155,7 @@ static void udp_irq(void)
 
 	if (isr & AT91C_UDP_EPINT0) {
 		DEBUGP("EP0INT(Control) ");
-		AT91F_CDC_Enumerate();
+		udp_ep0_handler();
 	}
 	if (isr & AT91C_UDP_EPINT1) {
 		u_int32_t cur_rcv_bank = pCDC.currentRcvBank;
@@ -236,12 +231,10 @@ static void udp_irq(void)
 	DEBUGP("END\r\n");
 }
 
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_CDC_Open
-//* \brief
-//*----------------------------------------------------------------------------
-AT91PS_CDC AT91F_CDC_Open(AT91PS_UDP pUdp)
+/* Open USB Device Port  */
+static AT91PS_CDC AT91F_PCD_Open(AT91PS_UDP pUdp)
 {
+	DEBUGPCRF("entering");
 	pCdc->pUdp = pUdp;
 	pCdc->currentConfiguration = 0;
 	pCdc->currentConnection = 0;
@@ -254,58 +247,38 @@ AT91PS_CDC AT91F_CDC_Open(AT91PS_UDP pUdp)
 
 	/* End-of-Bus-Reset is always enabled */
 
+	DEBUGPCRF("returning after enabling UDP irq");
+
 	return pCdc;
 }
 
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_UDP_IsConfigured
-//* \brief Test if the device is configured and handle enumeration
-//*----------------------------------------------------------------------------
-u_int8_t AT91F_UDP_IsConfigured(void)
+void udp_init(void)
 {
-	return pCdc->currentConfiguration;
-}
+	/* Set the PLL USB Divider */
+	AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
 
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_UDP_Read
-//* \brief Read available data from Endpoint OUT
-//*----------------------------------------------------------------------------
-static u_int32_t AT91F_UDP_Read(char *pData, u_int32_t length)
-{
-	AT91PS_UDP pUdp = pCdc->pUdp;
-	u_int32_t packetSize, nbBytesRcv = 0, currentReceiveBank =
-	    pCdc->currentRcvBank;
+	/* Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock */
+	AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_UDP;
+	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_UDP);
 
-	while (length) {
-		if (!AT91F_UDP_IsConfigured())
-			break;
-		if (pUdp->UDP_CSR[AT91C_EP_OUT] & currentReceiveBank) {
-			packetSize =
-			    MIN(pUdp->UDP_CSR[AT91C_EP_OUT] >> 16, length);
-			length -= packetSize;
-			if (packetSize < AT91C_EP_OUT_SIZE)
-				length = 0;
-			while (packetSize--)
-				pData[nbBytesRcv++] =
-				    pUdp->UDP_FDR[AT91C_EP_OUT];
-			pUdp->UDP_CSR[AT91C_EP_OUT] &= ~(currentReceiveBank);
-			if (currentReceiveBank == AT91C_UDP_RX_DATA_BK0)
-				currentReceiveBank = AT91C_UDP_RX_DATA_BK1;
-			else
-				currentReceiveBank = AT91C_UDP_RX_DATA_BK0;
+	/* Enable UDP PullUp (USB_DP_PUP) : enable & Clear of the corresponding PIO
+	 * Set in PIO mode and Configure in Output */
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+	/* Clear for set the Pul up resistor */
+	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
 
-		}
-	}
-	pCdc->currentRcvBank = currentReceiveBank;
-	return nbBytesRcv;
+	/* CDC Open by structure initialization */
+	AT91F_PCD_Open(AT91C_BASE_UDP);
+}
 
+/* Test if the device is configured and handle enumeration */
+u_int8_t udp_is_configured(void)
+{
+	return pCdc->currentConfiguration;
 }
 
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_CDC_Write
-//* \brief Send through endpoint 2/3
-//*----------------------------------------------------------------------------
-u_int32_t AT91F_UDP_Write(u_int8_t irq, const char *pData, u_int32_t length)
+/* Send data through endpoint 2/3 */
+u_int32_t AT91F_UDP_Write(u_int8_t irq, const unsigned char *pData, u_int32_t length)
 {
 	AT91PS_UDP pUdp = pCdc->pUdp;
 	u_int32_t cpt = 0;
@@ -336,7 +309,7 @@ u_int32_t AT91F_UDP_Write(u_int8_t irq, const char *pData, u_int32_t length)
 		DEBUGPCRF("waiting for end of further packet");
 		// Wait for the the first bank to be sent
 		while (!(pUdp->UDP_CSR[ep] & AT91C_UDP_TXCOMP))
-			if (!AT91F_UDP_IsConfigured()) {
+			if (!udp_is_configured()) {
 				DEBUGPCRF("return(!configured)");
 				return length;
 			}
@@ -347,7 +320,7 @@ u_int32_t AT91F_UDP_Write(u_int8_t irq, const char *pData, u_int32_t length)
 	// Wait for the end of transfer
 	DEBUGPCRF("waiting for end of transfer");
 	while (!(pUdp->UDP_CSR[ep] & AT91C_UDP_TXCOMP))
-		if (!AT91F_UDP_IsConfigured()) {
+		if (!udp_is_configured()) {
 			DEBUGPCRF("return(!configured)");
 			return length;
 		}
@@ -358,14 +331,8 @@ u_int32_t AT91F_UDP_Write(u_int8_t irq, const char *pData, u_int32_t length)
 	return length;
 }
 
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_USB_SendData
-//* \brief Send Data through the control endpoint
-//*----------------------------------------------------------------------------
-unsigned int csrTab[100];
-unsigned char csrIdx = 0;
-
-static void AT91F_USB_SendData(AT91PS_UDP pUdp, const char *pData, u_int32_t length)
+/* Send Data through the control endpoint */
+static void udp_ep0_send_data(AT91PS_UDP pUdp, const char *pData, u_int32_t length)
 {
 	u_int32_t cpt = 0;
 	AT91_REG csr;
@@ -404,11 +371,8 @@ static void AT91F_USB_SendData(AT91PS_UDP pUdp, const char *pData, u_int32_t len
 	}
 }
 
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_USB_SendZlp
-//* \brief Send zero length packet through the control endpoint
-//*----------------------------------------------------------------------------
-void AT91F_USB_SendZlp(AT91PS_UDP pUdp)
+/* Send zero length packet through the control endpoint */
+static void udp_ep0_send_zlp(AT91PS_UDP pUdp)
 {
 	pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
 	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP)) ;
@@ -416,11 +380,8 @@ void AT91F_USB_SendZlp(AT91PS_UDP pUdp)
 	while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
 }
 
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_USB_SendStall
-//* \brief Stall the control endpoint
-//*----------------------------------------------------------------------------
-void AT91F_USB_SendStall(AT91PS_UDP pUdp)
+/* Stall the control endpoint */
+static void udp_ep0_send_stall(AT91PS_UDP pUdp)
 {
 	pUdp->UDP_CSR[0] |= AT91C_UDP_FORCESTALL;
 	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_ISOERROR)) ;
@@ -428,11 +389,8 @@ void AT91F_USB_SendStall(AT91PS_UDP pUdp)
 	while (pUdp->UDP_CSR[0] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR)) ;
 }
 
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_CDC_Enumerate
-//* \brief This function is a callback invoked when a SETUP packet is received
-//*----------------------------------------------------------------------------
-static void AT91F_CDC_Enumerate(void)
+/* Handle requests on the USB Control Endpoint */
+static void udp_ep0_handler(void)
 {
 	AT91PS_UDP pUDP = pCdc->pUdp;
 	u_int8_t bmRequestType, bRequest;
@@ -464,17 +422,17 @@ static void AT91F_CDC_Enumerate(void)
 	case STD_GET_DESCRIPTOR:
 		DEBUGP("GET_DESCRIPTOR ");
 		if (wValue == 0x100)	// Return Device Descriptor
-			AT91F_USB_SendData(pUDP, (const char *) &devDescriptor,
+			udp_ep0_send_data(pUDP, (const char *) &devDescriptor,
 					   MIN(sizeof(devDescriptor), wLength));
 		else if (wValue == 0x200)	// Return Configuration Descriptor
-			AT91F_USB_SendData(pUDP, (const char *) &cfgDescriptor,
+			udp_ep0_send_data(pUDP, (const char *) &cfgDescriptor,
 					   MIN(sizeof(cfgDescriptor), wLength));
 		else
-			AT91F_USB_SendStall(pUDP);
+			udp_ep0_send_stall(pUDP);
 		break;
 	case STD_SET_ADDRESS:
 		DEBUGP("SET_ADDRESS ");
-		AT91F_USB_SendZlp(pUDP);
+		udp_ep0_send_zlp(pUDP);
 		pUDP->UDP_FADDR = (AT91C_UDP_FEN | wValue);
 		pUDP->UDP_GLBSTATE = (wValue) ? AT91C_UDP_FADDEN : 0;
 		break;
@@ -483,7 +441,7 @@ static void AT91F_CDC_Enumerate(void)
 		if (wValue)
 			DEBUGP("VALUE!=0 ");
 		pCdc->currentConfiguration = wValue;
-		AT91F_USB_SendZlp(pUDP);
+		udp_ep0_send_zlp(pUDP);
 		pUDP->UDP_GLBSTATE =
 		    (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN;
 		pUDP->UDP_CSR[1] =
@@ -498,18 +456,18 @@ static void AT91F_CDC_Enumerate(void)
 		break;
 	case STD_GET_CONFIGURATION:
 		DEBUGP("GET_CONFIG ");
-		AT91F_USB_SendData(pUDP, (char *)&(pCdc->currentConfiguration),
+		udp_ep0_send_data(pUDP, (char *)&(pCdc->currentConfiguration),
 				   sizeof(pCdc->currentConfiguration));
 		break;
 	case STD_GET_STATUS_ZERO:
 		DEBUGP("GET_STATUS_ZERO ");
 		wStatus = 0;
-		AT91F_USB_SendData(pUDP, (char *)&wStatus, sizeof(wStatus));
+		udp_ep0_send_data(pUDP, (char *)&wStatus, sizeof(wStatus));
 		break;
 	case STD_GET_STATUS_INTERFACE:
 		DEBUGP("GET_STATUS_INTERFACE ");
 		wStatus = 0;
-		AT91F_USB_SendData(pUDP, (char *)&wStatus, sizeof(wStatus));
+		udp_ep0_send_data(pUDP, (char *)&wStatus, sizeof(wStatus));
 		break;
 	case STD_GET_STATUS_ENDPOINT:
 		DEBUGP("GET_STATUS_ENDPOINT(EPidx=%u) ", wIndex&0x0f);
@@ -518,42 +476,42 @@ static void AT91F_CDC_Enumerate(void)
 		if ((pUDP->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex <= 3)) {
 			wStatus =
 			    (pUDP->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
-			AT91F_USB_SendData(pUDP, (char *)&wStatus,
+			udp_ep0_send_data(pUDP, (char *)&wStatus,
 					   sizeof(wStatus));
 		} else if ((pUDP->UDP_GLBSTATE & AT91C_UDP_FADDEN)
 			   && (wIndex == 0)) {
 			wStatus =
 			    (pUDP->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
-			AT91F_USB_SendData(pUDP, (char *)&wStatus,
+			udp_ep0_send_data(pUDP, (char *)&wStatus,
 					   sizeof(wStatus));
 		} else
-			AT91F_USB_SendStall(pUDP);
+			udp_ep0_send_stall(pUDP);
 		break;
 	case STD_SET_FEATURE_ZERO:
 		DEBUGP("SET_FEATURE_ZERO ");
-		AT91F_USB_SendStall(pUDP);
+		udp_ep0_send_stall(pUDP);
 		break;
 	case STD_SET_FEATURE_INTERFACE:
 		DEBUGP("SET_FEATURE_INTERFACE ");
-		AT91F_USB_SendZlp(pUDP);
+		udp_ep0_send_zlp(pUDP);
 		break;
 	case STD_SET_FEATURE_ENDPOINT:
 		DEBUGP("SET_FEATURE_ENDPOINT ");
-		AT91F_USB_SendZlp(pUDP);
+		udp_ep0_send_zlp(pUDP);
 		wIndex &= 0x0F;
 		if ((wValue == 0) && wIndex && (wIndex <= 3)) {
 			pUDP->UDP_CSR[wIndex] = 0;
-			AT91F_USB_SendZlp(pUDP);
+			udp_ep0_send_zlp(pUDP);
 		} else
-			AT91F_USB_SendStall(pUDP);
+			udp_ep0_send_stall(pUDP);
 		break;
 	case STD_CLEAR_FEATURE_ZERO:
 		DEBUGP("CLEAR_FEATURE_ZERO ");
-		AT91F_USB_SendStall(pUDP);
+		udp_ep0_send_stall(pUDP);
 		break;
 	case STD_CLEAR_FEATURE_INTERFACE:
 		DEBUGP("CLEAR_FEATURE_INTERFACE ");
-		AT91F_USB_SendZlp(pUDP);
+		udp_ep0_send_zlp(pUDP);
 		break;
 	case STD_CLEAR_FEATURE_ENDPOINT:
 		DEBUGP("CLEAR_FEATURE_ENDPOINT(EPidx=%u) ", wIndex & 0x0f);
@@ -580,17 +538,17 @@ static void AT91F_CDC_Enumerate(void)
 				pUDP->UDP_RSTEP |= AT91C_UDP_EP3;
 				pUDP->UDP_RSTEP &= ~AT91C_UDP_EP3;
 			}
-			AT91F_USB_SendZlp(pUDP);
+			udp_ep0_send_zlp(pUDP);
 		} else
-			AT91F_USB_SendStall(pUDP);
+			udp_ep0_send_stall(pUDP);
 		break;
 	case STD_SET_INTERFACE:
 		DEBUGP("SET INTERFACE ");
-		AT91F_USB_SendStall(pUDP);
+		udp_ep0_send_stall(pUDP);
 		break;
 	default:
 		DEBUGP("DEFAULT(req=0x%02x, type=0x%02x) ", bRequest, bmRequestType);
-		AT91F_USB_SendStall(pUDP);
+		udp_ep0_send_stall(pUDP);
 		break;
 	}
 }
diff --git a/openpcd/firmware/src/pcd_enumerate.h b/openpcd/firmware/src/pcd_enumerate.h
index d51ffa3..31d8f28 100644
--- a/openpcd/firmware/src/pcd_enumerate.h
+++ b/openpcd/firmware/src/pcd_enumerate.h
@@ -15,8 +15,8 @@
 #ifndef PCD_ENUMERATE_H
 #define PCD_ENUMERATE_H
 
-#include <include/AT91SAM7.h>
-#include <include/types.h>
+#include <sys/types.h>
+#include <AT91SAM7.h>
 
 #define AT91C_EP_OUT 1
 #define AT91C_EP_OUT_SIZE 0x40
@@ -36,9 +36,9 @@ typedef struct _AT91S_CDC
 
 //* external function description
 
-AT91PS_CDC AT91F_CDC_Open(AT91PS_UDP pUdp);
+extern void udp_init(void);
 u_int8_t AT91F_UDP_IsConfigured(void);
-u_int32_t AT91F_UDP_Write(u_int8_t irq, const char *pData, u_int32_t length);
+u_int32_t AT91F_UDP_Write(u_int8_t irq, const unsigned char *pData, u_int32_t length);
 
 #endif // CDC_ENUMERATE_H
 
diff --git a/openpcd/firmware/src/rc632.c b/openpcd/firmware/src/rc632.c
index c8aa093..b1e5a3c 100644
--- a/openpcd/firmware/src/rc632.c
+++ b/openpcd/firmware/src/rc632.c
@@ -11,6 +11,10 @@
 #include "openpcd.h"
 #include "fifo.h"
 #include "dbgu.h"
+#include "pcd_enumerate.h"
+
+#define SPI_DEBUG_LOOPBACK
+#define SPI_USES_DMA
 
 static AT91PS_SPI pSPI = AT91C_BASE_SPI;
 
@@ -19,18 +23,45 @@ static void spi_irq(void)
 {
 	u_int32_t status = pSPI->SPI_SR;
 
-	DEBUGP("spi_irq: 0x%08x", status);
+	DEBUGP("spi_irq: 0x%08x ", status);
 
 	AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_SPI);
 
 	if (status & AT91C_SPI_OVRES)
-		DEBUGP("Overrun detected ");
+		DEBUGP("Overrun ");
 	if (status & AT91C_SPI_MODF)
-		DEBUGP("Mode Fault detected ");
+		DEBUGP("ModeFault ");
+	if (status & AT91C_SPI_ENDRX) {
+		pSPI->SPI_IDR = AT91C_SPI_ENDRX;
+		DEBUGP("ENDRX ");
+	}
+	if (status & AT91C_SPI_ENDTX) {
+		pSPI->SPI_IDR = AT91C_SPI_ENDTX;
+		DEBUGP("ENDTX ");
+	}
 
 	DEBUGP("\r\n");
 }
 
+#ifdef SPI_USES_DMA
+int spi_transceive(const u_int8_t *tx_data, u_int16_t tx_len, 
+		   u_int8_t *rx_data, u_int16_t *rx_len)
+{
+	DEBUGP("Starting DMA Xfer: ");
+	AT91F_SPI_ReceiveFrame(pSPI, rx_data, *rx_len, NULL, 0);
+	AT91F_SPI_SendFrame(pSPI, tx_data, tx_len, NULL, 0);
+	AT91F_PDC_EnableRx(AT91C_BASE_PDC_SPI);
+	AT91F_PDC_EnableTx(AT91C_BASE_PDC_SPI);
+	pSPI->SPI_IER = AT91C_SPI_ENDTX|AT91C_SPI_ENDRX;
+	AT91F_SPI_Enable(pSPI);
+
+	while (!(pSPI->SPI_SR & (AT91C_SPI_ENDRX|AT91C_SPI_ENDTX))) ;
+
+	DEBUGPCR("DMA Xfer finished");
+
+	return 0;
+}
+#else
 /* stupid polling transceiver routine */
 int spi_transceive(const u_int8_t *tx_data, u_int16_t tx_len, 
 		   u_int8_t *rx_data, u_int16_t *rx_len)
@@ -77,6 +108,7 @@ int spi_transceive(const u_int8_t *tx_data, u_int16_t tx_len,
 
 	return 0;
 }
+#endif
 
 /* static buffers used by routines below */
 static u_int8_t spi_outbuf[64+1];
@@ -209,16 +241,18 @@ static void rc632_irq(void)
 	
 	irq_opcdh.val = cause;
 	
-	AT91F_UDP_Write(1, &irq_opcdh, sizeof(irq_opcdh));
+	AT91F_UDP_Write(1, (u_int8_t *) &irq_opcdh, sizeof(irq_opcdh));
 	DEBUGP("\n");
 }
 
 void rc632_power(u_int8_t up)
 {
 	if (up)
-		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_RC632_RESET);
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA,
+				      OPENPCD_PIO_RC632_RESET);
 	else
-		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_RC632_RESET);
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA,
+				    OPENPCD_PIO_RC632_RESET);
 }
 
 void rc632_reset(void)
@@ -251,8 +285,17 @@ void rc632_init(void)
 			      OPENPCD_IRQ_PRIO_SPI,
 			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &spi_irq);
 	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_SPI);
+
 	AT91F_SPI_EnableIt(pSPI, AT91C_SPI_MODF|AT91C_SPI_OVRES);
+#ifdef SPI_USES_DMA
+	AT91F_SPI_EnableIt(pSPI, AT91C_SPI_ENDRX|AT91C_SPI_ENDTX);
+#endif
+
+#ifdef SPI_DEBUG_LOOPBACK
+	AT91F_SPI_CfgMode(pSPI, AT91C_SPI_MSTR|AT91C_SPI_PS_FIXED|AT91C_SPI_LLB);
+#else
 	AT91F_SPI_CfgMode(pSPI, AT91C_SPI_MSTR|AT91C_SPI_PS_FIXED);
+#endif
 	/* CPOL = 0, NCPHA = 1, CSAAT = 0, BITS = 0000, SCBR = 10 (4.8MHz), 
 	 * DLYBS = 0, DLYBCT = 0 */
 	//AT91F_SPI_CfgCs(pSPI, 0, AT91C_SPI_BITS_8|AT91C_SPI_NCPHA|(10<<8));
@@ -261,12 +304,14 @@ void rc632_init(void)
 	//AT91F_SPI_Reset(pSPI);
 
 	/* Register rc632_irq */
-	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, OPENPCD_RC632_IRQ,
+	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, OPENPCD_IRQ_RC632,
 			      OPENPCD_IRQ_PRIO_RC632,
 			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &rc632_irq);
 	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_IRQ1);
 
-	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_RC632_RESET);
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_RC632_RESET);
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_MFIN);
+	AT91F_PIO_CfgInput(AT91C_BASE_PIOA, OPENPCD_PIO_MFOUT);
 
 	/* initialize static part of openpcd_hdr for USB IRQ reporting */
 	irq_opcdh.cmd = OPENPCD_CMD_IRQ;
diff --git a/openpcd/firmware/src/rc632.h b/openpcd/firmware/src/rc632.h
index 77f7c20..acede11 100644
--- a/openpcd/firmware/src/rc632.h
+++ b/openpcd/firmware/src/rc632.h
@@ -1,13 +1,13 @@
 #ifndef _RC623_API_H
 #define _RC632_API_H
 
-#include <include/types.h>
+#include <sys/types.h>
 #include <include/cl_rc632.h>
 
-extern void rc632_write_reg(u_int8_t addr, u_int8_t data);
-extern void rc632_write_fifo(u_int8_t len, u_int8_t *data);
-extern u_int8_t rc632_read_reg(u_int8_t addr);
-extern u_int8_t rc632_read_fifo(u_int8_t max_len, u_int8_t *data);
+extern void rc632_reg_write(u_int8_t addr, u_int8_t data);
+extern void rc632_fifo_write(u_int8_t len, u_int8_t *data);
+extern u_int8_t rc632_reg_read(u_int8_t addr);
+extern u_int8_t rc632_fifo_read(u_int8_t max_len, u_int8_t *data);
 extern u_int8_t rc632_clear_bits(u_int8_t reg, u_int8_t bits);
 extern u_int8_t rc632_set_bits(u_int8_t reg, u_int8_t bits);
 extern void rc632_init(void);
diff --git a/openpcd/firmware/src/rc632_highlevel.c b/openpcd/firmware/src/rc632_highlevel.c
new file mode 100644
index 0000000..9a50df1
--- /dev/null
+++ b/openpcd/firmware/src/rc632_highlevel.c
@@ -0,0 +1,489 @@
+/* ISO 14443 WUPA / SELECT anticollision implementation, part of OpenPCD 
+ * (C) 2006 by Harald Welte <laforge@gnumonks.org>
+ */
+
+static int
+rc632_iso14443a_init(void)
+{
+	int ret;
+
+	// FIXME: some fifo work (drain fifo?)
+	
+	/* flush fifo (our way) */
+	ret = rc632_reg_write(RC632_REG_CONTROL, 0x01);
+
+	ret = rc632_reg_write(RC632_REG_TX_CONTROL,
+			(RC632_TXCTRL_TX1_RF_EN |
+			 RC632_TXCTRL_TX2_RF_EN |
+			 RC632_TXCTRL_TX2_INV |
+			 RC632_TXCTRL_FORCE_100_ASK |
+			 RC632_TXCTRL_MOD_SRC_INT));
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_CW_CONDUCTANCE,
+				CM5121_CW_CONDUCTANCE);
+	if (ret < 0)
+		return ret;
+
+	/* Since FORCE_100_ASK is set (cf mc073930.pdf), this line may be left out? */
+	ret = rc632_reg_write(RC632_REG_MOD_CONDUCTANCE,
+				CM5121_MOD_CONDUCTANCE);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_CODER_CONTROL,
+				(RC632_CDRCTRL_TXCD_14443A |
+				 RC632_CDRCTRL_RATE_106K));
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_MOD_WIDTH, 0x13);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_MOD_WIDTH_SOF, 0x3f);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_TYPE_B_FRAMING, 0x00);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_RX_CONTROL1,
+			      (RC632_RXCTRL1_GAIN_35DB |
+			       RC632_RXCTRL1_ISO14443 |
+			       RC632_RXCTRL1_SUBCP_8));
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_DECODER_CONTROL,
+			      (RC632_DECCTRL_MANCHESTER |
+			       RC632_DECCTRL_RXFR_14443A));
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_BIT_PHASE,
+				CM5121_14443A_BITPHASE);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_RX_THRESHOLD,
+				CM5121_14443A_THRESHOLD);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_BPSK_DEM_CONTROL, 0x00);
+	if (ret < 0)
+		return ret;
+			      
+	ret = rc632_reg_write(RC632_REG_RX_CONTROL2,
+			      (RC632_RXCTRL2_DECSRC_INT |
+			       RC632_RXCTRL2_CLK_Q));
+	if (ret < 0)
+		return ret;
+
+	/* Omnikey proprietary driver has 0x03, but 0x06 is the default reset
+	 * value ?!? */
+	ret = rc632_reg_write(RC632_REG_RX_WAIT, 0x06);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_CHANNEL_REDUNDANCY,
+			      (RC632_CR_PARITY_ENABLE |
+			       RC632_CR_PARITY_ODD));
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_CRC_PRESET_LSB, 0x63);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_write(RC632_REG_CRC_PRESET_MSB, 0x63);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int
+rc632_transceive(const u_int8_t *tx_buf,
+		 u_int8_t tx_len,
+		 u_int8_t *rx_buf,
+		 u_int8_t *rx_len,
+		 u_int64_t timer,
+		 unsigned int toggle)
+{
+	int ret, cur_tx_len;
+	const u_int8_t *cur_tx_buf = tx_buf;
+
+	DEBUGP("timer = %u\n", timer);
+
+	if (tx_len > 64)
+		cur_tx_len = 64;
+	else
+		cur_tx_len = tx_len;
+
+	ret = rc632_timer_set(timer);
+	if (ret < 0)
+		return ret;
+	
+	/* clear all interrupts */
+	ret = rc632_reg_write(RC632_REG_INTERRUPT_RQ, 0x7f);
+
+	do {	
+		ret = rc632_fifo_write(cur_tx_len, cur_tx_buf, 0x03);
+		if (ret < 0)
+			return ret;
+
+		if (cur_tx_buf == tx_buf) {
+			ret = rc632_reg_write(RC632_REG_COMMAND,
+					      RC632_CMD_TRANSCEIVE);
+			if (ret < 0)
+				return ret;
+		}
+
+		cur_tx_buf += cur_tx_len;
+		if (cur_tx_buf < tx_buf + tx_len) {
+			u_int8_t fifo_fill;
+			ret = rc632_reg_read(RC632_REG_FIFO_LENGTH,
+					     &fifo_fill);
+			if (ret < 0)
+				return ret;
+
+			cur_tx_len = 64 - fifo_fill;
+			printf("refilling tx fifo with %u bytes\n", cur_tx_len);
+		} else
+			cur_tx_len = 0;
+
+	} while (cur_tx_len);
+
+	if (toggle == 1)
+		tcl_toggle_pcb(handle);
+
+	ret = rc632_wait_idle_timer(handle);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_reg_read(RC632_REG_FIFO_LENGTH, rx_len);
+	if (ret < 0)
+		return ret;
+
+	if (*rx_len == 0) {
+		u_int8_t tmp;
+
+		DEBUGP("rx_len == 0\n");
+
+		rc632_reg_read(RC632_REG_ERROR_FLAG, &tmp);
+		rc632_reg_read(RC632_REG_CHANNEL_REDUNDANCY, &tmp);
+
+		return -1; 
+	}
+
+	return rc632_fifo_read(*rx_len, rx_buf);
+}
+
+/* issue a 14443-3 A PCD -> PICC command in a short frame, such as REQA, WUPA */
+static int
+rc632_iso14443a_transceive_sf(u_int8_t cmd,
+		    		struct iso14443a_atqa *atqa)
+{
+	int ret;
+	u_int8_t tx_buf[1];
+	u_int8_t rx_len = 2;
+
+	memset(atqa, 0, sizeof(atqa));
+
+	tx_buf[0] = cmd;
+
+	/* transfer only 7 bits of last byte in frame */
+	ret = rc632_reg_write(RC632_REG_BIT_FRAMING, 0x07);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_clear_bits(RC632_REG_CONTROL,
+				RC632_CONTROL_CRYPTO1_ON);
+	if (ret < 0)
+		return ret;
+
+#if 0
+	ret = rc632_reg_write(RC632_REG_CHANNEL_REDUNDANCY,
+				(RC632_CR_PARITY_ENABLE |
+				 RC632_CR_PARITY_ODD));
+#else
+	ret = rc632_clear_bits(RC632_REG_CHANNEL_REDUNDANCY,
+				RC632_CR_RX_CRC_ENABLE|RC632_CR_TX_CRC_ENABLE);
+				
+#endif
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_transceive(tx_buf, sizeof(tx_buf),
+				(u_int8_t *)atqa, &rx_len,
+				ISO14443A_FDT_ANTICOL_LAST1, 0);
+	if (ret < 0) {
+		DEBUGP("error during rc632_transceive()\n");
+		return ret;
+	}
+
+	/* switch back to normal 8bit last byte */
+	ret = rc632_reg_write(RC632_REG_BIT_FRAMING, 0x00);
+	if (ret < 0)
+		return ret;
+
+	if (rx_len != 2) {
+		DEBUGP("rx_len(%d) != 2\n", rx_len);
+		return -1;
+	}
+
+	return 0;
+}
+
+/* transceive regular frame */
+static int
+rc632_iso14443ab_transceive(struct rfid_asic_handle *handle,
+			   unsigned int frametype,
+			   const u_int8_t *tx_buf, unsigned int tx_len,
+			   u_int8_t *rx_buf, unsigned int *rx_len,
+			   u_int64_t timeout, unsigned int flags)
+{
+	int ret;
+	u_int8_t rxl = *rx_len & 0xff;
+	u_int8_t channel_red;
+
+	memset(rx_buf, 0, *rx_len);
+
+	switch (frametype) {
+	case RFID_14443A_FRAME_REGULAR:
+	case RFID_MIFARE_FRAME:
+		channel_red = RC632_CR_RX_CRC_ENABLE|RC632_CR_TX_CRC_ENABLE
+				|RC632_CR_PARITY_ENABLE|RC632_CR_PARITY_ODD;
+		break;
+	case RFID_14443B_FRAME_REGULAR:
+		channel_red = RC632_CR_RX_CRC_ENABLE|RC632_CR_TX_CRC_ENABLE
+				|RC632_CR_CRC3309;
+		break;
+#if 0
+	case RFID_MIFARE_FRAME:
+		channel_red = RC632_CR_PARITY_ENABLE|RC632_CR_PARITY_ODD;
+		break;
+#endif
+	default:
+		return -EINVAL;
+		break;
+	}
+	ret = rc632_reg_write(RC632_REG_CHANNEL_REDUNDANCY,
+			      channel_red);
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_transceive(tx_buf, tx_len, rx_buf, &rxl, timeout, 0);
+	*rx_len = rxl;
+	if (ret < 0)
+		return ret;
+
+
+	return 0; 
+}
+
+/* transceive anti collission bitframe */
+static int
+rc632_iso14443a_transceive_acf(struct iso14443a_anticol_cmd *acf,
+			       unsigned int *bit_of_col)
+{
+	int ret;
+	u_int8_t rx_buf[64];
+	u_int8_t rx_len = sizeof(rx_buf);
+	u_int8_t rx_align = 0, tx_last_bits, tx_bytes;
+	u_int8_t boc;
+	u_int8_t error_flag;
+	*bit_of_col = ISO14443A_BITOFCOL_NONE;
+	memset(rx_buf, 0, sizeof(rx_buf));
+
+	/* disable mifare cryto */
+	ret = rc632_clear_bits(RC632_REG_CONTROL,
+				RC632_CONTROL_CRYPTO1_ON);
+	if (ret < 0)
+		return ret;
+
+	/* disable CRC summing */
+#if 0
+	ret = rc632_reg_write(RC632_REG_CHANNEL_REDUNDANCY,
+				(RC632_CR_PARITY_ENABLE |
+				 RC632_CR_PARITY_ODD));
+#else
+	ret = rc632_clear_bits(RC632_REG_CHANNEL_REDUNDANCY,
+				RC632_CR_TX_CRC_ENABLE|RC632_CR_TX_CRC_ENABLE);
+#endif
+	if (ret < 0)
+		return ret;
+
+	tx_last_bits = acf->nvb & 0x0f;	/* lower nibble indicates bits */
+	tx_bytes = acf->nvb >> 4;
+	if (tx_last_bits) {
+		tx_bytes++;
+		rx_align = (tx_last_bits+1) % 8;/* rx frame complements tx */
+	}
+
+	//rx_align = 8 - tx_last_bits;/* rx frame complements tx */
+
+	/* set RxAlign and TxLastBits*/
+	ret = rc632_reg_write(RC632_REG_BIT_FRAMING,
+				(rx_align << 4) | (tx_last_bits));
+	if (ret < 0)
+		return ret;
+
+	ret = rc632_transceive((u_int8_t *)acf, tx_bytes,
+				rx_buf, &rx_len, 0x32, 0);
+	if (ret < 0)
+		return ret;
+
+	/* bitwise-OR the two halves of the split byte */
+	acf->uid_bits[tx_bytes-2] = (
+		  (acf->uid_bits[tx_bytes-2] & (0xff >> (8-tx_last_bits)))
+		| rx_buf[0]);
+	/* copy the rest */
+	memcpy(&acf->uid_bits[tx_bytes+1-2], &rx_buf[1], rx_len-1);
+
+	/* determine whether there was a collission */
+	ret = rc632_reg_read(RC632_REG_ERROR_FLAG, &error_flag);
+	if (ret < 0)
+		return ret;
+
+	if (error_flag & RC632_ERR_FLAG_COL_ERR) {
+		/* retrieve bit of collission */
+		ret = rc632_reg_read(RC632_REG_COLL_POS, &boc);
+		if (ret < 0)
+			return ret;
+
+		/* bit of collission relative to start of part 1 of 
+		 * anticollision frame (!) */
+		*bit_of_col = 2*8 + boc;
+	}
+
+	return 0;
+}
+
+#if 0
+enum rc632_rate {
+	RC632_RATE_106	= 0x00,
+	RC632_RATE_212	= 0x01,
+	RC632_RATE_424	= 0x02,
+	RC632_RATE_848	= 0x03,
+};
+
+struct rx_config {
+	u_int8_t	subc_pulses;
+	u_int8_t	rx_coding;
+	u_int8_t	rx_threshold;
+	u_int8_t	bpsk_dem_ctrl;
+};
+
+struct tx_config {
+	u_int8_t	rate;
+	u_int8_t	mod_width;
+};
+
+static struct rx_config rx_configs[] = {
+	{
+		.subc_pulses 	= RC632_RXCTRL1_SUBCP_8,
+		.rx_coding	= RC632_DECCTRL_MANCHESTER,
+		.rx_threshold	= 0x88,
+		.bpsk_dem_ctrl	= 0x00,
+	},
+	{
+		.subc_pulses	= RC632_RXCTRL1_SUBCP_4,
+		.rx_coding	= RC632_DECCTRL_BPSK,
+		.rx_threshold	= 0x50,
+		.bpsk_dem_ctrl	= 0x0c,
+	},
+	{
+		.subc_pulses	= RC632_RXCTRL1_SUBCP_2,
+		.rx_coding	= RC632_DECCTRL_BPSK,
+		.rx_threshold	= 0x50,
+		.bpsk_dem_ctrl	= 0x0c,
+	},
+	{
+		.subc_pulses	= RC632_RXCTRL1_SUBCP_1,
+		.rx_coding	= RC632_DECCTRL_BPSK,
+		.rx_threshold	= 0x50,
+		.bpsk_dem_ctrl	= 0x0c,
+	},
+};
+
+static struct tx_config tx_configs[] = {
+	{
+		.rate 		= RC632_CDRCTRL_RATE_106K,
+		.mod_width	= 0x13,
+	},
+	{
+		.rate		= RC632_CDRCTRL_RATE_212K,
+		.mod_width	= 0x07,
+	},
+	{
+		.rate		= RC632_CDRCTRL_RATE_424K,
+		.mod_width	= 0x03,
+	},
+	{
+		.rate		= RC632_CDRCTRL_RATE_848K,
+		.mod_width	= 0x01,
+	},
+};
+
+static int rc632_iso14443a_set_speed(struct rfid_asic_handle *handle,
+				     unsigned int tx,
+				     u_int8_t rate)
+{
+	int rc;
+	u_int8_t reg;
+
+
+	if (!tx) {
+		/* Rx */
+		if (rate > ARRAY_SIZE(rx_configs))
+			return -EINVAL;
+
+		rc = rc632_set_bit_mask(RC632_REG_RX_CONTROL1,
+					RC632_RXCTRL1_SUBCP_MASK,
+					rx_configs[rate].subc_pulses);
+		if (rc < 0)
+			return rc;
+
+		rc = rc632_set_bit_mask(RC632_REG_DECODER_CONTROL,
+					RC632_DECCTRL_BPSK,
+					rx_configs[rate].rx_coding);
+		if (rc < 0)
+			return rc;
+
+		rc = rc632_reg_write(RC632_REG_RX_THRESHOLD,
+					rx_configs[rate].rx_threshold);
+		if (rc < 0)
+			return rc;
+
+		if (rx_configs[rate].rx_coding == RC632_DECCTRL_BPSK) {
+			rc = rc632_reg_write(
+					     RC632_REG_BPSK_DEM_CONTROL,
+					     rx_configs[rate].bpsk_dem_ctrl);
+			if (rc < 0)
+				return rc;
+		}
+	} else {
+		/* Tx */
+		if (rate > ARRAY_SIZE(tx_configs))
+			return -EINVAL;
+
+		rc = rc632_set_bit_mask(RC632_REG_CODER_CONTROL,
+					RC632_CDRCTRL_RATE_MASK,
+					tx_configs[rate].rate);
+		if (rc < 0)
+			return rc;
+
+		rc = rc632_reg_write(RC632_REG_MOD_WIDTH,
+				     tx_configs[rate].mod_width);
+		if (rc < 0)
+			return rc;
+	}
+
+	return 0;
+}
+#endif
diff --git a/openpcd/firmware/src/req_ctx.c b/openpcd/firmware/src/req_ctx.c
index 025bf5e..c0731fe 100644
--- a/openpcd/firmware/src/req_ctx.c
+++ b/openpcd/firmware/src/req_ctx.c
@@ -1,23 +1,22 @@
-
 #include <unistd.h>
 #include <stdlib.h>
-#include <include/types.h>
+#include <sys/types.h>
+#include <asm/bitops.h>
+
 #include "openpcd.h"
 #include "dbgu.h"
 
 /* FIXME: locking, FIFO order processing */
 
 static struct req_ctx req_ctx[8];
-static u_int8_t req_ctx_busy;		/* bitmask of used request contexts */
+static unsigned long req_ctx_busy;		/* bitmask of used request contexts */
 
 struct req_ctx *req_ctx_find_get(void)
 {
 	u_int8_t i;
 	for (i = 0; i < NUM_REQ_CTX; i++) {
-		if (!(req_ctx_busy & (1 << i))) {
-			req_ctx_busy |= (1 << i);
+		if (test_and_set_bit(i, &req_ctx_busy) == 1)
 			return &req_ctx[i];
-		}
 	}
 
 	return NULL;
@@ -27,11 +26,12 @@ struct req_ctx *req_ctx_find_busy(void)
 {
 	u_int8_t i;
 	for (i = 0; i < NUM_REQ_CTX; i++) {
-		if (req_ctx_busy & (1 << i))
+		if (test_bit(i, &req_ctx_busy))
 			return &req_ctx[i];
 	}
-}
 
+	return NULL;
+}
 
 u_int8_t req_ctx_num(struct req_ctx *ctx)
 {
@@ -44,5 +44,6 @@ void req_ctx_put(struct req_ctx *ctx)
 	if (offset > NUM_REQ_CTX)
 		DEBUGPCR("Error in offset calculation req_ctx_put");
 
-	req_ctx_busy &= ~(1 << offset);
+	clear_bit(offset, &req_ctx_busy);
+	//req_ctx_busy &= ~(1 << offset);
 }
diff --git a/openpcd/firmware/src/trigger.c b/openpcd/firmware/src/trigger.c
new file mode 100644
index 0000000..1768b21
--- /dev/null
+++ b/openpcd/firmware/src/trigger.c
@@ -0,0 +1,14 @@
+#include "openpcd.h"
+#include "trigger.h"
+#include <lib_AT91SAM7.h>
+
+void trigger_init(void)
+{
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_TRIGGER);
+}
+
+void trigger_pulse(void)
+{
+	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_TRIGGER);
+	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_TRIGGER);
+}
diff --git a/openpcd/firmware/src/trigger.h b/openpcd/firmware/src/trigger.h
new file mode 100644
index 0000000..597704b
--- /dev/null
+++ b/openpcd/firmware/src/trigger.h
@@ -0,0 +1,7 @@
+#ifndef _TRIGGER_H
+#define _TRIGGER_H
+
+extern void trigger_init(void);
+extern void trigger_pulse(void);
+
+#endif