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