Added support for multiple FIFOs.

1 files changed, 11 insertions, 11 deletions

diff --git a/main.cpp b/main.cpp
index 9337c20..ca9237f 100644
--- a/main.cpp
+++ b/main.cpp
@@ -39,16 +39,16 @@ bool jtag_tick(bool tdi, bool tms) {
 	return tdo;
 }
 
-void usb_write(uint32_t* bufp, uint32_t len) {
-	OTG_HS.dev_iep_reg[0].DIEPTSIZ = (1 << 19) | len;
+void usb_write(uint32_t ep, uint32_t* bufp, uint32_t len) {
+	OTG_HS.dev_iep_reg[ep].DIEPTSIZ = (1 << 19) | len;
 	//                               PKTCNT
-	OTG_HS.dev_iep_reg[0].DIEPCTL |= (1 << 31) | (1 << 26);
+	OTG_HS.dev_iep_reg[ep].DIEPCTL |= (1 << 31) | (1 << 26);
 	//                               EPENA       CNAK
 	
 	len = (len + 3) >> 2;
 	
 	while(len--) {
-		OTG_HS.fifo = *bufp++;
+		OTG_HS.fifo[ep].reg = *bufp++;
 	}
 }
 
@@ -65,10 +65,10 @@ uint8_t conf_desc[] = {
 bool get_descriptor(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
 	switch(wValue) {
 		case 0x100:
-			usb_write((uint32_t*)dev_desc, wLength > sizeof(dev_desc) ? sizeof(dev_desc) : wLength);
+			usb_write(0, (uint32_t*)dev_desc, wLength > sizeof(dev_desc) ? sizeof(dev_desc) : wLength);
 			return true;
 		case 0x200:
-			usb_write((uint32_t*)conf_desc, wLength > sizeof(conf_desc) ? sizeof(conf_desc) : wLength);
+			usb_write(0, (uint32_t*)conf_desc, wLength > sizeof(conf_desc) ? sizeof(conf_desc) : wLength);
 			return true;
 		default:
 			return false;
@@ -77,7 +77,7 @@ bool get_descriptor(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
 
 bool set_address(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
 	OTG_HS.dev_reg.DCFG |= wValue << 4;
-	usb_write(0, 0);
+	usb_write(0, 0, 0);
 	return true;
 }
 
@@ -86,7 +86,7 @@ bool set_configuration(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
 		return false;
 	}
 	
-	usb_write(0, 0);
+	usb_write(0, 0, 0);
 	return true;
 }
 
@@ -103,7 +103,7 @@ bool jtag_shift(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
 		wIndex >>= 1;
 	}
 	
-	usb_write(&tdo, (wLength + 7) >> 3);
+	usb_write(0, &tdo, (wLength + 7) >> 3);
 	return true;
 }
 
@@ -111,7 +111,7 @@ bool i2c_read(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
 	uint8_t buf[wLength];
 	I2C2.read_reg(wValue, wIndex, wLength, buf);
 	
-	usb_write((uint32_t*)buf, wLength);
+	usb_write(0, (uint32_t*)buf, wLength);
 	return true;
 }
 
@@ -172,7 +172,7 @@ void handle_rxfifo() {
 	uint32_t len = (status & 0x7ff0) >> 6;
 	
 	for(uint32_t i = 0; i < len; i++) {
-		buf[i] = OTG_HS.fifo;
+		buf[i] = OTG_HS.fifo[0].reg;
 	}
 	
 	//if(status == 0x000c0080) {