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