diff --git a/include/fifo.h b/include/fifo.h
index 8baf0fa..7d2ac3e 100644
--- a/include/fifo.h
+++ b/include/fifo.h
@@ -14,17 +14,18 @@ struct fifo {
 	char buf[0];
 };
 
-uint32_t fifo_put(struct fifo *fifo, const char *buf, uint32_t len);
+uint32_t fifo_put(struct fifo *fifo, char *buf, uint32_t len);
 uint32_t fifo_get(struct fifo *fifo, char *buf, uint32_t len);
 
+uint32_t fifo_peek(struct fifo *fifo, char *buf, uint32_t len);
+uint32_t fifo_remove(struct fifo *fifo, uint32_t len);
+
 uint32_t fifo_rxpdc(struct fifo *fifo, AT91S_PDC *pdc, uint16_t maxsize);
 uint32_t fifo_txpdc(struct fifo *fifo, AT91S_PDC *pdc, uint16_t maxsize);
 
 uint32_t fifo_putbyte(struct fifo *fifo, char c);
 uint32_t fifo_getbyte(struct fifo *fifo, char *p);
 
-char * fifo_peek(struct fifo *fifo, uint32_t len);
-
 struct fifo * fifo_alloc(uint32_t size);
 
 #endif /*FIFO_H_*/
diff --git a/include/telemetrie.h b/include/telemetrie.h
index a72a19e..6e233c1 100644
--- a/include/telemetrie.h
+++ b/include/telemetrie.h
@@ -86,6 +86,24 @@ struct tdc_reqvalues_reply {
 } __attribute__ ((packed));
 
 
+
+struct comm_device {
+	struct fifo *rxfifo;
+	struct fifo *txfifo;
+};
+
+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);
+
+
+
+
+
+
+
 struct tdc_value {
 	void *data;
 	const char *name;
@@ -114,5 +132,4 @@ struct tdc_value {
 #define TDC_FLOAT(var, desc)	TDC_VALUE(var, desc, float, TDC_FP)
 #define TDC_DOUBLE(var, desc)	TDC_VALUE(var, desc, double, TDC_FP)
 
-
 #endif /*TELEMETRIE_H_*/
diff --git a/src/at91_dbgu.c b/src/at91_dbgu.c
index 8bc424f..739c53c 100644
--- a/src/at91_dbgu.c
+++ b/src/at91_dbgu.c
@@ -76,14 +76,14 @@ void at91_dbgu_putc(char c)
 
 void at91_dbgu_puts(const char *p)
 {
-	fifo_put(txfifo, p, strlen(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, buf, len);
+	int retval = fifo_put(txfifo, (char *)buf, len);
 //	*AT91C_DBGU_IER = AT91C_US_TXEMPTY;
 	*AT91C_DBGU_IER = AT91C_US_TXBUFE;
 	return retval;
diff --git a/src/fifo.c b/src/fifo.c
index a17584b..e343af9 100644
--- a/src/fifo.c
+++ b/src/fifo.c
@@ -43,12 +43,11 @@ static uint32_t fifo_used(struct fifo *fifo)
  * append data to fifo
  * returns number of bytes copied
  */
-uint32_t fifo_put(struct fifo *fifo, const char *buf, uint32_t len)
+uint32_t fifo_put(struct fifo *fifo, char *buf, uint32_t len)
 {
 	uint32_t left = fifo->size - fifo_used(fifo);
-	// TODO: check semantic
 	if (len > left)
-		len = left;
+		return 0;
 
 	uint32_t count = len;
 	while (count--)
@@ -75,16 +74,31 @@ uint32_t fifo_get(struct fifo *fifo, char *buf, uint32_t len)
 }
 
 /*
- * returns a pointer to the data in the fifo
- * (without changing internal state)
+ * get data from fifo, without changing the internal state
  */
-char * fifo_peek(struct fifo *fifo, uint32_t len)
+uint32_t fifo_peek(struct fifo *fifo, char *buf, uint32_t len)
 {
 	uint32_t used = fifo_used(fifo);
 	if (len > used)
-		return NULL;
+		len = used;
 
-	return fifo->buf + (fifo->out & FIFO_MASK(fifo));
+	uint32_t count = len;
+	uint32_t out = fifo->out;
+	while (count--)
+		*buf++ = fifo->buf[out++ & FIFO_MASK(fifo)];
+
+	return len;
+}
+
+/* removes data without reading it (eg. after a peek) */
+uint32_t fifo_remove(struct fifo *fifo, uint32_t len)
+{
+	uint32_t used = fifo_used(fifo);
+	if (len > used)
+		len = used;
+
+	fifo->out += len;
+	return len;
 }
 
 /*
diff --git a/src/telemetrie.c b/src/telemetrie.c
index 4860673..cf784e7 100644
--- a/src/telemetrie.c
+++ b/src/telemetrie.c
@@ -20,62 +20,40 @@
 #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];
+
+static struct comm_device *routing_table[8];
+
 /*
- * transmit function
  * returns:
- * -1 : could not sent data (e.g. fifo full)
- * >= 0 : bytes sent
+ * -1: on routing error
+ *  0: no space left in txfifo (caller should retry)
+ * >0: success
  */
-typedef uint32_t (txfunc_t)(struct tdc_pkt_header *head);
-
-/* just eat the packet */
-static uint32_t txdummy(struct tdc_pkt_header *head)
-{
-	return head->size;
-}
-
-/* table of txfuncs for routing/forwarding */
-static txfunc_t *transmit_table[8] = {
-	&txdummy, &txdummy, &txdummy, &txdummy,
-	&txdummy, &txdummy, &txdummy, &txdummy,
-};
-
-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",
-};
-
-void tdc_register_txfunc(uint32_t addr, txfunc_t *txfunc)
-{
-	if (addr <= ARRAY_SIZE(transmit_table))
-		transmit_table[addr] = txfunc;
-}
-
 int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head)
 {
-	if (addr <= ARRAY_SIZE(transmit_table))
-		return transmit_table[addr](head);
+	if (addr >= ARRAY_SIZE(routing_table) || !routing_table[addr])
+		return -1;
 
-	return -1;
+	return fifo_put(routing_table[addr]->txfifo, (char *)head, head->size);
 }
 
-/*
- * send all variable names & flags
- * returns:
- * 0 - success (*id = 0)
- * -1 - not all data was sent, restart request later (*id has restart-point)
- */
-int32_t tdc_get_vars(uint32_t *id)
+static int32_t tdc_get_vars(void)
 {
-	struct tdc_value *value = &_tdc_value_table + *id;
+	/* 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);
@@ -83,27 +61,32 @@ int32_t tdc_get_vars(uint32_t *id)
 		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->id = id;
 		reply->flags = value->flags;
 		memcpy(reply->name, value->name, datalen);
 
-		uint32_t txerror = (tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply)) != reply->size);
+		uint32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
 		free(reply);
 
-		/* transmit fifo is full, remember position (*id) and restart */
-		if (txerror)
-			return -1;
+		/* push routing error(-1) and retry(0) */
+		if (ret <= 0)
+			return ret;
 
-		(*id)++;
+		id++;
 		value++;
 	}
-	*id = 0;
-	return 0;
+
+	/* dump complete, reset restart point */
+	id = 0;
+	return 1;
 }
 
-int32_t tdc_get_value(uint32_t id)
+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);
@@ -112,96 +95,160 @@ int32_t tdc_get_value(uint32_t id)
 	reply->id = id;
 	memcpy(reply->data, value->data, datalen);
 
-	int32_t txerror = (tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply)) != reply->size);
+	int32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
 	free(reply);
 
-	return (txerror) ? -1 : 0;
+	return ret;
 }
 
-void tdc_set_value(uint32_t id, uint8_t *data)
+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;
 }
 
-/*
- * parses tdc data
- * returns:
- * -1 : data too short / error
- * >= 0 : data read, come back if there is more
- */
-int32_t tdc_parse_pkt(txfunc_t *txfunc, const uint8_t *data, uint32_t size)
+static uint32_t tdc_timer_cb(struct pitc_timer *timer)
 {
-	// TODO: use a fifo, with a peek function?
+	uint32_t i, j;
+	for (i = 0; i < ARRAY_SIZE(tdc_varmap); i++) {
+		uint32_t tmp = tdc_varmap[i];
 
-	struct tdc_pkt_header *head = (struct tdc_pkt_header *)data;
+		for (j = 0; j < 32; j++) {
+			if (!tmp)
+				break;
 
-	if (size < sizeof(struct tdc_pkt_header))
-		return -1;
+			if (tmp & 0x01) {
+				if (tdc_get_value(i * 32 + j) <= 0)
+					return PITC_REMOVE_TIMER;
+			}
 
-	/* if it's a hello-request, remember the txfunc as path to the host */
-	if ((head->cmd & (TDC_OPCODEMASK & TDC_DIR)) == TDC_HELLO)
-		tdc_register_txfunc(TDC_ADDR0, txfunc);
-
-	/* all replys go to the HOST */
-	if (head->cmd & TDC_REPLY) {
-		// TODO: wrong returncode
-		return tdc_transmit(TDC_ADDR0, head);
+			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;
-		// TODO: wrong returncode
 		return tdc_transmit(addr, head);
 	}
 
+	int32_t ret = -1;
+
 	/* parse the packet */
 	switch (head->cmd & TDC_OPCODEMASK) {
-	/* HELLO from HOST */
 	case TDC_HELLO:
-		tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
+		/* answer the hello */
+		ret = tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
 		break;
 
-	case TDC_GETVARS: {
-		static uint32_t i;
-
-		/* tx-fifo is full, complete request later */
-		if (tdc_get_vars(&i) == -1)
-			return 0;
-
-	}	break;
+	case TDC_GETVARS:
+		ret = tdc_get_vars();
+		break;
 
 	case TDC_GETVALUE: {
-		struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)data;
-
-		/* tx-fifo is full, retry request later */
-		if (tdc_get_value(pkt->id) == -1)
-			return 0;
-
+		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 *)data;
-		tdc_set_value(pkt->id, pkt->data);
+		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 *)data;
-
+		struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)head;
+		ret = tdc_setup_timer(pkt->interval, pkt->varmap);
 	}	break;
-
-	case TDC_TERMINAL:
-		// TODO: not possible?
-		break;
-
-	case TDC_USERDATA:
-		// TODO: currently not used
-		break;
 	};
 
-	return head->size;
+	/*
+	 * 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);
+	}
 }