#include <xc.h>
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <sys/attribs.h>
#include <assert.h>
#include "io.h"
#include "dat.h"
#include "fns.h"
#include "usbif.h"
#include "usbdesc.h"

#define dprintf printf

static enum {
	USBRESET,
	USBADDRESS,
	USBCONFIG
} usbstate;
static uint8_t selfpower;
static uint8_t configuration;
enum { MAXCONFIG = 1 };

enum { MAXPKT = 64 };


enum {
	RTTOHOST = 0x80,
	RTTYPE = 0x60,
	RTSTANDARD = 0,
	RTCLASS = 0x20,
	RTVENDOR = 0x40,
	RTRECIP = 0x1f,
	RTRDEVICE = 0,
	RTRIFACE = 1,
	RTRENDP = 2,
	RTROTHER = 3,
};

enum {
	Get_Status = 0,
	Clear_Feature = 1,
	Set_Feature = 3,
	Set_Address = 5,
	Get_Descriptor = 6,
	Set_Descriptor = 7,
	Get_Configuration = 8,
	Set_Configuration = 9,
	Get_Interface = 10,
	Set_Interface = 11,
	Synch_Frame = 12,
};

USBQueue msgq = {.maxq = 4096};
USBQueue corrq = {.maxq = 16384};

USBEp eptx[8], eprx[8];
void (*poststatus)(void);

enum { USBMEMSZ = 1<<12 };
static int
usballoc(int sz)
{
	static int loc = 16;
	int rv;
	
	if(loc + sz > USBMEMSZ)
		panic("out of USB device memory");
	rv = loc;
	loc += sz;
	return rv;
}

static void
bulkep(int devep, int targep, int mult, USBQueue *queue)
{
	if(devep < 1 || devep >= 8 || (targep & 0x70) != 0){
		printf("bulkep(%d,%#.2x,%d,%p): invalid arguments\n", devep, targep, mult, queue);
		return;
	}
	if((targep & 0x80) != 0){
		assert(eptx[devep].targep == 0);
		eptx[devep].devep = devep;
		eptx[devep].targep = targep;
		eptx[devep].fifosz = (mult + 1) * 512;
		eptx[devep].fifoad = usballoc((mult + 1) * (512 / 8));
		eptx[devep].q = queue;
		queue->ep = &eptx[devep];
	}else{
		assert(eprx[devep].targep == 0);
		eprx[devep].devep = devep;
		eprx[devep].targep = targep;
		eprx[devep].fifosz = (mult + 1) * 512;
		eprx[devep].fifoad = usballoc((mult + 1) * (512 / 8));
		eprx[devep].q = queue;
		queue->ep = &eprx[devep];
	}
}

static void
resetep0(void)
{
	USBCSR3bits.ENDPOINT = 0;
	USBE0CSR0 = 0;
	USBFIFOAbits.RXFIFOAD = 0;
	USBFIFOAbits.TXFIFOAD = 8;
	USBOTGbits.TXFIFOSZ = 3;
	USBOTGbits.RXFIFOSZ = 3;
	USBCSR1bits.EP0IE = 1;
}

static void
resetbulkep(USBEp *ep)
{
	int mult, sz, i;
	
	mult = ep->fifosz / 512 - 1;
	sz = 6;
	for(i = mult + 1; i > 1; i >>= 1)
		sz++;
	
	USBCSR3bits.ENDPOINT = ep->devep;
	if((ep->targep & 0x80) != 0){
		USBIENCSR0 = mult << 11 | 512;
		USBIENCSR2 = 2 << 20 | (ep->targep & 0xf) << 16;
		USBOTGbits.TXFIFOSZ = sz;
		USBFIFOAbits.TXFIFOAD = ep->fifoad;
		USBCSR1 |= 1<<8 + ep->devep;
	}else{
		USBIENCSR1 = mult << 11 | 512;
		USBIENCSR3 = 1 << 6 | 2 << 4 | ep->targep & 0xf;
		USBOTGbits.RXFIFOSZ = sz;
		USBFIFOAbits.RXFIFOAD = ep->fifoad;
		USBCSR1 |= 1<<ep->devep;
	}
	USBCSR3bits.ENDPOINT = 0;
}

static void
usbreset(void)
{
	int i;

	printf("USB: reset\n");
	usbstate = USBRESET;
	USBCSR0bits.FUNC = 0;
	configuration = 0;
	
	resetep0();
	for(i = 1; i < 8; i++){
		if(eptx[i].targep != 0)
			resetbulkep(&eptx[i]);
		if(eprx[i].targep != 0)
			resetbulkep(&eprx[i]);
	}
}

static void
ep0stall(void)
{
	dprintf("USB: replying with stall\n");
	USBE0CSR0bits.STALL = 1;
}

static void
mkusbreq(USBReq *r, uint8_t *d)
{
	free(r->data);
	memset(r, 0, sizeof(*r));
	r->bmRequestType = d[0];
	r->bRequest = d[1];
	r->wValue = d[2] | d[3] << 8;
	r->wIndex = d[4] | d[5] << 8;
	r->wLength = d[6] | d[7] << 8;
	if(r->wLength != 0){
		r->data = malloc(r->wLength);
		assert(r->data != NULL);
	}
}

static int
ep0_Get_Configuration(USBReq *r)
{
	dprintf("USB: Get_Configuration()\n");
	if(r->bmRequestType != 0x80 || r->wValue != 0 || r->wIndex != 0 || r->wLength != 1)
		return -1;
	r->data[0] = configuration;
	return 1;
}

static void kickall(void);

static int
ep0_Set_Configuration(USBReq *r)
{
	dprintf("USB: Set_Configuration(%d)\n", r->wValue);
	if(r->bmRequestType != 0 || r->wIndex != 0 || r->wLength != 0 || r->wValue > MAXCONFIG)
		return -1;
	configuration = r->wValue;
	if(r->wValue == 0) usbstate = USBADDRESS;
	else{
		usbstate = USBCONFIG;
		kickall();
	}
	return 0;
}

static int
ep0_Get_Descriptor(USBReq *r)
{
	const uint8_t *d;
	int dlen;

	dprintf("USB: Get_Descriptor(%d, %d)\n", r->wValue >> 8, r->wValue & 0xff);	
	if(r->bmRequestType != 0x80)
		return -1;
	switch(r->wValue){
	case 1 << 8: d = devdesc; dlen = devdesc[0]; break;
	case 2 << 8: d = confdesc; dlen = *(uint16_t*)&confdesc[2]; break;
	case 3 << 8: d = langids; dlen = langids[0]; break;
	case 3 << 8 | 1: d = vendstr; dlen = vendstr[0]; break;
	case 3 << 8 | 2: d = prodstr; dlen = prodstr[0]; break;
	default: return -1;
	}
	if(r->wLength < dlen) dlen = r->wLength;
	memcpy(r->data, d, dlen);
	return dlen;
}

static int
ep0_Get_Interface(USBReq *r)
{
	dprintf("USB: Get_Interface(%d)\n", r->wIndex);
	if(r->bmRequestType != 0x81 || r->wValue != 0 || r->wIndex != 0 || r->wLength != 1 || usbstate != USBCONFIG)
		return -1;
	r->data[0] = 0;
	return 1;
}

static int
ep0_Get_Status(USBReq *r)
{
	uint16_t rv;
	
	dprintf("USB: Get_Status(%d)\n", r->bmRequestType & RTRECIP);
	if((int8_t) r->bmRequestType > -0x7e || r->wValue != 0 || r->wLength != 2)
		return -1;
	switch(r->bmRequestType & RTRECIP){
	case RTRDEVICE:
		if(r->wIndex != 0) return -1;
		rv = selfpower & 1;
		break;
	case RTRIFACE:
		if(r->wIndex != 0) return -1;
		rv = 0;
		break;
	case RTRENDP:
		if(r->wIndex != 0) return -1;
		rv = 0;
		break;
	}
	*(uint16_t*)r->data = rv;
	return 2;
}

static int8_t addrchange;

static void
ep0_Set_Address_Complete(void)
{
	USBCSR0bits.FUNC = addrchange;
	usbstate = USBADDRESS;
}

static int
ep0_Set_Address(USBReq *r)
{
	dprintf("USB: Set_Address(%d)\n", r->wValue);
	if(r->bmRequestType != 0 || r->wIndex != 0 || r->wLength != 0 || (uint16_t)(r->wValue - 1) > 126)
		return -1;
	addrchange = r->wValue;
	poststatus = ep0_Set_Address_Complete;
	return 0;
}

static int
ep0_Program_Mode(USBReq *r)
{
	if(r->bmRequestType != RTVENDOR || r->wIndex != 0 || r->wValue != 0 || r->wLength != 0)
		return -1;
	RTCCONbits.CAL = 0x15A;
	poststatus = reboot;
	return 0;
}

typedef int EP0Handler(USBReq *);
static EP0Handler * const ep0stdhandler[] = {
	[Get_Configuration] ep0_Get_Configuration,
	[Set_Configuration] ep0_Set_Configuration,
	[Get_Descriptor] ep0_Get_Descriptor,
	[Get_Interface] ep0_Get_Interface,
	[Get_Status] ep0_Get_Status,
	[Set_Address] ep0_Set_Address
};

EP0Handler ep0_qspi_status, ep0_qspi_erase, ep0_qspi_program, ep0_qspi_read, ep0_Send_Cmd, ep0_Cmd_Status, ep0_Corr_Data, ep0_FPGA_Read, ep0_FPGA_Write, ep0_Raw_Start, ep0_Raw_Stop;
static EP0Handler * const ep0vhandler[] = {
	[Send_Cmd] ep0_Send_Cmd,
	[Cmd_Status] ep0_Cmd_Status,
	[Corr_Data] ep0_Corr_Data,
	[FPGA_Read] ep0_FPGA_Read,
	[FPGA_Write] ep0_FPGA_Write,
	[QSPI_Status] ep0_qspi_status,
	[QSPI_Erase] ep0_qspi_erase,
	[QSPI_Program] ep0_qspi_program,
	[QSPI_Read] ep0_qspi_read,
	[Program_Mode] ep0_Program_Mode,
	[Raw_Start] ep0_Raw_Start,
	[Raw_Stop] ep0_Raw_Stop
};

static int
ep0req(USBReq *r)
{
	dprintf("USB: bmRequestType=%.2x bRequest=%.2x wValue=%.4x wIndex=%.4x wLength=%.4x\n", r->bmRequestType, r->bRequest, r->wValue, r->wIndex, r->wLength);
	switch(r->bmRequestType & RTTYPE){
	case RTSTANDARD:
		if(r->bRequest >= nelem(ep0stdhandler) || ep0stdhandler[r->bRequest] == NULL)
			return -1;
		return ep0stdhandler[r->bRequest](r);
	case RTVENDOR:
		if(r->bRequest >= nelem(ep0vhandler) || ep0vhandler[r->bRequest] == NULL)
			return -1;
		return ep0vhandler[r->bRequest](r);
	default:
		return -1;
	}
}

static void
fifoput(int ep, void *dv, int len)
{
	volatile void *r;
	uint8_t *d;
	
	r = &USBFIFO0 + ep;
	d = dv;
	while(len >= 4){
		*(uint32_t*)r = *(uint32_t*)d;
		d += 4;
		len -= 4;
	}
	if(len >= 2){
		*(uint16_t*)r = *(uint16_t*)d;
		d += 2;
		len -= 2;
	}
	if(len == 1)
		*(uint8_t*)r = *d;
	
}

static enum {
	EP0RESET,
	EP0SETUP,
	EP0TXDATA,
	EP0TXDATAL,
	EP0TXSTATUS,
	EP0RXDATA,
	EP0ZLEN,
} ep0state;

static void
ep0txkick(USBReq *r)
{
	int n;
	
	assert(r->ptr >= 0 && r->ptr <= r->len);
	if(USBE0CSR0bits.TXRDY){
		printf("USB: tried to write to full FIFO\n");
		return;
	}
	n = r->len - r->ptr;
	if(n > MAXPKT) n = MAXPKT;
	//dprintf("USB: TX: sending %d\n", n);
	fifoput(0, r->data + r->ptr, n);
	r->ptr += n;
	if(n == MAXPKT && r->ptr < r->wLength){
		ep0state = EP0TXDATA;
		USBE0CSR0bits.TXRDY = 1;
	}else{
		ep0state = EP0TXDATAL;
		USBE0CSR0 |= _USBE0CSR0_TXRDY_MASK | _USBE0CSR0_DATAEND_MASK;
	}
	
}

static void
usbep0(void)
{
	int nb, i, rc;
	static uint8_t d[MAXPKT];
	uint32_t v;
	static USBReq r;

	if(ep0state == EP0RESET){
		ep0state = EP0SETUP;
		poststatus = NULL;
	}
	if(USBE0CSR0bits.SETEND){
		USBE0CSR0bits.SETENDC = 1;
		printf("USB: setup ended prematurely\n");
		if(ep0state != EP0ZLEN)
			ep0state = EP0SETUP;
	}
	if(USBE0CSR0bits.STALLED){
		USBE0CSR0bits.STALLED = 0;
		return;
	}
	switch(ep0state){
	case EP0TXDATA:
		ep0txkick(&r);
		return;
	case EP0TXDATAL:
		dprintf("USB: TX: status done\n");
		if(0){
	case EP0TXSTATUS:
	case EP0ZLEN:
			dprintf("USB: sent status word\n");
		}
		if(poststatus != NULL){
			poststatus();
			poststatus = NULL;
		}
		ep0state = EP0SETUP;
		return;
	}
	if(!USBE0CSR0bits.RXRDY) return;
	nb = USBE0CSR2bits.RXCNT;
	if(nb > MAXPKT){
		printf("USB: received oversized packet, %d > %d", nb, MAXPKT);
		ep0stall();
		ep0state = EP0SETUP;
		return;
	}
	for(i = 0; i < nb; i += 4){
		v = USBFIFO0;
		if(i < MAXPKT)
			*(uint32_t*)(d + i) = v;
	}
	switch(ep0state){
	case EP0SETUP:
		if(nb != 8){
			printf("USB: setup packet has the wrong size, %d != 8\n", nb);
			USBE0CSR0bits.FLUSH = 1;
			return;
		}
		mkusbreq(&r, d);

		if(r.wLength == 0){
			if(ep0req(&r) >= 0){
				USBE0CSR0 |= _USBE0CSR0_RXRDYC_MASK | _USBE0CSR0_DATAEND_MASK;
				ep0state = EP0ZLEN;
			}else
				ep0stall();
		}else if((r.bmRequestType & RTTOHOST) == 0){
			USBE0CSR0bits.RXRDYC = 1;
			ep0state = EP0RXDATA;
		}else{
			rc = ep0req(&r);
			if(rc >= 0){
				r.len = rc;
				USBE0CSR0bits.RXRDYC = 1;
				ep0txkick(&r);
			}else
				ep0stall();
		}
		break;
	case EP0RXDATA:
		//dprintf("USB: RX: got %d bytes\n", nb);
		if(r.ptr + nb > r.wLength){
			printf("USB: received too much data during control transfer\n");
			ep0stall();
			ep0state = EP0SETUP;
		}
		memcpy(r.data + r.ptr, d, nb);
		r.ptr += nb;
		if(nb < MAXPKT || r.ptr == r.wLength){
			r.len = r.wLength;
			r.wLength = r.ptr;
			if(ep0req(&r) >= 0){
				USBE0CSR0bits.RXRDYC = 1;
				USBE0CSR0bits.DATAEND = 1;
				ep0state = EP0TXSTATUS;
			}else{
				ep0stall();
				ep0state = EP0SETUP;
			}
		}else
			USBE0CSR0bits.RXRDYC = 1;
		break;
	default:
		printf("RX in state %d\n", ep0state);
		ep0stall();
		ep0state = EP0SETUP;
	}
}

static void
qpop(USBQueue *q)
{
	int s;
	USBPkt *p;
	
	s = splhi();
	p = q->first;
	if(p == NULL) goto out;
	q->first = p->next;
	if(q->nextp == &p->next)
		q->nextp = &q->first;
	q->qalloc -= p->len + sizeof(USBPkt);
	free(p);
out:	splx(s);
}

static void
txkick(USBQueue *q)
{
	int s;
	int n;
	volatile void *fifo;
	USBPkt *p;
	USBEp *ep;

	if(q == NULL) return;
	s = splhi();
	ep = q->ep;
	if(ep == NULL || usbstate != USBCONFIG || ((&USBE0CSR0)[4*ep->devep] & _USBE1CSR0_TXPKTRDY_MASK) != 0 || q->first == NULL) goto out;
	fifo = &USBFIFO0 + ep->devep;
	p = q->first;
	for(n = 0; n < ep->fifosz; n++){
		if(p->ptr >= p->len){
			qpop(q);
			if(p = q->first, p == NULL) break;
		}
		*(uint8_t*)fifo = p->data[p->ptr++];
	}
	if(n != 0)
		(&USBE0CSR0)[4*ep->devep] |= _USBE1CSR0_TXPKTRDY_MASK;
out:
	splx(s);
}

static void
kickall(void)
{
	int i;
	
	for(i = 1; i < 8; i++)
		txkick(eptx[i].q);
}

static void
usbepntxirq(USBEp *ep)
{
	txkick(ep->q);
}

static void
usbepnrxirq(USBEp *ep)
{
	dprintf("EP%d RX interrupt\n", ep->devep);
	if(ep->targep == 0) return;
}

void __ISR(_USB_VECTOR, USBIPL)
usbirq(void)
{
	uint32_t csr0, csr1, csr2;
	int i;
	
	csr0 = USBCSR0;
	csr1 = USBCSR1;
	csr2 = USBCSR2;
	IFS4CLR = _IFS4_USBIF_MASK;
	if((csr0 & _USBCSR0_EP0IF_MASK) != 0)
		usbep0();
	if((csr0 & 0xfe0000) != 0)
		for(i = 1; i < 8; i++)
			if((csr0 & 1<<16+i) != 0)
				usbepntxirq(&eptx[i]);
	if((csr1 & 0xfe) != 0)
		for(i = 1; i < 8; i++)
			if((csr1 & 1<<16+i) != 0)
				usbepnrxirq(&eprx[i]);
	if((csr2 & _USBCSR2_RESETIF_MASK) != 0)
		usbreset();
}

void
usbinit(void)
{
	bulkep(1, 0x81, 0, &msgq);
	bulkep(2, 0x82, 0, &corrq);

	USBCRCONbits.USBIDOVEN = 1;
	USBCRCONbits.USBIDVAL = 1;
	USBCRCONbits.USBIE = 1;
	
	USBCSR0bits.HSEN = 1;
	USBCSR2bits.RESETIE = 1;
	
	IPC33bits.USBIP = USBIRQPRI;
	IPC33bits.USBIS = USBIRQSUB;
	IPC33bits.USBDMAIP = USBDMAIRQPRI;
	IPC33bits.USBDMAIS = USBDMAIRQSUB;
	IEC4bits.USBIE = 1;
	IEC4bits.USBDMAIE = 1;
	
	USBCSR0bits.SOFTCONN = 1;
}

int
qwrite(USBQueue *q, const void *data, int len)
{
	int n, s;
	USBPkt *p;
	int over;
	
	s = splhi();
	n = sizeof(USBPkt) + len;
	assert(n <= q->maxq);
	q->qalloc += n;
	over = 0;
	while(q->qalloc >= q->maxq){
		qpop(q);
		over++;
	}
	p = malloc(n);
	assert(p != NULL);
	p->len = len;
	p->ptr = 0;
	p->next = NULL;
	memcpy(p->data, data, len);
	if(q->nextp == NULL)
		q->first = p;
	else
		*q->nextp = p;
	q->nextp = &p->next;
	txkick(q);
	splx(s);
	return over;
}