From: dwhuseby <dwhuseby@12e2690d-a6be-4b82-a7b7-67c4a43b65c8>
Date: Tue, 8 Feb 2011 18:32:27 +0000 (+0000)
Subject: merging...
X-Git-Url: http://git.rot13.org/?p=goodfet;a=commitdiff_plain;h=cfbda5b069e5c792b4b3a91911c3fc9844b68fbc

merging...
cleaned up basic jtag, added ability to detect JTAG chain length, get chip ID's and detect overall IR length.




git-svn-id: https://svn.code.sf.net/p/goodfet/code/trunk@899 12e2690d-a6be-4b82-a7b7-67c4a43b65c8
---

diff --git a/firmware/Makefile b/firmware/Makefile
index d0bffc7..a4ffb43 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -1,6 +1,6 @@
 
 #include `uname`.mak
-#GOODFET?=/dev/ttyUSB0
+GOODFET?=/dev/ttyUSB0
 
 #For tos-bsl, use --invert-reset --invert-test
 BSL?=goodfet.bsl --speed=38400
@@ -9,9 +9,9 @@ BSL?=goodfet.bsl --speed=38400
 #Default removed because of confusion.
 
 #mcu=msp430x2274
-#mcu=msp430x2618
+mcu=msp430x2618
 #mcu?=msp430x1612
-mcu?=RUNCONFIG
+#mcu?=RUNCONFIG
 
 #platform=goodfet
 #platform=telosb
@@ -90,12 +90,11 @@ CC=msp430-gcc -Wall -Os -g -mmcu=$(mcu) -D$(mcu) -D$(platform) -Dplatform=$(plat
 # config = monitor jtagxscale
 
 # Old Default Config
-#config = monitor sbw  chipcon nrf ccspi spi jtagarm7 jtag430 jtag430x2 avr
+# config = monitor sbw  chipcon nrf ccspi spi jtagarm7 jtag430 jtag430x2 avr
 
 # New default.
 config ?= monitor chipcon ccspi nrf spi jtagarm7 jtag430 jtag430x2 avr
 
-
 # Build the needed list of app and lib object files from the config
 apps= 
 libs= lib/$(mcu).o lib/command.o lib/dco_calib.o lib/apps.o
@@ -230,8 +229,6 @@ ifeq ($(filter avr, $(config)), avr)
 	hdrs+= avr.h
 endif
 
-
-
 # include pic app
 ifeq ($(filter pic, $(config)), pic)
 	apps+= apps/pic/pic.o
@@ -311,7 +308,7 @@ verify:
 dumpinfo:
 	$(BSL) --dumpinfo
 $(app).c: config builddate appsfiles err
-$(app):  $(libs) $(apps)
+$(app): $(libs) $(apps)
 $(app).hex: $(app)
 	msp430-objcopy goodfet -O ihex goodfet.hex
 m4s: $(app).hex
diff --git a/firmware/apps/jtag/ejtag.c b/firmware/apps/jtag/ejtag.c
index 6bf6c8d..7c50275 100644
--- a/firmware/apps/jtag/ejtag.c
+++ b/firmware/apps/jtag/ejtag.c
@@ -38,7 +38,7 @@ void ejtag_handle_fn( uint8_t const app,
 	switch(verb)
 	{
 	case START:
-		cmddata[0] = jtag_ir_shift8(EJTAG_IR_BYPASS);
+		cmddata[0] = jtag_ir_shift_8(EJTAG_IR_BYPASS);
 		txdata(app, verb, 1);
 		break;
 	case STOP:
@@ -49,6 +49,6 @@ void ejtag_handle_fn( uint8_t const app,
 	case POKE:
 		//WRITEME
 	default:
-		(*(ejtag_app.handle))(app, verb, len);
+		(*(jtag_app.handle))(app, verb, len);
 	}
 }
diff --git a/firmware/apps/jtag/jtag.c b/firmware/apps/jtag/jtag.c
index bda0fac..b813d5d 100644
--- a/firmware/apps/jtag/jtag.c
+++ b/firmware/apps/jtag/jtag.c
@@ -38,292 +38,511 @@ app_t const jtag_app = {
 };
 
 
+//! Remembers what the current JTAG state is
+enum eTAPState jtag_state = UNKNOWN;
+
+//! Returns true if we're in any of the data register states
+int in_dr()
+{
+	return (int)(jtag_state & (SELECT_DR_SCAN | CAPTURE_DR |
+							   SHIFT_DR | EXIT1_DR | PAUSE_DR |
+							   EXIT2_DR | UPDATE_DR));
+}
+
+//! Returns true if we're in any of the instruction register states
+int in_ir()
+{
+	return (int)(jtag_state & (SELECT_IR_SCAN | CAPTURE_IR |
+							   SHIFT_IR | EXIT1_IR | PAUSE_IR |
+							   EXIT2_IR | UPDATE_IR));
+}
+
+//! Returns true if we're in run-test-idle state
+int in_run_test_idle()
+{
+	return (int)(jtag_state & RUN_TEST_IDLE);
+}
+
+//! Check the state
+int in_state(enum eTAPState state)
+{
+	return (int)(jtag_state & state);
+}
+
+
+//! Reset the target device
+void jtag_reset_target()
+{
+	SETRST;
+	PLEDOUT^=PLEDPIN; 
+	CLRRST;
+	PLEDOUT^=PLEDPIN; 
+}
+
+//! Clock the JTAG clock line
+void jtag_tcktock() 
+{
+	CLRTCK; 
+	PLEDOUT^=PLEDPIN; 
+	SETTCK; 
+	PLEDOUT^=PLEDPIN;
+}
+
+//! Goes through test-logic-reset and ends in run-test-idle
+void jtag_reset_tap()
+{
+	CLRMOSI;
+	SETTMS;
+	jtag_tcktock();
+	jtag_tcktock();
+	jtag_tcktock();
+	jtag_tcktock();
+	jtag_tcktock();  // now in Reset state
+	CLRTMS;
+	jtag_tcktock();  // now in Run-Test/Idle state
+	jtag_state = RUN_TEST_IDLE;
+}
+
 //! Set up the pins for JTAG mode.
-void jtagsetup(){
-  P5DIR|=MOSI+SCK+TMS;
-  P5DIR&=~MISO;
-  /*
-  P5OUT|=0xFFFF;
-  P5OUT=0;
-  */
-  P4DIR|=TST;
-  P2DIR|=RST;
-  msdelay(100);
+void jtag_setup()
+{
+	P5DIR|=MOSI+SCK+TMS;
+	P5DIR&=~MISO;
+	P4DIR|=TST;
+	P2DIR|=RST;
+	msdelay(100);
+	jtag_state = UNKNOWN;
 }
 
-/************************** JTAG Primitives ****************************/
-// these have been turned into functions to save flash space
-void jtag_tcktock() {
-  CLRTCK; 
-  PLEDOUT^=PLEDPIN; 
-  SETTCK; 
-  PLEDOUT^=PLEDPIN;
+//! Stop JTAG, release pins
+void jtag_stop()
+{
+	P5OUT=0;
+	P4OUT=0;
 }
 
-void jtag_goto_shift_ir() {
-  SETTMS;
-  jtag_tcktock();
-  jtag_tcktock();
-  CLRTMS;
-  jtag_tcktock();
-  jtag_tcktock();
+//! Get into Shift-IR or Shift-DR state
+void jtag_shift_register()
+{
+	// assumes we're in any state that can transition to Shift-IR or Shift-DR
+	if (!in_state(CAPTURE_DR | CAPTURE_IR | SHIFT_DR | 
+				  SHIFT_IR | EXIT2_DR | EXIT2_IR ))
+	{
+		debugstr("Invalid JTAG state");
+		return;
+	}
+
+	CLRMOSI;
+	CLRTMS;
+	jtag_tcktock();
 
+	if (in_dr())
+		jtag_state = SHIFT_DR;
+	else
+		jtag_state = SHIFT_IR;
 }
-void jtag_goto_shift_dr() {
-  SETTMS;
-  jtag_tcktock();
-  CLRTMS;
-  jtag_tcktock();
-  jtag_tcktock();
+
+//! Get into Capture-IR state
+void jtag_capture_ir()
+{
+	// assumes you're in Run-Test/Idle, Update-DR or Update-IR
+	if (!in_state(RUN_TEST_IDLE | UPDATE_DR | UPDATE_IR))
+	{
+		debugstr("Invalid JTAG state");
+		return;
+	}
+
+	CLRMOSI;
+	SETTMS;
+	jtag_tcktock(); // Select-DR-Scan
+	jtag_tcktock(); // Select-IR-Scan
+	CLRTMS;
+	jtag_tcktock(); // Capture-IR
+
+	jtag_state = CAPTURE_IR;
 }
 
-void jtag_resettap(){
-  SETTMS;
-  jtag_tcktock();
-  jtag_tcktock();
-  jtag_tcktock();
-  jtag_tcktock();
-  jtag_tcktock();  // now in Reset state
-  CLRTMS;
-  jtag_tcktock();  // now in Run-Test/Idle state
+//! Get into Capture-DR state
+void jtag_capture_dr()
+{
+	// assumes you're in Run-Test/Idle, Update-DR or Update-IR
+	if (!in_state(RUN_TEST_IDLE | UPDATE_DR | UPDATE_IR))
+	{
+		debugstr("Invalid JTAG state");
+		return;
+	}
+
+	CLRMOSI;
+	SETTMS;
+	jtag_tcktock(); // Select-DR-Scan
+	CLRTMS;
+	jtag_tcktock(); // Capture-IR
+
+	jtag_state = CAPTURE_DR;
 }
 
+//! Gets back to run-test-idle without going through the test-logic-reset
+void jtag_run_test_idle()
+{
+	CLRMOSI;
+
+	if (in_state(SELECT_DR_SCAN | SELECT_IR_SCAN))
+	{
+		CLRTMS;
+		jtag_tcktock();
+		jtag_state <<= 1; //CAPTURE_DR or CAPTURE_IR
+	}
+
+	if (in_state(CAPTURE_DR | CAPTURE_IR))
+	{
+		SETTMS;
+		jtag_tcktock();
+		jtag_state <<= 2; //EXIT1_DR or EXIT1_IR
+	}
+
+	if (in_state(SHIFT_DR | SHIFT_IR))
+	{
+		SETTMS;
+		jtag_tcktock();
+		jtag_state <<= 1; //EXIT1_DR or EXIT1_IR
+	}
+
+	if (in_state(EXIT1_DR | EXIT1_IR))
+	{
+		SETTMS;
+		jtag_tcktock();
+		jtag_state <<=3; //UPDATE_DR or UPDATE_IR
+	}
+
+	if (in_state(PAUSE_DR | PAUSE_IR))
+	{
+		SETTMS;
+		jtag_tcktock();
+		jtag_state <<= 1; // EXIT2_DR or EXIT2_IR
+	}
+
+	if (in_state(EXIT2_DR | EXIT2_IR))
+	{
+		SETTMS;
+		jtag_tcktock();
+		jtag_state <<= 1; // UPDATE_DR or UPDATE_IR
+	}
+
+	if (in_state(UPDATE_DR | UPDATE_IR | TEST_LOGIC_RESET))
+	{
+		CLRTMS;
+		jtag_tcktock();
+		jtag_state = RUN_TEST_IDLE;
+	}
+}
 
-int savedtclk=0;
-//  NOTE: important: THIS MODULE REVOLVES AROUND RETURNING TO RUNTEST/IDLE, OR 
-//  THE FUNCTIONAL EQUIVALENT
-//! Shift N bits over TDI/TDO.  May choose LSB or MSB, and select whether to 
-//  terminate (TMS-high on last bit) and whether to return to RUNTEST/IDLE
-//      flags should be 0 for most uses.  
-//      for the extreme case, flags should be  (NOEND|NORETDLE|LSB)
-//      other edge cases can involve a combination of those three flags
+int savedtclk;
+//	NOTE: important: THIS MODULE REVOLVES AROUND RETURNING TO RUNTEST/IDLE, OR 
+//	THE FUNCTIONAL EQUIVALENT
+//! Shift N bits over TDI/TDO.	May choose LSB or MSB, and select whether to 
+//	terminate (TMS-high on last bit) and whether to return to RUNTEST/IDLE
+//		flags should be 0 for most uses.  
+//		for the extreme case, flags should be  (NOEND|NORETDLE|LSB)
+//		other edge cases can involve a combination of those three flags
 //
-//      the max bit-size that can be be shifted is 32-bits.  
-//      for longer shifts, use the NOEND flag (which infers NORETIDLE so the 
-//      additional flag is unnecessary)
+//		the max bit-size that can be be shifted is 32-bits.  
+//		for longer shifts, use the NOEND flag (which infers NORETIDLE so the 
+//		additional flag is unnecessary)
 //
-//      NORETIDLE is used for special cases where (as with arm) the debug 
-//      subsystem does not want to return to the RUN-TEST/IDLE state between 
-//      setting IR and DR
-unsigned long jtagtransn(unsigned long word, 
-                         unsigned char bitcount, 
-                         unsigned char flags) {
-  unsigned char bit;
-  unsigned long high = 1L;
-  unsigned long mask;
-
-  //for (bit=(bitcount-1)/8; bit>0; bit--)
-  //  high <<= 8;
-  //high <<= ((bitcount-1)%8);
-  high <<= (bitcount-1);
-
-  mask = high-1;
-
-  SAVETCLK;
-  if (flags & LSB) {
-    for (bit = bitcount; bit > 0; bit--) {
-      /* write MOSI on trailing edge of previous clock */
-      if (word & 1)
-        {SETMOSI;}
-      else
-        {CLRMOSI;}
-      word >>= 1;
-
-      if (bit==1 && !(flags & NOEND))
-        SETTMS;//TMS high on last bit to exit.
-       
-      jtag_tcktock();
-
-      /* read MISO on trailing edge */
-      if (READMISO){
-        word += (high);
-      }
-    }
-  } else {
-    for (bit = bitcount; bit > 0; bit--) {
-      /* write MOSI on trailing edge of previous clock */
-      if (word & high)
-        {SETMOSI;}
-      else
-        {CLRMOSI;}
-      word = (word & mask) << 1;
-
-      if (bit==1 && !(flags & NOEND))
-        SETTMS;//TMS high on last bit to exit.
-
-      jtag_tcktock();
-
-      /* read MISO on trailing edge */
-      word |= (READMISO);
-    }
-  }
- 
-
-  RESTORETCLK;
-  //SETMOSI;
-
-  if (!(flags & NOEND)){
-    // exit state
-    jtag_tcktock();
-    // update state
-    if (!(flags & NORETIDLE)){
-      CLRTMS;
-      jtag_tcktock();
-    }
-  }
-  return word;
-}
+//		NORETIDLE is used for special cases where (as with arm) the debug 
+//		subsystem does not want to return to the RUN-TEST/IDLE state between 
+//		setting IR and DR
+uint32_t jtag_trans_n(uint32_t word, 
+					  uint8_t bitcount, 
+					  enum eTransFlags flags) 
+{
+	uint8_t bit;
+	uint32_t high = (1L << (bitcount - 1));
+	uint32_t mask = high - 1;
 
-//! Shift 8 bits in and out.
-unsigned char jtagtrans8(unsigned char byte){
-  unsigned int bit;
-  SAVETCLK;
-  for (bit = 0; bit < 8; bit++) {
-    /* write MOSI on trailing edge of previous clock */
-    if (byte & 0x80)
-      {SETMOSI;}
-    else
-      {CLRMOSI;}
-    byte <<= 1;
-    
-    if(bit==7)
-      SETTMS;//TMS high on last bit to exit.
-    
-    TCKTOCK;
-    /* read MISO on trailing edge */
-    byte |= READMISO;
-  }
-  RESTORETCLK;
-  
-  // exit state
-  TCKTOCK;
-  // update state
-  CLRTMS;
-  TCKTOCK;
-  
-  return byte;
-}
+	if (!in_state(SHIFT_IR | SHIFT_DR))
+	{
+		debugstr("jtag_trans_n from invalid TAP state");
+		return 0;
+	}
+
+	SAVETCLK;
+
+	if (flags & LSB) 
+	{
+		for (bit = bitcount; bit > 0; bit--) 
+		{
+			/* write MOSI on trailing edge of previous clock */
+			if (word & 1)
+			{
+				SETMOSI;
+			}
+			else
+			{
+				CLRMOSI;
+			}
+			word >>= 1;
+
+			if ((bit == 1) && !(flags & NOEND))
+				SETTMS; //TMS high on last bit to exit.
+
+			jtag_tcktock();
+
+			if ((bit == 1) && !(flags & NOEND))
+				jtag_state <<= 1; // Exit1-DR or Exit1-IR
+
+			/* read MISO on trailing edge */
+			if (READMISO)
+			{
+				word += (high);
+			}
+		}
+	} 
+	else 
+	{
+		for (bit = bitcount; bit > 0; bit--) 
+		{
+			/* write MOSI on trailing edge of previous clock */
+			if (word & high)
+			{
+				SETMOSI;
+			}
+			else
+			{
+				CLRMOSI;
+			}
+			word = (word & mask) << 1;
+
+			if ((bit==1) && !(flags & NOEND))
+				SETTMS; //TMS high on last bit to exit.
+
+			jtag_tcktock();
+
+			if ((bit == 1) && !(flags & NOEND))
+				jtag_state <<= 1; // Exit1-DR or Exit1-IR
+
+			/* read MISO on trailing edge */
+			word |= (READMISO);
+		}
+	}
+
+	RESTORETCLK;
+
+	if (!(flags & NOEND))
+	{
+		// exit state
+		jtag_tcktock();
 
-//! Shift n bits in and out.
-/*unsigned long jtagtransn(unsigned long word,
-			 unsigned int bitcount){
-  unsigned int bit;
-  //0x8000
-  unsigned long high=0x8000;
-  
-  if(bitcount==20)
-    high=0x80000;
-  if(bitcount==16)
-    high= 0x8000;
-  
-  SAVETCLK;
-  
-  for (bit = 0; bit < bitcount; bit++) {
-    // write MOSI on trailing edge of previous clock *
-    if (word & high)
-      {SETMOSI;}
-    else
-      {CLRMOSI;}
-    word <<= 1;
-    
-    if(bit==bitcount-1)
-      SETTMS;//TMS high on last bit to exit.
-    
-    TCKTOCK;
-    // read MISO on trailing edge *
-    word |= READMISO;
-  }
-  
-  if(bitcount==20){
-    word = ((word << 16) | (word >> 4)) & 0x000FFFFF;
-  }
-  
-  RESTORETCLK;
-  
-  // exit state
-  TCKTOCK;
-  // update state
-  CLRTMS;
-  TCKTOCK;
-  
-  return word;
+		jtag_state <<= 3; // Update-DR or Update-IR
+
+		// update state
+		if (!(flags & NORETIDLE))
+		{
+			CLRTMS;
+			jtag_tcktock();
+
+			jtag_state = RUN_TEST_IDLE;
+		}
+	}
+
+	return word;
 }
-*/
 
-//! Stop JTAG, release pins
-void jtag_stop(){
-  P5OUT=0;
-  P4OUT=0;
+//! Detects the width of the IR register
+uint16_t jtag_detect_ir_width()
+{
+	int i;
+	uint16_t width = 0;
+
+	if (!in_run_test_idle())
+	{
+		debugstr("Not in run-test-idle state");
+		return 0;
+	}
+
+	// get to shift-ir state
+	jtag_capture_ir();
+	jtag_shift_register();
+
+	// first we shift in 1024 zeros
+	CLRMOSI;
+	for(i = 0; i < 1024; i++)
+	{
+		jtag_tcktock();
+	}
+
+	// now we'll clock in ones until we see one
+	SETMOSI;
+	for(i = 0; i < 1024; i++)
+	{
+		jtag_tcktock();
+		if(READMISO)
+			break;
+		width++;
+	}
+
+	// now get back to run-test-idle
+	jtag_run_test_idle();
+
+	return width;
 }
 
-unsigned int drwidth=16;
-//! Shift all bits of the DR.
-unsigned long jtag_dr_shift20(unsigned long in){
-  // idle
-  SETTMS;
-  TCKTOCK;
-  // select DR
-  CLRTMS;
-  TCKTOCK;
-  // capture IR
-  TCKTOCK;
-  
-  // shift DR, then idle
-  return(jtagtransn(in,20,0));
+//! Detects how many TAPs are in the JTAG chain
+uint16_t jtag_detect_chain_length()
+{
+	int i;
+	int bit;
+	uint16_t length = 0;
+
+	if (!in_run_test_idle())
+	{
+		debugstr("detect chain length must start from run-test-idle");
+		return 0;
+	}
+
+	// The standard JTAG instruction for "bypass" mode is to set all 1's in the
+	// instruction register.  When in "bypass" mode, the DR acts like a 1-bit
+	// shift regiser.  So we can use that to detect how many TAPs are in the 
+	// chain.
+	
+	// first get into shift IR mode
+	jtag_capture_ir();
+	jtag_shift_register();
+
+	// then we flood the IR chain with all 1's to put all device's DRs
+	// into bypass mode
+	CLRTMS;
+	SETMOSI;
+	for (i = 0; i < 1024; i++)
+	{
+		if (i == 1023)
+			SETTMS; // exit on last bit
+		jtag_tcktock();
+	}
+	jtag_state = EXIT1_IR;
+
+	// go to Update-IR
+	CLRMOSI;
+	jtag_tcktock();
+	jtag_state = UPDATE_IR;
+
+	// go to Shift-DR state
+	jtag_capture_dr();
+	jtag_shift_register();
+
+	// flush the DR's with zeros
+	CLRTMS;
+	CLRMOSI;
+	for (i = 0; i < 1024; i++)
+	{
+		jtag_tcktock();
+	}
+
+	// send 1's into the DRs until we see one come out the other side
+	SETMOSI;
+	for (i = 0; i < 1024; i++)
+	{
+		jtag_tcktock();
+		bit = READMISO;
+		if (bit)
+			break;
+		length++;
+	}
+
+	// now get back to run-test-idle
+	jtag_run_test_idle();
+
+	return length;
 }
 
+//! Gets device ID for specified chip in the chain
+uint32_t jtag_get_device_id(int chip)
+{
+	int i;
+	uint16_t chain_length;
+	uint32_t id = 0;
+
+	// reset everything
+	jtag_reset_tap();
 
-//! Shift 16 bits of the DR.
-unsigned int jtag_dr_shift16(unsigned int in){
-  // idle
-  SETTMS;
-  TCKTOCK;
-  // select DR
-  CLRTMS;
-  TCKTOCK;
-  // capture IR
-  TCKTOCK;
-  
-  // shift DR, then idle
-  return(jtagtransn(in,16,0));
+	// figure out how many devices are in the chain
+	chain_length = jtag_detect_chain_length();
+
+	if (chip >= chain_length)
+	{
+		debugstr("invalid part index");
+		return 0;
+	}
+
+	// reset everything again because going through test-logic-reset forces
+	// all IR's to have their manufacturer specific instruction for IDCODE
+	// and loads the DR's with the chip's ID code.
+	jtag_reset_tap();
+
+	// get into shift DR state
+	jtag_capture_dr();
+	jtag_shift_register();
+
+	// read out the 32-bit ID codes for each device
+	CLRTMS;
+	CLRMOSI;
+	for (i = 0; i < (chip + 1); i++)
+	{
+		id = jtag_trans_n(0xFFFFFFFF, 32, LSB | NOEND | NORETIDLE);
+	}
+
+	jtag_run_test_idle();
+
+	return id;
 }
 
-//! Shift native width of the DR
-unsigned long jtag_dr_shiftadr(unsigned long in){
-  unsigned long out=0;
-  
-  // idle
-  SETTMS;
-  TCKTOCK;
-  // select DR
-  CLRTMS;
-  TCKTOCK;
-  // capture IR
-  TCKTOCK;
-
-  
-  out=jtagtransn(in,drwidth,0);
-  
-  // shift DR, then idle
-  return(out);
+//! Shift 8 bits in/out of selected register
+uint8_t jtag_trans_8(uint8_t in)
+{
+	uint32_t out = jtag_trans_n((uint32_t)in, 8, MSB);
+	return (uint8_t)(0x000000FF & out);
 }
 
+//! Shift 16 bits in/out of selected register
+uint16_t jtag_trans_16(uint16_t in)
+{
+	uint32_t out = jtag_trans_n((uint32_t)in, 16, MSB);
+	return (uint16_t)(0x0000FFFF & out);
+}
 
 //! Shift 8 bits of the IR.
-unsigned char jtag_ir_shift8(unsigned char in){
-  // idle
-  SETTMS;
-  TCKTOCK;
-  // select DR
-  TCKTOCK;
-  // select IR
-  CLRTMS;
-  TCKTOCK;
-  // capture IR
-  TCKTOCK;
-  
-  // shift IR, then idle.
-  return(jtagtrans8(in));
+uint8_t jtag_ir_shift_8(uint8_t in)
+{
+	if (!in_run_test_idle())
+	{
+		debugstr("Not in run-test-idle state");
+		return 0;
+	}
+
+	// get intot the right state
+	jtag_capture_ir();
+	jtag_shift_register();
+
+	// shift IR bits
+	return jtag_trans_8(in);
+}
+
+//! Shift 16 bits of the DR.
+uint16_t jtag_dr_shift_16(uint16_t in)
+{
+
+	if (!in_run_test_idle())
+	{
+		debugstr("Not in run-test-idle state");
+		return 0;
+	}
+
+	// get intot the right state
+	jtag_capture_dr();
+	jtag_shift_register();
+
+	// shift DR, then idle
+	return jtag_trans_16(in);
 }
 
 //! Handles a monitor command.
@@ -333,32 +552,56 @@ void jtag_handle_fn(uint8_t const app,
 {
 	switch(verb)
 	{
-		//START handled by specific JTAG
+	// START handled by specific JTAG
 	case STOP:
 		jtag_stop();
 		txdata(app,verb,0);
 		break;
 
 	case SETUP:
-		jtagsetup();
+		jtag_setup();
 		txdata(app,verb,0);
 		break;
 
 	case JTAG_IR_SHIFT:
-		cmddata[0]=jtag_ir_shift8(cmddata[0]);
+		cmddata[0] = jtag_ir_shift_8(cmddata[0]);
 		txdata(app,verb,1);
 		break;
 
 	case JTAG_DR_SHIFT:
-		cmddataword[0]=jtag_dr_shift16(cmddataword[0]);
+		cmddataword[0] = htons(jtag_dr_shift_16(ntohs(cmddataword[0])));
 		txdata(app,verb,2);
 		break;
 
-	case JTAG_RESETTAP:
-		jtag_resettap();
+	case JTAG_RESET_TAP:
+		jtag_reset_tap();
 		txdata(app,verb,0);
 		break;
 
+	case JTAG_RESET_TARGET:
+		jtag_reset_tap();
+		jtag_reset_target();
+		txdata(app,verb,0);
+		break;
+
+	case JTAG_DETECT_IR_WIDTH:
+		jtag_reset_tap();
+		cmddataword[0] = htons(jtag_detect_ir_width());
+		txdata(app,verb,2);
+		break;
+
+	case JTAG_DETECT_CHAIN_LENGTH:
+		jtag_reset_tap();
+		cmddataword[0] = htons(jtag_detect_chain_length());
+		txdata(app,verb,2);
+		break;
+
+	case JTAG_GET_DEVICE_ID:
+		jtag_reset_tap();
+		cmddatalong[0] = htonl(jtag_get_device_id(ntohs(cmddataword[0])));
+		txdata(app,verb,4);
+		break;
+
 	default:
 		txdata(app,NOK,0);
 	}
diff --git a/firmware/apps/jtag/jtag430.c b/firmware/apps/jtag/jtag430.c
index 2338847..edf18ec 100644
--- a/firmware/apps/jtag/jtag430.c
+++ b/firmware/apps/jtag/jtag430.c
@@ -30,24 +30,43 @@ app_t const jtag430_app = {
 
 unsigned int jtag430mode=MSP430X2MODE;
 
+unsigned int drwidth=16;
+
+//! Shift an address width of data
+uint32_t jtag430_shift_addr( uint32_t addr )
+{
+	if (!in_run_test_idle())
+	{
+		debugstr("Not in run-test-idle state");
+		return 0;
+	}
+
+	// get intot the right state
+	jtag_capture_dr();
+	jtag_shift_register();
+
+	// shift DR, then idle
+	return jtag_trans_n(addr, drwidth, MSB);
+}
+
 //! Set a register.
 void jtag430_setr(u8 reg, u16 val){
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x3401);// release low byte
-  jtag_ir_shift8(IR_DATA_16BIT);
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x3401);// release low byte
+  jtag_ir_shift_8(IR_DATA_16BIT);
   
   //0x4030 is "MOV #foo, r0"
   //Right-most field is register, so 0x4035 loads r5
-  jtag_dr_shift16(0x4030+reg);
+  jtag_dr_shift_16(0x4030+reg);
   CLRTCLK;
   SETTCLK;
-  jtag_dr_shift16(val);// Value for the register
+  jtag_dr_shift_16(val);// Value for the register
   CLRTCLK;
-  jtag_ir_shift8(IR_ADDR_CAPTURE);
+  jtag_ir_shift_8(IR_ADDR_CAPTURE);
   SETTCLK;
   CLRTCLK ;// Now reg is set to new value.
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x2401);// low byte controlled by JTAG
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x2401);// low byte controlled by JTAG
 }
 
 //! Set the program counter.
@@ -59,12 +78,12 @@ void jtag430_setpc(unsigned int adr){
 void jtag430_haltcpu(){
   //jtag430_setinstrfetch();
   
-  jtag_ir_shift8(IR_DATA_16BIT);
-  jtag_dr_shift16(0x3FFF);//JMP $+0
+  jtag_ir_shift_8(IR_DATA_16BIT);
+  jtag_dr_shift_16(0x3FFF);//JMP $+0
   
   CLRTCLK;
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x2409);//set JTAG_HALT bit
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x2409);//set JTAG_HALT bit
   SETTCLK;
 }
 
@@ -74,11 +93,11 @@ void jtag430_releasecpu(){
   //debugstr("Releasing target MSP430.");
   
   /*
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x2C01); //Apply reset.
-  jtag_dr_shift16(0x2401); //Release reset.
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x2C01); //Apply reset.
+  jtag_dr_shift_16(0x2401); //Release reset.
   */
-  jtag_ir_shift8(IR_CNTRL_SIG_RELEASE);
+  jtag_ir_shift_8(IR_CNTRL_SIG_RELEASE);
   SETTCLK;
 }
 
@@ -88,19 +107,19 @@ unsigned int jtag430_readmem(unsigned int adr){
   jtag430_haltcpu();
   
   CLRTCLK;
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
   
   if(adr>0xFF)
-    jtag_dr_shift16(0x2409);//word read
+    jtag_dr_shift_16(0x2409);//word read
   else
-    jtag_dr_shift16(0x2419);//byte read
-  jtag_ir_shift8(IR_ADDR_16BIT);
-  jtag_dr_shiftadr(adr);//address
-  jtag_ir_shift8(IR_DATA_TO_ADDR);
+    jtag_dr_shift_16(0x2419);//byte read
+  jtag_ir_shift_8(IR_ADDR_16BIT);
+  jtag430_shift_addr(adr);//address
+  jtag_ir_shift_8(IR_DATA_TO_ADDR);
   SETTCLK;
 
   CLRTCLK;
-  toret=jtag_dr_shift16(0x0000);//16 bit return
+  toret=jtag_dr_shift_16(0x0000);//16 bit return
   
   return toret;
 }
@@ -108,15 +127,15 @@ unsigned int jtag430_readmem(unsigned int adr){
 //! Write data to address.
 void jtag430_writemem(unsigned int adr, unsigned int data){
   CLRTCLK;
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
   if(adr>0xFF)
-    jtag_dr_shift16(0x2408);//word write
+    jtag_dr_shift_16(0x2408);//word write
   else
-    jtag_dr_shift16(0x2418);//byte write
-  jtag_ir_shift8(IR_ADDR_16BIT);
-  jtag_dr_shiftadr(adr);
-  jtag_ir_shift8(IR_DATA_TO_ADDR);
-  jtag_dr_shift16(data);
+    jtag_dr_shift_16(0x2418);//byte write
+  jtag_ir_shift_8(IR_ADDR_16BIT);
+  jtag430_shift_addr(adr);
+  jtag_ir_shift_8(IR_DATA_TO_ADDR);
+  jtag_dr_shift_16(data);
   SETTCLK;
 }
 
@@ -124,24 +143,24 @@ void jtag430_writemem(unsigned int adr, unsigned int data){
 void jtag430_writeflashword(unsigned int adr, unsigned int data){
   
   CLRTCLK;
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x2408);//word write
-  jtag_ir_shift8(IR_ADDR_16BIT);
-  jtag_dr_shiftadr(adr);
-  jtag_ir_shift8(IR_DATA_TO_ADDR);
-  jtag_dr_shift16(data);
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x2408);//word write
+  jtag_ir_shift_8(IR_ADDR_16BIT);
+  jtag430_shift_addr(adr);
+  jtag_ir_shift_8(IR_DATA_TO_ADDR);
+  jtag_dr_shift_16(data);
   SETTCLK;
   
   //Return to read mode.
   CLRTCLK;
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x2409);
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x2409);
   
   /*
   jtag430_writemem(adr,data);
   CLRTCLK;
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x2409);
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x2409);
   */
   
   //Pulse TCLK
@@ -174,15 +193,15 @@ void jtag430_writeflash(unsigned int adr, unsigned int data){
 //! Power-On Reset
 void jtag430_por(){
   // Perform Reset
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x2C01); // apply
-  jtag_dr_shift16(0x2401); // remove
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x2C01); // apply
+  jtag_dr_shift_16(0x2401); // remove
   CLRTCLK;
   SETTCLK;
   CLRTCLK;
   SETTCLK;
   CLRTCLK;
-  jtagid = jtag_ir_shift8(IR_ADDR_CAPTURE); // get JTAG identifier
+  jtagid = jtag_ir_shift_8(IR_ADDR_CAPTURE); // get JTAG identifier
   SETTCLK;
   
   jtag430_writemem(0x0120, 0x5A80);   // Diabled Watchdog
@@ -215,8 +234,8 @@ void jtag430_eraseflash(unsigned int mode, unsigned int adr, unsigned int count,
   jtag430_writemem(adr, 0x55AA);
   //Return to read mode.
   CLRTCLK;
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x2409);
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x2409);
   
   //Send the pulses.
   jtag430_tclk_flashpulses(count);
@@ -235,17 +254,17 @@ void jtag430_resettap(){
   SETTDI; //430X2
   SETTMS;
   //SETTDI; //classic
-  TCKTOCK;
+  jtag_tcktock();
 
   // Navigate to reset state.
   // Should be at least six.
   for(i=0;i<4;i++){
-    TCKTOCK;
+    jtag_tcktock();
   }
 
   // test-logic-reset
   CLRTMS;
-  TCKTOCK;
+  jtag_tcktock();
   SETTMS;
   // idle
 
@@ -267,7 +286,7 @@ void jtag430_resettap(){
 //! Get the JTAG ID
 unsigned char jtag430x2_jtagid(){
   jtag430_resettap();
-  jtagid=jtag_ir_shift8(IR_BYPASS);
+  jtagid = jtag_ir_shift_8(IR_BYPASS);
   if(jtagid!=0x89 && jtagid!=0x91){
     debugstr("Unknown JTAG ID");
     debughex(jtagid);
@@ -276,7 +295,7 @@ unsigned char jtag430x2_jtagid(){
 }
 //! Start JTAG, take pins
 unsigned char jtag430x2_start(){
-  jtagsetup();
+  jtag_setup();
   
   //Known-good starting position.
   //Might be unnecessary.
@@ -305,7 +324,7 @@ unsigned char jtag430x2_start(){
 
 //! Start JTAG, take pins
 void jtag430_start(){
-  jtagsetup();
+  jtag_setup();
   
   //Known-good starting position.
   //Might be unnecessary.
@@ -337,7 +356,7 @@ void jtag430_start(){
 //! Stop JTAG.
 void jtag430_stop(){
   debugstr("Exiting JTAG.");
-  jtagsetup();
+  jtag_setup();
   
   //Known-good starting position.
   //Might be unnecessary.
@@ -358,11 +377,11 @@ void jtag430_stop(){
 //! Set CPU to Instruction Fetch
 void jtag430_setinstrfetch(){
   
-  jtag_ir_shift8(IR_CNTRL_SIG_CAPTURE);
+  jtag_ir_shift_8(IR_CNTRL_SIG_CAPTURE);
 
   // Wait until instruction fetch state.
   while(1){
-    if (jtag_dr_shift16(0x0000) & 0x0080)
+    if (jtag_dr_shift_16(0x0000) & 0x0080)
       return;
     CLRTCLK;
     SETTCLK;
@@ -424,6 +443,7 @@ void jtag430_handle_fn(uint8_t const app,
     jtag430_resettap();
     txdata(app,verb,1);
     
+
     break;
   case STOP:
     jtag430_stop();
diff --git a/firmware/apps/jtag/jtag430x2.c b/firmware/apps/jtag/jtag430x2.c
index 1528e7c..67ef45b 100644
--- a/firmware/apps/jtag/jtag430x2.c
+++ b/firmware/apps/jtag/jtag430x2.c
@@ -29,42 +29,58 @@ app_t const jtag430x2_app = {
   "\tfor 20-bit MSP430X2 devices, such as the MSP430F5xx Family.\n"
 };
 
+//! Shift 20 bits of the DR.
+uint32_t jtag430_dr_shift_20(uint32_t in)
+{
+    if (!in_run_test_idle())
+    {
+        debugstr("Not in run-test-idle state");
+        return 0;
+    }
+
+    // get intot the right state
+    jtag_capture_dr();
+    jtag_shift_register();
+
+    // shift DR, then idle
+    return jtag_trans_n(in, 20, MSB);
+}
 
 //! Grab the core ID.
 unsigned int jtag430_coreid(){
-  jtag_ir_shift8(IR_COREIP_ID);
-  return jtag_dr_shift16(0);
+  jtag_ir_shift_8(IR_COREIP_ID);
+  return jtag_dr_shift_16(0);
 }
 
 //! Grab the device ID.
 unsigned long jtag430_deviceid(){
-  jtag_ir_shift8(IR_DEVICE_ID);
-  return jtag_dr_shift20(0);
+  jtag_ir_shift_8(IR_DEVICE_ID);
+  return jtag430_dr_shift_20(0);
 }
 
 
 //! Write data to address
 void jtag430x2_writemem(unsigned long adr,
 			unsigned int data){
-  jtag_ir_shift8(IR_CNTRL_SIG_CAPTURE);
-  if(jtag_dr_shift16(0) & 0x0301){
+  jtag_ir_shift_8(IR_CNTRL_SIG_CAPTURE);
+  if(jtag_dr_shift_16(0) & 0x0301){
     CLRTCLK;
-    jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
+    jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
     if(adr>=0x100)
-      jtag_dr_shift16(0x0500);//word mode
+      jtag_dr_shift_16(0x0500);//word mode
     else
-      jtag_dr_shift16(0x0510);//byte mode
-    jtag_ir_shift8(IR_ADDR_16BIT);
-    jtag_dr_shift20(adr);
+      jtag_dr_shift_16(0x0510);//byte mode
+    jtag_ir_shift_8(IR_ADDR_16BIT);
+    jtag430_dr_shift_20(adr);
     
     SETTCLK;
     
-    jtag_ir_shift8(IR_DATA_TO_ADDR);
-    jtag_dr_shift16(data);//16 word
+    jtag_ir_shift_8(IR_DATA_TO_ADDR);
+    jtag_dr_shift_16(data);//16 word
 
     CLRTCLK;
-    jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-    jtag_dr_shift16(0x0501);
+    jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+    jtag_dr_shift_16(0x0501);
     SETTCLK;
 
     CLRTCLK;
@@ -82,23 +98,23 @@ unsigned int jtag430x2_readmem(unsigned long adr){
   
   while(1){
     do{
-      jtag_ir_shift8(IR_CNTRL_SIG_CAPTURE);
-    }while(!(jtag_dr_shift16(0) & 0x0301));
+      jtag_ir_shift_8(IR_CNTRL_SIG_CAPTURE);
+    }while(!(jtag_dr_shift_16(0) & 0x0301));
     
-    if(jtag_dr_shift16(0) & 0x0301){
+    if(jtag_dr_shift_16(0) & 0x0301){
       // Read Memory
       CLRTCLK;
-      jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
+      jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
       
-      jtag_dr_shift16(0x0501);//word read
+      jtag_dr_shift_16(0x0501);//word read
       
-      jtag_ir_shift8(IR_ADDR_16BIT);
-      jtag_dr_shift20(adr); //20
+      jtag_ir_shift_8(IR_ADDR_16BIT);
+      jtag430_dr_shift_20(adr); //20
       
-      jtag_ir_shift8(IR_DATA_TO_ADDR);
+      jtag_ir_shift_8(IR_DATA_TO_ADDR);
       SETTCLK;
       CLRTCLK;
-      toret = jtag_dr_shift16(0x0000);
+      toret = jtag_dr_shift_16(0x0000);
       
       SETTCLK;
       
@@ -115,9 +131,9 @@ unsigned int jtag430x2_readmem(unsigned long adr){
 
 //! Syncs a POR.
 unsigned int jtag430x2_syncpor(){
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x1501); //JTAG mode
-  while(!(jtag_dr_shift16(0) & 0x200));  //0x100 or 0x200?
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x1501); //JTAG mode
+  while(!(jtag_dr_shift_16(0) & 0x200));  //0x100 or 0x200?
   return jtag430x2_por();
 }
 
@@ -129,9 +145,9 @@ unsigned int jtag430x2_por(){
   CLRTCLK;
   SETTCLK;
 
-  jtag_ir_shift8(IR_CNTRL_SIG_16BIT);
-  jtag_dr_shift16(0x0C01);
-  jtag_dr_shift16(0x0401);
+  jtag_ir_shift_8(IR_CNTRL_SIG_16BIT);
+  jtag_dr_shift_16(0x0C01);
+  jtag_dr_shift_16(0x0401);
   
   //cycle
   for (i = 0; i < 10; i++){
@@ -139,7 +155,7 @@ unsigned int jtag430x2_por(){
     SETTCLK;
   }
   
-  jtag_dr_shift16(0x0501);
+  jtag_dr_shift_16(0x0501);
   
   // tick
   CLRTCLK;
@@ -150,8 +166,8 @@ unsigned int jtag430x2_por(){
   jtag430x2_writemem(0x015C, 0x5A80);
   
   // check state
-  jtag_ir_shift8(IR_CNTRL_SIG_CAPTURE);
-  if(jtag_dr_shift16(0) & 0x0301)
+  jtag_ir_shift_8(IR_CNTRL_SIG_CAPTURE);
+  if(jtag_dr_shift_16(0) & 0x0301)
     return(1);//ok
   
   return 0;//error
@@ -162,8 +178,8 @@ unsigned int jtag430x2_por(){
 unsigned int jtag430x2_fusecheck(){
   int i;
   for(i=0;i<3;i++){
-    jtag_ir_shift8(IR_CNTRL_SIG_CAPTURE);
-    if(jtag_dr_shift16(0xAAAA)==0x5555)
+    jtag_ir_shift_8(IR_CNTRL_SIG_CAPTURE);
+    if(jtag_dr_shift_16(0xAAAA)==0x5555)
       return 1;//blown
   }
   return 0;//unblown
@@ -202,8 +218,8 @@ void jtag430x2_handle_fn( uint8_t const app,
 	 to be exchanged for addresses on 16-bit chips as well as the
 	 new MSP430X chips.  (This has only been verified on the
 	 MSP430F2xx family.  TODO verify for others.)
-      */
-      
+       */
+
       drwidth=20;
       
       //Perform a reset and disable watchdog.
@@ -214,7 +230,6 @@ void jtag430x2_handle_fn( uint8_t const app,
       
       jtag430_resettap();
       txdata(app,verb,1);
-      
       return;
     }else if(jtagid==MSP430X2JTAGID){
       jtag430mode=MSP430X2MODE;
@@ -225,8 +240,6 @@ void jtag430x2_handle_fn( uint8_t const app,
       return;
     }
     
-    
-    
     jtag430x2_fusecheck();
         
     jtag430x2_syncpor();
diff --git a/firmware/apps/jtag/jtagarm7.c b/firmware/apps/jtag/jtagarm7.c
index 8cd4c26..6587de1 100644
--- a/firmware/apps/jtag/jtagarm7.c
+++ b/firmware/apps/jtag/jtagarm7.c
@@ -80,17 +80,18 @@ http://hri.sourceforge.net/tools/jtag_faq_org.html
 
 // ! Start JTAG, setup pins, reset TAP and return IDCODE
 void jtagarm7tdmi_start() {
-  jtagsetup();
+  jtag_setup();
   SETTST;
-  jtag_resettap();
+  jtag_reset_tap();
 }
 
 
 u8 jtagarm_shift_ir(u8 ir, u8 flags){
   u8 retval = 0;
   if (last_ir != ir){
-    jtag_goto_shift_ir();
-    retval = jtagtransn(ir, 4, LSB|flags); 
+	jtag_capture_ir();
+	jtag_shift_register();
+    retval = jtag_trans_n(ir, 4, LSB|flags); 
     tapstate = RunTest_Idle;
     last_ir = ir;
   }
@@ -109,8 +110,9 @@ state” to the “Select DR” state each time the “Update” state is reache
   if (last_scanchain != chain){
     jtagarm_shift_ir(ARM7TDMI_IR_SCAN_N, NORETIDLE);
     last_scanchain = chain;
-    jtag_goto_shift_dr();
-    retval = jtagtransn(chain, 4, LSB | NORETIDLE);
+	jtag_capture_dr();
+	jtag_shift_register();
+    retval = jtag_trans_n(chain, 4, LSB | NORETIDLE);
     tapstate = Update_DR;
   }
   jtagarm_shift_ir(testmode, NORETIDLE); 
@@ -123,10 +125,11 @@ state” to the “Select DR” state each time the “Update” state is reache
 unsigned long eice_write(unsigned char reg, unsigned long data){
   unsigned long retval, temp;
   jtagarm7tdmi_scan(2, ARM7TDMI_IR_INTEST);
-  jtag_goto_shift_dr();
-  retval = jtagtransn(data, 32, LSB| NOEND| NORETIDLE);         // send in the data - 32-bits lsb
-  temp = jtagtransn(reg, 5, LSB| NOEND| NORETIDLE);             // send in the register address - 5 bits lsb
-  jtagtransn(1, 1, LSB);                                        // send in the WRITE bit
+  jtag_capture_dr();
+  jtag_shift_register();
+  retval = jtag_trans_n(data, 32, LSB| NOEND| NORETIDLE);         // send in the data - 32-bits lsb
+  temp = jtag_trans_n(reg, 5, LSB| NOEND| NORETIDLE);             // send in the register address - 5 bits lsb
+  jtag_trans_n(1, 1, LSB);                                        // send in the WRITE bit
   tapstate = RunTest_Idle;
   return(retval); 
 }
@@ -135,11 +138,13 @@ unsigned long eice_write(unsigned char reg, unsigned long data){
 unsigned long eice_read(unsigned char reg){               // PROVEN
   unsigned long temp, retval;
   jtagarm7tdmi_scan(2, ARM7TDMI_IR_INTEST);
-  jtag_goto_shift_dr();                                         // send in the register address - 5 bits LSB
-  temp = jtagtransn(reg, 5, LSB| NOEND| NORETIDLE);
-  jtagtransn(0L, 1, LSB);                                       // clear TDI to select "read only"
-  jtag_goto_shift_dr();                                         // Now shift out the 32 bits
-  retval = jtagtransn(0L, 32, LSB);                             // atmel arm jtag docs pp.10-11: LSB first
+  jtag_capture_dr();
+  jtag_shift_register(); // send in the register address - 5 bits LSB
+  temp = jtag_trans_n(reg, 5, LSB| NOEND| NORETIDLE);
+  jtag_trans_n(0L, 1, LSB);                                       // clear TDI to select "read only"
+  jtag_capture_dr();
+  jtag_shift_register(); // Now shift out the 32 bits
+  retval = jtag_trans_n(0L, 32, LSB);                             // atmel arm jtag docs pp.10-11: LSB first
   tapstate = RunTest_Idle;
   return(retval);
   
@@ -152,7 +157,8 @@ unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){
 
   //debughex32(instr);
   if (last_instr != instr && last_sysstate != breakpt){
-    jtag_goto_shift_dr();
+	  jtag_capture_dr();
+	  jtag_shift_register();
     // if the next instruction is to run using MCLK (master clock), set TDI
     if (breakpt)
       {
@@ -165,7 +171,7 @@ unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){
     jtag_tcktock();
     
     // Now shift in the 32 bits
-    retval = jtagtransn(instr, 32, 0);    // Must return to RUN-TEST/IDLE state for instruction to enter pipeline, and causes debug clock.
+    retval = jtag_trans_n(instr, 32, 0);    // Must return to RUN-TEST/IDLE state for instruction to enter pipeline, and causes debug clock.
     tapstate = RunTest_Idle;
     last_instr = instr;
     last_sysstate = breakpt;
@@ -294,22 +300,24 @@ void jtagarm7_handle_fn( uint8_t const app,
     txdata(app,verb,1);
     break;
   case JTAG_DR_SHIFT:
-    jtag_goto_shift_dr();
-    cmddatalong[0] = jtagtransn(cmddatalong[1],cmddata[0],cmddata[1]);
+	jtag_capture_dr();
+	jtag_shift_register();
+    cmddatalong[0] = jtag_trans_n(cmddatalong[1],cmddata[0],cmddata[1]);
     tapstate = (cmddata[1]&NORETIDLE)>0?Update_DR:RunTest_Idle;
     txdata(app,verb,4);
     break;
   case JTAGARM7_CHAIN0:
     jtagarm7tdmi_scan(0, ARM7TDMI_IR_INTEST);
-    jtag_goto_shift_dr();
+   	jtag_capture_dr();
+	jtag_shift_register();
     //debughex32(cmddatalong[0]);
     //debughex(cmddataword[4]);
     //debughex32(cmddatalong[1]);
     //debughex32(cmddatalong[3]);
-    cmddatalong[0] = jtagtransn(cmddatalong[0], 32, LSB| NOEND| NORETIDLE);
-    cmddatalong[2] = jtagtransn(cmddataword[4], 9, MSB| NOEND| NORETIDLE);
-    cmddatalong[1] = jtagtransn(cmddatalong[1], 32, MSB| NOEND| NORETIDLE);
-    cmddatalong[3] = jtagtransn(cmddatalong[3], 32, MSB);
+    cmddatalong[0] = jtag_trans_n(cmddatalong[0], 32, LSB| NOEND| NORETIDLE);
+    cmddatalong[2] = jtag_trans_n(cmddataword[4], 9, MSB| NOEND| NORETIDLE);
+    cmddatalong[1] = jtag_trans_n(cmddatalong[1], 32, MSB| NOEND| NORETIDLE);
+    cmddatalong[3] = jtag_trans_n(cmddatalong[3], 32, MSB);
     tapstate = RunTest_Idle;
     txdata(app,verb,16);
     break;
@@ -335,7 +343,7 @@ void jtagarm7_handle_fn( uint8_t const app,
     jtagarm7tdmi_set_register(cmddatalong[1], cmddatalong[0]);
     txdata(app,verb,4);
     break;
-  case JTAG_RESETTARGET:
+  case JTAG_RESET_TARGET:
     //FIXME: BORKEN
     debugstr("RESET TARGET");
     CLRTST;
diff --git a/firmware/apps/jtag/jtagxscale.c b/firmware/apps/jtag/jtagxscale.c
index bbf9b9b..bbe0d24 100644
--- a/firmware/apps/jtag/jtagxscale.c
+++ b/firmware/apps/jtag/jtagxscale.c
@@ -52,168 +52,55 @@ app_t const jtagxscale_app = {
 /* NOTE: I heavily cribbed from the ARM7TDMI jtag implementation. Credit where
  * credit is due. */
 
-/* this handles shifting arbitrary length bit strings into the instruction
- * register and clocking out bits while leaving the JTAG state machine in a
- * known state. it also handle bit swapping. */
-unsigned long jtag_xscale_shift_n(unsigned long word,
-                                  unsigned char nbits,
-                                  unsigned char flags)
+void jtag_xscale_reset_cpu(void)
 {
-    unsigned int bit;
-    unsigned long high = 1;
-    unsigned long mask;
-
-    for (bit = (nbits - 1) / 8; bit > 0; bit--)
-        high <<= 8;
-    
-    high <<= ((nbits - 1) % 8);
-
-    mask = high - 1;
-
-    if (flags & LSB) 
-    {
-        /* clock the bits into the IR from LSB to MSB order */
-        for (bit = nbits; bit > 0; bit--) 
-        {
-            /* write MOSI on trailing edge of previous clock */
-            if (word & 1)
-            {
-                SETMOSI;
-            }
-            else
-            {
-                CLRMOSI;
-            }
-            word >>= 1;
-
-            if (bit == 1 && !(flags & NOEND))
-                SETTMS; /* TMS high on last bit to exit. */
-
-            /* tick tock the clock line */
-            XTT;
-
-            /* read MISO on trailing edge */
-            if (READMISO)
-            {
-                word += (high);
-            }
-        }
-    } 
-    else 
-    {
-        /* clock the bits into the IR from MSB to LSB order */
-        for (bit = nbits; bit > 0; bit--) 
-        {
-            /* write MOSI on trailing edge of previous clock */
-            if (word & high)
-            {
-                SETMOSI;
-            }
-            else
-            {
-                CLRMOSI;
-            }
-            word = (word & mask) << 1;
-
-            if (bit == 1 && !(flags & NOEND))
-                SETTMS;//TMS high on last bit to exit.
-
-            /* tick tock the clock line */
-            XTT;
-
-            /* read MISO on trailing edge */
-            word |= (READMISO);
-        }
-    }
-
-    SETMOSI;
-
-    if (!(flags & NOEND))
-    {
-        /* exit state */
-        XTT;
-
-        /* update state */
-        if (!(flags & NORETIDLE))
-        {
-            CLRTMS;
-            XTT;
-        }
-    }
-
-    return word;
-}
-
-
-/* this handles shifting in the IDCODE instruction and shifting the result
- * out the TDO and return it. */
-unsigned long jtag_xscale_idcode()
-{
-    /* NOTE: this assumes that we're in the run-test-idle state */
-
-    /* get into the shift-ir state */
-    SHIFT_IR;
-
-    /* shift the ID code instruction into the IR and return to run-test-idle */
-    jtag_xscale_shift_n(XSCALE_IR_IDCODE, 5, LSB);
-
-    /* get into the shift-dr state */
-    SHIFT_DR;
-
-    /* now clock out the 32 bit ID code and return back to run-test-idle */
-    return jtag_xscale_shift_n(0, 32, LSB);
+	SETRST;
+	msdelay(100);
+	CLRRST;
+	msdelay(100);
+	SETRST;
+	msdelay(100);
 }
 
 /* Handles XScale JTAG commands.  Forwards others to JTAG. */
 void jtag_xscale_handle_fn( uint8_t const app,
 							uint8_t const verb,
 							uint32_t const len)
-{    
-    switch(verb) 
-    {
-        /*
-         * Standard Commands
-         */
-        case SETUP:
-
-            /* set up the pin I/O for JTAG */
-            jtagsetup();
-
-            /* reset to run-test-idle state */
-            RUN_TEST_IDLE;
-
-            /* send back OK */
-            txdata(app, OK, 0);
-
-            break;
-
-        case START:
-        case STOP:
-        case PEEK:
-        case POKE:
-        case READ:
-        case WRITE:
-        default:
- 
-            /* send back OK */
-            txdata(app, OK, 0);
-
-            break;
-
-        /*
-         * XScale Commands
-         */
-        case XSCALE_GET_CHIP_ID:
-
-            /* reset to run-test-idle state */
-            RUN_TEST_IDLE;
-
-            /* put the ID code in the data buffer */
-            cmddatalong[0] = jtag_xscale_idcode();
-
-            /* send it back to the client */
-            txdata(app,verb,4);
-
-            break;
-    }
+{	 
+	switch(verb) 
+	{
+	case SETUP:
+		/* set up the pin I/O for JTAG */
+		jtag_setup();
+		/* reset to run-test-idle state */
+		jtag_reset_tap();
+		/* send back OK */
+		txdata(app, verb, 0);
+		break;
+
+	case START:
+		txdata(app, verb, 0);
+		break;
+
+	case STOP:
+		txdata(app, verb, 0);
+		break;
+
+	case PEEK:
+	case POKE:
+	case READ:
+	case WRITE:
+		/* send back OK */
+		txdata(app, verb, 0);
+		break;
+
+	case JTAG_RESET_TARGET:
+		jtag_xscale_reset_cpu();
+		txdata(app, verb, 0);
+		break;
+
+	default:
+		(*(jtag_app.handle))(app,verb,len);
+		break;
+	}
 }
diff --git a/firmware/apps/jtag/openocd.c b/firmware/apps/jtag/openocd.c
new file mode 100644
index 0000000..6a0e4c1
--- /dev/null
+++ b/firmware/apps/jtag/openocd.c
@@ -0,0 +1,57 @@
+/*! \file openocd.c
+  \author Dave Huseby <dave at linuxprogrammer.org>
+  \brief OpenOCD firmware
+*/
+
+
+#include "platform.h"
+#include "command.h"
+
+#define OPENOCD_APP
+
+//! Handles a monitor command.
+void openocd_handle_fn(uint8_t const app,
+					   uint8_t const verb,
+					   uint32_t const len);
+
+// define the openocd app's app_t
+app_t const openocd_app = {
+
+	/* app number */
+	OPENOCD,
+
+	/* handle fn */
+	openocd_handle_fn,
+
+	/* name */
+	"OpenOCD",
+
+	/* desc */
+	"\tThe OpenOCD app handles the OpenOCD protocol.\n"
+};
+
+//! handles OpenOCD commands
+void openocd_handle_fn(uint8_t const app,
+					   uint8_t const verb,
+					   uint32_t const len)
+{
+	switch(verb)
+	{
+		case START:
+			txdata(app,verb,0);
+			break;
+
+		case STOP:
+			txdata(app,verb,0);
+			break;
+
+		case SETUP:
+			txdata(app,verb,0);
+			break;
+
+		default:
+			txdata(app,NOK,0);
+	}
+}
+
+
diff --git a/firmware/goodfet.c b/firmware/goodfet.c
index 0df57ed..c7563e6 100644
--- a/firmware/goodfet.c
+++ b/firmware/goodfet.c
@@ -56,13 +56,12 @@ void init()
 	//Setup clocks, unique to each '430.
 	msp430_init_dco();
 	msp430_init_uart();
-	
-	
+
 	//DAC should be at full voltage if it exists.
 #ifdef DAC12IR
 	//glitchvoltages(0xfff,0xfff);
-	ADC12CTL0 = REF2_5V + REFON; // Internal 2.5V ref on
-	//for(i=0;i!=0xFFFF;i++) asm("nop"); //DO NOT UNCOMMENT, break GCC4.
+	ADC12CTL0 = REF2_5V + REFON;					// Internal 2.5V ref on
+	for(i=0;i!=0xFFFF;i++) asm("nop");
 	DAC12_0CTL = DAC12IR + DAC12AMP_5 + DAC12ENC; // Int ref gain 1
 	DAC12_0DAT = 0xFFF; //Max voltage 0xfff
 	DAC12_1CTL = DAC12IR + DAC12AMP_5 + DAC12ENC; // Int ref gain 1
@@ -72,7 +71,7 @@ void init()
 	/** FIXME
 	  
 	  This part is really ugly.  GSEL (P5.7) must be high to select
-	  normal voltage, but a lot of applications like to swing it low
+	  normal voltage, but a lot of applications light to swing it low
 	  to be a nuissance.  To get around this, we assume that anyone
 	  with a glitching FET will also have a DAC, then we set that DAC
 	  to a high voltage.
@@ -85,99 +84,106 @@ void init()
 
 	//Enable Interrupts.
 	//eint();
-	
-	
-	
-	#ifdef INITPLATFORM
-	INITPLATFORM;
-	#endif
 }
 
 
 //! Handle a command.
 void handle(uint8_t const app,
-	    uint8_t const verb,
-	    uint32_t const len){
-  int i;
-
-  //debugstr("GoodFET");
-  PLEDOUT&=~PLEDPIN;
-
-  // find the app and call the handle fn
-  for(i = 0; i < num_apps; i++){
-    if(apps[i]->app == app){
-      // call the app's handle fn
-      (*(apps[i]->handle))(app, verb, len);
-      
-      // exit early
-      return;
-    }
-  }
-
-  // if we get here, then the desired app is not copiled in 
-  // this firmware
-  debugstr("App missing.");
-  debughex(app);
-  txdata(app, NOK, 0);
+			uint8_t const verb,
+			uint32_t const len)
+{
+	int i;
+
+	//debugstr("GoodFET");
+	PLEDOUT&=~PLEDPIN;
+
+	// find the app and call the handle fn
+	for(i = 0; i < num_apps; i++)
+	{
+		if(apps[i]->app == app)
+		{
+			// call the app's handle fn
+			(*(apps[i]->handle))(app, verb, len);
+
+			// exit early
+			return;
+		}
+	}
+
+	// if we get here, then the desired app is not copiled in 
+	// this firmware
+	debugstr("App missing.");
+	debughex(app);
+	txdata(app, NOK, 0);
 }
 
 
 //! Main loop.
-int main(void){
-  volatile unsigned int i;
-  unsigned char app, verb;
-  unsigned long len;
-  // MSP reboot count for reset input & reboot function located at 0xFFFE
-  volatile unsigned int reset_count = 0;
-  void (*reboot_function)(void) = (void *) 0xFFFE;
-  
-  init();
+int main(void)
+{
+	volatile unsigned int i;
+	unsigned char app, verb;
+	unsigned long len;
+	// MSP reboot count for reset input & reboot function located at 0xFFFE
+	volatile unsigned int reset_count = 0;
+	void (*reboot_function)(void) = (void *) 0xFFFE;
+
+	init();
   
-  txstring(MONITOR,OK,"http://goodfet.sf.net/");
+	txstring(MONITOR,OK,"http://goodfet.sf.net/");
   
-  //Command loop.  There's no end!
-  while(1){
-    //Magic 3
-    app = serial_rx();
-    
-    // If the app is the reset byte (0x80) increment and loop
-    if (app == RESET){
-      reset_count++;
-      
-      if (reset_count > 4){
-	// We could trigger the WDT with either:
-	// WDTCTL = 0;
-	// or
-	// WDTCTL = WDTPW + WDTCNTCL + WDTSSEL + 0x00;
-	// but instead we'll jump to our reboot function pointer
-	(*reboot_function)();
-      }
-      
-      continue;
-    }else{
-      reset_count = 0;
-    }
-
-    verb = serial_rx();
-    //len=serial_rx();
-    len = rxword();
-    
-    //Read data, looking for buffer overflow.y
-    if(len <= CMDDATALEN){
-      for(i = 0; i < len; i++){
-	cmddata[i] = serial_rx();
-      }
-
-      handle(app,verb,len);
-    }else{
-      //Listen to the blaberring.
-      for(i = 0; i < len; i++)
-	serial_rx();
-      
-      //Reply with an error.
-      debugstr("Buffer length exceeded.");
-      txdata(MONITOR,NOK,0);
-    }
-  }
+	//Command loop.  There's no end!
+	while(1)
+	{
+		//Magic 3
+		app = serial_rx();
+
+		// If the app is the reset byte (0x80) increment and loop
+		if (app == RESET)
+		{
+			reset_count++;
+
+			if (reset_count > 4) 
+			{
+				// We could trigger the WDT with either:
+				// WDTCTL = 0;
+				// or
+				// WDTCTL = WDTPW + WDTCNTCL + WDTSSEL + 0x00;
+				// but instead we'll jump to our reboot function pointer
+				(*reboot_function)();
+			}
+
+			continue;
+		} 
+		else 
+		{
+			reset_count = 0;
+		}
+
+		verb = serial_rx();
+		//len=serial_rx();
+		len = rxword();
+
+		//Read data, looking for buffer overflow.y
+		if(len <= CMDDATALEN)
+		{
+			for(i = 0; i < len; i++)
+			{
+				cmddata[i] = serial_rx();
+			}
+
+			handle(app,verb,len);
+		}
+		else
+		{
+			//Listen to the blaberring.
+			for(i = 0; i < len; i++)
+				serial_rx();
+
+			//Reply with an error.
+			debugstr("Buffer length exceeded.");
+			txdata(MONITOR,NOK,0);
+		}
+	}
 }
 
diff --git a/firmware/include/jtag.h b/firmware/include/jtag.h
index bdcd803..5baaf29 100644
--- a/firmware/include/jtag.h
+++ b/firmware/include/jtag.h
@@ -10,38 +10,87 @@
 
 #define JTAG 0x10
 
-//! JTAG device ID.
-extern unsigned char jtagid;
+//! All states in the JTAG TAP
+enum eTAPState
+{
+	UNKNOWN				= 0x0000,
+	TEST_LOGIC_RESET	= 0x0001,
+	RUN_TEST_IDLE		= 0x0002,
+	SELECT_DR_SCAN		= 0x0004,
+	CAPTURE_DR			= 0x0008,
+	SHIFT_DR			= 0x0010,
+	EXIT1_DR			= 0x0020,
+	PAUSE_DR			= 0x0040,
+	EXIT2_DR			= 0x0080,
+	UPDATE_DR			= 0x0100,
+	SELECT_IR_SCAN		= 0x0200,
+	CAPTURE_IR			= 0x0400,
+	SHIFT_IR			= 0x0800,
+	EXIT1_IR			= 0x1000,
+	PAUSE_IR			= 0x2000,
+	EXIT2_IR			= 0x4000,
+	UPDATE_IR			= 0x8000
+};
 
+extern unsigned char jtagid;
 
-// Generic Commands
+//! the global state of the JTAG TAP
+extern enum eTAPState jtag_state;
+
+//! Returns true if we're in any of the data register states
+int in_dr();
+//! Returns true if we're in any of the instruction register states
+int in_ir();
+//! Returns true if we're in run-test-idle state
+int in_run_test_idle();
+//! Check the state
+int in_state(enum eTAPState state);
+
+//! jtag_trans_n flags
+enum eTransFlags
+{
+	MSB					= 0x0,
+	LSB					= 0x1,
+	NOEND				= 0x2,
+	NORETIDLE			= 0x4
+};
 
 //! Shift n bytes.
-unsigned long jtagtransn(unsigned long word,
-			 unsigned char bitcount,
-             unsigned char flags);
-//! Shift the address width.
-unsigned long jtag_dr_shiftadr(unsigned long in);
+uint32_t jtag_trans_n(uint32_t word, 
+					  uint8_t bitcount, 
+					  enum eTransFlags flags);
+//! Shift 8 bits in/out of selected register
+uint8_t jtag_trans_8(uint8_t in);
+//! Shift 16 bits in/out of selected register
+uint16_t jtag_trans_16(uint16_t in);
 //! Shift 8 bits of the IR.
-unsigned char jtag_ir_shift8(unsigned char);
+uint8_t jtag_ir_shift_8(uint8_t in);
 //! Shift 16 bits of the DR.
-unsigned int jtag_dr_shift16(unsigned int);
-//! Shift 20 bits of the DR, MSP430 specific.
-unsigned long jtag_dr_shift20(unsigned long in);
+uint16_t jtag_dr_shift_16(uint16_t in);
 //! Stop JTAG, release pins
 void jtag_stop();
-
 //! Setup the JTAG pin directions.
-void jtagsetup();
-
+void jtag_setup();
 //! Ratchet Clock Down and Up
 void jtag_tcktock();
-//! Go to SHIFT_IR
-void jtag_goto_shift_ir();
-//! Go to SHIFT_DR
-void jtag_goto_shift_dr();
+//! Reset the target device
+void jtag_reset_target();
 //! TAP RESET
-void jtag_resettap();
+void jtag_reset_tap();
+//! Get into the Shift-IR or Shift-DR
+void jtag_shift_register();
+//! Get into Capture-IR state
+void jtag_capture_ir();
+//! Get into Capture-DR state
+void jtag_capture_dr();
+//! Get to Run-Test-Idle without going through Test-Logic-Reset
+void jtag_run_test_idle();
+//! Detect instruction register width
+uint16_t jtag_detect_ir_width();
+//! Detects how many TAPs are in the JTAG chain
+uint16_t jtag_detect_chain_length();
+//! Gets device ID for specified chip in the chain
+uint32_t jtag_get_device_id(int chip);
 
 //Pins.  Both SPI and JTAG names are acceptable.
 //#define SS   BIT0
@@ -90,38 +139,15 @@ extern int savedtclk;
 #define SAVETCLK savedtclk=SPIOUT&TCLK;
 #define RESTORETCLK if(savedtclk) SPIOUT|=TCLK; else SPIOUT&=~TCLK
 
-//Replace every "CLRTCK SETTCK" with this.
-#define TCKTOCK CLRTCK,SETTCK
-
 //JTAG commands
 #define JTAG_IR_SHIFT 0x80
 #define JTAG_DR_SHIFT 0x81
-#define JTAG_RESETTAP 0x82
-#define JTAG_RESETTARGET 0x83
-#define JTAG_DR_SHIFT20 0x91
-
-#define MSB         0
-#define LSB         1
-#define NOEND       2
-#define NORETIDLE   4
-
-//JTAG430 commands
-#define Exit2_DR 0x0
-#define Exit_DR 0x1
-#define Shift_DR 0x2
-#define Pause_DR 0x3
-#define Select_IR 0x4
-#define Update_DR 0x5
-#define Capture_DR 0x6
-#define Select_DR 0x7
-#define Exit2_IR 0x8
-#define Exit_IR 0x9
-#define Shift_IR 0xa
-#define Pause_IR 0xb
-#define RunTest_Idle 0xc
-#define Update_IR 0xd
-#define Capture_IR 0xe
-#define Test_Reset 0xf
+#define JTAG_RESET_TAP 0x82
+#define JTAG_RESET_TARGET 0x83
+#define JTAG_DETECT_IR_WIDTH 0x84
+#define JTAG_DETECT_CHAIN_LENGTH 0x85
+#define JTAG_GET_DEVICE_ID 0x86
+//#define JTAG_DR_SHIFT20 0x91
 
 extern app_t const jtag_app;
 
diff --git a/firmware/include/jtag430.h b/firmware/include/jtag430.h
index 950e80e..6cff0b7 100644
--- a/firmware/include/jtag430.h
+++ b/firmware/include/jtag430.h
@@ -45,6 +45,8 @@ void jtag430_setinstrfetch();
 void jtag430_setpc(unsigned int adr);
 //! Write data to address.
 void jtag430_writeflash(unsigned int adr, unsigned int data);
+//! Shift an address width of data
+uint32_t jtag430_shift_addr( uint32_t addr );
 
 
 //16-bit MSP430 JTAG commands, bit-swapped
diff --git a/firmware/include/jtag430x2.h b/firmware/include/jtag430x2.h
index 86598a8..5084976 100644
--- a/firmware/include/jtag430x2.h
+++ b/firmware/include/jtag430x2.h
@@ -12,5 +12,8 @@
 
 extern app_t const jtag430x2_app;
 
+//! Shift 20 bits of the DR.
+uint32_t jtag430_dr_shift_20(uint32_t in);
+
 #endif
 
diff --git a/firmware/include/jtagarm7.h b/firmware/include/jtagarm7.h
index ea13851..41ba258 100644
--- a/firmware/include/jtagarm7.h
+++ b/firmware/include/jtagarm7.h
@@ -12,6 +12,24 @@
 #define JTAGSTATE_ARM 0         // bit 4 on dbg status reg is low
 #define JTAGSTATE_THUMB 1
 
+// JTAG TAP states
+#define Exit2_DR 0x0
+#define Exit_DR 0x1
+#define Shift_DR 0x2
+#define Pause_DR 0x3
+#define Select_IR 0x4
+#define Update_DR 0x5
+#define Capture_DR 0x6
+#define Select_DR 0x7
+#define Exit2_IR 0x8
+#define Exit_IR 0x9
+#define Shift_IR 0xa
+#define Pause_IR 0xb
+#define RunTest_Idle 0xc
+#define Update_IR 0xd
+#define Capture_IR 0xe
+#define Test_Reset 0xf
+
 // JTAGARM7 Commands
 
 //! Start JTAG
diff --git a/firmware/include/jtagxscale.h b/firmware/include/jtagxscale.h
index 57973ba..0f85b0a 100644
--- a/firmware/include/jtagxscale.h
+++ b/firmware/include/jtagxscale.h
@@ -14,23 +14,6 @@
 
 #define JTAGXSCALE 0x15
 
-/* 
- * Utility Macros 
- */
-
-/* XTT (LED TCK TOCK) toggles the CLK line while turning on/off the LED */
-#define XTT             CLRTCK;PLEDOUT^=PLEDPIN;SETTCK;PLEDOUT^=PLEDPIN;
-
-/* RUN_TEST_IDLE gets us into run-test-idle from anywhere in the TAP FSM */
-#define RUN_TEST_IDLE   SETTMS;XTT;XTT;XTT;XTT;XTT;XTT;XTT;XTT;CLRTMS;XTT;
-
-/* SHIFT_IR gets us into the "Shift IR" state from the run-test-idle state */
-#define SHIFT_IR        SETTMS;XTT;XTT;CLRTMS;XTT;XTT;
-
-/* SHIFT_DIR gets us into the "Shift DR" state from the run-test-idle state */
-#define SHIFT_DR        SETTMS;XTT;CLRTMS;XTT;XTT;
-
-
 /*
  * XScale 5-bit JTAG Commands
  */
@@ -45,7 +28,7 @@
  * between TDI and TDO. This register can be accessed via the JTAG Test-Access 
  * Port throughout the device operation. Access to the Bypass register can also 
  * be obtained with the bypass instruction. */
-#define XSCALE_IR_HIGHZ             0x08
+#define XSCALE_IR_HIGHZ				0x08
 
 /* 11110 - Get ID Code
  * The idcode instruction is used in conjunction with the device identification 
@@ -54,7 +37,7 @@
  * identification code (32 bits) on TDO into the identification register on the 
  * rising edge of TCK in the Capture_DR state. Note: The device identification 
  * register is not altered by data being shifted in on TDI.*/
-#define XSCALE_IR_IDCODE            0x1E
+#define XSCALE_IR_IDCODE			0x1E
 
 /* 11111 - Bypass
  * The bypass instruction selects the Bypass register between TDI and TDO pins 
@@ -63,14 +46,12 @@
  * all other test data registers have no effect on the operation of the system. 
  * Test data registers with both test and system functionality perform their 
  * system functions when this instruction is selected. */
-#define XSCALE_IR_BYPASS            0x1F
+#define XSCALE_IR_BYPASS			0x1F
 
 /*
  * GoodFET Commands from the Client
  */
-
-/* Get CHIP ID */
-#define XSCALE_GET_CHIP_ID          0xF1
+/* commands start at 0xF0 */
 
 
 /*
@@ -84,10 +65,6 @@ unsigned long jtag_xscale_shift_n(unsigned long word,
                                   unsigned char nbits,
                                   unsigned char flags);
 
-/* this handles shifting in the IDCODE instruction and shifting the result
- * out the TDO and return it. */
-unsigned long jtag_xscale_idcode();
-
 extern app_t const jtagxscale_app;
 
 #endif // JTAGXSCALE_H
diff --git a/firmware/include/openocd.h b/firmware/include/openocd.h
new file mode 100644
index 0000000..59f8384
--- /dev/null
+++ b/firmware/include/openocd.h
@@ -0,0 +1,15 @@
+/*! \file openocd.h
+  \author Dave Huseby <dave at linuxprogrammer.org>
+  \brief OpenOCD firmware
+*/
+
+#ifndef OPENOCD_H
+#define OPENOCD_H
+
+#include "app.h"
+
+#define OPENOCD 0x18
+
+extern app_t const jtag_app;
+
+#endif
diff --git a/firmware/platforms/goodfet.h b/firmware/platforms/goodfet.h
index d421bd7..a9288b2 100644
--- a/firmware/platforms/goodfet.h
+++ b/firmware/platforms/goodfet.h
@@ -51,3 +51,14 @@
 #define CLRCE P5OUT&=~BIT5
 #define DIRCE P5DIR|=BIT5
 
+// network byte order converters
+#define htons(x) ((((uint16_t)(x) & 0xFF00) >> 8) | \
+				 (((uint16_t)(x) & 0x00FF) << 8))
+#define htonl(x) ((((uint32_t)(x) & 0xFF000000) >> 24) | \
+				  (((uint32_t)(x) & 0x00FF0000) >> 8) | \
+				  (((uint32_t)(x) & 0x0000FF00) << 8) | \
+				  (((uint32_t)(x) & 0x000000FF) << 24))
+
+#define ntohs htons
+#define ntohl htonl
+