w00t! jtagarm7 committed and included in build.
[goodfet] / firmware / apps / jtag / jtagarm7tdmi.c
index a006ff8..3eb3311 100644 (file)
@@ -93,45 +93,6 @@ for this module, we keep tck high for all changes/sampling, and then bounce it.
 
 
 
-/************************** JTAGARM7TDMI Primitives ****************************/
-void jtag_goto_shift_ir() {
-  SETTMS;
-  jtag_arm_tcktock();
-  jtag_arm_tcktock();
-  CLRTMS;
-  jtag_arm_tcktock();
-  jtag_arm_tcktock();
-
-}
-
-void jtag_goto_shift_dr() {
-  SETTMS;
-  jtag_arm_tcktock();
-  CLRTMS;
-  jtag_arm_tcktock();
-  jtag_arm_tcktock();
-}
-
-void jtag_reset_to_runtest_idle() {
-  SETTMS;
-  jtag_arm_tcktock();
-  jtag_arm_tcktock();
-  jtag_arm_tcktock();
-  jtag_arm_tcktock();
-  jtag_arm_tcktock();  // now in Reset state
-  CLRTMS;
-  jtag_arm_tcktock();  // now in Run-Test/Idle state
-}
-
-void jtag_arm_tcktock() {
-  delay(100);  // FIXME: Should never wait this long...
-  CLRTCK; 
-  PLEDOUT^=PLEDPIN; 
-  delay(100);  // FIXME: Should never wait this long...
-  SETTCK; 
-  PLEDOUT^=PLEDPIN;
-}
-
 
 // ! Start JTAG, setup pins, reset TAP and return IDCODE
 unsigned long jtagarm7tdmi_start() {
@@ -148,76 +109,6 @@ void jtagarm7tdmi_resettap(){               // PROVEN
 }
 
 
-//  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
-unsigned long jtagarmtransn(unsigned long word, unsigned char bitcount, unsigned char lsb, unsigned char end, unsigned char retidle){               // PROVEN
-  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;
-
-  if (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==2 && end)  //FIXME: DID THIS BREAK SOMETHING?
-        SETTMS;//TMS high on last bit to exit.
-       
-      jtag_arm_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==2 && end)  //FIXME: DID THIS BREAK SOMETHING?
-        SETTMS;//TMS high on last bit to exit.
-
-      jtag_arm_tcktock();
-
-      /* read MISO on trailing edge */
-      word |= (READMISO);
-    }
-  }
-
-  SETMOSI;
-
-  if (end){
-    // exit state
-    jtag_arm_tcktock();
-    // update state
-    if (retidle){
-      CLRTMS;
-      jtag_arm_tcktock();
-    }
-  }
-  return word;
-}
-
-
 
 /************************************************************************
 * ARM7TDMI core has 6 primary registers to be connected between TDI/TDO
@@ -235,29 +126,29 @@ unsigned long jtagarmtransn(unsigned long word, unsigned char bitcount, unsigned
 //! Grab the core ID.
 unsigned long jtagarm7tdmi_idcode(){               // PROVEN
   jtagarm7tdmi_resettap();
-  SHIFT_IR;
-  jtagarmtransn(ARM7TDMI_IR_IDCODE, 4, LSB, END, RETIDLE);
-  SHIFT_DR;
-  return jtagarmtransn(0,32, LSB, END, RETIDLE);
+  jtag_goto_shift_ir();
+  jtagtransn(ARM7TDMI_IR_IDCODE, 4, LSB);
+  jtag_goto_shift_dr();
+  return jtagtransn(0,32, LSB);
 }
 
 //!  Connect Bypass Register to TDO/TDI
-unsigned char jtagarm7tdmi_bypass(){               // PROVEN
-  jtagarm7tdmi_resettap();
-  SHIFT_IR;
-  return jtagarmtransn(ARM7TDMI_IR_BYPASS, 4, LSB, END, NORETIDLE);
-}
+//unsigned char jtagarm7tdmi_bypass(){               // PROVEN
+//  jtagarm7tdmi_resettap();
+//  jtag_goto_shift_ir();
+//  return jtagarmtransn(ARM7TDMI_IR_BYPASS, 4, LSB, END, NORETIDLE);
+//}
 //!  INTEST verb - do internal test
-unsigned char jtagarm7tdmi_intest() { 
-  SHIFT_IR;
-  return jtagarmtransn(ARM7TDMI_IR_INTEST, 4, LSB, END, NORETIDLE); 
-}
+//unsigned char jtagarm7tdmi_intest() { 
+//  jtag_goto_shift_ir();
+//  return jtagarmtransn(ARM7TDMI_IR_INTEST, 4, LSB, END, NORETIDLE); 
+//}
 
 //!  EXTEST verb - act like the processor to external components
-unsigned char jtagarm7tdmi_extest() { 
-  SHIFT_IR;
-  return jtagarmtransn(ARM7TDMI_IR_EXTEST, 4, LSB, END, NORETIDLE);
-}
+//unsigned char jtagarm7tdmi_extest() { 
+//  jtag_goto_shift_ir();
+//  return jtagarmtransn(ARM7TDMI_IR_EXTEST, 4, LSB, END, NORETIDLE);
+//}
 
 //!  SAMPLE verb
 //unsigned long jtagarm7tdmi_sample() { 
@@ -266,32 +157,35 @@ unsigned char jtagarm7tdmi_extest() {
 //}
 
 //!  RESTART verb
-unsigned char jtagarm7tdmi_restart() { 
-  jtagarm7tdmi_resettap();
-  SHIFT_IR;
-  return jtagarmtransn(ARM7TDMI_IR_RESTART, 4, LSB, END, RETIDLE); 
+unsigned long jtagarm7tdmi_restart() { 
+  unsigned long retval;
+  jtag_goto_shift_ir();
+  retval = jtagtransn(ARM7TDMI_IR_RESTART, 4, LSB); 
+  current_chain = -1;
+  //jtagarm7tdmi_resettap();
+  return retval;
 }
 
 //!  ARM7TDMI_IR_CLAMP               0x5
 //unsigned long jtagarm7tdmi_clamp() { 
 //  jtagarm7tdmi_resettap();
-//  SHIFT_IR;
+//  jtag_goto_shift_ir();
 //  jtagarmtransn(ARM7TDMI_IR_CLAMP, 4, LSB, END, NORETIDLE);
-//  SHIFT_DR;
+//  jtag_goto_shift_dr();
 //  return jtagarmtransn(0, 32, LSB, END, RETIDLE);
 //}
 
 //!  ARM7TDMI_IR_HIGHZ               0x7
 //unsigned char jtagarm7tdmi_highz() { 
 //  jtagarm7tdmi_resettap();
-//  SHIFT_IR;
+//  jtag_goto_shift_ir();
 //  return jtagarmtransn(ARM7TDMI_IR_HIGHZ, 4, LSB, END, NORETIDLE);
 //}
 
 //! define ARM7TDMI_IR_CLAMPZ              0x9
 //unsigned char jtagarm7tdmi_clampz() { 
 //  jtagarm7tdmi_resettap();
-//  SHIFT_IR;
+//  jtag_goto_shift_ir();
 //  return jtagarmtransn(ARM7TDMI_IR_CLAMPZ, 4, LSB, END, NORETIDLE);
 //}
 
@@ -305,19 +199,23 @@ commands occur. Therefore, it is recommended to pass directly from the “Update
 state” to the “Select DR” state each time the “Update” state is reached.
 */
   unsigned long retval;
-  if (current_chain != chain) {
-    //debugstr("===change chains===");
-    SHIFT_IR;
-    jtagarmtransn(ARM7TDMI_IR_SCAN_N, 4, LSB, END, NORETIDLE);
-    SHIFT_DR;
-    retval = jtagarmtransn(chain, 4, LSB, END, NORETIDLE);
+  //if (current_chain != chain) {
+  //  //debugstr("===change chains===");
+    jtag_goto_shift_ir();
+    jtagtransn(ARM7TDMI_IR_SCAN_N, 4, LSB | NORETIDLE);
+    jtag_goto_shift_dr();
+    retval = jtagtransn(chain, 4, LSB | NORETIDLE);
+    // put in test mode...
+    //jtag_goto_shift_ir();
+    //jtagarmtransn(testmode, 4, LSB, END, RETIDLE); 
     current_chain = chain;
-  }    else
-    //debugstr("===NOT change chains===");
-    retval = current_chain;
+  //}    else  {
+  //  //debugstr("===NOT change chains===");
+  //  retval = current_chain;
+  //}
   // put in test mode...
-  SHIFT_IR;
-  jtagarmtransn(testmode, 4, LSB, END, RETIDLE); 
+  jtag_goto_shift_ir();
+  jtagtransn(testmode, 4, LSB); 
   return(retval);
 }
 
@@ -335,7 +233,7 @@ unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){
   unsigned long retval;
   jtagarm7tdmi_scan_intest(1);
 
-  SHIFT_DR;
+  jtag_goto_shift_dr();
   // if the next instruction is to run using MCLK (master clock), set TDI
   if (breakpt)
     {
@@ -347,16 +245,18 @@ unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){
     CLRMOSI; 
     count_dbgspd_instr_since_debug++;
     }
-  jtag_arm_tcktock();
+  jtag_tcktock();
   
   // Now shift in the 32 bits
-  retval = jtagarmtransn(instr, 32, MSB, END, RETIDLE);    // Must return to RUN-TEST/IDLE state for instruction to enter pipeline, and causes debug clock.
+  retval = jtagtransn(instr, 32, 0);    // Must return to RUN-TEST/IDLE state for instruction to enter pipeline, and causes debug clock.
   return(retval);
   
 }
 
 //! 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 +273,53 @@ 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
+  jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
   unsigned long retval = 0xffL;
-  while ((jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
-    cmddataword[6] = 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);
-    jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
+  if ((current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)){
+    debugstr("=== Switching to ARM mode ===");
+    cmddatalong[1] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
+    cmddatalong[2] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
+    cmddatalong[3] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_MOV_R0_PC,0);
+    cmddatalong[4] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,restart);
+    cmddatalong[5] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_BX_PC,0);
+  } else {
+    jtagarm7tdmi_set_register(15,(last_halt_pc|0xfffffffc)-24);
+    jtagarm7tdmi_nop( restart);
+    cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_B_IMM,0);
+  }
+  if (restart) {
+    jtagarm7tdmi_restart();
+  } else {
+    jtagarm7tdmi_nop(0);
+    jtagarm7tdmi_nop(0);
+    jtagarm7tdmi_nop(0);
+    jtagarm7tdmi_set_register(0,cmddataword[5]);
+  }
+  jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
+  current_dbgstate = jtagarm7tdmi_get_dbgstate();
+  return(retval);
+}
+
+
+//! set the current mode to ARM, returns PC (FIXME).  Should be used by releasecpu()
+unsigned long jtagarm7tdmi_setMode_THUMB(unsigned char restart){               // PROVEN
+  jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
+  debugstr("=== Switching to THUMB mode ===");
+  unsigned long retval = 0xffL;
+  while (!(current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
+    last_halt_pc |= 1;
+    jtagarm7tdmi_set_register(0, last_halt_pc);
+    jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,restart);
+    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,20 +331,12 @@ 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;
-  retval = jtagarmtransn(data, 32, LSB, NOEND, NORETIDLE);          // send in the data - 32-bits lsb
-  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();
+  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
   
   return(retval); 
 }
@@ -417,21 +344,21 @@ unsigned long eice_write(unsigned char reg, unsigned long data){
 //! shifter for reading from chain2 (EmbeddedICE).
 unsigned long eice_read(unsigned char reg){               // PROVEN
   unsigned long temp, retval;
-  debugstr("eice_read");
-  debughex(reg);
+  //debugstr("eice_read");
+  //debughex(reg);
   jtagarm7tdmi_scan_intest(2);
 
   // send in the register address - 5 bits LSB
-  SHIFT_DR;
-  temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);
+  jtag_goto_shift_dr();
+  temp = jtagtransn(reg, 5, LSB| NOEND| NORETIDLE);
   
   // clear TDI to select "read only"
-  jtagarmtransn(0L, 1, LSB, END, RETIDLE);
+  jtagtransn(0L, 1, LSB);
   
-  SHIFT_DR;
+  jtag_goto_shift_dr();
   // Now shift out the 32 bits
-  retval = jtagarmtransn(0L, 32, LSB, END, RETIDLE);   // atmel arm jtag docs pp.10-11: LSB first
-  debughex32(retval);
+  retval = jtagtransn(0L, 32, LSB);   // atmel arm jtag docs pp.10-11: LSB first
+  //debughex32(retval);
   return(retval);   // atmel arm jtag docs pp.10-11: LSB first
   
 }
@@ -484,105 +411,58 @@ void jtagarm7tdmi_set_watchpoint1(unsigned long addr, unsigned long addrmask, un
   eice_write(EICE_WP1CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
 }
 
-//!  Disable Watchpoint 0
-void jtagarm7tdmi_disable_watchpoint0(){
-  eice_write(EICE_WP0CTRL, 0x0L); // write 0 in watchpoint 0 control value - disables watchpoint 0
-}
-  
-//!  Disable Watchpoint 1
-void jtagarm7tdmi_disable_watchpoint1(){
-  eice_write(EICE_WP1CTRL, 0x0L);            // write 0 in watchpoint 0 control value - disables watchpoint 0
-}
-
-
-
 /******************** Complex Commands **************************/
 
-//! 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;
-
-  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
-  retval = jtagarm7tdmi_nop( 0);
-  debughex32(retval);
-  debughex32(jtagarm7tdmi_nop( 0));
-  debughex32(jtagarm7tdmi_nop( 0));
-
-  return(retval);
-}
-
 //! Retrieve a 32-bit Register value
-unsigned long jtagarm7tdmi_get_register(unsigned long reg) {
-  unsigned long retval = 0L, instr, reg2;
-  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 jtagarm7tdmi_get_register(unsigned long reg) {                    //PROVEN
+  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);
-  jtagarm7tdmi_nop( 0);                // push nop into pipeline - fetched
-  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);
   jtagarm7tdmi_nop( 0);
   jtagarm7tdmi_nop( 0);
   jtagarm7tdmi_nop( 0);
+  retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
   return retval;
 }
 
 //! Set a 32-bit Register value
-void jtagarm7tdmi_set_register(unsigned long reg, unsigned long val) {
-  unsigned long instr, reg2;
-  reg2 = (reg&0xfL);
-  instr = (unsigned long)(((unsigned long)reg<<12L) | ARM_WRITE_REG); //  LDR Rx, [R14]
-  instr |= (unsigned long)((unsigned long)reg2<<8L)<<8L;
-  //instr |= (unsigned long)((((unsigned long)reg)&0x7)<<8)<<8;
-  //debugstr("Writing:");
-  debughex32(instr);
-  //debughex32(val);
+void jtagarm7tdmi_set_register(unsigned long reg, unsigned long val) {          // PROVEN (assuming target reg is word aligned)
+  unsigned long instr;
+  //if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
+    //instr = THUMB_WRITE_REG
+    instr = (unsigned long)(((unsigned long)reg<<12L) | ARM_WRITE_REG); //  LDR Rx, [R14]
+  
   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
-  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_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 
-
   if (reg == ARM_REG_PC){
-    jtagarm7tdmi_nop( 0);
-    jtagarm7tdmi_nop( 0);
+    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 
+  } 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 - executed 
+  jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
   jtagarm7tdmi_nop( 0);
 }
 
 
-
-//! Get all registers, placing them into cmddatalong[0-15]
-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));
+/*
+//! Get all registers, placing them into cmddatalong[0-14]
+void jtagarm7tdmi_get_registers() {         // BORKEN.  FIXME
+  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);
@@ -591,17 +471,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);
@@ -613,192 +482,230 @@ 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);
+}
+
+unsigned long wait_debug(unsigned long retval){
+  // 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();
+  }
+  return retval;
 }
 
+/****
 //! 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
 
+  if (wait_debug(0xffL) == 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;
+}
 
 
 
 //! 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;
 }
 
+*/
 
-//! Read Program Counter
-unsigned long jtagarm7tdmi_getpc(){
-  return jtagarm7tdmi_get_register(ARM_REG_PC);
-}
 
-//! Set Program Counter
-void jtagarm7tdmi_setpc(unsigned long adr){
-  jtagarm7tdmi_set_register(ARM_REG_PC, adr);
+//! Read Program Counter
+unsigned long jtagarm7tdmi_get_real_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;
 }
 
 //! Halt CPU - returns 0xffff if the operation fails to complete within 
 unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
-  int waitcount = 0xfffL;
+  int waitcount = 0xffL;
 
-/********  OLD WAY  ********/
+  // store the debug state
+  last_halt_debug_state = jtagarm7tdmi_get_dbgstate();
+
+  //jtagarm7tdmi_set_dbgctrl(7);
   // store watchpoint info?  - not right now
+  jtagarm7tdmi_set_watchpoint1(0, 0xffffffff, 0, 0xffffffff, 0x100L, 0xfffffff7);
+
+
+  /*  // old method
   eice_write(EICE_WP1ADDR, 0L);              // write 0 in watchpoint 1 address
   eice_write(EICE_WP1ADDRMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 address mask
   eice_write(EICE_WP1DATA, 0L);              // write 0 in watchpoint 1 data
   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
-/***************************/
+  //jtagarm7tdmi_set_dbgctrl(0);
+  jtagarm7tdmi_set_watchpoint1(0, 0x0, 0, 0x0, 0x0L, 0xfffffff7);
+  //jtagarm7tdmi_disable_watchpoint1();
 
-/********  NEW WAY  ********/
-//  eice_write(EICE_DBGCTRL, 0);        // r/o register?
-/***************************/
+  //eice_write(EICE_WP1CTRL, 0x0L);            // write 0 in watchpoint 0 control value - disables watchpoint 0
 
-  // 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_get_real_pc();    // FIXME: grag chain0 to get all state and PC
+  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;
 }
 
 unsigned long jtagarm7tdmi_releasecpu(){
-  int waitcount = 0xfff;
-  unsigned long instr;
-  // somehow determine what PC should be (a couple ways possible, calculations required)
+  int waitcount = 0xff;
   jtagarm7tdmi_nop(0);                          // NOP
   jtagarm7tdmi_nop(1);                          // NOP/BREAKPT
 
-  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);
+
+  // 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){
+    // need to get to thumb mode
+    jtagarm7tdmi_set_register(15,last_halt_pc-20);        // 20 bytes will be added to pc before the end of the write.  incorrect and must fix
+    jtagarm7tdmi_setMode_THUMB(1);
   } else {
-    instr = ARM_INSTR_B_PC + 0x1000000 - (count_dbgspd_instr_since_debug*4) - (count_sysspd_instr_since_debug*12);
-    jtagarm7tdmi_instr_primitive(instr,0);
+    jtagarm7tdmi_setMode_ARM(1);
+    //jtagarm7tdmi_set_register(15,last_halt_pc-20);        // 20 bytes will be added to pc before the end of the write.  incorrect and must fix
   }
 
-  SHIFT_IR;
-  jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); // VERB_RESTART
+
+  jtagarm7tdmi_restart();
+  jtagarm7tdmi_resettap();
+  //jtag_goto_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 > -1){
     msdelay(1);
     waitcount --;
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
   }
   last_halt_debug_state = -1;
   last_halt_pc = -1;
-  return 0;
+  return waitcount;
 }
  
 
@@ -807,72 +714,73 @@ unsigned long jtagarm7tdmi_releasecpu(){
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 //! Handles ARM7TDMI JTAG commands.  Forwards others to JTAG.
 void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len){
-  register char blocks;
+  //register char blocks;
   
-  unsigned int i,val;
-  unsigned long at;
+  unsigned int val; //, i;
+  //unsigned long at;
   
-  jtagarm7tdmi_resettap();
+  //jtagarm7tdmi_resettap();
+  //current_dbgstate = jtagarm7tdmi_get_dbgstate();
  
   switch(verb){
   case START:
     //Enter JTAG mode.
     debughex32(jtagarm7tdmi_start());
-    debughex32(jtagarm7tdmi_haltcpu());
-    //jtagarm7tdmi_resettap();
-    debughex32(jtagarm7tdmi_get_dbgstate());
-    
-    // DEBUG: FIXME: NOT PART OF OPERATIONAL CODE
-    //for (mlop=2;mlop<4;mlop++){
-    //  jtagarm7tdmi_set_register(mlop, 0x43424140);
-    //} 
-    /////////////////////////////////////////////
+    cmddatalong[0] = jtagarm7tdmi_get_dbgstate();
     txdata(app,verb,0x4);
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
     break;
+    /*
   case JTAGARM7TDMI_READMEM:
-  case PEEK:
-    blocks=(len>4?cmddata[4]:1);
-    at=cmddatalong[0];
+    at     = cmddatalong[0];
+    blocks = cmddatalong[1];
     
-    len=0x80;
     txhead(app,verb,len);
     
-    while(blocks--){
-      for(i=0;i<len;i+=2){
        jtagarm7tdmi_resettap();
-       delay(10);
+       delay(1);
        
-       val=jtagarm7tdmi_readmem(at);
+    for(i=0;i<blocks;i++){
+         val=jtagarm7tdmi_readmem(at);
                
-       at+=2;
-       serial_tx(val&0xFFL);
-       serial_tx((val&0xFF00L)>>8);
+         serial_tx(val&0xFFL);
+         serial_tx((val&0xFF00L)>>8);
+         serial_tx((val&0xFF0000L)>>8);
+         serial_tx((val&0xFF000000L)>>8);
+         at+=4;
       }
-    }
     
+    
+    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();
     cmddatalong[0] = jtagarm7tdmi_idcode();
     txdata(app,verb,4);
     break;
 
-
+/*
   case JTAGARM7TDMI_WRITEMEM:
   case POKE:
        jtagarm7tdmi_resettap();
     jtagarm7tdmi_writemem(cmddatalong[0],
                       cmddataword[2]);
     cmddataword[0]=jtagarm7tdmi_readmem(cmddatalong[0]);
-    txdata(app,verb,2);
+    txdata(app,verb,4);
     break;
-
+*/
   case JTAGARM7TDMI_HALTCPU:  
     cmddatalong[0] = jtagarm7tdmi_haltcpu();
     txdata(app,verb,4);
     break;
   case JTAGARM7TDMI_RELEASECPU:
-       jtagarm7tdmi_resettap();
+       //jtagarm7tdmi_resettap();
     cmddatalong[0] = jtagarm7tdmi_releasecpu();
     txdata(app,verb,4);
     break;
@@ -881,7 +789,8 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
   //case JTAGARM7TDMI_WRITEFLASH:
   //case JTAGARM7TDMI_ERASEFLASH:
   case JTAGARM7TDMI_SET_PC:
-    jtagarm7tdmi_setpc(cmddatalong[0]);
+    //jtagarm7tdmi_setpc(cmddatalong[0]);
+    last_halt_pc = cmddatalong[0];
     txdata(app,verb,0);
     break;
   case JTAGARM7TDMI_GET_DEBUG_CTRL:
@@ -893,44 +802,33 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
     txdata(app,verb,4);
     break;
   case JTAGARM7TDMI_GET_PC:
-    cmddatalong[0] = jtagarm7tdmi_getpc();
+    cmddatalong[0] = last_halt_pc;
     txdata(app,verb,4);
     break;
   case JTAGARM7TDMI_GET_DEBUG_STATE:
     //jtagarm7tdmi_resettap();            // Shouldn't need this, but currently do.  FIXME!
-    cmddatalong[0] = jtagarm7tdmi_get_dbgstate();
+    current_dbgstate = jtagarm7tdmi_get_dbgstate();
+    cmddatalong[0] = current_dbgstate;
     txdata(app,verb,4);
     break;
   //case JTAGARM7TDMI_GET_WATCHPOINT:
   //case JTAGARM7TDMI_SET_WATCHPOINT:
   case JTAGARM7TDMI_GET_REGISTER:
-       jtagarm7tdmi_resettap();
+       //jtagarm7tdmi_resettap();
     val = cmddata[0];
     cmddatalong[0] = jtagarm7tdmi_get_register(val);
-    //debughex32(cmddatalong[0]);
     txdata(app,verb,4);
     break;
-  case JTAGARM7TDMI_SET_REGISTER:           // FIXME: NOT AT ALL CORRECT, THIS IS TESTING CODE ONLY
-       jtagarm7tdmi_resettap();
-    debughex32(cmddatalong[1]);
-    jtagarm7tdmi_set_register(cmddata[0], cmddatalong[1]);
-    cmddatalong[0] = cmddatalong[1];
+  case JTAGARM7TDMI_SET_REGISTER:
+       //jtagarm7tdmi_resettap();
+    jtagarm7tdmi_set_register(cmddatalong[1], cmddatalong[0]);
     txdata(app,verb,4);
     break;
-  case JTAGARM7TDMI_GET_REGISTERS:
-       jtagarm7tdmi_resettap();
-    jtagarm7tdmi_get_registers();
-    txdata(app,verb,64);
-    break;
-  case JTAGARM7TDMI_SET_REGISTERS:
-       jtagarm7tdmi_resettap();
-    jtagarm7tdmi_set_registers();
-    txdata(app,verb,64);
-    break;
   case JTAGARM7TDMI_DEBUG_INSTR:
-       jtagarm7tdmi_resettap();
-    cmddataword[0] = jtagarm7tdmi_exec(cmddataword[0], cmddataword[1], cmddata[9]);
-    txdata(app,verb,80);
+       //jtagarm7tdmi_resettap();
+    //cmddataword[0] = jtagarm7tdmi_exec(cmddataword[0], cmddata[4]);
+    cmddatalong[0] = jtagarm7tdmi_instr_primitive(cmddatalong[0],cmddata[4]);
+    txdata(app,verb,8);
     break;
   //case JTAGARM7TDMI_STEP_INSTR:
 /*  case JTAGARM7TDMI_READ_CODE_MEMORY:
@@ -952,59 +850,64 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
     txdata(app,verb,4);
     break;
-  case JTAGARM7TDMI_GET_SPSR:           // FIXME: NOT CORRECT
+  case JTAGARM7TDMI_GET_SPSR:           // FIXME: NOT EVEN CLOSE TO CORRECT
        jtagarm7tdmi_resettap();
     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
     txdata(app,verb,4);
     break;
-  case JTAGARM7TDMI_SET_SPSR:           // FIXME: NOT CORRECT
+  case JTAGARM7TDMI_SET_SPSR:           // FIXME: NOT EVEN CLOSE TO CORRECT
        jtagarm7tdmi_resettap();
     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
     txdata(app,verb,4);
     break;
   case JTAGARM7TDMI_SET_MODE_THUMB:
-  case JTAGARM7TDMI_SET_MODE_ARM:
        jtagarm7tdmi_resettap();
-    cmddataword[0] = jtagarm7tdmi_setMode_ARM();
+    cmddatalong[0] = jtagarm7tdmi_setMode_THUMB(cmddata[0]);
     txdata(app,verb,4);
     break;
-    
-  case 0xD0:          // loopback test
-    jtagarm7tdmi_resettap();
-    cmddatalong[0] = jtagarm7tdmi_bypass(cmddatalong[0]);
+  case JTAGARM7TDMI_SET_MODE_ARM:
+       jtagarm7tdmi_resettap();
+    cmddatalong[0] = jtagarm7tdmi_setMode_ARM(cmddata[0]);
     txdata(app,verb,4);
     break;
-  case 0xD8:          // EICE_READ
-    jtagarm7tdmi_resettap();
-    cmddatalong[0] = eice_read(cmddatalong[0]);
+  case JTAGARM7TDMI_SET_IR:
+       //jtagarm7tdmi_resettap();
+    jtag_goto_shift_ir();
+    cmddataword[0] = jtagtransn(cmddata[0], 4, cmddata[1]);
+    current_chain = -1;
+    txdata(app,verb,2);
+    break;
+  case JTAGARM7TDMI_WAIT_DBG:
+    cmddatalong[0] = wait_debug(cmddatalong[0]);
     txdata(app,verb,4);
     break;
-  case 0xD9:          // EICE_WRITE
-    jtagarm7tdmi_resettap();
-    cmddatalong[0] = eice_write(cmddatalong[0], cmddatalong[1]);
+  case JTAGARM7TDMI_SHIFT_DR:
+       jtagarm7tdmi_resettap();
+    jtag_goto_shift_dr();
+    cmddatalong[0] = jtagtransn(cmddatalong[1],cmddata[0],cmddata[1]);
     txdata(app,verb,4);
     break;
-  case 0xDA:          // TEST MSB THROUGH CHAIN0 and CHAIN1
-    jtagarm7tdmi_resettap();
+  case JTAGARM7TDMI_CHAIN0:
     jtagarm7tdmi_scan_intest(0);
-    cmddatalong[0] = jtagarmtransn(0x41414141, 32, LSB, NOEND, NORETIDLE);
-    cmddatalong[1] = jtagarmtransn(0x42424242, 32, MSB, NOEND, NORETIDLE);
-    cmddatalong[2] = jtagarmtransn(0x43434343,  9, MSB, NOEND, NORETIDLE);
-    cmddatalong[3] = jtagarmtransn(0x44444444, 32, MSB, NOEND, NORETIDLE);
-    cmddatalong[4] = jtagarmtransn(cmddatalong[0], 32, LSB, NOEND, NORETIDLE);
-    cmddatalong[5] = jtagarmtransn(cmddatalong[1], 32, MSB, NOEND, NORETIDLE);
-    cmddatalong[6] = jtagarmtransn(cmddatalong[2],  9, MSB, NOEND, NORETIDLE);
-    cmddatalong[7] = jtagarmtransn(cmddatalong[3], 32, MSB, END, RETIDLE);
-    jtagarm7tdmi_resettap();
-    jtagarm7tdmi_scan_intest(1);
-    cmddatalong[8] = jtagarmtransn(0x41414141, 32, MSB, NOEND, NORETIDLE);
-    cmddatalong[9] = jtagarmtransn(0x44444444,  1, MSB, NOEND, NORETIDLE);
-    cmddatalong[10] = jtagarmtransn(cmddatalong[8], 32, MSB, NOEND, NORETIDLE);
-    cmddatalong[11] = jtagarmtransn(cmddatalong[9],  1, MSB, END, RETIDLE);
-    jtagarm7tdmi_resettap();
-    txdata(app,verb,48);
+    jtag_goto_shift_dr();
+    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);
+    txdata(app,verb,16);
+    break;
+  case JTAGARM7TDMI_SETWATCH0:
+    jtagarm7tdmi_set_watchpoint0(cmddatalong[0], cmddatalong[1], cmddatalong[2], cmddatalong[3], cmddatalong[4], cmddatalong[5]);
+    txdata(app,verb,4);
+    break;
+  case JTAGARM7TDMI_SETWATCH1:
+    jtagarm7tdmi_set_watchpoint0(cmddatalong[0], cmddatalong[1], cmddatalong[2], cmddatalong[3], cmddatalong[4], cmddatalong[5]);
+    txdata(app,verb,4);
     break;
-    
   default:
     jtaghandle(app,verb,len);
   }