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