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

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