Added jtag shift control request.

1 files changed, 51 insertions, 0 deletions

diff --git a/main.cpp b/main.cpp
index 327cd90..47c14e2 100644
--- a/main.cpp
+++ b/main.cpp
@@ -17,6 +17,26 @@ static Pin& usb_dm = PA11;
 static Pin& usb_dp = PA12;
 static Pin& usb_vbus = PA9;
 
+static Pin& jtag_tdo = PC3;
+static Pin& jtag_tms = PC13;
+static Pin& jtag_tck = PC14;
+static Pin& jtag_tdi = PC15;
+
+bool jtag_tick(bool tdi, bool tms) {
+	bool tdo = jtag_tdo.get();
+	jtag_tdi.set(tdi);
+	jtag_tms.set(tms);
+	jtag_tck.on();
+	for(uint32_t i = 0; i < 1000; i++) {
+		asm volatile("nop");
+	}
+	jtag_tck.off();
+	for(uint32_t i = 0; i < 1000; i++) {
+		asm volatile("nop");
+	}
+	return tdo;
+}
+
 void usb_write(uint32_t* bufp, uint32_t len) {
 	OTG_FS.dev_iep_reg[0].DIEPTSIZ = (1 << 19) | len;
 	//                               PKTCNT
@@ -52,6 +72,23 @@ bool set_address(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
 	return true;
 }
 
+bool jtag_shift(uint16_t wValue, uint16_t wIndex, uint16_t wLength) {
+	if(wLength > 16) {
+		return false;
+	}
+	
+	uint32_t tdo = 0;
+	
+	for(int16_t i = 0; i < wLength; i++) {
+		tdo |= jtag_tick(wValue & 1, wIndex & 1) ? 1 << i : 0;
+		wValue >>= 1;
+		wIndex >>= 1;
+	}
+	
+	usb_write(&tdo, (wLength + 7) >> 3);
+	return true;
+}
+
 void handle_setup(const uint32_t* bufp) {
 	led_red.toggle();
 	
@@ -75,6 +112,13 @@ void handle_setup(const uint32_t* bufp) {
 		}
 	}
 	
+	// JTAG_SHIFT
+	if(bmRequestType == 0xc0 && bRequest == 0xff) {
+		if(jtag_shift(wValue, wIndex, wLength)) {
+			return;
+		}
+	}
+	
 	OTG_FS.dev_iep_reg[0].DIEPCTL |= (1 << 21);
 }
 
@@ -109,6 +153,7 @@ void interrupt<(Interrupt::IRQ)67>() {
 		OTG_FS.dev_reg.DOEPMSK = (1 << 3) | 1;
 		OTG_FS.dev_reg.DIEPEMPMSK = (1 << 3) | 1;
 		OTG_FS.reg.GRXFSIZ = 256;
+		OTG_FS.reg.DIEPTXF0 = (64 << 16) | 256;
 		OTG_FS.dev_oep_reg[0].DOEPTSIZ = (3 << 29);
 	}
 	
@@ -134,6 +179,7 @@ int main() {
 	
 	RCC.enable(RCC.GPIOA);
 	RCC.enable(RCC.GPIOB);
+	RCC.enable(RCC.GPIOC);
 	RCC.enable(RCC.GPIOD);
 	
 	led_green.set_mode(Pin::Output);
@@ -145,6 +191,11 @@ int main() {
 	led_blue.off();
 	led_yellow.off();
 	
+	jtag_tdi.set_mode(Pin::Output);
+	jtag_tms.set_mode(Pin::Output);
+	jtag_tck.set_mode(Pin::Output);
+	jtag_tdo.set_mode(Pin::Input);
+	
 	usb_vbus.set_mode(Pin::AF);
 	usb_vbus.set_pull(Pin::PullNone);
 	usb_vbus.set_af(10);