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

   1 /*! \file jtagarm7tdmi.c
   2   \brief ARM7TDMI JTAG (AT91R40008)
   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
  88
  89 /************************** JTAGARM7TDMI Primitives ****************************/
  90 void jtag_goto_shift_ir() {
  91   SETTMS;
  92   jtag_arm_tcktock();
  93   jtag_arm_tcktock();
  94   CLRTMS;
  95   jtag_arm_tcktock();
  96   jtag_arm_tcktock();
  97
  98 }
  99
 100 void jtag_goto_shift_dr() {
 101   SETTMS;
 102   jtag_arm_tcktock();
 103   CLRTMS;
 104   jtag_arm_tcktock();
 105   jtag_arm_tcktock();
 106 }
 107
 108 void jtag_reset_to_runtest_idle() {
 109   SETTMS;
 110   jtag_arm_tcktock();
 111   jtag_arm_tcktock();
 112   jtag_arm_tcktock();
 113   jtag_arm_tcktock();
 114   jtag_arm_tcktock();
 115   jtag_arm_tcktock();
 116   jtag_arm_tcktock();
 117   jtag_arm_tcktock();  // now in Reset state
 118   CLRTMS;
 119   jtag_arm_tcktock();  // now in Run-Test/Idle state
 120 }
 121
 122 void jtag_arm_tcktock() {
 123   CLRTCK;
 124   PLEDOUT^=PLEDPIN;
 125   SETTCK;
 126   PLEDOUT^=PLEDPIN;
 127 }
 128
 129 //! Set up the pins for JTAG mode.
 130 void armjtagsetup(){
 131   P5DIR|=MOSI+SCK+TMS;
 132   P5DIR&=~MISO;
 133   P5OUT|=0xFFFF;
 134   P5OUT=0;
 135   P4DIR|=TST;
 136   P2DIR|=RST;
 137   msdelay(10);
 138 }
 139
 140
 141 // ! Start JTAG, setup pins, reset TAP and return IDCODE
 142 unsigned long jtagarm7tdmi_start() {
 143   armjtagsetup();
 144   //Known-good starting position.
 145   //Might be unnecessary.
 146   SETTST;
 147   SETRST;
 148
 149   delay(0x2);
 150
 151   CLRRST;
 152   delay(2);
 153   CLRTST;
 154
 155   msdelay(10);
 156   SETRST;
 157   /*
 158   P5DIR &=~RST;
 159   */
 160   delay(0x2);
 161   jtagarm7tdmi_resettap();
 162   return jtagarm7tdmi_idcode();
 163 }
 164
 165
 166 //! Reset TAP State Machine
 167 void jtagarm7tdmi_resettap(){               // PROVEN
 168   current_chain = -1;
 169   jtag_reset_to_runtest_idle();
 170 }
 171
 172
 173 //  NOTE: important: THIS MODULE REVOLVES AROUND RETURNING TO RUNTEST/IDLE, OR THE FUNCTIONAL EQUIVALENT
 174
 175
 176 //! 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
 177 unsigned long jtagarmtransn(unsigned long word, unsigned char bitcount, unsigned char lsb, unsigned char end, unsigned char retidle){               // PROVEN
 178   unsigned int bit;
 179   unsigned long high = 1;
 180   unsigned long mask;
 181
 182   for (bit=(bitcount-1)/8; bit>0; bit--)
 183     high <<= 8;
 184   high <<= ((bitcount-1)%8);
 185
 186   mask = high-1;
 187
 188   if (lsb) {
 189     for (bit = bitcount; bit > 0; bit--) {
 190       /* write MOSI on trailing edge of previous clock */
 191       if (word & 1)
 192         {SETMOSI;}
 193       else
 194         {CLRMOSI;}
 195       word >>= 1;
 196
 197       if (bit==1 && end)
 198         SETTMS;//TMS high on last bit to exit.
 199
 200       jtag_arm_tcktock();
 201
 202       /* read MISO on trailing edge */
 203       if (READMISO){
 204         word += (high);
 205       }
 206     }
 207   } else {
 208     for (bit = bitcount; bit > 0; bit--) {
 209       /* write MOSI on trailing edge of previous clock */
 210       if (word & high)
 211         {SETMOSI;}
 212       else
 213         {CLRMOSI;}
 214       word = (word & mask) << 1;
 215
 216       if (bit==1 && end)
 217         SETTMS;//TMS high on last bit to exit.
 218
 219       jtag_arm_tcktock();
 220
 221       /* read MISO on trailing edge */
 222       word |= (READMISO);
 223     }
 224   }
 225
 226
 227   SETMOSI;
 228
 229   if (end){
 230     // exit state
 231     jtag_arm_tcktock();
 232     // update state
 233     if (retidle){
 234       CLRTMS;
 235       jtag_arm_tcktock();
 236     }
 237   }
 238   return word;
 239 }
 240
 241
 242
 243 /************************************************************************
 244 * ARM7TDMI core has 6 primary registers to be connected between TDI/TDO
 245 *   * Bypass Register
 246 *   * ID Code Register
 247 *   * Scan Chain Select Register    (4 bits_lsb)
 248 *   * Scan Chain 0                  (64+* bits: 32_databits_lsb + ctrlbits + 32_addrbits_msb)
 249 *   * Scan Chain 1                  (33 bits: 32_bits + BREAKPT)
 250 *   * Scan Chain 2                  (38 bits: rw + 5_regbits_msb + 32_databits_msb)
 251 ************************************************************************/
 252
 253
 254
 255 /************************** Basic JTAG Verb Commands *******************************/
 256 //! Grab the core ID.
 257 unsigned long jtagarm7tdmi_idcode(){               // PROVEN
 258   jtagarm7tdmi_resettap();
 259   SHIFT_IR;
 260   jtagarmtransn(ARM7TDMI_IR_IDCODE, 4, LSB, END, RETIDLE);
 261   SHIFT_DR;
 262   return jtagarmtransn(0,32, LSB, END, RETIDLE);
 263 }
 264
 265 //!  Connect Bypass Register to TDO/TDI
 266 unsigned char jtagarm7tdmi_bypass(){               // PROVEN
 267   jtagarm7tdmi_resettap();
 268   SHIFT_IR;
 269   return jtagarmtransn(ARM7TDMI_IR_BYPASS, 4, LSB, END, NORETIDLE);
 270 }
 271 //!  INTEST verb - do internal test
 272 unsigned char jtagarm7tdmi_intest() {
 273   jtagarm7tdmi_resettap();
 274   SHIFT_IR;
 275   return jtagarmtransn(ARM7TDMI_IR_INTEST, 4, LSB, END, NORETIDLE);
 276 }
 277
 278 //!  EXTEST verb
 279 unsigned char jtagarm7tdmi_extest() {
 280   jtagarm7tdmi_resettap();
 281   SHIFT_IR;
 282   return jtagarmtransn(ARM7TDMI_IR_EXTEST, 4, LSB, END, NORETIDLE);
 283 }
 284
 285 //!  SAMPLE verb
 286 //unsigned long jtagarm7tdmi_sample() {
 287 //  jtagarm7tdmi_ir_shift4(ARM7TDMI_IR_SAMPLE);        // ???? same here.
 288 //  return jtagtransn(0,32);
 289 //}
 290
 291 //!  RESTART verb
 292 unsigned char jtagarm7tdmi_restart() {
 293   jtagarm7tdmi_resettap();
 294   SHIFT_IR;
 295   return jtagarmtransn(ARM7TDMI_IR_RESTART, 4, LSB, END, RETIDLE);
 296 }
 297
 298 //!  ARM7TDMI_IR_CLAMP               0x5
 299 //unsigned long jtagarm7tdmi_clamp() {
 300 //  jtagarm7tdmi_resettap();
 301 //  SHIFT_IR;
 302 //  jtagarmtransn(ARM7TDMI_IR_CLAMP, 4, LSB, END, NORETIDLE);
 303 //  SHIFT_DR;
 304 //  return jtagarmtransn(0, 32, LSB, END, RETIDLE);
 305 //}
 306
 307 //!  ARM7TDMI_IR_HIGHZ               0x7
 308 //unsigned char jtagarm7tdmi_highz() {
 309 //  jtagarm7tdmi_resettap();
 310 //  SHIFT_IR;
 311 //  return jtagarmtransn(ARM7TDMI_IR_HIGHZ, 4, LSB, END, NORETIDLE);
 312 //}
 313
 314 //! define ARM7TDMI_IR_CLAMPZ              0x9
 315 //unsigned char jtagarm7tdmi_clampz() {
 316 //  jtagarm7tdmi_resettap();
 317 //  SHIFT_IR;
 318 //  return jtagarmtransn(ARM7TDMI_IR_CLAMPZ, 4, LSB, END, NORETIDLE);
 319 //}
 320
 321
 322 //!  Connect the appropriate scan chain to TDO/TDI.  SCAN_N, INTEST, ENDS IN SHIFT_DR!!!!!
 323 unsigned long jtagarm7tdmi_scan(int chain, int testmode) {               // PROVEN
 324 /*
 325 When selecting a scan chain the “Run Test/Idle” state should never be reached, other-
 326 wise, when in debug state, the core will not be correctly isolated and intrusive
 327 commands occur. Therefore, it is recommended to pass directly from the “Update”
 328 state” to the “Select DR” state each time the “Update” state is reached.
 329 */
 330   unsigned long retval;
 331   if (current_chain != chain) {     // breaks shit when going from idcode back to scan chain
 332     SHIFT_IR;
 333     jtagarmtransn(ARM7TDMI_IR_SCAN_N, 4, LSB, END, NORETIDLE);
 334     SHIFT_DR;
 335     retval = jtagarmtransn(chain, 4, LSB, END, NORETIDLE);
 336     current_chain = chain;
 337   }    else
 338     retval = current_chain;
 339   // put in test mode...
 340   SHIFT_IR;
 341   jtagarmtransn(testmode, 4, LSB, END, RETIDLE);
 342   return(retval);
 343 }
 344
 345
 346 //!  Connect the appropriate scan chain to TDO/TDI.  SCAN_N, INTEST, ENDS IN SHIFT_DR!!!!!
 347 unsigned long jtagarm7tdmi_scan_intest(int chain) {               // PROVEN
 348   return jtagarm7tdmi_scan(chain, ARM7TDMI_IR_INTEST);
 349 }
 350
 351
 352
 353
 354 //! push an instruction into the pipeline  - Assumes scan-chain 1 is already INTEST
 355 unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){
 356   unsigned long retval;
 357   //jtagarm7tdmi_resettap();                  // FIXME: DEBUG: seems necessary for some reason.  ugh.
 358   jtagarm7tdmi_scan_intest(1);
 359
 360   SHIFT_DR;
 361   // if the next instruction is to run using MCLK (master clock), set TDI
 362   if (breakpt)
 363     {
 364     SETMOSI;
 365     count_sysspd_instr_since_debug++;
 366     }
 367   else
 368     {
 369     CLRMOSI;
 370     count_dbgspd_instr_since_debug++;
 371     }
 372   jtag_arm_tcktock();
 373
 374   // Now shift in the 32 bits
 375   retval = jtagarmtransn(instr, 32, MSB, END, RETIDLE);    // Must return to RUN-TEST/IDLE state for instruction to enter pipeline, and causes debug clock.
 376   //jtag_arm_tcktock();
 377   return(retval);
 378
 379 }
 380
 381
 382 unsigned long jtagarm7tdmi_nop(char breakpt){
 383   //jtagarm7tdmi_scan_intest(1);
 384   //SHIFT_DR
 385   //return jtagarmtransn(ARM_INSTR_NOP, 32, LSB, END, NORETIDLE);
 386   return jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP, breakpt);
 387 }
 388
 389 /*    stolen from ARM DDI0029G documentation, translated using ARM Architecture Reference Manual (14128.pdf)
 390 STR R0, [R0]; Save R0 before use
 391 MOV R0, PC ; Copy PC into R0
 392 STR R0, [R0]; Now save the PC in R0
 393 BX PC ; Jump into ARM state
 394 MOV R8, R8 ;
 395 MOV R8, R8 ;
 396 NOP
 397 NOP
 398
 399 */
 400 //! 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();
 401 unsigned long jtagarm7tdmi_setMode_ARM(){               // PROVEN
 402   unsigned long retval = 0xff;
 403   while ((jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
 404     cmddataword[6] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
 405     cmddataword[1] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
 406     cmddataword[2] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_MOV_R0_PC,0);
 407     cmddataword[3] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
 408     cmddataword[4] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_BX_PC,0);
 409     cmddataword[5] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
 410     cmddataword[6] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
 411     jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
 412   }
 413   return(retval);
 414 }
 415
 416
 417
 418
 419 /************************* EmbeddedICE Primitives ****************************/
 420 //! shifter for writing to chain2 (EmbeddedICE).
 421 unsigned long eice_write(unsigned char reg, unsigned long data){
 422   unsigned long retval, temp;
 423   jtagarm7tdmi_scan_intest(2);
 424   // Now shift in the 32 bits
 425   SHIFT_DR;
 426   retval = jtagarmtransn(data, 32, LSB, NOEND, NORETIDLE);          // send in the data - 32-bits lsb
 427   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);              // send in the register address - 5 bits lsb
 428   jtagarmtransn(1, 1, LSB, END, RETIDLE);                           // send in the WRITE bit
 429
 430   //SETTMS;   // Last Bit - Exit UPDATE_DR
 431   //// is this update a read/write or just read?
 432   //SETMOSI;
 433   //jtag_arm_tcktock();
 434
 435   return(retval);
 436 }
 437
 438 //! shifter for reading from chain2 (EmbeddedICE).
 439 unsigned long eice_read(unsigned char reg){               // PROVEN
 440   unsigned long temp;
 441   jtagarm7tdmi_scan_intest(2);
 442
 443   // send in the register address - 5 bits LSB
 444   SHIFT_DR;
 445   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);
 446
 447   // clear TDI to select "read only"
 448   jtagarmtransn(0, 1, LSB, END, RETIDLE);
 449
 450   SHIFT_DR;
 451   // Now shift out the 32 bits
 452   return(jtagarmtransn(0, 32, LSB, END, RETIDLE));   // atmel arm jtag docs pp.10-11: LSB first
 453
 454 }
 455
 456
 457
 458
 459 /************************* ICEBreaker/EmbeddedICE Stuff ******************************/
 460 //! Grab debug register
 461 unsigned long jtagarm7tdmi_get_dbgstate() {       // ICE Debug register, *NOT* ARM register DSCR    //    PROVEN
 462   //jtagarm7tdmi_resettap();
 463   return eice_read(EICE_DBGSTATUS);
 464 }
 465
 466 //! Grab debug register
 467 unsigned long jtagarm7tdmi_get_dbgctrl() {
 468   return eice_read(EICE_DBGCTRL);
 469 }
 470
 471 //! Update debug register
 472 unsigned long jtagarm7tdmi_set_dbgctrl(unsigned long bits) {
 473   return eice_write(EICE_DBGCTRL, bits);
 474 }
 475
 476
 477
 478 //!  Set and Enable Watchpoint 0
 479 void jtagarm7tdmi_set_watchpoint0(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
 480   // store watchpoint info?  - not right now
 481     // FIXME: store info
 482
 483   eice_write(EICE_WP0ADDR, addr);           // write 0 in watchpoint 0 address
 484   eice_write(EICE_WP0ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 0 address mask
 485   eice_write(EICE_WP0DATA, data);           // write 0 in watchpoint 0 data
 486   eice_write(EICE_WP0DATAMASK, datamask);   // write 0xffffffff in watchpoint 0 data mask
 487   eice_write(EICE_WP0CTRL, ctrlmask);       // write 0x00000100 in watchpoint 0 control value register (enables watchpoint)
 488   eice_write(EICE_WP0CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 0 control mask - only detect the fetch instruction
 489 }
 490
 491 //!  Set and Enable Watchpoint 1
 492 void jtagarm7tdmi_set_watchpoint1(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
 493   // store watchpoint info?  - not right now
 494     // FIXME: store info
 495
 496   eice_write(EICE_WP1ADDR, addr);           // write 0 in watchpoint 1 address
 497   eice_write(EICE_WP1ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 1 address mask
 498   eice_write(EICE_WP1DATA, data);           // write 0 in watchpoint 1 data
 499   eice_write(EICE_WP1DATAMASK, datamask);   // write 0xffffffff in watchpoint 1 data mask
 500   eice_write(EICE_WP1CTRL, ctrl);           // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
 501   eice_write(EICE_WP1CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
 502 }
 503
 504 //!  Disable Watchpoint 0
 505 void jtagarm7tdmi_disable_watchpoint0(){
 506   eice_write(EICE_WP0CTRL, 0x0); // write 0 in watchpoint 0 control value - disables watchpoint 0
 507 }
 508
 509 //!  Disable Watchpoint 1
 510 void jtagarm7tdmi_disable_watchpoint1(){
 511   eice_write(EICE_WP1CTRL, 0x0);            // write 0 in watchpoint 0 control value - disables watchpoint 0
 512 }
 513
 514
 515
 516 /******************** Complex Commands **************************/
 517 //! Push an instruction into the CPU pipeline
 518 //  NOTE!  Must provide EXECNOPARM for parameter if no parm is required.
 519 unsigned long test_exec(unsigned long instr, unsigned long parameter, unsigned char systemspeed) {
 520   unsigned long retval;
 521
 522   cmddatalong[1] = jtagarm7tdmi_nop( 0);
 523   cmddatalong[2] = jtagarm7tdmi_nop(systemspeed);
 524   cmddatalong[3] = jtagarm7tdmi_instr_primitive(instr, 0);      // write 32-bit instruction code into DR
 525   cmddatalong[4] = jtagarm7tdmi_nop( 0);
 526   cmddatalong[5] = jtagarm7tdmi_nop( 0);
 527   cmddatalong[6] = jtagarm7tdmi_instr_primitive(parameter, 0);  // inject long
 528   cmddatalong[7] = jtagarm7tdmi_nop( 0);
 529   cmddatalong[8] = jtagarm7tdmi_nop( 0);
 530   cmddatalong[9] = jtagarm7tdmi_nop( 0);
 531   retval = cmddatalong[9];
 532
 533   return(retval);
 534 }
 535
 536
 537 //! Push an instruction into the CPU pipeline
 538 //  NOTE!  Must provide EXECNOPARM for parameter if no parm is required.
 539 unsigned long jtagarm7tdmi_exec(unsigned long instr, unsigned long parameter, unsigned char systemspeed) {
 540   unsigned long retval;
 541
 542   cmddatalong[1] = jtagarm7tdmi_nop( 0);
 543   cmddatalong[2] = jtagarm7tdmi_nop(systemspeed);
 544   cmddatalong[3] = jtagarm7tdmi_instr_primitive(instr, 0);      // write 32-bit instruction code into DR
 545   cmddatalong[4] = jtagarm7tdmi_nop( 0);
 546   cmddatalong[5] = jtagarm7tdmi_nop( 0);
 547   cmddatalong[6] = jtagarm7tdmi_instr_primitive(parameter, 0);  // inject long
 548   cmddatalong[7] = jtagarm7tdmi_nop( 0);
 549   retval = jtagarm7tdmi_nop( 0);
 550   cmddatalong[9] = jtagarm7tdmi_nop( 0);
 551   cmddatalong[8] = retval;
 552
 553   return(retval);
 554 }
 555
 556 //! Retrieve a 32-bit Register value
 557 unsigned long jtagarm7tdmi_get_register(unsigned char reg) {
 558   unsigned long retval = 0, instr;
 559   // push nop into pipeline - clean out the pipeline...
 560   cmddatalong[2] = jtagarm7tdmi_nop( 0);
 561
 562   instr = ARM_READ_REG | (reg<<12);                     // push STR Rx, [R14] into pipeline
 563   cmddatalong[1] = jtagarm7tdmi_instr_primitive(instr, 0);
 564   cmddatalong[2] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - fetched
 565   cmddatalong[3] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - decoded
 566   cmddatalong[4] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - executed
 567   retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
 568   cmddatalong[5] = retval;
 569   cmddatalong[6] = jtagarm7tdmi_nop( 0);
 570   cmddatalong[7] = jtagarm7tdmi_nop( 0);
 571   cmddatalong[8] = jtagarm7tdmi_nop( 0);
 572   return retval;
 573 }
 574
 575 //! Retrieve a 32-bit Register value
 576 unsigned long test_get_register(unsigned char reg) {
 577   unsigned long retval = 0, instr;
 578   // push nop into pipeline - clean out the pipeline...
 579   cmddatalong[2] = jtagarm7tdmi_nop( 0);
 580
 581   instr = ARM_WRITE_REG | (reg<<12);                     // push STR Rx, [R14] into pipeline
 582   cmddatalong[1] = jtagarm7tdmi_instr_primitive(instr, 0);
 583   cmddatalong[2] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - fetched
 584   cmddatalong[3] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - decoded
 585   cmddatalong[4] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - executed
 586   retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
 587   cmddatalong[5] = retval;
 588   cmddatalong[6] = jtagarm7tdmi_nop( 0);
 589   cmddatalong[7] = jtagarm7tdmi_nop( 0);
 590   cmddatalong[8] = jtagarm7tdmi_nop( 0);
 591   return retval;
 592 }
 593
 594 //! Set a 32-bit Register value
 595 unsigned long jtagarm7tdmi_set_register(unsigned char reg, unsigned long val) {
 596   unsigned long retval = 0, instr;
 597   cmddatalong[2] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
 598
 599   instr = ARM_WRITE_REG | (reg<<12);     // push LDR Rx, [R14] into pipeline
 600   cmddatalong[1] = jtagarm7tdmi_instr_primitive(instr, 0);
 601   cmddatalong[2] = jtagarm7tdmi_nop( 0); // push nop into pipeline - fetched
 602   cmddatalong[3] = jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded
 603
 604   cmddatalong[4] = jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus - execute state
 605   cmddatalong[5] = jtagarm7tdmi_nop( 0); // push nop into pipeline - executed
 606
 607   if (reg == ARM_REG_PC){
 608     cmddatalong[6] = jtagarm7tdmi_nop( 0);
 609     cmddatalong[7] = jtagarm7tdmi_nop( 0);
 610   }
 611   cmddatalong[8] = jtagarm7tdmi_nop( 0);
 612
 613   retval = cmddatalong[5];
 614   return(retval);
 615 }
 616
 617 //! Set a 32-bit Register value
 618 unsigned long test_set_register(unsigned char reg, unsigned long val) {
 619   unsigned long retval = 0, instr;
 620   cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
 621
 622   instr = ARM_READ_REG | (reg<<12);     // push LDR Rx, [R14] into pipeline
 623   cmddatalong[2] = jtagarm7tdmi_instr_primitive(instr, 0);
 624
 625   cmddatalong[3] = jtagarm7tdmi_instr_primitive(val+32, 0); // push 32-bit word on data bus - execute state
 626   cmddatalong[4] = jtagarm7tdmi_instr_primitive(val+16, 0); // push 32-bit word on data bus - execute state
 627   cmddatalong[5] = jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus - execute state
 628   cmddatalong[6] = jtagarm7tdmi_instr_primitive(val-16, 0); // push 32-bit word on data bus - execute state
 629
 630   if (reg == ARM_REG_PC){
 631     cmddatalong[7] = jtagarm7tdmi_nop( 0);
 632     cmddatalong[8] = jtagarm7tdmi_nop( 0);
 633   }
 634   cmddatalong[9] = jtagarm7tdmi_instr_primitive(val-32, 0); // push 32-bit word on data bus - execute state
 635
 636   retval = cmddatalong[5];
 637   return(retval);
 638 }
 639
 640
 641
 642 //! Get all registers.  Return an array
 643 unsigned long* jtagarm7tdmi_get_registers() {
 644   cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0);
 645   cmddatalong[2] = jtagarm7tdmi_nop( 0);
 646   cmddatalong[3] = jtagarm7tdmi_nop( 0);
 647   cmddatalong[4] = jtagarm7tdmi_nop( 0);
 648   cmddatalong[5] = jtagarm7tdmi_nop( 0);
 649   cmddatalong[6] = jtagarm7tdmi_nop( 0);
 650   cmddatalong[7] = jtagarm7tdmi_nop( 0);
 651   cmddatalong[8] = jtagarm7tdmi_nop( 0);
 652   cmddatalong[9] = jtagarm7tdmi_nop( 0);
 653   cmddatalong[10] = jtagarm7tdmi_nop( 0);
 654   cmddatalong[11] = jtagarm7tdmi_nop( 0);
 655   cmddatalong[12] = jtagarm7tdmi_nop( 0);
 656   cmddatalong[13] = jtagarm7tdmi_nop( 0);
 657   cmddatalong[14] = jtagarm7tdmi_nop( 0);
 658   cmddatalong[15] = jtagarm7tdmi_nop( 0);
 659   cmddatalong[16] = jtagarm7tdmi_nop( 0);
 660   cmddatalong[17] = jtagarm7tdmi_nop( 0);
 661   cmddatalong[18] = jtagarm7tdmi_nop( 0);
 662   cmddatalong[19] = jtagarm7tdmi_nop( 0);
 663   cmddatalong[20] = jtagarm7tdmi_nop( 0);
 664   return registers;
 665 }
 666
 667 //! Retrieve the CPSR Register value
 668 unsigned long jtagarm7tdmi_get_regCPSR() {
 669   unsigned long retval = 0;
 670
 671   cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
 672   cmddatalong[2] = jtagarm7tdmi_instr_primitive(ARM_INSTR_MRS_R0_CPSR, 0); // push MRS_R0, CPSR into pipeline
 673   cmddatalong[3] = jtagarm7tdmi_nop( 0); // push nop into pipeline - fetched
 674   cmddatalong[4] = jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded
 675   cmddatalong[5] = jtagarm7tdmi_nop( 0); // push nop into pipeline - executed
 676   retval = jtagarm7tdmi_nop( 0);        // recover 32-bit word
 677   cmddatalong[6] = retval;
 678   return retval;
 679 }
 680
 681 //! Retrieve the CPSR Register value
 682 unsigned long jtagarm7tdmi_set_regCPSR(unsigned long val) {
 683   unsigned long retval = 0;
 684
 685   cmddatalong[1] = jtagarm7tdmi_nop( 0);        // push nop into pipeline - clean out the pipeline...
 686   cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_MSR_cpsr_cxsf_R0, 0); // push MSR cpsr_cxsf, R0 into pipeline
 687   cmddatalong[2] = jtagarm7tdmi_nop( 0);        // push nop into pipeline - fetched
 688   cmddatalong[3] = jtagarm7tdmi_nop( 0);        // push nop into pipeline - decoded
 689
 690   retval = jtagarm7tdmi_instr_primitive(val, 0);// push 32-bit word on data bus
 691   cmddatalong[5] = jtagarm7tdmi_nop( 0);        // push nop into pipeline - executed
 692   cmddatalong[4] = retval;
 693   return(retval);
 694 }
 695
 696 //! Write data to address - Assume TAP in run-test/idle state
 697 unsigned long jtagarm7tdmi_writemem(unsigned long adr, unsigned long data){
 698   unsigned long r0=0, r1=-1;
 699
 700   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
 701   r1 = jtagarm7tdmi_get_register(1);
 702   jtagarm7tdmi_set_register(0, adr);        // write address into R0
 703   jtagarm7tdmi_set_register(1, data);       // write data in R1
 704   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
 705   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
 706   jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline
 707   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
 708   jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1
 709   jtagarm7tdmi_set_register(0, r0);
 710   return(-1);
 711 }
 712
 713
 714
 715
 716 //! Read data from address
 717 unsigned long jtagarm7tdmi_readmem(unsigned long adr){
 718   unsigned long retval = 0;
 719   unsigned long r0=0, r1=-1;
 720   int waitcount = 0xfff;
 721
 722   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
 723   r1 = jtagarm7tdmi_get_register(1);
 724   jtagarm7tdmi_set_register(0, adr);        // write address into R0
 725   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
 726   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
 727   jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline
 728   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
 729   jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
 730
 731   // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
 732   while ((jtagarm7tdmi_get_dbgstate() & 9) == 0  && waitcount > 0){
 733     delay(1);
 734     waitcount --;
 735   }
 736   if (waitcount == 0){
 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   return jtagarm7tdmi_get_register(ARM_REG_PC);
 750 }
 751
 752 //! Set Program Counter
 753 unsigned long jtagarm7tdmi_setpc(unsigned long adr){
 754   return jtagarm7tdmi_set_register(ARM_REG_PC, adr);
 755 }
 756
 757 //! Halt CPU - returns 0xffff if the operation fails to complete within
 758 unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
 759   int waitcount = 0xfff;
 760
 761   // store watchpoint info?  - not right now
 762   eice_write(EICE_WP1ADDR, 0);              // write 0 in watchpoint 1 address
 763   eice_write(EICE_WP1ADDRMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 address mask
 764   eice_write(EICE_WP1DATA, 0);              // write 0 in watchpoint 1 data
 765   eice_write(EICE_WP1DATAMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 data mask
 766   eice_write(EICE_WP1CTRL, 0x100);          //!!!!! WTF!  THIS IS SUPPOSED TO BE 9 bits wide?!?  // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
 767   eice_write(EICE_WP1CTRLMASK, 0xfffffff7); //!!!!! WTF!  THIS IS SUPPOSED TO BE 8 bits wide?!?  // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
 768
 769   // poll until debug status says the cpu is in debug mode
 770   while (!(jtagarm7tdmi_get_dbgstate() & 0x1)   && waitcount-- > 0){
 771     delay(1);
 772   }
 773   eice_write(EICE_WP1CTRL, 0x0);            // write 0 in watchpoint 0 control value - disables watchpoint 0
 774
 775   // store the debug state
 776   last_halt_debug_state = jtagarm7tdmi_get_dbgstate();
 777   last_halt_pc = jtagarm7tdmi_getpc() - 4;  // assume -4 for entering debug mode via watchpoint.
 778   count_dbgspd_instr_since_debug = 0;
 779   count_sysspd_instr_since_debug = 0;
 780
 781   // get into ARM mode if the T flag is set (Thumb mode)
 782   while (jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT && waitcount-- > 0) {
 783     jtagarm7tdmi_setMode_ARM();
 784   }
 785   jtagarm7tdmi_resettap();
 786   return waitcount;
 787 }
 788
 789 unsigned long jtagarm7tdmi_releasecpu(){
 790   int waitcount = 0xfff;
 791   unsigned long instr;
 792   // somehow determine what PC should be (a couple ways possible, calculations required)
 793   jtagarm7tdmi_nop(0);                          // NOP
 794   jtagarm7tdmi_nop(1);                          // NOP/BREAKPT
 795
 796   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?
 797     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?
 798     jtagarm7tdmi_instr_primitive(instr,0);
 799   } else {
 800     instr = ARM_INSTR_B_PC + 0x1000000 - (count_dbgspd_instr_since_debug*4) - (count_sysspd_instr_since_debug*12);
 801     jtagarm7tdmi_instr_primitive(instr,0);
 802   }
 803
 804   SHIFT_IR;
 805   jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); // VERB_RESTART
 806
 807   // wait until restart-bit set in debug state register
 808   while ((jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_DBGACK) && waitcount > 0){
 809     msdelay(1);
 810     waitcount --;
 811   }
 812   last_halt_debug_state = -1;
 813   last_halt_pc = -1;
 814   return 0;
 815 }
 816
 817
 818
 819
 820 ///////////////////////////////////////////////////////////////////////////////////////////////////
 821 //! Handles ARM7TDMI JTAG commands.  Forwards others to JTAG.
 822 void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len){
 823   register char blocks;
 824
 825   unsigned int i,val,mlop;
 826   unsigned long at;
 827
 828   jtagarm7tdmi_resettap();
 829
 830   switch(verb){
 831   case START:
 832     //Enter JTAG mode.
 833     cmddatalong[0] = jtagarm7tdmi_start();
 834     cmddatalong[2] = jtagarm7tdmi_haltcpu();
 835     //jtagarm7tdmi_resettap();
 836     cmddatalong[1] = jtagarm7tdmi_get_dbgstate();
 837
 838     // DEBUG: FIXME: NOT PART OF OPERATIONAL CODE
 839     //for (mlop=2;mlop<4;mlop++){
 840     //  jtagarm7tdmi_set_register(mlop, 0x43424140);
 841     //}
 842     /////////////////////////////////////////////
 843     txdata(app,verb,0xc);
 844     break;
 845   case JTAGARM7TDMI_READMEM:
 846   case PEEK:
 847     blocks=(len>4?cmddata[4]:1);
 848     at=cmddatalong[0];
 849
 850     len=0x80;
 851     txhead(app,verb,len);
 852
 853     while(blocks--){
 854       for(i=0;i<len;i+=2){
 855         jtagarm7tdmi_resettap();
 856         delay(10);
 857
 858         val=jtagarm7tdmi_readmem(at);
 859
 860         at+=2;
 861         serial_tx(val&0xFF);
 862         serial_tx((val&0xFF00)>>8);
 863       }
 864     }
 865
 866     break;
 867   case JTAGARM7TDMI_GET_CHIP_ID:
 868         jtagarm7tdmi_resettap();
 869     cmddatalong[0] = jtagarm7tdmi_idcode();
 870     txdata(app,verb,4);
 871     break;
 872
 873
 874   case JTAGARM7TDMI_WRITEMEM:
 875   case POKE:
 876         jtagarm7tdmi_resettap();
 877     jtagarm7tdmi_writemem(cmddatalong[0],
 878                        cmddataword[2]);
 879     cmddataword[0]=jtagarm7tdmi_readmem(cmddatalong[0]);
 880     txdata(app,verb,2);
 881     break;
 882
 883   case JTAGARM7TDMI_HALTCPU:
 884     cmddatalong[0] = jtagarm7tdmi_haltcpu();
 885     txdata(app,verb,4);
 886     break;
 887   case JTAGARM7TDMI_RELEASECPU:
 888         jtagarm7tdmi_resettap();
 889     cmddatalong[0] = jtagarm7tdmi_releasecpu();
 890     txdata(app,verb,4);
 891     break;
 892   //unimplemented functions
 893   //case JTAGARM7TDMI_SETINSTRFETCH:
 894   //case JTAGARM7TDMI_WRITEFLASH:
 895   //case JTAGARM7TDMI_ERASEFLASH:
 896   case JTAGARM7TDMI_SET_PC:
 897     cmddatalong[0] = jtagarm7tdmi_setpc(cmddatalong[0]);
 898     txdata(app,verb,4);
 899     break;
 900   case JTAGARM7TDMI_GET_DEBUG_CTRL:
 901     cmddatalong[0] = jtagarm7tdmi_get_dbgctrl();
 902     txdata(app,verb,1);
 903     break;
 904   case JTAGARM7TDMI_SET_DEBUG_CTRL:
 905     cmddatalong[0] = jtagarm7tdmi_set_dbgctrl(cmddata[0]);
 906     txdata(app,verb,4);
 907     break;
 908   case JTAGARM7TDMI_GET_PC:
 909     cmddatalong[0] = jtagarm7tdmi_getpc();
 910     txdata(app,verb,4);
 911     break;
 912   case JTAGARM7TDMI_GET_DEBUG_STATE:
 913     //jtagarm7tdmi_resettap();            // Shouldn't need this, but currently do.  FIXME!
 914     cmddatalong[0] = jtagarm7tdmi_get_dbgstate();
 915     txdata(app,verb,4);
 916     break;
 917   //case JTAGARM7TDMI_GET_WATCHPOINT:
 918   //case JTAGARM7TDMI_SET_WATCHPOINT:
 919   case JTAGARM7TDMI_GET_REGISTER:
 920         jtagarm7tdmi_resettap();
 921     //cmddatalong[0] = jtagarm7tdmi_get_register(cmddata[0]);
 922     cmddatalong[0] = test_get_register(cmddata[0]);
 923     txdata(app,verb,96);
 924     break;
 925   case JTAGARM7TDMI_SET_REGISTER:
 926         jtagarm7tdmi_resettap();
 927     cmddatalong[0] = cmddatalong[1];
 928     test_set_register(cmddata[0], cmddatalong[1]);
 929     //jtagarm7tdmi_set_register(cmddata[0], cmddatalong[1]);
 930     txdata(app,verb,96);
 931     break;
 932   case JTAGARM7TDMI_GET_REGISTERS:
 933         jtagarm7tdmi_resettap();
 934     jtagarm7tdmi_get_registers();
 935     txdata(app,verb,80);
 936     break;
 937   //case JTAGARM7TDMI_SET_REGISTERS:
 938   case JTAGARM7TDMI_DEBUG_INSTR:
 939         jtagarm7tdmi_resettap();
 940     cmddataword[0] = jtagarm7tdmi_exec(cmddataword[0], cmddataword[1], cmddata[9]);
 941     txdata(app,verb,80);
 942     break;
 943   //case JTAGARM7TDMI_STEP_INSTR:
 944 /*  case JTAGARM7TDMI_READ_CODE_MEMORY:
 945   case JTAGARM7TDMI_WRITE_FLASH_PAGE:
 946   case JTAGARM7TDMI_READ_FLASH_PAGE:
 947   case JTAGARM7TDMI_MASS_ERASE_FLASH:
 948   case JTAGARM7TDMI_PROGRAM_FLASH:
 949   case JTAGARM7TDMI_LOCKCHIP:
 950   case JTAGARM7TDMI_CHIP_ERASE:
 951   */
 952 // Really ARM specific stuff
 953   case JTAGARM7TDMI_GET_CPSR:
 954         jtagarm7tdmi_resettap();
 955     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
 956     txdata(app,verb,4);
 957     break;
 958   case JTAGARM7TDMI_SET_CPSR:
 959         jtagarm7tdmi_resettap();
 960     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
 961     txdata(app,verb,4);
 962     break;
 963   case JTAGARM7TDMI_GET_SPSR:           // FIXME: NOT CORRECT
 964         jtagarm7tdmi_resettap();
 965     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
 966     txdata(app,verb,4);
 967     break;
 968   case JTAGARM7TDMI_SET_SPSR:           // FIXME: NOT CORRECT
 969         jtagarm7tdmi_resettap();
 970     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
 971     txdata(app,verb,4);
 972     break;
 973   case JTAGARM7TDMI_SET_MODE_THUMB:
 974   case JTAGARM7TDMI_SET_MODE_ARM:
 975         jtagarm7tdmi_resettap();
 976     cmddataword[0] = jtagarm7tdmi_setMode_ARM();
 977     txdata(app,verb,4);
 978     break;
 979
 980   case 0xD0:          // loopback test
 981     jtagarm7tdmi_resettap();
 982     cmddatalong[0] = jtagarm7tdmi_bypass(cmddatalong[0]);
 983     txdata(app,verb,4);
 984     break;
 985   case 0xD8:          // EICE_READ
 986     jtagarm7tdmi_resettap();
 987     cmddatalong[0] = eice_read(cmddatalong[0]);
 988     txdata(app,verb,4);
 989     break;
 990   case 0xD9:          // EICE_WRITE
 991     jtagarm7tdmi_resettap();
 992     cmddatalong[0] = eice_write(cmddatalong[0], cmddatalong[1]);
 993     txdata(app,verb,4);
 994     break;
 995   case 0xDA:          // TEST MSB THROUGH CHAIN0 and CHAIN1
 996     jtagarm7tdmi_resettap();
 997     jtagarm7tdmi_scan_intest(0);
 998     cmddatalong[0] = jtagarmtransn(0x41414141, 32, LSB, NOEND, NORETIDLE);
 999     cmddatalong[1] = jtagarmtransn(0x42424242, 32, MSB, NOEND, NORETIDLE);
1000     cmddatalong[2] = jtagarmtransn(0x43434343,  9, MSB, NOEND, NORETIDLE);
1001     cmddatalong[3] = jtagarmtransn(0x44444444, 32, MSB, NOEND, NORETIDLE);
1002     cmddatalong[4] = jtagarmtransn(cmddatalong[0], 32, LSB, NOEND, NORETIDLE);
1003     cmddatalong[5] = jtagarmtransn(cmddatalong[1], 32, MSB, NOEND, NORETIDLE);
1004     cmddatalong[6] = jtagarmtransn(cmddatalong[2],  9, MSB, NOEND, NORETIDLE);
1005     cmddatalong[7] = jtagarmtransn(cmddatalong[3], 32, MSB, END, RETIDLE);
1006     jtagarm7tdmi_resettap();
1007     jtagarm7tdmi_scan_intest(1);
1008     cmddatalong[8] = jtagarmtransn(0x41414141, 32, MSB, NOEND, NORETIDLE);
1009     cmddatalong[9] = jtagarmtransn(0x44444444,  1, MSB, NOEND, NORETIDLE);
1010     cmddatalong[10] = jtagarmtransn(cmddatalong[8], 32, MSB, NOEND, NORETIDLE);
1011     cmddatalong[11] = jtagarmtransn(cmddatalong[9],  1, MSB, END, RETIDLE);
1012     jtagarm7tdmi_resettap();
1013     txdata(app,verb,48);
1014     break;
1015
1016   default:
1017     jtaghandle(app,verb,len);
1018   }
1019 }