w00t! jtag arm is a reality on goodfet.
authordodge-this <dodge-this@12e2690d-a6be-4b82-a7b7-67c4a43b65c8>
Thu, 8 Jul 2010 19:03:11 +0000 (19:03 +0000)
committerdodge-this <dodge-this@12e2690d-a6be-4b82-a7b7-67c4a43b65c8>
Thu, 8 Jul 2010 19:03:11 +0000 (19:03 +0000)
got getCPSR and setCPSR working.
working on memory reading/writing still.

about to strip the whole thing out of goodfet firmware code into python, leaving the goodfet firmware code must slimmer.

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

client/GoodFETARM.py
client/gplay-arm.py
firmware/apps/jtag/jtagarm7tdmi.c
firmware/include/jtagarm7tdmi.h

index 3305820..889f2e4 100644 (file)
@@ -55,6 +55,14 @@ SET_SPSR            = 0x9b
 SET_MODE_THUMB      = 0x9c
 SET_MODE_ARM        = 0x9d
 
+
+platforms = {
+    "at91sam7": {0:(0x100000, "Flash before remap, SRAM after remap"),
+                 0x100000: (0x100000, "Internal Flash"),
+                 0x200000: (0x100000, "Internal SRAM"),
+                 },
+    }
+                
 from GoodFET import GoodFET
 from intelhex import IntelHex
 
@@ -71,6 +79,8 @@ class GoodFETARM(GoodFET):
         self.writecmd(0x13,RESUMECPU,0,self.data)
     def ARMsetModeArm(self):
         self.writecmd(0x13,SET_MODE_ARM,0,self.data)
+    def ARMsetModeThumb(self):
+        self.writecmd(0x13,SET_MODE_THUMB,0,self.data)
     def ARMtest(self):
         #self.ARMreleasecpu()
         #self.ARMhaltcpu()
@@ -145,9 +155,9 @@ class GoodFETARM(GoodFET):
     def ARMset_dbgctrl(self,config):
         """Write the config register of an ARM."""
         self.writecmd(0x13,SET_DEBUG_CTRL,1,[config&7])
-    def ARMlockchip(self):
-        """Set the flash lock bit in info mem."""
-        self.writecmd(0x13, LOCKCHIP, 0, [])
+    #def ARMlockchip(self):
+    #    """Set the flash lock bit in info mem."""
+    #    self.writecmd(0x13, LOCKCHIP, 0, [])
     
 
     def ARMidentstr(self):
@@ -192,6 +202,14 @@ class GoodFETARM(GoodFET):
         self.writecmd(0x13,SET_REGISTERS,16*4,regarry)
         retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
         return retval
+    def ARMget_regCPSR(self):
+        """Get an ARM's Register"""
+        self.writecmd(0x13,GET_CPSR,0,[])
+        retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
+        return retval
+    def ARMset_regCPSR(self, val):
+        """Get an ARM's Register"""
+        self.writecmd(0x13,SET_CPSR,4,[val&0xff, (val>>8)&0xff, (val>>16)&0xff, val>>24])
     def ARMcmd(self,phrase):
         self.writecmd(0x13,READ,len(phrase),phrase)
         val=ord(self.data[0])
@@ -202,29 +220,30 @@ class GoodFETARM(GoodFET):
             instr = struct.pack("<L", instr)
         self.writecmd(0x13,DEBUG_INSTR,len(instr),instr)
         return (self.data[0])
-    def ARMpeekcodebyte(self,adr,words):
+    def ARMpeekcodewords(self,adr,words):
         """Read the contents of code memory at an address."""
         self.data=[adr&0xff, (adr>>8)&0xff, (adr>>16)&0xff, (adr>>24)&0xff, words&0xff, (words>>8)&0xff, (words>>16)&0xff, (words>>24)&0xff ]
-        self.writecmd(0x13,PEEK,2,self.data)
+        self.writecmd(0x13,READ_CODE_MEMORY,8,self.data)
         retval = []
         retval.append(self.serialport.read(words*4))
         #retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
         return "".join(retval)
     def ARMpeekdatabyte(self,adr):
         """Read the contents of data memory at an address."""
-        self.data=[adr&0xff, (adr&0xff00)>>8, (adr&0xff0000)>>16, (adr&0xff000000)>>24]
-        self.writecmd(0x13, PEEK, 2, self.data)
+        self.data=[ adr&0xff, (adr>>8)&0xff, (adr>>16)&0xff, (adr>>24)&0xff ]
+        self.writecmd(0x13, PEEK, 4, self.data)
+        #retval.append(self.serialport.read(words*4))
         retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
         return retval
     def ARMpokedatabyte(self,adr,val):
         """Write a byte to data memory."""
-        self.data=[adr&0xff, (adr&0xff00)>>8, val]
-        self.writecmd(0x13, POKE, 3, self.data)
+        self.data=[adr&0xff, (adr>>8)&0xff, (adr>>16)&0xff, (adr>>24)&0xff, val&0xff, (val>>8)&0xff, (val>>16)&0xff, (val>>24)&0xff ]
+        self.writecmd(0x13, POKE, 8, self.data)
         retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
         return retval
-    def ARMchiperase(self):
-        """Erase all of the target's memory."""
-        self.writecmd(0x13,CHIP_ERASE,0,[])
+    #def ARMchiperase(self):
+    #    """Erase all of the target's memory."""
+    #    self.writecmd(0x13,CHIP_ERASE,0,[])
     def ARMstatus(self):
         """Check the status."""
         self.writecmd(0x13,GET_DEBUG_STATE,0,[])
@@ -255,9 +274,9 @@ class GoodFETARM(GoodFET):
     def start(self):
         """Start debugging."""
         self.writecmd(0x13,START,0,self.data)
-        #ident=self.ARMidentstr()
-        #print "Target identifies as %s." % ident
-        #print "Status: %s." % self.ARMstatusstr()
+        ident=self.ARMidentstr()
+        print "Target identifies as %s." % ident
+        print "Status: %s." % self.ARMstatusstr()
         #self.ARMreleasecpu()
         #self.ARMhaltcpu()
         #print "Status: %s." % self.ARMstatusstr()
@@ -265,17 +284,17 @@ class GoodFETARM(GoodFET):
     def stop(self):
         """Stop debugging."""
         self.writecmd(0x13,STOP,0,self.data)
-    def ARMstep_instr(self):
-        """Step one instruction."""
-        self.writecmd(0x13,STEP_INSTR,0,self.data)
-    def ARMflashpage(self,adr):
-        """Flash 2kB a page of flash from 0xF000 in XDATA"""
-        data=[adr&0xFF,
-              (adr>>8)&0xFF,
-              (adr>>16)&0xFF,
-              (adr>>24)&0xFF]
-        print "Flashing buffer to 0x%06x" % adr
-        self.writecmd(0x13,MASS_FLASH_PAGE,4,data)
+    #def ARMstep_instr(self):
+    #    """Step one instruction."""
+    #    self.writecmd(0x13,STEP_INSTR,0,self.data)
+    #def ARMflashpage(self,adr):
+    #    """Flash 2kB a page of flash from 0xF000 in XDATA"""
+    #    data=[adr&0xFF,
+    #          (adr>>8)&0xFF,
+    #          (adr>>16)&0xFF,
+    #          (adr>>24)&0xFF]
+    #    print "Flashing buffer to 0x%06x" % adr
+    #    self.writecmd(0x13,MASS_FLASH_PAGE,4,data)
 
     def writecmd(self, app, verb, count=0, data=[]):
         """Write a command and some data to the GoodFET."""
index f216a1f..f8dd394 100755 (executable)
@@ -27,6 +27,89 @@ def regwratchet(num,hi=13,lo=0):
     for x in xrange(lo,hi+1):
         client.ARMset_register(x,num)
 
+def test():
+    print "ARM JTAG Test Unit"
+    print " Chip ID", hex(client.ARMident())
+    print client.ARMidentstr()
+    print " Debug State: ",hex(client.ARMget_dbgstate())
+    print " Debug CTRL:  ",hex(client.ARMget_dbgctrl())
+    print
+    print "Testing Register Read/Writes"
+    print " Storing old values"
+    originalregs = [client.ARMget_register(x) for x in xrange(16)]
+
+    print "Simple test"
+    test = [None for x in xrange(15)]
+    control = [x for x in xrange(15)]
+    for x in xrange(15):
+        client.ARMset_register(x,control[x])
+        test[x] = client.ARMget_register(x)
+    if control != test:
+        print "FAIL"
+        print "Control:",control
+        print "Test:   ",test
+
+    print "A little harder..."
+    test = [None for x in xrange(15)]
+    control = [4*x for x in xrange(15)]
+    for x in xrange(15):
+        client.ARMset_register(x,control[x])
+        test[x] = client.ARMget_register(x)
+    if control != test:
+        print "FAIL"
+        print "Control:",control
+        print "Test:   ",test
+
+    print "Complex testing 1"
+    for y in xrange(0,0xffffffff,0x10101010):
+            test = [None for x in xrange(15)]
+            control = [y for x in xrange(15)]
+            print " Test parms: ",control
+            for x in xrange(15):
+                client.ARMset_register(x,control[x])
+                test[x] = client.ARMget_register(x)
+            if control != test:
+                print "FAIL"
+                print "Control:",control
+                print "Test:   ",test
+
+    print "Complex testing 2"
+    for y in xrange(0,0xffffffff,101010101):
+            test = [None for x in xrange(15)]
+            control = [y for x in xrange(15)]
+            print " Test parms: ",control
+            for x in xrange(15):
+                client.ARMset_register(x,control[x])
+                test[x] = client.ARMget_register(x)
+            if control != test:
+                print "FAIL"
+                print "Control:",control
+                print "Test:   ",test
+
+    test = [None for x in xrange(15)]
+    control = [0x100*x for x in xrange(15)]
+    for x in xrange(15):
+        client.ARMset_register(x,control[x])
+        test[x] = client.ARMget_register(x)
+    if control != test:
+        print "FAIL"
+        print "Control:",control
+        print "Test:   ",test
+
+    print "Resetting original register values..."
+    for x in xrange(16):
+        client.ARMset_register(x,originalregs[x])
+    regs = [client.ARMget_register(x) for x in xrange(16)]
+    print "Original:           \t",originalregs
+    print "Now (r15 may differ:\t",regs
+
+    print "Testing setting and movement of PC between instructions"
+    #client.ARMsetPC(0x0)
+    print "PC:",client.ARMgetPC()
+    print "PC:",client.ARMgetPC()
+    print "PC:",client.ARMgetPC()
+    print "PC:",client.ARMgetPC()
+
 def test1():
     global data
     print "\n\nTesting JTAG for ARM\n"
index cf708ae..61d851f 100644 (file)
@@ -94,7 +94,7 @@ for this module, we keep tck high for all changes/sampling, and then bounce it.
 
 
 /************************** JTAGARM7TDMI Primitives ****************************/
-void jtag_goto_shift_ir() {
+/*void jtag_goto_shift_ir() {
   SETTMS;
   jtag_arm_tcktock();
   jtag_arm_tcktock();
@@ -103,7 +103,6 @@ void jtag_goto_shift_ir() {
   jtag_arm_tcktock();
 
 }
-
 void jtag_goto_shift_dr() {
   SETTMS;
   jtag_arm_tcktock();
@@ -111,6 +110,7 @@ void jtag_goto_shift_dr() {
   jtag_arm_tcktock();
   jtag_arm_tcktock();
 }
+*/
 
 void jtag_reset_to_runtest_idle() {
   SETTMS;
@@ -357,6 +357,8 @@ unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){
 
 //! push NOP into the instruction pipeline
 unsigned long jtagarm7tdmi_nop(char breakpt){  // PROVEN
+  if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT) 
+    return jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP, breakpt);
   return jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP, breakpt);
 }
 
@@ -373,18 +375,47 @@ NOP
 */
 
 //! set the current mode to ARM, returns PC (FIXME).  Should be used by haltcpu(), which should also store PC and the THUMB state, for use by releasecpu();
-unsigned long jtagarm7tdmi_setMode_ARM(){               // PROVEN
-  debugstr("=== Thumb Mode... Switching to ARM mode ===");
+unsigned long jtagarm7tdmi_setMode_ARM(unsigned char restart){               // PROVEN  BUT FUGLY! FIXME: clean up and store and replace clobbered r0
+  debugstr("=== Switching to ARM mode ===");
   unsigned long retval = 0xffL;
-  while ((jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
-    cmddataword[6] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
+  while ((current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
+    cmddataword[9] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
     cmddataword[1] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
     cmddataword[2] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_MOV_R0_PC,0);
     cmddataword[3] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
     cmddataword[4] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_BX_PC,0);
     cmddataword[5] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
     cmddataword[6] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
+    cmddataword[7] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
     jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
+    jtagarm7tdmi_set_register(0,cmddataword[4]);
+    debugstr("PC:");
+    debughex32(cmddataword[6]);
+    debughex32(cmddataword[7]);
+    debughex32(cmddataword[9]);
+  }
+  return(retval);
+}
+
+
+//! set the current mode to ARM, returns PC (FIXME).  Should be used by haltcpu(), which should also store PC and the THUMB state, for use by releasecpu();
+unsigned long jtagarm7tdmi_setMode_THUMB(unsigned char restart){               // PROVEN
+  debugstr("=== Switching to THUMB mode ===");
+  unsigned long retval = 0xffL;
+  while (!(current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
+    last_halt_pc |= 1;
+    cmddataword[9] = jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
+    jtagarm7tdmi_set_register(0, last_halt_pc);
+    cmddataword[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_BX_R0,0);
+    if (restart) {
+      jtagarm7tdmi_restart();
+    } else {
+      jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
+      jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
+      jtagarm7tdmi_resettap();                  // seems necessary for some reason.
+    }
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
   }
   return(retval);
 }
@@ -396,9 +427,6 @@ unsigned long jtagarm7tdmi_setMode_ARM(){               // PROVEN
 //! shifter for writing to chain2 (EmbeddedICE). 
 unsigned long eice_write(unsigned char reg, unsigned long data){
   unsigned long retval, temp;
-  //debugstr("eice_write");
-  //debughex(reg);
-  //debughex32(data);
   jtagarm7tdmi_scan_intest(2);
   // Now shift in the 32 bits
   SHIFT_DR;
@@ -406,11 +434,6 @@ unsigned long eice_write(unsigned char reg, unsigned long data){
   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);              // send in the register address - 5 bits lsb
   jtagarmtransn(1, 1, LSB, END, RETIDLE);                           // send in the WRITE bit
   
-  //SETTMS;   // Last Bit - Exit UPDATE_DR
-  //// is this update a read/write or just read?
-  //SETMOSI;
-  //jtag_arm_tcktock();
-  
   return(retval); 
 }
 
@@ -531,78 +554,52 @@ unsigned long jtagarm7tdmi_exec(unsigned long instr, unsigned long parameter, un
 
 //! Retrieve a 32-bit Register value
 unsigned long jtagarm7tdmi_get_register(unsigned long reg) {
-  unsigned long retval=0L, instr, reg2=0L;
-  reg2 = (reg&0xfL)<<16;
-  // push nop into pipeline - clean out the pipeline...
-  instr = (unsigned long)(reg<<12L) | (unsigned long)ARM_READ_REG;   // STR Rx, [R14] 
-  instr ^= reg2;
-  //instr = (unsigned long)(((unsigned long)reg<<12) | ARM_READ_REG); 
-  //debugstr("Reading:");
-  //debughex32(instr);
+  unsigned long retval=0L, instr;
+  if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
+    instr = THUMB_INSTR_STR_R0_r0 | reg | (reg<<16);
+  else
+    instr = (unsigned long)(reg<<12L) | (unsigned long)ARM_READ_REG;   // STR Rx, [R14] 
 
-  jtagarm7tdmi_nop( 0);
   jtagarm7tdmi_nop( 0);
   jtagarm7tdmi_nop( 0);
   jtagarm7tdmi_instr_primitive(instr, 0);
-  //debughex32(jtagarm7tdmi_nop( 0));                // push nop into pipeline - fetched
-  //debughex32(jtagarm7tdmi_nop( 0));                // push nop into pipeline - decoded
-  //debughex32(jtagarm7tdmi_nop( 0));                // push nop into pipeline - executed 
   jtagarm7tdmi_nop( 0);
   jtagarm7tdmi_nop( 0);
   jtagarm7tdmi_nop( 0);
   retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
-  //debughex32(retval);
-  //debughex32(jtagarm7tdmi_nop( 0));
-  jtagarm7tdmi_nop( 0);
-  jtagarm7tdmi_nop( 0);
-  jtagarm7tdmi_nop( 0);
   return retval;
 }
 
 //! Set a 32-bit Register value
 void jtagarm7tdmi_set_register(unsigned long reg, unsigned long val) {
-  unsigned long instr, reg2=0;
-  reg2 = (reg&0xfL)<<16;
+  unsigned long instr;
   instr = (unsigned long)(((unsigned long)reg<<12L) | ARM_WRITE_REG); //  LDR Rx, [R14]
-  //instr ^= reg2;
-  //instr |= (unsigned long)((((unsigned long)reg)&0x7)<<8)<<8;
-  //debugstr("Writing:");
-  //debughex32(instr);
-  //debughex32(val);
+  
   jtagarm7tdmi_nop( 0);            // push nop into pipeline - clean out the pipeline...
   jtagarm7tdmi_nop( 0);            // push nop into pipeline - clean out the pipeline...
   jtagarm7tdmi_instr_primitive(instr, 0); // push instr into pipeline - fetch
-  
   if (reg == ARM_REG_PC){
-  //jtagarm7tdmi_nop( 0);            // push nop into pipeline - execute
     jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
     jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
     jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
-    jtagarm7tdmi_nop( 0);
-    jtagarm7tdmi_nop( 0);
   } else {
-  jtagarm7tdmi_nop( 0);            // push nop into pipeline - decode
-  jtagarm7tdmi_nop( 0);            // push nop into pipeline - execute
-    //jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
+    jtagarm7tdmi_nop( 0);            // push nop into pipeline - decode
+    jtagarm7tdmi_nop( 0);            // push nop into pipeline - execute
     jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
-    //jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
-    jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
-    jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
   }
+  jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
+  jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
   jtagarm7tdmi_nop( 0);
 }
 
 
 
-//! Get all registers, placing them into cmddatalong[0-15]
+//! Get all registers, placing them into cmddatalong[0-14]
 void jtagarm7tdmi_get_registers() {
-  debugstr("First 8 registers:");
-  debugstr("   Instr and the first few pops from the instruction chain:");
-  debughex32(ARM_INSTR_SKANKREGS1);
-  debughex32(jtagarm7tdmi_nop( 0));
-  debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS1,0));
-  debughex32(jtagarm7tdmi_nop( 0));
-  debughex32(jtagarm7tdmi_nop( 0));
+  jtagarm7tdmi_nop( 0);
+  jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0);
+  jtagarm7tdmi_nop( 0);
+  jtagarm7tdmi_nop( 0);
   cmddatalong[ 0] = jtagarm7tdmi_nop( 0);
   cmddatalong[ 1] = jtagarm7tdmi_nop( 0);
   cmddatalong[ 2] = jtagarm7tdmi_nop( 0);
@@ -611,17 +608,6 @@ void jtagarm7tdmi_get_registers() {
   cmddatalong[ 5] = jtagarm7tdmi_nop( 0);
   cmddatalong[ 6] = jtagarm7tdmi_nop( 0);
   cmddatalong[ 7] = jtagarm7tdmi_nop( 0);
-
-  debugstr("Last 8 registers:");
-  debugstr("   Instr and the first few pops from the instruction chain:");
-  debughex32(ARM_INSTR_SKANKREGS2);
-  debughex32(jtagarm7tdmi_nop( 0));
-  //jtagarm7tdmi_nop( 0);
-  debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS2,0));
-  debughex32(jtagarm7tdmi_nop( 0));
-  debughex32(jtagarm7tdmi_nop( 0));
-  //jtagarm7tdmi_nop( 0);
-  //jtagarm7tdmi_nop( 0);
   cmddatalong[ 8] = jtagarm7tdmi_nop( 0);
   cmddatalong[ 9] = jtagarm7tdmi_nop( 0);
   cmddatalong[10] = jtagarm7tdmi_nop( 0);
@@ -633,75 +619,91 @@ void jtagarm7tdmi_get_registers() {
   jtagarm7tdmi_nop( 0);
 }
 
-//! Set all registers from cmddatalong[0-15]
-void jtagarm7tdmi_set_registers() {   //FIXME: BORKEN... TOTALLY TRYING TO BUY A VOWEL
-  debughex32(ARM_INSTR_CLOBBEREGS);
+//! Set all registers from cmddatalong[0-14]
+void jtagarm7tdmi_set_registers() {   // using r15 to write through.  not including it.  use set_pc
   jtagarm7tdmi_nop( 0);
   debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_CLOBBEREGS,0));
   jtagarm7tdmi_nop( 0);
   jtagarm7tdmi_nop( 0);
-  debughex32(jtagarm7tdmi_instr_primitive(0x40L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x41L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x42L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x43L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x44L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x45L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x46L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x47L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x48L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x49L,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x4aL,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x4bL,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x4cL,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x4dL,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x4eL,0));
-  debughex32(jtagarm7tdmi_instr_primitive(0x4fL,0));
+  jtagarm7tdmi_instr_primitive(cmddatalong[0],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[1],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[2],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[3],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[4],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[5],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[6],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[7],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[8],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[9],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[10],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[11],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[12],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[13],0);
+  jtagarm7tdmi_instr_primitive(cmddatalong[14],0);
+  jtagarm7tdmi_nop( 0);
 }
 
 //! Retrieve the CPSR Register value
 unsigned long jtagarm7tdmi_get_regCPSR() {
-  unsigned long retval = 0L;
-
-  debughex32(jtagarm7tdmi_nop( 0)); // push nop into pipeline - clean out the pipeline...
-  debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_MRS_R0_CPSR, 0)); // push MRS_R0, CPSR into pipeline
-  debughex32(jtagarm7tdmi_nop( 0)); // push nop into pipeline - fetched
-  debughex32(jtagarm7tdmi_nop( 0)); // push nop into pipeline - decoded
-  debughex32(jtagarm7tdmi_nop( 0)); // push nop into pipeline - executed 
-  retval = jtagarm7tdmi_nop( 0);        // recover 32-bit word
-  debughex32(retval);
+  unsigned long retval = 0L, r0;
+
+  r0 = jtagarm7tdmi_get_register(0);
+  jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
+  jtagarm7tdmi_instr_primitive(ARM_INSTR_MRS_R0_CPSR, 0); // push MRS_R0, CPSR into pipeline - fetch
+  jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded
+  jtagarm7tdmi_nop( 0); // push nop into pipeline - execute
+  retval = jtagarm7tdmi_get_register(0);
+  jtagarm7tdmi_set_register(0, r0);
   return retval;
 }
 
 //! Retrieve the CPSR Register value
 unsigned long jtagarm7tdmi_set_regCPSR(unsigned long val) {
-  unsigned long retval = 0L;
+  unsigned long r0;
 
+  r0 = jtagarm7tdmi_get_register(0);
+  jtagarm7tdmi_set_register(0, val);
   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - clean out the pipeline...
-  debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_MSR_cpsr_cxsf_R0, 0)); // push MSR cpsr_cxsf, R0 into pipeline
-  debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - fetched
+  debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_MSR_cpsr_cxsf_R0, 0)); // push MSR cpsr_cxsf, R0 into pipeline - fetch
   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - decoded
-  
-  retval = jtagarm7tdmi_instr_primitive(val, 0);// push 32-bit word on data bus
-  debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - executed 
-  debughex32(retval);
-  return(retval);
+  debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - execute
+  jtagarm7tdmi_set_register(0, r0);
+  return(val);
 }
 
 //! Write data to address - Assume TAP in run-test/idle state
 unsigned long jtagarm7tdmi_writemem(unsigned long adr, unsigned long data){
+  unsigned long retval = 0xffL;
   unsigned long r0=0L, r1=-1L;
 
   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
   r1 = jtagarm7tdmi_get_register(1);
   jtagarm7tdmi_set_register(0, adr);        // write address into R0
   jtagarm7tdmi_set_register(1, data);       // write data in R1
+  debughex32(jtagarm7tdmi_get_register(0));
+  debughex32(jtagarm7tdmi_get_register(1));
   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
-  jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline
+  jtagarm7tdmi_instr_primitive(ARM_INSTR_STR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline
   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
-  jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1 
-  jtagarm7tdmi_set_register(0, r0);
-  return(-1);
+  jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
+
+  // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
+  current_dbgstate = jtagarm7tdmi_get_dbgstate();
+  while ((!(current_dbgstate & 9L) == 9)  && retval > 0){
+    delay(1);
+    retval --;
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
+  }
+  if (retval == 0){
+    debugstr("FAILED TO WRITE MEMORY/RE-ENTER DEBUG MODE");
+    return (-1);
+  } else {
+    retval = jtagarm7tdmi_get_register(1);  // read memory value from R1 register
+    jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1 
+    jtagarm7tdmi_set_register(0, r0);
+  }
+  return retval;
 }
 
 
@@ -709,30 +711,34 @@ unsigned long jtagarm7tdmi_writemem(unsigned long adr, unsigned long data){
 
 //! Read data from address
 unsigned long jtagarm7tdmi_readmem(unsigned long adr){
-  unsigned long retval = 0L;
+  unsigned long retval = 0xffL;
   unsigned long r0=0L, r1=-1L;
-  int waitcount = 0xfffL;
 
   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
   r1 = jtagarm7tdmi_get_register(1);
   jtagarm7tdmi_set_register(0, adr);        // write address into R0
   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
-  jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, [R0], #4 into instruction pipeline
+  jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, [R0], #4 into instruction pipeline  (autoincrements for consecutive reads)
   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
   jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
 
   // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
-  while ((jtagarm7tdmi_get_dbgstate() & 9L) == 0  && waitcount > 0){
+  current_dbgstate = jtagarm7tdmi_get_dbgstate();
+  debughex(current_dbgstate);
+  while ((!(current_dbgstate & 9L) == 9)  && retval > 0){
     delay(1);
-    waitcount --;
+    retval --;
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
   }
-  if (waitcount == 0){
+  // FIXME: this may end up changing te current debug-state.  should we compare to current_dbgstate?
+  if (retval == 0){
+    debugstr("FAILED TO READ MEMORY/RE-ENTER DEBUG MODE");
     return (-1);
   } else {
     retval = jtagarm7tdmi_get_register(1);  // read memory value from R1 register
-    jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1 
-    jtagarm7tdmi_set_register(0, r0);
+    //jtagarm7tdmi_set_register(1, r1);       // restore R0 and R1 
+    //jtagarm7tdmi_set_register(0, r0);
   }
   return retval;
 }
@@ -740,10 +746,16 @@ unsigned long jtagarm7tdmi_readmem(unsigned long adr){
 
 //! Read Program Counter
 unsigned long jtagarm7tdmi_getpc(){
-  return jtagarm7tdmi_get_register(ARM_REG_PC);
+  unsigned long val;
+  val = jtagarm7tdmi_get_register(ARM_REG_PC);
+  if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
+    val -= (4*2);                           // thumb uses 2 bytes per instruction.
+  else
+    val -= (6*4);                           // assume 6 instructions at 4 bytes a piece.
+  return val;
 }
 
-//! Set Program Counter
+//! Set Program Counter - if setting it to non-word-aligned anything, crap may not like you.  you've been warned
 void jtagarm7tdmi_setpc(unsigned long adr){
   jtagarm7tdmi_set_register(ARM_REG_PC, adr);
 }
@@ -752,7 +764,8 @@ void jtagarm7tdmi_setpc(unsigned long adr){
 unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
   int waitcount = 0xfffL;
 
-/********  OLD WAY  ********/
+  // store the debug state
+  last_halt_debug_state = jtagarm7tdmi_get_dbgstate();
   // store watchpoint info?  - not right now
   eice_write(EICE_WP1ADDR, 0L);              // write 0 in watchpoint 1 address
   eice_write(EICE_WP1ADDRMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 address mask
@@ -760,36 +773,28 @@ unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
   eice_write(EICE_WP1DATAMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 data mask
   eice_write(EICE_WP1CTRL, 0x100L);          // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
   eice_write(EICE_WP1CTRLMASK, 0xfffffff7); // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
-/***************************/
-
-/********  NEW WAY  *********/
-//  eice_write(EICE_DBGCTRL, JTAG_ARM7TDMI_DBG_DBGRQ);  // r/o register?
-/****************************/
 
   // poll until debug status says the cpu is in debug mode
-  while (!(jtagarm7tdmi_get_dbgstate() & 0x1L)   && waitcount-- > 0){
+  while (!(current_dbgstate & 0x1L)   && waitcount-- > 0){
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
     delay(1);
   }
 
-/********  OLD WAY  ********/
   eice_write(EICE_WP1CTRL, 0x0L);            // write 0 in watchpoint 0 control value - disables watchpoint 0
-/***************************/
 
-/********  NEW WAY  ********/
-//  eice_write(EICE_DBGCTRL, 0);        // r/o register?
-/***************************/
-
-  // store the debug state
-  last_halt_debug_state = jtagarm7tdmi_get_dbgstate();
-  last_halt_pc = jtagarm7tdmi_getpc() - 4;  // assume -4 for entering debug mode via watchpoint.
-  count_dbgspd_instr_since_debug = 0L;
-  count_sysspd_instr_since_debug = 0L;
+  // store the debug state program counter.
+  last_halt_pc = jtagarm7tdmi_getpc();
+  count_dbgspd_instr_since_debug = 0L;          // should be able to clean this up and remove all this tracking nonsense.
+  count_sysspd_instr_since_debug = 0L;          // should be able to clean this up and remove all this tracking nonsense.
 
+  //FIXME: is this necessary?  for now, yes... but perhaps make the rest of the module arm/thumb impervious.
   // get into ARM mode if the T flag is set (Thumb mode)
-  while (jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT && waitcount-- > 0) {
-    jtagarm7tdmi_setMode_ARM();
+  while (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT && waitcount-- > 0) {
+    jtagarm7tdmi_setMode_ARM(0);
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
   }
   jtagarm7tdmi_resettap();
+  jtagarm7tdmi_set_register(ARM_REG_PC, last_halt_pc & 0xfffffffc);     // make sure PC is word-aligned.  otherwise all other register accesses get all wonky.
   return waitcount;
 }
 
@@ -800,6 +805,10 @@ unsigned long jtagarm7tdmi_releasecpu(){
   jtagarm7tdmi_nop(0);                          // NOP
   jtagarm7tdmi_nop(1);                          // NOP/BREAKPT
 
+
+  // four possible states.  arm mode needing arm mode, arm mode needing thumb mode, thumb mode needing arm mode, and thumb mode needing thumb mode
+  // FIXME:  BX is bs.  it requires the clobbering of at least one register.... this is not acceptable.  
+  // FIXME:  so we either switch modes, then correct the register before restarting with bx, or find the way to use SPSR
   if (last_halt_debug_state & JTAG_ARM7TDMI_DBG_TBIT){      // FIXME:  FORNICATED!  BX requires register, thus more instrs... could we get away with the same instruction but +1 to offset?
     instr = ARM_INSTR_B_PC + 0x1000001 - (count_dbgspd_instr_since_debug) - (count_sysspd_instr_since_debug*3);  //FIXME: make this right  - can't we just do an a7solute b/bx?
     jtagarm7tdmi_instr_primitive(instr,0);
@@ -808,13 +817,15 @@ unsigned long jtagarm7tdmi_releasecpu(){
     jtagarm7tdmi_instr_primitive(instr,0);
   }
 
-  SHIFT_IR;
-  jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); // VERB_RESTART
+  jtagarm7tdmi_restart();
+  //SHIFT_IR;
+  //jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); // VERB_RESTART
 
   // wait until restart-bit set in debug state register
-  while ((jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_DBGACK) && waitcount > 0){
+  while ((current_dbgstate & JTAG_ARM7TDMI_DBG_DBGACK) && waitcount > 0){
     msdelay(1);
     waitcount --;
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
   }
   last_halt_debug_state = -1;
   last_halt_pc = -1;
@@ -831,6 +842,7 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
   
   unsigned int i,val;
   unsigned long at;
+  current_dbgstate = jtagarm7tdmi_get_dbgstate();
   
   jtagarm7tdmi_resettap();
  
@@ -841,10 +853,9 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
     debughex32(jtagarm7tdmi_haltcpu());
     //jtagarm7tdmi_resettap();
     cmddatalong[0] = jtagarm7tdmi_get_dbgstate();
-    txdata(app,verb,0x4);
+    txdata(app,verb,0x36);
     break;
   case JTAGARM7TDMI_READMEM:
-  case PEEK:
     at     = cmddatalong[0];
     blocks = cmddatalong[1];
     
@@ -864,6 +875,12 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
       }
     
     
+    break;
+  case PEEK:
+       jtagarm7tdmi_resettap();
+       delay(1);
+       cmddatalong[0] = jtagarm7tdmi_readmem(cmddatalong[0]);
+    txdata(app,verb,4);
     break;
   case JTAGARM7TDMI_GET_CHIP_ID:
        jtagarm7tdmi_resettap();
@@ -878,7 +895,7 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
     jtagarm7tdmi_writemem(cmddatalong[0],
                       cmddataword[2]);
     cmddataword[0]=jtagarm7tdmi_readmem(cmddatalong[0]);
-    txdata(app,verb,2);
+    txdata(app,verb,4);
     break;
 
   case JTAGARM7TDMI_HALTCPU:  
@@ -912,7 +929,7 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
     break;
   case JTAGARM7TDMI_GET_DEBUG_STATE:
     //jtagarm7tdmi_resettap();            // Shouldn't need this, but currently do.  FIXME!
-    cmddatalong[0] = jtagarm7tdmi_get_dbgstate();
+    cmddatalong[0] = current_dbgstate;
     txdata(app,verb,4);
     break;
   //case JTAGARM7TDMI_GET_WATCHPOINT:
@@ -976,7 +993,7 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
   case JTAGARM7TDMI_SET_MODE_THUMB:
   case JTAGARM7TDMI_SET_MODE_ARM:
        jtagarm7tdmi_resettap();
-    cmddataword[0] = jtagarm7tdmi_setMode_ARM();
+    cmddataword[0] = jtagarm7tdmi_setMode_ARM(0);
     txdata(app,verb,4);
     break;
     
index 7eab988..43ea82c 100644 (file)
@@ -16,6 +16,7 @@
 
 
 unsigned char current_chain;
+unsigned char current_dbgstate = -1;
 unsigned char last_halt_debug_state = -1;
 unsigned long last_halt_pc = -1;
 unsigned long count_dbgspd_instr_since_debug = 0;
@@ -63,6 +64,10 @@ unsigned long jtagarm7tdmi_idcode();
 unsigned char jtagarm7tdmi_bypass();
 //!  Connect the appropriate scan chain to TDO/TDI
 unsigned long jtagarm7tdmi_scan_intest(int n);
+//!  Set a 32-bit ARM register
+void jtagarm7tdmi_set_register(unsigned long reg, unsigned long val);
+//!  Get a 32-bit ARM register
+unsigned long jtagarm7tdmi_get_register(unsigned long reg);
 
 // ARM7TDMI-specific pins
 // DBGRQ - GoodFET Pin 8
@@ -171,19 +176,21 @@ The least significant bit of the instruction register is scanned in and scanned
 //      http://www.atmel.com/dyn/resources/prod_documents/DDI0029G_7TDMI_R3_trm.pdf
 #define EXECNOPARM                  0xe1a00000L
 #define ARM_INSTR_NOP               0xe1a00000L
+#define ARM_INSTR_BX_R0             0xe12fff10L
 #define ARM_INSTR_STR_Rx_r14        0xe58f0000L // from atmel docs
 #define ARM_READ_REG                ARM_INSTR_STR_Rx_r14
 #define ARM_INSTR_LDR_Rx_r14        0xe59f0000L // from atmel docs
 #define ARM_WRITE_REG               ARM_INSTR_LDR_Rx_r14
 #define ARM_INSTR_LDR_R1_r0_4       0xe4901004L
 #define ARM_READ_MEM                ARM_INSTR_LDR_R1_r0_4
-#define ARM_INSTR_MRS_R0_CPSR       0xf10f0000L
+#define ARM_INSTR_STR_R1_r0_4       0xe4801004L
+#define ARM_WRITE_MEM               ARM_INSTR_STR_R1_r0_4
+#define ARM_INSTR_MRS_R0_CPSR       0xe10f0000L
 #define ARM_INSTR_MSR_cpsr_cxsf_R0  0xe12ff000L
 #define ARM_INSTR_STMIA_R14_r0_rx   0xE88E0000L      // add up to 65k to indicate which registers...
 #define ARM_STORE_MULTIPLE          ARM_INSTR_STMIA_R14_r0-rx
-#define ARM_INSTR_SKANKREGS1        0xE88E00ffL
-#define ARM_INSTR_SKANKREGS2        0xE88Eff00L
-#define ARM_INSTR_CLOBBEREGS        0xE89EffffL
+#define ARM_INSTR_SKANKREGS         0xE88F7fffL
+#define ARM_INSTR_CLOBBEREGS        0xE89F7fffL
 
 #define ARM_INSTR_B_PC              0xea000000L
 #define ARM_INSTR_BX_PC             0xe1200010L      // need to set r0 to the desired address