move to new directory

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

31 files changed, 3058 insertions, 0 deletions

diff --git a/firmware/src/os/dbgu.c b/firmware/src/os/dbgu.c
new file mode 100644
index 0000000..dc1a040
--- /dev/null
+++ b/firmware/src/os/dbgu.c
@@ -0,0 +1,303 @@
+/*----------------------------------------------------------------------------
+ *         ATMEL Microcontroller Software Support  -  ROUSSET  -
+ *----------------------------------------------------------------------------
+ * The software is delivered "AS IS" without warranty or condition of any
+ * kind, either express, implied or statutory. This includes without
+ * limitation any warranty or condition with respect to merchantability or
+ * fitness for any particular purpose, or against the infringements of
+ * intellectual property rights of others.
+ *----------------------------------------------------------------------------
+ * File Name           : Debug.c
+ * Object              : Debug menu
+ * Creation            : JPP   14/Sep/2004
+ * 1.1 29/Aug/05 JPP   : Update AIC definion
+ *----------------------------------------------------------------------------*/
+
+// Include Standard files
+#include <board.h>
+#include <os/dbgu.h>
+#include "../openpcd.h"
+#include <os/led.h>
+#include <os/main.h>
+#include <asm/system.h>
+
+#define USART_SYS_LEVEL 4
+/*---------------------------- Global Variable ------------------------------*/
+//*--------------------------1--------------------------------------------------
+//* \fn    AT91F_DBGU_Printk
+//* \brief This function is used to send a string through the DBGU channel
+//*----------------------------------------------------------------------------
+void AT91F_DBGU_Ready(void)
+{
+	while (!(AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXEMPTY)) ;
+}
+
+//*----------------------------------------------------------------------------
+//* Function Name       : Send_reset
+//* Object              : Acknoledeg AIC and send reset
+//*----------------------------------------------------------------------------
+static void Send_reset(void)
+{
+	void (*pfct) (void) = (void (*)(void))0x00000000;
+
+	// Acknoledge the interrupt
+	// Mark the End of Interrupt on the AIC
+	AT91C_BASE_AIC->AIC_EOICR = 0;
+	AT91F_DBGU_Ready();
+	// Jump in reset
+	pfct();
+}
+
+//*----------------------------------------------------------------------------
+//* Function Name       : DBGU_irq_handler
+//* Object              : C handler interrupt function called by the interrupts
+//*                       assembling routine
+//*----------------------------------------------------------------------------
+static void DBGU_irq_handler(void)
+{
+	static char value;
+
+	AT91F_DBGU_Get(&value);
+	switch (value) {
+	case '0':		//* info
+		AT91F_DBGU_Frame("Clear Pull up\n\r");
+		// Set
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+		break;
+	case '1':		//* info
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+		AT91F_DBGU_Printk("Set Pull up\n\r");
+		// Reset Application
+		Send_reset();
+		break;
+	case '2':
+		AT91F_DBGU_Printk("Toggling LED 1\n\r");
+		led_toggle(1);
+		break;
+	case '3':
+		AT91F_DBGU_Printk("Toggling LED 2\n\r");
+		led_toggle(2);
+		break;
+	default:
+		if (_main_dbgu(value) < 0)
+			AT91F_DBGU_Printk("\n\r");
+		break;
+	}			// end switch
+}
+
+void dbgu_rb_init(void);
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_DBGU_Init
+//* \brief This function is used to send a string through the DBGU channel (Very low level debugging)
+//*----------------------------------------------------------------------------
+void AT91F_DBGU_Init(void)
+{
+	dbgu_rb_init();
+
+	//* Open PIO for DBGU
+	AT91F_DBGU_CfgPIO();
+	//* Enable Transmitter & receivier
+	((AT91PS_USART) AT91C_BASE_DBGU)->US_CR =
+	    AT91C_US_RSTTX | AT91C_US_RSTRX;
+
+	//* Configure DBGU
+	AT91F_US_Configure((AT91PS_USART) AT91C_BASE_DBGU,	// DBGU base address
+			   MCK, AT91C_US_ASYNC_MODE,	// Mode Register to be programmed
+			   AT91C_DBGU_BAUD,	// Baudrate to be programmed
+			   0);	// Timeguard to be programmed
+
+	//* Enable Transmitter & receivier
+	((AT91PS_USART) AT91C_BASE_DBGU)->US_CR = AT91C_US_RXEN | AT91C_US_TXEN;
+
+	//* Enable USART IT error and AT91C_US_ENDRX
+	AT91F_US_EnableIt((AT91PS_USART) AT91C_BASE_DBGU, AT91C_US_RXRDY);
+
+	//* open interrupt
+	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_SYS, USART_SYS_LEVEL,
+			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL,
+			      DBGU_irq_handler);
+	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_SYS);
+
+	AT91F_DBGU_Printk
+	    ("\n\r-I- OpenPCD test mode\n\r 0) Set Pull-up 1) Clear Pull-up "
+	     "2) Toggle LED1 3) Toggle LED2 4) Test RC632\n\r"
+	     "5) Read RxWait 6) Write RxWait 7) Dump RC632 Regs\n\r");
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_DBGU_Printk
+//* \brief This function is used to send a string through the DBGU channel (Very low level debugging)
+//*----------------------------------------------------------------------------
+void AT91F_DBGU_Printk(char *buffer)
+{
+	while (*buffer != '\0') {
+		while (!AT91F_US_TxReady((AT91PS_USART) AT91C_BASE_DBGU)) ;
+		AT91F_US_PutChar((AT91PS_USART) AT91C_BASE_DBGU, *buffer++);
+	}
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_DBGU_Frame
+//* \brief This function is used to send a string through the DBGU channel
+//*----------------------------------------------------------------------------
+void AT91F_DBGU_Frame(char *buffer)
+{
+	unsigned char len;
+
+	for (len = 0; buffer[len] != '\0'; len++) {
+	}
+
+	AT91F_US_SendFrame((AT91PS_USART) AT91C_BASE_DBGU, 
+			   (unsigned char *)buffer, len, 0, 0);
+
+}
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_US_Get
+//* \brief Get a Char to USART
+//*----------------------------------------------------------------------------
+int AT91F_DBGU_Get(char *val)
+{
+	if ((AT91F_US_RxReady((AT91PS_USART) AT91C_BASE_DBGU)) == 0)
+		return (0);
+	else {
+		*val = AT91F_US_GetChar((AT91PS_USART) AT91C_BASE_DBGU);
+		return (-1);
+	}
+}
+
+// mthomas: function not used in this application. avoid
+//  linking huge newlib code for sscanf.
+
+#ifdef DEBUG
+#include <stdio.h>
+#include <stdarg.h>
+#include <string.h>
+const char *
+hexdump(const void *data, unsigned int len)
+{
+	static char string[256];
+	unsigned char *d = (unsigned char *) data;
+	unsigned int i, left;
+
+	string[0] = '\0';
+	left = sizeof(string);
+	for (i = 0; len--; i += 3) {
+		if (i >= sizeof(string) -4)
+			break;
+		snprintf(string+i, 4, " %02x", *d++);
+	}
+	return string;
+}
+
+struct dbgu {
+	char buf[4096];
+	char *next_inbyte;
+	char *next_outbyte;
+};
+static struct dbgu dbgu;
+
+void dbgu_rb_init(void)
+{
+	memset(dbgu.buf, 0, sizeof(dbgu.buf));
+	dbgu.next_inbyte = &dbgu.buf[0];
+	dbgu.next_outbyte = &dbgu.buf[0];
+}
+
+/* pull one char out of debug ring buffer */
+static int dbgu_rb_pull(char *ret)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+
+	if (dbgu.next_outbyte == dbgu.next_inbyte) {
+		local_irq_restore(flags);
+		return -1;
+	}
+
+	*ret = *dbgu.next_outbyte;
+
+	dbgu.next_outbyte++;
+	if (dbgu.next_outbyte == &dbgu.buf[0]+sizeof(dbgu.buf)) {
+		//AT91F_DBGU_Printk("WRAP DURING PULL\r\n");
+		dbgu.next_outbyte = &dbgu.buf[0];
+	} else if (dbgu.next_outbyte > &dbgu.buf[0]+sizeof(dbgu.buf)) {
+		//AT91F_DBGU_Printk("OUTBYTE > END_OF_BUF!!\r\n");
+		dbgu.next_outbyte -= sizeof(dbgu.buf);
+	}
+
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static void __rb_flush(void)
+{
+	char ch;
+	while (dbgu_rb_pull(&ch) >= 0) {
+		while (!AT91F_US_TxReady((AT91PS_USART) AT91C_BASE_DBGU)) ;
+		AT91F_US_PutChar((AT91PS_USART) AT91C_BASE_DBGU, ch);
+	}
+}
+
+/* flush pending data from debug ring buffer to serial port */
+void dbgu_rb_flush(void)
+{
+	__rb_flush();
+}
+
+static void __dbgu_rb_append(char *data, int len)
+{
+	char *pos = dbgu.next_inbyte;
+
+	dbgu.next_inbyte += len;
+	if (dbgu.next_inbyte >= &dbgu.buf[0]+sizeof(dbgu.buf)) {
+		AT91F_DBGU_Printk("WRAP DURING APPEND\r\n");
+		dbgu.next_inbyte -= sizeof(dbgu.buf);
+	}
+
+	memcpy(pos, data, len);
+}
+
+void dbgu_rb_append(char *data, int len)
+{
+	unsigned long flags;
+	int bytes_left;
+	char *data_cur;
+	
+	local_irq_save(flags);
+
+	bytes_left = &dbgu.buf[0]+sizeof(dbgu.buf)-dbgu.next_inbyte;
+	data_cur = data;
+
+	if (len > bytes_left) {
+		AT91F_DBGU_Printk("LEN > BYTES_LEFT\r\n");
+		__rb_flush();
+		__dbgu_rb_append(data_cur, bytes_left);
+		len -= bytes_left;
+		data_cur += bytes_left;
+	}
+	__dbgu_rb_append(data_cur, len);
+
+	local_irq_restore(flags);
+}
+
+static char dbg_buf[256];
+void debugp(const char *format, ...)
+{
+	va_list ap;
+
+	va_start(ap, format);
+	vsnprintf(dbg_buf, sizeof(dbg_buf)-1, format, ap);
+	va_end(ap);
+
+	dbg_buf[sizeof(dbg_buf)-1] = '\0';			
+	//AT91F_DBGU_Frame(dbg_buf);
+	//AT91F_DBGU_Printk(dbg_buf);
+	dbgu_rb_append(dbg_buf, strlen(dbg_buf));
+}
+#else
+void dbgu_rb_flush(void) {}
+void dbgu_rb_init(void) {}
+#endif
diff --git a/firmware/src/os/dbgu.h b/firmware/src/os/dbgu.h
new file mode 100644
index 0000000..e29e351
--- /dev/null
+++ b/firmware/src/os/dbgu.h
@@ -0,0 +1,44 @@
+//*----------------------------------------------------------------------------

+//*         ATMEL Microcontroller Software Support  -  ROUSSET  -

+//*----------------------------------------------------------------------------

+//* The software is delivered "AS IS" without warranty or condition of any

+//* kind, either express, implied or statutory. This includes without

+//* limitation any warranty or condition with respect to merchantability or

+//* fitness for any particular purpose, or against the infringements of

+//* intellectual property rights of others.

+//*----------------------------------------------------------------------------

+//* File Name           : Debug.h

+//* Object              : Debug menu

+//* Creation            : JPP   02/Sep/2004

+//*----------------------------------------------------------------------------

+

+#ifndef dbgu_h

+#define dbgu_h

+

+#define AT91C_DBGU_BAUD 115200

+

+//#define DEBUGP(x)	AT91F_DBGU_Printk(x)

+

+//* ----------------------- External Function Prototype -----------------------

+

+extern const char *hexdump(const void *data, unsigned int len);

+void AT91F_DBGU_Init(void);

+void AT91F_DBGU_Printk(	char *buffer);

+void AT91F_DBGU_Frame(	char *buffer);

+int AT91F_DBGU_Get( char *val);

+void dbgu_rb_flush(void);

+#ifndef __WinARM__

+void AT91F_DBGU_scanf(char * type,unsigned int * val);

+#endif

+

+#ifdef DEBUG

+extern void debugp(const char *format, ...);

+#define DEBUGP(x, args ...) debugp(x, ## args)

+#else

+#define	DEBUGP(x, args ...) do {} while(0)

+#endif

+

+#define DEBUGPCR(x, args ...) DEBUGP(x "\r\n", ## args)

+#define DEBUGPCRF(x, args ...) DEBUGPCR("%s(%d): " x, __FUNCTION__, __LINE__, ## args)

+

+#endif /* dbgu_h */

diff --git a/firmware/src/os/dfu.c b/firmware/src/os/dfu.c
new file mode 100644
index 0000000..f2bc0ca
--- /dev/null
+++ b/firmware/src/os/dfu.c
@@ -0,0 +1,681 @@
+/* USB Device Firmware Update Implementation for OpenPCD
+ * (C) 2006 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ * This ought to be compliant to the USB DFU Spec 1.0 as available from
+ * http://www.usb.org/developers/devclass_docs/usbdfu10.pdf
+ *
+ */
+
+#include <errno.h>
+#include <usb_ch9.h>
+#include <usb_dfu.h>
+#include <lib_AT91SAM7.h>
+
+#include <os/dfu.h>
+#include <os/pcd_enumerate.h>
+#include <os/req_ctx.h>
+#include "../openpcd.h"
+
+/* If debug is enabled, we need to access debug functions from flash
+ * and therefore have to omit flashing */
+//#define DEBUG_DFU
+
+#ifdef DEBUG_DFU
+#define DEBUGE DEBUGP
+#define DEBUGI DEBUGP
+#else
+#define DEBUGE(x, args ...) do { } while (0)
+#define DEBUGI(x, args ...) do { } while (0)
+#endif
+
+
+void __dfufunc udp_init(void)
+{
+	/* Set the PLL USB Divider */
+	AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
+
+	/* Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock */
+	AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_UDP;
+	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_UDP);
+
+	/* Enable UDP PullUp (USB_DP_PUP) : enable & Clear of the corresponding PIO
+	 * Set in PIO mode and Configure in Output */
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+}
+
+/* Send Data through the control endpoint */
+static void __dfufunc udp_ep0_send_data(const char *pData, u_int32_t length)
+{
+	AT91PS_UDP pUdp = AT91C_BASE_UDP;
+	u_int32_t cpt = 0;
+	AT91_REG csr;
+
+	DEBUGE("send_data: %u bytes ", length);
+
+	do {
+		cpt = MIN(length, 8);
+		length -= cpt;
+
+		while (cpt--)
+			pUdp->UDP_FDR[0] = *pData++;
+
+		if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
+			pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+			while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+		}
+
+		pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
+		do {
+			csr = pUdp->UDP_CSR[0];
+
+			/* Data IN stage has been stopped by a status OUT */
+			if (csr & AT91C_UDP_RX_DATA_BK0) {
+				pUdp->UDP_CSR[0] &= ~(AT91C_UDP_RX_DATA_BK0);
+				DEBUGE("stopped by status out ");
+				return;
+			}
+		} while (!(csr & AT91C_UDP_TXCOMP));
+
+	} while (length);
+
+	if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
+		pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+		while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+	}
+}
+
+/* Send zero length packet through the control endpoint */
+static void __dfufunc udp_ep0_send_zlp(void)
+{
+	AT91PS_UDP pUdp = AT91C_BASE_UDP;
+	pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
+	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP)) ;
+	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
+	while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) ;
+}
+
+/* Stall the control endpoint */
+static void __dfufunc udp_ep0_send_stall(void)
+{
+	AT91PS_UDP pUdp = AT91C_BASE_UDP;
+	pUdp->UDP_CSR[0] |= AT91C_UDP_FORCESTALL;
+	while (!(pUdp->UDP_CSR[0] & AT91C_UDP_ISOERROR)) ;
+	pUdp->UDP_CSR[0] &= ~(AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR);
+	while (pUdp->UDP_CSR[0] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR)) ;
+}
+
+
+static u_int8_t status;
+static u_int8_t *ptr;
+static u_int8_t dfu_state;
+
+static int __dfufunc handle_dnload(u_int16_t val, u_int16_t len)
+{
+	volatile u_int32_t *p = (volatile u_int32_t *)ptr;
+
+	DEBUGE("download ");
+
+	if (len > AT91C_IFLASH_PAGE_SIZE) {
+		/* Too big */
+	    	dfu_state = DFU_STATE_dfuERROR;
+		status = DFU_STATUS_errADDRESS;
+		udp_ep0_send_stall();
+		return -EINVAL;
+	}
+	if (len & 0x3) {
+		dfu_state = DFU_STATE_dfuERROR;
+		status = DFU_STATUS_errADDRESS;
+		udp_ep0_send_stall();
+		return -EINVAL;
+	}
+	if (len == 0) {
+		dfu_state = DFU_STATE_dfuMANIFEST_SYNC;
+		return 0;
+	}
+#if 0
+	for (i = 0; i < len/4; i++)
+		p[i] =
+#endif
+	
+	/* FIXME: get data packet from FIFO, (erase+)write flash */
+#ifndef DEBUG_DFU
+
+#endif
+
+	return 0;
+}
+
+static __dfufunc int handle_upload(u_int16_t val, u_int16_t len)
+{
+	DEBUGE("upload ");
+	if (len > AT91C_IFLASH_PAGE_SIZE
+	    || ptr > AT91C_IFLASH_SIZE) {
+		/* Too big */
+		dfu_state = DFU_STATE_dfuERROR;
+		status = DFU_STATUS_errADDRESS;
+		udp_ep0_send_stall();
+		return -EINVAL;
+	}
+
+	if (ptr + len > AT91C_IFLASH_SIZE) 
+		len = AT91C_IFLASH_SIZE - (u_int32_t) ptr;
+		
+	udp_ep0_send_data((char *)ptr, len);
+	ptr+= len;
+
+	return len;
+}
+
+static __dfufunc void handle_getstatus(void)
+{
+	struct dfu_status dstat;
+
+	DEBUGE("getstatus ");
+
+	/* send status response */
+	dstat.bStatus = status;
+	dstat.bState = dfu_state;
+	dstat.iString = 0;
+	udp_ep0_send_data((char *)&dstat, sizeof(dstat));
+}
+
+static void __dfufunc handle_getstate(void)
+{
+	u_int8_t u8 = dfu_state;
+	DEBUGE("getstate ");
+	udp_ep0_send_data((char *)&u8, sizeof(u8));
+}
+
+/* callback function for DFU requests */
+int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
+		    u_int16_t val, u_int16_t len)
+{
+	int rc;
+
+	DEBUGE("old_state = %u ", dfu_state);
+
+	switch (dfu_state) {
+	case DFU_STATE_appIDLE:
+		if (req != USB_REQ_DFU_DETACH)
+			goto send_stall;
+		dfu_state = DFU_STATE_appDETACH;
+		goto send_zlp;
+		break;
+	case DFU_STATE_appDETACH:
+		switch (req) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+		default:
+			dfu_state = DFU_STATE_appIDLE;
+			goto send_stall;
+			break;
+		}
+		/* FIXME: implement timer to return to appIDLE */
+		break;
+	case DFU_STATE_dfuIDLE:
+		switch (req) {
+		case USB_REQ_DFU_DNLOAD:
+			if (len == 0) {
+				dfu_state = DFU_STATE_dfuERROR;
+				goto send_stall;
+			}
+			handle_dnload(val, len);
+			break;
+		case USB_REQ_DFU_UPLOAD:
+			ptr = 0;
+			dfu_state = DFU_STATE_dfuUPLOAD_IDLE;
+			handle_upload(val, len);
+			break;
+		case USB_REQ_DFU_ABORT:
+			/* no zlp? */
+			goto send_zlp;
+			break;
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			goto send_stall;
+			break;
+		}
+		break;
+	case DFU_STATE_dfuDNLOAD_SYNC:
+		switch (req) {
+		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;
+			goto send_stall;
+		}
+		break;
+	case DFU_STATE_dfuDNBUSY:
+		dfu_state = DFU_STATE_dfuERROR;
+		goto send_stall;
+		break;
+	case DFU_STATE_dfuDNLOAD_IDLE:
+		switch (req) {
+		case USB_REQ_DFU_DNLOAD:
+			if (handle_dnload(val, len))
+			/* FIXME: state transition */
+			break;
+		case USB_REQ_DFU_ABORT:
+			dfu_state = DFU_STATE_dfuIDLE;
+			goto send_zlp;
+			break;
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			goto send_stall;
+			break;
+		}
+		break;
+	case DFU_STATE_dfuMANIFEST_SYNC:
+		switch (req) {
+		case USB_REQ_DFU_GETSTATUS:
+			break;
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			goto send_stall;
+			break;
+		}
+		break;
+	case DFU_STATE_dfuMANIFEST:
+		dfu_state = DFU_STATE_dfuERROR;
+		goto send_stall;
+		break;
+	case DFU_STATE_dfuMANIFEST_WAIT_RST:
+		/* we should never go here */
+		break;
+	case DFU_STATE_dfuUPLOAD_IDLE:
+		switch (req) {
+		case USB_REQ_DFU_UPLOAD:
+			/* state transition if less data then requested */
+			rc = handle_upload(val, len);
+			if (rc >= 0 && rc < len)
+				dfu_state = DFU_STATE_dfuIDLE;
+			break;
+		case USB_REQ_DFU_ABORT:
+			dfu_state = DFU_STATE_dfuIDLE;
+			/* no zlp? */
+			goto send_zlp;
+			break;
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			goto send_stall;
+			break;
+		}
+		break;
+	case DFU_STATE_dfuERROR:
+		switch (req) {
+		case USB_REQ_DFU_GETSTATUS:
+			handle_getstatus();
+			break;
+		case USB_REQ_DFU_GETSTATE:
+			handle_getstate();
+			break;
+		case USB_REQ_DFU_CLRSTATUS:
+			dfu_state = DFU_STATE_dfuIDLE;
+			/* no zlp? */
+			goto send_zlp;
+			break;
+		default:
+			dfu_state = DFU_STATE_dfuERROR;
+			goto send_stall;
+			break;
+		}
+		break;
+	}
+
+	DEBUGE("OK new_state = %u\r\n", dfu_state);
+	return 0;
+
+send_stall:
+	udp_ep0_send_stall();
+	DEBUGE("STALL new_state = %u\r\n", dfu_state);
+	return -EINVAL;
+
+send_zlp:
+	udp_ep0_send_zlp();
+	DEBUGE("ZLP new_state = %u\r\n", dfu_state);
+	return 0;
+}
+static u_int8_t cur_config;
+
+/* USB DFU Device descriptor in DFU mode */
+__dfustruct const struct usb_device_descriptor dfu_dev_descriptor = {
+	.bLength		= USB_DT_DEVICE_SIZE,
+	.bDescriptorType	= USB_DT_DEVICE,
+	.bcdUSB			= 0x0100,
+	.bDeviceClass		= 0x00,
+	.bDeviceSubClass	= 0x00,
+	.bDeviceProtocol	= 0x00,
+	.bMaxPacketSize0	= 8,
+	.idVendor		= OPENPCD_VENDOR_ID,
+	.idProduct		= OPENPCD_PRODUCT_ID,
+	.bcdDevice		= 0x0000,
+	.iManufacturer		= 0x00,
+	.iProduct		= 0x00,
+	.iSerialNumber		= 0x00,
+	.bNumConfigurations	= 0x01,
+};
+
+/* USB DFU Config descriptor in DFU mode */
+__dfustruct const struct _dfu_desc dfu_cfg_descriptor = {
+	.ucfg = {
+		.bLength = USB_DT_CONFIG_SIZE,
+		.bDescriptorType = USB_DT_CONFIG,
+		.wTotalLength = USB_DT_CONFIG_SIZE + 
+				2* USB_DT_INTERFACE_SIZE +
+				USB_DT_DFU_SIZE,
+		.bNumInterfaces = 1,
+		.bConfigurationValue = 1,
+		.iConfiguration = 0,
+		.bmAttributes = USB_CONFIG_ATT_ONE,
+		.bMaxPower = 100,
+		},
+	.uif[0] = {
+		.bLength		= USB_DT_INTERFACE_SIZE,
+		.bDescriptorType	= USB_DT_INTERFACE,
+		.bInterfaceNumber	= 0x00,
+		.bAlternateSetting	= 0x00,
+		.bNumEndpoints		= 0x00,
+		.bInterfaceClass	= 0xfe,
+		.bInterfaceSubClass	= 0x01,
+		.bInterfaceProtocol	= 0x02,
+		.iInterface		= 0,
+		}, 
+	.uif[1] = {
+		.bLength		= USB_DT_INTERFACE_SIZE,
+		.bDescriptorType	= USB_DT_INTERFACE,
+		.bInterfaceNumber	= 0x00,
+		.bAlternateSetting	= 0x01,
+		.bNumEndpoints		= 0x00,
+		.bInterfaceClass	= 0xfe,
+		.bInterfaceSubClass	= 0x01,
+		.bInterfaceProtocol	= 0x02,
+		.iInterface		= 0,
+		}, 
+
+	.func_dfu = DFU_FUNC_DESC,
+};
+
+
+/* minimal USB EP0 handler in DFU mode */
+static __dfufunc void dfu_udp_ep0_handler(void)
+{
+	AT91PS_UDP pUDP = AT91C_BASE_UDP;
+	u_int8_t bmRequestType, bRequest;
+	u_int16_t wValue, wIndex, wLength, wStatus;
+	u_int32_t csr = pUDP->UDP_CSR[0];
+
+	DEBUGE("CSR=0x%04x ", csr);
+
+	if (csr & AT91C_UDP_STALLSENT) {
+		DEBUGE("ACK_STALLSENT ");
+		pUDP->UDP_CSR[0] = ~AT91C_UDP_STALLSENT;
+	}
+
+	if (csr & AT91C_UDP_RX_DATA_BK0) {
+		DEBUGE("ACK_BANK0 ");
+		pUDP->UDP_CSR[0] &= ~AT91C_UDP_RX_DATA_BK0;
+	}
+
+	if (!(csr & AT91C_UDP_RXSETUP)) {
+		DEBUGE("no setup packet ");
+		return;
+	}
+
+	DEBUGE("len=%d ", csr >> 16);
+	if (csr >> 16  == 0) {
+		DEBUGE("empty packet ");
+		return;
+	}
+
+	bmRequestType = pUDP->UDP_FDR[0];
+	bRequest = pUDP->UDP_FDR[0];
+	wValue = (pUDP->UDP_FDR[0] & 0xFF);
+	wValue |= (pUDP->UDP_FDR[0] << 8);
+	wIndex = (pUDP->UDP_FDR[0] & 0xFF);
+	wIndex |= (pUDP->UDP_FDR[0] << 8);
+	wLength = (pUDP->UDP_FDR[0] & 0xFF);
+	wLength |= (pUDP->UDP_FDR[0] << 8);
+
+	DEBUGE("bmRequestType=0x%2x ", bmRequestType);
+
+	if (bmRequestType & 0x80) {
+		DEBUGE("DATA_IN=1 ");
+		pUDP->UDP_CSR[0] |= AT91C_UDP_DIR;
+		while (!(pUDP->UDP_CSR[0] & AT91C_UDP_DIR)) ;
+	}
+	pUDP->UDP_CSR[0] &= ~AT91C_UDP_RXSETUP;
+	while ((pUDP->UDP_CSR[0] & AT91C_UDP_RXSETUP)) ;
+
+	/* Handle supported standard device request Cf Table 9-3 in USB
+	 * speciication Rev 1.1 */
+	switch ((bRequest << 8) | bmRequestType) {
+	case STD_GET_DESCRIPTOR:
+		DEBUGE("GET_DESCRIPTOR ");
+		if (wValue == 0x100) {
+			/* Return Device Descriptor */
+			udp_ep0_send_data((const char *) 
+					  &dfu_dev_descriptor,
+					  MIN(sizeof(dfu_dev_descriptor),
+					      wLength));
+		} else if (wValue == 0x200) {
+			/* Return Configuration Descriptor */
+			udp_ep0_send_data((const char *)
+					  &dfu_cfg_descriptor,
+					  MIN(sizeof(dfu_cfg_descriptor),
+					      wLength));
+#if 0
+		} else if (wValue == 0x400) {
+			/* Return Interface descriptor */
+			if (wIndex != 0x01)
+				udp_ep0_send_stall();
+			udp_ep0_send_data((const char *) 
+					  &dfu_if_descriptor,
+					  MIN(sizeof(dfu_if_descriptor),
+					      wLength));
+#endif
+		} else
+			udp_ep0_send_stall();
+		break;
+	case STD_SET_ADDRESS:
+		DEBUGE("SET_ADDRESS ");
+		udp_ep0_send_zlp();
+		pUDP->UDP_FADDR = (AT91C_UDP_FEN | wValue);
+		pUDP->UDP_GLBSTATE = (wValue) ? AT91C_UDP_FADDEN : 0;
+		break;
+	case STD_SET_CONFIGURATION:
+		DEBUGE("SET_CONFIG ");
+		if (wValue)
+			DEBUGE("VALUE!=0 ");
+		cur_config = wValue;
+		udp_ep0_send_zlp();
+		pUDP->UDP_GLBSTATE =
+		    (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN;
+		pUDP->UDP_CSR[1] =
+		    (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT) :
+		    0;
+		pUDP->UDP_CSR[2] =
+		    (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN) : 0;
+		pUDP->UDP_CSR[3] =
+		    (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN) : 0;
+		pUDP->UDP_IER = (AT91C_UDP_EPINT0|AT91C_UDP_EPINT1|
+				 AT91C_UDP_EPINT2|AT91C_UDP_EPINT3);
+		break;
+	case STD_GET_CONFIGURATION:
+		DEBUGE("GET_CONFIG ");
+		udp_ep0_send_data((char *)&(cur_config),
+				   sizeof(cur_config));
+		break;
+	case STD_GET_STATUS_ZERO:
+		DEBUGE("GET_STATUS_ZERO ");
+		wStatus = 0;
+		udp_ep0_send_data((char *)&wStatus, sizeof(wStatus));
+		break;
+	case STD_GET_STATUS_INTERFACE:
+		DEBUGE("GET_STATUS_INTERFACE ");
+		wStatus = 0;
+		udp_ep0_send_data((char *)&wStatus, sizeof(wStatus));
+		break;
+	case STD_GET_STATUS_ENDPOINT:
+		DEBUGE("GET_STATUS_ENDPOINT(EPidx=%u) ", wIndex&0x0f);
+		wStatus = 0;
+		wIndex &= 0x0F;
+		if ((pUDP->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex == 0)) {
+			wStatus =
+			    (pUDP->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
+			udp_ep0_send_data((char *)&wStatus,
+					   sizeof(wStatus));
+		} else if ((pUDP->UDP_GLBSTATE & AT91C_UDP_FADDEN)
+			   && (wIndex == 0)) {
+			wStatus =
+			    (pUDP->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
+			udp_ep0_send_data((char *)&wStatus,
+					   sizeof(wStatus));
+		} else
+			udp_ep0_send_stall();
+		break;
+	case STD_SET_FEATURE_ZERO:
+		DEBUGE("SET_FEATURE_ZERO ");
+		udp_ep0_send_stall();
+		break;
+	case STD_SET_FEATURE_INTERFACE:
+		DEBUGE("SET_FEATURE_INTERFACE ");
+		udp_ep0_send_zlp();
+		break;
+	case STD_SET_FEATURE_ENDPOINT:
+		DEBUGE("SET_FEATURE_ENDPOINT ");
+		udp_ep0_send_stall();
+		break;
+	case STD_CLEAR_FEATURE_ZERO:
+		DEBUGE("CLEAR_FEATURE_ZERO ");
+		udp_ep0_send_stall();
+		break;
+	case STD_CLEAR_FEATURE_INTERFACE:
+		DEBUGE("CLEAR_FEATURE_INTERFACE ");
+		udp_ep0_send_zlp();
+		break;
+	case STD_CLEAR_FEATURE_ENDPOINT:
+		DEBUGE("CLEAR_FEATURE_ENDPOINT(EPidx=%u) ", wIndex & 0x0f);
+		udp_ep0_send_stall();
+		break;
+	case STD_SET_INTERFACE:
+		DEBUGE("SET INTERFACE ");
+		udp_ep0_send_stall();
+		break;
+	default:
+		DEBUGE("DEFAULT(req=0x%02x, type=0x%02x) ", bRequest, bmRequestType);
+		if ((bmRequestType & 0x3f) == USB_TYPE_DFU) {
+			dfu_ep0_handler(bmRequestType, bRequest, wValue, wLength);
+		} else
+			udp_ep0_send_stall();
+		break;
+	}
+}
+
+/* minimal USB IRQ handler in DFU mode */
+static __dfufunc void dfu_udp_irq(void)
+{
+	AT91PS_UDP pUDP = AT91C_BASE_UDP;
+	AT91_REG isr = pUDP->UDP_ISR;
+
+	if (isr & AT91C_UDP_ENDBUSRES) {
+		pUDP->UDP_IER = AT91C_UDP_EPINT0;
+		/* reset all endpoints */
+		pUDP->UDP_RSTEP = (unsigned int)-1;
+		pUDP->UDP_RSTEP = 0;
+		/* Enable the function */
+		pUDP->UDP_FADDR = AT91C_UDP_FEN;
+		/* Configure endpoint 0 */
+		pUDP->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
+		cur_config = 0;
+	}
+
+	if (isr & AT91C_UDP_EPINT0)
+		dfu_udp_ep0_handler();
+
+	/* clear all interrupts */
+	pUDP->UDP_ICR = isr;
+
+	AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_UDP);
+}
+
+/* this is only called once before DFU mode, no __dfufunc required */
+static void dfu_switch(void)
+{
+	AT91PS_AIC pAic = AT91C_BASE_AIC;
+
+	DEBUGE("Switching to DFU mode ");
+
+	pAic->AIC_SVR[AT91C_ID_UDP] = (unsigned int) &dfu_udp_irq;
+	dfu_state = DFU_STATE_dfuIDLE;
+}
+
+void __dfufunc dfu_main(void)
+{
+	udp_init();
+
+	/* This implements 
+	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_UDP,
+			      OPENPCD_IRQ_PRIO_UDP,
+			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, dfu_udp_irq);
+	*/
+	AT91PS_AIC pAic = AT91C_BASE_AIC;
+	pAic->AIC_IDCR = 1 << AT91C_ID_UDP;
+	pAic->AIC_SVR[AT91C_ID_UDP] = (unsigned int) &dfu_udp_irq;
+	pAic->AIC_SMR[AT91C_ID_UDP] = AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL | 
+					OPENPCD_IRQ_PRIO_UDP;
+	pAic->AIC_ICCR = 1 << AT91C_ID_UDP;
+
+	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_UDP);
+
+	/* End-of-Bus-Reset is always enabled */
+
+	/* Clear for set the Pull up resistor */
+	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+
+	/* do nothing, since all of DFU is interrupt driven */
+	while (1) ;
+}
+
+const struct dfuapi __dfufunctab dfu_api = {
+	.ep0_send_data		= &udp_ep0_send_data,
+	.ep0_send_zlp		= &udp_ep0_send_zlp,
+	.ep0_send_stall		= &udp_ep0_send_stall,
+	.dfu_ep0_handler	= &dfu_ep0_handler,
+	.dfu_switch		= &dfu_switch,
+	.dfu_state		= &dfu_state,
+	.dfu_dev_descriptor	= &dfu_dev_descriptor,
+	.dfu_cfg_descriptor	= &dfu_cfg_descriptor,
+};
+
+/* just for testing */
+int foo = 12345;
diff --git a/firmware/src/os/dfu.h b/firmware/src/os/dfu.h
new file mode 100644
index 0000000..8786044
--- /dev/null
+++ b/firmware/src/os/dfu.h
@@ -0,0 +1,80 @@
+#ifndef _DFU_H
+#define _DFU_H
+
+/* USB Device Firmware Update Implementation for OpenPCD
+ * (C) 2006 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ * This ought to be compliant to the USB DFU Spec 1.0 as available from
+ * http://www.usb.org/developers/devclass_docs/usbdfu10.pdf
+ *
+ */
+
+#include <sys/types.h>
+#include <usb_ch9.h>
+#include <usb_dfu.h>
+
+#include "dbgu.h"
+
+/* USB DFU functional descriptor */
+#define DFU_FUNC_DESC  {						\
+	.bLength		= USB_DT_DFU_SIZE,			\
+	.bDescriptorType	= USB_DT_DFU,				\
+	.bmAttributes		= USB_DFU_CAN_UPLOAD | USB_DFU_CAN_DOWNLOAD, \
+	.wDetachTimeOut		= 0xff00,				\
+	.wTransferSize		= AT91C_IFLASH_PAGE_SIZE,		\
+	.bcdDFUVersion		= 0x0100,				\
+}
+
+/* USB Interface descriptor in Runtime mode */
+#define DFU_RT_IF_DESC	{						\
+	.bLength		= USB_DT_INTERFACE_SIZE,		\
+	.bDescriptorType	= USB_DT_INTERFACE,			\
+	.bInterfaceNumber	= 0x01,					\
+	.bAlternateSetting	= 0x00,					\
+	.bNumEndpoints		= 0x00,					\
+	.bInterfaceClass	= 0xfe,					\
+	.bInterfaceSubClass	= 0x01,					\
+	.bInterfaceProtocol	= 0x01,					\
+	.iInterface		= 1,					\
+}
+
+#define __dfufunc __attribute__ ((long_call, section (".dfu.func")))
+#define __dfustruct __attribute__ ((section (".dfu.struct"))) const
+#define __dfufunctab  __attribute__ ((section (".dfu.functab")))
+	
+#if 0
+extern void __dfufunc udp_ep0_send_data(const char *data, u_int32_t length);
+extern void __dfufunc udp_ep0_send_zlp(void);
+extern void __dfufunc udp_ep0_send_stall(void);
+extern __dfustruct struct usb_device_descriptor dfu_dev_descriptor;
+extern __dfustruct struct _dfu_desc dfu_cfg_descriptor;
+extern void dfu_switch(void);
+extern int __dfufunc dfu_ep0_handler(u_int8_t req_type, u_int8_t req,
+				     u_int16_t val, u_int16_t len);
+extern static u_int8_t dfu_state;
+struct udp_pcd;
+#endif
+
+
+extern void __dfufunc udp_init(void);
+
+struct _dfu_desc {
+	struct usb_config_descriptor ucfg;
+	struct usb_interface_descriptor uif[2];
+	struct usb_dfu_func_descriptor func_dfu;
+};
+
+struct dfuapi {
+	void (*ep0_send_data)(const char *data, u_int32_t len);
+	void (*ep0_send_zlp)(void);
+	void (*ep0_send_stall)(void);
+	int  (*dfu_ep0_handler)(u_int8_t req_type, u_int8_t req,
+				     u_int16_t val, u_int16_t len);
+	void (*dfu_switch)(void);
+	u_int8_t *dfu_state;
+	const struct usb_device_descriptor *dfu_dev_descriptor;
+	const struct _dfu_desc *dfu_cfg_descriptor;
+};
+
+
+#endif /* _DFU_H */
diff --git a/firmware/src/os/fifo.c b/firmware/src/os/fifo.c
new file mode 100644
index 0000000..b0ec152
--- /dev/null
+++ b/firmware/src/os/fifo.c
@@ -0,0 +1,108 @@
+/* Implementation of a virtual FIFO */
+
+#include "fifo.h"
+
+#include <errno.h>
+#include <string.h>
+
+#define FIFO_IRQ_LO	0x01
+#define FIFO_IRQ_HI	0x02
+#define FIFO_IRQ_OFLOW	0x04
+
+/* returns number of data bytes present in the fifo */
+int fifo_available(struct fifo *fifo)
+{
+	if (fifo->producer > fifo->consumer)
+		return fifo->producer - fifo->consumer;
+	else
+		return (fifo->size - fifo->consumer) + fifo->producer;
+}
+
+void fifo_check_water(struct fifo *fifo)
+{
+	int avail = fifo_available(fifo);
+
+	if (avail <= fifo->watermark)
+		fifo->irq |= FIFO_IRQ_LO;
+	else
+		fifo->irq &= FIFO_IRQ_LO;
+
+	if (fifo->size - avail >= fifo->watermark)
+		fifo->irq |= FIFO_IRQ_HI;
+	else
+		fifo->irq &= FIFO_IRQ_HI;
+}
+
+void fifo_check_raise_int(struct fifo *fifo)
+{
+	if (fifo->irq & fifo->irq_en)
+		fifo->callback(fifo, fifo->irq, fifo->cb_data);
+}
+
+
+u_int16_t fifo_data_put(struct fifo *fifo, u_int16_t len, u_int8_t *data)
+{
+	if (len > fifo_available(fifo)) {
+		len = fifo_available(fifo);
+		fifo->irq |= FIFO_IRQ_OFLOW;
+	}
+
+	if (len + fifo->producer <= fifo->size) {
+		/* easy case */
+		memcpy(&fifo->data[fifo->producer], data, len);
+		fifo->producer += len;
+	} else {
+		/* difficult: wrap around */
+		u_int16_t chunk_len;
+
+		chunk_len = fifo->size - fifo->producer;
+		memcpy(&fifo->data[fifo->producer], data, chunk_len);
+
+		memcpy(&fifo->data[0], data + chunk_len, len - chunk_len);
+		fifo->producer = len - chunk_len;
+	}
+
+	fifo_check_water(fifo);
+
+	return len;
+}
+
+
+u_int16_t fifo_data_get(struct fifo *fifo, u_int16_t len, u_int8_t *data)
+{
+	u_int16_t avail = fifo_available(fifo);
+
+	if (avail < len)
+		len = avail;
+
+	if (fifo->producer > fifo->consumer) {
+		/* easy case */
+		memcpy(data, &fifo->data[fifo->consumer], len);
+	} else {
+		/* difficult case: wrap */
+		u_int16_t chunk_len = fifo->size - fifo->consumer;
+		memcpy(data, &fifo->data[fifo->consumer], chunk_len);
+		memcpy(data+chunk_len, &fifo->data[0], len - chunk_len);
+	}
+
+	fifo_check_water(fifo);
+
+	return len;
+}
+
+int fifo_init(struct fifo *fifo, u_int16_t size, 
+	      void (*cb)(struct fifo *fifo, u_int8_t event, void *data), void *cb_data)
+{
+	if (size > sizeof(fifo->data))
+		return -EINVAL;
+
+	memset(fifo->data, 0, sizeof(fifo->data));
+	fifo->size = size;
+	fifo->producer = fifo->consumer = 0;
+	fifo->watermark = 0;
+	fifo->callback = cb;
+	fifo->cb_data = cb_data;
+
+	return 0;
+}
+
diff --git a/firmware/src/os/fifo.h b/firmware/src/os/fifo.h
new file mode 100644
index 0000000..d91c6c2
--- /dev/null
+++ b/firmware/src/os/fifo.h
@@ -0,0 +1,28 @@
+#ifndef _FIFO_H
+#define _FIFO_H
+
+#include <sys/types.h>
+
+#define FIFO_SIZE	1024
+
+struct fifo {
+	u_int16_t size;		/* actual FIFO size, can be smaller than 'data' */
+	u_int16_t producer;	/* index of producer */
+	u_int16_t consumer;	/* index of consumer */
+	u_int16_t watermark;
+	u_int8_t irq;
+	u_int8_t irq_en;
+	u_int8_t status;
+	void (*callback)(struct fifo *fifo, u_int8_t event, void *data);
+	void *cb_data;
+	u_int8_t data[FIFO_SIZE];
+};
+
+
+extern int fifo_init(struct fifo *fifo, u_int16_t size, 
+		     void (*callback)(struct fifo *fifo, u_int8_t event, void *data), void *cb_data);
+extern u_int16_t fifo_data_get(struct fifo *fifo, u_int16_t len, u_int8_t *data);
+extern u_int16_t fifo_data_put(struct fifo *fifo, u_int16_t len, u_int8_t *data);
+extern int fifo_available(struct fifo *fifo);
+
+#endif
diff --git a/firmware/src/os/flash.c b/firmware/src/os/flash.c
new file mode 100644
index 0000000..2aaf760
--- /dev/null
+++ b/firmware/src/os/flash.c
@@ -0,0 +1,45 @@
+
+
+#define EFCS_CMD_WRITE_PAGE		0x01
+#define EFCS_CMD_SET_LOCK_BIT		0x02
+#define EFCS_CMD_WRITE_PAGE_LOCK	0x03
+#define EFCS_CMD_CLEAR_LOCK		0x04
+#define EFCS_CMD_ERASE_ALL		0x08
+#define EFCS_CMD_SET_NVM_BIT		0x0b
+#define EFCS_CMD_CLEAR_NVM_BIT		0x0d
+#define EFCS_CMD_SET_SECURITY_BIT	0x0f
+
+
+int unlock_page(u_int16_t page)
+{
+	AT91C_MC_FCMD_UNLOCK | AT91C_MC_CORRECT_KEY | 
+
+}
+
+int flash_sector(unsigned int sector, const u_int8_t *data, unsigned int len)
+{
+	volatile u_int32_t *p = (volatile u_int32_t *)0;
+	u_int32_t *src32 = (u_int32_t *)data;
+	int i;
+
+	/* hand-code memcpy because we need to make sure only 32bit accesses
+	 * are used */
+	for (i = 0; i < len/4; i++)
+		p[i] = src32[i];
+
+	AT91F_MC_EFC_PerformCmd(pmc , AT91C_MC_FCMD_START_PROG|
+				AT91C_MC_CORRECT_KEY | );
+}
+
+
+void flash_init(void)
+{
+	unsigned int fmcn = AT91F_MC_EFC_ComputerFMCN(48000000);
+
+	AT91F_MC_EFC_CfgModeReg(ff, fmcn << 16 | AT91C_MC_FWS_3FWS |
+				AT91C_MC_FRDY | AT91C_MC_LOCKE | 
+				AT91C_MC_PROGE);
+
+	AT91F_AIC_EnableIt();
+
+}
diff --git a/firmware/src/os/led.c b/firmware/src/os/led.c
new file mode 100644
index 0000000..8c34c6c
--- /dev/null
+++ b/firmware/src/os/led.c
@@ -0,0 +1,85 @@
+
+#include <sys/types.h>
+#include <errno.h>
+#include <lib_AT91SAM7.h>
+#include <openpcd.h>
+#include "../openpcd.h"
+#include <os/usb_handler.h>
+#include <os/req_ctx.h>
+#include <os/dbgu.h>
+
+static int led2port(int led)
+{
+	if (led == 1)
+		return OPENPCD_PIO_LED1;
+	else if (led == 2)
+		return OPENPCD_PIO_LED2;
+	else
+		return 0;
+}
+
+void led_switch(int led, int on)
+{
+	int port = led2port(led);
+
+	if (port == -1)
+		return;
+
+	if (on)
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, port);
+	else
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, port);
+}
+
+int led_get(int led)
+{
+	int port = led2port(led);
+
+	if (port == -1)
+		return -1;
+
+	return !(AT91F_PIO_GetOutputDataStatus(AT91C_BASE_PIOA) & port);
+}
+
+int led_toggle(int led)
+{
+	int on = led_get(led);
+	if (on == -1)
+		return -1;
+
+	if (on)
+		led_switch(led, 0);
+	else
+		led_switch(led, 1);
+	
+	return !on;
+}
+
+static int led_usb_rx(struct req_ctx *rctx)
+{
+	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
+	int ret = 1;
+
+	switch (poh->cmd) {
+	case OPENPCD_CMD_SET_LED:
+		DEBUGP("SET LED(%u,%u) ", poh->reg, poh->val);
+		led_switch(poh->reg, poh->val);
+		break;
+	default:
+		DEBUGP("UNKNOWN ");
+		ret = -EINVAL;
+		break;
+	}
+	req_ctx_put(rctx);
+	return 1;
+}
+
+void led_init(void)
+{
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_LED1);
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_LED2);
+	led_switch(1, 0);
+	led_switch(2, 0);
+
+	usb_hdlr_register(&led_usb_rx, OPENPCD_CMD_CLS_LED);
+}
diff --git a/firmware/src/os/led.h b/firmware/src/os/led.h
new file mode 100644
index 0000000..394107b
--- /dev/null
+++ b/firmware/src/os/led.h
@@ -0,0 +1,9 @@
+#ifndef _LED_H
+#define _LED_H
+
+extern void led_init(void);
+extern void led_switch(int led, int on);
+extern int led_get(int led);
+extern int led_toggle(int led);
+
+#endif
diff --git a/firmware/src/os/main.c b/firmware/src/os/main.c
new file mode 100644
index 0000000..4b7a7a2
--- /dev/null
+++ b/firmware/src/os/main.c
@@ -0,0 +1,47 @@
+#include <errno.h>
+#include <string.h>
+#include <include/lib_AT91SAM7.h>
+#include <os/dbgu.h>
+#include <os/led.h>
+#include <os/dfu.h>
+#include <os/main.h>
+#include <os/power.h>
+#include <os/pcd_enumerate.h>
+#include "../openpcd.h"
+
+int main(void)
+{
+	/* initialize LED and debug unit */
+	led_init();
+	AT91F_DBGU_Init();
+
+	AT91F_PIOA_CfgPMC();
+	/* call application specific init function */
+	_init_func();
+
+	/* initialize USB */
+	udp_init();
+	udp_open();
+
+	// Enable User Reset and set its minimal assertion to 960 us
+	AT91C_BASE_RSTC->RSTC_RMR =
+	    AT91C_RSTC_URSTEN | (0x4 << 8) | (unsigned int)(0xA5 << 24);
+
+#ifdef DEBUG_CLOCK_PA6
+	AT91F_PMC_EnablePCK(AT91C_BASE_PMC, 0, AT91C_PMC_CSS_PLL_CLK);
+	AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, 0, AT91C_PA6_PCK0);
+#endif
+
+	/* switch on first led */
+	led_switch(2, 1);
+
+	DEBUGPCRF("entering main (idle) loop");
+	while (1) {
+		/* Call application specific main idle function */
+		_main_func();
+		dbgu_rb_flush();
+#ifdef CONFIG_IDLE
+		//cpu_idle();
+#endif
+	}
+}
diff --git a/firmware/src/os/main.h b/firmware/src/os/main.h
new file mode 100644
index 0000000..1adc8f6
--- /dev/null
+++ b/firmware/src/os/main.h
@@ -0,0 +1,8 @@
+#ifndef _MAIN_H
+#define _MAIN_H
+
+extern void _init_func(void);
+extern int _main_dbgu(char key);
+extern void _main_func(void);
+
+#endif
diff --git a/firmware/src/os/pcd_enumerate.c b/firmware/src/os/pcd_enumerate.c
new file mode 100644
index 0000000..cded8c8
--- /dev/null
+++ b/firmware/src/os/pcd_enumerate.c
@@ -0,0 +1,605 @@
+/* AT91SAM7 USB interface code for OpenPCD 
+ *
+ * (C) 2006 by Harald Welte <laforge@gnumonks.org>
+ *
+ * based on existing AT91SAM7 UDP CDC ACM example code, licensed as followed:
+ *----------------------------------------------------------------------------
+ *      ATMEL Microcontroller Software Support  -  ROUSSET  -
+ *----------------------------------------------------------------------------
+ * The software is delivered "AS IS" without warranty or condition of any
+ * kind, either express, implied or statutory. This includes without
+ * limitation any warranty or condition with respect to merchantability or
+ * fitness for any particular purpose, or against the infringements of
+ * intellectual property rights of others.
+ *----------------------------------------------------------------------------
+ */
+
+#include <errno.h>
+#include <usb_ch9.h>
+#include <sys/types.h>
+#include <asm/atomic.h>
+#include <lib_AT91SAM7.h>
+#include <openpcd.h>
+
+#include <os/pcd_enumerate.h>
+#include <os/req_ctx.h>
+#include <os/dfu.h>
+#include "../openpcd.h"
+#include <os/dbgu.h>
+
+#define DEBUG_UDP_IRQ
+#define DEBUG_UDP_EP0
+
+#define CONFIG_DFU
+
+#define AT91C_EP_OUT 1
+#define AT91C_EP_OUT_SIZE 0x40
+#define AT91C_EP_IN  2
+#define AT91C_EP_IN_SIZE 0x40
+#define AT91C_EP_INT  3
+
+#ifdef CONFIG_DFU
+#define DFU_API_LOCATION	((const struct dfuapi *) 0x00102100)
+static const struct dfuapi *dfu = DFU_API_LOCATION;
+#define udp_ep0_send_data	dfu->ep0_send_data
+#define udp_ep0_send_zlp	dfu->ep0_send_zlp
+#define udp_ep0_send_stall	dfu->ep0_send_stall
+#else
+#error non-DFU builds currently not supported (yet) again
+#endif
+
+static struct udp_pcd upcd;
+
+const struct usb_device_descriptor dev_descriptor = {
+	.bLength = USB_DT_DEVICE_SIZE,
+	.bDescriptorType = USB_DT_DEVICE,
+	.bcdUSB = 0x0200,
+	.bDeviceClass = USB_CLASS_VENDOR_SPEC,
+	.bDeviceSubClass = 0xff,
+	.bDeviceProtocol = 0xff,
+	.bMaxPacketSize0 = 0x08,
+	.idVendor = OPENPCD_VENDOR_ID,
+	.idProduct = OPENPCD_PRODUCT_ID,
+	.bcdDevice = 0x0000,
+	.iManufacturer = 0x00,
+	.iProduct = 0x00,
+	.iSerialNumber = 0x00,
+	.bNumConfigurations = 0x01,
+};
+
+struct _desc {
+	struct usb_config_descriptor ucfg;
+	struct usb_interface_descriptor uif;
+	struct usb_endpoint_descriptor ep[3];
+#ifdef CONFIG_DFU
+	struct usb_interface_descriptor uif_dfu;
+	struct usb_dfu_func_descriptor func_dfu;
+#endif
+};
+
+const struct _desc cfg_descriptor = {
+	.ucfg = {
+		 .bLength = USB_DT_CONFIG_SIZE,
+		 .bDescriptorType = USB_DT_CONFIG,
+		 .wTotalLength = USB_DT_CONFIG_SIZE +
+#ifdef CONFIG_DFU
+		 		 2 * USB_DT_INTERFACE_SIZE + 
+				 3 * USB_DT_ENDPOINT_SIZE +
+				 USB_DT_DFU_SIZE,
+		 .bNumInterfaces = 2,
+#else
+		 		 1 * USB_DT_INTERFACE_SIZE + 
+				 3 * USB_DT_ENDPOINT_SIZE,
+		 .bNumInterfaces = 1,
+#endif
+		 .bConfigurationValue = 1,
+		 .iConfiguration = 0,
+		 .bmAttributes = USB_CONFIG_ATT_ONE,
+		 .bMaxPower = 100,	/* 200mA */
+		 },
+	.uif = {
+		.bLength = USB_DT_INTERFACE_SIZE,
+		.bDescriptorType = USB_DT_INTERFACE,
+		.bInterfaceNumber = 0,
+		.bAlternateSetting = 0,
+		.bNumEndpoints = 3,
+		.bInterfaceClass = USB_CLASS_VENDOR_SPEC,
+		.bInterfaceSubClass = 0,
+		.bInterfaceProtocol = 0xff,
+		.iInterface = 0,
+		},
+	.ep= {
+		{
+			  .bLength = USB_DT_ENDPOINT_SIZE,
+			  .bDescriptorType = USB_DT_ENDPOINT,
+			  .bEndpointAddress = OPENPCD_OUT_EP,
+			  .bmAttributes = USB_ENDPOINT_XFER_BULK,
+			  .wMaxPacketSize = AT91C_EP_OUT_SIZE,
+			  .bInterval = 0x00,
+		  }, {
+			  .bLength = USB_DT_ENDPOINT_SIZE,
+			  .bDescriptorType = USB_DT_ENDPOINT,
+			  .bEndpointAddress = OPENPCD_IN_EP,
+			  .bmAttributes = USB_ENDPOINT_XFER_BULK,
+			  .wMaxPacketSize = AT91C_EP_IN_SIZE,
+			  .bInterval = 0x00,
+		  }, {
+			  .bLength = USB_DT_ENDPOINT_SIZE,
+			  .bDescriptorType = USB_DT_ENDPOINT,
+			  .bEndpointAddress = OPENPCD_IRQ_EP,
+			  .bmAttributes = USB_ENDPOINT_XFER_INT,
+			  .wMaxPacketSize = AT91C_EP_IN_SIZE,
+			  .bInterval = 0xff,	/* FIXME */
+		  },
+	},
+#ifdef CONFIG_DFU
+	.uif_dfu = DFU_RT_IF_DESC,
+	.func_dfu = DFU_FUNC_DESC,
+#endif
+};
+
+static const struct usb_string_descriptor string0 = {
+	.bLength = sizeof(string0),
+	.bDescriptorType = USB_DT_STRING,
+	.wData[0] = 0x0409,	/* English */
+};
+
+
+static void udp_ep0_handler(void);
+
+void udp_unthrottle(void)
+{
+	AT91PS_UDP pUDP = upcd.pUdp;
+	pUDP->UDP_IER = AT91C_UDP_EPINT1;
+}
+
+int udp_refill_ep(int ep, struct req_ctx *rctx)
+{
+	u_int16_t i;
+	AT91PS_UDP pUDP = upcd.pUdp;
+
+	if (!upcd.cur_config)
+		return -ENXIO;
+	
+	if (rctx->tx.tot_len > AT91C_EP_IN_SIZE) {
+		DEBUGPCRF("TOO LARGE!!!!!!!!!!!!!!!!!!!!!!!!!!! (%d > %d)",
+			  rctx->tx.tot_len, AT91C_EP_IN_SIZE);
+		return -EINVAL;
+	}
+
+	if (atomic_read(&upcd.ep[ep].pkts_in_transit) == 2)
+		return -EBUSY;
+		
+	/* fill FIFO/DPR */
+	for (i = 0; i < rctx->tx.tot_len; i++) 
+		pUDP->UDP_FDR[ep] = rctx->tx.data[i];
+
+	if (atomic_inc_return(&upcd.ep[ep].pkts_in_transit) == 1) {
+		/* not been transmitting before, start transmit */
+		pUDP->UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
+	}
+
+	/* return rctx to pool of free contexts */
+	req_ctx_put(rctx);
+
+	return 0;
+}
+
+#ifdef DEBUG_UDP_IRQ
+#define DEBUGI(x, args ...)	DEBUGP(x, ## args)
+#else
+#define DEBUGI(x, args ...)	do { } while (0)
+#endif
+
+static void udp_irq(void)
+{
+	u_int32_t csr;
+	AT91PS_UDP pUDP = upcd.pUdp;
+	AT91_REG isr = pUDP->UDP_ISR;
+
+	DEBUGI("udp_irq(imr=0x%04x, isr=0x%04x): ", pUDP->UDP_IMR, isr);
+
+	if (isr & AT91C_UDP_ENDBUSRES) {
+		DEBUGI("ENDBUSRES ");
+		pUDP->UDP_ICR = AT91C_UDP_ENDBUSRES;
+		pUDP->UDP_IER = AT91C_UDP_EPINT0;
+		/* reset all endpoints */
+		pUDP->UDP_RSTEP = (unsigned int)-1;
+		pUDP->UDP_RSTEP = 0;
+		/* Enable the function */
+		pUDP->UDP_FADDR = AT91C_UDP_FEN;
+		/* Configure endpoint 0 */
+		pUDP->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
+		upcd.cur_config = 0;
+		
+#ifdef CONFIG_DFU
+		if (*dfu->dfu_state == DFU_STATE_appDETACH) {
+			/* now we need to switch to DFU mode */
+			dfu->dfu_switch();
+		}
+#endif
+	}
+
+	if (isr & AT91C_UDP_EPINT0) {
+		DEBUGI("EP0INT(Control) ");
+		udp_ep0_handler();
+	}
+	if (isr & AT91C_UDP_EPINT1) {
+		u_int32_t cur_rcv_bank = upcd.cur_rcv_bank;
+		csr = pUDP->UDP_CSR[1];
+		DEBUGI("EP1INT(Out, CSR=0x%08x) ", csr);
+		if (cur_rcv_bank == AT91C_UDP_RX_DATA_BK1)
+			DEBUGI("cur_bank=1 ");
+		else if (cur_rcv_bank == AT91C_UDP_RX_DATA_BK0)
+			DEBUGI("cur_bank=0 ");
+		else
+			DEBUGI("cur_bank INVALID ");
+
+		if (csr & AT91C_UDP_RX_DATA_BK1)
+			DEBUGI("BANK1 ");
+		if (csr & AT91C_UDP_RX_DATA_BK0)
+			DEBUGI("BANK0 ");
+
+		if (csr & cur_rcv_bank) {
+			u_int16_t pkt_recv = 0;
+			u_int16_t pkt_size = csr >> 16;
+			struct req_ctx *rctx = req_ctx_find_get(RCTX_STATE_FREE,
+								RCTX_STATE_UDP_RCV_BUSY);
+
+			if (rctx) {
+				rctx->rx.tot_len = pkt_size;
+				while (pkt_size--)
+					rctx->rx.data[pkt_recv++] = pUDP->UDP_FDR[1];
+				pUDP->UDP_CSR[1] &= ~cur_rcv_bank;
+				if (cur_rcv_bank == AT91C_UDP_RX_DATA_BK0)
+					cur_rcv_bank = AT91C_UDP_RX_DATA_BK1;
+				else
+					cur_rcv_bank = AT91C_UDP_RX_DATA_BK0;
+				upcd.cur_rcv_bank = cur_rcv_bank;
+				req_ctx_set_state(rctx, RCTX_STATE_UDP_RCV_DONE);
+				DEBUGI("RCTX=%u ", req_ctx_num(rctx));
+			} else {
+				/* disable interrupts for now */
+				pUDP->UDP_IDR = AT91C_UDP_EPINT1;
+				DEBUGP("NO_RCTX_AVAIL! ");
+			}
+		}
+	}
+	if (isr & AT91C_UDP_EPINT2) {
+		csr = pUDP->UDP_CSR[2];
+		DEBUGI("EP2INT(In, CSR=0x%08x) ", csr);
+		if (csr & AT91C_UDP_TXCOMP) {
+			struct req_ctx *rctx;
+
+			DEBUGI("ACK_TX_COMP ");
+			/* acknowledge TX completion */
+			pUDP->UDP_CSR[2] &= ~AT91C_UDP_TXCOMP;
+			while (pUDP->UDP_CSR[2] & AT91C_UDP_TXCOMP) ;
+
+			/* if we already have another packet in DPR, send it */
+			if (atomic_dec_return(&upcd.ep[2].pkts_in_transit) == 1)
+				pUDP->UDP_CSR[2] |= AT91C_UDP_TXPKTRDY;
+
+			/* try to re-fill from pending rcts for EP2 */
+			rctx = req_ctx_find_get(RCTX_STATE_UDP_EP2_PENDING,
+						RCTX_STATE_UDP_EP2_BUSY);
+			if (rctx)
+				udp_refill_ep(2, rctx);
+			else
+				DEBUGI("NO_RCTX_pending ");
+		}
+	}
+	if (isr & AT91C_UDP_EPINT3) {
+		csr = pUDP->UDP_CSR[3];
+		DEBUGI("EP3INT(Interrupt, CSR=0x%08x) ", csr);
+		/* Transmit has completed, re-fill from pending rcts for EP3 */
+	}
+
+	if (isr & AT91C_UDP_RXSUSP) {
+		pUDP->UDP_ICR = AT91C_UDP_RXSUSP;
+		DEBUGI("RXSUSP ");
+		/* FIXME: implement suspend/resume */
+	}
+	if (isr & AT91C_UDP_RXRSM) {
+		pUDP->UDP_ICR = AT91C_UDP_RXRSM;
+		DEBUGI("RXRSM ");
+		/* FIXME: implement suspend/resume */
+	}
+	if (isr & AT91C_UDP_EXTRSM) {
+		pUDP->UDP_ICR = AT91C_UDP_EXTRSM;
+		DEBUGI("EXTRSM ");
+		/* FIXME: implement suspend/resume */
+	}
+	if (isr & AT91C_UDP_SOFINT) {
+		pUDP->UDP_ICR = AT91C_UDP_SOFINT;
+		DEBUGI("SOFINT ");
+	}
+	if (isr & AT91C_UDP_WAKEUP) {
+		pUDP->UDP_ICR = AT91C_UDP_WAKEUP;
+		DEBUGI("WAKEUP ");
+		/* FIXME: implement suspend/resume */
+	}
+
+	DEBUGI("END\r\n");
+	AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_UDP);
+}
+
+/* Open USB Device Port  */
+void udp_open(void)
+{
+	DEBUGPCRF("entering");
+	upcd.pUdp = AT91C_BASE_UDP;
+	upcd.cur_config = 0;
+	upcd.cur_rcv_bank = AT91C_UDP_RX_DATA_BK0;
+
+	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_UDP,
+			      OPENPCD_IRQ_PRIO_UDP,
+			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, udp_irq);
+	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_UDP);
+
+	/* End-of-Bus-Reset is always enabled */
+
+	/* Set the Pull up resistor */
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+}
+
+void udp_reset(void)
+{
+	volatile int i;
+
+	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+	for (i = 0; i < 0xffff; i++)
+		;
+	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_UDP_PUP);
+}
+
+#ifdef DEBUG_UDP_EP0
+#define DEBUGE(x, args ...)	DEBUGP(x, ## args)
+#else
+#define DEBUGE(x, args ...)	do { } while (0)
+#endif
+
+/* Handle requests on the USB Control Endpoint */
+static void udp_ep0_handler(void)
+{
+	AT91PS_UDP pUDP = upcd.pUdp;
+	u_int8_t bmRequestType, bRequest;
+	u_int16_t wValue, wIndex, wLength, wStatus;
+	u_int32_t csr = pUDP->UDP_CSR[0];
+
+	DEBUGE("CSR=0x%04x ", csr);
+
+	if (csr & AT91C_UDP_STALLSENT) {
+		DEBUGE("ACK_STALLSENT ");
+		pUDP->UDP_CSR[0] = ~AT91C_UDP_STALLSENT;
+	}
+
+	if (csr & AT91C_UDP_RX_DATA_BK0) {
+		DEBUGE("ACK_BANK0 ");
+		pUDP->UDP_CSR[0] &= ~AT91C_UDP_RX_DATA_BK0;
+	}
+
+	if (!(csr & AT91C_UDP_RXSETUP)) {
+		DEBUGE("no setup packet ");
+		return;
+	}
+
+	DEBUGE("len=%d ", csr >> 16);
+	if (csr >> 16  == 0) {
+		DEBUGE("empty packet ");
+		return;
+	}
+
+	bmRequestType = pUDP->UDP_FDR[0];
+	bRequest = pUDP->UDP_FDR[0];
+	wValue = (pUDP->UDP_FDR[0] & 0xFF);
+	wValue |= (pUDP->UDP_FDR[0] << 8);
+	wIndex = (pUDP->UDP_FDR[0] & 0xFF);
+	wIndex |= (pUDP->UDP_FDR[0] << 8);
+	wLength = (pUDP->UDP_FDR[0] & 0xFF);
+	wLength |= (pUDP->UDP_FDR[0] << 8);
+
+	DEBUGE("bmRequestType=0x%2x ", bmRequestType);
+
+	if (bmRequestType & 0x80) {
+		DEBUGE("DATA_IN=1 ");
+		pUDP->UDP_CSR[0] |= AT91C_UDP_DIR;
+		while (!(pUDP->UDP_CSR[0] & AT91C_UDP_DIR)) ;
+	}
+	pUDP->UDP_CSR[0] &= ~AT91C_UDP_RXSETUP;
+	while ((pUDP->UDP_CSR[0] & AT91C_UDP_RXSETUP)) ;
+
+	/* Handle supported standard device request Cf Table 9-3 in USB
+	 * speciication Rev 1.1 */
+	switch ((bRequest << 8) | bmRequestType) {
+	case STD_GET_DESCRIPTOR:
+		DEBUGE("GET_DESCRIPTOR ");
+		if (wValue == 0x100) {
+			/* Return Device Descriptor */
+#ifdef CONFIG_DFU
+			if (*dfu->dfu_state != DFU_STATE_appIDLE)
+				udp_ep0_send_data((const char *) 
+						  dfu->dfu_dev_descriptor,
+						  MIN(dfu->dfu_dev_descriptor->bLength,
+						      wLength));
+			else
+#endif
+			udp_ep0_send_data((const char *) &dev_descriptor,
+					   MIN(sizeof(dev_descriptor), wLength));
+		} else if (wValue == 0x200) {
+			/* Return Configuration Descriptor */
+#ifdef CONFIG_DFU
+			if (*dfu->dfu_state != DFU_STATE_appIDLE)
+				udp_ep0_send_data((const char *)
+						  dfu->dfu_cfg_descriptor,
+						  MIN(dfu->dfu_cfg_descriptor->ucfg.wTotalLength,
+						      wLength));
+			else
+#endif
+			udp_ep0_send_data((const char *) &cfg_descriptor,
+					   MIN(sizeof(cfg_descriptor), wLength));
+		} else if (wValue == 0x300) {
+			/* Return String descriptor */
+			switch (wIndex) {
+			case 0:
+				udp_ep0_send_data((const char *) &string0,
+						  MIN(sizeof(string0), wLength));
+				break;
+			default:
+				/* FIXME: implement this */
+				udp_ep0_send_stall();
+				break;
+			}
+#if 0
+		} else if (wValue == 0x400) {
+			/* Return Interface descriptor */
+			if (wIndex != 0x01)
+				udp_ep0_send_stall();
+			udp_ep0_send_data((const char *) 
+					  &dfu_if_descriptor,
+					  MIN(sizeof(dfu_if_descriptor),
+					      wLength));
+#endif
+		} else
+			udp_ep0_send_stall();
+		break;
+	case STD_SET_ADDRESS:
+		DEBUGE("SET_ADDRESS ");
+		udp_ep0_send_zlp();
+		pUDP->UDP_FADDR = (AT91C_UDP_FEN | wValue);
+		pUDP->UDP_GLBSTATE = (wValue) ? AT91C_UDP_FADDEN : 0;
+		break;
+	case STD_SET_CONFIGURATION:
+		DEBUGE("SET_CONFIG ");
+		if (wValue)
+			DEBUGE("VALUE!=0 ");
+		upcd.cur_config = wValue;
+		udp_ep0_send_zlp();
+		pUDP->UDP_GLBSTATE =
+		    (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN;
+		pUDP->UDP_CSR[1] =
+		    (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT) :
+		    0;
+		pUDP->UDP_CSR[2] =
+		    (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN) : 0;
+		pUDP->UDP_CSR[3] =
+		    (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN) : 0;
+		pUDP->UDP_IER = (AT91C_UDP_EPINT0|AT91C_UDP_EPINT1|
+				 AT91C_UDP_EPINT2|AT91C_UDP_EPINT3);
+		break;
+	case STD_GET_CONFIGURATION:
+		DEBUGE("GET_CONFIG ");
+		udp_ep0_send_data((char *)&(upcd.cur_config),
+				   sizeof(upcd.cur_config));
+		break;
+	case STD_GET_STATUS_ZERO:
+		DEBUGE("GET_STATUS_ZERO ");
+		wStatus = 0;
+		udp_ep0_send_data((char *)&wStatus, sizeof(wStatus));
+		break;
+	case STD_GET_STATUS_INTERFACE:
+		DEBUGE("GET_STATUS_INTERFACE ");
+		wStatus = 0;
+		udp_ep0_send_data((char *)&wStatus, sizeof(wStatus));
+		break;
+	case STD_GET_STATUS_ENDPOINT:
+		DEBUGE("GET_STATUS_ENDPOINT(EPidx=%u) ", wIndex&0x0f);
+		wStatus = 0;
+		wIndex &= 0x0F;
+		if ((pUDP->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex <= 3)) {
+			wStatus =
+			    (pUDP->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
+			udp_ep0_send_data((char *)&wStatus,
+					   sizeof(wStatus));
+		} else if ((pUDP->UDP_GLBSTATE & AT91C_UDP_FADDEN)
+			   && (wIndex == 0)) {
+			wStatus =
+			    (pUDP->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
+			udp_ep0_send_data((char *)&wStatus,
+					   sizeof(wStatus));
+		} else
+			udp_ep0_send_stall();
+		break;
+	case STD_SET_FEATURE_ZERO:
+		DEBUGE("SET_FEATURE_ZERO ");
+		udp_ep0_send_stall();
+		break;
+	case STD_SET_FEATURE_INTERFACE:
+		DEBUGE("SET_FEATURE_INTERFACE ");
+		udp_ep0_send_zlp();
+		break;
+	case STD_SET_FEATURE_ENDPOINT:
+		DEBUGE("SET_FEATURE_ENDPOINT ");
+		udp_ep0_send_zlp();
+		wIndex &= 0x0F;
+		if ((wValue == 0) && wIndex && (wIndex <= 3)) {
+			pUDP->UDP_CSR[wIndex] = 0;
+			udp_ep0_send_zlp();
+		} else
+			udp_ep0_send_stall();
+		break;
+	case STD_CLEAR_FEATURE_ZERO:
+		DEBUGE("CLEAR_FEATURE_ZERO ");
+		udp_ep0_send_stall();
+		break;
+	case STD_CLEAR_FEATURE_INTERFACE:
+		DEBUGP("CLEAR_FEATURE_INTERFACE ");
+		udp_ep0_send_zlp();
+		break;
+	case STD_CLEAR_FEATURE_ENDPOINT:
+		DEBUGE("CLEAR_FEATURE_ENDPOINT(EPidx=%u) ", wIndex & 0x0f);
+		wIndex &= 0x0F;
+		if ((wValue == 0) && wIndex && (wIndex <= 3)) {
+			struct req_ctx *rctx;
+			if (wIndex == 1) {
+				pUDP->UDP_CSR[1] =
+				    (AT91C_UDP_EPEDS |
+				     AT91C_UDP_EPTYPE_BULK_OUT);
+				pUDP->UDP_RSTEP |= AT91C_UDP_EP1;
+				pUDP->UDP_RSTEP &= ~AT91C_UDP_EP1;
+			}
+			else if (wIndex == 2) {
+				pUDP->UDP_CSR[2] =
+				    (AT91C_UDP_EPEDS |
+				     AT91C_UDP_EPTYPE_BULK_IN);
+				pUDP->UDP_RSTEP |= AT91C_UDP_EP2;
+				pUDP->UDP_RSTEP &= ~AT91C_UDP_EP2;
+
+				/* free all currently transmitting contexts */
+				while (rctx = req_ctx_find_get(RCTX_STATE_UDP_EP2_BUSY,
+							       RCTX_STATE_FREE)) {}
+				atomic_set(&upcd.ep[wIndex].pkts_in_transit, 0);
+			}
+			else if (wIndex == 3) {
+				pUDP->UDP_CSR[3] =
+				    (AT91C_UDP_EPEDS |
+				     AT91C_UDP_EPTYPE_INT_IN);
+				pUDP->UDP_RSTEP |= AT91C_UDP_EP3;
+				pUDP->UDP_RSTEP &= ~AT91C_UDP_EP3;
+
+				/* free all currently transmitting contexts */
+				while (rctx = req_ctx_find_get(RCTX_STATE_UDP_EP3_BUSY,
+							       RCTX_STATE_FREE)) {}
+				atomic_set(&upcd.ep[wIndex].pkts_in_transit, 0);
+			}
+			udp_ep0_send_zlp();
+		} else
+			udp_ep0_send_stall();
+		break;
+	case STD_SET_INTERFACE:
+		DEBUGE("SET INTERFACE ");
+		udp_ep0_send_stall();
+		break;
+	default:
+		DEBUGE("DEFAULT(req=0x%02x, type=0x%02x) ", bRequest, bmRequestType);
+#ifdef CONFIG_DFU
+		if ((bmRequestType & 0x3f) == USB_TYPE_DFU) {
+			dfu->dfu_ep0_handler(bmRequestType, bRequest, wValue, wLength);
+		} else
+#endif
+		udp_ep0_send_stall();
+		break;
+	}
+}
+
diff --git a/firmware/src/os/pcd_enumerate.h b/firmware/src/os/pcd_enumerate.h
new file mode 100644
index 0000000..57ff88c
--- /dev/null
+++ b/firmware/src/os/pcd_enumerate.h
@@ -0,0 +1,56 @@
+#ifndef _OPCD_USB_H
+#define _OPCD_USB_H
+
+#include <lib_AT91SAM7.h>
+#include <sys/types.h>
+#include <asm/atomic.h>
+#include "openpcd.h"
+#include "dfu.h"
+
+struct req_ctx;
+
+extern void udp_open(void);
+extern int udp_refill_ep(int ep, struct req_ctx *rctx);
+extern void udp_unthrottle(void);
+extern void udp_reset(void);
+
+struct ep_ctx {
+	atomic_t pkts_in_transit;
+	void *ctx;
+};
+
+struct udp_pcd {
+	AT91PS_UDP pUdp;
+	unsigned char cur_config;
+	unsigned int  cur_rcv_bank;
+	struct ep_ctx ep[4];
+};
+
+/* USB standard request code */
+
+#define STD_GET_STATUS_ZERO           0x0080
+#define STD_GET_STATUS_INTERFACE      0x0081
+#define STD_GET_STATUS_ENDPOINT       0x0082
+
+#define STD_CLEAR_FEATURE_ZERO        0x0100
+#define STD_CLEAR_FEATURE_INTERFACE   0x0101
+#define STD_CLEAR_FEATURE_ENDPOINT    0x0102
+
+#define STD_SET_FEATURE_ZERO          0x0300
+#define STD_SET_FEATURE_INTERFACE     0x0301
+#define STD_SET_FEATURE_ENDPOINT      0x0302
+
+#define STD_SET_ADDRESS               0x0500
+#define STD_GET_DESCRIPTOR            0x0680
+#define STD_SET_DESCRIPTOR            0x0700
+#define STD_GET_CONFIGURATION         0x0880
+#define STD_SET_CONFIGURATION         0x0900
+#define STD_GET_INTERFACE             0x0A81
+#define STD_SET_INTERFACE             0x0B01
+#define STD_SYNCH_FRAME               0x0C82
+
+#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+
+
+#endif 
+
diff --git a/firmware/src/os/pio_irq.c b/firmware/src/os/pio_irq.c
new file mode 100644
index 0000000..8dae806
--- /dev/null
+++ b/firmware/src/os/pio_irq.c
@@ -0,0 +1,122 @@
+#include <errno.h>
+#include <sys/types.h>
+#include <lib_AT91SAM7.h>
+#include <os/pio_irq.h>
+#include <os/dbgu.h>
+#include <os/req_ctx.h>
+#include <openpcd.h>
+
+struct pioirq_state {
+	irq_handler_t *handlers[NR_PIO];
+	u_int32_t usbmask;
+	u_int32_t usb_throttled; /* atomic? */
+};
+
+static struct pioirq_state pirqs;
+
+static void pio_irq_demux(void)
+{
+	u_int32_t pio = AT91F_PIO_GetInterruptStatus(AT91C_BASE_PIOA);
+	u_int8_t send_usb = 0;
+	int i;
+
+	DEBUGPCRF("PIO_ISR_STATUS = 0x%08x", pio);
+
+	for (i = 0; i < NR_PIO; i++) {
+		if (pio & (1 << i) && pirqs.handlers[i])
+			pirqs.handlers[i](i);
+		if (pirqs.usbmask & (1 << i))
+			send_usb = 1;
+	}
+
+	if (send_usb && !pirqs.usb_throttled) {
+		struct req_ctx *irq_rctx;
+		irq_rctx = req_ctx_find_get(RCTX_STATE_FREE,
+					    RCTX_STATE_PIOIRQ_BUSY);
+		if (!irq_rctx) {
+			/* we cannot disable the interrupt, since we have
+			 * non-usb listeners */
+			pirqs.usb_throttled = 1;
+		} else {
+			struct openpcd_hdr *opcdh;
+			u_int32_t *regmask;
+			opcdh = (struct openpcd_hdr *) &irq_rctx->tx.data[0];
+			regmask = (u_int32_t *) (&irq_rctx->tx.data[0] + sizeof(*opcdh));
+			opcdh->cmd = OPENPCD_CMD_PIO_IRQ;
+			opcdh->reg = 0x00;
+			opcdh->flags = 0x00;
+			opcdh->val = 0x00;
+
+			irq_rctx->tx.tot_len = sizeof(*opcdh) + sizeof(u_int32_t);
+			req_ctx_set_state(irq_rctx, RCTX_STATE_UDP_EP3_PENDING);
+		}
+	}
+
+	AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_PIOA);
+}
+
+void pio_irq_enable(u_int32_t pio)
+{
+	AT91F_PIO_InterruptEnable(AT91C_BASE_PIOA, pio);
+}
+
+void pio_irq_disable(u_int32_t pio)
+{
+	AT91F_PIO_InterruptDisable(AT91C_BASE_PIOA, pio);
+}
+
+int pio_irq_register(u_int32_t pio, irq_handler_t *handler)
+{
+	u_int8_t num = ffs(pio);
+
+	if (num == 0)
+		return -EINVAL;
+	num--;
+
+	if (pirqs.handlers[num])
+		return -EBUSY;
+
+	pio_irq_disable(pio);
+	AT91F_PIO_CfgInput(AT91C_BASE_PIOA, pio);
+	pirqs.handlers[num] = handler;
+
+	return 0;
+}
+
+void pio_irq_unregister(u_int32_t pio)
+{
+	u_int8_t num = ffs(pio);
+
+	if (num == 0)
+		return;
+	num--;
+
+	pio_irq_disable(pio);
+	pirqs.handlers[num] = NULL;
+}
+
+static int pio_irq_usb_in(struct req_ctx *rctx)
+{
+	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
+	struct openpcd_hdr *pih = (struct openpcd_hdr *) &rctx->tx.data[0];
+
+	switch (poh->cmd) {
+	case OPENPCD_CMD_PIO_IRQ:
+		pirqs.usbmask = poh->val;
+		break;
+	default:
+		DEBUGP("UNKNOWN ");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void pio_irq_init(void)
+{
+	AT91F_PIOA_CfgPMC();
+	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_PIOA,
+			      AT91C_AIC_PRIOR_LOWEST,
+			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &pio_irq_demux);
+	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_PIOA);
+}
diff --git a/firmware/src/os/pio_irq.h b/firmware/src/os/pio_irq.h
new file mode 100644
index 0000000..33f4656
--- /dev/null
+++ b/firmware/src/os/pio_irq.h
@@ -0,0 +1,13 @@
+#ifndef _PIO_IRQ_H
+#define _PIO_IRQ_H
+
+#define NR_PIO 32
+typedef void irq_handler_t(u_int32_t pio);
+
+extern void pio_irq_enable(u_int32_t pio);
+extern void pio_irq_disable(u_int32_t pio);
+extern int pio_irq_register(u_int32_t pio, irq_handler_t *func);
+extern void pio_irq_unregister(u_int32_t pio);
+extern void pio_irq_init(void);
+
+#endif
diff --git a/firmware/src/os/pit.c b/firmware/src/os/pit.c
new file mode 100644
index 0000000..409faef
--- /dev/null
+++ b/firmware/src/os/pit.c
@@ -0,0 +1,38 @@
+
+
+#include <errno.h>
+#include <sys/types.h>
+#include <lib_AT91SAM7.h>
+#include <AT91SAM7.h>
+#include "../openpcd.h"
+
+/* PIT runs at MCK/16 (= 3MHz) */
+#define PIV_MS(x)		(x * 3000)
+
+static void pit_irq(void)
+{
+	/* FIXME: do something */	
+}
+
+void pit_mdelay(u_int32_t ms)
+{
+	u_int32_t end;
+
+	end = (AT91F_PITGetPIIR(AT91C_BASE_PITC) + ms) % 20;
+
+	while (end < AT91F_PITGetPIIR(AT91C_BASE_PITC)) { }
+}
+
+void pit_init(void)
+{
+	AT91F_PITC_CfgPMC();
+
+	AT91F_PITInit(AT91C_BASE_PITC, 1000 /* uS */, 48 /* MHz */);
+
+	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_SYS,
+			      OPENPCD_IRQ_PRIO_PIT,
+			      AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE,
+			      &pit_irq);
+
+	//AT91F_PITEnableInt(AT91C_BASE_PITC);
+}
diff --git a/firmware/src/os/pit.h b/firmware/src/os/pit.h
new file mode 100644
index 0000000..92426e9
--- /dev/null
+++ b/firmware/src/os/pit.h
@@ -0,0 +1,7 @@
+#ifndef _PIT_H
+#define _PIT_H
+
+extern void pit_init(void);
+extern void pit_mdelay(u_int32_t ms);
+
+#endif
diff --git a/firmware/src/os/power.h b/firmware/src/os/power.h
new file mode 100644
index 0000000..bfc6989
--- /dev/null
+++ b/firmware/src/os/power.h
@@ -0,0 +1,8 @@
+#ifndef _POWER_H
+
+static inline void cpu_idle(void)
+{
+	AT91F_PMC_DisablePCK(AT91C_BASE_PMC, AT91C_PMC_PCK);
+}
+
+#endif
diff --git a/firmware/src/os/pwm.c b/firmware/src/os/pwm.c
new file mode 100644
index 0000000..e05699b
--- /dev/null
+++ b/firmware/src/os/pwm.c
@@ -0,0 +1,165 @@
+/* AT91SAM7 PWM routines for OpenPCD / OpenPICC
+ *
+ * (C) 2006 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ */
+
+#include <lib_AT91SAM7.h>
+#include <AT91SAM7.h>
+#include <sys/types.h>
+#include <errno.h>
+#include <openpcd.h>
+#include <os/usb_handler.h>
+#include <os/pcd_enumerate.h>
+#include <os/req_ctx.h>
+#include <os/dbgu.h>
+#include "../openpcd.h"
+
+#define Hz
+#define	kHz	*1000 Hz
+#define MHz	*1000 kHz
+#define MCLK	(48 MHz)
+
+#if 1
+#define DEBUGPWM DEBUGPCRF
+#else
+#define DEBUGPWM(x, args...)
+#endif
+
+static AT91PS_PWMC pwm = AT91C_BASE_PWMC;
+
+/* find highest bit set. returns bit (32..1) or 0 in case no bit set  */
+static int fhs(u_int32_t val)
+{
+	int i;
+
+	for (i = 32; i > 0; i--) {
+		if (val & (1 << (i-1)))
+			return i;
+	}
+
+	return 0;
+}
+
+/* set frequency of PWM signal to freq */
+int pwm_freq_set(int channel, u_int32_t freq)
+{
+	/* in order to get maximum resolution, the pre-scaler must be set to
+	 * something like freq << 16.  However, the mimimum pre-scaled frequency
+	 * we can get is MCLK (48MHz), the minimum is MCLK/(1024*255) =
+	 * 48MHz/261120 = 183Hz */
+	u_int32_t overall_div;
+	u_int32_t presc_total;
+	u_int8_t cpre = 0;
+	u_int16_t cprd;
+
+	if (freq > MCLK)
+		return -ERANGE;
+	
+	overall_div = MCLK / freq;
+	DEBUGPCRF("mclk=%u, freq=%u, overall_div=%u", MCLK, freq, overall_div);
+
+	if (overall_div > 0x7fff) {
+		/* divisor is larger than half the maximum CPRD register, we
+		 * have to configure prescalers */
+		presc_total = overall_div >> 15;
+
+		/* find highest 2^n fitting in prescaler (highest bit set) */
+		cpre = fhs(presc_total);
+		if (cpre > 0) {
+			/* subtract one, because of fhs semantics */
+			cpre--;
+		}
+		cprd = overall_div / (1 << cpre);
+	} else
+		cprd = overall_div;
+	
+	DEBUGPCRF("cpre=%u, cprd=%u", cpre, cprd);
+	AT91F_PWMC_CfgChannel(AT91C_BASE_PWMC, channel, 
+			      cpre|AT91C_PWMC_CPOL, cprd, 1);
+
+	return 0;
+}
+
+void pwm_start(int channel)
+{
+	AT91F_PWMC_StartChannel(AT91C_BASE_PWMC, (1 << channel));
+}
+
+void pwm_stop(int channel)
+{
+	AT91F_PWMC_StopChannel(AT91C_BASE_PWMC, (1 << channel));
+}
+
+void pwm_duty_set_percent(int channel, u_int16_t duty)
+{
+	u_int32_t tmp = pwm->PWMC_CH[channel].PWMC_CPRDR & 0xffff;
+	
+	tmp = tmp << 16;	/* extend value by 2^16 */
+	tmp = tmp / 100;	/* tmp = 1 % of extended cprd */
+	tmp = duty * tmp;	/* tmp = 'duty' % of extended cprd */
+	tmp = tmp >> 16;	/* un-extend tmp (divide by 2^16) */
+
+	DEBUGPWM("Writing %u to Update register\n", tmp);
+	AT91F_PWMC_UpdateChannel(AT91C_BASE_PWMC, channel, tmp);
+}
+
+static int pwm_usb_in(struct req_ctx *rctx)
+{
+	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
+	/* struct openpcd_hdr *pih = (struct openpcd_hdr *) &rctx->tx.data[0]; */
+	u_int32_t *freq;
+
+	switch (poh->cmd) {
+	case OPENPCD_CMD_PWM_ENABLE:
+		if (poh->val)
+			pwm_start(0);
+		else
+			pwm_stop(0);
+		break;
+	case OPENPCD_CMD_PWM_DUTY_SET:
+		pwm_duty_set_percent(0, poh->val);
+		break;
+	case OPENPCD_CMD_PWM_DUTY_GET:
+		goto respond;
+		break;
+	case OPENPCD_CMD_PWM_FREQ_SET:
+		if (rctx->rx.tot_len < sizeof(*poh)+4)
+			break;
+		freq = (unsigned char *) poh + sizeof(*poh);
+		pwm_freq_set(0, *freq);
+		break;
+	case OPENPCD_CMD_PWM_FREQ_GET:
+		goto respond;
+		break;
+	default:
+		break;
+	}
+
+	req_ctx_put(rctx);
+	return 0;
+respond:
+	req_ctx_set_state(rctx, RCTX_STATE_UDP_EP2_PENDING);
+	udp_refill_ep(2, rctx);
+	return 1;
+}
+
+void pwm_init(void)
+{
+	/* IMPORTANT: Disable PA17 (SSC TD) output */
+	AT91F_PIO_CfgInput(AT91C_BASE_PIOA, AT91C_PIO_PA17);
+
+	/* Set PA23 to Peripheral A (PWM0) */
+	AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, 0, OPENPCD_PIO_MFIN_PWM);
+
+	/* Enable Clock for PWM controller */
+	AT91F_PWMC_CfgPMC();
+
+	usb_hdlr_register(&pwm_usb_in, OPENPCD_CMD_CLS_PWM);
+}
+
+void pwm_fini(void)
+{
+	usb_hdlr_unregister(OPENPCD_CMD_CLS_PWM);
+	AT91F_PMC_DisablePeriphClock(AT91C_BASE_PMC, (1 << AT91C_ID_PWMC));
+}
diff --git a/firmware/src/os/pwm.h b/firmware/src/os/pwm.h
new file mode 100644
index 0000000..8836c32
--- /dev/null
+++ b/firmware/src/os/pwm.h
@@ -0,0 +1,11 @@
+#ifndef _PWM_H
+#define _PWM_H
+
+extern void pwm_freq_set(int channel, u_int32_t freq);
+extern void pwm_start(int channel);
+extern void pwm_stop(int channel);
+extern void pwm_duty_set_percent(int channel, u_int16_t duty);
+extern void pwm_init(void);
+extern void pwm_fini(void);
+
+#endif
diff --git a/firmware/src/os/req_ctx.c b/firmware/src/os/req_ctx.c
new file mode 100644
index 0000000..99d248b
--- /dev/null
+++ b/firmware/src/os/req_ctx.c
@@ -0,0 +1,50 @@
+#include <unistd.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <asm/bitops.h>
+#include <os/dbgu.h>
+#include <os/req_ctx.h>
+
+#include "../openpcd.h"
+
+/* FIXME: locking, FIFO order processing */
+
+static struct req_ctx req_ctx[NUM_REQ_CTX];
+
+struct req_ctx *req_ctx_find_get(unsigned long old_state, unsigned long new_state)
+{
+	unsigned long flags;
+	u_int8_t i;
+
+	for (i = 0; i < NUM_REQ_CTX; i++) {
+		local_irq_save(flags);
+		if (req_ctx[i].state == old_state) {
+			req_ctx[i].state = new_state;
+			local_irq_restore(flags);
+			return &req_ctx[i];
+		}
+		local_irq_restore(flags);
+	}
+
+	return NULL;
+}
+
+u_int8_t req_ctx_num(struct req_ctx *ctx)
+{
+	return ((char *)ctx - (char *)&req_ctx[0])/sizeof(*ctx);
+}
+
+void req_ctx_set_state(struct req_ctx *ctx, unsigned long new_state)
+{
+	unsigned long flags;
+
+	/* FIXME: do we need this kind of locking, we're UP! */
+	local_irq_save(flags);
+	ctx->state = new_state;
+	local_irq_restore(flags);
+}
+
+void req_ctx_put(struct req_ctx *ctx)
+{
+	req_ctx_set_state(ctx, RCTX_STATE_FREE);
+}
diff --git a/firmware/src/os/req_ctx.h b/firmware/src/os/req_ctx.h
new file mode 100644
index 0000000..82a133f
--- /dev/null
+++ b/firmware/src/os/req_ctx.h
@@ -0,0 +1,49 @@
+#ifndef _REQ_CTX_H
+#define _REQ_CTX_H
+
+#define MAX_HDRSIZE	sizeof(struct openpcd_hdr)
+#define MAX_REQSIZE	(64-MAX_HDRSIZE)
+
+#define req_buf_payload(x)	(x->data[x->hdr_len])
+#define req_buf_hdr(x)		(x->data[0])
+
+#include <sys/types.h>
+
+struct req_buf {
+	u_int16_t hdr_len;
+	u_int16_t tot_len;
+	u_int8_t data[64];
+};
+
+struct req_ctx {
+	u_int16_t seq;		/* request sequence number */
+	u_int16_t flags;
+	volatile u_int32_t state;
+	struct req_buf rx;
+	struct req_buf tx;
+};
+
+#define RCTX_STATE_FREE			0x00
+#define RCTX_STATE_UDP_RCV_BUSY		0x01
+#define RCTX_STATE_UDP_RCV_DONE		0x02
+#define RCTX_STATE_MAIN_PROCESSING	0x03
+#define RCTX_STATE_RC632IRQ_BUSY	0x04
+
+#define RCTX_STATE_UDP_EP2_PENDING	0x10
+#define RCTX_STATE_UDP_EP2_BUSY		0x11
+
+#define RCTX_STATE_UDP_EP3_PENDING	0x12
+#define RCTX_STATE_UDP_EP3_BUSY		0x13
+
+#define RCTX_STATE_SSC_RX_BUSY		0x20
+
+#define RCTX_STATE_PIOIRQ_BUSY		0x80
+
+#define NUM_REQ_CTX	8
+extern struct req_ctx *req_ctx_find_get(unsigned long old_state, unsigned long new_state);
+extern struct req_ctx *req_ctx_find_busy(void);
+extern void req_ctx_set_state(struct req_ctx *ctx, unsigned long new_state);
+extern void req_ctx_put(struct req_ctx *ctx);
+extern u_int8_t req_ctx_num(struct req_ctx *ctx);
+
+#endif /* _REQ_CTX_H */
diff --git a/firmware/src/os/syscalls.c b/firmware/src/os/syscalls.c
new file mode 100644
index 0000000..ed989f1
--- /dev/null
+++ b/firmware/src/os/syscalls.c
@@ -0,0 +1,169 @@
+/***********************************************************************/
+/*                                                                     */
+/*  SYSCALLS.C:  System Calls                                          */
+/*  most of this is from newlib-lpc and a Keil-demo                    */
+/*                                                                     */
+/*  These are "reentrant functions" as needed by                       */
+/*  the WinARM-newlib-config, see newlib-manual.                       */
+/*  Collected and modified by Martin Thomas                            */
+/*                                                                     */
+/***********************************************************************/
+
+
+#include <stdlib.h>
+#include <reent.h>
+#include <sys/stat.h>
+
+#include <board.h>
+
+static void my_putc(char c) 
+{
+	while (!AT91F_US_TxReady((AT91PS_USART)AT91C_BASE_DBGU));
+	AT91F_US_PutChar((AT91PS_USART)AT91C_BASE_DBGU, c);
+}
+
+static int  my_kbhit( void ) 
+{
+	if ((AT91F_US_RxReady((AT91PS_USART)AT91C_BASE_DBGU)) == 0) return 0;
+	else return 1;
+}
+
+static char my_getc( void )
+{
+	return AT91F_US_GetChar((AT91PS_USART)AT91C_BASE_DBGU);
+}
+
+_ssize_t _read_r(
+    struct _reent *r, 
+    int file, 
+    void *ptr, 
+    size_t len)
+{
+	char c;
+	int  i;
+	unsigned char *p;
+	
+	p = (unsigned char*)ptr;
+	
+	for (i = 0; i < len; i++) {
+		// c = uart0Getch();
+		// c = uart0GetchW();
+		while ( !my_kbhit() ) ;
+		c = (char) my_getc();
+		if (c == 0x0D) {
+			*p='\0';
+			break;
+		}
+		*p++ = c;
+		////// uart0_putc(c);
+	}
+	return len - i;
+}
+
+
+_ssize_t _write_r (
+    struct _reent *r, 
+    int file, 
+    const void *ptr, 
+    size_t len)
+{
+	int i;
+	const unsigned char *p;
+	
+	p = (const unsigned char*) ptr;
+	
+	for (i = 0; i < len; i++) {
+		if (*p == '\n' ) my_putc('\r');
+		my_putc(*p++);
+	}
+	
+	return len;
+}
+
+
+int _close_r(
+    struct _reent *r, 
+    int file)
+{
+	return 0;
+}
+
+
+_off_t _lseek_r(
+    struct _reent *r, 
+    int file, 
+    _off_t ptr, 
+    int dir)
+{
+	return (_off_t)0;	/*  Always indicate we are at file beginning.  */
+}
+
+
+int _fstat_r(
+    struct _reent *r, 
+    int file, 
+    struct stat *st)
+{
+	/*  Always set as character device.				*/
+	st->st_mode = S_IFCHR;
+	/* assigned to strong type with implicit 	*/
+	/* signed/unsigned conversion.  Required by 	*/
+	/* newlib.					*/
+
+	return 0;
+}
+
+
+int isatty(int file); /* avoid warning */
+
+int isatty(int file)
+{
+	return 1;
+}
+
+
+#if 0
+static void _exit (int n) {
+label:  goto label; /* endless loop */
+}
+#endif 
+
+
+/* "malloc clue function" from newlib-lpc/Keil-Demo/"generic" */
+
+/**** Locally used variables. ****/
+// mt: "cleaner": extern char* end;
+extern char end[];              /*  end is set in the linker command 	*/
+				/* file and is the end of statically 	*/
+				/* allocated data (thus start of heap).	*/
+
+static char *heap_ptr;		/* Points to current end of the heap.	*/
+
+/************************** _sbrk_r *************************************
+ * Support function. Adjusts end of heap to provide more memory to
+ * memory allocator. Simple and dumb with no sanity checks.
+
+ *  struct _reent *r -- re-entrancy structure, used by newlib to
+ *                      support multiple threads of operation.
+ *  ptrdiff_t nbytes -- number of bytes to add.
+ *                      Returns pointer to start of new heap area.
+ *
+ *  Note:  This implementation is not thread safe (despite taking a
+ *         _reent structure as a parameter).
+ *         Since _s_r is not used in the current implementation, 
+ *         the following messages must be suppressed.
+ */
+void * _sbrk_r(
+    struct _reent *_s_r, 
+    ptrdiff_t nbytes)
+{
+	char  *base;		/*  errno should be set to  ENOMEM on error  */
+
+	if (!heap_ptr) {	/*  Initialize if first time through.  */
+		heap_ptr = end;
+	}
+	base = heap_ptr;	/*  Point to end of heap.  */
+	heap_ptr += nbytes;	/*  Increase heap.  */
+	
+	return base;		/*  Return pointer to start of new heap area.  */
+}
diff --git a/firmware/src/os/tc_cdiv.c b/firmware/src/os/tc_cdiv.c
new file mode 100644
index 0000000..6c4024c
--- /dev/null
+++ b/firmware/src/os/tc_cdiv.c
@@ -0,0 +1,92 @@
+/* OpenPC TC (Timer / Clock) support code
+ * (C) 2006 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ * This idea of this code is to feed the 13.56MHz carrier clock of RC632
+ * into TCLK1, which is routed to XC1.  Then configure TC0 to divide this
+ * clock by a configurable divider.
+ *
+ */
+
+#include <lib_AT91SAM7.h>
+#include <AT91SAM7.h>
+#include <os/dbgu.h>
+
+#include "../openpcd.h"
+#include <os/tc_cdiv.h>
+
+static AT91PS_TCB tcb = AT91C_BASE_TCB;
+
+/* set carrier divider to a specific */
+void tc_cdiv_set_divider(u_int16_t div)
+{
+	tcb->TCB_TC0.TC_RC = div;
+
+	/* set to 50% duty cycle */
+	tcb->TCB_TC0.TC_RA = 1;
+	tcb->TCB_TC0.TC_RB = 1 + (div >> 1);
+}
+
+void tc_cdiv_phase_add(int16_t inc)
+{
+	tcb->TCB_TC0.TC_RA = (tcb->TCB_TC0.TC_RA + inc) % tcb->TCB_TC0.TC_RC;
+	tcb->TCB_TC0.TC_RB = (tcb->TCB_TC0.TC_RB + inc) % tcb->TCB_TC0.TC_RC;
+
+	/* FIXME: can this be done more elegantly? */
+	if (tcb->TCB_TC0.TC_RA == 0) {
+		tcb->TCB_TC0.TC_RA += 1;
+		tcb->TCB_TC0.TC_RB += 1;
+	}
+}
+
+void tc_cdiv_init(void)
+{
+	/* Cfg PA28(TCLK1), PA0(TIOA0), PA1(TIOB0), PA20(TCLK2) as Periph B */
+	AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, 0, 
+			    OPENPCD_PIO_CARRIER_IN |
+			    OPENPCD_PIO_CARRIER_DIV_OUT |
+			    OPENPCD_PIO_CDIV_HELP_OUT |
+			    OPENPCD_PIO_CDIV_HELP_IN);
+
+	AT91F_PMC_EnablePeriphClock(AT91C_BASE_PMC, 
+				    ((unsigned int) 1 << AT91C_ID_TC0));
+
+	/* Enable Clock for TC0 */
+	tcb->TCB_TC0.TC_CCR = AT91C_TC_CLKEN;
+
+	/* Connect TCLK1 to XC1, TCLK2 to XC2 */
+	tcb->TCB_BMR &= ~(AT91C_TCB_TC1XC1S | AT91C_TCB_TC2XC2S);
+	tcb->TCB_BMR |=  (AT91C_TCB_TC1XC1S_TCLK1 | AT91C_TCB_TC2XC2S_TCLK2);
+
+	/* Clock XC1, Wave mode, Reset on RC comp
+	 * TIOA0 on RA comp = set, * TIOA0 on RC comp = clear,
+	 * TIOB0 on EEVT = set, TIOB0 on RB comp = clear,
+	 * EEVT = XC2 (TIOA0) */
+	tcb->TCB_TC0.TC_CMR = AT91C_TC_CLKS_XC1 | AT91C_TC_WAVE |
+			      AT91C_TC_WAVESEL_UP_AUTO | 
+			      AT91C_TC_ACPA_SET | AT91C_TC_ACPC_CLEAR |
+			      AT91C_TC_BEEVT_SET | AT91C_TC_BCPB_CLEAR |
+			      AT91C_TC_EEVT_XC2 | AT91C_TC_ETRGEDG_RISING;
+
+	tc_cdiv_set_divider(128);
+
+	/* Reset to start timers */
+	tcb->TCB_BCR = 1;
+}
+
+void tc_cdiv_print(void)
+{
+	DEBUGP("TCB_BMR=0x%08x ", tcb->TCB_BMR);
+	DEBUGP("TC0_CV=0x%08x ", tcb->TCB_TC0.TC_CV);
+	DEBUGP("TC0_CMR=0x%08x ", tcb->TCB_TC0.TC_CMR);
+	DEBUGPCR("TC0_SR=0x%08x", tcb->TCB_TC0.TC_SR);
+
+	DEBUGPCR("TC0_RA=0x%04x, TC0_RB=0x%04x, TC0_RC=0x%04x",
+		 tcb->TCB_TC0.TC_RA, tcb->TCB_TC0.TC_RB, tcb->TCB_TC0.TC_RC);
+}
+
+void tc_cdiv_fini(void)
+{
+	tcb->TCB_TC0.TC_CCR = AT91C_TC_CLKDIS;
+	AT91F_PMC_DisablePeriphClock(AT91C_BASE_PMC,
+				     ((unsigned int) 1 << AT91C_ID_TC0));
+}
diff --git a/firmware/src/os/tc_cdiv.h b/firmware/src/os/tc_cdiv.h
new file mode 100644
index 0000000..4f2bc02
--- /dev/null
+++ b/firmware/src/os/tc_cdiv.h
@@ -0,0 +1,26 @@
+#ifndef _TC_CDIV_H
+#define _TC_CDIV_H
+
+#include <sys/types.h>
+#include <lib_AT91SAM7.h>
+
+static AT91PS_TCB tcb;
+
+extern void tc_cdiv_phase_add(int16_t inc);
+extern void tc_cdiv_set_divider(u_int16_t div);
+
+static inline void tc_cdiv_phase_inc(void)
+{
+       tc_cdiv_phase_add(1);
+}
+
+static inline void tc_cdiv_phase_dec(void)
+{
+       tc_cdiv_phase_add(-1);
+}
+
+extern void tc_cdiv_print(void);
+extern void tc_cdiv_init(void);
+extern void tc_cdiv_fini(void);
+
+#endif
diff --git a/firmware/src/os/trigger.c b/firmware/src/os/trigger.c
new file mode 100644
index 0000000..b8cedf3
--- /dev/null
+++ b/firmware/src/os/trigger.c
@@ -0,0 +1,17 @@
+#include <lib_AT91SAM7.h>
+#include <os/trigger.h>
+#include "../openpcd.h"
+
+void trigger_init(void)
+{
+	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_TRIGGER);
+}
+
+void trigger_pulse(void)
+{
+	volatile int i;
+	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, OPENPCD_PIO_TRIGGER);
+	for (i=0; i < 0xff; i++)
+		{ }
+	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, OPENPCD_PIO_TRIGGER);
+}
diff --git a/firmware/src/os/trigger.h b/firmware/src/os/trigger.h
new file mode 100644
index 0000000..597704b
--- /dev/null
+++ b/firmware/src/os/trigger.h
@@ -0,0 +1,7 @@
+#ifndef _TRIGGER_H
+#define _TRIGGER_H
+
+extern void trigger_init(void);
+extern void trigger_pulse(void);
+
+#endif
diff --git a/firmware/src/os/usb_benchmark.c b/firmware/src/os/usb_benchmark.c
new file mode 100644
index 0000000..3637bc3
--- /dev/null
+++ b/firmware/src/os/usb_benchmark.c
@@ -0,0 +1,57 @@
+#include <errno.h>
+#include <string.h>
+#include <lib_AT91SAM7.h>
+#include <os/led.h>
+#include <os/pcd_enumerate.h>
+#include <os/usb_handler.h>
+#include <os/req_ctx.h>
+#include "../openpcd.h"
+
+static struct req_ctx dummy_rctx;
+static struct req_ctx empty_rctx;
+
+static void usbtest_tx_transfer(unsigned int num_pkts)
+{
+	unsigned int i;
+
+	for (i = 0; i < num_pkts; i++) {
+		/* send 16 packets of 64byte */
+		while (udp_refill_ep(2, &dummy_rctx) < 0) 
+			;
+	}
+	/* send one packet of 0 byte */
+	while (udp_refill_ep(2, &empty_rctx) < 0) 
+		;
+}
+
+static int usbtest_rx(struct req_ctx *rctx)
+{
+	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
+	int i;
+
+	switch (poh->cmd) {
+	case OPENPCD_CMD_USBTEST_IN:
+		/* test bulk in pipe */
+		for (i = 0; i < poh->reg; i++) {
+			usbtest_tx_transfer(poh->val);
+			led_toggle(2);
+		}
+		break;
+	case OPENPCD_CMD_USBTEST_OUT:
+		/* test bulk out pipe */
+		break;
+	}
+
+	req_ctx_put(rctx);
+	return 1;
+}
+
+void usbtest_init(void)
+{
+	dummy_rctx.tx.tot_len = 64;
+	memset(dummy_rctx.tx.data, 0x23, 64);
+
+	empty_rctx.tx.tot_len = 0;
+
+	usb_hdlr_register(&usbtest_rx, OPENPCD_CMD_CLS_USBTEST);
+}
diff --git a/firmware/src/os/usb_handler.c b/firmware/src/os/usb_handler.c
new file mode 100644
index 0000000..274353c
--- /dev/null
+++ b/firmware/src/os/usb_handler.c
@@ -0,0 +1,88 @@
+/* OpenPCD USB handler - handle incoming USB requests on OUT pipe
+ * (C) 2006 by Harald Welte
+ */
+
+#include <sys/types.h>
+#include <errno.h>
+#include <string.h>
+
+#include <openpcd.h>
+
+#include <os/pcd_enumerate.h>
+#include <os/usb_handler.h>
+#include <os/req_ctx.h>
+#include <os/led.h>
+#include <os/dbgu.h>
+
+#include "../openpcd.h"
+
+static usb_cmd_fn *cmd_hdlrs[16];
+
+int usb_hdlr_register(usb_cmd_fn *hdlr, u_int8_t class)
+{
+	cmd_hdlrs[class] = hdlr;
+	return 0;
+}
+
+void usb_hdlr_unregister(u_int8_t class)
+{
+	cmd_hdlrs[class] = NULL;
+}
+
+static int usb_in(struct req_ctx *rctx)
+{
+	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
+	struct openpcd_hdr *pih = (struct openpcd_hdr *) &rctx->tx.data[0];
+	usb_cmd_fn *hdlr;
+
+	DEBUGP("usb_in(cls=%d) ", OPENPCD_CMD_CLS(poh->cmd));
+
+	if (rctx->rx.tot_len < sizeof(*poh))
+		return -EINVAL;
+	
+	memcpy(pih, poh, sizeof(*poh));
+	rctx->tx.tot_len = sizeof(*poh);
+
+	hdlr = cmd_hdlrs[OPENPCD_CMD_CLS(poh->cmd)];
+	if (hdlr) 
+		return (hdlr)(rctx);
+	else
+		DEBUGPCR("no handler for this class\n");
+}
+
+/* Process all pending request contexts that want to Tx on either
+ * IN or INTERRUPT endpoint */
+void usb_out_process(void)
+{
+	struct req_ctx *rctx;
+
+	while (rctx = req_ctx_find_get(RCTX_STATE_UDP_EP3_PENDING,
+				       RCTX_STATE_UDP_EP3_BUSY)) {
+		DEBUGPCRF("EP3_BUSY for ctx %u", req_ctx_num(rctx));
+		if (udp_refill_ep(3, rctx) < 0)
+			req_ctx_set_state(rctx, RCTX_STATE_UDP_EP3_PENDING);
+	}
+
+	while (rctx = req_ctx_find_get(RCTX_STATE_UDP_EP2_PENDING,
+				       RCTX_STATE_UDP_EP2_BUSY)) {
+		DEBUGPCRF("EP2_BUSY for ctx %u", req_ctx_num(rctx));
+		if (udp_refill_ep(2, rctx) < 0)
+			req_ctx_set_state(rctx, RCTX_STATE_UDP_EP2_PENDING);
+	}
+}
+
+/* process incoming USB packets (OUT pipe) that have already been 
+ * put into request contexts by the UDP IRQ handler */
+void usb_in_process(void)
+{
+	struct req_ctx *rctx;
+
+	while (rctx = req_ctx_find_get(RCTX_STATE_UDP_RCV_DONE,
+				       RCTX_STATE_MAIN_PROCESSING)) {
+	     	DEBUGPCRF("found used ctx %u: len=%u", 
+			req_ctx_num(rctx), rctx->rx.tot_len);
+		usb_in(rctx);
+	}
+	udp_unthrottle();
+}
+
diff --git a/firmware/src/os/usb_handler.h b/firmware/src/os/usb_handler.h
new file mode 100644
index 0000000..3efcc1f
--- /dev/null
+++ b/firmware/src/os/usb_handler.h
@@ -0,0 +1,17 @@
+#ifndef _USB_HANDLER_H
+#define _USB_HANDLER_H
+
+#include "openpcd.h"
+#include <os/req_ctx.h>
+
+#define MAX_PAYLOAD_LEN	(64 - sizeof(struct openpcd_hdr))
+
+typedef int usb_cmd_fn(struct req_ctx *rctx);
+
+extern int usb_hdlr_register(usb_cmd_fn *hdlr, u_int8_t class);
+extern void usb_hdlr_unregister(u_int8_t class);
+
+extern void usb_in_process(void);
+extern void usb_out_process(void);
+
+#endif
diff --git a/firmware/src/os/wdt.c b/firmware/src/os/wdt.c
new file mode 100644
index 0000000..d8a2145
--- /dev/null
+++ b/firmware/src/os/wdt.c
@@ -0,0 +1,23 @@
+/* AT91SAM7 Watch Dog Timer code for OpenPCD / OpenPICC
+ * (C) 2006 by Harald Welte <hwelte@hmw-consulting.de>
+ */
+
+#define WDT_DEBUG
+ 
+void wdt_irq(void)
+{
+	DEBUGPCRF("================> WATCHDOG EXPIRED !!!!!");
+}
+
+void wdt_init(void)
+{
+#ifdef WDT_DEBUG
+	AT91F_WDTSetMode(AT91C_BASE_WDT, (0xfff << 16) |
+			 AT91C_WDTC_WDDBGHLT | AT91C_WDTC_WDIDLEHLT |
+			 AT91C_WDTC_WDFIEN);
+#else
+	AT91F_WDTSetMode(AT91C_BASE_WDT, (0xfff << 16) |
+			 AT91C_WDTC_WDDBGHLT | AT91C_WDTC_WDIDLEHLT |
+			 AT91C_WDTC_WDRSTEN);
+#endif
+}