From: dodge-this <dodge-this@12e2690d-a6be-4b82-a7b7-67c4a43b65c8>
Date: Mon, 12 Apr 2010 20:38:01 +0000 (+0000)
Subject: Updates to ARM7TDMI JTAG app and optimizations for debughex() and added debughex32... 
X-Git-Url: http://git.rot13.org/?p=goodfet;a=commitdiff_plain;h=64ef261d7416ef8cb3ec6cdf7130fc4d8afad8cd

Updates to ARM7TDMI JTAG app and optimizations for debughex() and added debughex32() for 32-bit longs



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

diff --git a/client/GoodFET.py b/client/GoodFET.py
index a2f3160..518cf47 100755
--- a/client/GoodFET.py
+++ b/client/GoodFET.py
@@ -8,6 +8,8 @@
 import sys, time, string, cStringIO, struct, glob, serial, os;
 import sqlite3;
 
+fmt = ("B", "<H", None, "<L")
+
 def getClient(name="GoodFET"):
     import GoodFET, GoodFETCC, GoodFETAVR, GoodFETSPI, GoodFETMSP430;
     if(name=="GoodFET" or name=="monitor"): return GoodFET.GoodFET();
@@ -159,10 +161,12 @@ class GoodFET:
                     print "Rx: ( 0x%02x, 0x%02x, 0x%04x )" % ( self.app, self.verb, self.count )
             
                 #Debugging string; print, but wait.
-                if self.app==0xFF and self.verb==0xFF:
-                    print "# DEBUG %s" % self.serialport.read(self.count);
+                if self.app==0xFF:
+                    if self.verb==0xFF:
+                        print "# DEBUG %s" % self.serialport.read(self.count)
+               	    elif self.verb==0xFE:
+                        print "# DEBUG 0x%x" % struct.unpack(fmt[self.count-1], self.serialport.read(self.count))[0]
                     sys.stdout.flush();
-                    return []
                 else:
                     self.data=self.serialport.read(self.count);
                     return self.data;
diff --git a/client/GoodFETARM.py b/client/GoodFETARM.py
index 9991d55..d254ed9 100644
--- a/client/GoodFETARM.py
+++ b/client/GoodFETARM.py
@@ -7,6 +7,7 @@
 #
 
 import sys, binascii, struct
+import atlasutils.smartprint as asp
 
 #Global Commands
 READ  = 0x00
@@ -151,30 +152,34 @@ class GoodFETARM(GoodFET):
     def ARMget_register(self, reg):
         """Get an ARM's Register"""
         self.writecmd(0x33,GET_REGISTER,1,[reg&0xff])
-        print "DEBUG:GET_REGISTER: %s"%repr(self.data)
+        print "DEBUG:GET_REGISTER: %s"%asp.hexText(self.data)
         retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
         return retval
     def ARMset_register(self, reg, val):
         """Get an ARM's Register"""
-        self.writecmd(0x33,GET_REGISTER,8,[reg,0,0,0,val>>24, (val>>16)&0xff, (val>>8)&0xff, val&0xff])
-        print "DEBUG:SET_REGISTER: %s"%repr(self.data)
+        self.writecmd(0x33,GET_REGISTER,20,[reg,0,0,0,val>>24, (val>>16)&0xff, (val>>8)&0xff, val&0xff,9,8,7,6,5,4,3,2,1,0,2,3])
+        print "DEBUG:SET_REGISTER: %s"%asp.hexText(self.data)
         retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
         return retval
     def ARMget_registers(self):
         """Get an ARM's Register"""
-        self.writecmd(0x33,GET_REGISTERS,0,[])
-        print "DEBUG:GET_REGISTER: %s"%repr(self.data)
+        clear = [x for x in range(20)]
+        self.writecmd(0x33,GET_REGISTERS,20,clear)
+        print "DEBUG:GET_REGISTER: %s"%asp.hexText(self.data)
+        retval = []
+        for x in range(0,len(self.data), 4):
+          retval.append(struct.unpack("<L", self.data[x:x+4])[0])
         #retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
         return retval
     def ARMset_registers(self, regs):
         """Get an ARM's Register"""
         regarry = []
         for reg in regs:
-          regarray.merge([reg>>24, (reg>>16)&0xff, (reg>>8)&0xff, reg&0xff])
-        self.writecmd(0x33,GET_REGISTER,16*4,regarray)
-        print "DEBUG:SET_REGISTER: %s"%repr(self.data)
-        retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
-        return retval
+          regarry.extend([reg>>24, (reg>>16)&0xff, (reg>>8)&0xff, reg&0xff])
+        self.writecmd(0x33,GET_REGISTER,16*4,regarry)
+        print "DEBUG:SET_REGISTER: %s"%asp.hexText(self.data)
+        #retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
+        #return retval
     def ARMcmd(self,phrase):
         self.writecmd(0x33,READ,len(phrase),phrase)
         val=ord(self.data[0])
diff --git a/client/gplay-arm.py b/client/gplay-arm.py
index e30a76d..8154015 100755
--- a/client/gplay-arm.py
+++ b/client/gplay-arm.py
@@ -79,11 +79,11 @@ def test2():
     print "Debug State: %x"%client.ARMget_dbgstate ()
     print "Debug State: %x"%client.ARMget_dbgstate ()
     print "Debug CTRL:  %x"%client.ARMget_dbgctrl()
-    #client.writecmd(0x33,0xda,0,[])
-    #print "TEST CHAIN0: %s"%repr(client.data)
+    client.writecmd(0x33,0xda,0,[])
+    print "TEST CHAIN0: %s"%repr(client.data)
     print "Debug State: %x"%client.ARMget_dbgstate ()
     print "IDCODE:      %x"%client.ARMident()
-    #print "Debug State: %x"%client.ARMget_dbgstate ()
+    print "Debug State: %x"%client.ARMget_dbgstate ()
     client.writecmd(0x33,0xd0,4,[0xf7,0xf7,0xf7,0xf7])
     print "Loopback:   \t %s"%repr(client.data)                  # loopback
     print "Debug State: %x"%client.ARMget_dbgstate ()
diff --git a/firmware/Makefile b/firmware/Makefile
index 2e7b08b..55953d2 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -24,8 +24,9 @@ CC=msp430-gcc -Wall -Os -g -mmcu=$(mcu) -D$(mcu) -DGCC $(GCCINC) -I include $(CC
 
 #Define extra modules here.
 moreapps?=apps/i2c/i2c.o apps/chipcon/chipcon.o apps/glitch/glitch.o apps/jtag/sbw.o apps/smartcard/smartcard.o
+# should include apps/jtag/jtagarm7tdmi.o to build jtag for ARM7
 
-apps= $(moreapps) apps/monitor/monitor.o apps/spi/spi.o   apps/jtag/jtag.o apps/jtag/jtag430.o apps/jtag/jtag430x2.o apps/avr/avr.o apps/jtag/ejtag.o apps/jtag/jtagxscale.o apps/jtag/jtagarm7tdmi-disabled.o
+apps= $(moreapps) apps/monitor/monitor.o apps/spi/spi.o   apps/jtag/jtag.o apps/jtag/jtag430.o apps/jtag/jtag430x2.o apps/avr/avr.o apps/jtag/ejtag.o apps/jtag/jtagxscale.o
 libs= lib/$(mcu).o lib/command.o apps/jtag/jtag430asm.o apps/chipcon/chipconasm.o
 app= goodfet
 
diff --git a/firmware/apps/jtag/jtagarm7tdmi.c b/firmware/apps/jtag/jtagarm7tdmi.c
index 776029b..cf85067 100644
--- a/firmware/apps/jtag/jtagarm7tdmi.c
+++ b/firmware/apps/jtag/jtagarm7tdmi.c
@@ -139,21 +139,21 @@ unsigned long jtagarm7tdmi_start() {
   jtagsetup();
   //Known-good starting position.
   //Might be unnecessary.
-  SETTST;
-  SETRST;
+  //SETTST;
+  //SETRST;
   
-  delay(0x2);
+  //delay(0x2);
   
-  CLRRST;
-  delay(2);
-  CLRTST;
+  //CLRRST;
+  //delay(2);
+  //CLRTST;
 
-  msdelay(10);
-  SETRST;
+  //msdelay(10);
+  //SETRST;
   /*
   P5DIR &=~RST;
   */
-  delay(0x2);
+  //delay(0x2);
   jtagarm7tdmi_resettap();
   return jtagarm7tdmi_idcode();
 }
@@ -507,41 +507,22 @@ void jtagarm7tdmi_disable_watchpoint1(){
 
 
 /******************** Complex Commands **************************/
-//! Push an instruction into the CPU pipeline
-//  NOTE!  Must provide EXECNOPARM for parameter if no parm is required.
-unsigned long test_exec(unsigned long instr, unsigned long parameter, unsigned char systemspeed) {
-  unsigned long retval;
-
-  cmddatalong[1] = jtagarm7tdmi_nop( 0);
-  cmddatalong[2] = jtagarm7tdmi_nop(systemspeed);
-  cmddatalong[3] = jtagarm7tdmi_instr_primitive(instr, 0);      // write 32-bit instruction code into DR
-  cmddatalong[4] = jtagarm7tdmi_nop( 0);
-  cmddatalong[5] = jtagarm7tdmi_nop( 0);
-  cmddatalong[6] = jtagarm7tdmi_instr_primitive(parameter, 0);  // inject long
-  cmddatalong[7] = jtagarm7tdmi_nop( 0);
-  cmddatalong[8] = jtagarm7tdmi_nop( 0);
-  cmddatalong[9] = jtagarm7tdmi_nop( 0);
-  retval = cmddatalong[9];
-
-  return(retval);
-}
-
 
 //! Push an instruction into the CPU pipeline
 //  NOTE!  Must provide EXECNOPARM for parameter if no parm is required.
 unsigned long jtagarm7tdmi_exec(unsigned long instr, unsigned long parameter, unsigned char systemspeed) {
   unsigned long retval;
 
-  cmddatalong[1] = jtagarm7tdmi_nop( 0);
-  cmddatalong[2] = jtagarm7tdmi_nop(systemspeed);
-  cmddatalong[3] = jtagarm7tdmi_instr_primitive(instr, 0);      // write 32-bit instruction code into DR
-  cmddatalong[4] = jtagarm7tdmi_nop( 0);
-  cmddatalong[5] = jtagarm7tdmi_nop( 0);
-  cmddatalong[6] = jtagarm7tdmi_instr_primitive(parameter, 0);  // inject long
-  cmddatalong[7] = jtagarm7tdmi_nop( 0);
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop(systemspeed));
+  debughex32(jtagarm7tdmi_instr_primitive(instr, 0));      // write 32-bit instruction code into DR
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_instr_primitive(parameter, 0));  // inject long
+  debughex32(jtagarm7tdmi_nop( 0));
   retval = jtagarm7tdmi_nop( 0);
-  cmddatalong[9] = jtagarm7tdmi_nop( 0);
-  cmddatalong[8] = retval;
+  debughex32(retval);
+  debughex32(jtagarm7tdmi_nop( 0));
 
   return(retval);
 }
@@ -550,137 +531,92 @@ unsigned long jtagarm7tdmi_exec(unsigned long instr, unsigned long parameter, un
 unsigned long jtagarm7tdmi_get_register(unsigned char reg) {
   unsigned long retval = 0, instr;
   // push nop into pipeline - clean out the pipeline...
-  cmddatalong[2] = jtagarm7tdmi_nop( 0);
-
   instr = ARM_READ_REG | (reg<<12);                     // push STR Rx, [R14] into pipeline
-  cmddatalong[1] = jtagarm7tdmi_instr_primitive(instr, 0);
-  cmddatalong[2] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - fetched
-  cmddatalong[3] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - decoded
-  cmddatalong[4] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - executed 
-  retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
-  cmddatalong[5] = retval;
-  cmddatalong[6] = jtagarm7tdmi_nop( 0);
-  cmddatalong[7] = jtagarm7tdmi_nop( 0);
-  cmddatalong[8] = jtagarm7tdmi_nop( 0);
-  return retval;
-}
-
-//! Retrieve a 32-bit Register value
-unsigned long test_get_register(unsigned char reg) {
-  unsigned long retval = 0, instr;
-  // push nop into pipeline - clean out the pipeline...
-  cmddatalong[2] = jtagarm7tdmi_nop( 0);
 
-  instr = ARM_READ_REG | (reg<<12);                     // push STR Rx, [R14] into pipeline
-  cmddatalong[1] = jtagarm7tdmi_instr_primitive(instr, 0);      // fetch
-  cmddatalong[2] = jtagarm7tdmi_nop( 0);                        // decode
-  cmddatalong[3] = jtagarm7tdmi_nop( 0);                        // execute
-  cmddatalong[4] = jtagarm7tdmi_nop( 0);                        // ??? what happens here ???
-  retval = jtagarm7tdmi_nop( 0);                                // recover 32-bit word
-  cmddatalong[5] = retval;
-  cmddatalong[6] = jtagarm7tdmi_nop( 0);
-  cmddatalong[7] = jtagarm7tdmi_nop( 0);
-  cmddatalong[8] = jtagarm7tdmi_nop( 0);
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_instr_primitive(instr, 0));
+  debughex32(jtagarm7tdmi_nop( 0));                // push nop into pipeline - fetched
+  debughex32(jtagarm7tdmi_nop( 0));                // push nop into pipeline - decoded
+  jtagarm7tdmi_nop( 0);                // push nop into pipeline - executed 
+  retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
+  debughex32(retval);
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
   return retval;
 }
 
 //! Set a 32-bit Register value
 unsigned long jtagarm7tdmi_set_register(unsigned char reg, unsigned long val) {
   unsigned long retval = 0, instr;
-  cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
+  instr = ARM_WRITE_REG | (reg<<12);                // push LDR Rx, [R14] into pipeline
 
-  instr = ARM_WRITE_REG | (reg<<12);     // push LDR Rx, [R14] into pipeline
-  cmddatalong[2] = jtagarm7tdmi_instr_primitive(instr, 0); // push nop into pipeline - fetch
-  cmddatalong[3] = jtagarm7tdmi_nop( 0); // push nop into pipeline - decode
-  cmddatalong[4] = jtagarm7tdmi_instr_primitive(val-16, 0); // push 32-bit word on data bus
-  //cmddatalong[4] = jtagarm7tdmi_nop( 0); // push nop into pipeline - execute
+  debughex32(jtagarm7tdmi_nop( 0));            // push nop into pipeline - clean out the pipeline...
+  debughex32(jtagarm7tdmi_instr_primitive(instr, 0)); // push nop into pipeline - fetch
+  debughex32(jtagarm7tdmi_nop( 0));            // push nop into pipeline - decode
+  debughex32(jtagarm7tdmi_nop( 0));            // push nop into pipeline - execute
   
-  cmddatalong[5] = jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
-  cmddatalong[6] = jtagarm7tdmi_instr_primitive(val+16, 0); // push 32-bit word on data bus
-  //cmddatalong[6] = jtagarm7tdmi_nop( 0); // push nop into pipeline - executed 
+  debughex32(jtagarm7tdmi_instr_primitive(val, 0)); // push 32-bit word on data bus
+  debughex32(jtagarm7tdmi_nop( 0));            // push nop into pipeline - executed 
 
-  if (reg == ARM_REG_PC){
-    cmddatalong[7] = jtagarm7tdmi_nop( 0);
-    cmddatalong[8] = jtagarm7tdmi_nop( 0);
-  }
-  cmddatalong[9] = jtagarm7tdmi_nop( 0);
+  //if (reg == ARM_REG_PC){
+    debughex32(jtagarm7tdmi_nop( 0));
+    debughex32(jtagarm7tdmi_nop( 0));
+  //}
+  debughex32(jtagarm7tdmi_nop( 0));
 
   retval = cmddatalong[5];
   return(retval);
 }
 
-//! Set a 32-bit Register value
-unsigned long test_set_register(unsigned char reg, unsigned long val) {
-  unsigned long retval = 0, instr;
-  cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
-
-  instr = ARM_WRITE_REG | (reg<<12);     // push LDR Rx, [R14] into pipeline
-  cmddatalong[2] = jtagarm7tdmi_instr_primitive(instr, 0);
-  
-  cmddatalong[3] = jtagarm7tdmi_instr_primitive(val+32, 0); // push 32-bit word on data bus - execute state
-  cmddatalong[4] = jtagarm7tdmi_instr_primitive(val+16, 0); // push 32-bit word on data bus - execute state
-  cmddatalong[5] = jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus - execute state
-  cmddatalong[6] = jtagarm7tdmi_instr_primitive(val-16, 0); // push 32-bit word on data bus - execute state
-
-  if (reg == ARM_REG_PC){
-    cmddatalong[7] = jtagarm7tdmi_nop( 0);
-    cmddatalong[8] = jtagarm7tdmi_nop( 0);
-  }
-  cmddatalong[9] = jtagarm7tdmi_instr_primitive(val-32, 0); // push 32-bit word on data bus - execute state
-
-  retval = cmddatalong[5];
-  return(retval);
-}
-
-
 
 
 //! Get all registers.  Return an array
 unsigned long* jtagarm7tdmi_get_registers() {
-  cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0);
-  cmddatalong[2] = jtagarm7tdmi_nop( 0);
-  cmddatalong[3] = jtagarm7tdmi_nop( 0);
-  cmddatalong[4] = jtagarm7tdmi_nop( 0);
-  cmddatalong[5] = jtagarm7tdmi_nop( 0);
-  cmddatalong[6] = jtagarm7tdmi_nop( 0);
-  cmddatalong[7] = jtagarm7tdmi_nop( 0);
-  cmddatalong[8] = jtagarm7tdmi_nop( 0);
-  cmddatalong[9] = jtagarm7tdmi_nop( 0);
-  cmddatalong[10] = jtagarm7tdmi_nop( 0);
-  cmddatalong[11] = jtagarm7tdmi_nop( 0);
-  cmddatalong[12] = jtagarm7tdmi_nop( 0);
-  cmddatalong[13] = jtagarm7tdmi_nop( 0);
-  cmddatalong[14] = jtagarm7tdmi_nop( 0);
-  cmddatalong[15] = jtagarm7tdmi_nop( 0);
-  cmddatalong[16] = jtagarm7tdmi_nop( 0);
-  cmddatalong[17] = jtagarm7tdmi_nop( 0);
-  cmddatalong[18] = jtagarm7tdmi_nop( 0);
-  cmddatalong[19] = jtagarm7tdmi_nop( 0);
-  cmddatalong[20] = jtagarm7tdmi_nop( 0);
+  debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
   return registers;
 }
 
 //! Get all registers.  Return an array
-unsigned long* jtagarm7tdmi_set_registers() {
-  cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0);
-  cmddatalong[2] = jtagarm7tdmi_nop( 0);
-  cmddatalong[3] = jtagarm7tdmi_nop( 0);
-  cmddatalong[4] = jtagarm7tdmi_instr_primitive(0x40,0);
-  cmddatalong[5] = jtagarm7tdmi_instr_primitive(0x41,0);
-  cmddatalong[6] = jtagarm7tdmi_instr_primitive(0x42,0);
-  cmddatalong[7] = jtagarm7tdmi_instr_primitive(0x43,0);
-  cmddatalong[8] = jtagarm7tdmi_instr_primitive(0x44,0);
-  cmddatalong[9] = jtagarm7tdmi_instr_primitive(0x45,0);
-  cmddatalong[10] = jtagarm7tdmi_instr_primitive(0x46,0);
-  cmddatalong[11] = jtagarm7tdmi_instr_primitive(0x47,0);
-  cmddatalong[12] = jtagarm7tdmi_instr_primitive(0x48,0);
-  cmddatalong[13] = jtagarm7tdmi_instr_primitive(0x49,0);
-  cmddatalong[14] = jtagarm7tdmi_instr_primitive(0x4a,0);
-  cmddatalong[15] = jtagarm7tdmi_instr_primitive(0x4b,0);
-  cmddatalong[16] = jtagarm7tdmi_instr_primitive(0x4c,0);
-  cmddatalong[17] = jtagarm7tdmi_instr_primitive(0x4d,0);
-  cmddatalong[18] = jtagarm7tdmi_instr_primitive(0x4e,0);
-  cmddatalong[19] = jtagarm7tdmi_instr_primitive(0x4f,0);
+unsigned long* jtagarm7tdmi_set_registers() {   //FIXME: BORKEN... TOTALLY TRYING TO BUY A VOWEL
+  debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_nop( 0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x40,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x41,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x42,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x43,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x44,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x45,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x46,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x47,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x48,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x49,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x4a,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x4b,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x4c,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x4d,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x4e,0));
+  debughex32(jtagarm7tdmi_instr_primitive(0x4f,0));
   return registers;
 }
 
@@ -1334,4 +1270,4 @@ IRQ mode shadow registers
 Undefined instruction mode shadow registers
   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
 >
-
+*/
diff --git a/firmware/include/command.h b/firmware/include/command.h
index dda9b64..743a008 100644
--- a/firmware/include/command.h
+++ b/firmware/include/command.h
@@ -115,10 +115,14 @@ void txlong(unsigned long l);
 //! Transmit a word.
 void txword(unsigned int l);
 
+//! Transmit a debug sequence of bytes
+void debugbytes(const char *bytes, unsigned int len);
 //! Transmit a debug string.
 void debugstr(const char *str);
 //! brief Debug a hex word string.
 void debughex(u16 v);
+//! brief Debug a hex long string.
+void debughex32(u32 v);
 
 //! Delay for a count.
 void delay(unsigned int count);
diff --git a/firmware/lib/command.c b/firmware/lib/command.c
index 1de6f0a..571d0c0 100644
--- a/firmware/lib/command.c
+++ b/firmware/lib/command.c
@@ -32,24 +32,12 @@ void debugstr(const char *str){
 
 //! brief Debug a hex word string.
 void debughex(u16 v) {
-  unsigned char a[7];
-  a[0]='0'; a[1]='x';
-    
-  a[2]=0xf&(v>>12);
-  a[2]+=(a[2]>9)?('a'-10):'0';
-
-  a[3]=0xf&(v>>8);
-  a[3]+=(a[3]>9)?('a'-10):'0';
-
-  a[4]=0xf&(v>>4);
-  a[4]+=(a[4]>9)?('a'-10):'0';
-
-  a[5]=0xf&(v>>0);
-  a[5]+=(a[5]>9)?('a'-10):'0';
-
-  a[6]=0;
+  debugbytes((void *)&v, 2);
+}
 
-  txstring(0xFF,0xFF,a);
+//! brief Debug a hex word string.
+void debughex32(u32 v) {
+  debugbytes((void *)&v, 4);
 }
 
 /*! \brief Transmit debug bytes.