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