blctrl flash write

reset AIC - finally working
reset AIC - 2nd try
2008-06-23 15:17:28 +02:00 · 2008-06-23 15:17:28 +02:00 · 2008-06-23 15:17:28 +02:00 · 2008-06-23 15:17:28 +02:00 · 2008-06-23 15:17:28 +02:00 · 2008-06-23 15:17:28 +02:00
36 changed files with 924 additions and 1950 deletions
--- a/7
+++ b/7
@ -10,8 +10,8 @@ SIZE = $(TOOLCHAIN)/bin/arm-elf-size
 OBJCOPY = $(TOOLCHAIN)/bin/arm-elf-objcopy
 OBJDUMP = $(TOOLCHAIN)/bin/arm-elf-objdump

-INCDIRS = include $(TOOLCHAIN)/lib/gcc/arm-elf/4.3.3/include $(TOOLCHAIN)/arm-elf/include
-LIBDIRS = $(TOOLCHAIN)/arm-elf/lib $(TOOLCHAIN)/lib/gcc/arm-elf/4.3.3
+INCDIRS = include $(TOOLCHAIN)/lib/gcc/arm-elf/4.1.1/include $(TOOLCHAIN)/arm-elf/include
+LIBDIRS = $(TOOLCHAIN)/arm-elf/lib $(TOOLCHAIN)/lib/gcc/arm-elf/4.1.1

 # ------

@ -73,7 +73,6 @@ openocd:
 	$(shell $(OPENOCD) -f scripts/openocd.cfg)

 install: all
-#	./scripts/download.sh
-	$(TOOLCHAIN)/dfu-util -i 0 -D $(BUILD)/$(TARGET).elf.bin
+	./scripts/download.sh

 -include $(shell find $(BUILD) -name *.d 2> /dev/null)
--- a/at91_init0.s
+++ b/at91_init0.s
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - stack/bss/data init                                            *
- *                                                                         *
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -19,6 +17,7 @@
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
 	.equ AT91C_BASE_AIC,	(0xFFFFF000)
+	.equ AT91C_BASE_MC,	(0xFFFFFF00)

 	.equ IRQ_Stack_Size,	(3 * 8 * 4)
 	.equ FIQ_Stack_Size,	(0 * 8 * 4)
@ -32,6 +31,49 @@
 	.equ I_BIT,		0x80
 	.equ F_BIT,		0x40

+	.section .init_dummy, "x"
+	.global _hwstart
+	.func _hwstart
+_hwstart:
+		ldr     pc, [pc, #24]		/* 0x00 Reset handler */
+		ldr     pc, [pc, #24]		/* 0x04 Undefined Instruction */
+		ldr     pc, [pc, #20]		/* 0x08 Software Interrupt */
+		ldr     pc, [pc, #16]		/* 0x0C Prefetch Abort */
+		ldr     pc, [pc, #12]		/* 0x10 Data Abort */
+		ldr     pc, [pc, #8]		/* 0x14 reserved  */
+		ldr     pc, [pc, #4]		/* 0x18 IRQ */
+		ldr     pc, [pc, #0]		/* 0x1c FIQ */
+
+	.word _relocate + 0x13c000
+	.word 0x80000000
+
+	.endfunc
+
+	.section .init, "x"
+	.global _relocate
+	.func _relocate
+_relocate:
+		msr	CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT
+
+		/* Remap RAM to 0x00000000 */
+		ldr	r1, =AT91C_BASE_MC
+		mov	r2, #0x01
+		str	r2, [r1]
+
+		/* Relocate .text section */
+		ldr	r1, =__text_copy_start__
+		ldr	r2, =__text_start__
+		ldr	r3, =__text_end__
+_relocate1:	cmp	r2, r3
+		ldrlo	r0, [r1], #4
+		strlo	r0, [r2], #4
+		blo	_relocate1
+
+		ldr	r0, =InitReset
+		mov	pc, r0
+
+	.endfunc
+
 	.section .text
 	.global _start
 	.func   _start
@ -95,7 +137,7 @@ InitReset:
 		mov	sp, r0

 		/* Relocate .data section */
-		ldr	r1, =__text_end__
+		ldr	r1, =__data_copy_start__
 		ldr	r2, =__data_start__
 		ldr	r3, =__data_end__
 LoopRel:	cmp	r2, r3
@ -109,7 +151,7 @@ LoopRel:	cmp	r2, r3
 		ldr	r2, =__bss_end__
 LoopZI:		cmp	r1, r2
 		strlo	r0, [r1], #4
-		BLO	LoopZI
+		blo	LoopZI

 		/* Start main() */
 		ldr	lr, =exit
--- a/at91_init1.c
+++ b/at91_init1.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - PLL Init, IRQ/FIQ Vectors                                      *
- *                                                                         *
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -20,6 +18,7 @@
 ***************************************************************************/
 #include <stdint.h>
 #include "AT91SAM7S256.h"
+#include "board.h"

 static void empty_isr(void) {}

@ -38,7 +37,7 @@ void at91_init1(void)
 	*AT91C_RSTC_RMR = (AT91C_RSTC_KEY & 0xA5 << 24) | AT91C_RSTC_URSTEN;

 	/* Set Flash Waitstates */
-	*AT91C_MC_FMR = AT91C_MC_FWS_1FWS;
+	*AT91C_MC_FMR = (AT91C_MC_FMCN & ((MCK / 666666) << 16)) | AT91C_MC_FWS_1FWS;

 	/*
 	 * Enable main oscillator (MAINCK)
@ -80,6 +79,11 @@ void at91_init1(void)
 		aic->AIC_SVR[i] = (uint32_t)empty_isr;
 	}
 	aic->AIC_SPU = (uint32_t)empty_isr;
+
+	/* end-of-interrupt signal */
+	do {
+		aic->AIC_EOICR = ~0;
+	} while (aic->AIC_ISR != 0x00);
 }

 __attribute__((naked)) void IRQ_Handler(void)
--- a/eagle/SAM7_FC.brd
+++ b/eagle/SAM7_FC.brd
--- a/eagle/SAM7_FC.sch
+++ b/eagle/SAM7_FC.sch
--- a/eagle/sam7fc-bot.png
+++ b/eagle/sam7fc-bot.png
--- a/eagle/sam7fc-sch.png
+++ b/eagle/sam7fc-sch.png
--- a/eagle/sam7fc-top.png
+++ b/eagle/sam7fc-top.png
--- a/include/at91_adc.h
+++ b/include/at91_adc.h
@ -1,32 +0,0 @@
-#ifndef AT91_ADC_H_
-#define AT91_ADC_H_
-
-#include <stdint.h>
-
-#define ADC_GYRO_NICK	0
-#define ADC_GYRO_ROLL	1
-#define ADC_GYRO_GIER	2
-
-#define ADC_ACC_NICK	3
-#define ADC_ACC_ROLL	4
-#define ADC_ACC_GIER	5
-
-#define ADC_VOLTAGE	6
-
-// TODO: not all flags are public
-#define ADC_COMPLETE		0x0001
-#define ADC_CAL_GYRO		0x0100
-#define ADC_CAL_GYRO_COMPLETE	0x0200
-#define ADC_CAL_ACC		0x1000
-#define ADC_CAL_ACC_COMPLETE	0x2000
-#define ADC_CAL_ACC_LOAD	0x4000
-
-void adc_trigger(void);
-void adc_get_results(int16_t *adc_result);
-
-void adc_calibrate(uint32_t mode);
-void adc_drift_adjust(int16_t nick, int16_t roll, int16_t yaw);
-
-void at91_adc_init(void);
-
-#endif /* AT91_ADC_H_ */
--- a/include/at91_dbgu.h
+++ b/include/at91_dbgu.h
@ -1,10 +1,12 @@
 #ifndef AT91_DBGU_H_
 #define AT91_DBGU_H_

+#include <stddef.h>
+
 void at91_dbgu_init(void);

 void at91_dbgu_putc(char c);
 void at91_dbgu_puts(const char *p);
 int at91_dbgu_write(void *base, const char *buf, size_t len);

-#endif /* AT91_DBGU_H_ */
+#endif /*AT91_DBGU_H_*/
--- a/include/at91_pitc.h
+++ b/include/at91_pitc.h
@ -17,7 +17,6 @@ struct pitc_timer {
 };

 void pitc_schedule_timer(struct pitc_timer *timer);
-void pitc_remove_timer(struct pitc_timer *timer);

 uint32_t pitc_get_ticks(void);
 void at91_pitc_init(void);
--- a/include/at91_tc1.h
+++ b/include/at91_tc1.h
@ -5,18 +5,13 @@

 #define RC_CAL_START	0
 #define RC_CAL_END	1
-#define RC_CAL_LOAD	2
-#define RC_CAL_SAVE	3

 struct rc_values {
 	int16_t chan[MAX_CHANNELS];
 };

 uint32_t rcontrol_getvalues(struct rc_values *rc);
-uint32_t rcontrol_getswitches(struct rc_values *rc);
-
 void rcontrol_calibrate(uint32_t mode);
-void rcontrol_print_cal(void);

 void at91_tc1_init(void);

--- a/include/at91_tests.h
+++ b/include/at91_tests.h
@ -3,4 +3,7 @@

 void at91_rttc_test_init(void);

+void at91_adc_test_init(void);
+void at91_adc_printresults(void);
+
 #endif /*AT91_TESTS_H_*/
--- a/include/at91_twi.h
+++ b/include/at91_twi.h
@ -8,7 +8,7 @@
 #define TWI_ADDR_BL2		0x22
 #define TWI_ADDR_BL3		0x23
 #define TWI_ADDR_BL4		0x24
-#define TWI_ADDR_EEPROM		0x50
+#define TWI_ADDR_EEPROM		0x40

 /* TWIBOOT commands */
 #define CMD_WAIT		0x00
@ -25,53 +25,12 @@
 //#define CMD_GET_INFO		0x10
 #define CMD_SET_PWM		0x21
 #define CMD_GET_STATUS		0x22
-#define CMD_SET_PARAM		0x23
-#define CMD_GET_PARAM		0x24
+//#define CMD_SET_PARAM		0x23
+//#define CMD_GET_PARAM		0x24
 #define CMD_BOOT_LOADER		0x2F

-struct blmc_status {
-	uint8_t pwm_ist;
-	uint8_t pwm_soll;

-	uint16_t rpm;
-	uint16_t current;
-	uint16_t voltage;
-} __attribute__ ((packed));
-
-struct blmc_param {
-	uint16_t spinup_ticks;
-
-	uint8_t spinup_tick;
-	uint8_t spinup_step;
-
-	uint8_t spinup_wait;
-	uint8_t spinup_pwm;
-
-	uint8_t pwm_min;
-	uint8_t pwm_max;
-
-	uint16_t current_limit;
-	uint16_t current_max;
-
-	uint16_t voltage_min;
-
-	uint16_t crc16;
-} __attribute__ ((packed));
-
-
-#define EE_PARAMETER_SET_START		0x0000
-
-/* remote control calibration data */
-#define EE_RC_CAL_DATA			(EE_PARAMETER_SET_START)
-#define EE_RC_CAL_DATA_SIZE		48
-
-/* ACC calibration data */
-#define EE_ACC_CAL_DATA			(EE_RC_CAL_DATA + EE_RC_CAL_DATA_SIZE)
-#define EE_ACC_CAL_DATA_SIZE		6
-
-#define EE_PARAMETER_SET_END		(EE_ACC_CAL_DATA + EE_ACC_CAL_DATA_SIZE)
-
-struct twi_cmd {
+struct blmc_cmd {
 	uint32_t cmd;		/* cmd byte(s) */
 	uint8_t mode;		/* read/write, cmdlen (1-3 bytes) */
 	uint16_t size;		/* data size */
@ -79,18 +38,18 @@ struct twi_cmd {
 };

 /* same bits as TWI_MMR[8..15] */
-#define TWI_MODE_READ		0x10
-#define TWI_MODE_WRITE		0x00
-#define TWI_MODE_0_ARG		0x01
-#define TWI_MODE_1_ARG		0x02
-#define TWI_MODE_2_ARG		0x03
+#define BLMC_CMD_READ		0x10
+#define BLMC_CMD_WRITE		0x00
+#define BLMC_CMD_0_ARG		0x01
+#define BLMC_CMD_1_ARG		0x02
+#define BLMC_CMD_2_ARG		0x03

 uint32_t twi_read_eeprom(uint32_t addr, uint8_t *buf, uint32_t size);
 uint32_t twi_write_eeprom(uint32_t addr, uint8_t *buf, uint32_t size);

 uint32_t twi_setpwm(uint8_t *values);

-uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd);
+uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd);

 void at91_twi_init(void);
 void at91_twi_test(void);
--- a/include/board.h
+++ b/include/board.h
@ -40,6 +40,4 @@
 #define MIN(a,b) ((a) < (b) ? (a) : (b))
 #define MAX(a,b) ((a) > (b) ? (a) : (b))

-#define LIMIT(val, min, max) (((val) < (min)) ? (min) : (((val) > (max)) ? (max) : (val)))
-
 #endif /*BOARD_H_*/
--- a/include/pidctrl.h
+++ b/include/pidctrl.h
@ -1,23 +0,0 @@
-#ifndef _PIDCTRL_H_
-#define _PIDCTRL_H_
-
-#include <stdint.h>
-
-struct pid_data {
-	int32_t kp;
-
-	int32_t ki;
-	int32_t err_sum;
-	int32_t err_sum_max;
-	int32_t err_sum_min;
-
-	int32_t kd;
-	int32_t err_old;
-
-	int32_t out_max;
-	int32_t out_min;
-};
-
-int32_t pid_ctrl(struct pid_data *pid, int32_t error);
-
-#endif /* _PIDCTRL_H_ */
--- a/include/tdc_proto.h
+++ b/include/tdc_proto.h
@ -1,103 +0,0 @@
-#ifndef TDCPROTO_H_
-#define TDCPROTO_H_
-
-#include <stdint.h>
-
-/*
- * 0: this is a request (host -> board)
- * 1: this is a reply (board -> host)
- */
-#define TDC_DIR		0x80
-#define TDC_REPLY	TDC_DIR
-
-/*
- * TDC_DIR = 0: destination address
- * TDC_DIR = 1: source address
- */
-#define TDC_ADDRMASK	0x70
-#define TDC_ADDR0	0x00		// host (dynamic!, sends to interface of last hello)
-#define TDC_ADDR1	0x10		// flightcontrol
-#define TDC_ADDR2	0x20		// missioncontrol
-#define TDC_ADDR3	0x30		// videocontrol
-#define TDC_ADDR4	0x40
-#define TDC_ADDR5	0x50
-#define TDC_ADDR6	0x60
-#define TDC_ADDR7	0x70
-
-#define TDC_OPCODEMASK	0x0F
-#define TDC_HELLO	0x00		// sets the path/interface to the host, reply is a info string
-#define TDC_GETVARS	0x01		// request variable names, many replies
-#define TDC_GETVALUE	0x02		// get one value, one reply
-#define TDC_SETVALUE	0x03		// set one value, no reply
-#define TDC_REQVALUES	0x04		// registers a periodic update, timed replies
-#define TDC_TERMINAL	0x05		// stdout data
-
-#define TDC_USERDATA	0x0F		// user-defined data e.g. between boards
-
-struct tdc_pkt_header {
-	uint8_t cmd;			// TDC_*
-	uint8_t size;			// size including this header
-} __attribute__ ((packed));
-
-struct tdc_hello_reply {
-	uint8_t cmd;
-	uint8_t size;
-	char name[32];			// name of device, version string
-} __attribute__ ((packed));
-
-struct tdc_getvars_reply {
-	uint8_t cmd;
-	uint8_t size;
-	uint8_t id;			// variable ID (max 256 vars / board)
-	uint32_t flags;			// variable parameters (type, size, ro/rw)
-	uint8_t name_len;		// size of variable name
-	char name[0];			// variable name, excluding '\0'
-} __attribute__ ((packed));
-
-struct tdc_getvalue_request {
-	uint8_t cmd;
-	uint8_t size;
-	uint8_t id;			// variable ID (max 256 vars / board)
-} __attribute__ ((packed));
-
-struct tdc_getvalue_reply {
-	uint8_t cmd;
-	uint8_t size;
-	uint8_t id;			// variable ID (max 256 vars / board)
-	uint8_t data[0];		// variable data 1-8 bytes
-} __attribute__ ((packed));
-
-struct tdc_setvalue_request {
-	uint8_t cmd;
-	uint8_t size;
-	uint8_t id;			// variable ID (max 256 vars / board)
-	uint8_t data[0];		// variable data 1-8 bytes
-} __attribute__ ((packed));
-
-struct tdc_reqvalues_request {
-	uint8_t cmd;
-	uint8_t size;
-	uint16_t interval;		// interval in ms
-	uint32_t varmap[8];		// bitmap of variables (32 * 8 = 256)
-} __attribute__ ((packed));
-
-struct tdc_reqvalues_reply {
-	uint8_t cmd;
-	uint8_t size;
-	uint32_t timestamp;		// internal jiffie count
-	uint8_t cnt;			// number of variables
-} __attribute__ ((packed));
-
-#define TDC_SIZEMASK	0x000F
-
-#define TDC_TYPEMASK	0x00F0
-#define TDC_UNSIGNED	0x0000
-#define TDC_SIGNED	0x0010
-#define TDC_FP		0x0020
-#define TDC_FIXED	0x0040
-
-#define TDC_READONLY	0x0100
-
-#define TDC_GUI_GRAPH	0x8000
-
-#endif /* TDCPROTO_H_ */
--- a/include/telemetrie.h
+++ b/include/telemetrie.h
@ -1,19 +1,108 @@
 #ifndef TELEMETRIE_H_
 #define TELEMETRIE_H_

-#include "tdc_proto.h"
+/*
+ * 0: this is a request (host -> board)
+ * 1: this is a reply (board -> host)
+ */
+#define TDC_DIR		0x80
+#define TDC_REPLY	TDC_DIR
+
+/*
+ * TDC_DIR = 0: destination address
+ * TDC_DIR = 1: source address
+ */
+#define TDC_ADDRMASK	0x70
+#define TDC_ADDR0	0x00		// host (dynamic!, sends to interface of last hello)
+#define TDC_ADDR1	0x10		// flightcontrol
+#define TDC_ADDR2	0x20		// missioncontrol
+#define TDC_ADDR3	0x30		// videocontrol
+#define TDC_ADDR4	0x40
+#define TDC_ADDR5	0x50
+#define TDC_ADDR6	0x60
+#define TDC_ADDR7	0x70
+
+#define TDC_OPCODEMASK	0x0F
+#define TDC_HELLO	0x00		// sets the path/interface to the host, reply is a info string
+#define TDC_GETVARS	0x01		// request variable names, many replies
+#define TDC_GETVALUE	0x02		// get one value, one reply
+#define TDC_SETVALUE	0x03		// set one value, no reply
+#define TDC_REQVALUES	0x04		// registers a periodic update, timed replies
+#define TDC_TERMINAL	0x05		// stdout data
+
+#define TDC_USERDATA	0x0F		// user-defined data e.g. between boards
+
+struct tdc_pkt_header {
+	uint8_t cmd;			// TDC_*
+	uint8_t size;			// bytes after size
+} __attribute__ ((packed));
+
+struct tdc_hello_reply {
+	uint8_t cmd;
+	uint8_t size;
+	char name[32];			// name of device, version string
+} __attribute__ ((packed));
+
+struct tdc_getvars_reply {
+	uint8_t cmd;
+	uint8_t size;
+	uint8_t id;			// variable ID (max 256 vars / board)
+	uint32_t flags;			// variable parameters (type, size, ro/rw)
+	char name[0];		// variable name, excluding '\0'
+} __attribute__ ((packed));
+
+struct tdc_getvalue_request {
+	uint8_t cmd;
+	uint8_t size;
+	uint8_t id;			// variable ID (max 256 vars / board)
+} __attribute__ ((packed));
+
+struct tdc_getvalue_reply {
+	uint8_t cmd;
+	uint8_t size;
+	uint8_t id;			// variable ID (max 256 vars / board)
+	uint8_t data[0];		// variable data 1-8 bytes
+} __attribute__ ((packed));
+
+struct tdc_setvalue_request {
+	uint8_t cmd;
+	uint8_t size;
+	uint8_t id;			// variable ID (max 256 vars / board)
+	uint8_t data[0];		// variable data 1-8 bytes
+} __attribute__ ((packed));
+
+struct tdc_reqvalues_request {
+	uint8_t cmd;
+	uint8_t size;
+	uint16_t interval;		// interval in ms
+	uint32_t varmap[8];		// bitmap of variables (32 * 8 = 256)
+} __attribute__ ((packed));
+
+struct tdc_reqvalues_reply {
+	uint8_t cmd;
+	uint8_t size;
+	uint32_t timestamp;		// internal jiffie count
+	uint8_t cnt;			// number of variables
+} __attribute__ ((packed));
+
+

 struct comm_device {
 	struct fifo *rxfifo;
 	struct fifo *txfifo;
-	void (*trigger_tx)(void);
 };

 void tdc_register_device(uint32_t addr, struct comm_device *device);
+
 int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head);

-void tdc_check(void);
-void tdc_init(void);
+void tdc_receive(struct comm_device *device);
+
+
+
+
+
+

 struct tdc_value {
 	void *data;
@ -21,45 +110,26 @@ struct tdc_value {
 	uint32_t flags;
 };

-#if 1
+#define TDC_SIZEMASK	0x0F
+#define TDC_UNSIGNED	0x00
+#define TDC_SIGNED	0x10
+#define TDC_FP		0x20

-#define TDC_VALUE(name, var, desc, type, flags) \
-	type * tdc_check_##name(void) { return (&var); } \
+#define TDC_VALUE(var, desc, type, flags) \
+	type * tdc_check_##var(void) { return (&var); } \
 	static struct tdc_value __attribute__((used, section(".tdc_value"))) \
-	tdc_value_##name = { &var, desc, sizeof(type) | flags }; \
-	tdc_value_##name = tdc_value_##name;
+	tdc_value_##var = { &var, desc, sizeof(type) | flags }; \
+	tdc_value_##var = tdc_value_##var;

-#define TDC_PTR(name, ptr, desc, type, flags) \
-	static struct tdc_value __attribute__((used, section(".tdc_value"))) \
-	tdc_value_##name = { ptr, desc, sizeof(type) | flags }; \
-
-#else
-
-#define TDC_VALUE(name, var, desc, type, flags)
-#define TDC_PTR(name, ptr, desc, type, flags);
-
-#endif
-
-#define TDC_UINT8(var, desc)	TDC_VALUE(var, var, desc, uint8_t, TDC_UNSIGNED)
-#define TDC_UINT16(var, desc)	TDC_VALUE(var, var, desc, uint16_t, TDC_UNSIGNED)
-#define TDC_UINT32(var, desc)	TDC_VALUE(var, var, desc, uint32_t, TDC_UNSIGNED)
-#define TDC_UINT64(var, desc)	TDC_VALUE(var, var, desc, uint64_t, TDC_UNSIGNED)
-#define TDC_INT8(var, desc)	TDC_VALUE(var, var, desc, int8_t, TDC_SIGNED)
-#define TDC_INT16(var, desc)	TDC_VALUE(var, var, desc, int16_t, TDC_SIGNED)
-#define TDC_INT32(var, desc)	TDC_VALUE(var, var, desc, int32_t, TDC_SIGNED)
-#define TDC_INT64(var, desc)	TDC_VALUE(var, var, desc, int64_t, TDC_SIGNED)
-#define TDC_FLOAT(var, desc)	TDC_VALUE(var, var, desc, float, TDC_FP)
-#define TDC_DOUBLE(var, desc)	TDC_VALUE(var, var, desc, double, TDC_FP)
-
-#define TDC_UINT8_RO(var, desc)		TDC_VALUE(var, var, desc, uint8_t, TDC_UNSIGNED | TDC_READONLY)
-#define TDC_UINT16_RO(var, desc)	TDC_VALUE(var, var, desc, uint16_t, TDC_UNSIGNED | TDC_READONLY)
-#define TDC_UINT32_RO(var, desc)	TDC_VALUE(var, var, desc, uint32_t, TDC_UNSIGNED | TDC_READONLY)
-#define TDC_UINT64_RO(var, desc)	TDC_VALUE(var, var, desc, uint64_t, TDC_UNSIGNED | TDC_READONLY)
-#define TDC_INT8_RO(var, desc)		TDC_VALUE(var, var, desc, int8_t, TDC_SIGNED | TDC_READONLY)
-#define TDC_INT16_RO(var, desc)		TDC_VALUE(var, var, desc, int16_t, TDC_SIGNED | TDC_READONLY)
-#define TDC_INT32_RO(var, desc)		TDC_VALUE(var, var, desc, int32_t, TDC_SIGNED | TDC_READONLY)
-#define TDC_INT64_RO(var, desc)		TDC_VALUE(var, var, desc, int64_t, TDC_SIGNED | TDC_READONLY)
-#define TDC_FLOAT_RO(var, desc)		TDC_VALUE(var, var, desc, float, TDC_FP | TDC_READONLY)
-#define TDC_DOUBLE_RO(var, desc)	TDC_VALUE(var, var, desc, double, TDC_FP | TDC_READONLY)
+#define TDC_UINT8(var, desc)	TDC_VALUE(var, desc, uint8_t, TDC_UNSIGNED)
+#define TDC_UINT16(var, desc)	TDC_VALUE(var, desc, uint16_t, TDC_UNSIGNED)
+#define TDC_UINT32(var, desc)	TDC_VALUE(var, desc, uint32_t, TDC_UNSIGNED)
+#define TDC_UINT64(var, desc)	TDC_VALUE(var, desc, uint64_t, TDC_UNSIGNED)
+#define TDC_INT8(var, desc)	TDC_VALUE(var, desc, int8_t, TDC_SIGNED)
+#define TDC_INT16(var, desc)	TDC_VALUE(var, desc, int16_t, TDC_SIGNED)
+#define TDC_INT32(var, desc)	TDC_VALUE(var, desc, int32_t, TDC_SIGNED)
+#define TDC_INT64(var, desc)	TDC_VALUE(var, desc, int64_t, TDC_SIGNED)
+#define TDC_FLOAT(var, desc)	TDC_VALUE(var, desc, float, TDC_FP)
+#define TDC_DOUBLE(var, desc)	TDC_VALUE(var, desc, double, TDC_FP)

 #endif /*TELEMETRIE_H_*/
--- a/ldscript.ld
+++ b/ldscript.ld
@ -1,15 +1,28 @@
 MEMORY
 {
-	CODE (rx)  : ORIGIN = 0x00100000, LENGTH = 256k
-	DATA (rwx) : ORIGIN = 0x00200000, LENGTH = 64k
+	FLASH(rx)  : ORIGIN = 0x00100000, LENGTH = 256k
+	CODE (rx)  : ORIGIN = 0x0013C000, LENGTH = 16k
+	DATA (rwx) : ORIGIN = 0x00000000, LENGTH = 64k
 }

 SECTIONS
 {
+	. = ORIGIN(FLASH);
+
+	.init_dummy : {
+		*(.init_dummy)
+	} >FLASH =0
+
 	. = ORIGIN(CODE);
-	.text : {
-		*at91_init0.o (.text)
-		*at91_init1.o (.text)
+
+	.init : {
+		*(.init)
+		__text_copy_start__ = .;
+	} >CODE =0
+
+	.text : AT (LOADADDR(.init) + SIZEOF(.init)) {
+		*at91_init0.o(*.text)
+		*at91_init1.o(*.text)
 		*(.text)
 		*(.glue_7t)
 		*(.glue_7)
@ -28,10 +41,13 @@ SECTIONS
 		*(.rodata)
 		*(.rodata.*)

-		__text_end__ = .;
-	} >CODE =0
+		__data_copy_start__ = (__text_copy_start__ + (__text_end__ - __text_start__));
+	} >DATA =0

-	.data : AT (ADDR(.text) + SIZEOF(.text)) {
+	__text_start__ = ADDR(.text);
+	__text_end__ = ADDR(.text) + SIZEOF(.text);
+
+	.data : AT (LOADADDR(.text) + SIZEOF(.text)) {
 		*(.data)
 	} >DATA =0

--- a/main.c
+++ b/main.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - main loop                                                    *
- *                                                                         *
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -18,42 +16,32 @@
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
-#include <stdio.h>
-#include <string.h>
-
 #include "AT91SAM7S256.h"
-#include "board.h"
-
 #include "at91_sysc.h"
 #include "at91_dbgu.h"
 #include "at91_pitc.h"
-#include "at91_adc.h"
-#include "at91_tests.h"
 #include "at91_udp.h"
-#include "at91_pio.h"
 #include "at91_twi.h"

-#include "at91_tc1.h"
 #include "memalloc.h"
-#include "telemetrie.h"

-extern void base_ctrl(void);
+#include "board.h"
+#include <stdio.h>
+#include <string.h>

-volatile static uint32_t run_ctrl;
-
-static uint32_t base_ctrl_trigger(struct pitc_timer *timer)
+static uint32_t pitc_test(struct pitc_timer *timer)
 {
-	/* trigger adc, after ~15us () result should be ready */
-	adc_trigger();
-
-	run_ctrl = 1;
+	static uint32_t i;
+	*AT91C_PIOA_SODR = i;
+	i = i ^ LED_GREEN;
+	*AT91C_PIOA_CODR = i;

 	return PITC_RESTART_TIMER;
 }

-static struct pitc_timer base_timer = {
-	.interval = 2,
-	.func = &base_ctrl_trigger,
+static struct pitc_timer pitc_test_timer = {
+	.interval = 10,
+	.func = &pitc_test,
 };

 int main(void)
@ -68,39 +56,19 @@ int main(void)
 	at91_dbgu_init();
 	at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");

-	/* triggers pinchange-isrs */
-	at91_pio_init();
-
 	/* timer */
 	at91_pitc_init();
-	at91_rttc_test_init();

 	/* twi */
 	at91_twi_init();
 	at91_twi_test();

-	/* remote control, needs twi */
-	at91_tc1_init();
-
 	/* usb */
 	at91_udp_init();

-	/* adc, need timer, twi */
-	at91_adc_init();
-
-	pitc_schedule_timer(&base_timer);
-
-	tdc_init();
-
 	printf("static alloc: %5ld bytes\n\r", static_alloc_used());

-	while (1) {
-		if (run_ctrl) {
-			// TODO: racy?
-			run_ctrl = 0;
+	pitc_schedule_timer(&pitc_test_timer);

-			base_ctrl();
-		}
-		tdc_check();
-	}
+	while (1);
 }
--- a/src/at91_adc.c
+++ b/src/at91_adc.c
@ -1,238 +0,0 @@
-/***************************************************************************
- *   sam7fc - ADC routines / calibration                                     *
- *                                                                         *
- *   Copyright (C) 01/2008 by Olaf Rempel                                  *
- *   razzor@kopf-tisch.de                                                  *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; version 2 of the License                *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#include <stdint.h>
-#include <stdio.h>
-#include "AT91SAM7S256.h"
-#include "board.h"
-#include "at91_adc.h"
-#include "at91_pitc.h"
-#include "at91_twi.h"
-#include "telemetrie.h"
-
-static uint32_t adc_status;
-
-static uint16_t adc_tmp[7];
-static uint16_t adc_offset[6];
-
-TDC_PTR(adc_offset0, &adc_offset[ADC_GYRO_NICK], "XADC_GYRO_NICK (offset)", int16_t, TDC_UNSIGNED);
-TDC_PTR(adc_offset1, &adc_offset[ADC_GYRO_ROLL], "XADC_GYRO_ROLL (offset)", int16_t, TDC_UNSIGNED);
-TDC_PTR(adc_offset2, &adc_offset[ADC_GYRO_GIER], "XADC_GYRO_GIER (offset)", int16_t, TDC_UNSIGNED);
-TDC_PTR(adc_offset3, &adc_offset[ADC_ACC_NICK], "XADC_ACC_NICK (offset)", int16_t, TDC_UNSIGNED);
-TDC_PTR(adc_offset4, &adc_offset[ADC_ACC_ROLL], "XADC_ACC_ROLL (offset)", int16_t, TDC_UNSIGNED);
-TDC_PTR(adc_offset5, &adc_offset[ADC_ACC_GIER], "XADC_ACC_GIER (offset)", int16_t, TDC_UNSIGNED);
-
-#define ADC_CAL_COUNT_MAX 1024
-
-static uint32_t adc_cal_count;
-static uint32_t adc_cal_data[3];
-
-/* check eeprom parameter size (3x uint16_t) */
-#if ((3 * 2) != EE_ACC_CAL_DATA_SIZE)
-#error "invalid EE_ACC_CAL_DATA_SIZE"
-#endif
-
-static void at91_adc_isr(void)
-{
-	AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
-	pdc->PDC_RPR = (uint32_t) &adc_tmp;
-	pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
-	pdc->PDC_PTCR = AT91C_PDC_RXTEN;
-
-	/* clear interrupts */
-	uint32_t dummy = *AT91C_ADC_SR;
-	dummy = dummy;
-
-	if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC)))
-		return;
-
-	if (adc_status & ADC_CAL_GYRO) {
-		adc_cal_data[0] += adc_tmp[ADC_GYRO_NICK];
-		adc_cal_data[1] += adc_tmp[ADC_GYRO_ROLL];
-		adc_cal_data[2] += adc_tmp[ADC_GYRO_GIER];
-
-	} else {
-		adc_cal_data[0] += adc_tmp[ADC_ACC_NICK];
-		adc_cal_data[1] += adc_tmp[ADC_ACC_ROLL];
-		adc_cal_data[2] += adc_tmp[ADC_ACC_GIER];
-	}
-
-	adc_cal_count++;
-}
-
-static uint32_t adc_calibrate_cb(struct pitc_timer *timer)
-{
-	if (adc_cal_count < ADC_CAL_COUNT_MAX) {
-		/* trigger next cycle */
-		*AT91C_ADC_CR = AT91C_ADC_START;
-		return PITC_RESTART_TIMER;
-
-	} else {
-		if (adc_status & ADC_CAL_GYRO) {
-			adc_offset[ADC_GYRO_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX;
-			adc_offset[ADC_GYRO_ROLL] = adc_cal_data[1] / ADC_CAL_COUNT_MAX;
-			adc_offset[ADC_GYRO_GIER] = adc_cal_data[2] / ADC_CAL_COUNT_MAX;
-
-			adc_calibrate(ADC_CAL_GYRO_COMPLETE);
-
-		} else {
-			adc_offset[ADC_ACC_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX;
-			adc_offset[ADC_ACC_ROLL] = adc_cal_data[1] / ADC_CAL_COUNT_MAX;
-			adc_offset[ADC_ACC_GIER] = adc_cal_data[2] / ADC_CAL_COUNT_MAX;
-
-			adc_calibrate(ADC_CAL_ACC_COMPLETE);
-		}
-		return PITC_REMOVE_TIMER;
-	}
-}
-
-static struct pitc_timer adc_cal_timer = {
-	.interval = 1,
-	.func = &adc_calibrate_cb,
-};
-
-void adc_trigger(void)
-{
-	// TODO: err in retvalue?
-	if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC)))
-		*AT91C_ADC_CR = AT91C_ADC_START;
-}
-
-void adc_get_results(int16_t *adc_result)
-{
-	// TODO: err in retvalue?
-	if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) {
-		uint32_t i;
-		for (i = ADC_GYRO_NICK; i <= ADC_GYRO_GIER; i++)
-			adc_result[i] = (int16_t)(adc_offset[i]) - (int16_t)(adc_tmp[i]);
-
-		for (i = ADC_ACC_NICK; i <= ADC_ACC_GIER; i++)
-			adc_result[i] = (int16_t)(adc_tmp[i]) - (int16_t)(adc_offset[i]);
-
-		/* (adc / 1024) * 3.3V * (11k / 1k) * 100 */
-		adc_result[ADC_VOLTAGE] = ((uint32_t)adc_tmp[ADC_VOLTAGE] * 3630) / 1024;
-	}
-}
-
-void adc_calibrate(uint32_t mode)
-{
-	/* disable interrupt */
-	*AT91C_ADC_IDR = AT91C_ADC_ENDRX;
-
-	switch (mode) {
-	case ADC_CAL_GYRO_COMPLETE:
-		adc_status &= ~ADC_CAL_GYRO;
-		break;
-
-	case ADC_CAL_ACC_COMPLETE:
-		twi_write_eeprom(EE_ACC_CAL_DATA,
-				(uint8_t *)&(adc_offset[ADC_ACC_NICK]),
-				EE_ACC_CAL_DATA_SIZE);
-
-		adc_status &= ~ADC_CAL_ACC;
-		break;
-
-	case ADC_CAL_ACC_LOAD:
-		twi_read_eeprom(EE_ACC_CAL_DATA,
-				(uint8_t *)&(adc_offset[ADC_ACC_NICK]),
-				EE_ACC_CAL_DATA_SIZE);
-		break;
-
-	case ADC_CAL_GYRO:
-	case ADC_CAL_ACC:
-		/* calibration in progress.. */
-		if (adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))
-			break;
-
-		adc_status |= mode;
-		adc_cal_count = 0;
-		adc_cal_data[0] = 0;
-		adc_cal_data[1] = 0;
-		adc_cal_data[2] = 0;
-		pitc_schedule_timer(&adc_cal_timer);
-
-		/* trigger next cycle */
-		*AT91C_ADC_CR = AT91C_ADC_START;
-		break;
-	}
-
-	if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) {
-		printf("ADC offsets: %d/%d/%d %d/%d/%d\n\r",
-			adc_offset[ADC_GYRO_NICK],
-			adc_offset[ADC_GYRO_ROLL],
-			adc_offset[ADC_GYRO_GIER],
-			adc_offset[ADC_ACC_NICK],
-			adc_offset[ADC_ACC_ROLL],
-			adc_offset[ADC_ACC_GIER]);
-	}
-
-	/* enable interrupt */
-	*AT91C_ADC_IER = AT91C_ADC_ENDRX;
-}
-
-void adc_drift_adjust(int16_t nick, int16_t roll, int16_t yaw)
-{
-	adc_offset[ADC_GYRO_NICK] += nick;
-	adc_offset[ADC_GYRO_ROLL] += roll;
-	adc_offset[ADC_GYRO_GIER] += yaw;
-}
-
-void at91_adc_init(void)
-{
-	/* enable ADC clock */
-	*AT91C_PMC_PCER = (1 << AT91C_ID_ADC);
-
-	/* ADC Software reset */
-	AT91S_ADC *adc = AT91C_BASE_ADC;
-	adc->ADC_CR = AT91C_ADC_SWRST;
-
-	/*
-	 * ADC config: 10bit, no sleep
-	 * 4.8MHz (48MHz / ((4 +1) * 2) = 4.8MHz)
-	 * 96 cycles Startup ((11 +1) * 8 / 4.8MHz = 20us)
-	 * 3 cycles SH ((2 +1) / 4.8MHz = 625ns)
-	 * Conversion time per channel @5MHz ~2us
-	 */
-	adc->ADC_MR = AT91C_ADC_TRGEN_DIS |
-			AT91C_ADC_LOWRES_10_BIT |
-			AT91C_ADC_SLEEP_NORMAL_MODE |
-			(AT91C_ADC_PRESCAL & (4 << 8)) |
-			(AT91C_ADC_STARTUP & (11 << 16)) |
-			(AT91C_ADC_SHTIM & (2 << 24));
-
-	/* setup PDC */
-	AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
-	pdc->PDC_RPR = (uint32_t) &adc_tmp;
-	pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
-	pdc->PDC_PTCR = AT91C_PDC_RXTEN;
-
-	/* enable 7 channels (0-1-2-4-5-6-7), PDC Interrupt */
-	adc->ADC_CHER = 0xF7;
-	adc->ADC_IER = AT91C_ADC_ENDRX;
-
-	/* low priority, level triggered, own vector */
-	AT91S_AIC *aic = AT91C_BASE_AIC;
-	aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
-	aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
-	aic->AIC_IECR = (1<<AT91C_ID_ADC);
-
-	adc_calibrate(ADC_CAL_ACC_LOAD);
-	adc_calibrate(ADC_CAL_GYRO);
-}
--- a/src/at91_dbgu.c
+++ b/src/at91_dbgu.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - Debug Unit RS232 Port                                        *
- *                                                                         *
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -26,18 +24,27 @@
 #include "at91_sysc.h"
 #include "fifo.h"

-#define DBGU_BAUDRATE	115200
-#define DBGU_FIFO_SIZE	1024
-#define DBGU_TX_CHUNKS	16
-
 static struct fifo *txfifo;

 static void dbgu_isr(uint32_t status)
 {
 	/* only enabled interrupts */
 	status &= *AT91C_DBGU_IMR;
+/*
+	if (status & AT91C_US_TXEMPTY) {
+		static char c;
+		if (c == '\n') {
+			*AT91C_DBGU_THR = '\r';
+			c = 0;
+
+		} else if (fifo_getbyte(&txfifo, &c) == 1)
+			*AT91C_DBGU_THR = c;
+		else
+			*AT91C_DBGU_IDR = AT91C_US_TXEMPTY;
+	}
+*/
 	if (status & AT91C_US_TXBUFE)
-		if (fifo_txpdc(txfifo, AT91C_BASE_PDC_DBGU, DBGU_TX_CHUNKS) == 0)
+		if (fifo_txpdc(txfifo, AT91C_BASE_PDC_DBGU, 32) == 0)
 			*AT91C_DBGU_IDR = AT91C_US_TXBUFE;
 }

@ -48,11 +55,11 @@ void at91_dbgu_init(void)

 	/* enable Debug Port with 115200 Baud (+0.16%) */
 	AT91S_DBGU *dbgu = AT91C_BASE_DBGU;
-	dbgu->DBGU_BRGR = BAUD_TO_DIV(DBGU_BAUDRATE);
+	dbgu->DBGU_BRGR = BAUD_TO_DIV(115200);
 	dbgu->DBGU_MR = AT91C_US_PAR_NONE | AT91C_US_CHMODE_NORMAL;
 	dbgu->DBGU_CR = AT91C_US_RXEN | AT91C_US_TXEN | AT91C_US_RSTSTA;

-	txfifo = fifo_alloc(DBGU_FIFO_SIZE);
+	txfifo = fifo_alloc(16384);

 	/* enable TX PDC */
 	dbgu->DBGU_PTCR = AT91C_PDC_TXTEN;
@ -63,18 +70,21 @@ void at91_dbgu_init(void)
 void at91_dbgu_putc(char c)
 {
 	fifo_putbyte(txfifo, c);
+//	*AT91C_DBGU_IER = AT91C_US_TXEMPTY;
 	*AT91C_DBGU_IER = AT91C_US_TXBUFE;
 }

 void at91_dbgu_puts(const char *p)
 {
 	fifo_put(txfifo, (char *)p, strlen(p));
+//	*AT91C_DBGU_IER = AT91C_US_TXEMPTY;
 	*AT91C_DBGU_IER = AT91C_US_TXBUFE;
 }

 int at91_dbgu_write(void *base, const char *buf, size_t len)
 {
 	int retval = fifo_put(txfifo, (char *)buf, len);
+//	*AT91C_DBGU_IER = AT91C_US_TXEMPTY;
 	*AT91C_DBGU_IER = AT91C_US_TXBUFE;
 	return retval;
 }
--- a/src/at91_exceptions.c
+++ b/src/at91_exceptions.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - Abort Handler with Register/Stackdump                        *
- *                                                                         *
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -183,8 +181,9 @@ __attribute__((naked)) void ABT_Handler(void)
 		"mov	r3, lr					\n\t"

 		/* enter previous mode and get lr(r14), sp(r13) */
-		"orr	r1, r0, #I_BIT | F_BIT			\n\t"
-		"msr	CPSR_c, r1				\n\t"
+		/* TODO: interrupts might be enabled? */
+		/* TODO: thumb mode enabled? */
+		"msr	CPSR_c, r0				\n\t"
 		"mov	r1, sp					\n\t"
 		"mov	r2, lr					\n\t"

@ -195,7 +194,7 @@ __attribute__((naked)) void ABT_Handler(void)
 		"stmfd	sp!, { r1-r3 }				\n\t"
 		"mov	r1, sp					\n\t"

-		/* execute C Handler (cpsr, *registers) */
+		/* execute C Handler (cpsr, registers) */
 		"ldr	r5, =at91_abt_handler			\n\t"
 		"mov	lr, pc					\n\t"
 		"bx	r5					\n\t"
--- a/src/at91_pio.c
+++ b/src/at91_pio.c
@ -1,71 +0,0 @@
-/***************************************************************************
- *   sam7fc - Pinchange Interrupt Handler                                  *
- *                                                                         *
- *   Copyright (C) 01/2008 by Olaf Rempel                                  *
- *   razzor@kopf-tisch.de                                                  *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; version 2 of the License                *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#include <stdint.h>
-#include <stdio.h>
-#include "AT91SAM7S256.h"
-#include "board.h"
-#include "at91_pio.h"
-
-/* extern symbols, defined in ldscript */
-extern struct pio_pinchange_isr _pio_isr_table;
-extern struct pio_pinchange_isr _pio_isr_table_end;
-
-static void pio_isr(void)
-{
-	uint32_t status = *AT91C_PIOA_ISR;
-	uint32_t input = *AT91C_PIOA_PDSR;
-
-	struct pio_pinchange_isr *isr;
-	for (isr = &_pio_isr_table; isr < &_pio_isr_table_end; isr++)
-		if (isr->mask & status)
-			isr->func(status, input);
-}
-
-void pio_trigger_isr(uint32_t mask)
-{
-	uint32_t input = *AT91C_PIOA_PDSR;
-
-	struct pio_pinchange_isr *isr;
-	for (isr = &_pio_isr_table; isr < &_pio_isr_table_end; isr++)
-		if (isr->mask & mask)
-			isr->func(mask, input);
-}
-
-void at91_pio_init(void)
-{
-	/* enable PIO clock */
-	*AT91C_PMC_PCER = (1 << AT91C_ID_PIOA);
-
-	/* enable pinchange interrupts */
-	struct pio_pinchange_isr *isr;
-	for (isr = &_pio_isr_table; isr < &_pio_isr_table_end; isr++)
-		*AT91C_PIOA_IER = isr->mask;
-
-	/* dummy read to clear interrupts */
-	uint32_t dummy = *AT91C_PIOA_ISR;
-	dummy = dummy;
-
-	/* low priority, level triggered, own vector */
-	AT91S_AIC *aic = AT91C_BASE_AIC;
-	aic->AIC_SMR[AT91C_ID_PIOA] = IRQPRIO_PIOA | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
-	aic->AIC_SVR[AT91C_ID_PIOA] = (uint32_t)pio_isr;
-	aic->AIC_IECR = (1 << AT91C_ID_PIOA);
-}
--- a/src/at91_pitc.c
+++ b/src/at91_pitc.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - Periodic Timer Handling                                      *
- *                                                                         *
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -23,15 +21,13 @@
 #include "at91_sysc.h"
 #include "board.h"

-#define PITC_HZ		1000
-
 /* PIV is 20bit -> min. 3Hz @48MHz MCK */
 #define HZ_TO_PIV(HZ) (MCK / (16 * HZ))

 static LIST_HEAD(timer_list);
 volatile static uint32_t pitc_ticks;

-static void _pitc_schedule_timer(struct pitc_timer *timer)
+void pitc_schedule_timer(struct pitc_timer *timer)
 {
 	timer->nextrun = timer->interval + pitc_ticks;

@ -43,26 +39,6 @@ static void _pitc_schedule_timer(struct pitc_timer *timer)
 	list_add_tail(&timer->list, &search->list);
 }

-void pitc_schedule_timer(struct pitc_timer *timer)
-{
-	/* disable PITC interrupt */
-	*AT91C_PITC_PIMR &= ~AT91C_PITC_PITIEN;
-
-	/* check it timer is already running */
-	if (timer->nextrun == 0 && timer->interval > 0)
-		_pitc_schedule_timer(timer);
-
-	// TODO: if timer is running, interval changes are delayed
-
-	/* enable PITC interrupt */
-	*AT91C_PITC_PIMR |= AT91C_PITC_PITIEN;
-}
-
-void pitc_remove_timer(struct pitc_timer *timer)
-{
-	timer->interval = 0;
-}
-
 static void pitc_isr(uint32_t status)
 {
 	/* get Ticks and clear interrupt */
@ -78,14 +54,13 @@ static void pitc_isr(uint32_t status)
 		list_del(&search->list);

 		/* exec handler */
-		if ((search->interval == 0) || search->func(search) == PITC_REMOVE_TIMER) {
+		if (search->func(search) == PITC_REMOVE_TIMER) {
 			/* one-shot timer, mark as completed */
-			search->nextrun = 0;
+			search->nextrun = 0x00;
 			continue;
 		}
-
 		/* interval timer, reschedule it */
-		_pitc_schedule_timer(search);
+		pitc_schedule_timer(search);
 	}
 }

@ -98,7 +73,7 @@ void at91_pitc_init(void)
 {
 	sysc_register_isr(AT91_SYSIRQ_PIT, &pitc_isr);

-	*AT91C_PITC_PIMR = (AT91C_PITC_PIV & HZ_TO_PIV(PITC_HZ)) |
+	*AT91C_PITC_PIMR = (AT91C_PITC_PIV & HZ_TO_PIV(100)) |
 			   AT91C_PITC_PITEN |
 			   AT91C_PITC_PITIEN;
 }
--- a/src/at91_rttc_test.c
+++ b/src/at91_rttc_test.c
@ -1,48 +0,0 @@
-/***************************************************************************
- *   sam7fc - Real Time Clock Calibration                                  *
- *                                                                         *
- *   Copyright (C) 02/2008 by Olaf Rempel                                  *
- *   razzor@kopf-tisch.de                                                  *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; version 2 of the License                *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#include <stdio.h>
-#include "AT91SAM7S256.h"
-#include "at91_sysc.h"
-#include "board.h"
-
-static void rtt_isr(uint32_t status)
-{
-	*AT91C_RTTC_RTAR = *AT91C_RTTC_RTVR +1;
-}
-
-void at91_rttc_test_init(void)
-{
-	/* calculate SLOWCK from MAINCK and measured MAINF */
-	uint32_t prescaler = MAINCK * 16 / (*AT91C_CKGR_MCFR & AT91C_CKGR_MAINF);
-
-	sysc_register_isr(AT91_SYSIRQ_RTT, &rtt_isr);
-
-	/*
-	 * AT91C_RTTC_RTTINCIEN doesn't work
-	 * use AT91C_RTTC_ALMIEN and increment RTAR in isr
-	 */
-	*AT91C_RTTC_RTAR = *AT91C_RTTC_RTVR +1;
-	*AT91C_RTTC_RTMR = (AT91C_RTTC_RTPRES & prescaler) |
-			   AT91C_RTTC_ALMIEN |
-			   AT91C_RTTC_RTTRST;
-
-	printf("rttc running at %ld Hz\n\r", prescaler);
-}
--- a/src/at91_sysc.c
+++ b/src/at91_sysc.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - System Interrupt Dispatcher                                  *
- *                                                                         *
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -33,15 +31,15 @@ struct _irq_regs {

 /* same IDs as sysc_isrs enum! */
 static const struct _irq_regs irq_regs[] = {
-	/*
+	/* 
 	 * PIT (Periodic Intervall Timer)
 	 * ISR must read AT91C_PITC_PIVR to clear interrupt
 	 * - PITIEN (Periodic Intervall Timer Interrupt Enable)
-	 */
+	 */ 
 	{	AT91C_PITC_PIMR, AT91C_PITC_PITIEN,
 		AT91C_PITC_PISR, AT91C_PITC_PITS
 	},
-
+	
 	/*
 	 * DBGU (Debug Unit)
 	 * TODO: must use AT91C_DBGU_IDR to fault disable interrupt
@ -60,12 +58,12 @@ static const struct _irq_regs irq_regs[] = {
 				AT91C_US_TXBUFE | AT91C_US_RXBUFF |
 				AT91C_US_COMM_TX | AT91C_US_COMM_RX)
 	},
-
+	
 	/*
 	 * EFC (Embedded Flash Controller / Memory Controller)
 	 */
 	{	AT91C_MC_FMR, (AT91C_MC_FRDY | AT91C_MC_LOCKE | AT91C_MC_PROGE),
-		AT91C_MC_FSR, (AT91C_MC_FRDY | AT91C_MC_LOCKE | AT91C_MC_PROGE)
+		AT91C_MC_FSR, (AT91C_MC_FRDY | AT91C_MC_LOCKE | AT91C_MC_PROGE)	
 	},

 	/*
@ -86,7 +84,7 @@ static const struct _irq_regs irq_regs[] = {
 	{	AT91C_RTTC_RTMR, (AT91C_RTTC_ALMIEN | AT91C_RTTC_RTTINCIEN),
 		AT91C_RTTC_RTSR, (AT91C_RTTC_ALMS | AT91C_RTTC_RTTINC)
 	},
-
+	
 	/*
 	 * RSTC (Reset Controller)
 	 * - AT91C_RSTC_URSTIEN (User Reset Interrupt Enable)
@ -106,27 +104,27 @@ static const struct _irq_regs irq_regs[] = {
 				AT91C_PMC_PCK1RDY | AT91C_PMC_PCK2RDY),
 		AT91C_PMC_SR, (AT91C_PMC_MOSCS | AT91C_PMC_LOCK |
 				AT91C_PMC_MCKRDY | AT91C_PMC_PCK0RDY |
-				AT91C_PMC_PCK1RDY | AT91C_PMC_PCK2RDY)
+				AT91C_PMC_PCK1RDY | AT91C_PMC_PCK2RDY)	
 	},
-};
+};	

 /* check Interrupts from internal hardware */
 static void at91_sysc_isr(void)
 {
 	uint32_t id;
 	for (id = 0; id < AT91_SYSIRQ_COUNT; id++) {
-
+		
 		/* check if interrups are enabled */
 		if (*(irq_regs[id].mask_register) & irq_regs[id].mask) {

 			/* check if interrupts are active */
 			uint32_t status = *(irq_regs[id].status_register);
 			if (status & irq_regs[id].status) {
-
+				
 				/* execute handler */
 				if (sysc_isrs[id]) {
 					sysc_isrs[id](status);
-
+					
 				} else {
 					/* disable interrupts */
 					*(irq_regs[id].mask_register) &= ~irq_regs[id].mask;
@ -140,9 +138,9 @@ void at91_sysc_init(void)
 {
 	/* high priority, level triggered, own vector */
 	AT91S_AIC *aic = AT91C_BASE_AIC;
-	aic->AIC_SMR[AT91C_ID_SYS] = IRQPRIO_SYSC |
+	aic->AIC_SMR[AT91C_ID_SYS] = IRQPRIO_SYSC | 
 				     AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
-
+	
 	aic->AIC_SVR[AT91C_ID_SYS] = (uint32_t)at91_sysc_isr;
 	aic->AIC_IECR = (1<<AT91C_ID_SYS);
 }
@ -151,6 +149,6 @@ void sysc_register_isr(enum syscirqs irq, SYSCISR *isr)
 {
 	if (irq >= AT91_SYSIRQ_COUNT)
 		return;
-
+	
 	sysc_isrs[irq] = isr;
 }
--- a/src/at91_tc1.c
+++ b/src/at91_tc1.c
@ -1,257 +0,0 @@
-/***************************************************************************
- *   sam7fc - RC-PPM Signal decoder                                        *
- *                                                                         *
- *   Copyright (C) 01/2008 by Olaf Rempel                                  *
- *   razzor@kopf-tisch.de                                                  *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; version 2 of the License                *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>		/* abs() */
-#include "AT91SAM7S256.h"
-#include "board.h"
-#include "at91_tc1.h"
-#include "at91_twi.h"
-
-/* Hard limits for ISR */
-#define PULSE_MIN	0x0500
-#define PULSE_MAX	0x0D00
-#define PULSE_TIMEOUT	0x0F00
-
-#define PULSE_CENTER	0x08C0
-#define PULSE_SWITCH	0x01F0
-
-/* moving average filters */
-#define PULSE_FILTER_FAST	(1<<2)
-#define PULSE_FILTER_SLOW	(1<<4)
-#define PULSE_FILTER_DIFF	16		/* point to switch filters */
-#define PULSE_MID_DIFF		50		/* minimum diff to center */
-
-#define VALUE_RANGE		256
-
-#define ROUND_DIV256(x) ((x >> 8) + ((x & 0x80) ? 1 : 0))
-
-struct channel_data {
-	uint16_t width;
-	uint16_t width_slow;
-	uint16_t filter;	/* 0 - fast filter, 1 - slow filter */
-	uint16_t min;		/* minimum value during calibration */
-	uint16_t mid;		/* center value */
-	uint16_t max;		/* maximum value */
-};
-
-/* check eeprom parameter size (uint16_t min/mid/max) */
-#if ((MAX_CHANNELS * 3 * 2) != EE_RC_CAL_DATA_SIZE)
-#error "invalid EE_RC_CAL_DATA_SIZE"
-#endif
-
-static struct channel_data ch_data[MAX_CHANNELS];
-static uint32_t count, valid, cal_in_progress;
-
-static void ppm_isr(void)
-{
-	static uint32_t i;
-
-	/* RC Compare -> no TIOA1 edge for 2.5ms */
-	uint32_t status = *AT91C_TC1_SR;
-
-	if (status & AT91C_TC_CPCS) {
-		/* average channel count */
-		count = ((count * 7) + (i << 8)) / 8;
-
-		/* at least 4 channels and a stable channel count */
-		if ((ROUND_DIV256(count) == i) && (i >= 4)) {
-			if (valid < 10)
-				valid++;
-
-		} else if (valid > 0) {
-			valid--;
-		}
-
-		/* reset index */
-		i = 0;
-	}
-
-	/* edge on TIOA1 */
-	if (status & AT91C_TC_LDRAS) {
-		/* get impulse width */
-		uint16_t width = *AT91C_TC1_RA;
-
-		/* valid range: 1 - 2ms */
-		if (width > PULSE_MIN && width < PULSE_MAX) {
-			if (i < ARRAY_SIZE(ch_data)) {
-				/* calc both filters */
-				ch_data[i].width = ((ch_data[i].width * (PULSE_FILTER_FAST -1)) + width) / PULSE_FILTER_FAST;
-				ch_data[i].width_slow = ((ch_data[i].width_slow * (PULSE_FILTER_SLOW -1)) + width) / PULSE_FILTER_SLOW;
-
-				if (cal_in_progress) {
-					/* use slow filter values, calc center */
-					ch_data[i].min = MIN(ch_data[i].width_slow, ch_data[i].min);
-					ch_data[i].max = MAX(ch_data[i].width_slow, ch_data[i].max);
-					ch_data[i].mid = (ch_data[i].min + ch_data[i].max) / 2;
-				}
-			}
-			i++;
-		}
-	}
-}
-
-uint32_t rcontrol_getvalues(struct rc_values *rc)
-{
-	if (valid < 5)
-		return 0;
-
-	uint32_t i;
-	uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
-	for (i = 0; i < cnt; i++) {
-		/* switch between fast and slow filter */
-		uint16_t filter = (abs(ch_data[i].width - ch_data[i].width_slow) < PULSE_FILTER_DIFF);
-
-		/*
-		 * transition fast -> slow filter
-		 * slow filter is lagging behind, so give it a boost
-		 */
-		if (filter && !ch_data[i].filter && !cal_in_progress)
-			ch_data[i].width_slow = ch_data[i].width;
-
-		ch_data[i].filter = filter;
-
-		uint16_t width = (filter) ? ch_data[i].width_slow : ch_data[i].width;
-
-		/* expand the value to +/- VALUE_RANGE */
-		int32_t tmp = (uint32_t)(width - ch_data[i].mid) * VALUE_RANGE;
-		tmp = tmp / ((tmp > 0) ? (ch_data[i].max - ch_data[i].mid) : (ch_data[i].mid - ch_data[i].min));
-
-		// TODO: stick mapping
-		/* keep result in range */
-		rc->chan[i] = LIMIT(tmp, -VALUE_RANGE, +VALUE_RANGE);
-	}
-	return cnt;
-}
-
-uint32_t rcontrol_getswitches(struct rc_values *rc)
-{
-	if (valid < 5)
-		return 0;
-
-	uint32_t i;
-	uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
-	for (i = 0; i < cnt; i++) {
-		if (ch_data[i].width > (PULSE_CENTER + PULSE_SWITCH))
-			rc->chan[i] = VALUE_RANGE;
-
-		else if (ch_data[i].width < (PULSE_CENTER - PULSE_SWITCH))
-			rc->chan[i] = -VALUE_RANGE;
-
-		else
-			rc->chan[i] = 0;
-	}
-	return cnt;
-}
-
-void rcontrol_calibrate(uint32_t mode)
-{
-	uint32_t i;
-	uint8_t buf[EE_RC_CAL_DATA_SIZE];
-	uint16_t *ptr = (uint16_t *)buf;
-
-	switch (mode) {
-	case RC_CAL_START:
-		cal_in_progress = 1;
-		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
-			/* use hard limits as hint */
-			ch_data[i].max = PULSE_MIN;
-			ch_data[i].mid = (PULSE_MIN + PULSE_MAX) / 2;
-			ch_data[i].min = PULSE_MAX;
-		}
-		break;
-
-	case RC_CAL_END:
-		cal_in_progress = 0;
-		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
-			/* treat current position as center */
-			ch_data[i].mid = ch_data[i].width_slow;
-
-			/* if center is near minimum, clamp output to 0..+RANGE */
-			if (ch_data[i].mid - ch_data[i].min < PULSE_MID_DIFF)
-				ch_data[i].mid = ch_data[i].min;
-
-			/* if center is near maximum, clamp output to -RANGE..0 */
-			if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF)
-				ch_data[i].mid = ch_data[i].max;
-		}
-		break;
-
-	case RC_CAL_LOAD:
-		twi_read_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
-		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
-			ch_data[i].min = *ptr++;
-			ch_data[i].mid = *ptr++;
-			ch_data[i].max = *ptr++;
-		}
-		break;
-
-	case RC_CAL_SAVE:
-		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
-			*ptr++ = ch_data[i].min;
-			*ptr++ = ch_data[i].mid;
-			*ptr++ = ch_data[i].max;
-		}
-		twi_write_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
-		break;
-	}
-}
-
-void rcontrol_print_cal(void)
-{
-	uint32_t i;
-	printf("stick-calibration:\n\r");
-	for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
-		printf(" %ld: %d(%+d) - %d(0) - %d(%+d)\n\r", i,
-			ch_data[i].min, ch_data[i].min - ch_data[i].mid,
-			ch_data[i].mid,
-			ch_data[i].max, ch_data[i].max - ch_data[i].mid
-			);
-	}
-}
-
-void at91_tc1_init(void)
-{
-	/* enable TC1 clock */
-	*AT91C_PMC_PCER = (1 << AT91C_ID_TC1);
-
-	/* MCK /32, trigger & capture on falling TIOA1 edge */
-	*AT91C_TC1_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | AT91C_TC_LDRA_FALLING |
-			AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG;
-
-	/* enable RA load and RC compare interrupt */
-	*AT91C_TC1_IER = AT91C_TC_LDRAS | AT91C_TC_CPCS;
-
-	/* RC Compare Interrupt if no rising Edge on TIOA1 for 2.56ms */
-	*AT91C_TC1_RC = PULSE_TIMEOUT;
-
-	/* enable & trigger the clock */
-	*AT91C_TC1_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
-
-	/* level triggered, own vector */
-	AT91S_AIC *aic = AT91C_BASE_AIC;
-	aic->AIC_SMR[AT91C_ID_TC1] = IRQPRIO_TC1 | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
-	aic->AIC_SVR[AT91C_ID_TC1] = (uint32_t)ppm_isr;
-	aic->AIC_IECR = (1 << AT91C_ID_TC1);
-
-	rcontrol_calibrate(RC_CAL_LOAD);
-	rcontrol_print_cal();
-}
--- a/src/at91_twi.c
+++ b/src/at91_twi.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - TWI/I2C Handling                                             *
- *                                                                         *
 *   Copyright (C) 02/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -24,6 +22,7 @@
 #include "AT91SAM7S256.h"
 #include "board.h"
 #include "at91_twi.h"
+#include "at91_pio.h"

 /*
 * undocumented TWI_SR flags, at least OVRE seems to be present on a sam7s256
@ -62,6 +61,8 @@ static void twi_isr(void)
 	if (status & AT91C_TWI_NACK) {
 		*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
 		twi_state = TWI_ERROR;
+
+		// TODO: TWI_BLMC_UPDATE?
 		return;
 	}

@ -94,15 +95,19 @@ static void twi_isr(void)

 	/* transfer really complete? */
 	if (status & AT91C_TWI_TXCOMP) {
-		uint32_t addr = (*AT91C_TWI_MMR >> 16) & 0x7F;

 		/* are we doing a blmc update? */
-		if (twi_state == TWI_BLMC_UPDATE && addr != TWI_ADDR_BL4) {
-			/* increase address */
-			*AT91C_TWI_MMR += (1<<16);
+		if (twi_state == TWI_BLMC_UPDATE) {
+			uint32_t addr = (*AT91C_TWI_MMR >> 16) & 0x7F;
+			if (addr != TWI_ADDR_BL4) {
+				/* increase address */
+				*AT91C_TWI_MMR += (1<<16);

-			/* send next value to next blmc */
-			*AT91C_TWI_THR = *twi_data++;
+				/* send next value to next blmc */
+				*AT91C_TWI_THR = *twi_data++;
+			} else {
+				// TODO:
+			}

 		} else {
 			*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
@ -113,14 +118,11 @@ static void twi_isr(void)

 uint32_t twi_setpwm(uint8_t *values)
 {
-	if (twi_state == TWI_ERROR)
-		twi_state = TWI_IDLE;
-
 	if (twi_state != TWI_IDLE)
 		return 1;

 	twi_state = TWI_BLMC_UPDATE;
-	twi_data = values;
+	twi_data = values;		/* data is not copied! */
 	twi_size = 0;
 	twi_count = 0;

@ -133,11 +135,8 @@ uint32_t twi_setpwm(uint8_t *values)
 	return 0;
 }

-uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd)
+uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd)
 {
-	if (twi_state == TWI_ERROR)
-		twi_state = TWI_IDLE;
-
 	if (twi_state != TWI_IDLE)
 		return 1;

@ -145,7 +144,7 @@ uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd)
 	twi_state = TWI_GENERIC_CMD;

 	/* read transfer, or write transfer with payload */
-	if (cmd->mode & TWI_MODE_READ || cmd->size != 0) {
+	if (cmd->mode & BLMC_CMD_READ || cmd->size != 0) {
 		/* set address, direction, argument count and command bytes */
 		*AT91C_TWI_MMR = (addr << 16) | (cmd->mode & 0xFF) << 8;
 		*AT91C_TWI_IADR = cmd->cmd;
@ -163,7 +162,7 @@ uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd)
 	twi_size = cmd->size;
 	twi_count = 0;

-	if (cmd->mode & TWI_MODE_READ) {
+	if (cmd->mode & BLMC_CMD_READ) {
 		*AT91C_TWI_CR = AT91C_TWI_START;
 		*AT91C_TWI_IER = AT91C_TWI_RXRDY | AT91C_TWI_NACK;

@ -186,113 +185,47 @@ uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd)
 	return 0;
 }

-uint32_t twi_read_eeprom(uint32_t addr, uint8_t *buf, uint32_t size)
-{
-	struct twi_cmd cmd = {
-		.cmd = (addr & 0x7FFF),
-		.mode = TWI_MODE_READ | TWI_MODE_1_ARG,
-		.size = (size & 0x7FFF),
-		.data = buf,
-	};
-
-	if (twi_cmd(TWI_ADDR_EEPROM, &cmd) != 0)
-		size = 0;
-
-	return size;
-}
-
-uint32_t twi_write_eeprom(uint32_t addr, uint8_t *buf, uint32_t size)
-{
-	uint32_t len = size;
-
-	while (len > 0) {
-		uint32_t count = 0x40 - (addr & 0x3F);
-		if (count > len)
-			count = len;
-
-		/* TODO: write complete polling */
-		volatile uint32_t x;
-		for (x = 0; x < 200000; x++);
-
-		struct twi_cmd cmd = {
-			.cmd = (addr & 0x7FFF),
-			.mode = TWI_MODE_WRITE | TWI_MODE_1_ARG,
-			.size = count,
-			.data = buf,
-		};
-
-		if (twi_cmd(TWI_ADDR_EEPROM, &cmd) != 0)
-			break;
-
-		addr += count;
-		buf += count;
-		len -= count;
-	}
-
-	return size - len;
-}
-
 void at91_twi_test(void)
 {
 	uint32_t i;
 	for (i = TWI_ADDR_BL1; i <= TWI_ADDR_BL4; i++) {
-		printf("twi[0x%02lx] ", i);
-
-		struct twi_cmd cmd = {
+		struct blmc_cmd cmd = {
 			.cmd = CMD_BOOT_LOADER,
-			.mode = TWI_MODE_WRITE | TWI_MODE_0_ARG,
+			.mode = BLMC_CMD_WRITE | BLMC_CMD_0_ARG,
 		};
-		twi_cmd(i, &cmd);
+		uint32_t ret = twi_cmd(i, &cmd);
+		printf("twi[0x%02lx](%ld) ", i, ret);

-		/* TODO: sleep */
 		volatile uint32_t x;
 		for (x = 0; x < 200000; x++);

 		uint8_t buf[16];
-		buf[0] = '\0';
-		cmd.cmd = CMD_GET_INFO;
-		cmd.mode = TWI_MODE_READ | TWI_MODE_0_ARG;
-		cmd.size = sizeof(buf);
-		cmd.data = buf;
-		twi_cmd(i, &cmd);
-		printf("boot:'%s' ", buf);
-
-		/* TODO: single 32bit write */
-		buf[0] = 0xFF;
-		buf[1] = 0xFF;
-		buf[2] = 0xFF;
+		cmd.cmd = CMD_GET_INFO,
+		cmd.mode = BLMC_CMD_READ | BLMC_CMD_0_ARG,
+		cmd.size = sizeof(buf),
+		cmd.data = buf,
+		ret = twi_cmd(i, &cmd);
+		printf("boot(%ld):'%s' ", ret, buf);

 		cmd.cmd = CMD_GET_SIGNATURE;
 		cmd.size = 4;
-		twi_cmd(i, &cmd);
-		printf("sig:0x%02x%02x%02x\n\r", buf[0], buf[1], buf[2]);
-
+		ret = twi_cmd(i, &cmd);
+		printf("sig(%ld):0x%02x%02x%02x\n\r", ret, buf[0], buf[1], buf[2]);
+/*
 		cmd.cmd = CMD_BOOT_APPLICATION;
-		cmd.mode = TWI_MODE_WRITE | TWI_MODE_0_ARG;
+		cmd.mode = BLMC_CMD_WRITE | BLMC_CMD_0_ARG;
 		cmd.size = 0;
-		twi_cmd(i, &cmd);
+		ret = twi_cmd(i, &cmd);

-		/* TODO: sleep */
-		for (x = 0; x < 200000; x++);
+		for (x = 0; x < 400000; x++);

-		buf[0] = '\0';
-		cmd.cmd = CMD_GET_INFO;
-		cmd.mode = TWI_MODE_READ | TWI_MODE_0_ARG;
-		cmd.size = sizeof(buf);
-		cmd.data = buf;
-		twi_cmd(i, &cmd);
-		printf("          app :'%s' ", buf);
-
-		struct blmc_param param;
-		cmd.cmd = CMD_GET_PARAM;
-		cmd.mode = TWI_MODE_READ | TWI_MODE_0_ARG;
-		cmd.size = sizeof(param);
-		cmd.data = (uint8_t *)&param;
-		twi_cmd(i, &cmd);
-
-		printf("pwm:0x%02x-0x%02x Ilimit:0x%03x Imax:0x%03x\n\r",
-			param.pwm_min, param.pwm_max,
-			param.current_limit, param.current_max);
+		cmd.cmd = CMD_GET_INFO,
+		cmd.mode = BLMC_CMD_READ | BLMC_CMD_0_ARG,
+		cmd.size = sizeof(buf),
+		cmd.data = buf,
+		ret = twi_cmd(i, &cmd);
+		printf("app(%ld):'%s'\n\r", ret, buf);
+*/
 	}
 }

--- a/src/at91_udp.c
+++ b/src/at91_udp.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - USB Device Port with logical Serial Port                     *
- *                                                                         *
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -20,15 +18,16 @@
 ***************************************************************************/
 #include <stdio.h>
 #include "AT91SAM7S256.h"
-#include "at91_pio.h"
 #include "board.h"
 #include "fifo.h"
-#include "telemetrie.h"

 #include "usb_ch9.h"
-#include "usb_cdc.h"
 #include "usb_dfu.h"

+/* from dfu.c */
+extern void ep0_handle_dfu(struct usb_ctrlrequest *req);
+
+
 #define csr_clear_flags(csr, flags) \
 	while ((csr) & (flags)) \
 		(csr) &= ~(flags);
@ -69,8 +68,6 @@ static uint16_t current_address;
 static uint16_t current_config;
 static uint16_t current_interface;

-static struct comm_device usb_comm;
-
 static const struct usb_device_descriptor dev_descriptor = {
 	.bLength		= sizeof(struct usb_device_descriptor),
 	.bDescriptorType	= USB_DT_DEVICE,
@ -79,22 +76,17 @@ static const struct usb_device_descriptor dev_descriptor = {
 	.idVendor		= USB_VENDOR_ID,
 	.idProduct		= USB_PRODUCT_ID +1,
 	.bcdDevice		= 0x0001,
-	.iProduct               = 0x01,
+	.iProduct		= 0x01,
 	.bNumConfigurations	= 1,
 };
-
 struct my_config {
 	struct usb_config_descriptor		cfg;
-	struct usb_interface_descriptor		ctrl_iface;
-	struct usb_cdc_header_desc		cdc_header;
-	struct usb_cdc_call_mgmt_descriptor	cdc_call_mgmt;
-	struct usb_cdc_acm_descriptor		cdc_acm;
-	struct usb_cdc_union_desc		cdc_union;
-	struct usb_endpoint_descriptor		notify_ep;
-	struct usb_interface_descriptor		data_iface;
-	struct usb_endpoint_descriptor		dataout_ep;
-	struct usb_endpoint_descriptor		datain_ep;
-	struct usb_interface_descriptor		dfu_iface;
+	struct usb_interface_descriptor         iface0;
+	struct usb_interface_descriptor         iface1;
+	struct usb_interface_descriptor         iface2;
+	struct usb_interface_descriptor         iface3;
+	struct usb_interface_descriptor         iface4;
+	struct usb_interface_descriptor         iface5;
 	struct usb_dfu_descriptor               dfu;
 } __attribute__ ((packed));

@ -103,82 +95,65 @@ static const struct my_config cfg_descriptor = {
 	.bLength		= sizeof(struct usb_config_descriptor),
 	.bDescriptorType	= USB_DT_CONFIG,
 	.wTotalLength		= sizeof(struct my_config),
-	.bNumInterfaces		= 3,
+	.bNumInterfaces		= 6,
 	.bConfigurationValue	= 1,
+	.iConfiguration		= 0x02,
 	.bmAttributes		= USB_CONFIG_ATT_SELFPOWER | USB_CONFIG_ATT_WAKEUP,
 	.bMaxPower		= 50,
 },
-.ctrl_iface = {
+.iface0 = {
 	.bLength		= sizeof(struct usb_interface_descriptor),
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= 0,
-	.bNumEndpoints		= 1,
-	.bInterfaceClass	= USB_CLASS_COMM,
-	.bInterfaceSubClass	= USB_CDC_SUBCLASS_ACM,
-	.bInterfaceProtocol	= 1,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x03,
 },
-.cdc_header = {
-	.bLength		= sizeof(struct usb_cdc_header_desc),
-	.bDescriptorType	= USB_DT_CS_INTERFACE,
-	.bDescriptorSubType	= USB_CDC_HEADER_TYPE,
-	.bcdCDC			= 0x0110,
-},
-.cdc_call_mgmt = {
-	.bLength		= sizeof(struct usb_cdc_call_mgmt_descriptor),
-	.bDescriptorType	= USB_DT_CS_INTERFACE,
-	.bDescriptorSubType	= USB_CDC_CALL_MANAGEMENT_TYPE,
-	.bmCapabilities		= USB_CDC_CALL_MGMT_CAP_CALL_MGMT,
-	.bDataInterface		= 1,
-},
-.cdc_acm = {
-	.bLength		= sizeof(struct usb_cdc_acm_descriptor),
-	.bDescriptorType	= USB_DT_CS_INTERFACE,
-	.bDescriptorSubType	= USB_CDC_ACM_TYPE,
-	.bmCapabilities		= (USB_CDC_CAP_BRK | USB_CDC_CAP_LINE | USB_CDC_COMM_FEATURE),
-},
-.cdc_union = {
-	.bLength		= sizeof(struct usb_cdc_union_desc),
-	.bDescriptorType	= USB_DT_CS_INTERFACE,
-	.bDescriptorSubType	= USB_CDC_UNION_TYPE,
-	.bMasterInterface0	= 0,
-	.bSlaveInterface0	= 1,
-},
-.notify_ep = {
-	.bLength		= sizeof(struct usb_endpoint_descriptor),
-	.bDescriptorType	= USB_DT_ENDPOINT,
-	.bEndpointAddress	= USB_DIR_IN | 0x03,
-	.bmAttributes		= USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize		= 64,
-	.bInterval		= 10,
-},
-.data_iface = {
+.iface1 = {
 	.bLength		= sizeof(struct usb_interface_descriptor),
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= 1,
-	.bNumEndpoints		= 2,
-	.bInterfaceClass	= USB_CLASS_CDC_DATA,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x04,
 },
-.dataout_ep = {
-	.bLength		= sizeof(struct usb_endpoint_descriptor),
-	.bDescriptorType	= USB_DT_ENDPOINT,
-	.bEndpointAddress	= USB_DIR_OUT | 0x01,
-	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize		= 64,
+.iface2 = {
+	.bLength		= sizeof(struct usb_interface_descriptor),
+	.bDescriptorType	= USB_DT_INTERFACE,
+	.bInterfaceNumber	= 2,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x05,
 },
-.datain_ep = {
-	.bLength		= sizeof(struct usb_endpoint_descriptor),
-	.bDescriptorType	= USB_DT_ENDPOINT,
-	.bEndpointAddress	= USB_DIR_IN | 0x02,
-	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize		= 64,
+.iface3 = {
+	.bLength		= sizeof(struct usb_interface_descriptor),
+	.bDescriptorType	= USB_DT_INTERFACE,
+	.bInterfaceNumber	= 3,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x06,
 },
-.dfu_iface = {
-	.bLength                = sizeof(struct usb_interface_descriptor),
-	.bDescriptorType        = USB_DT_INTERFACE,
-	.bInterfaceNumber       = 2,
-	.bInterfaceClass        = USB_CLASS_APP_SPEC,
-	.bInterfaceSubClass     = 0x01, /* DFU */
-	.bInterfaceProtocol     = 0x01,
+.iface4 = {
+	.bLength		= sizeof(struct usb_interface_descriptor),
+	.bDescriptorType	= USB_DT_INTERFACE,
+	.bInterfaceNumber	= 4,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x07,
+},
+.iface5 = {
+	.bLength		= sizeof(struct usb_interface_descriptor),
+	.bDescriptorType	= USB_DT_INTERFACE,
+	.bInterfaceNumber	= 5,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x08,
 },
 .dfu = {
 	.bLength                = sizeof(struct usb_dfu_descriptor),
@ -190,20 +165,6 @@ static const struct my_config cfg_descriptor = {
 },
 };

-/* not const! */
-static struct usb_cdc_line_coding cdc_line_coding = {
-	.dwDTERate		= 9600,
-	.bCharFormat		= USB_CDC_1_STOP_BITS,
-	.bParityType		= USB_CDC_NO_PARITY,
-	.bDataBits		= 8,
-};
-
-/* not const! */
-static struct dfu_status dfu_status = {
-	.bStatus		= DFU_STATUS_OK,
-	.bState			= DFU_STATE_appIDLE,
-};
-
 static const struct usb_string_descriptor usb_string0 = {
 	/* String 0 - Language */
 	.bLength = sizeof(struct usb_string_descriptor) + 1 * sizeof(uint16_t),
@ -220,15 +181,87 @@ static const struct usb_string_descriptor usb_string1 = {
 	},
 };

+static const struct usb_string_descriptor usb_string2 = {
+	/* String 2 "sam7fc-dfu" */
+	.bLength = sizeof(struct usb_string_descriptor) + 10 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+        .wData = {
+		0x0073, 0x0061, 0x006d, 0x0037, 0x0066, 0x0063, 0x002d, 0x0064,
+		0x0066, 0x0075,
+        },
+};
+
+static const struct usb_string_descriptor usb_string3 = {
+	/* String 3 "sam7fc-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 12 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0073, 0x0061, 0x006d, 0x0037, 0x0066, 0x0063, 0x002d, 0x0066,
+		0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
+static const struct usb_string_descriptor usb_string4 = {
+	/* String 4 "sam7fc-eeprom" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0073, 0x0061, 0x006d, 0x0037, 0x0066, 0x0063, 0x002d, 0x0065,
+		0x0065, 0x0070, 0x0072, 0x006f, 0x006d,
+	},
+};
+
+static const struct usb_string_descriptor usb_string5 = {
+	/* String 5 "blctrl1-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0062, 0x006c, 0x0063, 0x0074, 0x0072, 0x006c, 0x0031, 0x002d,
+		0x0066, 0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
+static const struct usb_string_descriptor usb_string6 = {
+	/* String 6 "blctrl2-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0062, 0x006c, 0x0063, 0x0074, 0x0072, 0x006c, 0x0032, 0x002d,
+		0x0066, 0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
+static const struct usb_string_descriptor usb_string7 = {
+	/* String 7 "blctrl3-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0062, 0x006c, 0x0063, 0x0074, 0x0072, 0x006c, 0x0033, 0x002d,
+		0x0066, 0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
+static const struct usb_string_descriptor usb_string8 = {
+	/* String 8 "blctrl4-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0062, 0x006c, 0x0063, 0x0074, 0x0072, 0x006c, 0x0034, 0x002d,
+		0x0066, 0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
 static const struct usb_string_descriptor *usb_strings[] = {
-	&usb_string0, &usb_string1,
+	&usb_string0, &usb_string1, &usb_string2, &usb_string3,
+	&usb_string4, &usb_string5, &usb_string6, &usb_string7,
+	&usb_string8,
 };

 void ep_transfer_send(uint32_t ep, char *data, uint32_t length,
 				void (*complete_cb)(void))
 {
 	struct ep_ctx *ctx = &ep_ctx[ep];
-//	printf("ep_transfer_send(%ld) size=%ld flags=0x%x\n\r", ep, length, ctx->flags);
+//	printf("ep_transfer_send(%ld) size=%ld(%d) flags=0x%x\n\r", ep, length, ctx->maxpktsize, ctx->flags);
 	if (!(ctx->flags & CTX_TRANSFER) || (ctx->flags & (CTX_IN | CTX_OUT)))
 		return;

@ -270,9 +303,9 @@ void ep_transfer_receive(uint32_t ep, char *data, uint32_t length,
 }

 /* stalls the endpoint */
-static void ep_send_stall(uint32_t ep)
+void ep_send_stall(uint32_t ep)
 {
-	printf("usb stall\n\r");
+	printf("stall\n\r");
 	AT91C_UDP_CSR[ep] |= AT91C_UDP_FORCESTALL;
 }

@ -295,21 +328,15 @@ static void udp_configure_ep(const struct usb_endpoint_descriptor *desc)
 	*AT91C_UDP_IER = (1 << ep);
 }

-static void udp_print_config(void)
-{
-	printf("usb: addr=%d cfg=%d if=%d\n\r",
-		current_address, current_config, current_interface);
-}
-
 /*
 * set local address
 * (USB_REQ_SET_ADDRESS callback)
 */
 static void udp_txcb_setaddress(void)
 {
+	printf("usb address: %d\n\r", current_address);
 	*AT91C_UDP_FADDR = (AT91C_UDP_FEN | current_address);
 	*AT91C_UDP_GLBSTATE = AT91C_UDP_FADDEN;
-	udp_print_config();
 }

 /*
@ -318,19 +345,14 @@ static void udp_txcb_setaddress(void)
 */
 static void udp_txcb_setconfig(void)
 {
-	udp_configure_ep(&cfg_descriptor.notify_ep);
-	udp_configure_ep(&cfg_descriptor.datain_ep);
-	udp_configure_ep(&cfg_descriptor.dataout_ep);
-
-	ep_ctx[1].fifo = usb_comm.rxfifo;
-	ep_ctx[1].flags |= CTX_FIFO;
-
-	ep_ctx[2].fifo = usb_comm.txfifo;
-	ep_ctx[2].flags |= CTX_FIFO;
-
+	printf("usb configuration: %d\n\r", current_config);
 	/* set UDP to "configured" */
 	*AT91C_UDP_GLBSTATE = AT91C_UDP_CONFG;
-	udp_print_config();
+}
+
+static void udp_txcb_setinterface(void)
+{
+	printf("claim interface %d\n\r", current_interface);
 }

 static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
@ -399,7 +421,7 @@ static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
 		switch (req->bRequest) {
 		case USB_REQ_SET_INTERFACE: /* 0x0b */
 			current_interface = req->wValue;
-			ep_transfer_send(0, NULL, 0, udp_print_config);
+			ep_transfer_send(0, NULL, 0, udp_txcb_setinterface);
 			break;

 		default:
@ -409,33 +431,7 @@ static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
 		break;

 	case (USB_TYPE_CLASS | USB_RECIP_INTERFACE): /* 0x21/0xA1 */
-		// TODO: follow current_interface
-		switch (req->bRequest) {
-		case USB_REQ_DFU_DETACH: /* 0x00 */
-			dfu_status.bStatus = DFU_STATE_appDETACH;
-			ep_transfer_send(0, NULL, 0, NULL);
-			break;
-
-		case USB_REQ_DFU_GETSTATUS: /* 0x03 */
-			ep_transfer_send(0, (char *)&dfu_status, sizeof(dfu_status), NULL);
-			break;
-
-		case USB_CDC_REQ_SET_LINE_CODING: /* 0x20 */
-			ep_transfer_receive(0, (char *)&cdc_line_coding, sizeof(cdc_line_coding), NULL);
-			break;
-
-		case USB_CDC_REQ_GET_LINE_CODING: /* 0x21 */
-			ep_transfer_send(0, (char *)&cdc_line_coding, sizeof(cdc_line_coding), NULL);
-			break;
-
-		case USB_CDC_REQ_SET_CONTROL_LINE_STATE: /* 0x22 */
-			ep_transfer_send(0, NULL, 0, NULL);
-			break;
-
-		default:
-			ep_send_stall(0);
-			break;
-		}
+		ep0_handle_dfu(req);
 		break;

 	default:
@ -484,13 +480,9 @@ static void udp_handle_ep(uint32_t ep)

 		if (ctx->flags & CTX_FIFO) {
 			/* get data from fifo */
-			if (fifo_txudp(ctx->fifo, ep, ctx->maxpktsize)) {
+			if (fifo_txudp(ctx->fifo, ep, ctx->maxpktsize))
 				AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;

-			} else {
-				ctx->flags &= ~CTX_IN;
-			}
-
 		} else if ((ctx->flags & (CTX_TRANSFER | CTX_IN)) == (CTX_TRANSFER | CTX_IN)) {
 			/* transfer not complete */
 			struct ep_transfer *transfer = ctx->transfer;
@ -518,6 +510,7 @@ static void udp_handle_ep(uint32_t ep)
 	/* data ready to read? */
 	if (*csr & (AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1)) {
 		struct ep_ctx *ctx = &ep_ctx[ep];
+
 		uint16_t len = (*csr & AT91C_UDP_RXBYTECNT) >> 16;

 		// TODO: only ep0 status OUT?
@ -571,6 +564,8 @@ static void udp_isr(void)
 {
 	uint32_t isr = *AT91C_UDP_ISR;
 	if (isr & AT91C_UDP_ENDBUSRES) {
+		printf("USB reset\n\r");
+
 		AT91S_UDP *udp = AT91C_BASE_UDP;

 		/* reset all endpoints */
@ -594,41 +589,17 @@ static void udp_isr(void)
 		udp->UDP_IDR = AT91C_UDP_EPINT1 | AT91C_UDP_EPINT2 | AT91C_UDP_EPINT3 |
 				AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM | AT91C_UDP_SOFINT |
 				AT91C_UDP_WAKEUP;
-
-		if (dfu_status.bStatus == DFU_STATE_appDETACH) {
-			void (* bootloader)(void) = (void *)0x13c000;
-			bootloader();
-			while (1);
-		}
 	}

 	/* Handle Endpoint Interrupts */
-	uint32_t i;
-	for (i = 0; i < 4; i++) {
-		if (isr & *AT91C_UDP_IMR & (1<<i))
-			udp_handle_ep(i);
-	}
+	if (isr & AT91C_UDP_EPINT0)
+		udp_handle_ep(0);

 	/* clear all unhandled interrupts */
 	*AT91C_UDP_ICR = isr & (AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM |
 				AT91C_UDP_ENDBUSRES | AT91C_UDP_WAKEUP);
 }

-static void trigger_fifo_tx(void)
-{
-	struct ep_ctx *ctx = &ep_ctx[2];
-
-	/* currently transmitting, no need to trigger */
-	// TODO: racy?
-	if (ctx->flags & CTX_IN)
-		return;
-
-	if (fifo_txudp(ctx->fifo, 2, ctx->maxpktsize)) {
-		ctx->flags |= CTX_IN;
-		AT91C_UDP_CSR[2] |= AT91C_UDP_TXPKTRDY;
-	}
-}
-
 void at91_udp_init(void)
 {
 	/* configure & disable Pullup, disable Pullup von VBUS */
@ -661,23 +632,6 @@ void at91_udp_init(void)
 	aic->AIC_SVR[AT91C_ID_UDP] = (uint32_t)udp_isr;
 	aic->AIC_IECR = (1 << AT91C_ID_UDP);

-	usb_comm.rxfifo = fifo_alloc(1024);
-	usb_comm.txfifo = fifo_alloc(1024);
-	usb_comm.trigger_tx = trigger_fifo_tx;
-
-	tdc_register_device(0, &usb_comm);
-
-	pio_trigger_isr(UDP_VBUS_MON);
+	/* usb connected -> enable pullup */
+	*AT91C_PIOA_CODR = UDP_PULLUP;
 }
-
-static void udp_vbus_monitor(uint32_t status, uint32_t input)
-{
-	if (input & UDP_VBUS_MON)
-		/* usb connected -> enable pullup */
-		*AT91C_PIOA_CODR = UDP_PULLUP;
-	else
-		/* usb got diconnected -> disable pullup */
-		*AT91C_PIOA_SODR = UDP_PULLUP;
-}
-
-PIO_PINCHANGE_ISR(UDP_VBUS_MON, udp_vbus_monitor);
--- a/src/dfu.c
+++ b/src/dfu.c
@ -0,0 +1,472 @@
+/***************************************************************************
+ *   Copyright (C) 03/2008 by Olaf Rempel                                  *
+ *   razzor@kopf-tisch.de                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; version 2 of the License                *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#include <stdio.h>
+#include "AT91SAM7S256.h"
+#include "board.h"
+
+#include "at91_twi.h"
+#include "at91_udp.h"
+
+#include "usb_ch9.h"
+#include "usb_dfu.h"
+
+static uint8_t dfu_state = DFU_STATE_dfuIDLE;
+static uint8_t dfu_status = DFU_STATUS_OK;
+
+#define RET_NOTHING	0
+#define RET_STALL	1
+#define RET_ZLP		2
+
+static void handle_getstatus(void)
+{
+	struct dfu_status dstat;
+
+	uint32_t fmr = *AT91C_MC_FSR;
+
+	switch (dfu_state) {
+	case DFU_STATE_dfuDNLOAD_SYNC:
+	case DFU_STATE_dfuDNBUSY:
+		if (fmr & AT91C_MC_PROGE) {
+			dfu_status = DFU_STATUS_errPROG;
+			dfu_state = DFU_STATE_dfuERROR;
+
+		} else if (fmr & AT91C_MC_LOCKE) {
+			dfu_status = DFU_STATUS_errWRITE;
+			dfu_state = DFU_STATE_dfuERROR;
+
+		} else if (fmr & AT91C_MC_FRDY) {
+			dfu_state = DFU_STATE_dfuDNLOAD_IDLE;
+
+		} else {
+			dfu_state = DFU_STATE_dfuDNBUSY;
+		}
+		break;
+	}
+
+	/* send status response */
+	dstat.bStatus = dfu_status;
+	dstat.bState = dfu_state;
+	dstat.iString = 0;
+	/* FIXME: set dstat.bwPollTimeout */
+
+	ep_transfer_send(0, (char *)&dstat, sizeof(dstat), NULL);
+}
+
+static void handle_getstate(void)
+{
+	ep_transfer_send(0, (char *)&dfu_state, sizeof(dfu_state), NULL);
+}
+
+static uint16_t prog_iface;
+static uint16_t prog_block;
+static uint16_t prog_length;
+static uint32_t prog_buf[AT91C_IFLASH_PAGE_SIZE /4];
+
+static void handle_dnload_flash(void)
+{
+	uint32_t *ptr = (uint32_t *)((uint8_t *)0x100000 + (prog_block * 0x100));
+
+	uint32_t i;
+	prog_length += 3;
+	for (i = 0; i < prog_length /4; i++)
+		*ptr++ = prog_buf[i];
+
+	if (!(*AT91C_MC_FSR & AT91C_MC_FRDY)) {
+		printf("flash not ready!\n\r");
+		return;
+	}
+
+	*AT91C_MC_FCR = (AT91C_MC_KEY & (0x5A << 24)) |
+			(((uint32_t)ptr - 0x100004) & AT91C_MC_PAGEN) |
+			AT91C_MC_FCMD_START_PROG;
+
+	ep_transfer_send(0, NULL, 0, NULL);
+}
+
+static void handle_dnload_eeprom(void)
+{
+	/*
+	 * TODO: write buf to onboard eeprom @ address
+	 * inc. address
+	 */
+	ep_transfer_send(0, NULL, 0, NULL);
+}
+
+static void handle_dnload_blctrl(void)
+{
+	uint32_t i2c_dev = TWI_ADDR_BL1 + (prog_iface -2);
+
+	uint8_t buf[66] = { 0x47, 0x11 };
+
+	uint32_t i;
+	for (i = 0; i < prog_length; i += 0x40) {
+
+		uint32_t j;
+		for (j = 0; j < 0x40; j++)
+			buf[j +2] = ((i + j) < prog_length) ? ((uint8_t *)prog_buf)[i + j] : 0xFF;
+
+		struct blmc_cmd cmd = {
+			.cmd = (CMD_WRITE_FLASH << 16) | (prog_block * 0x100 + i),
+			.mode = BLMC_CMD_WRITE | BLMC_CMD_2_ARG,
+			.size = sizeof(buf),
+			.data = buf,
+		};
+
+		printf("cmd:0x%08lx\n\r", cmd.cmd);
+
+		if (twi_cmd(i2c_dev, &cmd) != 0) {
+			printf("flash write failed\n\r");
+			dfu_state = DFU_STATE_dfuERROR;
+			dfu_status = DFU_STATUS_errUNKNOWN;
+		}
+	}
+
+	ep_transfer_send(0, NULL, 0, NULL);
+}
+
+static uint32_t handle_dnload(uint16_t iface, uint16_t block, uint16_t length)
+{
+	printf("down:%x-%x-%x\n\r", iface, block, length);
+
+	if (length == 0) {
+		dfu_state = DFU_STATE_dfuMANIFEST_SYNC;
+		return RET_ZLP;
+	}
+
+	prog_iface = iface;
+	prog_block = block;
+	prog_length = length;
+
+	void *callback = NULL;
+	switch (iface) {
+	case 0:	/* internal flash */
+		if (block >= 0x3C0) {
+			dfu_state = DFU_STATE_dfuERROR;
+			dfu_status = DFU_STATUS_errADDRESS;
+			return RET_STALL;
+		}
+
+		callback = handle_dnload_flash;
+		break;
+
+	case 1:	/* onboard i2c-eeprom */
+		callback = handle_dnload_eeprom;
+		break;
+
+	case 2:
+	case 3:
+	case 4:
+	case 5:	/* blctrl 1-4 */
+		callback = handle_dnload_blctrl;
+		break;
+	}
+
+	ep_transfer_receive(0, (char *)&prog_buf, length, callback);
+	return RET_NOTHING;
+}
+
+static void handle_upload(uint16_t iface, uint16_t block, uint16_t length)
+{
+	printf("up:%x-%x-%x\n\r", iface, block, length);
+
+	char *ptr = (char *)&prog_buf;
+
+	switch (iface) {
+	case 0:	/* internal flash */
+		if (block >= 0x400) {
+			length = 0;
+			break;
+		}
+		ptr = (char *)0x100000 + (block * 0x100);
+		break;
+
+	case 1:	/* onboard i2c-eeprom */
+		if (block >= 0x80) {
+			length = 0;
+			break;
+		}
+
+		/* TODO */
+
+		break;
+
+	case 2:
+	case 3:
+	case 4:
+	case 5:	/* blctrl 1-4 */
+		if (block >= 0x20) {
+			length = 0;
+			dfu_state = DFU_STATE_dfuIDLE;
+			break;
+		}
+
+		uint32_t i2c_dev = TWI_ADDR_BL1 + (iface -2);
+
+		struct blmc_cmd cmd = {
+			.cmd = (CMD_READ_FLASH << 16) | (block * 0x100),
+			.mode = BLMC_CMD_READ | BLMC_CMD_2_ARG,
+			.size = length,
+			.data = (uint8_t *)&prog_buf,
+		};
+
+		if (twi_cmd(i2c_dev, &cmd) != 0) {
+			length = 0;
+			dfu_state = DFU_STATE_dfuERROR;
+			dfu_status = DFU_STATUS_errUNKNOWN;
+		}
+		break;
+	}
+
+	ep_transfer_send(0, ptr, length, NULL);
+}
+
+void ep0_handle_dfu(struct usb_ctrlrequest *req)
+{
+	uint32_t ret = RET_NOTHING;
+
+	switch (dfu_state) {
+	case DFU_STATE_appIDLE:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_DETACH:
+			dfu_state = DFU_STATE_appDETACH;
+			ret = RET_ZLP;
+			break;
+
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_appDETACH:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_appIDLE;
+			ret = RET_STALL;
+			break;
+		}
+		/* FIXME: implement timer to return to appIDLE */
+		break;
+
+	case DFU_STATE_dfuIDLE:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_DNLOAD:
+			if (req->wLength == 0) {
+				dfu_state = DFU_STATE_dfuERROR;
+				ret = RET_STALL;
+				break;
+			}
+			dfu_state = DFU_STATE_dfuDNLOAD_SYNC;
+			ret = handle_dnload(req->wIndex, req->wValue, req->wLength);
+			break;
+
+		case USB_REQ_DFU_UPLOAD:
+			dfu_state = DFU_STATE_dfuUPLOAD_IDLE;
+			handle_upload(req->wIndex, req->wValue, req->wLength);
+			break;
+
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		case USB_REQ_DFU_ABORT:
+			/* no zlp? */
+			ret = RET_ZLP;
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuDNLOAD_SYNC:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			/* FIXME: state transition depending on block completeness */
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuDNBUSY:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			/* FIXME: only accept getstatus if bwPollTimeout
+			 * has elapsed */
+			handle_getstatus();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuDNLOAD_IDLE:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_DNLOAD:
+			dfu_state = DFU_STATE_dfuDNLOAD_SYNC;
+			ret = handle_dnload(req->wIndex, req->wValue, req->wLength);
+			break;
+
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		case USB_REQ_DFU_ABORT:
+			dfu_state = DFU_STATE_dfuIDLE;
+			ret = RET_ZLP;
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuMANIFEST_SYNC:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			dfu_state = DFU_STATE_dfuIDLE;
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuMANIFEST:
+		dfu_state = DFU_STATE_dfuERROR;
+		ret = RET_STALL;
+		break;
+
+	case DFU_STATE_dfuMANIFEST_WAIT_RST:
+		/* we should never go here */
+		break;
+
+	case DFU_STATE_dfuUPLOAD_IDLE:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_UPLOAD:
+			/* state transition if less data then requested */
+			handle_upload(req->wIndex, req->wValue, req->wLength);
+			break;
+
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		case USB_REQ_DFU_ABORT:
+			dfu_state = DFU_STATE_dfuIDLE;
+			/* no zlp? */
+			ret = RET_ZLP;
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuERROR:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_CLRSTATUS:
+			dfu_state = DFU_STATE_dfuIDLE;
+			dfu_status = DFU_STATUS_OK;
+			/* no zlp? */
+			ret = RET_ZLP;
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+	}
+
+	switch (ret) {
+	case RET_NOTHING:
+		break;
+
+	case RET_ZLP:
+		ep_transfer_send(0, NULL, 0, NULL);
+		break;
+
+	case RET_STALL:
+		ep_send_stall(0);
+		break;
+	}
+}
--- a/src/fifo.c
+++ b/src/fifo.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - FIFOs for use with PDC / USB Hardware                        *
- *                                                                         *
 *   Copyright (C) 01/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
@ -31,7 +29,7 @@
 * all other operations don't need locks:
 * - only fifo_put/fifo_rxpdc are allowed to increment fifo->in
 * - only fifo_get/fifo_txpdc are allowed to increment fifo->out
- * FIXME: a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
+ * a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
 */
 static uint32_t fifo_used(struct fifo *fifo)
 {
--- a/src/flightctrl.c
+++ b/src/flightctrl.c
@ -1,264 +0,0 @@
-/***************************************************************************
- *   sam7fc - Flight Control                                               *
- *                                                                         *
- *   Copyright (C) 03/2008 by Olaf Rempel                                  *
- *   razzor@kopf-tisch.de                                                  *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; version 2 of the License                *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#include <stdio.h>
-#include <stdlib.h>	/* abs() */
-
-#include "AT91SAM7S256.h"
-#include "board.h"
-
-#include <at91_adc.h>
-#include <at91_pitc.h>
-#include <at91_tc1.h>
-#include <at91_twi.h>
-#include <telemetrie.h>
-#include <pidctrl.h>
-
-static uint32_t global_state;
-#define MOTOR_RUNNING			0x0001
-#define MOTOR_MIXER_ENABLE		0x0002
-#define STICK_CALIBRATION		0x0004
-#define STICK_CALIBRATION_COMPLETE	0x0008
-
-static struct pid_data pid_nick = {
-	.kp = 1,
-	.ki = 0,
-	.err_sum_max = +1024,
-	.err_sum_min = -1024,
-	.out_max = +256,
-	.out_min = -256,
-};
-TDC_PTR(pid_nick_ki, &pid_nick.ki, "flctrl: pid nick ki", int32_t, TDC_SIGNED);
-TDC_PTR(pid_nick_errsum, &pid_nick.err_sum, "flctrl: pid nick errsum", int32_t, TDC_SIGNED | TDC_READONLY);
-
-static struct pid_data pid_roll = {
-	.kp = 1,
-	.ki = 0,
-	.err_sum_max = +1024,
-	.err_sum_min = -1024,
-	.out_max = +256,
-	.out_min = -256,
-};
-TDC_PTR(pid_roll_ki, &pid_roll.ki, "flctrl: pid roll ki", int32_t, TDC_SIGNED);
-TDC_PTR(pid_roll_errsum, &pid_roll.err_sum, "flctrl: pid roll errsum", int32_t, TDC_SIGNED | TDC_READONLY);
-
-static struct pid_data pid_gier = {
-	.kp = 1,
-	.ki = 0,
-	.err_sum_max = +1024,
-	.err_sum_min = -1024,
-	.out_max = +256,
-	.out_min = -256,
-};
-TDC_PTR(pid_gier_ki, &pid_gier.ki, "flctrl: pid gier ki", int32_t, TDC_SIGNED);
-TDC_PTR(pid_gier_errsum, &pid_gier.err_sum, "flctrl: pid gier errsum", int32_t, TDC_SIGNED | TDC_READONLY);
-
-static void motor_mixer(int32_t gas, int32_t nick, int32_t roll, int32_t gier)
-{
-	static uint8_t pwm[4];
-
-	TDC_VALUE(pwm0, pwm[0], "Motor PWM 1", uint8_t, TDC_UNSIGNED | TDC_READONLY);
-	TDC_VALUE(pwm1, pwm[1], "Motor PWM 2", uint8_t, TDC_UNSIGNED | TDC_READONLY);
-	TDC_VALUE(pwm2, pwm[2], "Motor PWM 3", uint8_t, TDC_UNSIGNED | TDC_READONLY);
-	TDC_VALUE(pwm3, pwm[3], "Motor PWM 4", uint8_t, TDC_UNSIGNED | TDC_READONLY);
-
-	pwm[0] = LIMIT((gas - nick + gier), 0x0F, 0xFF);
-	pwm[1] = LIMIT((gas + nick + gier), 0x0F, 0xFF);
-	pwm[2] = LIMIT((gas + roll - gier), 0x0F, 0xFF);
-	pwm[3] = LIMIT((gas - roll - gier), 0x0F, 0xFF);
-
-	if (!(global_state & MOTOR_RUNNING)) {
-		pwm[0] = 0x00;
-		pwm[1] = 0x00;
-		pwm[2] = 0x00;
-		pwm[3] = 0x00;
-
-	} else if (!(global_state & MOTOR_MIXER_ENABLE)) {
-		pwm[0] = 0x0F;
-		pwm[1] = 0x0F;
-		pwm[2] = 0x0F;
-		pwm[3] = 0x0F;
-	}
-
-	twi_setpwm(pwm);
-}
-
-static uint32_t base_ctrl_cb(struct pitc_timer *timer)
-{
-	if (global_state & STICK_CALIBRATION)
-		global_state |= STICK_CALIBRATION_COMPLETE;
-
-	return PITC_REMOVE_TIMER;
-}
-
-static struct pitc_timer base_ctrl_timer = {
-	.func = base_ctrl_cb,
-};
-
-void base_ctrl(void)
-{
-	static int32_t nick_integral, roll_integral, gier_integral;
-
-	/* get stick switches & values */
-	struct rc_values rc_sw;
-	uint32_t count = rcontrol_getswitches(&rc_sw);
-
-	if (count < 4) {
-		global_state &= ~MOTOR_RUNNING;
-
-	} else if (global_state & STICK_CALIBRATION_COMPLETE) {
-//		rcontrol_calibrate(RC_CAL_END);
-//		rcontrol_calibrate(RC_CAL_SAVE);
-		rcontrol_print_cal();
-		global_state &= ~(STICK_CALIBRATION | STICK_CALIBRATION_COMPLETE);
-
-	} else if (global_state & STICK_CALIBRATION) {
-		/* do nothing during calibration */
-
-	} else if (count >= 4) {
-		/* Motor stop */
-		if (rc_sw.chan[2] < 0 && rc_sw.chan[3] > 0)
-			global_state &= ~MOTOR_RUNNING;
-
-		/* Motor start */
-		if (rc_sw.chan[2] < 0 && rc_sw.chan[3] < 0)
-			global_state |= MOTOR_RUNNING;
-
-		/* Gyro calibration */
-		if (rc_sw.chan[2] > 0 && rc_sw.chan[3] > 0)
-			adc_calibrate(ADC_CAL_GYRO);
-
-		/* ACC + Stick calibration */
-		if (rc_sw.chan[2] > 0 && rc_sw.chan[3] < 0) {
-			adc_calibrate(ADC_CAL_ACC);
-//			rcontrol_calibrate(RC_CAL_START);
-			global_state |= STICK_CALIBRATION;
-			base_ctrl_timer.interval = 1000;
-			pitc_schedule_timer(&base_ctrl_timer);
-		}
-	}
-
-	struct rc_values rc;
-	rcontrol_getvalues(&rc);
-
-	if (rc.chan[2] < 15) {
-		global_state &= ~MOTOR_MIXER_ENABLE;
-
-		/* reset integrals */
-		nick_integral = roll_integral = gier_integral = 0;
-		pid_nick.err_sum = pid_roll.err_sum = pid_gier.err_sum = 0;
-
-	} else {
-		global_state |= MOTOR_MIXER_ENABLE;
-	}
-
-	/* get adc results */
-	static int16_t adc_result[7];
-
-	TDC_PTR(adc_result0, &adc_result[ADC_GYRO_NICK], "ADC_GYRO_NICK", int16_t, TDC_SIGNED | TDC_READONLY);
-	TDC_PTR(adc_result1, &adc_result[ADC_GYRO_ROLL], "ADC_GYRO_ROLL", int16_t, TDC_SIGNED | TDC_READONLY);
-	TDC_PTR(adc_result2, &adc_result[ADC_GYRO_GIER], "ADC_GYRO_GIER", int16_t, TDC_SIGNED | TDC_READONLY);
-	TDC_PTR(adc_result3, &adc_result[ADC_ACC_NICK], "ADC_ACC_NICK", int16_t, TDC_SIGNED | TDC_READONLY);
-	TDC_PTR(adc_result4, &adc_result[ADC_ACC_ROLL], "ADC_ACC_ROLL", int16_t, TDC_SIGNED | TDC_READONLY);
-	TDC_PTR(adc_result5, &adc_result[ADC_ACC_GIER], "ADC_ACC_GIER", int16_t, TDC_SIGNED | TDC_READONLY);
-	TDC_PTR(adc_result6, &adc_result[ADC_VOLTAGE], "ADC_VOLTAGE", int16_t, TDC_SIGNED | TDC_READONLY);
-
-	adc_get_results(adc_result);
-
-	if (count != 0 && adc_result[ADC_VOLTAGE] > 960)
-		*AT91C_PIOA_CODR = LED_GREEN;
-	else
-		*AT91C_PIOA_SODR = LED_GREEN;
-
-	nick_integral += adc_result[ADC_GYRO_NICK];
-	TDC_INT32(nick_integral, "flctrl: Base Integral Nick");
-
-	roll_integral += adc_result[ADC_GYRO_ROLL];
-	TDC_INT32(roll_integral, "flctrl: Base Integral Roll");
-
-	gier_integral += adc_result[ADC_GYRO_GIER];
-	TDC_INT32(gier_integral, "flctrl: Base Integral Gier");
-
-	static int32_t integral_gyro_mix = 1;
-	TDC_INT32(integral_gyro_mix, "flctrl: Mix Integral/ACC (0-1024)");
-
-	static int32_t acc_faktor = 300;
-	TDC_INT32(acc_faktor, "flctrl: Mix Faktor");
-
-	/*
-	 * 90° -> ADC_ACC_* ~210
-	 * 90° -> Integral ~60000
-	 */
-	int32_t mix_integral = 1024 - integral_gyro_mix;
-	int32_t mix_acc = integral_gyro_mix * acc_faktor;
-	nick_integral = ((nick_integral * mix_integral) + (adc_result[ADC_ACC_NICK] * mix_acc)) / 1024;
-	roll_integral = ((roll_integral * mix_integral) + (adc_result[ADC_ACC_ROLL] * mix_acc)) / 1024;
-
-	/*
-	 * hoher gyro_faktor -> bessere stabilität, aber langsames zurückkehren nach vollausschlag
-	 * niedriger integral_faktor -> geringere abweichungbei einseitigem anheben, aber schwingungsanfälliger
-	 */
-	static int32_t integral_faktor = 256;
-	static int32_t gyro_faktor = 4;
-	TDC_INT32(integral_faktor, "flctrl: Integral Divisior");
-	TDC_INT32(gyro_faktor, "flctrl: Integral Gyro Faktor /16");
-
-	static int32_t nick;
-	static int32_t roll;
-	static int32_t gier;
-	nick = (nick_integral / integral_faktor) + (adc_result[ADC_GYRO_NICK] * gyro_faktor) / 16;
-	roll = (roll_integral / integral_faktor) + (adc_result[ADC_GYRO_ROLL] * gyro_faktor) / 16;
-	gier = (gier_integral / integral_faktor) + (adc_result[ADC_GYRO_GIER] * gyro_faktor) / 16;
-	TDC_INT32_RO(nick, "flctrl: Integral + Gyro Nick");
-	TDC_INT32_RO(roll, "flctrl: Integral + Gyro Roll");
-	TDC_INT32_RO(gier, "flctrl: Integral + Gyro Gier");
-
-	static int32_t stick_kp = 8;
-	TDC_INT32(stick_kp, "flctrl: Stick-P /16");
-
-	static int32_t stick_gas, stick_nick, stick_roll, stick_gier;
-	stick_gas = rc.chan[2];
-	stick_nick = (rc.chan[0] * stick_kp) / 16;
-	stick_roll = (rc.chan[1] * stick_kp) / 16;
-	stick_gier = rc.chan[3] / 4;
-
-	TDC_INT32_RO(stick_gas, "Stick Gas");
-	TDC_INT32_RO(stick_nick, "Stick Nick");
-	TDC_INT32_RO(stick_roll, "Stick Roll");
-	TDC_INT32_RO(stick_gier, "Stick Gier");
-
-	static int32_t gier_kp = 32;
-	TDC_INT32(gier_kp, "flctrl: Gier-P (/256)");
-	gier_integral -= gier_kp * stick_gier * abs(stick_gier) / 256;
-
-	static int32_t mixer_gas, mixer_nick, mixer_roll, mixer_gier;
-	mixer_gas = stick_gas;
-	mixer_nick = pid_ctrl(&pid_nick, stick_nick - nick);
-	mixer_roll = pid_ctrl(&pid_roll, stick_roll - roll);
-	mixer_gier = pid_ctrl(&pid_gier, stick_gier - gier);
-
-	/* mix gas/nick/roll/gier -> 4 motors */
-	motor_mixer(mixer_gas, mixer_nick, mixer_roll, mixer_gier);
-
-	TDC_INT32_RO(mixer_gas, "Mixer Gas");
-	TDC_INT32_RO(mixer_nick, "Mixer Nick");
-	TDC_INT32_RO(mixer_roll, "Mixer Roll");
-	TDC_INT32_RO(mixer_gier, "Mixer Gier");
-}
--- a/src/memalloc.c
+++ b/src/memalloc.c
@ -1,6 +1,4 @@
 /***************************************************************************
- *   sam7fc - dynamic Memory Allocation                                    *
- *                                                                         *
 *   Copyright (C) 02/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
--- a/src/pidctrl.c
+++ b/src/pidctrl.c
@ -1,44 +0,0 @@
-/***************************************************************************
- *   Copyright (C) 03/2008 by Olaf Rempel                                  *
- *   razzor@kopf-tisch.de                                                  *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; version 2 of the License                *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#include <stdint.h>
-
-#include "pidctrl.h"
-#include "board.h"
-
-int32_t pid_ctrl(struct pid_data *pid, int32_t error)
-{
-	int32_t out = 0;
-
-	if (pid->kp != 0)
-		out += pid->kp * error;
-
-	if (pid->ki != 0) {
-		pid->err_sum += error;
-		pid->err_sum = LIMIT(pid->err_sum, pid->err_sum_min, pid->err_sum_max);
-
-		out += (pid->err_sum / pid->ki);
-	}
-
-	if (pid->kd != 0) {
-		out += pid->kd * (error - pid->err_old);
-		pid->err_old = error;
-	}
-
-	return LIMIT(out, pid->out_min, pid->out_max);
-}
--- a/src/telemetrie.c
+++ b/src/telemetrie.c
@ -1,338 +0,0 @@
-/***************************************************************************
- *   sam7fc - Telemetrie Handling                                          *
- *                                                                         *
- *   Copyright (C) 02/2008 by Olaf Rempel                                  *
- *   razzor@kopf-tisch.de                                                  *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; version 2 of the License                *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#include <stdio.h>
-#include <stdint.h>
-#include <string.h>
-#include "board.h"		// ARRAY_SIZE()
-#include "at91_pitc.h"
-#include "telemetrie.h"
-#include "tdc_proto.h"
-#include "memalloc.h"
-#include "fifo.h"
-
-#define TDC_OWN_ADDRESS		TDC_ADDR1
-
-/* extern symbols, defined in ldscript */
-extern struct tdc_value _tdc_value_table;
-extern struct tdc_value _tdc_value_table_end;
-
-/* max. 8x 32 = 256 variables */
-static uint32_t tdc_varmap[8];
-
-/* array of devices, that are used to reach address X */
-static struct comm_device *routing_table[8];
-
-/*
- * returns:
- * -1: on routing error
- *  0: no space left in txfifo (caller should retry)
- * >0: success
- */
-int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head)
-{
-	if (addr >= ARRAY_SIZE(routing_table) || !routing_table[addr])
-		return -1;
-
-	int32_t retval = fifo_put(routing_table[addr]->txfifo, (char *)head, head->size);
-	if (routing_table[addr]->trigger_tx)
-		routing_table[addr]->trigger_tx();
-
-	return retval;
-}
-
-static int32_t tdc_get_vars(void)
-{
-	/* restart point */
-	static uint32_t id;
-
-	struct tdc_value *value = &_tdc_value_table + id;
-
-	while (value < &_tdc_value_table_end) {
-		uint32_t datalen = strlen(value->name);
-
-		struct tdc_getvars_reply *reply = alloc(sizeof(struct tdc_getvars_reply) + datalen);
-		reply->cmd = TDC_REPLY | TDC_ADDR1 | TDC_GETVARS;
-		reply->size = sizeof(struct tdc_getvars_reply) + datalen;
-		reply->id = (id & 0xFF);
-		reply->flags = value->flags;
-		reply->name_len = datalen;
-		memcpy(reply->name, value->name, datalen);
-
-		uint32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
-		free(reply);
-
-		/* push routing error(-1) and retry(0) */
-		if (ret <= 0)
-			return ret;
-
-		id++;
-		value++;
-	}
-
-	/* dump complete, reset restart point */
-	id = 0;
-	return 1;
-}
-
-static int32_t tdc_get_value(uint32_t id)
-{
-	struct tdc_value *value = &_tdc_value_table + id;
-	if (value >= &_tdc_value_table_end)
-		return -1;
-
-	uint32_t datalen = value->flags & TDC_SIZEMASK;
-
-	struct tdc_getvalue_reply *reply = alloc(sizeof(struct tdc_getvalue_reply) + datalen);
-	reply->cmd = TDC_REPLY | TDC_ADDR1 | TDC_GETVALUE;
-	reply->size = sizeof(struct tdc_getvalue_reply) + datalen;
-	reply->id = id;
-	memcpy(reply->data, value->data, datalen);
-
-	int32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
-	free(reply);
-
-	return ret;
-}
-
-static int32_t tdc_set_value(uint32_t id, uint8_t *data, uint32_t data_size)
-{
-	struct tdc_value *value = &_tdc_value_table + id;
-	if (value >= &_tdc_value_table_end)
-		return -1;
-
-	uint32_t len = value->flags & TDC_SIZEMASK;
-
-	if (len != data_size)
-		return -1;
-
-	// TODO: atomic?
-	memcpy(value->data, data, len);
-	return len;
-}
-
-static uint32_t tdc_timer_cb(struct pitc_timer *timer)
-{
-	uint32_t i, j;
-	for (i = 0; i < ARRAY_SIZE(tdc_varmap); i++) {
-		uint32_t bitmask = tdc_varmap[i];
-
-		for (j = 0; j < 32; j++) {
-			if (!bitmask)
-				break;
-
-			if (bitmask & 0x01) {
-				if (tdc_get_value(i * 32 + j) < 0)
-					tdc_varmap[i] &= ~(1 << j);
-			}
-			bitmask >>= 1;
-		}
-	}
-	return PITC_RESTART_TIMER;
-}
-
-static struct pitc_timer tdc_timer = {
-	.func = tdc_timer_cb,
-};
-
-static int32_t tdc_setup_timer(uint32_t interval, uint32_t *varmap)
-{
-	memcpy(tdc_varmap, varmap, sizeof(tdc_varmap));
-
-	uint32_t i;
-	uint32_t tmp = 0;
-	for (i = 0; i < ARRAY_SIZE(tdc_varmap); i++)
-		tmp |= tdc_varmap[i];
-
-	if ((interval > 0) && (tmp != 0)) {
-		tdc_timer.interval = interval;
-		pitc_schedule_timer(&tdc_timer);
-
-	} else {
-		pitc_remove_timer(&tdc_timer);
-	}
-	return 1;
-}
-
-static const struct tdc_hello_reply hello_reply = {
-	.cmd = TDC_REPLY | TDC_OWN_ADDRESS | TDC_HELLO,
-	.size = sizeof(struct tdc_hello_reply),
-	.name = "sam7fc-v0.01",
-};
-
-void tdc_register_device(uint32_t addr, struct comm_device *device)
-{
-	if (addr < ARRAY_SIZE(routing_table))
-		routing_table[addr] = device;
-}
-
-struct tdc_pkt_header * tdc_alloc_fullpkt(struct comm_device *device, uint32_t size)
-{
-	struct tdc_pkt_header *head = alloc(size);
-
-	/* peek the whole packet */
-	uint32_t len = fifo_peek(device->rxfifo, (char *)head, size);
-	if (len != size) {
-		free(head);
-		head = NULL;
-	}
-
-	return head;
-}
-
-static int32_t tdc_receive(struct comm_device *device)
-{
-	struct tdc_pkt_header tmp_head;
-	struct tdc_pkt_header *head = &tmp_head;
-
-	/* peek the header, return retry(0) if not enough bytes are available */
-	uint32_t len = fifo_peek(device->rxfifo, (char *)head, sizeof(tmp_head));
-	if (len != sizeof(tmp_head))
-		return 0;
-
-	/* assume an error, remove one byte from fifo */
-	uint32_t used_bytes = 1;
-	int32_t ret = -1;
-
-	/* remember the device as path to the host */
-	if ((head->cmd & (TDC_REPLY | TDC_OPCODEMASK)) == TDC_HELLO) {
-		tdc_register_device(TDC_ADDR0, device);
-	}
-
-	/* reply packets / forward packets */
-	if (head->cmd & TDC_REPLY || (head->cmd & TDC_ADDRMASK) != TDC_OWN_ADDRESS) {
-		/* peek complete packet, return retry(0) if not enough bytes are available */
-		head = tdc_alloc_fullpkt(device, head->size);
-		if (head == NULL)
-			return 0;
-
-		/* reply packets go to ADDR0, forwards to others */
-		uint32_t addr = (head->cmd & TDC_REPLY) ? TDC_ADDR0 : ((head->cmd & TDC_ADDRMASK) >> 4);
-
-		used_bytes = head->size;
-		ret = tdc_transmit(addr, head);
-
-	} else {
-		/* parse cmd */
-		switch (head->cmd & TDC_OPCODEMASK) {
-		case TDC_HELLO: {
-			/* check packet size */
-			struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)head;
-			if (pkt->size != sizeof(*pkt))
-				break;
-
-			/* send reply */
-			ret = tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
-			used_bytes = pkt->size;
-		}	break;
-
-		case TDC_GETVARS: {
-			struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)head;
-			if (pkt->size != sizeof(*pkt))
-				break;
-
-			/* send reply */
-			ret = tdc_get_vars();
-			used_bytes = pkt->size;
-		}	break;
-
-		case TDC_GETVALUE: {
-			struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)head;
-			if (pkt->size != sizeof(*pkt))
-				break;
-
-			/* peek complete packet, return retry(0) if not enough bytes are available */
-			head = tdc_alloc_fullpkt(device, head->size);
-			if (head != NULL) {
-				pkt = (struct tdc_getvalue_request *)head;
-				ret = tdc_get_value(pkt->id);
-				used_bytes = pkt->size;
-
-			} else {
-				ret = 0;
-			}
-		}	break;
-
-		case TDC_SETVALUE: {
-			struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)head;
-			if (pkt->size < sizeof(*pkt) +1 || pkt->size > sizeof(*pkt) +8)
-				break;
-
-			/* peek complete packet, return retry(0) if not enough bytes are available */
-			head = tdc_alloc_fullpkt(device, head->size);
-			if (head != NULL) {
-				pkt = (struct tdc_setvalue_request *)head;
-				ret = tdc_set_value(pkt->id, pkt->data, pkt->size - sizeof(*pkt));
-				used_bytes = pkt->size;
-
-			} else {
-				ret = 0;
-			}
-		}	break;
-
-		case TDC_REQVALUES: {
-			struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)head;
-			if (pkt->size != sizeof(*pkt))
-				break;
-
-			/* peek complete packet, return retry(0) if not enough bytes are available */
-			head = tdc_alloc_fullpkt(device, head->size);
-			if (head != NULL) {
-				pkt = (struct tdc_reqvalues_request *)head;
-				ret = tdc_setup_timer(pkt->interval, pkt->varmap);
-				used_bytes = pkt->size;
-
-			} else {
-				ret = 0;
-			}
-		}	break;
-
-		}
-	}
-
-	/* on success(>0) or routing error(-1) remove the packet */
-	if (ret != 0) {
-		/* remove bytes from fifo */
-		fifo_remove(device->rxfifo, used_bytes);
-	}
-
-	/* free allocated memory */
-	if (head != NULL && head != &tmp_head)
-		free(head);
-
-	return ret;
-}
-
-void tdc_check(void)
-{
-	uint32_t i;
-	for (i = 0; i < ARRAY_SIZE(routing_table); i++) {
-		if (routing_table[i] != NULL) {
-			tdc_receive(routing_table[i]);
-			// TODO: handle retry
-		}
-	}
-}
-
-void tdc_init(void)
-{
-	uint32_t count = &_tdc_value_table_end - &_tdc_value_table;
-	printf("found %ld TDC variables\n\r", count);
-}
Author	SHA1	Message	Date
Olaf Rempel	4b6c9c2af0	blctrl flash write	2008-06-23 15:17:28 +02:00
Olaf Rempel	2ae6881a67	reset AIC - finally working	2008-06-23 15:17:28 +02:00
Olaf Rempel	5792ee5bf4	reset AIC - 2nd try	2008-06-23 15:17:28 +02:00
Olaf Rempel	66ba982b84	reset AIC	2008-06-23 15:17:28 +02:00
Olaf Rempel	2a57bfea62	callback fix	2008-06-23 15:17:28 +02:00
Olaf Rempel	0be9113381	moved to 0x13c000	2008-06-23 15:17:28 +02:00
Olaf Rempel	bd59d4924f	working version	2008-06-23 15:17:28 +02:00
Olaf Rempel	fded24a803	working dfu framework	2008-06-23 15:17:28 +02:00
Olaf Rempel	588cfbc2c2	work work	2008-06-23 15:17:28 +02:00
Olaf Rempel	e3ed2ef4e7	make it work without dummy	2008-06-23 15:17:28 +02:00
Olaf Rempel	6825dbb2f4	more init0 work	2008-06-23 15:17:28 +02:00
Olaf Rempel	192c3e132c	get ldscript working	2008-06-23 15:17:28 +02:00