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_scan_intest(1);
 358
 359   SHIFT_DR;
 360   // if the next instruction is to run using MCLK (master clock), set TDI
 361   if (breakpt)
 362     {
 363     SETMOSI;
 364     count_sysspd_instr_since_debug++;
 365     }
 366   else
 367     {
 368     CLRMOSI;
 369     count_dbgspd_instr_since_debug++;
 370     }
 371   jtag_arm_tcktock();
 372
 373   // Now shift in the 32 bits
 374   retval = jtagarmtransn(instr, 32, MSB, END, RETIDLE);    // Must return to RUN-TEST/IDLE state for instruction to enter pipeline, and causes debug clock.
 375   //jtag_arm_tcktock();
 376   return(retval);
 377
 378 }
 379
 380
 381 unsigned long jtagarm7tdmi_nop(char breakpt){
 382   //jtagarm7tdmi_scan_intest(1);
 383   //SHIFT_DR
 384   //return jtagarmtransn(ARM_INSTR_NOP, 32, LSB, END, NORETIDLE);
 385   return jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP, breakpt);
 386 }
 387
 388 /*    stolen from ARM DDI0029G documentation, translated using ARM Architecture Reference Manual (14128.pdf)
 389 STR R0, [R0]; Save R0 before use
 390 MOV R0, PC ; Copy PC into R0
 391 STR R0, [R0]; Now save the PC in R0
 392 BX PC ; Jump into ARM state
 393 MOV R8, R8 ;
 394 MOV R8, R8 ;
 395 NOP
 396 NOP
 397
 398 */
 399 //! 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();
 400 unsigned long jtagarm7tdmi_setMode_ARM(){               // PROVEN
 401   unsigned long retval = 0xff;
 402   while ((jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
 403     cmddataword[6] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
 404     cmddataword[1] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
 405     cmddataword[2] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_MOV_R0_PC,0);
 406     cmddataword[3] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
 407     cmddataword[4] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_BX_PC,0);
 408     cmddataword[5] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
 409     cmddataword[6] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
 410     jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
 411   }
 412   return(retval);
 413 }
 414
 415
 416
 417
 418 /************************* EmbeddedICE Primitives ****************************/
 419 //! shifter for writing to chain2 (EmbeddedICE).
 420 unsigned long eice_write(unsigned char reg, unsigned long data){
 421   unsigned long retval, temp;
 422   jtagarm7tdmi_scan_intest(2);
 423   // Now shift in the 32 bits
 424   SHIFT_DR;
 425   retval = jtagarmtransn(data, 32, LSB, NOEND, NORETIDLE);          // send in the data - 32-bits lsb
 426   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);              // send in the register address - 5 bits lsb
 427   jtagarmtransn(1, 1, LSB, END, RETIDLE);                           // send in the WRITE bit
 428
 429   //SETTMS;   // Last Bit - Exit UPDATE_DR
 430   //// is this update a read/write or just read?
 431   //SETMOSI;
 432   //jtag_arm_tcktock();
 433
 434   return(retval);
 435 }
 436
 437 //! shifter for reading from chain2 (EmbeddedICE).
 438 unsigned long eice_read(unsigned char reg){               // PROVEN
 439   unsigned long temp;
 440   jtagarm7tdmi_scan_intest(2);
 441
 442   // send in the register address - 5 bits LSB
 443   SHIFT_DR;
 444   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);
 445
 446   // clear TDI to select "read only"
 447   jtagarmtransn(0, 1, LSB, END, RETIDLE);
 448
 449   SHIFT_DR;
 450   // Now shift out the 32 bits
 451   return(jtagarmtransn(0, 32, LSB, END, RETIDLE));   // atmel arm jtag docs pp.10-11: LSB first
 452
 453 }
 454
 455
 456
 457
 458 /************************* ICEBreaker/EmbeddedICE Stuff ******************************/
 459 //! Grab debug register
 460 unsigned long jtagarm7tdmi_get_dbgstate() {       // ICE Debug register, *NOT* ARM register DSCR    //    PROVEN
 461   //jtagarm7tdmi_resettap();
 462   return eice_read(EICE_DBGSTATUS);
 463 }
 464
 465 //! Grab debug register
 466 unsigned long jtagarm7tdmi_get_dbgctrl() {
 467   return eice_read(EICE_DBGCTRL);
 468 }
 469
 470 //! Update debug register
 471 unsigned long jtagarm7tdmi_set_dbgctrl(unsigned long bits) {
 472   return eice_write(EICE_DBGCTRL, bits);
 473 }
 474
 475
 476
 477 //!  Set and Enable Watchpoint 0
 478 void jtagarm7tdmi_set_watchpoint0(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
 479   // store watchpoint info?  - not right now
 480     // FIXME: store info
 481
 482   eice_write(EICE_WP0ADDR, addr);           // write 0 in watchpoint 0 address
 483   eice_write(EICE_WP0ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 0 address mask
 484   eice_write(EICE_WP0DATA, data);           // write 0 in watchpoint 0 data
 485   eice_write(EICE_WP0DATAMASK, datamask);   // write 0xffffffff in watchpoint 0 data mask
 486   eice_write(EICE_WP0CTRL, ctrlmask);       // write 0x00000100 in watchpoint 0 control value register (enables watchpoint)
 487   eice_write(EICE_WP0CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 0 control mask - only detect the fetch instruction
 488 }
 489
 490 //!  Set and Enable Watchpoint 1
 491 void jtagarm7tdmi_set_watchpoint1(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
 492   // store watchpoint info?  - not right now
 493     // FIXME: store info
 494
 495   eice_write(EICE_WP1ADDR, addr);           // write 0 in watchpoint 1 address
 496   eice_write(EICE_WP1ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 1 address mask
 497   eice_write(EICE_WP1DATA, data);           // write 0 in watchpoint 1 data
 498   eice_write(EICE_WP1DATAMASK, datamask);   // write 0xffffffff in watchpoint 1 data mask
 499   eice_write(EICE_WP1CTRL, ctrl);           // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
 500   eice_write(EICE_WP1CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
 501 }
 502
 503 //!  Disable Watchpoint 0
 504 void jtagarm7tdmi_disable_watchpoint0(){
 505   eice_write(EICE_WP0CTRL, 0x0); // write 0 in watchpoint 0 control value - disables watchpoint 0
 506 }
 507
 508 //!  Disable Watchpoint 1
 509 void jtagarm7tdmi_disable_watchpoint1(){
 510   eice_write(EICE_WP1CTRL, 0x0);            // write 0 in watchpoint 0 control value - disables watchpoint 0
 511 }
 512
 513
 514
 515 /******************** Complex Commands **************************/
 516 //! Push an instruction into the CPU pipeline
 517 //  NOTE!  Must provide EXECNOPARM for parameter if no parm is required.
 518 unsigned long test_exec(unsigned long instr, unsigned long parameter, unsigned char systemspeed) {
 519   unsigned long retval;
 520
 521   cmddatalong[1] = jtagarm7tdmi_nop( 0);
 522   cmddatalong[2] = jtagarm7tdmi_nop(systemspeed);
 523   cmddatalong[3] = jtagarm7tdmi_instr_primitive(instr, 0);      // write 32-bit instruction code into DR
 524   cmddatalong[4] = jtagarm7tdmi_nop( 0);
 525   cmddatalong[5] = jtagarm7tdmi_nop( 0);
 526   cmddatalong[6] = jtagarm7tdmi_instr_primitive(parameter, 0);  // inject long
 527   cmddatalong[7] = jtagarm7tdmi_nop( 0);
 528   cmddatalong[8] = jtagarm7tdmi_nop( 0);
 529   cmddatalong[9] = jtagarm7tdmi_nop( 0);
 530   retval = cmddatalong[9];
 531
 532   return(retval);
 533 }
 534
 535
 536 //! Push an instruction into the CPU pipeline
 537 //  NOTE!  Must provide EXECNOPARM for parameter if no parm is required.
 538 unsigned long jtagarm7tdmi_exec(unsigned long instr, unsigned long parameter, unsigned char systemspeed) {
 539   unsigned long retval;
 540
 541   cmddatalong[1] = jtagarm7tdmi_nop( 0);
 542   cmddatalong[2] = jtagarm7tdmi_nop(systemspeed);
 543   cmddatalong[3] = jtagarm7tdmi_instr_primitive(instr, 0);      // write 32-bit instruction code into DR
 544   cmddatalong[4] = jtagarm7tdmi_nop( 0);
 545   cmddatalong[5] = jtagarm7tdmi_nop( 0);
 546   cmddatalong[6] = jtagarm7tdmi_instr_primitive(parameter, 0);  // inject long
 547   cmddatalong[7] = jtagarm7tdmi_nop( 0);
 548   retval = jtagarm7tdmi_nop( 0);
 549   cmddatalong[9] = jtagarm7tdmi_nop( 0);
 550   cmddatalong[8] = retval;
 551
 552   return(retval);
 553 }
 554
 555 //! Retrieve a 32-bit Register value
 556 unsigned long jtagarm7tdmi_get_register(unsigned char reg) {
 557   unsigned long retval = 0, instr;
 558   // push nop into pipeline - clean out the pipeline...
 559   cmddatalong[2] = jtagarm7tdmi_nop( 0);
 560
 561   instr = ARM_READ_REG | (reg<<12);                     // push STR Rx, [R14] into pipeline
 562   cmddatalong[1] = jtagarm7tdmi_instr_primitive(instr, 0);
 563   cmddatalong[2] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - fetched
 564   cmddatalong[3] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - decoded
 565   cmddatalong[4] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - executed
 566   //retval = jtagarmtransn(ARM_INSTR_NOP, 32, LSB, END, NORETIDLE); //DEBUGGING NOT FOR RESALE!
 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 == 0xffff){
 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 }