new toolchain

Makefile

@@ -73,6 +73,7 @@ openocd:
 	$(shell $(OPENOCD) -f scripts/openocd.cfg)
 
 install: all
-	./scripts/download.sh
+#	./scripts/download.sh
+	$(TOOLCHAIN)/dfu-util -i 0 -D $(BUILD)/$(TARGET).elf.bin
 
 -include $(shell find $(BUILD) -name *.d 2> /dev/null)

at91_init0.s

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - stack/bss/data init                                            *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -16,9 +18,6 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
-	.equ AIC_IVR,		(256)
-	.equ AIC_FVR,		(260)
-	.equ AIC_EOICR,		(304)
 	.equ AT91C_BASE_AIC,	(0xFFFFF000)
 
 	.equ IRQ_Stack_Size,	(3 * 8 * 4)
@@ -29,7 +28,6 @@
 	.equ ARM_MODE_IRQ,	0x12
 	.equ ARM_MODE_SVC,	0x13
 	.equ ARM_MODE_ABT,	0x17
-	.equ ARM_MODE_SYS,	0x1F
 
 	.equ I_BIT,		0x80
 	.equ F_BIT,		0x40
@@ -47,14 +45,14 @@ rsvdvec:	ldr     pc, [pc, #24]		/* 0x14 reserved  */
 irqvec:		ldr     pc, [pc, #24]		/* 0x18 IRQ */
 fiqvec:		ldr     pc, [pc, #24]		/* 0x1c FIQ */
 
-	.extern	SWI_Handler
 	.extern ABT_Handler
 	.extern IRQ_Handler
+	.extern FIQ_Handler
 
 	/* 0x80000000 will result in Prefetch Abort */
 	.word InitReset
 	.word 0x80000000
-	.word SWI_Handler
+	.word 0x80000000
 	.word ABT_Handler
 	.word ABT_Handler
 	.word 0x80000000
@@ -79,7 +77,7 @@ InitReset:
 		mov	sp, r0
 		sub	r0, r0, #FIQ_Stack_Size
 
-		/* store AIC Base in ARM_MODE_FIQ:r8 for faster access */
+		/* store AIC Base in r8_fiq for faster access */
 		ldr	r8, =AT91C_BASE_AIC
 
 		/* Setup IRQ Mode Stack */
@@ -93,7 +91,7 @@ InitReset:
 		sub	r0, r0, #ABT_Stack_Size
 
 		/* Setup Supervisor Mode Stack (IRQ & NMI enabled) */
-		msr	CPSR_c, #ARM_MODE_SYS
+		msr	CPSR_c, #ARM_MODE_SVC
 		mov	sp, r0
 
 		/* Relocate .data section */
@@ -125,31 +123,3 @@ LoopZI:		cmp	r1, r2
 		/* exit dummy for newlib */
 exit:		b	.
 	.endfunc
-
-        .global FIQ_Handler
-        .func   FIQ_Handler
-FIQ_Handler:
-		/* Save r0 to ARM_MODE_FIQ:r9 */
-		mov	r9, r0
-
-		/* get FIQ Vector from AIC and thus clear FIQ */
-		ldr	r0, [r8, #AIC_FVR]
-
-		/* Switch to ARM_MODE_SVC and save registers there */
-		msr	CPSR_c, #ARM_MODE_SYS | I_BIT | F_BIT
-		stmfd	sp!, { r1-r3, r12, lr }
-
-		/* execute FIQ in SVC_MODE */
-		mov	r14, pc
-		bx	r0
-
-		/* restore registers and switch back to ARM_MODE_FIQ */
-		ldmia	sp!, { r1-r3, r12, lr }
-		msr	CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT
-
-		/* restore the ARM_MODE_SVC:r0 */
-		mov	r0, r9
-
-		/* restore PC using the LR_fiq directly */
-		subs	pc, lr, #4
-	.endfunc

at91_init1.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - PLL Init, IRQ/FIQ Vectors                                      *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -80,91 +82,95 @@ void at91_init1(void)
 	aic->AIC_SPU = (uint32_t)empty_isr;
 }
 
-/* TODO: make it static */
-uint32_t nested_count = 0;
-
 __attribute__((naked)) void IRQ_Handler(void)
 {
 	asm volatile (
-		".equ AIC_IVR_OFF,	(256)		\n\t"
-		".equ AIC_EOICR_OFF,	(304)		\n\t"
-		".equ AT91C_BASE_AIC,	(0xFFFFF000)	\n\t"
 		".equ ARM_MODE_IRQ,	0x12		\n\t"
-		".equ ARM_MODE_SYS,	0x1F		\n\t"
+		".equ ARM_MODE_SVC,	0x13		\n\t"
 		".equ I_BIT,		0x80		\n\t"
-		".equ F_BIT,		0x40		\n\t"
 
-		/* Manage Exception Entry */
-		/* Save LR_irq to IRQ stack */
+		".equ AIC_IVR,		(256)		\n\t"
+		".equ AIC_EOICR,	(304)		\n\t"
+		".equ AT91C_BASE_AIC,	(0xFFFFF000)	\n\t"
+
+		/* Adjust and save lr_irq on IRQ stack */
 		"sub	lr, lr, #4				\n\t"
-		"stmdb	sp!, { lr }			\n\t"
+		"stmfd	sp!, { lr }				\n\t"
 
-		/* Save r0 and SPSR to IRQ stack */
+		/* Save SPSR (for nested interrupts) */
 		"mrs	r14, SPSR				\n\t"
-		"stmdb	sp!, { r0, r14 }		\n\t"
+		"stmfd	sp!, { r14 }				\n\t"
 
-		/* count nested interrupts */
-		"ldr	r14, =nested_count		\n\t"
-		"ldr	r0, [r14]			\n\t"
-		"add	r0, r0, #1			\n\t"
-		"str	r0, [r14]			\n\t"
+		/* Save r0 on IRQ stack */
+		"stmfd	sp!, { r0 }				\n\t"
 
-		/* De-assert the NIRQ and clear the source in Protect Mode */
+		/* Write in the IVR to support Protect Mode */
 		"ldr	r14, =AT91C_BASE_AIC			\n\t"
-		"ldr	r0, [r14, #AIC_IVR_OFF]		\n\t"
-		"str	r14, [r14, #AIC_IVR_OFF]	\n\t"
+		"ldr	r0, [r14, #AIC_IVR]			\n\t"
+		"str	r14, [r14, #AIC_IVR]			\n\t"
 
-		/* Enable Interrupt and Switch in Supervisor Mode */
-		"msr	CPSR_c, #ARM_MODE_SYS		\n\t"
+		/* Enable Interrupt and switch to SVC mode */
+		"msr	CPSR_c, #ARM_MODE_SVC			\n\t"
 
-		/* Save scratch/used registers and LR in User Stack */
-		"stmdb	sp!, { r1-r3, r12, r14 }	\n\t"
+		/* Save scratch/used registers and lr on SVC Stack */
+		"stmfd	sp!, { r1-r3, r12, r14 }		\n\t"
 
 		/* Branch to the routine pointed by the AIC_IVR */
 		"mov	r14, pc					\n\t"
 		"bx	r0					\n\t"
 
-		/* Restore scratch/used registers and LR from User Stack */
+		/* Restore scratch/used registers and lr from SVC Stack */
 		"ldmia	sp!, { r1-r3, r12, r14 }		\n\t"
 
-		/* Disable Interrupt and switch back in IRQ mode */
+		/* Disable Interrupt and switch back to IRQ mode */
 		"msr	CPSR_c, #ARM_MODE_IRQ | I_BIT		\n\t"
 
 		/* Mark the End of Interrupt on the AIC */
 		"ldr	r14, =AT91C_BASE_AIC			\n\t"
-		"str	r14, [r14, #AIC_EOICR_OFF]	\n\t"
+		"str	r14, [r14, #AIC_EOICR]			\n\t"
 
-		/* count nested interrupts */
-		"ldr	r14, =nested_count		\n\t"
-		"ldr	r0, [r14]			\n\t"
-		"subs	r0, r0, #1			\n\t"
-		"str	r0, [r14]			\n\t"
-		"beq	irq_ctx_save			\n\t"
-
-		/* Restore r0 and SPSR_irq from IRQ stack */
-		"ldmia	sp!, { r0, r14 }		\n\t"
+		/* Restore SPSR_irq and r0 from IRQ stack */
+		"ldmia	sp!, { r0 }				\n\t"
+		"ldmia	sp!, { r14 }				\n\t"
 		"msr	SPSR_cxsf, r14				\n\t"
 
-		/* Restore adjusted LR_irq from IRQ stack */
+		/* Restore adjusted lr_irq from IRQ stack */
 		"ldmia	sp!, { pc }^				\n\t"
-
-		/* save the userspace context to current_context */
-		"irq_ctx_save:				\n\t"
-
-		/* get top of struct register_context */
-		"ldr	r0, =current_context		\n\t"
-		"ldr	r0, [r0]			\n\t"
-		"add	r0, r0, #68			\n\t"
-
-		/* save usermode cpsr & r2-14 */
-		"mrs	r1, spsr		\n\t"
-		"stmdb	r0, {r1-r14}^		\n\t"
-		"nop				\n\t"
-		"sub	r0, r0, #56		\n\t"
-
-		/* save r0-1 and svc_lr (= pc) */
-		"ldmia	sp!, {r1, r2}		\n\t"
-		"stmdb	r0!, {r1, r2, r14}	\n\t"
-
+	);
+}
+
+__attribute__((naked)) void FIQ_Handler(void)
+{
+	asm volatile (
+		".equ ARM_MODE_FIQ,	0x11		\n\t"
+		".equ ARM_MODE_SVC,	0x13		\n\t"
+		".equ I_BIT,		0x80		\n\t"
+		".equ F_BIT,		0x40		\n\t"
+
+		".equ AIC_FVR,		(260)		\n\t"
+
+		/* Save r0 to r9_fiq */
+		"mov	r9, r0					\n\t"
+
+		/* get FIQ Vector from AIC and thus clear FIQ */
+		"ldr	r0, [r8, #AIC_FVR]			\n\t"
+
+		/* Switch to SVC and save registers there */
+		"msr	CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT	\n\t"
+		"stmfd	sp!, { r1-r3, r12, lr }			\n\t"
+
+		/* execute FIQ in SVC_MODE */
+		"mov	r14, pc					\n\t"
+		"bx	r0					\n\t"
+
+		/* restore registers and switch back to FIQ */
+		"ldmia	sp!, { r1-r3, r12, lr }			\n\t"
+		"msr	CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT	\n\t"
+
+		/* restore the r0 from r9_fiq */
+		"mov	r0, r9					\n\t"
+
+		/* restore PC using the lr_fiq directly */
+		"subs	pc, lr, #4				\n\t"
 	);
 }

include/at91_adc.h

@@ -0,0 +1,32 @@
+#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_ */

include/at91_dbgu.h

@@ -1,8 +1,6 @@
 #ifndef AT91_DBGU_H_
 #define AT91_DBGU_H_
 
-#include <stddef.h>
-
 void at91_dbgu_init(void);
 
 void at91_dbgu_putc(char c);

include/at91_pitc.h

@@ -17,6 +17,7 @@ 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);

include/at91_tc1.h

@@ -5,13 +5,18 @@
 
 #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);
 

include/at91_tests.h

@@ -3,7 +3,4 @@
 
 void at91_rttc_test_init(void);
 
-void at91_adc_test_init(void);
-void at91_adc_printresults(void);
-
 #endif /*AT91_TESTS_H_*/

include/at91_twi.h

@@ -1,14 +1,14 @@
 #ifndef AT91TWI_H_
 #define AT91TWI_H_
 
-#include <list.h>
+#include <stdint.h>
 
 /* TWI slave addresses */
 #define TWI_ADDR_BL1		0x21
 #define TWI_ADDR_BL2		0x22
 #define TWI_ADDR_BL3		0x23
 #define TWI_ADDR_BL4		0x24
-#define TWI_ADDR_EEPROM		0x40
+#define TWI_ADDR_EEPROM		0x50
 
 /* TWIBOOT commands */
 #define CMD_WAIT		0x00
@@ -25,31 +25,72 @@
 //#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;
 
-struct blmc_cmd {
+	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 {
 	uint32_t cmd;		/* cmd byte(s) */
 	uint8_t mode;		/* read/write, cmdlen (1-3 bytes) */
-	uint8_t size;		/* data size */
+	uint16_t size;		/* data size */
 	uint8_t *data;		/* read/write data */
 };
 
 /* same bits as TWI_MMR[8..15] */
-#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
+#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
 
 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 blmc_cmd *cmd);
+uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd);
 
 void at91_twi_init(void);
 void at91_twi_test(void);

include/at91_udp.h

@@ -1,6 +1,20 @@
 #ifndef AT91_UDP_H_
 #define AT91_UDP_H_
 
+#include <stdint.h>
+
+void ep_transfer_send(uint32_t ep,
+			char *data,
+			uint32_t length,
+			void (*complete_cb)(void));
+
+void ep_transfer_receive(uint32_t ep,
+			char *data,
+			uint32_t length,
+			void (*complete_cb)(void));
+
+void ep_send_stall(uint32_t ep);
+
 void at91_udp_init(void);
 
 #endif /*AT91_UDP_H_*/

include/board.h

@@ -12,16 +12,16 @@
 #define BAUD_TO_DIV(BAUD) (MCK / (16 * BAUD))
 
 /* LED PIOs */
-#define LED_ORANGE	AT91C_PIO_PA17
-#define LED_GREEN	AT91C_PIO_PA18
+#define LED_ORANGE	AT91C_PIO_PA24
+#define LED_GREEN	AT91C_PIO_PA23
 
 /* Taster PIOs */
-#define TAST1		AT91C_PIO_PA19
-#define TAST2		AT91C_PIO_PA20
+#define TAST1		AT91C_PIO_PA0
+#define TAST2		AT91C_PIO_PA1
 
 /* USB PIOs */
-#define UDP_VBUS_MON	AT91C_PIO_PA24
-#define UDP_PULLUP	AT91C_PIO_PA25
+#define UDP_VBUS_MON	AT91C_PIO_PA8
+#define UDP_PULLUP	AT91C_PIO_PA16
 
 /* ATMEL IDs */
 #define USB_VENDOR_ID	0x03EB
@@ -40,4 +40,6 @@
 #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_*/

include/pidctrl.h

@@ -0,0 +1,23 @@
+#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_ */

include/rtos/context.h

@@ -1,70 +0,0 @@
-#ifndef _CONTEXT_H_
-#define _CONTEXT_H_
-
-#include <stdint.h>
-#include "rtos/spinlock.h"
-
-struct register_context {
-	uint32_t r0;
-	uint32_t r1;
-	uint32_t pc;
-	uint32_t cpsr;
-	uint32_t r2;
-	uint32_t r3;
-	uint32_t r4;
-	uint32_t r5;
-	uint32_t r6;
-	uint32_t r7;
-	uint32_t r8;
-	uint32_t r9;
-	uint32_t r10;
-	uint32_t r11;
-	uint32_t r12;
-	uint32_t sp;
-	uint32_t r14;
-};
-
-enum ctx_state {
-	CONTEXT_NULL = 0,
-	CONTEXT_RUNNING = 1,
-	CONTEXT_READY = 2,
-	CONTEXT_INTERRUPTED = 3,
-	CONTEXT_SLEEP = 4,
-	CONTEXT_SLEEP_QUEUE = 5,
-};
-
-struct context {
-	/* regs *MUST* be first in struct! */
-	struct register_context regs;
-	void *stack;
-	uint8_t state;
-	uint8_t priority;
-
-	/* next pointers for run & sleep queues */
-	struct context *run_queue;
-	struct context *sleep_queue;
-};
-
-extern struct context *current_context;
-
-void isr_context_yield(void);
-void context_yield(void);
-
-uint8_t isr_context_wait(struct spinlock *lock);
-uint8_t context_wait(struct spinlock *lock);
-uint8_t context_wait_queue(struct spinlock *lock, struct context **queue);
-uint8_t context_wait_pri_queue(struct spinlock *lock, struct context **queue);
-
-void isr_context_signal(struct context *c);
-void context_signal(struct context *c);
-uint32_t context_signal_queue(struct context **queue);
-
-void isr_context_interrupt(struct context *c);
-void context_interrupt(struct context *c);
-uint32_t context_interrupt_queue(struct context *c, struct context **queue);
-
-struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg);
-
-void init_context(void);
-
-#endif /* _CONTEXT_H_ */

include/rtos/semaphore.h

@@ -1,22 +0,0 @@
-#ifndef _SEMSPHORE_H_
-#define _SEMSPHORE_H_
-
-#include <stdint.h>
-
-#include "rtos/context.h"
-#include "rtos/spinlock.h"
-
-struct semaphore {
-	struct context *sleep_queue;
-	struct spinlock lock;
-	int32_t count;
-};
-
-uint8_t sem_wait(struct semaphore *sem);
-void sem_post(struct semaphore *sem);
-
-void sem_interrupt(struct semaphore *sem, struct context *c);
-int32_t sem_get_count(struct semaphore *sem);
-void sem_init(struct semaphore *sem, int32_t count);
-
-#endif /* _SEMSPHORE_H_ */

include/rtos/spinlock.h

@@ -1,19 +0,0 @@
-#ifndef _SPINLOCK_H_
-#define _SPINLOCK_H_
-
-#include <stdint.h>
-
-struct spinlock {
-	uint8_t locked;
-	uint8_t priority_unlocked;
-};
-
-void isr_spinlock_lock(struct spinlock *lock);
-void spinlock_lock(struct spinlock *lock);
-
-void isr_spinlock_unlock(struct spinlock *lock);
-void spinlock_unlock(struct spinlock *lock);
-
-void spinlock_init(struct spinlock *lock);
-
-#endif /* _SPINLOCK_H_ */

include/tdc_proto.h

@@ -0,0 +1,103 @@
+#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_ */

include/telemetrie.h

@@ -1,108 +1,19 @@
 #ifndef TELEMETRIE_H_
 #define TELEMETRIE_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));
-
-
+#include "tdc_proto.h"
 
 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_receive(struct comm_device *device);
-
-
-
-
-
-
+void tdc_check(void);
+void tdc_init(void);
 
 struct tdc_value {
 	void *data;
@@ -110,26 +21,45 @@ struct tdc_value {
 	uint32_t flags;
 };
 
-#define TDC_SIZEMASK	0x0F
-#define TDC_UNSIGNED	0x00
-#define TDC_SIGNED	0x10
-#define TDC_FP		0x20
+#if 1
 
-#define TDC_VALUE(var, desc, type, flags) \
-	type * tdc_check_##var(void) { return (&var); } \
+#define TDC_VALUE(name, var, desc, type, flags) \
+	type * tdc_check_##name(void) { return (&var); } \
 	static struct tdc_value __attribute__((used, section(".tdc_value"))) \
-	tdc_value_##var = { &var, desc, sizeof(type) | flags }; \
-	tdc_value_##var = tdc_value_##var;
+	tdc_value_##name = { &var, desc, sizeof(type) | flags }; \
+	tdc_value_##name = tdc_value_##name;
 
-#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)
+#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)
 
 #endif /*TELEMETRIE_H_*/

main.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - main loop                                                    *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -16,34 +18,43 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
-#include <stdint.h>
 #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 "rtos/context.h"
-#include "rtos/semaphore.h"
+#include "at91_tc1.h"
+#include "memalloc.h"
+#include "telemetrie.h"
 
-static struct semaphore sem;
+extern void base_ctrl(void);
 
-void testisr(uint32_t status, uint32_t input)
+volatile static uint32_t run_ctrl;
+
+static uint32_t base_ctrl_trigger(struct pitc_timer *timer)
 {
-	printf("testisr: sem_post()\n\r");
-	sem_post(&sem);
+	/* trigger adc, after ~15us () result should be ready */
+	adc_trigger();
+
+	run_ctrl = 1;
+
+	return PITC_RESTART_TIMER;
 }
 
-void testfunc(void *p)
-{
-	while (1) {
-		printf("testfunc(%ld): sem_wait()\n\r", (uint32_t)p);
-		sem_wait(&sem);
-	}
-}
+static struct pitc_timer base_timer = {
+	.interval = 2,
+	.func = &base_ctrl_trigger,
+};
 
 int main(void)
 {
@@ -51,21 +62,45 @@ int main(void)
 	*AT91C_PIOA_PER = LED_GREEN | LED_ORANGE;
 	*AT91C_PIOA_OER = LED_GREEN | LED_ORANGE;
 
+	/* needed for dbgu */
 	at91_sysc_init();
+
 	at91_dbgu_init();
 	at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");
 
+	/* triggers pinchange-isrs */
 	at91_pio_init();
 
-	struct context *text_ctx1 = create_ctx(512, 0x80, testfunc, (void *)1);
-	printf("test_ctx(1)=%p\n\r", text_ctx1);
+	/* timer */
+	at91_pitc_init();
+	at91_rttc_test_init();
 
-	struct context *text_ctx2 = create_ctx(512, 0x80, testfunc, (void *)2);
-	printf("test_ctx(2)=%p\n\r", text_ctx2);
+	/* twi */
+	at91_twi_init();
+	at91_twi_test();
 
-	sem_init(&sem, 0);
+	/* remote control, needs twi */
+	at91_tc1_init();
 
-	init_context();
+	/* 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;
+
+			base_ctrl();
+		}
+		tdc_check();
+	}
 }
-
-PIO_PINCHANGE_ISR(TAST1, testisr);

src/at91_adc.c

@@ -0,0 +1,238 @@
+/***************************************************************************
+ *   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);
+}

src/at91_dbgu.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - Debug Unit RS232 Port                                        *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -24,27 +26,18 @@
 #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, 16) == 0)
+		if (fifo_txpdc(txfifo, AT91C_BASE_PDC_DBGU, DBGU_TX_CHUNKS) == 0)
 			*AT91C_DBGU_IDR = AT91C_US_TXBUFE;
 }
 
@@ -55,11 +48,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(115200);
+	dbgu->DBGU_BRGR = BAUD_TO_DIV(DBGU_BAUDRATE);
 	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(1024);
+	txfifo = fifo_alloc(DBGU_FIFO_SIZE);
 
 	/* enable TX PDC */
 	dbgu->DBGU_PTCR = AT91C_PDC_TXTEN;
@@ -70,21 +63,18 @@ 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;
 }

src/at91_exceptions.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - Abort Handler with Register/Stackdump                        *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -161,7 +163,6 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers)
 	dbgu_putchar('\n');
 }
 
-
 __attribute__((naked)) void ABT_Handler(void)
 {
 	asm volatile (
@@ -182,9 +183,8 @@ __attribute__((naked)) void ABT_Handler(void)
 		"mov	r3, lr					\n\t"
 
 		/* enter previous mode and get lr(r14), sp(r13) */
-		/* TODO: interrupts might be enabled? */
-		/* TODO: thumb mode enabled? */
-		"msr	CPSR_c, r0				\n\t"
+		"orr	r1, r0, #I_BIT | F_BIT			\n\t"
+		"msr	CPSR_c, r1				\n\t"
 		"mov	r1, sp					\n\t"
 		"mov	r2, lr					\n\t"
 
@@ -195,7 +195,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"

src/at91_pio.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - Pinchange Interrupt Handler                                  *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *

src/at91_pitc.c

@@ -0,0 +1,104 @@
+/***************************************************************************
+ *   sam7fc - Periodic Timer Handling                                      *
+ *                                                                         *
+ *   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 "AT91SAM7S256.h"
+#include "at91_pitc.h"
+#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)
+{
+	timer->nextrun = timer->interval + pitc_ticks;
+
+	struct pitc_timer *search;
+	list_for_each_entry(search, &timer_list, list)
+		if (search->nextrun > timer->nextrun)
+			break;
+
+	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 */
+	pitc_ticks += (*AT91C_PITC_PIVR & AT91C_PITC_PICNT) >> 20;
+
+	struct pitc_timer *search, *tmp;
+	list_for_each_entry_safe(search, tmp, &timer_list, list) {
+		/* if this entry not scheduled yet, abort search */
+		if (pitc_ticks < search->nextrun)
+			break;
+
+		/* remove from list */
+		list_del(&search->list);
+
+		/* exec handler */
+		if ((search->interval == 0) || search->func(search) == PITC_REMOVE_TIMER) {
+			/* one-shot timer, mark as completed */
+			search->nextrun = 0;
+			continue;
+		}
+
+		/* interval timer, reschedule it */
+		_pitc_schedule_timer(search);
+	}
+}
+
+uint32_t pitc_get_ticks(void)
+{
+	return pitc_ticks;
+}
+
+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_PITEN |
+			   AT91C_PITC_PITIEN;
+}

src/at91_rttc_test.c

@@ -0,0 +1,48 @@
+/***************************************************************************
+ *   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);
+}

src/at91_sysc.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - System Interrupt Dispatcher                                  *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *

src/at91_tc1.c

@@ -0,0 +1,257 @@
+/***************************************************************************
+ *   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();
+}

src/at91_twi.c

@@ -0,0 +1,329 @@
+/***************************************************************************
+ *   sam7fc - TWI/I2C 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 <stdint.h>
+#include <stdio.h>
+
+#include "AT91SAM7S256.h"
+#include "board.h"
+#include "at91_twi.h"
+
+/*
+ * undocumented TWI_SR flags, at least OVRE seems to be present on a sam7s256
+ * taken from linux-2.6.24/include/asm-arm/arch-at91/at91_twi.h
+ */
+#define AT91_TWI_OVRE	(1<<6)	/* Overrun */
+#define AT91_TWI_UNRE	(1<<7)	/* Underrun */
+
+/*
+ * while ((cldiv = ((MCK / (2 * TWI)) -3) / (1 << ckdiv)) > 255)
+ *   	ckdiv++ ;
+ *
+ * works for TWI >= 100kHz
+ */
+#define TWI_CLK(x) (((MCK / (2 * (x))) -3)<<8 | ((MCK / (2 * (x))) -3))
+
+enum twi_states {
+	TWI_IDLE = 0x00,
+	TWI_ERROR,
+	TWI_GENERIC_CMD = 0x10,
+	TWI_BLMC_UPDATE = 0x20,
+};
+
+static volatile uint32_t twi_state = TWI_IDLE;
+static uint8_t *twi_data;
+static uint32_t twi_size;
+static uint32_t twi_count;
+
+static void twi_isr(void)
+{
+	/* get status */
+	uint32_t status = *AT91C_TWI_SR;
+	status &= *AT91C_TWI_IMR;
+
+	/* NACK - disable all interrupts and go to state TWI_ERROR */
+	if (status & AT91C_TWI_NACK) {
+		*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
+		twi_state = TWI_ERROR;
+		return;
+	}
+
+	/* tx register ready for new data */
+	if (status & AT91C_TWI_TXRDY) {
+		if (twi_count != twi_size) {
+			/* feed next byte */
+			*AT91C_TWI_THR = twi_data[twi_count++];
+
+		} else {
+			/* wait for TXCOMP */
+			*AT91C_TWI_IDR = AT91C_TWI_RXRDY | AT91C_TWI_TXRDY;
+			*AT91C_TWI_IER = AT91C_TWI_TXCOMP | AT91C_TWI_NACK;
+		}
+	}
+
+	/* rx register has data */
+	if (status & AT91C_TWI_RXRDY) {
+		/* get data */
+		twi_data[twi_count++] = *AT91C_TWI_RHR;
+
+		/* transfer complete? */
+		if (twi_count == twi_size) {
+			/* send STOP and wait for TXCOMP */
+			*AT91C_TWI_CR = AT91C_TWI_STOP;
+			*AT91C_TWI_IDR = AT91C_TWI_TXRDY;
+			*AT91C_TWI_IER = AT91C_TWI_TXCOMP | AT91C_TWI_NACK;
+		}
+	}
+
+	/* 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);
+
+			/* send next value to next blmc */
+			*AT91C_TWI_THR = *twi_data++;
+
+		} else {
+			*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
+			twi_state = TWI_IDLE;
+		}
+	}
+}
+
+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_size = 0;
+	twi_count = 0;
+
+	*AT91C_TWI_MMR = (TWI_ADDR_BL1 << 16) | AT91C_TWI_IADRSZ_1_BYTE;
+	*AT91C_TWI_IADR = CMD_SET_PWM;
+
+	*AT91C_TWI_THR = *twi_data++;
+	*AT91C_TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;
+
+	return 0;
+}
+
+uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd)
+{
+	if (twi_state == TWI_ERROR)
+		twi_state = TWI_IDLE;
+
+	if (twi_state != TWI_IDLE)
+		return 1;
+
+	/* TODO: locking needed? */
+	twi_state = TWI_GENERIC_CMD;
+
+	/* read transfer, or write transfer with payload */
+	if (cmd->mode & TWI_MODE_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;
+
+	/* write transfer without payload */
+	} else {
+		/* use one cmd byte as payload (needed to start transfer) */
+		cmd->mode--;
+		*AT91C_TWI_MMR = (addr << 16) | (cmd->mode & 0xFF) << 8;
+		*AT91C_TWI_IADR = (cmd->cmd) >> 8;
+	}
+
+	/* isr needs data & size parameters */
+	twi_data = cmd->data;
+	twi_size = cmd->size;
+	twi_count = 0;
+
+	if (cmd->mode & TWI_MODE_READ) {
+		*AT91C_TWI_CR = AT91C_TWI_START;
+		*AT91C_TWI_IER = AT91C_TWI_RXRDY | AT91C_TWI_NACK;
+
+	} else {
+		*AT91C_TWI_THR = (twi_size != 0) ? cmd->data[twi_count++] : (cmd->cmd & 0xFF);
+		*AT91C_TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;
+	}
+
+	/*
+	 * wait for end
+	 * TODO: locking needed?
+	 * TODO: timeout?
+	 */
+	while (twi_state != TWI_IDLE && twi_state != TWI_ERROR);
+	if (twi_state != TWI_IDLE) {
+		twi_state = TWI_IDLE;
+		return 1;
+	}
+
+	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 = {
+			.cmd = CMD_BOOT_LOADER,
+			.mode = TWI_MODE_WRITE | TWI_MODE_0_ARG,
+		};
+		twi_cmd(i, &cmd);
+
+		/* 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_SIGNATURE;
+		cmd.size = 4;
+		twi_cmd(i, &cmd);
+		printf("sig:0x%02x%02x%02x\n\r", buf[0], buf[1], buf[2]);
+
+		cmd.cmd = CMD_BOOT_APPLICATION;
+		cmd.mode = TWI_MODE_WRITE | TWI_MODE_0_ARG;
+		cmd.size = 0;
+		twi_cmd(i, &cmd);
+
+		/* TODO: sleep */
+		for (x = 0; x < 200000; 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);
+	}
+}
+
+void at91_twi_init(void)
+{
+	/* enable Clock */
+	*AT91C_PMC_PCER = (1 << AT91C_ID_TWI);
+
+	/* SDA & SCL from Peripheral A, Open Drain, no Pullup */
+	AT91S_PIO *pio = AT91C_BASE_PIOA;
+
+	/* do a software reset (bus not connected) */
+	*AT91C_TWI_CR = AT91C_TWI_SWRST;
+
+	pio->PIO_MDER = AT91C_PA3_TWD | AT91C_PA4_TWCK;
+	pio->PIO_PPUDR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
+	pio->PIO_ASR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
+	pio->PIO_PDR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
+
+	/* set TWI Clock */
+	*AT91C_TWI_CWGR = TWI_CLK(400000); //| (5<<16);
+
+	/* disable all (known) interrupts */
+	*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
+
+	/* level triggered, own vector */
+	AT91S_AIC *aic = AT91C_BASE_AIC;
+	aic->AIC_SMR[AT91C_ID_TWI] = IRQPRIO_TWI | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
+	aic->AIC_SVR[AT91C_ID_TWI] = (uint32_t)twi_isr;
+	aic->AIC_IECR = (1 << AT91C_ID_TWI);
+
+	/* enable teh monster */
+	*AT91C_TWI_CR = AT91C_TWI_MSEN;
+}

src/at91_udp.c

@@ -0,0 +1,683 @@
+/***************************************************************************
+ *   sam7fc - USB Device Port with logical Serial Port                     *
+ *                                                                         *
+ *   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 <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"
+
+#define csr_clear_flags(csr, flags) \
+	while ((csr) & (flags)) \
+		(csr) &= ~(flags);
+
+#define csr_set_flags(csr, flags) \
+	while (((csr) & (flags)) != (flags)) \
+		(csr) |= (flags);
+
+struct ep_transfer {
+	uint16_t length;
+	uint16_t curpos;
+
+	char *data;
+	void (*complete_cb)(void);
+};
+
+struct ep_ctx {
+	uint16_t maxpktsize;
+	uint16_t flags;
+
+	union {
+		struct ep_transfer *transfer;
+		struct fifo *fifo;
+	};
+};
+
+#define CTX_TRANSFER	0x01	/* ctx use ep_transfer struct */
+#define CTX_FIFO	0x02	/* ctx use fifo */
+#define CTX_IN		0x04	/* write to the host */
+#define CTX_OUT		0x08	/* read from the host */
+#define CTX_RXBANK0	0x10
+#define CTX_RXBANK1	0x20
+
+static struct ep_transfer ep0_transfer;
+static struct ep_ctx ep_ctx[4];
+
+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,
+	.bcdUSB			= 0x0110,
+	.bMaxPacketSize0	= 8,
+	.idVendor		= USB_VENDOR_ID,
+	.idProduct		= USB_PRODUCT_ID +1,
+	.bcdDevice		= 0x0001,
+	.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_dfu_descriptor               dfu;
+} __attribute__ ((packed));
+
+static const struct my_config cfg_descriptor = {
+.cfg = {
+	.bLength		= sizeof(struct usb_config_descriptor),
+	.bDescriptorType	= USB_DT_CONFIG,
+	.wTotalLength		= sizeof(struct my_config),
+	.bNumInterfaces		= 3,
+	.bConfigurationValue	= 1,
+	.bmAttributes		= USB_CONFIG_ATT_SELFPOWER | USB_CONFIG_ATT_WAKEUP,
+	.bMaxPower		= 50,
+},
+.ctrl_iface = {
+	.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,
+},
+.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 = {
+	.bLength		= sizeof(struct usb_interface_descriptor),
+	.bDescriptorType	= USB_DT_INTERFACE,
+	.bInterfaceNumber	= 1,
+	.bNumEndpoints		= 2,
+	.bInterfaceClass	= USB_CLASS_CDC_DATA,
+},
+.dataout_ep = {
+	.bLength		= sizeof(struct usb_endpoint_descriptor),
+	.bDescriptorType	= USB_DT_ENDPOINT,
+	.bEndpointAddress	= USB_DIR_OUT | 0x01,
+	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize		= 64,
+},
+.datain_ep = {
+	.bLength		= sizeof(struct usb_endpoint_descriptor),
+	.bDescriptorType	= USB_DT_ENDPOINT,
+	.bEndpointAddress	= USB_DIR_IN | 0x02,
+	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize		= 64,
+},
+.dfu_iface = {
+	.bLength                = sizeof(struct usb_interface_descriptor),
+	.bDescriptorType        = USB_DT_INTERFACE,
+	.bInterfaceNumber       = 2,
+	.bInterfaceClass        = USB_CLASS_APP_SPEC,
+	.bInterfaceSubClass     = 0x01, /* DFU */
+	.bInterfaceProtocol     = 0x01,
+},
+.dfu = {
+	.bLength                = sizeof(struct usb_dfu_descriptor),
+	.bDescriptorType        = USB_TYPE_DFU,
+	.bmAttributes           = USB_DFU_CAN_DOWNLOAD | USB_DFU_CAN_UPLOAD | USB_DFU_MANIFEST_TOL | USB_DFU_WILL_DETACH,
+	.wDetachTimeOut         = 0xff00,
+	.wTransferSize          = AT91C_IFLASH_PAGE_SIZE,
+	.bcdDFUVersion          = 0x0101,
+},
+};
+
+/* 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),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = { 0x0409 /* English */ },
+};
+
+static const struct usb_string_descriptor usb_string1 = {
+	/* String 1 "sam7fc" */
+	.bLength = sizeof(struct usb_string_descriptor) + 6 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0073, 0x0061, 0x006d, 0x0037, 0x0066, 0x0063,
+	},
+};
+
+static const struct usb_string_descriptor *usb_strings[] = {
+	&usb_string0, &usb_string1,
+};
+
+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);
+	if (!(ctx->flags & CTX_TRANSFER) || (ctx->flags & (CTX_IN | CTX_OUT)))
+		return;
+
+	/* from buffer to usb */
+	ctx->flags |= CTX_IN;
+
+	struct ep_transfer *transfer = ctx->transfer;
+	transfer->length = length;
+	transfer->curpos = 0;
+	transfer->data = data;
+	transfer->complete_cb = complete_cb;
+
+	uint32_t maxsize = ctx->maxpktsize;
+
+	/* get data from transfer */
+	while (transfer->curpos < transfer->length && maxsize--)
+		AT91C_UDP_FDR[ep] = transfer->data[transfer->curpos++];
+
+	/* trigger tx */
+	AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
+}
+
+void ep_transfer_receive(uint32_t ep, char *data, uint32_t length,
+				void (*complete_cb)(void))
+{
+	struct ep_ctx *ctx = &ep_ctx[ep];
+//	printf("ep_transfer_receive(%ld) size=%ld flags=0x%x\n\r", ep, length, ctx->flags);
+	if (!(ctx->flags & CTX_TRANSFER) || (ctx->flags & (CTX_IN | CTX_OUT)))
+		return;
+
+	/* from usb to buffer */
+	ctx->flags |= CTX_OUT;
+
+	struct ep_transfer *transfer = ctx->transfer;
+	transfer->length = length;
+	transfer->curpos = 0;
+	transfer->data = data;
+	transfer->complete_cb = complete_cb;
+}
+
+/* stalls the endpoint */
+static void ep_send_stall(uint32_t ep)
+{
+	printf("usb stall\n\r");
+	AT91C_UDP_CSR[ep] |= AT91C_UDP_FORCESTALL;
+}
+
+static void udp_configure_ep(const struct usb_endpoint_descriptor *desc)
+{
+	/* get endpoint address, set Max Packet Size */
+	uint32_t ep = desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
+	ep_ctx[ep].maxpktsize = desc->wMaxPacketSize;
+
+	/* get endpoint type (ctrl, iso, bulb, int) */
+	uint32_t eptype = desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
+	if (desc->bEndpointAddress & USB_ENDPOINT_DIR_MASK) {
+		eptype |= 0x04;
+	} else {
+		ep_ctx[ep].flags |= CTX_RXBANK0;
+	}
+
+	/* configure UDP endpoint and enable interrupt */
+	AT91C_UDP_CSR[ep] = AT91C_UDP_EPEDS | (eptype << 8);
+	*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)
+{
+	*AT91C_UDP_FADDR = (AT91C_UDP_FEN | current_address);
+	*AT91C_UDP_GLBSTATE = AT91C_UDP_FADDEN;
+	udp_print_config();
+}
+
+/*
+ * configure endpoints
+ * (USB_REQ_SET_CONFIGURATION callback)
+ */
+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;
+
+	/* set UDP to "configured" */
+	*AT91C_UDP_GLBSTATE = AT91C_UDP_CONFG;
+	udp_print_config();
+}
+
+static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
+{
+//	printf("typ:0x%02x req:0x%02x val:0x%04x idx:0x%04x len:0x%04x\n\r",
+//		req->bRequestType, req->bRequest, req->wValue, req->wIndex, req->wLength);
+
+	switch (req->bRequestType & (USB_TYPE_MASK | USB_RECIP_MASK)) {
+	case (USB_TYPE_STANDARD | USB_RECIP_DEVICE): /* 0x00/0x80 */
+	    	switch (req->bRequest) {
+	    	case USB_REQ_SET_ADDRESS: /* 0x05 */
+			current_address = req->wValue;
+			ep_transfer_send(0, NULL, 0, udp_txcb_setaddress);
+			break;
+
+		case USB_REQ_GET_DESCRIPTOR: /* 0x06 */
+			switch (req->wValue >> 8) {
+			case USB_DT_DEVICE: /* 0x01 */
+				ep_transfer_send(0, (char *)&dev_descriptor,
+					MIN(sizeof(dev_descriptor), req->wLength),
+					NULL);
+				break;
+
+			case USB_DT_CONFIG: /* 0x02 */
+				ep_transfer_send(0, (char *)&cfg_descriptor,
+					MIN(sizeof(cfg_descriptor), req->wLength),
+					NULL);
+				break;
+
+			case USB_DT_STRING: /* 0x03 */
+				;
+				uint8_t index = req->wValue & 0xFF;
+				if (index < ARRAY_SIZE(usb_strings)) {
+					ep_transfer_send(0, (char *)usb_strings[index],
+						MIN(usb_strings[index]->bLength, req->wLength),
+						NULL);
+				} else {
+					ep_send_stall(0);
+				}
+				break;
+
+			case USB_DT_CS_DEVICE: /* 0x21 */
+				ep_transfer_send(0, (char *)&cfg_descriptor.dfu,
+					MIN(sizeof(cfg_descriptor.dfu), req->wLength),
+					NULL);
+				break;
+
+			default:
+				ep_send_stall(0);
+				break;
+			}
+			break;
+
+		case USB_REQ_SET_CONFIGURATION: /* 0x09 */
+			current_config = req->wValue;
+			ep_transfer_send(0, NULL, 0, udp_txcb_setconfig);
+			break;
+
+		default:
+			ep_send_stall(0);
+			break;
+		}
+		break;
+
+	case (USB_TYPE_STANDARD | USB_RECIP_INTERFACE): /* 0x01/0x81 */
+		switch (req->bRequest) {
+		case USB_REQ_SET_INTERFACE: /* 0x0b */
+			current_interface = req->wValue;
+			ep_transfer_send(0, NULL, 0, udp_print_config);
+			break;
+
+		default:
+			ep_send_stall(0);
+			break;
+		}
+		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;
+		}
+		break;
+
+	default:
+		ep_send_stall(0);
+		break;
+	}
+}
+
+static void udp_handle_ep(uint32_t ep)
+{
+	/* endpoint enabled? */
+	AT91_REG *csr = &AT91C_UDP_CSR[ep];
+	if (!(*csr & AT91C_UDP_EPEDS))
+		return;
+
+	/* clear STALLSENT interrupt */
+	if (*csr & AT91C_UDP_STALLSENT)
+		csr_clear_flags(*csr, (AT91C_UDP_STALLSENT | AT91C_UDP_FORCESTALL));
+
+	/* ctrl request packet? */
+	if (*csr & AT91C_UDP_RXSETUP) {
+		struct usb_ctrlrequest req;
+		uint8_t *p;
+		for (p = (uint8_t *)&req; p < (uint8_t *)(&req +1); p++)
+			*p = AT91C_UDP_FDR[ep];
+
+		/* ack bank0 *now */
+		if (*csr & AT91C_UDP_RX_DATA_BK0)
+			csr_clear_flags(*csr, AT91C_UDP_RX_DATA_BK0);
+
+		/* set data phase transfer direction */
+		if (req.bRequestType & USB_DIR_IN)
+			*csr |= AT91C_UDP_DIR;
+
+		/* clear interrupt - *MUST* use csr_clear_flags() here */
+		csr_clear_flags(*csr, AT91C_UDP_RXSETUP);
+
+		ep_handle_ctrlrequest(&req);
+	}
+
+	void (* transfer_cb)(void) = NULL;
+
+	/* transmit complete? */
+	if (*csr & AT91C_UDP_TXCOMP) {
+		struct ep_ctx *ctx = &ep_ctx[ep];
+
+		if (ctx->flags & CTX_FIFO) {
+			/* get data from fifo */
+			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;
+			if (transfer->length != transfer->curpos) {
+				uint32_t maxsize = ctx->maxpktsize;
+
+				/* get data from transfer */
+				while (transfer->curpos < transfer->length && maxsize--)
+					AT91C_UDP_FDR[ep] = transfer->data[transfer->curpos++];
+
+				/* trigger tx */
+				AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
+
+			/* transfer complete, execute callback */
+			} else {
+				ctx->flags &= ~CTX_IN;
+				transfer_cb = transfer->complete_cb;
+			}
+		}
+
+		/* clear interrupt */
+		*csr &= ~(AT91C_UDP_TXCOMP);
+	}
+
+	/* 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?
+		if (!len && (ctx->flags & CTX_TRANSFER)) {
+			ctx->flags &= ~(CTX_OUT | CTX_IN);
+			ctx->transfer->length = 0;
+			ctx->transfer->curpos = 0;
+		}
+
+		if (ctx->flags & CTX_FIFO) {
+			fifo_rxudp(ctx->fifo, ep, len);
+
+		} else if ((ctx->flags & (CTX_TRANSFER | CTX_OUT)) == (CTX_TRANSFER | CTX_OUT)) {
+
+			/* transfer not complete */
+			struct ep_transfer *transfer = ctx->transfer;
+			if (transfer->length != transfer->curpos) {
+				/* get data from transfer */
+				while (transfer->curpos < transfer->length && len--)
+					transfer->data[transfer->curpos++] = AT91C_UDP_FDR[ep];
+			}
+
+			/* test again */
+			if (transfer->length == transfer->curpos) {
+				ctx->flags &= ~CTX_OUT;
+				transfer_cb = transfer->complete_cb;
+			}
+		}
+
+		if (ctx->flags & CTX_RXBANK0) {
+			if (*csr & AT91C_UDP_RX_DATA_BK0)
+				csr_clear_flags(*csr, AT91C_UDP_RX_DATA_BK0);
+
+			/* all but ep0 have ping pong buffers */
+			if (ep > 0)
+				ctx->flags = (ctx->flags & ~CTX_RXBANK0) | CTX_RXBANK1;
+
+		} else if (ctx->flags & CTX_RXBANK1) {
+			if (*csr & AT91C_UDP_RX_DATA_BK1)
+				csr_clear_flags(*csr, AT91C_UDP_RX_DATA_BK1);
+
+			ctx->flags = (ctx->flags & ~CTX_RXBANK1) | CTX_RXBANK0;
+		}
+	}
+
+	if (transfer_cb)
+		transfer_cb();
+}
+
+static void udp_isr(void)
+{
+	uint32_t isr = *AT91C_UDP_ISR;
+	if (isr & AT91C_UDP_ENDBUSRES) {
+		AT91S_UDP *udp = AT91C_BASE_UDP;
+
+		/* reset all endpoints */
+		udp->UDP_RSTEP = (AT91C_UDP_EP0 | AT91C_UDP_EP1 |
+				 AT91C_UDP_EP2 | AT91C_UDP_EP3) ;
+		udp->UDP_RSTEP = 0;
+
+		/* init ep0 */
+		struct ep_ctx *ctx = &ep_ctx[0];
+		ctx->maxpktsize = 8;
+		ctx->flags = CTX_TRANSFER | CTX_RXBANK0;
+		ctx->transfer = &ep0_transfer;
+		ctx->transfer->length = 0;
+		ctx->transfer->curpos = 0;
+
+		/* Configure endpoint0 as Control EP */
+		udp->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
+
+		/* enable ep0 Interrupt, disable all others */
+		udp->UDP_IER = AT91C_UDP_EPINT0;
+		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);
+	}
+
+	/* 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 */
+	AT91PS_PIO pio = AT91C_BASE_PIOA;
+	pio->PIO_CODR = UDP_PULLUP;
+	pio->PIO_PER = UDP_PULLUP;
+	pio->PIO_OER = UDP_PULLUP;
+
+	/* UDPCK (48MHz) = PLLCK / 2 */
+	*AT91C_CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
+
+	/* enable UDP clocks */
+	*AT91C_PMC_SCER = AT91C_PMC_UDP;
+	*AT91C_PMC_PCER = (1 << AT91C_ID_UDP);
+
+	/* enable transmitter */
+	*AT91C_UDP_TXVC &= ~AT91C_UDP_TXVDIS;
+
+	/* clear & disable all UDP interrupts */
+	*AT91C_UDP_IDR = AT91C_UDP_EPINT0 | AT91C_UDP_EPINT1 | AT91C_UDP_EPINT2 |
+			AT91C_UDP_EPINT3 | AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM |
+			AT91C_UDP_SOFINT | AT91C_UDP_WAKEUP;
+
+	*AT91C_UDP_ICR = AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM | AT91C_UDP_SOFINT |
+			AT91C_UDP_ENDBUSRES | AT91C_UDP_WAKEUP ;
+
+	/* level triggered, own vector */
+	AT91S_AIC *aic = AT91C_BASE_AIC;
+	aic->AIC_SMR[AT91C_ID_UDP] = IRQPRIO_UDP | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
+	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);
+}
+
+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);

src/fifo.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - FIFOs for use with PDC / USB Hardware                        *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -29,7 +31,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
- * a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
+ * FIXME: a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
  */
 static uint32_t fifo_used(struct fifo *fifo)
 {

src/flightctrl.c

@@ -0,0 +1,264 @@
+/***************************************************************************
+ *   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");
+}

src/memalloc.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - dynamic Memory Allocation                                    *
+ *                                                                         *
  *   Copyright (C) 02/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *

src/pidctrl.c

@@ -0,0 +1,44 @@
+/***************************************************************************
+ *   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);
+}

src/rtos/context.c

@@ -1,308 +0,0 @@
-#include <stdint.h>
-#include <stdio.h>
-#include <string.h>
-
-#include "atomic.h"
-#include "memalloc.h"
-
-#include "rtos/context.h"
-#include "rtos/spinlock.h"
-
-/* linked list of ready contexts, #1 is the running thread */
-struct context *volatile run_queue = NULL;
-
-/* pointer to the running thread */
-struct context *current_context = NULL;
-
-/*
- * SWI_Handler:
- * Entry with SVC mode, IRQs are disabled, FIQ is enabled
- */
-__attribute__((naked)) void SWI_Handler(void)
-{
-	/* store register context to current_context */
-	asm volatile (
-		/* r0-1 -> svc_stack */
-		"stmdb	sp!, {r0-r1}		\n\t"
-
-		/* get top of struct register_context */
-		"ldr	r0, =current_context	\n\t"
-		"ldr	r0, [r0]		\n\t"
-		"add	r0, r0, #68		\n\t"
-
-		/* save usermode cpsr & r2-14 */
-		"mrs	r1, spsr		\n\t"
-		"stmdb	r0, {r1-r14}^		\n\t"
-		"nop				\n\t"
-		"sub	r0, r0, #56		\n\t"
-
-		/* save r0-1 and svc_lr (= pc) */
-		"ldmia	sp!, {r1, r2}		\n\t"
-		"stmdb	r0!, {r1, r2, r14}	\n\t"
-	);
-
-	/* we're no longer #1 in run_queue, switch to new #1 */
-	if (current_context != run_queue)
-		current_context = run_queue;
-
-	current_context->state = CONTEXT_RUNNING;
-
-	if (((uint32_t *)current_context->stack)[0] != 0xdeadbeef)
-		printf("<- task stack corrupt (%p)\n\r", current_context);
-
-	/* restore register context from current_context */
-	asm volatile (
-		/* get pointer to struct register_context */
-		"ldr	r0, =current_context	\n\t"
-		"ldr	r0, [r0]		\n\t"
-
-		/* get values of r0-1 and pc (= svc_lr) */
-		"ldmia	r0!, {r1-r2,r14}	\n\t"
-		"stmdb	sp!, {r1-r2}		\n\t"
-
-		/* restore usermode cpsr & r2-14 */
-		"ldmia	r0, {r1-r14}^		\n\t"
-		"nop				\n\t"
-		"msr	spsr, r1		\n\t"
-
-		/* get r0-1 from svc_stack, jump back */
-		"ldmia	sp!, {r0, r1}		\n\t"
-		"movs	pc, lr			\n\t"
-	);
-}
-
-static uint8_t isr_context_switch(void)
-{
-	asm volatile ("swi");
-
-	// TODO: return previous state of now running thread
-	return CONTEXT_READY;
-}
-
-/* inserts context into run_queue */
-static void isr_context_ready(struct context *ctx)
-{
-	struct context *q = run_queue;
-	struct context *volatile *qprev = &run_queue;
-
-	while (q && (q->priority <= ctx->priority)) {
-		qprev = &q->run_queue;
-		q = q->run_queue;
-	}
-
-	ctx->run_queue = q;
-	*qprev = ctx;
-}
-
-/* process yields, try to switch to process with lower or same prio */
-void isr_context_yield(void)
-{
-	run_queue = current_context->run_queue;
-	isr_context_ready(current_context);
-
-	if (current_context != run_queue) {
-		current_context->state = CONTEXT_READY;
-		isr_context_switch();
-	}
-}
-
-void context_yield(void)
-{
-	disable_irqs();
-	isr_context_yield();
-	restore_irqs();
-}
-
-static uint8_t __isr_context_wait(struct spinlock *lock, uint8_t sleepstate)
-{
-	isr_spinlock_unlock(lock);
-
-	run_queue = current_context->run_queue;
-	current_context->state = sleepstate;
-
-	uint8_t retval = isr_context_switch();
-
-	isr_spinlock_lock(lock);
-
-	return retval;
-}
-
-uint8_t isr_context_wait(struct spinlock *lock)
-{
-	return  __isr_context_wait(lock, CONTEXT_SLEEP);
-}
-
-uint8_t context_wait(struct spinlock *lock)
-{
-	disable_irqs();
-	uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP);
-	restore_irqs();
-
-	return retval;
-}
-
-uint8_t context_wait_queue(struct spinlock *lock, struct context **queue)
-{
-	disable_irqs();
-	current_context->sleep_queue = *queue;
-	*queue = current_context;
-
-	uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP_QUEUE);
-	restore_irqs();
-
-	return retval;
-}
-
-uint8_t context_wait_pri_queue(struct spinlock *lock, struct context **queue)
-{
-	disable_irqs();
-
-	struct context *q = *queue;
-	while (q && (q->priority <= lock->priority_unlocked)) {
-		queue = &q->sleep_queue;
-		q = q->sleep_queue;
-	}
-
-	current_context->sleep_queue = q;
-	*queue = current_context;
-
-	uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP_QUEUE);
-	restore_irqs();
-
-	return retval;
-}
-
-void isr_context_signal(struct context *c)
-{
-	if (c->state == CONTEXT_SLEEP) {
-		c->state = CONTEXT_READY;
-		isr_context_ready(c);
-	}
-}
-
-void context_signal(struct context *c)
-{
-	disable_irqs();
-	isr_context_signal(c);
-	restore_irqs();
-}
-
-uint32_t context_signal_queue(struct context **queue)
-{
-	disable_irqs();
-
-	uint32_t retval = 0;
-	if (*queue) {
-		struct context *c = *queue;
-		*queue = c->sleep_queue;
-		c->state = CONTEXT_READY;
-		isr_context_ready(c);
-		retval = 1;
-	}
-
-	restore_irqs();
-
-	return retval;
-}
-
-void isr_context_interrupt(struct context *c)
-{
-	if (c->state == CONTEXT_SLEEP) {
-		c->state = CONTEXT_INTERRUPTED;
-		isr_context_ready(c);
-	}
-
-}
-
-void context_interrupt(struct context *c)
-{
-	disable_irqs();
-	isr_context_signal(c);
-	restore_irqs();
-}
-
-uint32_t context_interrupt_queue(struct context *c, struct context **queue)
-{
-	disable_irqs();
-
-	struct context *q = *queue;
-	while (q && (q != c)) {
-		queue = &q->sleep_queue;
-		q = q->sleep_queue;
-	}
-
-	uint32_t retval = 0;
-
-	if (q) {
-		*queue = c->sleep_queue;
-		c->state = CONTEXT_INTERRUPTED;
-		isr_context_ready(c);
-		retval = 1;
-	}
-
-	restore_irqs();
-
-	return retval;
-}
-
-struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg)
-{
-	uint32_t *stack = static_alloc(sizeof(struct context) + stacksize);
-
-	memset(stack, 0, stacksize);
-
-	// TODO: check on context-switch for corruption */
-	stack[0] = 0xdeadbeef;
-
-	struct context *ctx = (struct context *)((uint8_t *)stack + stacksize);
-
-	ctx->regs.r0 = (uint32_t)arg;
-	ctx->regs.pc = (uint32_t)code;
-	ctx->regs.cpsr = 0x0000001F;
-	ctx->regs.sp = (uint32_t)ctx;
-
-	ctx->regs.r1 = 0x01010101;
-	ctx->regs.r2 = 0x02020202;
-	ctx->regs.r3 = 0x03030303;
-	ctx->regs.r4 = 0x04040404;
-	ctx->regs.r5 = 0x05050505;
-	ctx->regs.r6 = 0x06060606;
-	ctx->regs.r7 = 0x07070707;
-	ctx->regs.r8 = 0x08080808;
-	ctx->regs.r9 = 0x09090909;
-	ctx->regs.r10 = 0x10101010;
-	ctx->regs.r11 = 0x11111111;
-	ctx->regs.r12 = 0x12121212;
-	ctx->regs.r14 = 0x14141414;
-
-	ctx->stack = stack;
-	ctx->priority = priority;
-
-	disable_irqs();
-	isr_context_ready(ctx);
-	restore_irqs();
-
-	return ctx;
-}
-
-void init_context(void)
-{
-	/*
-	 * create shallow idle context
-	 * idle context runs in SVC mode, so no real stack is needed
-	 */
-	current_context = create_ctx(4, 255, NULL, NULL);
-	printf("idle_ctx=%p\n\r", current_context);
-
-
-	/* For now, we're the only thread, so this simply safes our context */
-	disable_irqs();
-	isr_context_switch();
-	restore_irqs();
-
-	/* idle loop */
-	while (1) {
-		// TODO: this sucks, context switch after interrupt are way better..
-		context_yield();
-	}
-}

src/rtos/semaphore.c

@@ -1,56 +0,0 @@
-#include <stdint.h>
-
-#include "rtos/context.h"
-#include "rtos/semaphore.h"
-#include "rtos/spinlock.h"
-
-uint8_t sem_wait(struct semaphore *sem)
-{
-	uint32_t retval = CONTEXT_READY;
-
-	spinlock_lock(&sem->lock);
-
-	sem->count--;
-	if (sem->count < 0)
-		retval = context_wait_pri_queue(&sem->lock, &sem->sleep_queue);
-
-	spinlock_unlock(&sem->lock);
-
-	return retval;
-}
-
-void sem_post(struct semaphore *sem)
-{
-	spinlock_lock(&sem->lock);
-
-	sem->count++;
-	if (sem->count <= 0)
-		context_signal_queue(&sem->sleep_queue);
-
-	spinlock_unlock(&sem->lock);
-}
-
-void sem_interrupt(struct semaphore *sem, struct context *c)
-{
-	spinlock_lock(&sem->lock);
-	context_interrupt_queue(c, &sem->sleep_queue);
-	spinlock_unlock(&sem->lock);
-}
-
-int32_t sem_get_count(struct semaphore *sem)
-{
-	int32_t retval;
-
-	spinlock_lock(&sem->lock);
-	retval = sem->count;
-	spinlock_unlock(&sem->lock);
-
-	return retval;
-}
-
-void sem_init(struct semaphore *sem, int32_t count)
-{
-	sem->sleep_queue = 0;
-	spinlock_init(&sem->lock);
-	sem->count = 0;
-}

src/rtos/spinlock.c

@@ -1,40 +0,0 @@
-#include <stdint.h>
-
-#include "atomic.h"
-
-#include "rtos/context.h"
-#include "rtos/spinlock.h"
-
-void isr_spinlock_lock(struct spinlock *lock)
-{
-	lock->priority_unlocked = current_context->priority;
-	current_context->priority = 0;
-	lock->locked = 1;
-}
-
-void spinlock_lock(struct spinlock *lock)
-{
-	disable_irqs()
-	isr_spinlock_lock(lock);
-	restore_irqs();
-}
-
-void isr_spinlock_unlock(struct spinlock *lock)
-{
-	current_context->priority = lock->priority_unlocked;
-	lock->locked = 0;
-
-	// TODO: check for context-switch...
-}
-
-void spinlock_unlock(struct spinlock *lock)
-{
-	disable_irqs()
-	isr_spinlock_unlock(lock);
-	restore_irqs();
-}
-
-void spinlock_init(struct spinlock *lock)
-{
-	lock->locked = 0;
-}

src/telemetrie.c

@@ -0,0 +1,338 @@
+/***************************************************************************
+ *   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);
+}