#include "stm32.h"
#include "rcc.h"
#include "interrupt.h"
#include "thread.h"
#include "time.h"

#include "pin.h"

#include "usb.h"

static Pin& led_green = PD12;
static Pin& led_yellow = PD13;
static Pin& led_red = PD14;
static Pin& led_blue = PD15;

static Pin& usb_dm = PA11;
static Pin& usb_dp = PA12;
static Pin& usb_vbus = PA9;

void usb_write(uint32_t* bufp, uint32_t len) {
	OTG_FS.dev_iep_reg[0].DIEPTSIZ = (1 << 19) | len;
	//                               PKTCNT
	OTG_FS.dev_iep_reg[0].DIEPCTL |= (1 << 31) | (1 << 26);
	//                               EPENA       CNAK
	
	len = (len + 3) >> 2;
	
	while(len--) {
		OTG_FS.fifo = *bufp++;
	}
}

uint32_t dev_desc[] = {0x02000112, 0x400000ff, 0x56781234, 0x00000000, 0x0100};
uint32_t conf_desc[] = {0x00090209, 0xc0000100, 0x00};

bool get_descriptor(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
	switch(wValue) {
		case 0x100:
			usb_write(dev_desc, wLength);
			return true;
		case 0x200:
			usb_write(conf_desc, wLength);
			return true;
		default:
			return false;
	}
}

bool set_address(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
	OTG_FS.dev_reg.DCFG |= wValue << 4;
	usb_write(0, 0);
	return true;
}

void handle_setup(const uint32_t* bufp) {
	led_red.toggle();
	
	uint8_t bmRequestType = bufp[0] & 0xff;
	uint8_t bRequest = (bufp[0] >> 8) & 0xff;
	uint16_t wValue = (bufp[0] >> 16) & 0xffff;
	uint16_t wIndex = bufp[1] & 0xffff;
	uint16_t wLength = (bufp[1] >> 16) & 0xffff;
	
	// GET_DESCRIPTOR
	if(bmRequestType == 0x80 && bRequest == 0x06) {
		if(get_descriptor(wValue, wIndex, wLength)) {
			return;
		}
	}
	
	// SET_ADDRESS
	if(bmRequestType == 0x00 && bRequest == 0x05) {
		if(set_address(wValue, wIndex, wLength)) {
			return;
		}
	}
	
	OTG_FS.dev_iep_reg[0].DIEPCTL |= (1 << 21);
}

static uint32_t buf[16];

void handle_rxfifo() {
	
	uint32_t status = OTG_FS.reg.GRXSTSP;
	
	uint32_t len = (status & 0x7ff0) >> 6;
	
	for(uint32_t i = 0; i < len; i++) {
		buf[i] = OTG_FS.fifo;
	}
	
	//if(status == 0x000c0080) {
	if((status & (0xf << 17)) == (0x4 << 17)) {
		handle_setup(buf);
		OTG_FS.dev_oep_reg[0].DOEPCTL |= (1 << 26);
	}
}

template<>
void interrupt<(Interrupt::IRQ)67>() {
	//led_red.toggle();
	
	// USB reset.
	if(OTG_FS.reg.GINTSTS & (1 << 12)) {
		led_yellow.toggle();
		OTG_FS.dev_oep_reg[0].DOEPCTL = (1 << 27);
		OTG_FS.dev_reg.DAINTMSK = (1 << 16) | 1;
		OTG_FS.dev_reg.DOEPMSK = (1 << 3) | 1;
		OTG_FS.dev_reg.DIEPEMPMSK = (1 << 3) | 1;
		OTG_FS.reg.GRXFSIZ = 256;
		OTG_FS.dev_oep_reg[0].DOEPTSIZ = (3 << 29);
	}
	
	// OTG interrupt.
	if(OTG_FS.reg.GINTSTS & (1 << 2)) {
		led_blue.toggle();
		
		OTG_FS.reg.GOTGINT = (1 << 2); // SEDET
	}
	
	// RxFIFO non-empty.
	if(OTG_FS.reg.GINTSTS & (1 << 4)) {
		handle_rxfifo();
	}
	
	OTG_FS.reg.GINTSTS = 0xffffffff;
}

int main() {
	// Initialize system timer.
	STK.LOAD = 168000000 / 8 / 1000; // 1000 Hz.
	STK.CTRL = 0x03;
	
	RCC.enable(RCC.GPIOA);
	RCC.enable(RCC.GPIOB);
	RCC.enable(RCC.GPIOD);
	
	led_green.set_mode(Pin::Output);
	led_yellow.set_mode(Pin::Output);
	led_red.set_mode(Pin::Output);
	led_blue.set_mode(Pin::Output);
	
	led_red.off();
	led_blue.off();
	led_yellow.off();
	
	usb_vbus.set_mode(Pin::AF);
	usb_vbus.set_pull(Pin::PullNone);
	usb_vbus.set_af(10);
	usb_dm.set_mode(Pin::AF);
	usb_dm.set_pull(Pin::PullNone);
	usb_dm.set_af(10);
	usb_dp.set_mode(Pin::AF);
	usb_dp.set_pull(Pin::PullNone);
	usb_dp.set_af(10);
	
	RCC.enable(RCC.OTGFS);
	Interrupt::enable((Interrupt::IRQ)67);
	
	OTG_FS.reg.GAHBCFG = 1;
	
	//OTG_FS.reg.GRSTCTL |= 1;
	//while(OTG_FS.reg.GRSTCTL & 1);
	//Time::sleep(10);
	
	// USB configuration
	OTG_FS.reg.GUSBCFG = (1 << 30) | (0xf << 10) | (0 << 9) | (0 << 8);
	//                   FDMOD       TRDT          HNPCAP     SRPCAP

	// interrupt mask
	OTG_FS.reg.GINTMSK = (1 << 13) | (1 << 12) | (1 << 11) | (1 << 10) | (1 << 3) | (1 << 2) | (1 << 1) | (1 << 4);
	//                   ENUMDNEM    USBRST      USBSUSPM    ESUSPM      SOFM       OTGINT     MMISM

	// device configuration
	OTG_FS.dev_reg.DCFG = (1 << 2) | 3;
	//                    NZLSOHSK   DSPD

	// core configuration
	OTG_FS.reg.GCCFG = (1 << 19) | (1 << 16);
	//                 VBUSBSEN    PWRDWN
	
	while(1) {
		led_green.toggle();
		Time::sleep(100);
	}
}