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