git.rot13.org Git - goodfet/blob - firmware/apps/jtag/jtagarm7tdmi.c

   1 /*! \file jtagarm7tdmi.c
   2   \brief ARM7TDMI JTAG (AT91R40008, AT91SAM7xxx)
   3 */
   4
   5 #include "platform.h"
   6 #include "command.h"
   7 #include "jtag.h"
   8 #include "jtagarm7tdmi.h"
   9
  10
  11 /**** 20-pin Connection Information (pin1 is on top-right for both connectors)****
  12 GoodFET  ->  7TDMI 20-pin connector (HE-10 connector)
  13   1               13 (TDO)
  14   2               1  (Vdd)
  15   3               5  (TDI)
  16   5               7  (TMS)
  17   7               9  (TCK)
  18   8               15 (nRST)
  19   9               4,6,8,10,12,14,16,18,20 (GND)
  20   11              17/3 (nTRST)  (different sources suggest 17 or 3 alternately)
  21 ********************************/
  22
  23 /**** 14-pin Connection Information (pin1 is on top-right for both connectors)****
  24 GoodFET  ->  7TDMI 14-pin connector
  25   1               11 (TDO)
  26   2               1  (Vdd)
  27   3               5  (TDI)
  28   5               7  (TMS)
  29   7               9  (TCK)
  30   8               12 (nRST)
  31   9               2,4,6,8,10,14 (GND)
  32   11              3 (nTRST)
  33
  34 http://hri.sourceforge.net/tools/jtag_faq_org.html
  35 ********************************/
  36
  37
  38
  39
  40
  41
  42 /****************************************************************
  43 Enabling jtag likely differs with most platforms.  We will attempt to enable most from here.  Override jtagarm7tdmi_start() to extend for other implementations
  44 ARM7TDMI enables three different scan chains:
  45     * Chain0 - "entire periphery" including data bus
  46     * Chain1 - core data bus (subset of Chain0)  - Instruction Pipeline
  47     * Chain2 - EmbeddedICE Logic Registers - This is our way into the fun stuff.
  48
  49
  50 ---
  51 You can disable EmbeddedICE-RT by setting the DBGEN input LOW.
  52 Caution
  53 Hard wiring the DBGEN input LOW permanently disables all debug functionality.
  54 When DBGEN is LOW, it inhibits DBGDEWPT, DBGIEBKPT, and EDBGRQ to
  55 the core, and DBGACK from the ARM9E-S core is always LOW.
  56 ---
  57
  58
  59 ---
  60 When the ARM9E-S core is in debug state, you can examine the core and system state
  61 by forcing the load and store multiples into the instruction pipeline.
  62 Before you can examine the core and system state, the debugger must determine
  63 whether the processor entered debug from Thumb state or ARM state, by examining
  64 bit 4 of the EmbeddedICE-RT debug status register. If bit 4 is HIGH, the core has
  65 entered debug from Thumb state.
  66 For more details about determining the core state, see Determining the core and system
  67 state on page B-18.
  68 ---
  69
  70
  71 --- olimex - http://www.olimex.com/dev/pdf/arm-jtag.pdf
  72 JTAG signals description:
  73 PIN.1 (VTREF) Target voltage sense. Used to indicate the target’s operating voltage to thedebug tool.
  74 PIN.2 (VTARGET) Target voltage. May be used to supply power to the debug tool.
  75 PIN.3 (nTRST) JTAG TAP reset, this signal should be pulled up to Vcc in target board.
  76 PIN4,6, 8, 10,12,14,16,18,20 Ground. The Gnd-Signal-Gnd-Signal strategy implemented on the 20-way connection scheme improves noiseimmunity on the target connect cable.
  77 *PIN.5 (TDI) JTAG serial data in, should be pulled up to Vcc on target board.
  78 *PIN.7 (TMS) JTAG TAP Mode Select, should be pulled up to Vcc on target board.
  79 *PIN.9 (TCK) JTAG clock.
  80 PIN.11 (RTCK) JTAG retimed clock.Implemented on certain ASIC ARM implementations the host ASIC may need to synchronize external inputs (such as JTAG inputs) with its own internal clock.
  81 *PIN.13 (TDO) JTAG serial data out.
  82 *PIN.15 (nSRST) Target system reset.
  83 *PIN.17 (DBGRQ) Asynchronous debug request.  DBGRQ allows an external signal to force the ARM core into debug mode, should be pull down to GND.
  84 PIN.19 (DBGACK) Debug acknowledge. The ARM core acknowledges debug-mode inresponse to a DBGRQ input.
  85
  86
  87 -----------  SAMPLE TIMES  -----------
  88
  89 TDI and TMS are sampled on the rising edge of TCK and TDO transitions appear on the falling edge of TCK. Therefore, TDI and TMS must be written after the falling edge of TCK and TDO must be read after the rising edge of TCK.
  90
  91 for this module, we keep tck high for all changes/sampling, and then bounce it.
  92 ****************************************************************/
  93
  94
  95
  96 /************************** JTAGARM7TDMI Primitives ****************************/
  97 void jtag_goto_shift_ir() {
  98   SETTMS;
  99   jtag_arm_tcktock();
 100   jtag_arm_tcktock();
 101   CLRTMS;
 102   jtag_arm_tcktock();
 103   jtag_arm_tcktock();
 104
 105 }
 106 void jtag_goto_shift_dr() {
 107   SETTMS;
 108   jtag_arm_tcktock();
 109   CLRTMS;
 110   jtag_arm_tcktock();
 111   jtag_arm_tcktock();
 112 }
 113
 114 void jtag_reset_to_runtest_idle() {
 115   SETTMS;
 116   jtag_arm_tcktock();
 117   jtag_arm_tcktock();
 118   jtag_arm_tcktock();
 119   jtag_arm_tcktock();
 120   jtag_arm_tcktock();  // now in Reset state
 121   CLRTMS;
 122   jtag_arm_tcktock();  // now in Run-Test/Idle state
 123 }
 124
 125 void jtag_arm_tcktock() {
 126   delay(1);  // FIXME: Should never wait this long...
 127   CLRTCK;
 128   PLEDOUT^=PLEDPIN;
 129   delay(1);  // FIXME: Should never wait this long...
 130   SETTCK;
 131   PLEDOUT^=PLEDPIN;
 132 }
 133
 134
 135 // ! Start JTAG, setup pins, reset TAP and return IDCODE
 136 unsigned long jtagarm7tdmi_start() {
 137   jtagsetup();
 138   jtagarm7tdmi_resettap();
 139   return jtagarm7tdmi_idcode();
 140 }
 141
 142
 143 //! Reset TAP State Machine
 144 void jtagarm7tdmi_resettap(){               // PROVEN
 145   current_chain = -1;
 146   jtag_reset_to_runtest_idle();
 147 }
 148
 149
 150 //  NOTE: important: THIS MODULE REVOLVES AROUND RETURNING TO RUNTEST/IDLE, OR THE FUNCTIONAL EQUIVALENT
 151
 152
 153 //! 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
 154 unsigned long jtagarmtransn(unsigned long word, unsigned char bitcount, unsigned char lsb, unsigned char end, unsigned char retidle){               // PROVEN
 155   unsigned char bit;
 156   unsigned long high = 1L;
 157   unsigned long mask;
 158
 159   //for (bit=(bitcount-1)/8; bit>0; bit--)
 160   //  high <<= 8;
 161   //high <<= ((bitcount-1)%8);
 162   high <<= (bitcount-1);
 163
 164   mask = high-1;
 165
 166   if (lsb) {
 167     for (bit = bitcount; bit > 0; bit--) {
 168       /* write MOSI on trailing edge of previous clock */
 169       if (word & 1)
 170         {SETMOSI;}
 171       else
 172         {CLRMOSI;}
 173       word >>= 1;
 174
 175       if (bit==1 && end)
 176         SETTMS;//TMS high on last bit to exit.
 177
 178       jtag_arm_tcktock();
 179
 180       /* read MISO on trailing edge */
 181       if (READMISO){
 182         word += (high);
 183       }
 184     }
 185   } else {
 186     for (bit = bitcount; bit > 0; bit--) {
 187       /* write MOSI on trailing edge of previous clock */
 188       if (word & high)
 189         {SETMOSI;}
 190       else
 191         {CLRMOSI;}
 192       word = (word & mask) << 1;
 193
 194       if (bit==1 && end)
 195         SETTMS;//TMS high on last bit to exit.
 196
 197       jtag_arm_tcktock();
 198
 199       /* read MISO on trailing edge */
 200       word |= (READMISO);
 201     }
 202   }
 203
 204
 205   SETMOSI;
 206
 207   if (end){
 208     // exit state
 209     jtag_arm_tcktock();
 210     // update state
 211     if (retidle){
 212       CLRTMS;
 213       jtag_arm_tcktock();
 214     }
 215   }
 216   return word;
 217 }
 218
 219
 220
 221 /************************************************************************
 222 * ARM7TDMI core has 6 primary registers to be connected between TDI/TDO
 223 *   * Bypass Register
 224 *   * ID Code Register
 225 *   * Scan Chain Select Register    (4 bits_lsb)
 226 *   * Scan Chain 0                  (105 bits: 32_databits_lsb + ctrlbits + 32_addrbits_msb)
 227 *   * Scan Chain 1                  (33 bits: 32_bits + BREAKPT)
 228 *   * Scan Chain 2                  (38 bits: rw + 5_regbits_msb + 32_databits_msb)
 229 ************************************************************************/
 230
 231
 232
 233 /************************** Basic JTAG Verb Commands *******************************/
 234 //! Grab the core ID.
 235 unsigned long jtagarm7tdmi_idcode(){               // PROVEN
 236   jtagarm7tdmi_resettap();
 237   jtag_goto_shift_ir();
 238   jtagarmtransn(ARM7TDMI_IR_IDCODE, 4, LSB, END, RETIDLE);
 239   jtag_goto_shift_dr();
 240   return jtagarmtransn(0,32, LSB, END, RETIDLE);
 241 }
 242
 243 //!  Connect Bypass Register to TDO/TDI
 244 //unsigned char jtagarm7tdmi_bypass(){               // PROVEN
 245 //  jtagarm7tdmi_resettap();
 246 //  jtag_goto_shift_ir();
 247 //  return jtagarmtransn(ARM7TDMI_IR_BYPASS, 4, LSB, END, NORETIDLE);
 248 //}
 249 //!  INTEST verb - do internal test
 250 //unsigned char jtagarm7tdmi_intest() {
 251 //  jtag_goto_shift_ir();
 252 //  return jtagarmtransn(ARM7TDMI_IR_INTEST, 4, LSB, END, NORETIDLE);
 253 //}
 254
 255 //!  EXTEST verb - act like the processor to external components
 256 //unsigned char jtagarm7tdmi_extest() {
 257 //  jtag_goto_shift_ir();
 258 //  return jtagarmtransn(ARM7TDMI_IR_EXTEST, 4, LSB, END, NORETIDLE);
 259 //}
 260
 261 //!  SAMPLE verb
 262 //unsigned long jtagarm7tdmi_sample() {
 263 //  jtagarm7tdmi_ir_shift4(ARM7TDMI_IR_SAMPLE);        // ???? same here.
 264 //  return jtagtransn(0,32);
 265 //}
 266
 267 //!  RESTART verb
 268 unsigned long jtagarm7tdmi_restart() {
 269   unsigned long retval;
 270   jtag_goto_shift_ir();
 271   retval = jtagarmtransn(ARM7TDMI_IR_RESTART, 4, LSB, END, RETIDLE);
 272   current_chain = -1;
 273   //jtagarm7tdmi_resettap();
 274   return retval;
 275 }
 276
 277 //!  ARM7TDMI_IR_CLAMP               0x5
 278 //unsigned long jtagarm7tdmi_clamp() {
 279 //  jtagarm7tdmi_resettap();
 280 //  jtag_goto_shift_ir();
 281 //  jtagarmtransn(ARM7TDMI_IR_CLAMP, 4, LSB, END, NORETIDLE);
 282 //  jtag_goto_shift_dr();
 283 //  return jtagarmtransn(0, 32, LSB, END, RETIDLE);
 284 //}
 285
 286 //!  ARM7TDMI_IR_HIGHZ               0x7
 287 //unsigned char jtagarm7tdmi_highz() {
 288 //  jtagarm7tdmi_resettap();
 289 //  jtag_goto_shift_ir();
 290 //  return jtagarmtransn(ARM7TDMI_IR_HIGHZ, 4, LSB, END, NORETIDLE);
 291 //}
 292
 293 //! define ARM7TDMI_IR_CLAMPZ              0x9
 294 //unsigned char jtagarm7tdmi_clampz() {
 295 //  jtagarm7tdmi_resettap();
 296 //  jtag_goto_shift_ir();
 297 //  return jtagarmtransn(ARM7TDMI_IR_CLAMPZ, 4, LSB, END, NORETIDLE);
 298 //}
 299
 300
 301 //!  Connect the appropriate scan chain to TDO/TDI.  SCAN_N, INTEST, ENDS IN SHIFT_DR!!!!!
 302 unsigned long jtagarm7tdmi_scan(int chain, int testmode) {               // PROVEN
 303 /*
 304 When selecting a scan chain the “Run Test/Idle” state should never be reached, other-
 305 wise, when in debug state, the core will not be correctly isolated and intrusive
 306 commands occur. Therefore, it is recommended to pass directly from the “Update”
 307 state” to the “Select DR” state each time the “Update” state is reached.
 308 */
 309   unsigned long retval;
 310   //if (current_chain != chain) {
 311   //  //debugstr("===change chains===");
 312     jtag_goto_shift_ir();
 313     jtagarmtransn(ARM7TDMI_IR_SCAN_N, 4, LSB, END, NORETIDLE);
 314     jtag_goto_shift_dr();
 315     retval = jtagarmtransn(chain, 4, LSB, END, NORETIDLE);
 316     // put in test mode...
 317     //jtag_goto_shift_ir();
 318     //jtagarmtransn(testmode, 4, LSB, END, RETIDLE);
 319     current_chain = chain;
 320   //}    else  {
 321   //  //debugstr("===NOT change chains===");
 322   //  retval = current_chain;
 323   //}
 324   // put in test mode...
 325   jtag_goto_shift_ir();
 326   jtagarmtransn(testmode, 4, LSB, END, RETIDLE);
 327   return(retval);
 328 }
 329
 330
 331 //!  Connect the appropriate scan chain to TDO/TDI.  SCAN_N, INTEST, ENDS IN SHIFT_DR!!!!!
 332 unsigned long jtagarm7tdmi_scan_intest(int chain) {               // PROVEN
 333   return jtagarm7tdmi_scan(chain, ARM7TDMI_IR_INTEST);
 334 }
 335
 336
 337
 338
 339 //! push an instruction into the pipeline
 340 unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){  // PROVEN
 341   unsigned long retval;
 342   jtagarm7tdmi_scan_intest(1);
 343
 344   jtag_goto_shift_dr();
 345   // if the next instruction is to run using MCLK (master clock), set TDI
 346   if (breakpt)
 347     {
 348     SETMOSI;
 349     count_sysspd_instr_since_debug++;
 350     }
 351   else
 352     {
 353     CLRMOSI;
 354     count_dbgspd_instr_since_debug++;
 355     }
 356   jtag_arm_tcktock();
 357
 358   // Now shift in the 32 bits
 359   retval = jtagarmtransn(instr, 32, MSB, END, RETIDLE);    // Must return to RUN-TEST/IDLE state for instruction to enter pipeline, and causes debug clock.
 360   return(retval);
 361
 362 }
 363
 364 //! push NOP into the instruction pipeline
 365 unsigned long jtagarm7tdmi_nop(char breakpt){  // PROVEN
 366   if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
 367     return jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP, breakpt);
 368   return jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP, breakpt);
 369 }
 370
 371 /*    stolen from ARM DDI0029G documentation, translated using ARM Architecture Reference Manual (14128.pdf)
 372 STR R0, [R0]; Save R0 before use
 373 MOV R0, PC ; Copy PC into R0
 374 STR R0, [R0]; Now save the PC in R0
 375 BX PC ; Jump into ARM state
 376 MOV R8, R8 ;
 377 MOV R8, R8 ;
 378 NOP
 379 NOP
 380
 381 */
 382
 383 //! 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();
 384 unsigned long jtagarm7tdmi_setMode_ARM(unsigned char restart){               // PROVEN  BUT FUGLY! FIXME: clean up and store and replace clobbered r0
 385   jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
 386   unsigned long retval = 0xffL;
 387   if ((current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)){
 388     debugstr("=== Switching to ARM mode ===");
 389     cmddatalong[1] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
 390     cmddatalong[2] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
 391     cmddatalong[3] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_MOV_R0_PC,0);
 392     cmddatalong[4] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,restart);
 393     cmddatalong[5] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_BX_PC,0);
 394   } else {
 395     jtagarm7tdmi_set_register(15,(last_halt_pc|0xfffffffc)-24);
 396     jtagarm7tdmi_nop( restart);
 397     cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_B_IMM,0);
 398   }
 399   if (restart) {
 400     jtagarm7tdmi_restart();
 401   } else {
 402     jtagarm7tdmi_nop(0);
 403     jtagarm7tdmi_nop(0);
 404     jtagarm7tdmi_nop(0);
 405     jtagarm7tdmi_set_register(0,cmddataword[5]);
 406   }
 407   jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
 408   current_dbgstate = jtagarm7tdmi_get_dbgstate();
 409   return(retval);
 410 }
 411
 412
 413 //! set the current mode to ARM, returns PC (FIXME).  Should be used by releasecpu()
 414 unsigned long jtagarm7tdmi_setMode_THUMB(unsigned char restart){               // PROVEN
 415   jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
 416   debugstr("=== Switching to THUMB mode ===");
 417   unsigned long retval = 0xffL;
 418   while (!(current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
 419     last_halt_pc |= 1;
 420     jtagarm7tdmi_set_register(0, last_halt_pc);
 421     jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,restart);
 422     jtagarm7tdmi_instr_primitive(ARM_INSTR_BX_R0,0);
 423     if (restart) {
 424       jtagarm7tdmi_restart();
 425     } else {
 426       jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
 427       jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
 428       jtagarm7tdmi_resettap();                  // seems necessary for some reason.
 429     }
 430     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 431   }
 432   return(retval);
 433 }
 434
 435
 436
 437
 438 /************************* EmbeddedICE Primitives ****************************/
 439 //! shifter for writing to chain2 (EmbeddedICE).
 440 unsigned long eice_write(unsigned char reg, unsigned long data){
 441   unsigned long retval, temp;
 442   jtagarm7tdmi_scan_intest(2);
 443   // Now shift in the 32 bits
 444   jtag_goto_shift_dr();
 445   retval = jtagarmtransn(data, 32, LSB, NOEND, NORETIDLE);          // send in the data - 32-bits lsb
 446   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);              // send in the register address - 5 bits lsb
 447   jtagarmtransn(1, 1, LSB, END, RETIDLE);                           // send in the WRITE bit
 448
 449   return(retval);
 450 }
 451
 452 //! shifter for reading from chain2 (EmbeddedICE).
 453 unsigned long eice_read(unsigned char reg){               // PROVEN
 454   unsigned long temp, retval;
 455   //debugstr("eice_read");
 456   //debughex(reg);
 457   jtagarm7tdmi_scan_intest(2);
 458
 459   // send in the register address - 5 bits LSB
 460   jtag_goto_shift_dr();
 461   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);
 462
 463   // clear TDI to select "read only"
 464   jtagarmtransn(0L, 1, LSB, END, RETIDLE);
 465
 466   jtag_goto_shift_dr();
 467   // Now shift out the 32 bits
 468   retval = jtagarmtransn(0L, 32, LSB, END, RETIDLE);   // atmel arm jtag docs pp.10-11: LSB first
 469   //debughex32(retval);
 470   return(retval);   // atmel arm jtag docs pp.10-11: LSB first
 471
 472 }
 473
 474
 475
 476
 477 /************************* ICEBreaker/EmbeddedICE Stuff ******************************/
 478 //! Grab debug register
 479 unsigned long jtagarm7tdmi_get_dbgstate() {       // ICE Debug register, *NOT* ARM register DSCR    //    PROVEN
 480   //jtagarm7tdmi_resettap();
 481   return eice_read(EICE_DBGSTATUS);
 482 }
 483
 484 //! Grab debug register
 485 unsigned long jtagarm7tdmi_get_dbgctrl() {
 486   return eice_read(EICE_DBGCTRL);
 487 }
 488
 489 //! Update debug register
 490 unsigned long jtagarm7tdmi_set_dbgctrl(unsigned long bits) {
 491   return eice_write(EICE_DBGCTRL, bits);
 492 }
 493
 494
 495
 496 //!  Set and Enable Watchpoint 0
 497 void jtagarm7tdmi_set_watchpoint0(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
 498   // store watchpoint info?  - not right now
 499     // FIXME: store info
 500
 501   eice_write(EICE_WP0ADDR, addr);           // write 0 in watchpoint 0 address
 502   eice_write(EICE_WP0ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 0 address mask
 503   eice_write(EICE_WP0DATA, data);           // write 0 in watchpoint 0 data
 504   eice_write(EICE_WP0DATAMASK, datamask);   // write 0xffffffff in watchpoint 0 data mask
 505   eice_write(EICE_WP0CTRL, ctrlmask);       // write 0x00000100 in watchpoint 0 control value register (enables watchpoint)
 506   eice_write(EICE_WP0CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 0 control mask - only detect the fetch instruction
 507 }
 508
 509 //!  Set and Enable Watchpoint 1
 510 void jtagarm7tdmi_set_watchpoint1(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
 511   // store watchpoint info?  - not right now
 512     // FIXME: store info
 513
 514   eice_write(EICE_WP1ADDR, addr);           // write 0 in watchpoint 1 address
 515   eice_write(EICE_WP1ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 1 address mask
 516   eice_write(EICE_WP1DATA, data);           // write 0 in watchpoint 1 data
 517   eice_write(EICE_WP1DATAMASK, datamask);   // write 0xffffffff in watchpoint 1 data mask
 518   eice_write(EICE_WP1CTRL, ctrl);           // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
 519   eice_write(EICE_WP1CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
 520 }
 521
 522 /******************** Complex Commands **************************/
 523
 524 //! Retrieve a 32-bit Register value
 525 unsigned long jtagarm7tdmi_get_register(unsigned long reg) {                    //PROVEN
 526   unsigned long retval=0L, instr;
 527   if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
 528     instr = THUMB_INSTR_STR_R0_r0 | reg | (reg<<16);
 529   else
 530     instr = (unsigned long)(reg<<12L) | (unsigned long)ARM_READ_REG;   // STR Rx, [R14]
 531
 532   jtagarm7tdmi_nop( 0);
 533   jtagarm7tdmi_nop( 0);
 534   jtagarm7tdmi_instr_primitive(instr, 0);
 535   jtagarm7tdmi_nop( 0);
 536   jtagarm7tdmi_nop( 0);
 537   jtagarm7tdmi_nop( 0);
 538   retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
 539   return retval;
 540 }
 541
 542 //! Set a 32-bit Register value
 543 void jtagarm7tdmi_set_register(unsigned long reg, unsigned long val) {          // PROVEN (assuming target reg is word aligned)
 544   unsigned long instr;
 545   //if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
 546     //instr = THUMB_WRITE_REG
 547     instr = (unsigned long)(((unsigned long)reg<<12L) | ARM_WRITE_REG); //  LDR Rx, [R14]
 548
 549   jtagarm7tdmi_nop( 0);            // push nop into pipeline - clean out the pipeline...
 550   jtagarm7tdmi_nop( 0);            // push nop into pipeline - clean out the pipeline...
 551   jtagarm7tdmi_instr_primitive(instr, 0); // push instr into pipeline - fetch
 552   if (reg == ARM_REG_PC){
 553     jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
 554     jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed
 555     jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed
 556   } else {
 557     jtagarm7tdmi_nop( 0);            // push nop into pipeline - decode
 558     jtagarm7tdmi_nop( 0);            // push nop into pipeline - execute
 559     jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
 560   }
 561   jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed
 562   jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed
 563   jtagarm7tdmi_nop( 0);
 564 }
 565
 566
 567 /*
 568 //! Get all registers, placing them into cmddatalong[0-14]
 569 void jtagarm7tdmi_get_registers() {         // BORKEN.  FIXME
 570   jtagarm7tdmi_nop( 0);
 571   jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0);
 572   jtagarm7tdmi_nop( 0);
 573   jtagarm7tdmi_nop( 0);
 574   cmddatalong[ 0] = jtagarm7tdmi_nop( 0);
 575   cmddatalong[ 1] = jtagarm7tdmi_nop( 0);
 576   cmddatalong[ 2] = jtagarm7tdmi_nop( 0);
 577   cmddatalong[ 3] = jtagarm7tdmi_nop( 0);
 578   cmddatalong[ 4] = jtagarm7tdmi_nop( 0);
 579   cmddatalong[ 5] = jtagarm7tdmi_nop( 0);
 580   cmddatalong[ 6] = jtagarm7tdmi_nop( 0);
 581   cmddatalong[ 7] = jtagarm7tdmi_nop( 0);
 582   cmddatalong[ 8] = jtagarm7tdmi_nop( 0);
 583   cmddatalong[ 9] = jtagarm7tdmi_nop( 0);
 584   cmddatalong[10] = jtagarm7tdmi_nop( 0);
 585   cmddatalong[11] = jtagarm7tdmi_nop( 0);
 586   cmddatalong[12] = jtagarm7tdmi_nop( 0);
 587   cmddatalong[13] = jtagarm7tdmi_nop( 0);
 588   cmddatalong[14] = jtagarm7tdmi_nop( 0);
 589   cmddatalong[15] = jtagarm7tdmi_nop( 0);
 590   jtagarm7tdmi_nop( 0);
 591 }
 592
 593 //! Set all registers from cmddatalong[0-14]
 594 void jtagarm7tdmi_set_registers() {   // using r15 to write through.  not including it.  use set_pc
 595   jtagarm7tdmi_nop( 0);
 596   debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_CLOBBEREGS,0));
 597   jtagarm7tdmi_nop( 0);
 598   jtagarm7tdmi_nop( 0);
 599   jtagarm7tdmi_instr_primitive(cmddatalong[0],0);
 600   jtagarm7tdmi_instr_primitive(cmddatalong[1],0);
 601   jtagarm7tdmi_instr_primitive(cmddatalong[2],0);
 602   jtagarm7tdmi_instr_primitive(cmddatalong[3],0);
 603   jtagarm7tdmi_instr_primitive(cmddatalong[4],0);
 604   jtagarm7tdmi_instr_primitive(cmddatalong[5],0);
 605   jtagarm7tdmi_instr_primitive(cmddatalong[6],0);
 606   jtagarm7tdmi_instr_primitive(cmddatalong[7],0);
 607   jtagarm7tdmi_instr_primitive(cmddatalong[8],0);
 608   jtagarm7tdmi_instr_primitive(cmddatalong[9],0);
 609   jtagarm7tdmi_instr_primitive(cmddatalong[10],0);
 610   jtagarm7tdmi_instr_primitive(cmddatalong[11],0);
 611   jtagarm7tdmi_instr_primitive(cmddatalong[12],0);
 612   jtagarm7tdmi_instr_primitive(cmddatalong[13],0);
 613   jtagarm7tdmi_instr_primitive(cmddatalong[14],0);
 614   jtagarm7tdmi_nop( 0);
 615 }
 616 */
 617 //! Retrieve the CPSR Register value
 618 unsigned long jtagarm7tdmi_get_regCPSR() {
 619   unsigned long retval = 0L, r0;
 620
 621   r0 = jtagarm7tdmi_get_register(0);
 622   jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
 623   jtagarm7tdmi_instr_primitive(ARM_INSTR_MRS_R0_CPSR, 0); // push MRS_R0, CPSR into pipeline - fetch
 624   jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded
 625   jtagarm7tdmi_nop( 0); // push nop into pipeline - execute
 626   retval = jtagarm7tdmi_get_register(0);
 627   jtagarm7tdmi_set_register(0, r0);
 628   return retval;
 629 }
 630
 631 //! Retrieve the CPSR Register value
 632 unsigned long jtagarm7tdmi_set_regCPSR(unsigned long val) {
 633   unsigned long r0;
 634
 635   r0 = jtagarm7tdmi_get_register(0);
 636   jtagarm7tdmi_set_register(0, val);
 637   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - clean out the pipeline...
 638   debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_MSR_cpsr_cxsf_R0, 0)); // push MSR cpsr_cxsf, R0 into pipeline - fetch
 639   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - decoded
 640   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - execute
 641   jtagarm7tdmi_set_register(0, r0);
 642   return(val);
 643 }
 644
 645 unsigned long wait_debug(unsigned long retval){
 646   // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
 647   current_dbgstate = jtagarm7tdmi_get_dbgstate();
 648   while ((!(current_dbgstate & 9L) == 9)  && retval > 0){
 649     delay(1);
 650     retval --;
 651     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 652   }
 653   return retval;
 654 }
 655
 656 /****
 657 //! Write data to address - Assume TAP in run-test/idle state
 658 unsigned long jtagarm7tdmi_writemem(unsigned long adr, unsigned long data){
 659   unsigned long retval = 0xffL;
 660   unsigned long r0=0L, r1=-1L;
 661
 662   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
 663   r1 = jtagarm7tdmi_get_register(1);
 664   jtagarm7tdmi_set_register(0, adr);        // write address into R0
 665   jtagarm7tdmi_set_register(1, data);       // write data in R1
 666   debughex32(jtagarm7tdmi_get_register(0));
 667   debughex32(jtagarm7tdmi_get_register(1));
 668   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
 669   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
 670   jtagarm7tdmi_instr_primitive(ARM_INSTR_STR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline
 671   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
 672   jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
 673
 674   if (wait_debug(0xffL) == 0){
 675     debugstr("FAILED TO WRITE MEMORY/RE-ENTER DEBUG MODE");
 676     return (-1);
 677   } else {
 678     retval = jtagarm7tdmi_get_register(1);  // read memory value from R1 register
 679     jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1
 680     jtagarm7tdmi_set_register(0, r0);
 681   }
 682   return retval;
 683 }
 684
 685
 686
 687 //! Read data from address
 688 unsigned long jtagarm7tdmi_readmem(unsigned long adr){
 689   unsigned long retval = 0xffL;
 690   unsigned long r0=0L, r1=-1L;
 691
 692   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
 693   r1 = jtagarm7tdmi_get_register(1);
 694   jtagarm7tdmi_set_register(0, adr);        // write address into R0
 695   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
 696   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
 697   jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, [R0], #4 into instruction pipeline  (autoincrements for consecutive reads)
 698   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
 699   jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
 700
 701   // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
 702   current_dbgstate = jtagarm7tdmi_get_dbgstate();
 703   debughex(current_dbgstate);
 704   while ((!(current_dbgstate & 9L) == 9)  && retval > 0){
 705     delay(1);
 706     retval --;
 707     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 708   }
 709   // FIXME: this may end up changing te current debug-state.  should we compare to current_dbgstate?
 710   if (retval == 0){
 711     debugstr("FAILED TO READ MEMORY/RE-ENTER DEBUG MODE");
 712     return (-1);
 713   } else {
 714     retval = jtagarm7tdmi_get_register(1);  // read memory value from R1 register
 715     //jtagarm7tdmi_set_register(1, r1);       // restore R0 and R1
 716     //jtagarm7tdmi_set_register(0, r0);
 717   }
 718   return retval;
 719 }
 720
 721 */
 722
 723
 724 //! Read Program Counter
 725 unsigned long jtagarm7tdmi_get_real_pc(){
 726     unsigned long val;
 727     val = jtagarm7tdmi_get_register(ARM_REG_PC);
 728     if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
 729         val -= (4*2);                           // thumb uses 2 bytes per instruction.
 730     else
 731         val -= (6*4);                           // assume 6 instructions at 4 bytes a piece.
 732     return val;
 733 }
 734
 735 //! Halt CPU - returns 0xffff if the operation fails to complete within
 736 unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
 737   int waitcount = 0xffL;
 738
 739   // store the debug state
 740   last_halt_debug_state = jtagarm7tdmi_get_dbgstate();
 741
 742   //jtagarm7tdmi_set_dbgctrl(7);
 743   // store watchpoint info?  - not right now
 744   jtagarm7tdmi_set_watchpoint1(0, 0xffffffff, 0, 0xffffffff, 0x100L, 0xfffffff7);
 745
 746
 747   /*  // old method
 748   eice_write(EICE_WP1ADDR, 0L);              // write 0 in watchpoint 1 address
 749   eice_write(EICE_WP1ADDRMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 address mask
 750   eice_write(EICE_WP1DATA, 0L);              // write 0 in watchpoint 1 data
 751   eice_write(EICE_WP1DATAMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 data mask
 752   eice_write(EICE_WP1CTRL, 0x100L);          // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
 753   eice_write(EICE_WP1CTRLMASK, 0xfffffff7); // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
 754   */
 755
 756   // poll until debug status says the cpu is in debug mode
 757   while (!(current_dbgstate & 0x1L)   && waitcount-- > 0){
 758     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 759     delay(1);
 760   }
 761
 762   //jtagarm7tdmi_set_dbgctrl(0);
 763   jtagarm7tdmi_set_watchpoint1(0, 0x0, 0, 0x0, 0x0L, 0xfffffff7);
 764   //jtagarm7tdmi_disable_watchpoint1();
 765
 766   //eice_write(EICE_WP1CTRL, 0x0L);            // write 0 in watchpoint 0 control value - disables watchpoint 0
 767
 768   // store the debug state program counter.
 769   last_halt_pc = jtagarm7tdmi_get_real_pc();
 770   count_dbgspd_instr_since_debug = 0L;          // should be able to clean this up and remove all this tracking nonsense.
 771   count_sysspd_instr_since_debug = 0L;          // should be able to clean this up and remove all this tracking nonsense.
 772
 773   //FIXME: is this necessary?  for now, yes... but perhaps make the rest of the module arm/thumb impervious.
 774   // get into ARM mode if the T flag is set (Thumb mode)
 775   while (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT && waitcount-- > 0) {
 776     jtagarm7tdmi_setMode_ARM(0);
 777     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 778   }
 779   jtagarm7tdmi_resettap();
 780   jtagarm7tdmi_set_register(ARM_REG_PC, last_halt_pc & 0xfffffffc);     // make sure PC is word-aligned.  otherwise all other register accesses get all wonky.
 781   return waitcount;
 782 }
 783
 784 unsigned long jtagarm7tdmi_releasecpu(){
 785   int waitcount = 0xff;
 786   jtagarm7tdmi_nop(0);                          // NOP
 787   jtagarm7tdmi_nop(1);                          // NOP/BREAKPT
 788
 789
 790   // four possible states.  arm mode needing arm mode, arm mode needing thumb mode, thumb mode needing arm mode, and thumb mode needing thumb mode
 791   // FIXME:  BX is bs.  it requires the clobbering of at least one register.... this is not acceptable.
 792   // FIXME:  so we either switch modes, then correct the register before restarting with bx, or find the way to use SPSR
 793   if (last_halt_debug_state & JTAG_ARM7TDMI_DBG_TBIT){
 794     // need to get to thumb mode
 795     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
 796     jtagarm7tdmi_setMode_THUMB(1);
 797   } else {
 798     jtagarm7tdmi_setMode_ARM(1);
 799     //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
 800   }
 801
 802
 803   jtagarm7tdmi_restart();
 804   jtagarm7tdmi_resettap();
 805   //jtag_goto_shift_ir();
 806   //jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); // VERB_RESTART
 807
 808   // wait until restart-bit set in debug state register
 809   while ((current_dbgstate & JTAG_ARM7TDMI_DBG_DBGACK) && waitcount > -1){
 810     msdelay(1);
 811     waitcount --;
 812     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 813   }
 814   last_halt_debug_state = -1;
 815   last_halt_pc = -1;
 816   return waitcount;
 817 }
 818
 819
 820
 821
 822 ///////////////////////////////////////////////////////////////////////////////////////////////////
 823 //! Handles ARM7TDMI JTAG commands.  Forwards others to JTAG.
 824 void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len){
 825   //register char blocks;
 826
 827   unsigned int val; //, i;
 828   //unsigned long at;
 829
 830   //jtagarm7tdmi_resettap();
 831   //current_dbgstate = jtagarm7tdmi_get_dbgstate();
 832
 833   switch(verb){
 834   case START:
 835     //Enter JTAG mode.
 836     debughex32(jtagarm7tdmi_start());
 837     cmddatalong[0] = jtagarm7tdmi_get_dbgstate();
 838     txdata(app,verb,0x4);
 839     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 840     break;
 841     /*
 842   case JTAGARM7TDMI_READMEM:
 843     at     = cmddatalong[0];
 844     blocks = cmddatalong[1];
 845
 846     txhead(app,verb,len);
 847
 848         jtagarm7tdmi_resettap();
 849         delay(1);
 850
 851     for(i=0;i<blocks;i++){
 852           val=jtagarm7tdmi_readmem(at);
 853
 854           serial_tx(val&0xFFL);
 855           serial_tx((val&0xFF00L)>>8);
 856           serial_tx((val&0xFF0000L)>>8);
 857           serial_tx((val&0xFF000000L)>>8);
 858           at+=4;
 859       }
 860
 861
 862     break;
 863   case PEEK:
 864         jtagarm7tdmi_resettap();
 865         delay(1);
 866         cmddatalong[0] = jtagarm7tdmi_readmem(cmddatalong[0]);
 867     txdata(app,verb,4);
 868     break;
 869     */
 870   case JTAGARM7TDMI_GET_CHIP_ID:
 871         jtagarm7tdmi_resettap();
 872     cmddatalong[0] = jtagarm7tdmi_idcode();
 873     txdata(app,verb,4);
 874     break;
 875
 876 /*
 877   case JTAGARM7TDMI_WRITEMEM:
 878   case POKE:
 879         jtagarm7tdmi_resettap();
 880     jtagarm7tdmi_writemem(cmddatalong[0],
 881                        cmddataword[2]);
 882     cmddataword[0]=jtagarm7tdmi_readmem(cmddatalong[0]);
 883     txdata(app,verb,4);
 884     break;
 885 */
 886   case JTAGARM7TDMI_HALTCPU:
 887     cmddatalong[0] = jtagarm7tdmi_haltcpu();
 888     txdata(app,verb,4);
 889     break;
 890   case JTAGARM7TDMI_RELEASECPU:
 891         //jtagarm7tdmi_resettap();
 892     cmddatalong[0] = jtagarm7tdmi_releasecpu();
 893     txdata(app,verb,4);
 894     break;
 895   //unimplemented functions
 896   //case JTAGARM7TDMI_SETINSTRFETCH:
 897   //case JTAGARM7TDMI_WRITEFLASH:
 898   //case JTAGARM7TDMI_ERASEFLASH:
 899   case JTAGARM7TDMI_SET_PC:
 900     //jtagarm7tdmi_setpc(cmddatalong[0]);
 901     last_halt_pc = cmddatalong[0];
 902     txdata(app,verb,0);
 903     break;
 904   case JTAGARM7TDMI_GET_DEBUG_CTRL:
 905     cmddatalong[0] = jtagarm7tdmi_get_dbgctrl();
 906     txdata(app,verb,1);
 907     break;
 908   case JTAGARM7TDMI_SET_DEBUG_CTRL:
 909     cmddatalong[0] = jtagarm7tdmi_set_dbgctrl(cmddata[0]);
 910     txdata(app,verb,4);
 911     break;
 912   case JTAGARM7TDMI_GET_PC:
 913     cmddatalong[0] = last_halt_pc;
 914     txdata(app,verb,4);
 915     break;
 916   case JTAGARM7TDMI_GET_DEBUG_STATE:
 917     //jtagarm7tdmi_resettap();            // Shouldn't need this, but currently do.  FIXME!
 918     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 919     cmddatalong[0] = current_dbgstate;
 920     txdata(app,verb,4);
 921     break;
 922   //case JTAGARM7TDMI_GET_WATCHPOINT:
 923   //case JTAGARM7TDMI_SET_WATCHPOINT:
 924   case JTAGARM7TDMI_GET_REGISTER:
 925         //jtagarm7tdmi_resettap();
 926     val = cmddata[0];
 927     cmddatalong[0] = jtagarm7tdmi_get_register(val);
 928     txdata(app,verb,4);
 929     break;
 930   case JTAGARM7TDMI_SET_REGISTER:
 931         //jtagarm7tdmi_resettap();
 932     jtagarm7tdmi_set_register(cmddatalong[1], cmddatalong[0]);
 933     txdata(app,verb,4);
 934     break;
 935   case JTAGARM7TDMI_DEBUG_INSTR:
 936         //jtagarm7tdmi_resettap();
 937     //cmddataword[0] = jtagarm7tdmi_exec(cmddataword[0], cmddata[4]);
 938     cmddatalong[0] = jtagarm7tdmi_instr_primitive(cmddatalong[0],cmddata[4]);
 939     txdata(app,verb,8);
 940     break;
 941   //case JTAGARM7TDMI_STEP_INSTR:
 942 /*  case JTAGARM7TDMI_READ_CODE_MEMORY:
 943   case JTAGARM7TDMI_WRITE_FLASH_PAGE:
 944   case JTAGARM7TDMI_READ_FLASH_PAGE:
 945   case JTAGARM7TDMI_MASS_ERASE_FLASH:
 946   case JTAGARM7TDMI_PROGRAM_FLASH:
 947   case JTAGARM7TDMI_LOCKCHIP:
 948   case JTAGARM7TDMI_CHIP_ERASE:
 949   */
 950 // Really ARM specific stuff
 951   case JTAGARM7TDMI_GET_CPSR:
 952         jtagarm7tdmi_resettap();
 953     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
 954     txdata(app,verb,4);
 955     break;
 956   case JTAGARM7TDMI_SET_CPSR:
 957         jtagarm7tdmi_resettap();
 958     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
 959     txdata(app,verb,4);
 960     break;
 961   case JTAGARM7TDMI_GET_SPSR:           // FIXME: NOT EVEN CLOSE TO CORRECT
 962         jtagarm7tdmi_resettap();
 963     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
 964     txdata(app,verb,4);
 965     break;
 966   case JTAGARM7TDMI_SET_SPSR:           // FIXME: NOT EVEN CLOSE TO CORRECT
 967         jtagarm7tdmi_resettap();
 968     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
 969     txdata(app,verb,4);
 970     break;
 971   case JTAGARM7TDMI_SET_MODE_THUMB:
 972         jtagarm7tdmi_resettap();
 973     cmddatalong[0] = jtagarm7tdmi_setMode_THUMB(cmddata[0]);
 974     txdata(app,verb,4);
 975     break;
 976   case JTAGARM7TDMI_SET_MODE_ARM:
 977         jtagarm7tdmi_resettap();
 978     cmddatalong[0] = jtagarm7tdmi_setMode_ARM(cmddata[0]);
 979     txdata(app,verb,4);
 980     break;
 981   case JTAGARM7TDMI_SET_IR:
 982         //jtagarm7tdmi_resettap();
 983     jtag_goto_shift_ir();
 984     cmddataword[0] = jtagarmtransn(cmddata[0], 4, LSB, END, cmddata[1]);
 985     current_chain = -1;
 986     txdata(app,verb,2);
 987     break;
 988   case JTAGARM7TDMI_WAIT_DBG:
 989     cmddatalong[0] = wait_debug(cmddatalong[0]);
 990     txdata(app,verb,4);
 991     break;
 992   case JTAGARM7TDMI_SHIFT_DR:
 993         jtagarm7tdmi_resettap();
 994     jtag_goto_shift_dr();
 995     cmddatalong[0] = jtagarmtransn(cmddatalong[1],cmddata[0],cmddata[1],cmddata[2],cmddata[3]);
 996     txdata(app,verb,4);
 997     break;
 998   case JTAGARM7TDMI_CHAIN0:
 999     jtagarm7tdmi_scan_intest(0);
1000     jtag_goto_shift_dr();
1001     debughex32(cmddatalong[0]);
1002     debughex(cmddataword[4]);
1003     debughex32(cmddatalong[1]);
1004     debughex32(cmddatalong[3]);
1005     cmddatalong[0] = jtagarmtransn(cmddatalong[0], 32, LSB, NOEND, NORETIDLE);
1006     cmddatalong[2] = jtagarmtransn(cmddataword[4], 9, MSB, NOEND, NORETIDLE);
1007     cmddatalong[1] = jtagarmtransn(cmddatalong[1], 32, MSB, NOEND, NORETIDLE);
1008     cmddatalong[3] = jtagarmtransn(cmddatalong[3], 32, MSB, END, RETIDLE);
1009     txdata(app,verb,16);
1010     break;
1011   case JTAGARM7TDMI_SETWATCH0:
1012     jtagarm7tdmi_set_watchpoint0(cmddatalong[0], cmddatalong[1], cmddatalong[2], cmddatalong[3], cmddatalong[4], cmddatalong[5]);
1013     txdata(app,verb,4);
1014     break;
1015   case JTAGARM7TDMI_SETWATCH1:
1016     jtagarm7tdmi_set_watchpoint0(cmddatalong[0], cmddatalong[1], cmddatalong[2], cmddatalong[3], cmddatalong[4], cmddatalong[5]);
1017     txdata(app,verb,4);
1018     break;
1019   default:
1020     jtaghandle(app,verb,len);
1021   }
1022 }
1023
1024
1025
1026
1027 /*****************************
1028 Captured from FlySwatter against AT91SAM7S, to be used by me for testing.  ignore
1029
1030 > arm reg
1031 System and User mode registers
1032       r0: 300000df       r1: 00000000       r2: 58000000       r3: 00200a75
1033       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1034       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1035      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 000000fc
1036     cpsr: 00000093
1037
1038 FIQ mode shadow registers
1039   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1040  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1041
1042 Supervisor mode shadow registers
1043   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1044
1045 Abort mode shadow registers
1046   sp_abt: 00000000   lr_abt: 00000000 spsr_abt: e00000ff
1047
1048 IRQ mode shadow registers
1049   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1050
1051 Undefined instruction mode shadow registers
1052   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1053 >
1054 > step;arm reg
1055 target state: halted
1056 target halted in ARM state due to single-step, current mode: Supervisor
1057 cpsr: 0x00000093 pc: 0x00000100
1058 System and User mode registers
1059       r0: 300000df       r1: 00000000       r2: 00200000       r3: 00200a75
1060       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1061       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1062      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000100
1063     cpsr: 00000093
1064
1065 FIQ mode shadow registers
1066   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1067  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1068
1069 Supervisor mode shadow registers
1070   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1071
1072 Abort mode shadow registers
1073   sp_abt: 00000000   lr_abt: 00000000 spsr_abt: e00000ff
1074
1075 IRQ mode shadow registers
1076   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1077
1078 Undefined instruction mode shadow registers
1079   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1080 >
1081  step;arm reg
1082 target state: halted
1083 target halted in ARM state due to single-step, current mode: Abort
1084 cpsr: 0x00000097 pc: 0x00000010
1085 System and User mode registers
1086       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1087       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1088       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1089      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1090     cpsr: 00000097
1091
1092 FIQ mode shadow registers
1093   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1094  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1095
1096 Supervisor mode shadow registers
1097   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1098
1099 Abort mode shadow registers
1100   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1101
1102 IRQ mode shadow registers
1103   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1104
1105 Undefined instruction mode shadow registers
1106   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1107 > step;arm reg
1108 target state: halted
1109 target halted in ARM state due to single-step, current mode: Abort
1110 cpsr: 0x00000097 pc: 0x00000010
1111 System and User mode registers
1112       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1113       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1114       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1115      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1116     cpsr: 00000097
1117
1118 FIQ mode shadow registers
1119   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1120  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1121
1122 Supervisor mode shadow registers
1123   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1124
1125 Abort mode shadow registers
1126   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1127
1128 IRQ mode shadow registers
1129   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1130
1131 Undefined instruction mode shadow registers
1132   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1133 > step;arm reg
1134 target state: halted
1135 target halted in ARM state due to single-step, current mode: Abort
1136 cpsr: 0x00000097 pc: 0x00000010
1137 System and User mode registers
1138       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1139       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1140       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1141      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1142     cpsr: 00000097
1143
1144 FIQ mode shadow registers
1145   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1146  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1147
1148 Supervisor mode shadow registers
1149   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1150
1151 Abort mode shadow registers
1152   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1153
1154 IRQ mode shadow registers
1155   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1156
1157 Undefined instruction mode shadow registers
1158   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1159 > step;arm reg
1160 target state: halted
1161 target halted in ARM state due to single-step, current mode: Abort
1162 cpsr: 0x00000097 pc: 0x00000010
1163 System and User mode registers
1164       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1165       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1166       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1167      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1168     cpsr: 00000097
1169
1170 FIQ mode shadow registers
1171   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1172  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1173
1174 Supervisor mode shadow registers
1175   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1176
1177 Abort mode shadow registers
1178   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1179
1180 IRQ mode shadow registers
1181   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1182
1183 Undefined instruction mode shadow registers
1184   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1185 > step;arm reg
1186 target state: halted
1187 target halted in ARM state due to single-step, current mode: Abort
1188 cpsr: 0x00000097 pc: 0x00000010
1189 System and User mode registers
1190       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1191       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1192       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1193      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1194     cpsr: 00000097
1195
1196 FIQ mode shadow registers
1197   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1198  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1199
1200 Supervisor mode shadow registers
1201   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1202
1203 Abort mode shadow registers
1204   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1205
1206 IRQ mode shadow registers
1207   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1208
1209 Undefined instruction mode shadow registers
1210   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1211 > step;arm reg
1212 target state: halted
1213 target halted in ARM state due to single-step, current mode: Abort
1214 cpsr: 0x00000097 pc: 0x00000010
1215 System and User mode registers
1216       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1217       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1218       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1219      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1220     cpsr: 00000097
1221
1222 FIQ mode shadow registers
1223   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1224  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1225
1226 Supervisor mode shadow registers
1227   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1228
1229 Abort mode shadow registers
1230   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1231
1232 IRQ mode shadow registers
1233   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1234
1235 Undefined instruction mode shadow registers
1236   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1237 > step;arm reg
1238 target state: halted
1239 target halted in ARM state due to single-step, current mode: Abort
1240 cpsr: 0x00000097 pc: 0x00000010
1241 System and User mode registers
1242       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1243       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1244       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1245      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1246     cpsr: 00000097
1247
1248 FIQ mode shadow registers
1249   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1250  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1251
1252 Supervisor mode shadow registers
1253   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1254
1255 Abort mode shadow registers
1256   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1257
1258 IRQ mode shadow registers
1259   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1260
1261 Undefined instruction mode shadow registers
1262   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1263 > step;arm reg
1264 target state: halted
1265 target halted in ARM state due to single-step, current mode: Abort
1266 cpsr: 0x00000097 pc: 0x00000010
1267 System and User mode registers
1268       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1269       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1270       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1271      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1272     cpsr: 00000097
1273
1274 FIQ mode shadow registers
1275   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1276  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1277
1278 Supervisor mode shadow registers
1279   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1280
1281 Abort mode shadow registers
1282   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1283
1284 IRQ mode shadow registers
1285   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1286
1287 Undefined instruction mode shadow registers
1288   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1289 > step;arm reg
1290 target state: halted
1291 target halted in ARM state due to single-step, current mode: Abort
1292 cpsr: 0x00000097 pc: 0x00000010
1293 System and User mode registers
1294       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1295       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1296       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1297      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1298     cpsr: 00000097
1299
1300 FIQ mode shadow registers
1301   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1302  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1303
1304 Supervisor mode shadow registers
1305   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1306
1307 Abort mode shadow registers
1308   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1309
1310 IRQ mode shadow registers
1311   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1312
1313 Undefined instruction mode shadow registers
1314   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1315 >
1316 */