blctrl flash write

at91_init0.s

@@ -17,6 +17,7 @@
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
 	.equ AT91C_BASE_AIC,	(0xFFFFF000)
+	.equ AT91C_BASE_MC,	(0xFFFFFF00)
 
 	.equ IRQ_Stack_Size,	(3 * 8 * 4)
 	.equ FIQ_Stack_Size,	(0 * 8 * 4)
@@ -30,6 +31,49 @@
 	.equ I_BIT,		0x80
 	.equ F_BIT,		0x40
 
+	.section .init_dummy, "x"
+	.global _hwstart
+	.func _hwstart
+_hwstart:
+		ldr     pc, [pc, #24]		/* 0x00 Reset handler */
+		ldr     pc, [pc, #24]		/* 0x04 Undefined Instruction */
+		ldr     pc, [pc, #20]		/* 0x08 Software Interrupt */
+		ldr     pc, [pc, #16]		/* 0x0C Prefetch Abort */
+		ldr     pc, [pc, #12]		/* 0x10 Data Abort */
+		ldr     pc, [pc, #8]		/* 0x14 reserved  */
+		ldr     pc, [pc, #4]		/* 0x18 IRQ */
+		ldr     pc, [pc, #0]		/* 0x1c FIQ */
+
+	.word _relocate + 0x13c000
+	.word 0x80000000
+
+	.endfunc
+
+	.section .init, "x"
+	.global _relocate
+	.func _relocate
+_relocate:
+		msr	CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT
+
+		/* Remap RAM to 0x00000000 */
+		ldr	r1, =AT91C_BASE_MC
+		mov	r2, #0x01
+		str	r2, [r1]
+
+		/* Relocate .text section */
+		ldr	r1, =__text_copy_start__
+		ldr	r2, =__text_start__
+		ldr	r3, =__text_end__
+_relocate1:	cmp	r2, r3
+		ldrlo	r0, [r1], #4
+		strlo	r0, [r2], #4
+		blo	_relocate1
+
+		ldr	r0, =InitReset
+		mov	pc, r0
+
+	.endfunc
+
 	.section .text
 	.global _start
 	.func   _start
@@ -93,7 +137,7 @@ InitReset:
 		mov	sp, r0
 
 		/* Relocate .data section */
-		ldr	r1, =__text_end__
+		ldr	r1, =__data_copy_start__
 		ldr	r2, =__data_start__
 		ldr	r3, =__data_end__
 LoopRel:	cmp	r2, r3
@@ -107,7 +151,7 @@ LoopRel:	cmp	r2, r3
 		ldr	r2, =__bss_end__
 LoopZI:		cmp	r1, r2
 		strlo	r0, [r1], #4
-		BLO	LoopZI
+		blo	LoopZI
 
 		/* Start main() */
 		ldr	lr, =exit

at91_init1.c

@@ -18,6 +18,7 @@
  ***************************************************************************/
 #include <stdint.h>
 #include "AT91SAM7S256.h"
+#include "board.h"
 
 static void empty_isr(void) {}
 
@@ -36,7 +37,7 @@ void at91_init1(void)
 	*AT91C_RSTC_RMR = (AT91C_RSTC_KEY & 0xA5 << 24) | AT91C_RSTC_URSTEN;
 
 	/* Set Flash Waitstates */
-	*AT91C_MC_FMR = AT91C_MC_FWS_1FWS;
+	*AT91C_MC_FMR = (AT91C_MC_FMCN & ((MCK / 666666) << 16)) | AT91C_MC_FWS_1FWS;
 
 	/*
 	 * Enable main oscillator (MAINCK)
@@ -78,6 +79,11 @@ void at91_init1(void)
 		aic->AIC_SVR[i] = (uint32_t)empty_isr;
 	}
 	aic->AIC_SPU = (uint32_t)empty_isr;
+
+	/* end-of-interrupt signal */
+	do {
+		aic->AIC_EOICR = ~0;
+	} while (aic->AIC_ISR != 0x00);
 }
 
 __attribute__((naked)) void IRQ_Handler(void)

include/at91_twi.h

@@ -1,7 +1,7 @@
 #ifndef AT91TWI_H_
 #define AT91TWI_H_
 
-#include <list.h>
+#include <stdint.h>
 
 /* TWI slave addresses */
 #define TWI_ADDR_BL1		0x21
@@ -33,7 +33,7 @@
 struct blmc_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 */
 };
 

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_*/

ldscript.ld

@@ -1,15 +1,28 @@
 MEMORY
 {
-	CODE (rx)  : ORIGIN = 0x00100000, LENGTH = 256k
+	FLASH(rx)  : ORIGIN = 0x00100000, LENGTH = 256k
-	DATA (rwx) : ORIGIN = 0x00200000, LENGTH = 64k
+	CODE (rx)  : ORIGIN = 0x0013C000, LENGTH = 16k
+	DATA (rwx) : ORIGIN = 0x00000000, LENGTH = 64k
 }
 
 SECTIONS
 {
+	. = ORIGIN(FLASH);
+
+	.init_dummy : {
+		*(.init_dummy)
+	} >FLASH =0
+
 	. = ORIGIN(CODE);
-	.text : {
+
-		*at91_init0.o (.text)
+	.init : {
-		*at91_init1.o (.text)
+		*(.init)
+		__text_copy_start__ = .;
+	} >CODE =0
+
+	.text : AT (LOADADDR(.init) + SIZEOF(.init)) {
+		*at91_init0.o(*.text)
+		*at91_init1.o(*.text)
 		*(.text)
 		*(.glue_7t)
 		*(.glue_7)
@@ -28,10 +41,13 @@ SECTIONS
 		*(.rodata)
 		*(.rodata.*)
 
-		__text_end__ = .;
+		__data_copy_start__ = (__text_copy_start__ + (__text_end__ - __text_start__));
-	} >CODE =0
+	} >DATA =0
 
-	.data : AT (ADDR(.text) + SIZEOF(.text)) {
+	__text_start__ = ADDR(.text);
+	__text_end__ = ADDR(.text) + SIZEOF(.text);
+
+	.data : AT (LOADADDR(.text) + SIZEOF(.text)) {
 		*(.data)
 	} >DATA =0
 

main.c

@@ -20,12 +20,9 @@
 #include "at91_sysc.h"
 #include "at91_dbgu.h"
 #include "at91_pitc.h"
-#include "at91_tests.h"
 #include "at91_udp.h"
-#include "at91_pio.h"
 #include "at91_twi.h"
 
-#include "at91_tc1.h"
 #include "memalloc.h"
 
 #include "board.h"
@@ -38,17 +35,7 @@ static uint32_t pitc_test(struct pitc_timer *timer)
 	*AT91C_PIOA_SODR = i;
 	i = i ^ LED_GREEN;
 	*AT91C_PIOA_CODR = i;
-/*
-	struct rc_values rc;
-	uint32_t count = rcontrol_getvalues(&rc);
 
-	printf("%ld channels: ", count);
-	uint32_t j;
-	for (j = 0; j < count; j++)
-		printf("%+5d ", rc.chan[j]);
-
-	printf("\r");
-*/
 	return PITC_RESTART_TIMER;
 }
 
@@ -69,16 +56,8 @@ int main(void)
 	at91_dbgu_init();
 	at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");
 
-	/* triggers pinchange-isrs */
-	at91_pio_init();
-
 	/* timer */
 	at91_pitc_init();
-	at91_rttc_test_init();
-	at91_tc1_init();
-
-	/* adc, need timer */
-	at91_adc_test_init();
 
 	/* twi */
 	at91_twi_init();

src/at91_adc.c

@@ -1,98 +0,0 @@
-/***************************************************************************
- *   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_pitc.h"
-
-static uint16_t adc_result[7];
-
-static void at91_adc_isr(void)
-{
-	AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
-	pdc->PDC_RPR = (uint32_t) &adc_result;
-	pdc->PDC_RCR = ARRAY_SIZE(adc_result);
-	pdc->PDC_PTCR = AT91C_PDC_RXTEN;
-
-	/* clear interrupts */
-	AT91S_ADC *adc = AT91C_BASE_ADC;
-	uint32_t status = adc->ADC_SR;
-	status = status;
-}
-
-static uint32_t adc_trigger(struct pitc_timer *timer)
-{
-	uint32_t i;
-	for (i = 0; i < ARRAY_SIZE(adc_result); i++)
-		printf("0x%03x ", adc_result[i]);
-
-	printf("\n\r");
-
-	*AT91C_ADC_CR = AT91C_ADC_START;
-	return PITC_RESTART_TIMER;
-}
-
-static struct pitc_timer adc_timer = {
-	.interval = 10,
-	.func = &adc_trigger,
-};
-
-void at91_adc_test_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_result;
-	pdc->PDC_RCR = ARRAY_SIZE(adc_result);
-	pdc->PDC_PTCR = AT91C_PDC_RXTEN;
-
-	/* enable 4 channels, 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);
-
-//	pitc_schedule_timer(&adc_timer);
-}

src/at91_dbgu.c

@@ -44,7 +44,7 @@ static void dbgu_isr(uint32_t status)
 	}
 */
 	if (status & AT91C_US_TXBUFE)
-		if (fifo_txpdc(txfifo, AT91C_BASE_PDC_DBGU, 16) == 0)
+		if (fifo_txpdc(txfifo, AT91C_BASE_PDC_DBGU, 32) == 0)
 			*AT91C_DBGU_IDR = AT91C_US_TXBUFE;
 }
 
@@ -59,7 +59,7 @@ void at91_dbgu_init(void)
 	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(16384);
 
 	/* enable TX PDC */
 	dbgu->DBGU_PTCR = AT91C_PDC_TXTEN;

src/at91_pio.c

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

src/at91_rttc_test.c

@@ -1,33 +0,0 @@
-#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;
-
-	static uint32_t i;
-	*AT91C_PIOA_SODR = i;
-	i = i ^ LED_ORANGE;
-	*AT91C_PIOA_CODR = i;
-}
-
-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_tc1.c

@@ -1,227 +0,0 @@
-/***************************************************************************
- *   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_pio.h"
-
-/* Hard limits for ISR */
-#define PULSE_MIN	0x0500
-#define PULSE_MAX	0x0D00
-#define PULSE_TIMEOUT	0x0F00
-#define PULSE_CENTER	0x08C0
-
-/* 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 */
-};
-
-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 = (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));
-
-		/* keep result in range */
-		if (tmp > VALUE_RANGE)
-			tmp = VALUE_RANGE;
-
-		if (tmp < -VALUE_RANGE)
-			tmp = -VALUE_RANGE;
-
-		rc->chan[i] = tmp;
-	}
-	return cnt;
-}
-
-void rcontrol_calibrate(uint32_t mode)
-{
-	uint32_t i;
-
-	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..+1024 */
-			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 -1024..0 */
-			if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF)
-				ch_data[i].mid = ch_data[i].max;
-		}
-		break;
-	}
-}
-
-void rcontrol_print_cal(void)
-{
-	uint32_t i;
-	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);
-}
-
-static void tast_monitor(uint32_t status, uint32_t input)
-{
-	if (!(input & TAST1) && (cal_in_progress == 0)) {
-		printf("start calibration\n\r");
-
-		rcontrol_calibrate(RC_CAL_START);
-
-
-	} else if (!(input & TAST2) && (cal_in_progress == 1)) {
-		printf("end calibration\n\r");
-
-		rcontrol_calibrate(RC_CAL_END);
-		rcontrol_print_cal();
-	}
-}
-
-PIO_PINCHANGE_ISR(TAST1 | TAST2, tast_monitor);

src/at91_twi.c

@@ -210,14 +210,14 @@ void at91_twi_test(void)
 		cmd.cmd = CMD_GET_SIGNATURE;
 		cmd.size = 4;
 		ret = twi_cmd(i, &cmd);
-		printf("sig(%ld):0x%02x%02x%02x ", ret, buf[0], buf[1], buf[2]);
+		printf("sig(%ld):0x%02x%02x%02x\n\r", ret, buf[0], buf[1], buf[2]);
-
+/*
 		cmd.cmd = CMD_BOOT_APPLICATION;
 		cmd.mode = BLMC_CMD_WRITE | BLMC_CMD_0_ARG;
 		cmd.size = 0;
 		ret = twi_cmd(i, &cmd);
 
-		for (x = 0; x < 200000; x++);
+		for (x = 0; x < 400000; x++);
 
 		cmd.cmd = CMD_GET_INFO,
 		cmd.mode = BLMC_CMD_READ | BLMC_CMD_0_ARG,
@@ -225,6 +225,7 @@ void at91_twi_test(void)
 		cmd.data = buf,
 		ret = twi_cmd(i, &cmd);
 		printf("app(%ld):'%s'\n\r", ret, buf);
+*/
 	}
 }
 

src/at91_udp.c

@@ -18,12 +18,15 @@
  ***************************************************************************/
 #include <stdio.h>
 #include "AT91SAM7S256.h"
-#include "at91_pio.h"
 #include "board.h"
 #include "fifo.h"
 
 #include "usb_ch9.h"
-#include "usb_cdc.h"
+#include "usb_dfu.h"
+
+/* from dfu.c */
+extern void ep0_handle_dfu(struct usb_ctrlrequest *req);
+
 
 #define csr_clear_flags(csr, flags) \
 	while ((csr) & (flags)) \
@@ -73,20 +76,18 @@ static const struct usb_device_descriptor dev_descriptor = {
 	.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_interface_descriptor         iface0;
-	struct usb_cdc_header_desc		cdc_header;
+	struct usb_interface_descriptor         iface1;
-	struct usb_cdc_call_mgmt_descriptor	cdc_call_mgmt;
+	struct usb_interface_descriptor         iface2;
-	struct usb_cdc_acm_descriptor		cdc_acm;
+	struct usb_interface_descriptor         iface3;
-	struct usb_cdc_union_desc		cdc_union;
+	struct usb_interface_descriptor         iface4;
-	struct usb_endpoint_descriptor		notify_ep;
+	struct usb_interface_descriptor         iface5;
-	struct usb_interface_descriptor		data_iface;
+	struct usb_dfu_descriptor               dfu;
-	struct usb_endpoint_descriptor		dataout_ep;
-	struct usb_endpoint_descriptor		datain_ep;
 } __attribute__ ((packed));
 
 static const struct my_config cfg_descriptor = {
@@ -94,82 +95,173 @@ static const struct my_config cfg_descriptor = {
 	.bLength		= sizeof(struct usb_config_descriptor),
 	.bDescriptorType	= USB_DT_CONFIG,
 	.wTotalLength		= sizeof(struct my_config),
-	.bNumInterfaces		= 2,
+	.bNumInterfaces		= 6,
 	.bConfigurationValue	= 1,
+	.iConfiguration		= 0x02,
 	.bmAttributes		= USB_CONFIG_ATT_SELFPOWER | USB_CONFIG_ATT_WAKEUP,
 	.bMaxPower		= 50,
 },
-.ctrl_iface = {
+.iface0 = {
 	.bLength		= sizeof(struct usb_interface_descriptor),
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= 0,
-	.bNumEndpoints		= 1,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
-	.bInterfaceClass	= USB_CLASS_COMM,
+	.bInterfaceSubClass	= 0x01, /* DFU */
-	.bInterfaceSubClass	= USB_CDC_SUBCLASS_ACM,
+	.bInterfaceProtocol	= 0x02,
-	.bInterfaceProtocol	= 1,
+	.iInterface             = 0x03,
 },
-.cdc_header = {
+.iface1 = {
-	.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_APP_SPEC,
-	.bInterfaceClass	= USB_CLASS_CDC_DATA,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x04,
 },
-.dataout_ep = {
+.iface2 = {
-	.bLength		= sizeof(struct usb_endpoint_descriptor),
+	.bLength		= sizeof(struct usb_interface_descriptor),
-	.bDescriptorType	= USB_DT_ENDPOINT,
+	.bDescriptorType	= USB_DT_INTERFACE,
-	.bEndpointAddress	= USB_DIR_OUT | 0x01,
+	.bInterfaceNumber	= 2,
-	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
-	.wMaxPacketSize		= 64,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x05,
 },
-.datain_ep = {
+.iface3 = {
-	.bLength		= sizeof(struct usb_endpoint_descriptor),
+	.bLength		= sizeof(struct usb_interface_descriptor),
-	.bDescriptorType	= USB_DT_ENDPOINT,
+	.bDescriptorType	= USB_DT_INTERFACE,
-	.bEndpointAddress	= USB_DIR_IN | 0x02,
+	.bInterfaceNumber	= 3,
-	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
-	.wMaxPacketSize		= 64,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x06,
+},
+.iface4 = {
+	.bLength		= sizeof(struct usb_interface_descriptor),
+	.bDescriptorType	= USB_DT_INTERFACE,
+	.bInterfaceNumber	= 4,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x07,
+},
+.iface5 = {
+	.bLength		= sizeof(struct usb_interface_descriptor),
+	.bDescriptorType	= USB_DT_INTERFACE,
+	.bInterfaceNumber	= 5,
+	.bInterfaceClass	= USB_CLASS_APP_SPEC,
+	.bInterfaceSubClass	= 0x01, /* DFU */
+	.bInterfaceProtocol	= 0x02,
+	.iInterface             = 0x08,
+},
+.dfu = {
+	.bLength                = sizeof(struct usb_dfu_descriptor),
+	.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,
 },
 };
 
-static void ep_transfer_send(uint32_t ep, char *data, uint32_t length,
+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_string2 = {
+	/* String 2 "sam7fc-dfu" */
+	.bLength = sizeof(struct usb_string_descriptor) + 10 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+        .wData = {
+		0x0073, 0x0061, 0x006d, 0x0037, 0x0066, 0x0063, 0x002d, 0x0064,
+		0x0066, 0x0075,
+        },
+};
+
+static const struct usb_string_descriptor usb_string3 = {
+	/* String 3 "sam7fc-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 12 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0073, 0x0061, 0x006d, 0x0037, 0x0066, 0x0063, 0x002d, 0x0066,
+		0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
+static const struct usb_string_descriptor usb_string4 = {
+	/* String 4 "sam7fc-eeprom" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0073, 0x0061, 0x006d, 0x0037, 0x0066, 0x0063, 0x002d, 0x0065,
+		0x0065, 0x0070, 0x0072, 0x006f, 0x006d,
+	},
+};
+
+static const struct usb_string_descriptor usb_string5 = {
+	/* String 5 "blctrl1-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0062, 0x006c, 0x0063, 0x0074, 0x0072, 0x006c, 0x0031, 0x002d,
+		0x0066, 0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
+static const struct usb_string_descriptor usb_string6 = {
+	/* String 6 "blctrl2-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0062, 0x006c, 0x0063, 0x0074, 0x0072, 0x006c, 0x0032, 0x002d,
+		0x0066, 0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
+static const struct usb_string_descriptor usb_string7 = {
+	/* String 7 "blctrl3-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0062, 0x006c, 0x0063, 0x0074, 0x0072, 0x006c, 0x0033, 0x002d,
+		0x0066, 0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
+static const struct usb_string_descriptor usb_string8 = {
+	/* String 8 "blctrl4-flash" */
+	.bLength = sizeof(struct usb_string_descriptor) + 13 * sizeof(uint16_t),
+	.bDescriptorType = USB_DT_STRING,
+	.wData = {
+		0x0062, 0x006c, 0x0063, 0x0074, 0x0072, 0x006c, 0x0034, 0x002d,
+		0x0066, 0x006c, 0x0061, 0x0073, 0x0068,
+	},
+};
+
+static const struct usb_string_descriptor *usb_strings[] = {
+	&usb_string0, &usb_string1, &usb_string2, &usb_string3,
+	&usb_string4, &usb_string5, &usb_string6, &usb_string7,
+	&usb_string8,
+};
+
+void ep_transfer_send(uint32_t ep, char *data, uint32_t length,
 				void (*complete_cb)(void))
 {
 	struct ep_ctx *ctx = &ep_ctx[ep];
-//	printf("ep_transfer_send(%ld) size=%ld flags=0x%x\n\r", ep, length, ctx->flags);
+//	printf("ep_transfer_send(%ld) size=%ld(%d) flags=0x%x\n\r", ep, length, ctx->maxpktsize, ctx->flags);
 	if (!(ctx->flags & CTX_TRANSFER) || (ctx->flags & (CTX_IN | CTX_OUT)))
 		return;
 
@@ -192,7 +284,7 @@ static void ep_transfer_send(uint32_t ep, char *data, uint32_t length,
 	AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
 }
 
-static void ep_transfer_receive(uint32_t ep, char *data, uint32_t length,
+void ep_transfer_receive(uint32_t ep, char *data, uint32_t length,
 				void (*complete_cb)(void))
 {
 	struct ep_ctx *ctx = &ep_ctx[ep];
@@ -211,7 +303,7 @@ static void ep_transfer_receive(uint32_t ep, char *data, uint32_t length,
 }
 
 /* stalls the endpoint */
-static void ep_send_stall(uint32_t ep)
+void ep_send_stall(uint32_t ep)
 {
 	printf("stall\n\r");
 	AT91C_UDP_CSR[ep] |= AT91C_UDP_FORCESTALL;
@@ -242,6 +334,7 @@ static void udp_configure_ep(const struct usb_endpoint_descriptor *desc)
  */
 static void udp_txcb_setaddress(void)
 {
+	printf("usb address: %d\n\r", current_address);
 	*AT91C_UDP_FADDR = (AT91C_UDP_FEN | current_address);
 	*AT91C_UDP_GLBSTATE = AT91C_UDP_FADDEN;
 }
@@ -252,10 +345,7 @@ static void udp_txcb_setaddress(void)
  */
 static void udp_txcb_setconfig(void)
 {
-	udp_configure_ep(&cfg_descriptor.notify_ep);
+	printf("usb configuration: %d\n\r", current_config);
-	udp_configure_ep(&cfg_descriptor.datain_ep);
-	udp_configure_ep(&cfg_descriptor.dataout_ep);
-
 	/* set UDP to "configured" */
 	*AT91C_UDP_GLBSTATE = AT91C_UDP_CONFG;
 }
@@ -267,8 +357,8 @@ static void udp_txcb_setinterface(void)
 
 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",
+//	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);
+//		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 */
@@ -292,6 +382,24 @@ static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
 					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;
@@ -310,7 +418,6 @@ static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
 		break;
 
 	case (USB_TYPE_STANDARD | USB_RECIP_INTERFACE): /* 0x01/0x81 */
-		// TODO: follow current_interface
 		switch (req->bRequest) {
 		case USB_REQ_SET_INTERFACE: /* 0x0b */
 			current_interface = req->wValue;
@@ -324,20 +431,7 @@ static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
 		break;
 
 	case (USB_TYPE_CLASS | USB_RECIP_INTERFACE): /* 0x21/0xA1 */
-		// TODO: follow current_interface
+		ep0_handle_dfu(req);
-		switch (req->bRequest) {
-		case USB_CDC_REQ_SET_LINE_CODING: /* 0x20 */
-			/* read 7 bytes */
-			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:
@@ -353,6 +447,10 @@ static void udp_handle_ep(uint32_t 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;
@@ -360,6 +458,10 @@ static void udp_handle_ep(uint32_t ep)
 		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;
@@ -370,6 +472,8 @@ static void udp_handle_ep(uint32_t ep)
 		ep_handle_ctrlrequest(&req);
 	}
 
+	void (* transfer_cb)(void) = NULL;
+
 	/* transmit complete? */
 	if (*csr & AT91C_UDP_TXCOMP) {
 		struct ep_ctx *ctx = &ep_ctx[ep];
@@ -395,9 +499,7 @@ static void udp_handle_ep(uint32_t ep)
 			/* transfer complete, execute callback */
 			} else {
 				ctx->flags &= ~CTX_IN;
-
+				transfer_cb = transfer->complete_cb;
-				if (transfer->complete_cb)
-					transfer->complete_cb();
 			}
 		}
 
@@ -405,10 +507,6 @@ static void udp_handle_ep(uint32_t ep)
 		*csr &= ~(AT91C_UDP_TXCOMP);
 	}
 
-	/* clear STALLSENT interrupt */
-	if (*csr & AT91C_UDP_STALLSENT)
-		csr_clear_flags(*csr, (AT91C_UDP_STALLSENT | AT91C_UDP_FORCESTALL));
-
 	/* data ready to read? */
 	if (*csr & (AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1)) {
 		struct ep_ctx *ctx = &ep_ctx[ep];
@@ -438,9 +536,7 @@ static void udp_handle_ep(uint32_t ep)
 			/* test again */
 			if (transfer->length == transfer->curpos) {
 				ctx->flags &= ~CTX_OUT;
-
+				transfer_cb = transfer->complete_cb;
-				if (transfer->complete_cb)
-					transfer->complete_cb();
 			}
 		}
 
@@ -459,12 +555,17 @@ static void udp_handle_ep(uint32_t ep)
 			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) {
+		printf("USB reset\n\r");
+
 		AT91S_UDP *udp = AT91C_BASE_UDP;
 
 		/* reset all endpoints */
@@ -491,11 +592,8 @@ static void udp_isr(void)
 	}
 
 	/* Handle Endpoint Interrupts */
-	uint32_t i;
+	if (isr & AT91C_UDP_EPINT0)
-	for (i = 0; i < 4; i++) {
+		udp_handle_ep(0);
-		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 |
@@ -509,8 +607,6 @@ void at91_udp_init(void)
 	pio->PIO_CODR = UDP_PULLUP;
 	pio->PIO_PER = UDP_PULLUP;
 	pio->PIO_OER = UDP_PULLUP;
-	// TODO: needed?
-	pio->PIO_PPUDR = UDP_VBUS_MON;
 
 	/* UDPCK (48MHz) = PLLCK / 2 */
 	*AT91C_CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
@@ -536,17 +632,6 @@ void at91_udp_init(void)
 	aic->AIC_SVR[AT91C_ID_UDP] = (uint32_t)udp_isr;
 	aic->AIC_IECR = (1 << AT91C_ID_UDP);
 
-	pio_trigger_isr(UDP_VBUS_MON);
+	/* usb connected -> enable pullup */
+	*AT91C_PIOA_CODR = UDP_PULLUP;
 }
-
-static void udp_vbus_monitor(uint32_t status, uint32_t input)
-{
-	if (input & UDP_VBUS_MON)
-		/* usb connected -> enable pullup */
-		*AT91C_PIOA_CODR = UDP_PULLUP;
-	else
-		/* usb got diconnected -> disable pullup */
-		*AT91C_PIOA_SODR = UDP_PULLUP;
-}
-
-PIO_PINCHANGE_ISR(UDP_VBUS_MON, udp_vbus_monitor);

src/dfu.c

@@ -0,0 +1,472 @@
+/***************************************************************************
+ *   Copyright (C) 03/2008 by Olaf Rempel                                  *
+ *   razzor@kopf-tisch.de                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; version 2 of the License                *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#include <stdio.h>
+#include "AT91SAM7S256.h"
+#include "board.h"
+
+#include "at91_twi.h"
+#include "at91_udp.h"
+
+#include "usb_ch9.h"
+#include "usb_dfu.h"
+
+static uint8_t dfu_state = DFU_STATE_dfuIDLE;
+static uint8_t dfu_status = DFU_STATUS_OK;
+
+#define RET_NOTHING	0
+#define RET_STALL	1
+#define RET_ZLP		2
+
+static void handle_getstatus(void)
+{
+	struct dfu_status dstat;
+
+	uint32_t fmr = *AT91C_MC_FSR;
+
+	switch (dfu_state) {
+	case DFU_STATE_dfuDNLOAD_SYNC:
+	case DFU_STATE_dfuDNBUSY:
+		if (fmr & AT91C_MC_PROGE) {
+			dfu_status = DFU_STATUS_errPROG;
+			dfu_state = DFU_STATE_dfuERROR;
+
+		} else if (fmr & AT91C_MC_LOCKE) {
+			dfu_status = DFU_STATUS_errWRITE;
+			dfu_state = DFU_STATE_dfuERROR;
+
+		} else if (fmr & AT91C_MC_FRDY) {
+			dfu_state = DFU_STATE_dfuDNLOAD_IDLE;
+
+		} else {
+			dfu_state = DFU_STATE_dfuDNBUSY;
+		}
+		break;
+	}
+
+	/* send status response */
+	dstat.bStatus = dfu_status;
+	dstat.bState = dfu_state;
+	dstat.iString = 0;
+	/* FIXME: set dstat.bwPollTimeout */
+
+	ep_transfer_send(0, (char *)&dstat, sizeof(dstat), NULL);
+}
+
+static void handle_getstate(void)
+{
+	ep_transfer_send(0, (char *)&dfu_state, sizeof(dfu_state), NULL);
+}
+
+static uint16_t prog_iface;
+static uint16_t prog_block;
+static uint16_t prog_length;
+static uint32_t prog_buf[AT91C_IFLASH_PAGE_SIZE /4];
+
+static void handle_dnload_flash(void)
+{
+	uint32_t *ptr = (uint32_t *)((uint8_t *)0x100000 + (prog_block * 0x100));
+
+	uint32_t i;
+	prog_length += 3;
+	for (i = 0; i < prog_length /4; i++)
+		*ptr++ = prog_buf[i];
+
+	if (!(*AT91C_MC_FSR & AT91C_MC_FRDY)) {
+		printf("flash not ready!\n\r");
+		return;
+	}
+
+	*AT91C_MC_FCR = (AT91C_MC_KEY & (0x5A << 24)) |
+			(((uint32_t)ptr - 0x100004) & AT91C_MC_PAGEN) |
+			AT91C_MC_FCMD_START_PROG;
+
+	ep_transfer_send(0, NULL, 0, NULL);
+}
+
+static void handle_dnload_eeprom(void)
+{
+	/*
+	 * TODO: write buf to onboard eeprom @ address
+	 * inc. address
+	 */
+	ep_transfer_send(0, NULL, 0, NULL);
+}
+
+static void handle_dnload_blctrl(void)
+{
+	uint32_t i2c_dev = TWI_ADDR_BL1 + (prog_iface -2);
+
+	uint8_t buf[66] = { 0x47, 0x11 };
+
+	uint32_t i;
+	for (i = 0; i < prog_length; i += 0x40) {
+
+		uint32_t j;
+		for (j = 0; j < 0x40; j++)
+			buf[j +2] = ((i + j) < prog_length) ? ((uint8_t *)prog_buf)[i + j] : 0xFF;
+
+		struct blmc_cmd cmd = {
+			.cmd = (CMD_WRITE_FLASH << 16) | (prog_block * 0x100 + i),
+			.mode = BLMC_CMD_WRITE | BLMC_CMD_2_ARG,
+			.size = sizeof(buf),
+			.data = buf,
+		};
+
+		printf("cmd:0x%08lx\n\r", cmd.cmd);
+
+		if (twi_cmd(i2c_dev, &cmd) != 0) {
+			printf("flash write failed\n\r");
+			dfu_state = DFU_STATE_dfuERROR;
+			dfu_status = DFU_STATUS_errUNKNOWN;
+		}
+	}
+
+	ep_transfer_send(0, NULL, 0, NULL);
+}
+
+static uint32_t handle_dnload(uint16_t iface, uint16_t block, uint16_t length)
+{
+	printf("down:%x-%x-%x\n\r", iface, block, length);
+
+	if (length == 0) {
+		dfu_state = DFU_STATE_dfuMANIFEST_SYNC;
+		return RET_ZLP;
+	}
+
+	prog_iface = iface;
+	prog_block = block;
+	prog_length = length;
+
+	void *callback = NULL;
+	switch (iface) {
+	case 0:	/* internal flash */
+		if (block >= 0x3C0) {
+			dfu_state = DFU_STATE_dfuERROR;
+			dfu_status = DFU_STATUS_errADDRESS;
+			return RET_STALL;
+		}
+
+		callback = handle_dnload_flash;
+		break;
+
+	case 1:	/* onboard i2c-eeprom */
+		callback = handle_dnload_eeprom;
+		break;
+
+	case 2:
+	case 3:
+	case 4:
+	case 5:	/* blctrl 1-4 */
+		callback = handle_dnload_blctrl;
+		break;
+	}
+
+	ep_transfer_receive(0, (char *)&prog_buf, length, callback);
+	return RET_NOTHING;
+}
+
+static void handle_upload(uint16_t iface, uint16_t block, uint16_t length)
+{
+	printf("up:%x-%x-%x\n\r", iface, block, length);
+
+	char *ptr = (char *)&prog_buf;
+
+	switch (iface) {
+	case 0:	/* internal flash */
+		if (block >= 0x400) {
+			length = 0;
+			break;
+		}
+		ptr = (char *)0x100000 + (block * 0x100);
+		break;
+
+	case 1:	/* onboard i2c-eeprom */
+		if (block >= 0x80) {
+			length = 0;
+			break;
+		}
+
+		/* TODO */
+
+		break;
+
+	case 2:
+	case 3:
+	case 4:
+	case 5:	/* blctrl 1-4 */
+		if (block >= 0x20) {
+			length = 0;
+			dfu_state = DFU_STATE_dfuIDLE;
+			break;
+		}
+
+		uint32_t i2c_dev = TWI_ADDR_BL1 + (iface -2);
+
+		struct blmc_cmd cmd = {
+			.cmd = (CMD_READ_FLASH << 16) | (block * 0x100),
+			.mode = BLMC_CMD_READ | BLMC_CMD_2_ARG,
+			.size = length,
+			.data = (uint8_t *)&prog_buf,
+		};
+
+		if (twi_cmd(i2c_dev, &cmd) != 0) {
+			length = 0;
+			dfu_state = DFU_STATE_dfuERROR;
+			dfu_status = DFU_STATUS_errUNKNOWN;
+		}
+		break;
+	}
+
+	ep_transfer_send(0, ptr, length, NULL);
+}
+
+void ep0_handle_dfu(struct usb_ctrlrequest *req)
+{
+	uint32_t ret = RET_NOTHING;
+
+	switch (dfu_state) {
+	case DFU_STATE_appIDLE:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_DETACH:
+			dfu_state = DFU_STATE_appDETACH;
+			ret = RET_ZLP;
+			break;
+
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_appDETACH:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_appIDLE;
+			ret = RET_STALL;
+			break;
+		}
+		/* FIXME: implement timer to return to appIDLE */
+		break;
+
+	case DFU_STATE_dfuIDLE:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_DNLOAD:
+			if (req->wLength == 0) {
+				dfu_state = DFU_STATE_dfuERROR;
+				ret = RET_STALL;
+				break;
+			}
+			dfu_state = DFU_STATE_dfuDNLOAD_SYNC;
+			ret = handle_dnload(req->wIndex, req->wValue, req->wLength);
+			break;
+
+		case USB_REQ_DFU_UPLOAD:
+			dfu_state = DFU_STATE_dfuUPLOAD_IDLE;
+			handle_upload(req->wIndex, req->wValue, req->wLength);
+			break;
+
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		case USB_REQ_DFU_ABORT:
+			/* no zlp? */
+			ret = RET_ZLP;
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuDNLOAD_SYNC:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			/* FIXME: state transition depending on block completeness */
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuDNBUSY:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			/* FIXME: only accept getstatus if bwPollTimeout
+			 * has elapsed */
+			handle_getstatus();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuDNLOAD_IDLE:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_DNLOAD:
+			dfu_state = DFU_STATE_dfuDNLOAD_SYNC;
+			ret = handle_dnload(req->wIndex, req->wValue, req->wLength);
+			break;
+
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		case USB_REQ_DFU_ABORT:
+			dfu_state = DFU_STATE_dfuIDLE;
+			ret = RET_ZLP;
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuMANIFEST_SYNC:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			dfu_state = DFU_STATE_dfuIDLE;
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuMANIFEST:
+		dfu_state = DFU_STATE_dfuERROR;
+		ret = RET_STALL;
+		break;
+
+	case DFU_STATE_dfuMANIFEST_WAIT_RST:
+		/* we should never go here */
+		break;
+
+	case DFU_STATE_dfuUPLOAD_IDLE:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_UPLOAD:
+			/* state transition if less data then requested */
+			handle_upload(req->wIndex, req->wValue, req->wLength);
+			break;
+
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		case USB_REQ_DFU_ABORT:
+			dfu_state = DFU_STATE_dfuIDLE;
+			/* no zlp? */
+			ret = RET_ZLP;
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+
+	case DFU_STATE_dfuERROR:
+		switch (req->bRequest) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+
+		case USB_REQ_DFU_CLRSTATUS:
+			dfu_state = DFU_STATE_dfuIDLE;
+			dfu_status = DFU_STATUS_OK;
+			/* no zlp? */
+			ret = RET_ZLP;
+			break;
+
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			ret = RET_STALL;
+			break;
+		}
+		break;
+	}
+
+	switch (ret) {
+	case RET_NOTHING:
+		break;
+
+	case RET_ZLP:
+		ep_transfer_send(0, NULL, 0, NULL);
+		break;
+
+	case RET_STALL:
+		ep_send_stall(0);
+		break;
+	}
+}

src/telemetrie.c

@@ -1,255 +0,0 @@
-/***************************************************************************
- *   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 "memalloc.h"
-#include "fifo.h"
-
-/* 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;
-
-	return fifo_put(routing_table[addr]->txfifo, (char *)head, head->size);
-}
-
-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;
-		reply->flags = value->flags;
-		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_getvars_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)
-{
-	struct tdc_value *value = &_tdc_value_table + id;
-	if (value >= &_tdc_value_table_end)
-		return -1;
-
-	uint32_t datalen = value->flags & TDC_SIZEMASK;
-
-	// TODO: atomic?
-	memcpy(value->data, data, datalen);
-
-	return 1;
-}
-
-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 tmp = tdc_varmap[i];
-
-		for (j = 0; j < 32; j++) {
-			if (!tmp)
-				break;
-
-			if (tmp & 0x01) {
-				if (tdc_get_value(i * 32 + j) <= 0)
-					return PITC_REMOVE_TIMER;
-			}
-
-			tmp >>= 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));
-
-	if (interval > 0) {
-		tdc_timer.interval = interval;
-		// TODO: timer already running
-		pitc_schedule_timer(&tdc_timer);
-
-	} else {
-		// TODO: timer stop
-	}
-	return 1;
-}
-
-static const struct tdc_hello_reply hello_reply = {
-	.cmd = TDC_REPLY | TDC_ADDR1 | TDC_HELLO,
-	.size = sizeof(struct tdc_hello_reply),
-	.name = "sam7fc-v0.01",
-};
-
-/*
- * returns:
- * -1: on routing error
- *  0: no space left in txfifo (caller should retry)
- * >0: success
- */
-static int32_t tdc_parse_packet(struct tdc_pkt_header *head)
-{
-	/* all replys go to the HOST */
-	if (head->cmd & TDC_REPLY)
-		return tdc_transmit(TDC_ADDR0, head);
-
-	/* forward this packet? */
-	if ((head->cmd & TDC_ADDRMASK) != TDC_ADDR1) {
-		uint32_t addr = (head->cmd & TDC_ADDRMASK) >> 4;
-		return tdc_transmit(addr, head);
-	}
-
-	int32_t ret = -1;
-
-	/* parse the packet */
-	switch (head->cmd & TDC_OPCODEMASK) {
-	case TDC_HELLO:
-		/* answer the hello */
-		ret = tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
-		break;
-
-	case TDC_GETVARS:
-		ret = tdc_get_vars();
-		break;
-
-	case TDC_GETVALUE: {
-		struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)head;
-		ret = tdc_get_value(pkt->id);
-	}	break;
-
-	case TDC_SETVALUE: {
-		struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)head;
-		ret = tdc_set_value(pkt->id, pkt->data);
-	}	break;
-
-	case TDC_REQVALUES: {
-		struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)head;
-		ret = tdc_setup_timer(pkt->interval, pkt->varmap);
-	}	break;
-	};
-
-	/*
-	 * on succes(>0) return size of request,
-	 * and push retry(0) and routing error(-1) up
-	 */
-	return (ret > 0) ? head->size : ret;
-}
-
-void tdc_register_device(uint32_t addr, struct comm_device *device)
-{
-	if (addr < ARRAY_SIZE(routing_table))
-		routing_table[addr] = device;
-}
-
-void tdc_receive(struct comm_device *device)
-{
-	while (1) {
-		/* peek the header */
-		struct tdc_pkt_header tmp_head;
-		uint32_t len = fifo_peek(device->rxfifo, (char *)&tmp_head, sizeof(tmp_head));
-		if (len != sizeof(tmp_head))
-			return;
-
-		/* peek the whole packet */
-		struct tdc_pkt_header *head = alloc(tmp_head.size);
-		len = fifo_peek(device->rxfifo, (char *)head, tmp_head.size);
-		if (len != tmp_head.size)
-			return;
-
-		/* if it's a hello-request, remember the device as path to the host */
-		if ((head->cmd & (TDC_OPCODEMASK & TDC_DIR)) == TDC_HELLO)
-			tdc_register_device(TDC_ADDR0, device);
-
-		/* parse packet, remove data if no restart is needed */
-		int32_t ret = tdc_parse_packet(head);
-		free(head);
-
-		/* some tx-fifo was full(0), return to caller and let it retry */
-		if (!ret)
-			return;
-
-		/* success(>0) or routing error(-1) -> remove the packet */
-		fifo_remove(device->rxfifo, tmp_head.size);
-	}
-}