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