for (bit = bitcount; bit > 0; bit++)

[goodfet] / firmware / apps / jtag / jtagarm7tdmi.c

diff --git a/firmware/apps/jtag/jtagarm7tdmi.c b/firmware/apps/jtag/jtagarm7tdmi.c
index 056f42a..176ba0b 100644 (file)
--- a/firmware/apps/jtag/jtagarm7tdmi.c
+++ b/firmware/apps/jtag/jtagarm7tdmi.c
@@ -354,6 +354,7 @@ unsigned long jtagarm7tdmi_scan_intest(int chain) {               // PROVEN
 //! push an instruction into the pipeline  - Assumes scan-chain 1 is already INTEST
 unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){
   unsigned long retval;
+  //jtagarm7tdmi_resettap();                  // FIXME: DEBUG: seems necessary for some reason.  ugh.
   jtagarm7tdmi_scan_intest(1);
 
   SHIFT_DR;
@@ -563,7 +564,25 @@ unsigned long jtagarm7tdmi_get_register(unsigned char reg) {
   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 = jtagarmtransn(ARM_INSTR_NOP, 32, LSB, END, NORETIDLE); //DEBUGGING NOT FOR RESALE!
+  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_WRITE_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);
@@ -595,6 +614,29 @@ unsigned long jtagarm7tdmi_set_register(unsigned char reg, unsigned long val) {
   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_READ_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
@@ -691,12 +733,12 @@ unsigned long jtagarm7tdmi_readmem(unsigned long adr){
     delay(1);
     waitcount --;
   }
-  if (waitcount == 0xffff){
+  if (waitcount == 0){
     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;
 }
@@ -876,13 +918,15 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
   //case JTAGARM7TDMI_SET_WATCHPOINT:
   case JTAGARM7TDMI_GET_REGISTER:
        jtagarm7tdmi_resettap();
-    cmddatalong[0] = jtagarm7tdmi_get_register(cmddata[0]);
+    //cmddatalong[0] = jtagarm7tdmi_get_register(cmddata[0]);
+    cmddatalong[0] = test_get_register(cmddata[0]);
     txdata(app,verb,96);
     break;
   case JTAGARM7TDMI_SET_REGISTER:
        jtagarm7tdmi_resettap();
     cmddatalong[0] = cmddatalong[1];
-    jtagarm7tdmi_set_register(cmddata[0], cmddatalong[1]);
+    test_set_register(cmddata[0], cmddatalong[1]);
+    //jtagarm7tdmi_set_register(cmddata[0], cmddatalong[1]);
     txdata(app,verb,96);
     break;
   case JTAGARM7TDMI_GET_REGISTERS: