#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();
	}
    }
}