--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/usb.c	Mon Dec 12 23:32:59 2005 +1030
@@ -0,0 +1,852 @@
+#include <stdlib.h>
+#include <avr/io.h>
+#include <avr/pgmspace.h>
+#include <avr/interrupt.h>
+#include <avr/eeprom.h>
+#include <util/delay.h>
+
+#include "usb.h"
+
+#define EP0_SIZE 16
+
+#define D12_MODE_0	0x14	/* Endpoint config = 0, SoftConnect = 1, IRQ Mode = 1,
+				 * Clock running = 0, No Lazy Clock = 0 
+				 */
+#define D12_MODE_1	0x02	/* SOF mode = 0, Set-to-one = 0, Clock div = 2 (16Mhz) */
+/* Debugging stuff */
+void		uart_putsP(const char *addr);
+void		uart_puts(const char *addr);
+int		uart_putc(char c);
+void		uart_puts_dec(uint8_t a, uint8_t l);
+void		uart_puts_hex(uint8_t a);
+
+/* USB administrivia */
+uint8_t deviceaddress;
+uint8_t deviceconfigured;
+
+/* End point buffer points and such */
+const uint8_t *pSendBuffer;
+uint8_t BytesToSend;
+static uint8_t send_Bytes;
+static uint8_t exact_Bytes;
+
+static const uint8_t *pSendBuffer2;
+static uint8_t BytesToSend2;
+
+/* Data packet buffer */
+static uint8_t send_packet2;
+static uint8_t packet2[270];
+static uint16_t packetlen2;
+
+/* XXX: Not actually used */
+void (*bootloader)(void) = (void*)0xe000;
+
+/* Device/endpoint/etc descriptions */
+const USB_DEVICE_DESCRIPTOR DeviceDescriptor = {
+    sizeof(USB_DEVICE_DESCRIPTOR),	/* bLength */
+    TYPE_DEVICE_DESCRIPTOR,		/* bDescriptorType */
+    0x0110,				/* bcdUSB USB Version 1.1 */
+    0,					/* bDeviceClass */
+    0,					/* bDeviceSubclass */
+    0,					/* bDeviceProtocol */
+    EP0_SIZE,				/* bMaxPacketSize in Bytes */
+    0x4753,				/* idVendor (inofficial GS) */
+    0x0001,				/* idProduct */
+    0x0100,				/* bcdDevice */
+    1,					/* iManufacturer String Index */
+    2,					/* iProduct String Index */
+    3,					/* iSerialNumber String Index */
+    1					/* bNumberConfigurations */
+};
+
+const USB_CONFIG_DATA ConfigurationDescriptor = {
+    {	/* configuration descriptor */
+	sizeof(USB_CONFIGURATION_DESCRIPTOR),	/* bLength */
+	TYPE_CONFIGURATION_DESCRIPTOR,		/* bDescriptorType */
+	sizeof(USB_CONFIG_DATA),		/* wTotalLength */
+	2,					/* bNumInterfaces */
+	1,					/* bConfigurationValue */
+	0,					/* iConfiguration String Index */
+	0x80,					/* bmAttributes Bus Powered, No Remote Wakeup */
+	100/2					/* bMaxPower in mA */
+    },
+    {	/* interface descriptor */
+	sizeof(USB_INTERFACE_DESCRIPTOR),	/* bLength */
+	TYPE_INTERFACE_DESCRIPTOR,		/* bDescriptorType */
+	0,					/* bInterface Number */
+	0,					/* bAlternateSetting */
+	2,					/* bNumEndpoints */
+	0xFF,					/* bInterfaceClass (Vendor specific) */
+	0x02,					/* bInterfaceSubClass */
+	0x00,					/* bInterfaceProtocol */
+	0					/* iInterface String Index */
+    },
+    {	/* endpoint descriptor */
+	sizeof(USB_ENDPOINT_DESCRIPTOR),	/* bLength */
+	TYPE_ENDPOINT_DESCRIPTOR,		/* bDescriptorType */
+	0x02,					/* bEndpoint Address EP2 OUT */
+	0x02,					/* bmAttributes - Bulk */
+	0x0040,					/* wMaxPacketSize */
+	0x00					/* bInterval */
+    },
+    {	/* endpoint descriptor */
+	sizeof(USB_ENDPOINT_DESCRIPTOR),	/* bLength */
+	TYPE_ENDPOINT_DESCRIPTOR,		/* bDescriptorType */
+	0x82,					/* bEndpoint Address EP2 IN */
+	0x02,					/* bmAttributes - Bulk */
+	0x0040,					/* wMaxPacketSize */
+	0x00					/* bInterval */
+    },
+    {	/* interface descriptor */
+	sizeof(USB_INTERFACE_DESCRIPTOR),	/* bLength */
+	TYPE_INTERFACE_DESCRIPTOR,		/* bDescriptorType */
+	1,					/* bInterface Number */
+	0,					/* bAlternateSetting */
+	2,					/* bNumEndpoints */
+	0xFF,					/* bInterfaceClass (Vendor specific) */
+	0x02,					/* bInterfaceSubClass */
+	0x00,					/* bInterfaceProtocol */
+	0					/* iInterface String Index */
+    },
+    {	/* endpoint descriptor */
+	sizeof(USB_ENDPOINT_DESCRIPTOR),	/* bLength */
+	TYPE_ENDPOINT_DESCRIPTOR,		/* bDescriptorType */
+	0x01,					/* bEndpoint Address EP1 OUT */
+	0x02,					/* bmAttributes - Bulk */
+	0x0010,					/* wMaxPacketSize */
+	0x00					/* bInterval */
+    },
+    {	/* endpoint descriptor */
+	sizeof(USB_ENDPOINT_DESCRIPTOR),	/* bLength */
+	TYPE_ENDPOINT_DESCRIPTOR,		/* bDescriptorType */
+	0x81,					/* bEndpoint Address EP1 IN */
+	0x02,					/* bmAttributes - Bulk */
+	0x0010,					/* wMaxPacketSize */
+	0x00					/* bInterval */
+    }
+};
+const LANGID_DESCRIPTOR LANGID_Descriptor = {	/* LANGID String Descriptor
+						 * Zero */
+	sizeof(LANGID_DESCRIPTOR),		/* bLength - must match string below */
+	TYPE_STRING_DESCRIPTOR,			/* bDescriptorType */
+	0x0409					/* LANGID US English */
+};
+
+/* These are really STRING_DESCRIPTOR's but we can't statically
+ * declare them as that and have the compiler generate the right size
+ * structure */
+typedef struct {
+	uint8_t bLenght;
+      	uint8_t bDescriptorType;
+      	char     bString[32];
+} MANUFACTURER_DESCRIPTOR, *PMANUFACTURER_DESCRIPTOR;
+
+const MANUFACTURER_DESCRIPTOR Manufacturer_Descriptor = {	/* ManufacturerString 1 */
+    sizeof(MANUFACTURER_DESCRIPTOR),				/* bLength */
+    TYPE_STRING_DESCRIPTOR,					/* bDescriptorType */
+    "G\0e\0n\0e\0s\0i\0s\0 \0S\0o\0f\0t\0w\0a\0r\0e\0" 		/* ManufacturerString in
+								 * UNICODE */
+};
+
+typedef struct {
+	uint8_t bLenght;
+      	uint8_t bDescriptorType;
+      	char     bString[48];
+} PRODUCT_DESCRIPTOR, *PPRODUCT_DESCRIPTOR;
+
+const PRODUCT_DESCRIPTOR Product_Descriptor = {	/* ProductString 2 */
+    sizeof(PRODUCT_DESCRIPTOR),			/* bLength */
+    TYPE_STRING_DESCRIPTOR,			/* bDescriptorType */
+    /* ProductString in
+     * UNICODE */
+    "R\0S\0""-\0""4\0""8\0""5\0"" \0M\0u\0l\0t\0i\0d\0r\0o\0p\0 \0A\0d\0a\0p\0t\0e\0r\0"
+    /* XXX: dunno why I need the double quote magic above.. */
+};
+
+typedef struct {
+	uint8_t bLenght;
+      	uint8_t bDescriptorType;
+      	char     bString[20];
+} SERIAL_DESCRIPTOR, *PSERIAL_DESCRIPTOR;
+
+const SERIAL_DESCRIPTOR EE_Serial_Descriptor __attribute__ ((section (".eeprom"))) = { /* SerialString 3 */
+    sizeof(SERIAL_DESCRIPTOR),		/* bLength - must match string below */
+    TYPE_STRING_DESCRIPTOR,		/* bDescriptorType */
+    "1\02\03\0"
+};
+
+SERIAL_DESCRIPTOR Serial_Descriptor;
+
+/*
+ * The PDIUSBD12 is wired up like so
+ *
+ *  PDI		     AVR	
+ * ======================
+ * D7:0     <=>     PA7:0	   
+ * INT_N     =>     PB0
+ * RD_N     <=      PB1
+ * WR_N     <=      PB2
+ * A0       <=      PB3 (0 = data, 1 = cmd)
+ * SUSPEND  <=>     PB4
+ */
+uint8_t
+d12_get_data(void) {
+    uint8_t data;
+#if 0
+    _delay_us(1);
+#endif
+    PORTB &= ~_BV(PB3);	/* Data phase */
+    DDRA = 0x00;	/* Set to input */
+    PORTB &= ~_BV(PB1);	/* Pull RD_N low */
+    PORTB &= ~_BV(PB1);	/* Delay 40ns */
+    PORTB &= ~_BV(PB1);
+    PORTB &= ~_BV(PB1);
+    data = PINA;	/* Read the data */
+    PORTB |= _BV(PB1);	/* Pull RD_N high */
+    return(data);
+}
+
+void
+set_d12_data(uint8_t data) {
+#if 0
+    _delay_us(1);
+#endif
+    PORTB &= ~_BV(PB3);	/* Data phase */
+    DDRA = 0xff;	/* Set to output */
+    PORTA = data;	/* Put the data on the bus */
+    PORTB &= ~_BV(PB2);	/* Pull WR_N low */
+    PORTB &= ~_BV(PB2);	/* Delay 40ns */
+    PORTB &= ~_BV(PB2);
+    PORTB &= ~_BV(PB2);
+    PORTB |= _BV(PB2);	/* Pull WR_N high */
+    PORTB |= _BV(PB2);	/* Delay 40 ns */
+    PORTB |= _BV(PB2);	 
+    PORTB |= _BV(PB2);	 
+    DDRA = 0x00;	/* Back to input */
+}
+
+void
+set_d12_cmd(uint8_t cmd) {
+#if 0
+    _delay_us(1);
+#endif
+    PORTB |= _BV(PB3);	/* Command phase */
+    DDRA = 0xff;	/* Set to output */
+    PORTA = cmd;	/* Put the data on the bus */
+    PORTB &= ~_BV(PB2);	/* Pull WR_N low */
+    PORTB &= ~_BV(PB2);	/* Delay 40ns */
+    PORTB &= ~_BV(PB2);
+    PORTB &= ~_BV(PB2);
+    PORTB |= _BV(PB2);	/* Pull WR_N high */
+    PORTB |= _BV(PB2);	/* Delay 40ns */
+    PORTB |= _BV(PB2);
+    PORTB |= _BV(PB2);
+    DDRA = 0x00;	/* Back to input */
+}
+
+void 
+d12_write_cmd(uint8_t command, const uint8_t *buffer, uint8_t count) {
+    uint8_t i;
+
+    set_d12_cmd(command);
+#if 0
+    _delay_us(1);
+#endif
+    if (count) {
+	for (i = 0; i < count; i++) {
+	    set_d12_data(buffer[i]);
+	}
+    }
+}
+
+void 
+d12_read_cmd(uint8_t command, uint8_t *buffer, uint8_t count) {
+    uint8_t i;
+
+    set_d12_cmd(command);
+    if (count) {
+	for (i = 0; i < count; i++) {
+	    buffer[i] = d12_get_data();
+	}
+    }
+}
+
+/* Set up the PDIUSBD12 */
+void
+usb_init(void) {
+    uint8_t	buffer[2];
+    
+    /* pull EE_Serial_Descriptor into RAM */
+    eeprom_read_block(&Serial_Descriptor, &EE_Serial_Descriptor, sizeof(Serial_Descriptor));
+
+    /* Set Address to zero (default) and enable function */
+    buffer[0] = 0x80;
+    d12_write_cmd(D12_SET_ADDRESS_ENABLE, buffer, 1);
+
+    /* Enable function generic endpoints */
+    buffer[0] = 0x01;
+    d12_write_cmd(D12_SET_ENDPOINT_ENABLE, buffer, 1);
+ 
+    /* Configure the device (soft connect off) */
+    buffer[0] = D12_MODE_0 & 0xef;
+    buffer[1] = D12_MODE_1;
+    d12_write_cmd(D12_SET_MODE, buffer, 2);
+    
+    /* Delay long enough for the PC to notice the disconnect */
+    _delay_us(1000);
+    
+    buffer[0] |= 0x10; /* Soft connect on */
+    d12_write_cmd(D12_SET_MODE, buffer, 2);
+
+    /* Endpoint 2 IN/OUT IRQ enable */
+    buffer[0] = 0xc0;
+    d12_write_cmd(D12_SET_DMA, buffer, 1);
+}
+
+/* Process an interrupt */
+void
+usb_intr(void) {
+    uint8_t	irq[2];
+    uint8_t	buffer[8];
+    
+    d12_read_cmd(D12_READ_INTERRUPT_REGISTER, (uint8_t *)&irq, 2);
+
+    /* Why do we get interrupts when this is 0? */
+    if (irq[0] == 0)
+	return;
+    
+    if (irq[0] & D12_INT_BUS_RESET) {
+	uart_putsP(PSTR("Bus reset\n\r"));
+	usb_init();
+	return;
+    }
+    
+    if (irq[0] & D12_INT_SUSPEND) {
+	uart_putsP(PSTR("Suspend change\n\r"));
+    }
+	
+    if (irq[0] & D12_INT_EP0_IN) {
+	d12_read_cmd(D12_READ_LAST_TRANSACTION + D12_ENDPOINT_EP0_IN, buffer, 1);
+
+	/* Handle any outgoing data for EP0 */
+	d12_write_buffer_ep0();
+    }
+    
+    /* Handle configuration and misc stuff */
+    if (irq[0] & D12_INT_EP0_OUT) {
+	d12_ep0_irq();
+    }
+    
+    /* EPx_IN is when the host has had a packet of data and is expecting more */
+    if (irq[0] & D12_INT_EP1_IN) {
+	/* XXX: Not yet implemented */
+    }
+    
+    /* EPx_OUT is when we have gotten a packet from the host */
+    if (irq[0] & D12_INT_EP1_OUT) {
+	/* XXX: Not yet implemented */
+    }
+
+    if (irq[0] & D12_INT_EP2_IN) {
+	d12_read_cmd(D12_READ_LAST_TRANSACTION + D12_ENDPOINT_EP2_IN, buffer, 1);
+	d12_send_data_ep2();
+    }
+    
+    if (irq[0] & D12_INT_EP2_OUT) {
+	d12_read_cmd(D12_READ_LAST_TRANSACTION + D12_ENDPOINT_EP2_OUT, buffer, 1);
+	d12_receive_data_ep2();
+    }
+}
+
+void
+usb_gendata(void) {
+    packet2[0] = 'a';
+    packet2[1] = 'b';
+    packet2[2] = 'c';
+    packet2[3] = '\n';
+    packet2[4] = '\r';
+    BytesToSend2 = 5;
+    pSendBuffer2 = (uint8_t *)&packet2[0];
+    send_packet2 = 1;
+
+    /* Kick off the data transfer */
+    d12_send_data_ep2();
+}
+
+void
+d12_ep0_irq(void) {
+    uint8_t		buffer[2];
+    USB_SETUP_REQUEST	setuppkt;
+
+    d12_read_cmd(D12_READ_LAST_TRANSACTION + D12_ENDPOINT_EP0_OUT, buffer, 1);
+
+    if (buffer[0] & D12_LAST_TRAN_SETUP) {
+	/* Read the setup packet */
+	d12_read_endpt(D12_ENDPOINT_EP0_OUT, (uint8_t *)&setuppkt);
+	
+	/* Ack the packet to EP0_OUT */
+	d12_write_cmd(D12_ENDPOINT_EP0_OUT, NULL, 0);
+	d12_write_cmd(D12_ACK_SETUP, NULL, 0);
+	d12_write_cmd(D12_CLEAR_BUFFER, NULL, 0);
+	
+	/* Ack the packet to EP0_IN */
+	d12_write_cmd(D12_ENDPOINT_EP0_IN, NULL, 0);
+	d12_write_cmd(D12_ACK_SETUP, NULL, 0);
+	
+	/* It's a new xfer, so forget about any old one */
+	send_Bytes = 0;
+	
+	/* Parse request type */
+	switch (setuppkt.bmRequestType & 0x7f) {
+	    case STANDARD_DEVICE_REQUEST:
+		switch (setuppkt.bRequest) {
+		    case GET_STATUS:
+			/* Get status request should return remote
+			 * wakeup and self powered status
+			 */
+			buffer[0] = 0x01;
+			buffer[1] = 0x00;
+			d12_write_endpt(D12_ENDPOINT_EP0_IN, buffer, 2);
+			break;
+		    case CLEAR_FEATURE:
+		    case SET_FEATURE:
+			/* We don't support DEVICE_REMOTE_WAKEUP or
+			 * TEST_MODE
+			 */
+			d12_stallctrlendpt();
+			break;
+			
+		    case SET_ADDRESS:
+			deviceaddress = setuppkt.wValue | 0x80;
+			d12_write_cmd(D12_SET_ADDRESS_ENABLE, &deviceaddress, 1);
+			d12_write_endpt(D12_ENDPOINT_EP0_IN, NULL, 0);
+			break;
+
+		    case GET_DESCRIPTOR:	
+			d12_getdescriptor(&setuppkt);
+			break;
+			
+		    case GET_CONFIGURATION:
+			d12_write_endpt(D12_ENDPOINT_EP0_IN, &deviceconfigured, 1);
+			break;
+			
+		    case SET_CONFIGURATION:
+			deviceconfigured = setuppkt.wValue & 0xff;
+			d12_write_endpt(D12_ENDPOINT_EP0_IN, NULL, 0);
+			break;
+			
+
+		    case SET_DESCRIPTOR:
+		    default:
+			/* Unsupported, stall */
+			d12_stallctrlendpt();
+			break;
+		}
+		break;
+		
+	    case STANDARD_INTERFACE_REQUEST:
+		switch (setuppkt.bRequest) {
+		    case GET_STATUS:
+			/* Should return 0, 0 (reserved) */
+			buffer[0] = 0x00;
+			buffer[1] = 0x00;
+			d12_write_endpt(D12_ENDPOINT_EP0_IN, buffer, 2);
+			break;
+			
+		    case SET_INTERFACE:
+			if (setuppkt.wIndex == 0 && setuppkt.wValue == 0)
+			    d12_write_endpt(D12_ENDPOINT_EP0_IN, NULL, 0);
+			else
+			    d12_stallctrlendpt();
+			break;
+
+		    case GET_INTERFACE:
+			/* Can only handle interface 0 ... */
+			if (setuppkt.wIndex == 0) {
+			    buffer[0] = 0;
+			    d12_write_endpt(D12_ENDPOINT_EP0_IN, buffer, 1);
+			    break;
+			}
+			/* .. otherwise fall through to error */
+
+		    case CLEAR_FEATURE:
+		    case SET_FEATURE:
+		    default:
+			d12_stallctrlendpt();
+			break;
+		}
+		break;
+		
+	    case STANDARD_ENDPOINT_REQUEST:
+		switch (setuppkt.bRequest) {
+		    case CLEAR_FEATURE:
+		    case SET_FEATURE:
+			/* Halt(stall) is required to be implemented on
+			 * interrupt and bulk endpoints.
+			 */
+			if (setuppkt.wValue == ENDPOINT_HALT) {
+			    if (setuppkt.bRequest == CLEAR_FEATURE)
+				buffer[0] = 0x00;
+			    else
+				buffer[0] = 0x01;
+			    switch (setuppkt.wIndex & 0xFF) {
+				case 0x01:
+				    d12_write_cmd(D12_SET_ENDPOINT_STATUS + \
+						  D12_ENDPOINT_EP1_OUT, buffer, 1);
+				    break;
+				case 0x81:
+				    d12_write_cmd(D12_SET_ENDPOINT_STATUS + \
+						  D12_ENDPOINT_EP1_IN, buffer, 1);
+				    break;
+				case 0x02:
+				    d12_write_cmd(D12_SET_ENDPOINT_STATUS + \
+						  D12_ENDPOINT_EP2_OUT, buffer, 1);
+				    break;
+				case 0x82:
+				    d12_write_cmd(D12_SET_ENDPOINT_STATUS + \
+						  D12_ENDPOINT_EP2_IN, buffer, 1);
+				    break;
+				default:	/* Invalid Endpoint -
+						 * RequestError */
+				    d12_stallctrlendpt();
+				    break;
+			    }
+			    d12_write_endpt(D12_ENDPOINT_EP0_IN, NULL, 0);
+			} else {
+			    /*
+			     * No other Features for Endpoint -
+			     * Request Error
+			     */
+			    d12_stallctrlendpt();
+			}
+			break;
+
+		    case GET_STATUS:
+			/*
+			 * Get Status Request to Endpoint should
+			 * return Halt Status in D0 for Interrupt and Bulk
+			 */
+			switch (setuppkt.wIndex & 0xFF) {
+			    case 0x01:
+				d12_read_cmd(D12_READ_ENDPOINT_STATUS + \
+					       D12_ENDPOINT_EP1_OUT, buffer, 1);
+				break;
+			    case 0x81:
+				d12_read_cmd(D12_READ_ENDPOINT_STATUS + \
+					       D12_ENDPOINT_EP1_IN, buffer, 1);
+				break;
+			    case 0x02:
+				d12_read_cmd(D12_READ_ENDPOINT_STATUS + \
+					       D12_ENDPOINT_EP2_OUT, buffer, 1);
+				break;
+			    case 0x82:
+				d12_read_cmd(D12_READ_ENDPOINT_STATUS + \
+					       D12_ENDPOINT_EP2_IN, buffer, 1);
+				break;
+			    default:	/* Invalid Endpoint -
+					 * RequestError */
+				d12_stallctrlendpt();
+				break;
+			}
+			if (buffer[0] & 0x08)
+			    buffer[0] = 0x01;
+			else
+			    buffer[0] = 0x00;
+			buffer[1] = 0x00;
+			d12_write_endpt(D12_ENDPOINT_EP0_IN, buffer, 2);
+			break;
+
+		    default:
+			/* Unsupported - Request Error - Stall */
+			d12_stallctrlendpt();
+			break;
+		}
+		break;
+	    case VENDOR_DEVICE_REQUEST:
+	    case VENDOR_ENDPOINT_REQUEST:
+		switch (setuppkt.bRequest) {
+		    case VENDOR_RESET:
+			d12_write_endpt(D12_ENDPOINT_EP0_IN, NULL, 0);
+			_delay_us(1000);
+			/* disconnect from USB */
+			buffer[0] = D12_MODE_0 & 0xef;
+			buffer[1] = D12_MODE_1;
+			d12_write_cmd(D12_SET_MODE, buffer, 2);
+			_delay_us(1000);
+			cli();
+			reset();
+			/* NOT REACHED */
+			break;
+		    case VENDOR_UPDATE:
+			d12_write_endpt(D12_ENDPOINT_EP0_IN, NULL, 0);
+			_delay_us(1000);
+			/* disconnect from USB */
+			buffer[0] = D12_MODE_0 & 0xef;
+			buffer[1] = D12_MODE_1;
+			d12_write_cmd(D12_SET_MODE, buffer, 2);
+			_delay_us(1000);
+			cli();
+			bootloader();
+			/* NOT REACHED */
+			break;
+		    default:
+			d12_stallctrlendpt();
+			break;
+		}
+		break;
+	    case VENDOR_INTERFACE_REQUEST:
+		switch (setuppkt.bRequest) {
+		    default:
+			d12_stallctrlendpt();
+			break;
+		}
+		break;
+	    default:
+		d12_stallctrlendpt();
+		break;
+	}
+    } else {
+	/* This is a Data Packet */
+    }
+		
+}
+
+/* Reset the micro
+static void
+reset(void) {
+    MCUCR = _BV(IVCE);
+    MCUCR = 0;
+    WDTCR = _BV(WDE);
+    for (;;)
+	;
+}
+
+void 
+d12_getdescriptor(USB_SETUP_REQUEST *setuppkt) {
+    switch ((setuppkt->wValue & 0xff00) >> 8) {
+	case TYPE_DEVICE_DESCRIPTOR:
+	    pSendBuffer = (const uint8_t *)&DeviceDescriptor;
+	    BytesToSend = DeviceDescriptor.bLength;
+	    send_Bytes = 1;
+	    if (BytesToSend >= setuppkt->wLength) {
+		BytesToSend = setuppkt->wLength;
+		exact_Bytes = 1;
+	    } else {
+		exact_Bytes = 0;
+	    }
+	    d12_write_buffer_ep0();
+	    break;
+
+	case TYPE_CONFIGURATION_DESCRIPTOR:
+	    pSendBuffer = (const uint8_t *)&ConfigurationDescriptor;
+	    BytesToSend = sizeof(ConfigurationDescriptor);
+	    send_Bytes = 1;
+	    if (BytesToSend >= setuppkt->wLength) {
+		BytesToSend = setuppkt->wLength;
+		exact_Bytes = 1;
+	    } else {
+		exact_Bytes = 0;
+	    }
+	    d12_write_buffer_ep0();
+	    break;
+
+	case TYPE_STRING_DESCRIPTOR:
+	    switch (setuppkt->wValue & 0xFF) {
+
+		case 0:
+		    pSendBuffer = (const uint8_t *)&LANGID_Descriptor;
+		    BytesToSend = sizeof(LANGID_Descriptor);
+		    break;
+
+		case 1:
+		    pSendBuffer = (const uint8_t *)&Manufacturer_Descriptor;
+		    BytesToSend = sizeof(Manufacturer_Descriptor);
+		    break;
+
+		case 2:
+		    pSendBuffer = (const uint8_t *)&Product_Descriptor;
+		    BytesToSend = sizeof(Product_Descriptor);
+		    break;
+
+		case 3:
+		    pSendBuffer = (const uint8_t *)&Serial_Descriptor;
+		    BytesToSend = sizeof(Serial_Descriptor);
+		    break;
+
+		default:
+		    pSendBuffer = NULL;
+		    BytesToSend = 0;
+	    }
+	    send_Bytes = 1;
+	    if (BytesToSend >= setuppkt->wLength) {
+		BytesToSend = setuppkt->wLength;
+		exact_Bytes = 1;
+	    } else {
+		exact_Bytes = 0;
+	    }
+	    d12_write_buffer_ep0();
+	    break;
+	default:
+	    d12_stallctrlendpt();
+	    break;
+    }
+}
+
+
+void 
+d12_stallctrlendpt(void) {
+    uint8_t   Buffer[] = {0x01};
+    /*
+     * 9.2.7 RequestError - return STALL PID in response to next DATA
+     * Stage Transaction
+     */
+    d12_write_cmd(D12_SET_ENDPOINT_STATUS + D12_ENDPOINT_EP0_IN, Buffer, 1);
+    /* or in the status stage of the message. */
+    d12_write_cmd(D12_SET_ENDPOINT_STATUS + D12_ENDPOINT_EP0_OUT, Buffer, 1);
+}
+    
+uint8_t
+d12_read_endpt(uint8_t endpt, uint8_t *buffer) {
+    uint8_t d12header[2];
+    uint8_t status = 0;
+    uint8_t i;
+
+    /* Select Endpoint */
+    d12_read_cmd(endpt, &status, 1);
+
+    /* Check if Buffer is Full */
+    if (status & 0x01) {
+	set_d12_cmd(D12_READ_BUFFER);
+	d12header[0] = d12_get_data();
+	d12header[1] = d12_get_data();
+	if (d12header[1]) {
+	    for (i = 0; i < d12header[1]; i++)
+		buffer[i] = d12_get_data();
+	}
+	/* Allow new packets to be accepted */
+	d12_write_cmd(D12_CLEAR_BUFFER, NULL, 0);
+	
+    }
+    return d12header[1];
+}
+
+void 
+d12_write_endpt(uint8_t endpt, const uint8_t *buffer, uint8_t bytes) {
+    uint8_t status = 0;
+    uint8_t i;
+
+    /* Select Endpoint */
+    d12_read_cmd(endpt, &status, 1);
+    /* Write Header */
+    set_d12_cmd(D12_WRITE_BUFFER);
+    set_d12_data(0x00);
+    set_d12_data(bytes);
+    /* Write Packet */
+    if (bytes) {
+	for (i = 0; i < bytes; i++) {
+	    set_d12_data(buffer[i]);
+	}
+    }
+    /* Validate Buffer */
+    d12_write_cmd(D12_VALIDATE_BUFFER, NULL, 0);
+}
+
+void 
+d12_write_buffer_ep0(void) {
+    uint8_t BufferStatus = 1;
+    
+    if (send_Bytes == 0) {
+	return;
+    }
+	
+    /* Read Buffer Full Status */
+    d12_read_cmd(D12_ENDPOINT_EP0_IN, &BufferStatus, 1);
+    if (BufferStatus != 0) {/* Buffer Full */
+	return;
+    }
+    
+    if (BytesToSend == 0) {
+	/*
+	 * If BytesToSend is Zero and we get called again, assume
+	 * buffer is smaller
+	 * than Setup Request Size and indicate end by sending Zero
+	 * Lenght packet
+	 */
+	d12_write_endpt(D12_ENDPOINT_EP0_IN, NULL, 0);
+	send_Bytes = 0;
+    } else if (BytesToSend >= EP0_SIZE) {	
+	/* Write another EP0_SIZE Bytes to buffer and send */
+	d12_write_endpt(D12_ENDPOINT_EP0_IN, pSendBuffer, EP0_SIZE);
+	pSendBuffer += EP0_SIZE;
+	BytesToSend -= EP0_SIZE;
+	if (BytesToSend == 0 && exact_Bytes) {
+	    send_Bytes = 0;
+	}
+    } else {
+	/* Buffer must have less than EP0_SIZE bytes left */	
+	d12_write_endpt(D12_ENDPOINT_EP0_IN, pSendBuffer, BytesToSend);
+	
+	BytesToSend = 0;
+	send_Bytes = 0;
+    }
+}
+
+void
+d12_send_data_ep2(void) {
+    uint8_t BufferStatus = 1;
+
+    /* Read Buffer Full Status */
+    d12_read_cmd(D12_ENDPOINT_EP2_IN, &BufferStatus, 1);
+    if (BufferStatus == 0) {   /* Buffer Empty */
+	if (BytesToSend2 == 0) {
+	    /* Nothing to do */
+	    send_packet2 = 0;
+	} else if (BytesToSend2 >= 64) {
+	    /* Write another 64 Bytes to buffer and send */
+	    d12_write_endpt(D12_ENDPOINT_EP2_IN, pSendBuffer2, 64);
+	    pSendBuffer2 += 64;
+	    BytesToSend2 -= 64;
+	} else {
+	    /* Buffer must have less than 64 bytes left */
+	    d12_write_endpt(D12_ENDPOINT_EP2_IN, pSendBuffer2, BytesToSend2);
+	    BytesToSend2 = 0;
+	    send_packet2 = 0;
+	}
+    }
+}
+
+void
+d12_receive_data_ep2(void) {
+    uint8_t D12Header[2];
+    uint8_t bytes;
+    uint8_t BufferStatus = 0;
+    uint8_t i;
+
+    /* Select Endpoint */
+    d12_read_cmd(D12_ENDPOINT_EP2_OUT, &BufferStatus, 1);
+    /* Check if Buffer is Full */
+    if (BufferStatus & 0x01) {
+	/* Read header */
+	set_d12_cmd(D12_READ_BUFFER);
+	D12Header[0] = d12_get_data();
+	D12Header[1] = d12_get_data();
+	bytes = D12Header[1];
+	/* TODO check if the packet is not too long */
+	uart_putsP(PSTR("Got data\n\r"));
+	for (i = 0; i < bytes; i++) {
+	    packet2[packetlen2 + i] = d12_get_data();
+	    uart_puts_hex(packet2[packetlen2 + i]);
+	    uart_putsP(PSTR(" "));
+	}
+	uart_putsP(PSTR("\n\r"));
+	packetlen2 += bytes;
+	/* Allow new packets to be accepted */
+	d12_write_cmd(D12_CLEAR_BUFFER, NULL, 0);
+	if (bytes == 0 || bytes < 64) { /* request complete */
+	    pSendBuffer2 = packet2;
+	    send_packet2 = 1;
+	    packetlen2 = 0;
+	    d12_send_data_ep2();
+	}
+    }
+}