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

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