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

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