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   return(retval);
 324 }
 325
 326
 327 //!  Connect the appropriate scan chain to TDO/TDI.  SCAN_N, INTEST, ENDS IN SHIFT_DR!!!!!
 328 unsigned long jtagarm7tdmi_scan_intest(int chain) {               // PROVEN
 329   return jtagarm7tdmi_scan(chain, ARM7TDMI_IR_INTEST);
 330 }
 331
 332
 333
 334
 335 //! push an instruction into the pipeline
 336 unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){  // PROVEN
 337   unsigned long retval;
 338   jtagarm7tdmi_scan_intest(1);
 339
 340   jtag_goto_shift_dr();
 341   // if the next instruction is to run using MCLK (master clock), set TDI
 342   if (breakpt)
 343     {
 344     SETMOSI;
 345     count_sysspd_instr_since_debug++;
 346     }
 347   else
 348     {
 349     CLRMOSI;
 350     count_dbgspd_instr_since_debug++;
 351     }
 352   jtag_arm_tcktock();
 353
 354   // Now shift in the 32 bits
 355   retval = jtagarmtransn(instr, 32, MSB, END, RETIDLE);    // Must return to RUN-TEST/IDLE state for instruction to enter pipeline, and causes debug clock.
 356   return(retval);
 357
 358 }
 359
 360 //! push NOP into the instruction pipeline
 361 unsigned long jtagarm7tdmi_nop(char breakpt){  // PROVEN
 362   if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
 363     return jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP, breakpt);
 364   return jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP, breakpt);
 365 }
 366
 367 /*    stolen from ARM DDI0029G documentation, translated using ARM Architecture Reference Manual (14128.pdf)
 368 STR R0, [R0]; Save R0 before use
 369 MOV R0, PC ; Copy PC into R0
 370 STR R0, [R0]; Now save the PC in R0
 371 BX PC ; Jump into ARM state
 372 MOV R8, R8 ;
 373 MOV R8, R8 ;
 374 NOP
 375 NOP
 376
 377 */
 378
 379 //! 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();
 380 unsigned long jtagarm7tdmi_setMode_ARM(unsigned char restart){               // PROVEN  BUT FUGLY! FIXME: clean up and store and replace clobbered r0
 381   jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
 382   unsigned long retval = 0xffL;
 383   if ((current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)){
 384     debugstr("=== Switching to ARM mode ===");
 385     cmddatalong[1] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
 386     cmddatalong[2] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
 387     cmddatalong[3] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_MOV_R0_PC,0);
 388     cmddatalong[4] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,restart);
 389     cmddatalong[5] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_BX_PC,0);
 390   } else {
 391     jtagarm7tdmi_set_register(15,(last_halt_pc|0xfffffffc)-24);
 392     jtagarm7tdmi_nop( restart);
 393     cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_B_IMM,0);
 394   }
 395   if (restart) {
 396     jtagarm7tdmi_restart();
 397   } else {
 398     jtagarm7tdmi_nop(0);
 399     jtagarm7tdmi_nop(0);
 400     jtagarm7tdmi_nop(0);
 401     jtagarm7tdmi_set_register(0,cmddataword[5]);
 402   }
 403   jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
 404   current_dbgstate = jtagarm7tdmi_get_dbgstate();
 405   return(retval);
 406 }
 407
 408
 409 //! set the current mode to ARM, returns PC (FIXME).  Should be used by releasecpu()
 410 unsigned long jtagarm7tdmi_setMode_THUMB(unsigned char restart){               // PROVEN
 411   jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
 412   debugstr("=== Switching to THUMB mode ===");
 413   unsigned long retval = 0xffL;
 414   while (!(current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
 415     last_halt_pc |= 1;
 416     jtagarm7tdmi_set_register(0, last_halt_pc);
 417     jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,restart);
 418     jtagarm7tdmi_instr_primitive(ARM_INSTR_BX_R0,0);
 419     if (restart) {
 420       jtagarm7tdmi_restart();
 421     } else {
 422       jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
 423       jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
 424       jtagarm7tdmi_resettap();                  // seems necessary for some reason.
 425     }
 426     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 427   }
 428   return(retval);
 429 }
 430
 431
 432
 433
 434 /************************* EmbeddedICE Primitives ****************************/
 435 //! shifter for writing to chain2 (EmbeddedICE).
 436 unsigned long eice_write(unsigned char reg, unsigned long data){
 437   unsigned long retval, temp;
 438   jtagarm7tdmi_scan_intest(2);
 439   // Now shift in the 32 bits
 440   jtag_goto_shift_dr();
 441   retval = jtagarmtransn(data, 32, LSB, NOEND, NORETIDLE);          // send in the data - 32-bits lsb
 442   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);              // send in the register address - 5 bits lsb
 443   jtagarmtransn(1, 1, LSB, END, RETIDLE);                           // send in the WRITE bit
 444
 445   return(retval);
 446 }
 447
 448 //! shifter for reading from chain2 (EmbeddedICE).
 449 unsigned long eice_read(unsigned char reg){               // PROVEN
 450   unsigned long temp, retval;
 451   //debugstr("eice_read");
 452   //debughex(reg);
 453   jtagarm7tdmi_scan_intest(2);
 454
 455   // send in the register address - 5 bits LSB
 456   jtag_goto_shift_dr();
 457   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);
 458
 459   // clear TDI to select "read only"
 460   jtagarmtransn(0L, 1, LSB, END, RETIDLE);
 461
 462   jtag_goto_shift_dr();
 463   // Now shift out the 32 bits
 464   retval = jtagarmtransn(0L, 32, LSB, END, RETIDLE);   // atmel arm jtag docs pp.10-11: LSB first
 465   //debughex32(retval);
 466   return(retval);   // atmel arm jtag docs pp.10-11: LSB first
 467
 468 }
 469
 470
 471
 472
 473 /************************* ICEBreaker/EmbeddedICE Stuff ******************************/
 474 //! Grab debug register
 475 unsigned long jtagarm7tdmi_get_dbgstate() {       // ICE Debug register, *NOT* ARM register DSCR    //    PROVEN
 476   //jtagarm7tdmi_resettap();
 477   return eice_read(EICE_DBGSTATUS);
 478 }
 479
 480 //! Grab debug register
 481 unsigned long jtagarm7tdmi_get_dbgctrl() {
 482   return eice_read(EICE_DBGCTRL);
 483 }
 484
 485 //! Update debug register
 486 unsigned long jtagarm7tdmi_set_dbgctrl(unsigned long bits) {
 487   return eice_write(EICE_DBGCTRL, bits);
 488 }
 489
 490
 491
 492 //!  Set and Enable Watchpoint 0
 493 void jtagarm7tdmi_set_watchpoint0(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
 494   // store watchpoint info?  - not right now
 495     // FIXME: store info
 496
 497   eice_write(EICE_WP0ADDR, addr);           // write 0 in watchpoint 0 address
 498   eice_write(EICE_WP0ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 0 address mask
 499   eice_write(EICE_WP0DATA, data);           // write 0 in watchpoint 0 data
 500   eice_write(EICE_WP0DATAMASK, datamask);   // write 0xffffffff in watchpoint 0 data mask
 501   eice_write(EICE_WP0CTRL, ctrlmask);       // write 0x00000100 in watchpoint 0 control value register (enables watchpoint)
 502   eice_write(EICE_WP0CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 0 control mask - only detect the fetch instruction
 503 }
 504
 505 //!  Set and Enable Watchpoint 1
 506 void jtagarm7tdmi_set_watchpoint1(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
 507   // store watchpoint info?  - not right now
 508     // FIXME: store info
 509
 510   eice_write(EICE_WP1ADDR, addr);           // write 0 in watchpoint 1 address
 511   eice_write(EICE_WP1ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 1 address mask
 512   eice_write(EICE_WP1DATA, data);           // write 0 in watchpoint 1 data
 513   eice_write(EICE_WP1DATAMASK, datamask);   // write 0xffffffff in watchpoint 1 data mask
 514   eice_write(EICE_WP1CTRL, ctrl);           // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
 515   eice_write(EICE_WP1CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
 516 }
 517
 518 /******************** Complex Commands **************************/
 519
 520 //! Retrieve a 32-bit Register value
 521 unsigned long jtagarm7tdmi_get_register(unsigned long reg) {                    //PROVEN
 522   unsigned long retval=0L, instr;
 523   if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
 524     instr = THUMB_INSTR_STR_R0_r0 | reg | (reg<<16);
 525   else
 526     instr = (unsigned long)(reg<<12L) | (unsigned long)ARM_READ_REG;   // STR Rx, [R14]
 527
 528   jtagarm7tdmi_nop( 0);
 529   jtagarm7tdmi_nop( 0);
 530   jtagarm7tdmi_instr_primitive(instr, 0);
 531   jtagarm7tdmi_nop( 0);
 532   jtagarm7tdmi_nop( 0);
 533   jtagarm7tdmi_nop( 0);
 534   retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
 535   return retval;
 536 }
 537
 538 //! Set a 32-bit Register value
 539 void jtagarm7tdmi_set_register(unsigned long reg, unsigned long val) {          // PROVEN (assuming target reg is word aligned)
 540   unsigned long instr;
 541   //if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
 542     //instr = THUMB_WRITE_REG
 543     instr = (unsigned long)(((unsigned long)reg<<12L) | ARM_WRITE_REG); //  LDR Rx, [R14]
 544
 545   jtagarm7tdmi_nop( 0);            // push nop into pipeline - clean out the pipeline...
 546   jtagarm7tdmi_nop( 0);            // push nop into pipeline - clean out the pipeline...
 547   jtagarm7tdmi_instr_primitive(instr, 0); // push instr into pipeline - fetch
 548   if (reg == ARM_REG_PC){
 549     jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
 550     jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed
 551     jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed
 552   } else {
 553     jtagarm7tdmi_nop( 0);            // push nop into pipeline - decode
 554     jtagarm7tdmi_nop( 0);            // push nop into pipeline - execute
 555     jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
 556   }
 557   jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed
 558   jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed
 559   jtagarm7tdmi_nop( 0);
 560 }
 561
 562
 563 /*
 564 //! Get all registers, placing them into cmddatalong[0-14]
 565 void jtagarm7tdmi_get_registers() {         // BORKEN.  FIXME
 566   jtagarm7tdmi_nop( 0);
 567   jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0);
 568   jtagarm7tdmi_nop( 0);
 569   jtagarm7tdmi_nop( 0);
 570   cmddatalong[ 0] = jtagarm7tdmi_nop( 0);
 571   cmddatalong[ 1] = jtagarm7tdmi_nop( 0);
 572   cmddatalong[ 2] = jtagarm7tdmi_nop( 0);
 573   cmddatalong[ 3] = jtagarm7tdmi_nop( 0);
 574   cmddatalong[ 4] = jtagarm7tdmi_nop( 0);
 575   cmddatalong[ 5] = jtagarm7tdmi_nop( 0);
 576   cmddatalong[ 6] = jtagarm7tdmi_nop( 0);
 577   cmddatalong[ 7] = jtagarm7tdmi_nop( 0);
 578   cmddatalong[ 8] = jtagarm7tdmi_nop( 0);
 579   cmddatalong[ 9] = jtagarm7tdmi_nop( 0);
 580   cmddatalong[10] = jtagarm7tdmi_nop( 0);
 581   cmddatalong[11] = jtagarm7tdmi_nop( 0);
 582   cmddatalong[12] = jtagarm7tdmi_nop( 0);
 583   cmddatalong[13] = jtagarm7tdmi_nop( 0);
 584   cmddatalong[14] = jtagarm7tdmi_nop( 0);
 585   cmddatalong[15] = jtagarm7tdmi_nop( 0);
 586   jtagarm7tdmi_nop( 0);
 587 }
 588
 589 //! Set all registers from cmddatalong[0-14]
 590 void jtagarm7tdmi_set_registers() {   // using r15 to write through.  not including it.  use set_pc
 591   jtagarm7tdmi_nop( 0);
 592   debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_CLOBBEREGS,0));
 593   jtagarm7tdmi_nop( 0);
 594   jtagarm7tdmi_nop( 0);
 595   jtagarm7tdmi_instr_primitive(cmddatalong[0],0);
 596   jtagarm7tdmi_instr_primitive(cmddatalong[1],0);
 597   jtagarm7tdmi_instr_primitive(cmddatalong[2],0);
 598   jtagarm7tdmi_instr_primitive(cmddatalong[3],0);
 599   jtagarm7tdmi_instr_primitive(cmddatalong[4],0);
 600   jtagarm7tdmi_instr_primitive(cmddatalong[5],0);
 601   jtagarm7tdmi_instr_primitive(cmddatalong[6],0);
 602   jtagarm7tdmi_instr_primitive(cmddatalong[7],0);
 603   jtagarm7tdmi_instr_primitive(cmddatalong[8],0);
 604   jtagarm7tdmi_instr_primitive(cmddatalong[9],0);
 605   jtagarm7tdmi_instr_primitive(cmddatalong[10],0);
 606   jtagarm7tdmi_instr_primitive(cmddatalong[11],0);
 607   jtagarm7tdmi_instr_primitive(cmddatalong[12],0);
 608   jtagarm7tdmi_instr_primitive(cmddatalong[13],0);
 609   jtagarm7tdmi_instr_primitive(cmddatalong[14],0);
 610   jtagarm7tdmi_nop( 0);
 611 }
 612 */
 613 //! Retrieve the CPSR Register value
 614 unsigned long jtagarm7tdmi_get_regCPSR() {
 615   unsigned long retval = 0L, r0;
 616
 617   r0 = jtagarm7tdmi_get_register(0);
 618   jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
 619   jtagarm7tdmi_instr_primitive(ARM_INSTR_MRS_R0_CPSR, 0); // push MRS_R0, CPSR into pipeline - fetch
 620   jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded
 621   jtagarm7tdmi_nop( 0); // push nop into pipeline - execute
 622   retval = jtagarm7tdmi_get_register(0);
 623   jtagarm7tdmi_set_register(0, r0);
 624   return retval;
 625 }
 626
 627 //! Retrieve the CPSR Register value
 628 unsigned long jtagarm7tdmi_set_regCPSR(unsigned long val) {
 629   unsigned long r0;
 630
 631   r0 = jtagarm7tdmi_get_register(0);
 632   jtagarm7tdmi_set_register(0, val);
 633   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - clean out the pipeline...
 634   debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_MSR_cpsr_cxsf_R0, 0)); // push MSR cpsr_cxsf, R0 into pipeline - fetch
 635   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - decoded
 636   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - execute
 637   jtagarm7tdmi_set_register(0, r0);
 638   return(val);
 639 }
 640
 641 unsigned long wait_debug(unsigned long retval){
 642   // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
 643   current_dbgstate = jtagarm7tdmi_get_dbgstate();
 644   while ((!(current_dbgstate & 9L) == 9)  && retval > 0){
 645     delay(1);
 646     retval --;
 647     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 648   }
 649   return retval;
 650 }
 651
 652 /****
 653 //! Write data to address - Assume TAP in run-test/idle state
 654 unsigned long jtagarm7tdmi_writemem(unsigned long adr, unsigned long data){
 655   unsigned long retval = 0xffL;
 656   unsigned long r0=0L, r1=-1L;
 657
 658   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
 659   r1 = jtagarm7tdmi_get_register(1);
 660   jtagarm7tdmi_set_register(0, adr);        // write address into R0
 661   jtagarm7tdmi_set_register(1, data);       // write data in R1
 662   debughex32(jtagarm7tdmi_get_register(0));
 663   debughex32(jtagarm7tdmi_get_register(1));
 664   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
 665   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
 666   jtagarm7tdmi_instr_primitive(ARM_INSTR_STR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline
 667   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
 668   jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
 669
 670   if (wait_debug(0xffL) == 0){
 671     debugstr("FAILED TO WRITE MEMORY/RE-ENTER DEBUG MODE");
 672     return (-1);
 673   } else {
 674     retval = jtagarm7tdmi_get_register(1);  // read memory value from R1 register
 675     jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1
 676     jtagarm7tdmi_set_register(0, r0);
 677   }
 678   return retval;
 679 }
 680
 681
 682
 683 //! Read data from address
 684 unsigned long jtagarm7tdmi_readmem(unsigned long adr){
 685   unsigned long retval = 0xffL;
 686   unsigned long r0=0L, r1=-1L;
 687
 688   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
 689   r1 = jtagarm7tdmi_get_register(1);
 690   jtagarm7tdmi_set_register(0, adr);        // write address into R0
 691   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
 692   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
 693   jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, [R0], #4 into instruction pipeline  (autoincrements for consecutive reads)
 694   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
 695   jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
 696
 697   // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
 698   current_dbgstate = jtagarm7tdmi_get_dbgstate();
 699   debughex(current_dbgstate);
 700   while ((!(current_dbgstate & 9L) == 9)  && retval > 0){
 701     delay(1);
 702     retval --;
 703     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 704   }
 705   // FIXME: this may end up changing te current debug-state.  should we compare to current_dbgstate?
 706   if (retval == 0){
 707     debugstr("FAILED TO READ MEMORY/RE-ENTER DEBUG MODE");
 708     return (-1);
 709   } else {
 710     retval = jtagarm7tdmi_get_register(1);  // read memory value from R1 register
 711     //jtagarm7tdmi_set_register(1, r1);       // restore R0 and R1
 712     //jtagarm7tdmi_set_register(0, r0);
 713   }
 714   return retval;
 715 }
 716
 717 */
 718
 719
 720 //! Read Program Counter
 721 unsigned long jtagarm7tdmi_get_real_pc(){
 722     unsigned long val;
 723     val = jtagarm7tdmi_get_register(ARM_REG_PC);
 724     if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
 725         val -= (4*2);                           // thumb uses 2 bytes per instruction.
 726     else
 727         val -= (6*4);                           // assume 6 instructions at 4 bytes a piece.
 728     return val;
 729 }
 730
 731 //! Halt CPU - returns 0xffff if the operation fails to complete within
 732 unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
 733   int waitcount = 0xffL;
 734
 735   // store the debug state
 736   last_halt_debug_state = jtagarm7tdmi_get_dbgstate();
 737
 738   //jtagarm7tdmi_set_dbgctrl(7);
 739   // store watchpoint info?  - not right now
 740   jtagarm7tdmi_set_watchpoint1(0, 0xffffffff, 0, 0xffffffff, 0x100L, 0xfffffff7);
 741
 742
 743   /*  // old method
 744   eice_write(EICE_WP1ADDR, 0L);              // write 0 in watchpoint 1 address
 745   eice_write(EICE_WP1ADDRMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 address mask
 746   eice_write(EICE_WP1DATA, 0L);              // write 0 in watchpoint 1 data
 747   eice_write(EICE_WP1DATAMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 data mask
 748   eice_write(EICE_WP1CTRL, 0x100L);          // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
 749   eice_write(EICE_WP1CTRLMASK, 0xfffffff7); // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
 750   */
 751
 752   // poll until debug status says the cpu is in debug mode
 753   while (!(current_dbgstate & 0x1L)   && waitcount-- > 0){
 754     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 755     delay(1);
 756   }
 757
 758   //jtagarm7tdmi_set_dbgctrl(0);
 759   jtagarm7tdmi_set_watchpoint1(0, 0x0, 0, 0x0, 0x0L, 0xfffffff7);
 760   //jtagarm7tdmi_disable_watchpoint1();
 761
 762   //eice_write(EICE_WP1CTRL, 0x0L);            // write 0 in watchpoint 0 control value - disables watchpoint 0
 763
 764   // store the debug state program counter.
 765   last_halt_pc = jtagarm7tdmi_get_real_pc();
 766   count_dbgspd_instr_since_debug = 0L;          // should be able to clean this up and remove all this tracking nonsense.
 767   count_sysspd_instr_since_debug = 0L;          // should be able to clean this up and remove all this tracking nonsense.
 768
 769   //FIXME: is this necessary?  for now, yes... but perhaps make the rest of the module arm/thumb impervious.
 770   // get into ARM mode if the T flag is set (Thumb mode)
 771   while (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT && waitcount-- > 0) {
 772     jtagarm7tdmi_setMode_ARM(0);
 773     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 774   }
 775   jtagarm7tdmi_resettap();
 776   jtagarm7tdmi_set_register(ARM_REG_PC, last_halt_pc & 0xfffffffc);     // make sure PC is word-aligned.  otherwise all other register accesses get all wonky.
 777   return waitcount;
 778 }
 779
 780 unsigned long jtagarm7tdmi_releasecpu(){
 781   int waitcount = 0xff;
 782   jtagarm7tdmi_nop(0);                          // NOP
 783   jtagarm7tdmi_nop(1);                          // NOP/BREAKPT
 784
 785
 786   // four possible states.  arm mode needing arm mode, arm mode needing thumb mode, thumb mode needing arm mode, and thumb mode needing thumb mode
 787   // FIXME:  BX is bs.  it requires the clobbering of at least one register.... this is not acceptable.
 788   // FIXME:  so we either switch modes, then correct the register before restarting with bx, or find the way to use SPSR
 789   if (last_halt_debug_state & JTAG_ARM7TDMI_DBG_TBIT){
 790     // need to get to thumb mode
 791     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
 792     jtagarm7tdmi_setMode_THUMB(1);
 793   } else {
 794     jtagarm7tdmi_setMode_ARM(1);
 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   }
 797
 798
 799   jtagarm7tdmi_restart();
 800   jtagarm7tdmi_resettap();
 801   //jtag_goto_shift_ir();
 802   //jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); // VERB_RESTART
 803
 804   // wait until restart-bit set in debug state register
 805   while ((current_dbgstate & JTAG_ARM7TDMI_DBG_DBGACK) && waitcount > -1){
 806     msdelay(1);
 807     waitcount --;
 808     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 809   }
 810   last_halt_debug_state = -1;
 811   last_halt_pc = -1;
 812   return waitcount;
 813 }
 814
 815
 816
 817
 818 ///////////////////////////////////////////////////////////////////////////////////////////////////
 819 //! Handles ARM7TDMI JTAG commands.  Forwards others to JTAG.
 820 void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len){
 821   //register char blocks;
 822
 823   unsigned int val; //, i;
 824   //unsigned long at;
 825
 826   //jtagarm7tdmi_resettap();
 827   //current_dbgstate = jtagarm7tdmi_get_dbgstate();
 828
 829   switch(verb){
 830   case START:
 831     //Enter JTAG mode.
 832     debughex32(jtagarm7tdmi_start());
 833     cmddatalong[0] = jtagarm7tdmi_get_dbgstate();
 834     txdata(app,verb,0x4);
 835     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 836     break;
 837     /*
 838   case JTAGARM7TDMI_READMEM:
 839     at     = cmddatalong[0];
 840     blocks = cmddatalong[1];
 841
 842     txhead(app,verb,len);
 843
 844         jtagarm7tdmi_resettap();
 845         delay(1);
 846
 847     for(i=0;i<blocks;i++){
 848           val=jtagarm7tdmi_readmem(at);
 849
 850           serial_tx(val&0xFFL);
 851           serial_tx((val&0xFF00L)>>8);
 852           serial_tx((val&0xFF0000L)>>8);
 853           serial_tx((val&0xFF000000L)>>8);
 854           at+=4;
 855       }
 856
 857
 858     break;
 859   case PEEK:
 860         jtagarm7tdmi_resettap();
 861         delay(1);
 862         cmddatalong[0] = jtagarm7tdmi_readmem(cmddatalong[0]);
 863     txdata(app,verb,4);
 864     break;
 865     */
 866   case JTAGARM7TDMI_GET_CHIP_ID:
 867         jtagarm7tdmi_resettap();
 868     cmddatalong[0] = jtagarm7tdmi_idcode();
 869     txdata(app,verb,4);
 870     break;
 871
 872 /*
 873   case JTAGARM7TDMI_WRITEMEM:
 874   case POKE:
 875         jtagarm7tdmi_resettap();
 876     jtagarm7tdmi_writemem(cmddatalong[0],
 877                        cmddataword[2]);
 878     cmddataword[0]=jtagarm7tdmi_readmem(cmddatalong[0]);
 879     txdata(app,verb,4);
 880     break;
 881 */
 882   case JTAGARM7TDMI_HALTCPU:
 883     cmddatalong[0] = jtagarm7tdmi_haltcpu();
 884     txdata(app,verb,4);
 885     break;
 886   case JTAGARM7TDMI_RELEASECPU:
 887         //jtagarm7tdmi_resettap();
 888     cmddatalong[0] = jtagarm7tdmi_releasecpu();
 889     txdata(app,verb,4);
 890     break;
 891   //unimplemented functions
 892   //case JTAGARM7TDMI_SETINSTRFETCH:
 893   //case JTAGARM7TDMI_WRITEFLASH:
 894   //case JTAGARM7TDMI_ERASEFLASH:
 895   case JTAGARM7TDMI_SET_PC:
 896     //jtagarm7tdmi_setpc(cmddatalong[0]);
 897     last_halt_pc = cmddatalong[0];
 898     txdata(app,verb,0);
 899     break;
 900   case JTAGARM7TDMI_GET_DEBUG_CTRL:
 901     cmddatalong[0] = jtagarm7tdmi_get_dbgctrl();
 902     txdata(app,verb,1);
 903     break;
 904   case JTAGARM7TDMI_SET_DEBUG_CTRL:
 905     cmddatalong[0] = jtagarm7tdmi_set_dbgctrl(cmddata[0]);
 906     txdata(app,verb,4);
 907     break;
 908   case JTAGARM7TDMI_GET_PC:
 909     cmddatalong[0] = last_halt_pc;
 910     txdata(app,verb,4);
 911     break;
 912   case JTAGARM7TDMI_GET_DEBUG_STATE:
 913     //jtagarm7tdmi_resettap();            // Shouldn't need this, but currently do.  FIXME!
 914     current_dbgstate = jtagarm7tdmi_get_dbgstate();
 915     cmddatalong[0] = current_dbgstate;
 916     txdata(app,verb,4);
 917     break;
 918   //case JTAGARM7TDMI_GET_WATCHPOINT:
 919   //case JTAGARM7TDMI_SET_WATCHPOINT:
 920   case JTAGARM7TDMI_GET_REGISTER:
 921         //jtagarm7tdmi_resettap();
 922     val = cmddata[0];
 923     cmddatalong[0] = jtagarm7tdmi_get_register(val);
 924     txdata(app,verb,4);
 925     break;
 926   case JTAGARM7TDMI_SET_REGISTER:
 927         //jtagarm7tdmi_resettap();
 928     jtagarm7tdmi_set_register(cmddatalong[1], cmddatalong[0]);
 929     txdata(app,verb,4);
 930     break;
 931   case JTAGARM7TDMI_DEBUG_INSTR:
 932         //jtagarm7tdmi_resettap();
 933     //cmddataword[0] = jtagarm7tdmi_exec(cmddataword[0], cmddata[4]);
 934     cmddatalong[0] = jtagarm7tdmi_instr_primitive(cmddatalong[0],cmddata[4]);
 935     txdata(app,verb,8);
 936     break;
 937   //case JTAGARM7TDMI_STEP_INSTR:
 938 /*  case JTAGARM7TDMI_READ_CODE_MEMORY:
 939   case JTAGARM7TDMI_WRITE_FLASH_PAGE:
 940   case JTAGARM7TDMI_READ_FLASH_PAGE:
 941   case JTAGARM7TDMI_MASS_ERASE_FLASH:
 942   case JTAGARM7TDMI_PROGRAM_FLASH:
 943   case JTAGARM7TDMI_LOCKCHIP:
 944   case JTAGARM7TDMI_CHIP_ERASE:
 945   */
 946 // Really ARM specific stuff
 947   case JTAGARM7TDMI_GET_CPSR:
 948         jtagarm7tdmi_resettap();
 949     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
 950     txdata(app,verb,4);
 951     break;
 952   case JTAGARM7TDMI_SET_CPSR:
 953         jtagarm7tdmi_resettap();
 954     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
 955     txdata(app,verb,4);
 956     break;
 957   case JTAGARM7TDMI_GET_SPSR:           // FIXME: NOT EVEN CLOSE TO CORRECT
 958         jtagarm7tdmi_resettap();
 959     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
 960     txdata(app,verb,4);
 961     break;
 962   case JTAGARM7TDMI_SET_SPSR:           // FIXME: NOT EVEN CLOSE TO CORRECT
 963         jtagarm7tdmi_resettap();
 964     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
 965     txdata(app,verb,4);
 966     break;
 967   case JTAGARM7TDMI_SET_MODE_THUMB:
 968         jtagarm7tdmi_resettap();
 969     cmddatalong[0] = jtagarm7tdmi_setMode_THUMB(cmddata[0]);
 970     txdata(app,verb,4);
 971     break;
 972   case JTAGARM7TDMI_SET_MODE_ARM:
 973         jtagarm7tdmi_resettap();
 974     cmddatalong[0] = jtagarm7tdmi_setMode_ARM(cmddata[0]);
 975     txdata(app,verb,4);
 976     break;
 977   case JTAGARM7TDMI_SET_IR:
 978         //jtagarm7tdmi_resettap();
 979     jtag_goto_shift_ir();
 980     cmddataword[0] = jtagarmtransn(cmddata[0], 4, LSB, END, cmddata[1]);
 981     current_chain = -1;
 982     txdata(app,verb,2);
 983     break;
 984   case JTAGARM7TDMI_WAIT_DBG:
 985     cmddatalong[0] = wait_debug(cmddatalong[0]);
 986     txdata(app,verb,4);
 987     break;
 988   case JTAGARM7TDMI_SHIFT_DR:
 989         jtagarm7tdmi_resettap();
 990     jtag_goto_shift_dr();
 991     cmddatalong[0] = jtagarmtransn(cmddatalong[1],cmddata[0],cmddata[1],cmddata[2],cmddata[3]);
 992     txdata(app,verb,4);
 993     break;
 994   case JTAGARM7TDMI_SETWATCH0:
 995     jtagarm7tdmi_set_watchpoint0(cmddatalong[0], cmddatalong[1], cmddatalong[2], cmddatalong[3], cmddatalong[4], cmddatalong[5]);
 996     txdata(app,verb,4);
 997     break;
 998   case JTAGARM7TDMI_SETWATCH1:
 999     jtagarm7tdmi_set_watchpoint0(cmddatalong[0], cmddatalong[1], cmddatalong[2], cmddatalong[3], cmddatalong[4], cmddatalong[5]);
1000     txdata(app,verb,4);
1001     break;
1002   default:
1003     jtaghandle(app,verb,len);
1004   }
1005 }
1006
1007
1008
1009
1010 /*****************************
1011 Captured from FlySwatter against AT91SAM7S, to be used by me for testing.  ignore
1012
1013 > arm reg
1014 System and User mode registers
1015       r0: 300000df       r1: 00000000       r2: 58000000       r3: 00200a75
1016       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1017       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1018      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 000000fc
1019     cpsr: 00000093
1020
1021 FIQ mode shadow registers
1022   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1023  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1024
1025 Supervisor mode shadow registers
1026   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1027
1028 Abort mode shadow registers
1029   sp_abt: 00000000   lr_abt: 00000000 spsr_abt: e00000ff
1030
1031 IRQ mode shadow registers
1032   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1033
1034 Undefined instruction mode shadow registers
1035   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1036 >
1037 > step;arm reg
1038 target state: halted
1039 target halted in ARM state due to single-step, current mode: Supervisor
1040 cpsr: 0x00000093 pc: 0x00000100
1041 System and User mode registers
1042       r0: 300000df       r1: 00000000       r2: 00200000       r3: 00200a75
1043       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1044       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1045      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000100
1046     cpsr: 00000093
1047
1048 FIQ mode shadow registers
1049   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1050  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1051
1052 Supervisor mode shadow registers
1053   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1054
1055 Abort mode shadow registers
1056   sp_abt: 00000000   lr_abt: 00000000 spsr_abt: e00000ff
1057
1058 IRQ mode shadow registers
1059   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1060
1061 Undefined instruction mode shadow registers
1062   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1063 >
1064  step;arm reg
1065 target state: halted
1066 target halted in ARM state due to single-step, current mode: Abort
1067 cpsr: 0x00000097 pc: 0x00000010
1068 System and User mode registers
1069       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1070       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1071       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1072      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1073     cpsr: 00000097
1074
1075 FIQ mode shadow registers
1076   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1077  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1078
1079 Supervisor mode shadow registers
1080   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1081
1082 Abort mode shadow registers
1083   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1084
1085 IRQ mode shadow registers
1086   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1087
1088 Undefined instruction mode shadow registers
1089   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1090 > step;arm reg
1091 target state: halted
1092 target halted in ARM state due to single-step, current mode: Abort
1093 cpsr: 0x00000097 pc: 0x00000010
1094 System and User mode registers
1095       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1096       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1097       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1098      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1099     cpsr: 00000097
1100
1101 FIQ mode shadow registers
1102   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1103  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1104
1105 Supervisor mode shadow registers
1106   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1107
1108 Abort mode shadow registers
1109   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1110
1111 IRQ mode shadow registers
1112   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1113
1114 Undefined instruction mode shadow registers
1115   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1116 > step;arm reg
1117 target state: halted
1118 target halted in ARM state due to single-step, current mode: Abort
1119 cpsr: 0x00000097 pc: 0x00000010
1120 System and User mode registers
1121       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1122       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1123       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1124      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1125     cpsr: 00000097
1126
1127 FIQ mode shadow registers
1128   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1129  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1130
1131 Supervisor mode shadow registers
1132   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1133
1134 Abort mode shadow registers
1135   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1136
1137 IRQ mode shadow registers
1138   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1139
1140 Undefined instruction mode shadow registers
1141   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1142 > step;arm reg
1143 target state: halted
1144 target halted in ARM state due to single-step, current mode: Abort
1145 cpsr: 0x00000097 pc: 0x00000010
1146 System and User mode registers
1147       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1148       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1149       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1150      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1151     cpsr: 00000097
1152
1153 FIQ mode shadow registers
1154   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1155  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1156
1157 Supervisor mode shadow registers
1158   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1159
1160 Abort mode shadow registers
1161   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1162
1163 IRQ mode shadow registers
1164   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1165
1166 Undefined instruction mode shadow registers
1167   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1168 > step;arm reg
1169 target state: halted
1170 target halted in ARM state due to single-step, current mode: Abort
1171 cpsr: 0x00000097 pc: 0x00000010
1172 System and User mode registers
1173       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1174       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1175       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1176      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1177     cpsr: 00000097
1178
1179 FIQ mode shadow registers
1180   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1181  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1182
1183 Supervisor mode shadow registers
1184   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1185
1186 Abort mode shadow registers
1187   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1188
1189 IRQ mode shadow registers
1190   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1191
1192 Undefined instruction mode shadow registers
1193   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1194 > step;arm reg
1195 target state: halted
1196 target halted in ARM state due to single-step, current mode: Abort
1197 cpsr: 0x00000097 pc: 0x00000010
1198 System and User mode registers
1199       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1200       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1201       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1202      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1203     cpsr: 00000097
1204
1205 FIQ mode shadow registers
1206   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1207  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1208
1209 Supervisor mode shadow registers
1210   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1211
1212 Abort mode shadow registers
1213   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1214
1215 IRQ mode shadow registers
1216   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1217
1218 Undefined instruction mode shadow registers
1219   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1220 > step;arm reg
1221 target state: halted
1222 target halted in ARM state due to single-step, current mode: Abort
1223 cpsr: 0x00000097 pc: 0x00000010
1224 System and User mode registers
1225       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1226       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1227       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1228      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1229     cpsr: 00000097
1230
1231 FIQ mode shadow registers
1232   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1233  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1234
1235 Supervisor mode shadow registers
1236   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1237
1238 Abort mode shadow registers
1239   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1240
1241 IRQ mode shadow registers
1242   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1243
1244 Undefined instruction mode shadow registers
1245   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1246 > step;arm reg
1247 target state: halted
1248 target halted in ARM state due to single-step, current mode: Abort
1249 cpsr: 0x00000097 pc: 0x00000010
1250 System and User mode registers
1251       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1252       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1253       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1254      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1255     cpsr: 00000097
1256
1257 FIQ mode shadow registers
1258   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1259  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1260
1261 Supervisor mode shadow registers
1262   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1263
1264 Abort mode shadow registers
1265   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1266
1267 IRQ mode shadow registers
1268   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1269
1270 Undefined instruction mode shadow registers
1271   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1272 > step;arm reg
1273 target state: halted
1274 target halted in ARM state due to single-step, current mode: Abort
1275 cpsr: 0x00000097 pc: 0x00000010
1276 System and User mode registers
1277       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1278       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1279       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1280      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1281     cpsr: 00000097
1282
1283 FIQ mode shadow registers
1284   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1285  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1286
1287 Supervisor mode shadow registers
1288   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1289
1290 Abort mode shadow registers
1291   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1292
1293 IRQ mode shadow registers
1294   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1295
1296 Undefined instruction mode shadow registers
1297   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1298 >
1299 */