#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 handle_setup(const uint32_t* bufp) {
	led_red.toggle();
	
	if(bufp[0] == 0x01000680 && bufp[1] == 0x00080000) {
		OTG_FS.dev_iep_reg[0].DIEPTSIZ = (1 << 19) | 8;
		//                               PKTCNT
		OTG_FS.dev_iep_reg[0].DIEPCTL |= (1 << 31) | (1 << 26);
		//                               EPENA       CNAK
		OTG_FS.fifo = 0x02000112;
		OTG_FS.fifo = 0x400000ff;
	} else if((bufp[0] & 0xffff) == 0x0500) {
		OTG_FS.dev_reg.DCFG |= ((bufp[0] >> 16) & 0x7f) << 4;
		
		OTG_FS.dev_iep_reg[0].DIEPTSIZ = (1 << 19) | 0;
		//                               PKTCNT
		OTG_FS.dev_iep_reg[0].DIEPCTL |= (1 << 31) | (1 << 26);
		//                               EPENA       CNAK
	} else if(bufp[0] == 0x01000680 && bufp[1] == 0x00120000) {
		OTG_FS.dev_iep_reg[0].DIEPTSIZ = (1 << 19) | 18;
		//                               PKTCNT
		OTG_FS.dev_iep_reg[0].DIEPCTL |= (1 << 31) | (1 << 26);
		//                               EPENA       CNAK
		OTG_FS.fifo = 0x02000112;
		OTG_FS.fifo = 0x400000ff;
		OTG_FS.fifo = 0x56781234;
		OTG_FS.fifo = 0x00000000;
		OTG_FS.fifo = 0x00000100;
	} else if(bufp[0] == 0x02000680 && bufp[1] == 0x00040000) {
		OTG_FS.dev_iep_reg[0].DIEPTSIZ = (1 << 19) | 4;
		//                               PKTCNT
		OTG_FS.dev_iep_reg[0].DIEPCTL |= (1 << 31) | (1 << 26);
		//                               EPENA       CNAK
		OTG_FS.fifo = 0x00090209;
	} else if(bufp[0] == 0x02000680 && bufp[1] == 0x00090000) {
		OTG_FS.dev_iep_reg[0].DIEPTSIZ = (1 << 19) | 9;
		//                               PKTCNT
		OTG_FS.dev_iep_reg[0].DIEPCTL |= (1 << 31) | (1 << 26);
		//                               EPENA       CNAK
		OTG_FS.fifo = 0x00090209;
		OTG_FS.fifo = 0xc0000100;
		OTG_FS.fifo = 0x00000000;
	} else {
		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);
	}
}