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