for (bit = bitcount; bit > 0; bit++)
[goodfet] / 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 //! Retrieve a 32-bit Register value
576 unsigned long test_get_register(unsigned char reg) {
577   unsigned long retval = 0, instr;
578   // push nop into pipeline - clean out the pipeline...
579   cmddatalong[2] = jtagarm7tdmi_nop( 0);
580
581   instr = ARM_WRITE_REG | (reg<<12);                     // push STR Rx, [R14] into pipeline
582   cmddatalong[1] = jtagarm7tdmi_instr_primitive(instr, 0);
583   cmddatalong[2] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - fetched
584   cmddatalong[3] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - decoded
585   cmddatalong[4] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - executed 
586   retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
587   cmddatalong[5] = retval;
588   cmddatalong[6] = jtagarm7tdmi_nop( 0);
589   cmddatalong[7] = jtagarm7tdmi_nop( 0);
590   cmddatalong[8] = jtagarm7tdmi_nop( 0);
591   return retval;
592 }
593
594 //! Set a 32-bit Register value
595 unsigned long jtagarm7tdmi_set_register(unsigned char reg, unsigned long val) {
596   unsigned long retval = 0, instr;
597   cmddatalong[2] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
598
599   instr = ARM_WRITE_REG | (reg<<12);     // push LDR Rx, [R14] into pipeline
600   cmddatalong[1] = jtagarm7tdmi_instr_primitive(instr, 0);
601   cmddatalong[2] = jtagarm7tdmi_nop( 0); // push nop into pipeline - fetched
602   cmddatalong[3] = jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded
603   
604   cmddatalong[4] = jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus - execute state
605   cmddatalong[5] = jtagarm7tdmi_nop( 0); // push nop into pipeline - executed 
606
607   if (reg == ARM_REG_PC){
608     cmddatalong[6] = jtagarm7tdmi_nop( 0);
609     cmddatalong[7] = jtagarm7tdmi_nop( 0);
610   }
611   cmddatalong[8] = jtagarm7tdmi_nop( 0);
612
613   retval = cmddatalong[5];
614   return(retval);
615 }
616
617 //! Set a 32-bit Register value
618 unsigned long test_set_register(unsigned char reg, unsigned long val) {
619   unsigned long retval = 0, instr;
620   cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
621
622   instr = ARM_READ_REG | (reg<<12);     // push LDR Rx, [R14] into pipeline
623   cmddatalong[2] = jtagarm7tdmi_instr_primitive(instr, 0);
624   
625   cmddatalong[3] = jtagarm7tdmi_instr_primitive(val+32, 0); // push 32-bit word on data bus - execute state
626   cmddatalong[4] = jtagarm7tdmi_instr_primitive(val+16, 0); // push 32-bit word on data bus - execute state
627   cmddatalong[5] = jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus - execute state
628   cmddatalong[6] = jtagarm7tdmi_instr_primitive(val-16, 0); // push 32-bit word on data bus - execute state
629
630   if (reg == ARM_REG_PC){
631     cmddatalong[7] = jtagarm7tdmi_nop( 0);
632     cmddatalong[8] = jtagarm7tdmi_nop( 0);
633   }
634   cmddatalong[9] = jtagarm7tdmi_instr_primitive(val-32, 0); // push 32-bit word on data bus - execute state
635
636   retval = cmddatalong[5];
637   return(retval);
638 }
639
640
641
642 //! Get all registers.  Return an array
643 unsigned long* jtagarm7tdmi_get_registers() {
644   cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0);
645   cmddatalong[2] = jtagarm7tdmi_nop( 0);
646   cmddatalong[3] = jtagarm7tdmi_nop( 0);
647   cmddatalong[4] = jtagarm7tdmi_nop( 0);
648   cmddatalong[5] = jtagarm7tdmi_nop( 0);
649   cmddatalong[6] = jtagarm7tdmi_nop( 0);
650   cmddatalong[7] = jtagarm7tdmi_nop( 0);
651   cmddatalong[8] = jtagarm7tdmi_nop( 0);
652   cmddatalong[9] = jtagarm7tdmi_nop( 0);
653   cmddatalong[10] = jtagarm7tdmi_nop( 0);
654   cmddatalong[11] = jtagarm7tdmi_nop( 0);
655   cmddatalong[12] = jtagarm7tdmi_nop( 0);
656   cmddatalong[13] = jtagarm7tdmi_nop( 0);
657   cmddatalong[14] = jtagarm7tdmi_nop( 0);
658   cmddatalong[15] = jtagarm7tdmi_nop( 0);
659   cmddatalong[16] = jtagarm7tdmi_nop( 0);
660   cmddatalong[17] = jtagarm7tdmi_nop( 0);
661   cmddatalong[18] = jtagarm7tdmi_nop( 0);
662   cmddatalong[19] = jtagarm7tdmi_nop( 0);
663   cmddatalong[20] = jtagarm7tdmi_nop( 0);
664   return registers;
665 }
666
667 //! Retrieve the CPSR Register value
668 unsigned long jtagarm7tdmi_get_regCPSR() {
669   unsigned long retval = 0;
670
671   cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
672   cmddatalong[2] = jtagarm7tdmi_instr_primitive(ARM_INSTR_MRS_R0_CPSR, 0); // push MRS_R0, CPSR into pipeline
673   cmddatalong[3] = jtagarm7tdmi_nop( 0); // push nop into pipeline - fetched
674   cmddatalong[4] = jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded
675   cmddatalong[5] = jtagarm7tdmi_nop( 0); // push nop into pipeline - executed 
676   retval = jtagarm7tdmi_nop( 0);        // recover 32-bit word
677   cmddatalong[6] = retval;
678   return retval;
679 }
680
681 //! Retrieve the CPSR Register value
682 unsigned long jtagarm7tdmi_set_regCPSR(unsigned long val) {
683   unsigned long retval = 0;
684
685   cmddatalong[1] = jtagarm7tdmi_nop( 0);        // push nop into pipeline - clean out the pipeline...
686   cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_MSR_cpsr_cxsf_R0, 0); // push MSR cpsr_cxsf, R0 into pipeline
687   cmddatalong[2] = jtagarm7tdmi_nop( 0);        // push nop into pipeline - fetched
688   cmddatalong[3] = jtagarm7tdmi_nop( 0);        // push nop into pipeline - decoded
689   
690   retval = jtagarm7tdmi_instr_primitive(val, 0);// push 32-bit word on data bus
691   cmddatalong[5] = jtagarm7tdmi_nop( 0);        // push nop into pipeline - executed 
692   cmddatalong[4] = retval;
693   return(retval);
694 }
695
696 //! Write data to address - Assume TAP in run-test/idle state
697 unsigned long jtagarm7tdmi_writemem(unsigned long adr, unsigned long data){
698   unsigned long r0=0, r1=-1;
699
700   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
701   r1 = jtagarm7tdmi_get_register(1);
702   jtagarm7tdmi_set_register(0, adr);        // write address into R0
703   jtagarm7tdmi_set_register(1, data);       // write data in R1
704   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
705   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
706   jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline
707   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
708   jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1 
709   jtagarm7tdmi_set_register(0, r0);
710   return(-1);
711 }
712
713
714
715
716 //! Read data from address
717 unsigned long jtagarm7tdmi_readmem(unsigned long adr){
718   unsigned long retval = 0;
719   unsigned long r0=0, r1=-1;
720   int waitcount = 0xfff;
721
722   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
723   r1 = jtagarm7tdmi_get_register(1);
724   jtagarm7tdmi_set_register(0, adr);        // write address into R0
725   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
726   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
727   jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline
728   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
729   jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
730
731   // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
732   while ((jtagarm7tdmi_get_dbgstate() & 9) == 0  && waitcount > 0){
733     delay(1);
734     waitcount --;
735   }
736   if (waitcount == 0){
737     return (-1);
738   } else {
739     retval = jtagarm7tdmi_get_register(1);  // read memory value from R1 register
740     jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1 
741     jtagarm7tdmi_set_register(0, r0);
742   }
743   return retval;
744 }
745
746
747 //! Read Program Counter
748 unsigned long jtagarm7tdmi_getpc(){
749   return jtagarm7tdmi_get_register(ARM_REG_PC);
750 }
751
752 //! Set Program Counter
753 unsigned long jtagarm7tdmi_setpc(unsigned long adr){
754   return jtagarm7tdmi_set_register(ARM_REG_PC, adr);
755 }
756
757 //! Halt CPU - returns 0xffff if the operation fails to complete within 
758 unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
759   int waitcount = 0xfff;
760
761   // store watchpoint info?  - not right now
762   eice_write(EICE_WP1ADDR, 0);              // write 0 in watchpoint 1 address
763   eice_write(EICE_WP1ADDRMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 address mask
764   eice_write(EICE_WP1DATA, 0);              // write 0 in watchpoint 1 data
765   eice_write(EICE_WP1DATAMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 data mask
766   eice_write(EICE_WP1CTRL, 0x100);          //!!!!! WTF!  THIS IS SUPPOSED TO BE 9 bits wide?!?  // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
767   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
768
769   // poll until debug status says the cpu is in debug mode
770   while (!(jtagarm7tdmi_get_dbgstate() & 0x1)   && waitcount-- > 0){
771     delay(1);
772   }
773   eice_write(EICE_WP1CTRL, 0x0);            // write 0 in watchpoint 0 control value - disables watchpoint 0
774
775   // store the debug state
776   last_halt_debug_state = jtagarm7tdmi_get_dbgstate();
777   last_halt_pc = jtagarm7tdmi_getpc() - 4;  // assume -4 for entering debug mode via watchpoint.
778   count_dbgspd_instr_since_debug = 0;
779   count_sysspd_instr_since_debug = 0;
780
781   // get into ARM mode if the T flag is set (Thumb mode)
782   while (jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT && waitcount-- > 0) {
783     jtagarm7tdmi_setMode_ARM();
784   }
785   jtagarm7tdmi_resettap();
786   return waitcount;
787 }
788
789 unsigned long jtagarm7tdmi_releasecpu(){
790   int waitcount = 0xfff;
791   unsigned long instr;
792   // somehow determine what PC should be (a couple ways possible, calculations required)
793   jtagarm7tdmi_nop(0);                          // NOP
794   jtagarm7tdmi_nop(1);                          // NOP/BREAKPT
795
796   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?
797     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?
798     jtagarm7tdmi_instr_primitive(instr,0);
799   } else {
800     instr = ARM_INSTR_B_PC + 0x1000000 - (count_dbgspd_instr_since_debug*4) - (count_sysspd_instr_since_debug*12);
801     jtagarm7tdmi_instr_primitive(instr,0);
802   }
803
804   SHIFT_IR;
805   jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); // VERB_RESTART
806
807   // wait until restart-bit set in debug state register
808   while ((jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_DBGACK) && waitcount > 0){
809     msdelay(1);
810     waitcount --;
811   }
812   last_halt_debug_state = -1;
813   last_halt_pc = -1;
814   return 0;
815 }
816  
817
818
819
820 ///////////////////////////////////////////////////////////////////////////////////////////////////
821 //! Handles ARM7TDMI JTAG commands.  Forwards others to JTAG.
822 void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len){
823   register char blocks;
824   
825   unsigned int i,val,mlop;
826   unsigned long at;
827   
828   jtagarm7tdmi_resettap();
829  
830   switch(verb){
831   case START:
832     //Enter JTAG mode.
833     cmddatalong[0] = jtagarm7tdmi_start();
834     cmddatalong[2] = jtagarm7tdmi_haltcpu();
835     //jtagarm7tdmi_resettap();
836     cmddatalong[1] = jtagarm7tdmi_get_dbgstate();
837     
838     // DEBUG: FIXME: NOT PART OF OPERATIONAL CODE
839     //for (mlop=2;mlop<4;mlop++){
840     //  jtagarm7tdmi_set_register(mlop, 0x43424140);
841     //} 
842     /////////////////////////////////////////////
843     txdata(app,verb,0xc);
844     break;
845   case JTAGARM7TDMI_READMEM:
846   case PEEK:
847     blocks=(len>4?cmddata[4]:1);
848     at=cmddatalong[0];
849     
850     len=0x80;
851     txhead(app,verb,len);
852     
853     while(blocks--){
854       for(i=0;i<len;i+=2){
855         jtagarm7tdmi_resettap();
856         delay(10);
857         
858         val=jtagarm7tdmi_readmem(at);
859                 
860         at+=2;
861         serial_tx(val&0xFF);
862         serial_tx((val&0xFF00)>>8);
863       }
864     }
865     
866     break;
867   case JTAGARM7TDMI_GET_CHIP_ID:
868         jtagarm7tdmi_resettap();
869     cmddatalong[0] = jtagarm7tdmi_idcode();
870     txdata(app,verb,4);
871     break;
872
873
874   case JTAGARM7TDMI_WRITEMEM:
875   case POKE:
876         jtagarm7tdmi_resettap();
877     jtagarm7tdmi_writemem(cmddatalong[0],
878                        cmddataword[2]);
879     cmddataword[0]=jtagarm7tdmi_readmem(cmddatalong[0]);
880     txdata(app,verb,2);
881     break;
882
883   case JTAGARM7TDMI_HALTCPU:  
884     cmddatalong[0] = jtagarm7tdmi_haltcpu();
885     txdata(app,verb,4);
886     break;
887   case JTAGARM7TDMI_RELEASECPU:
888         jtagarm7tdmi_resettap();
889     cmddatalong[0] = jtagarm7tdmi_releasecpu();
890     txdata(app,verb,4);
891     break;
892   //unimplemented functions
893   //case JTAGARM7TDMI_SETINSTRFETCH:
894   //case JTAGARM7TDMI_WRITEFLASH:
895   //case JTAGARM7TDMI_ERASEFLASH:
896   case JTAGARM7TDMI_SET_PC:
897     cmddatalong[0] = jtagarm7tdmi_setpc(cmddatalong[0]);
898     txdata(app,verb,4);
899     break;
900   case JTAGARM7TDMI_GET_DEBUG_CTRL:
901     cmddatalong[0] = jtagarm7tdmi_get_dbgctrl();
902     txdata(app,verb,1);
903     break;
904   case JTAGARM7TDMI_SET_DEBUG_CTRL:
905     cmddatalong[0] = jtagarm7tdmi_set_dbgctrl(cmddata[0]);
906     txdata(app,verb,4);
907     break;
908   case JTAGARM7TDMI_GET_PC:
909     cmddatalong[0] = jtagarm7tdmi_getpc();
910     txdata(app,verb,4);
911     break;
912   case JTAGARM7TDMI_GET_DEBUG_STATE:
913     //jtagarm7tdmi_resettap();            // Shouldn't need this, but currently do.  FIXME!
914     cmddatalong[0] = jtagarm7tdmi_get_dbgstate();
915     txdata(app,verb,4);
916     break;
917   //case JTAGARM7TDMI_GET_WATCHPOINT:
918   //case JTAGARM7TDMI_SET_WATCHPOINT:
919   case JTAGARM7TDMI_GET_REGISTER:
920         jtagarm7tdmi_resettap();
921     //cmddatalong[0] = jtagarm7tdmi_get_register(cmddata[0]);
922     cmddatalong[0] = test_get_register(cmddata[0]);
923     txdata(app,verb,96);
924     break;
925   case JTAGARM7TDMI_SET_REGISTER:
926         jtagarm7tdmi_resettap();
927     cmddatalong[0] = cmddatalong[1];
928     test_set_register(cmddata[0], cmddatalong[1]);
929     //jtagarm7tdmi_set_register(cmddata[0], cmddatalong[1]);
930     txdata(app,verb,96);
931     break;
932   case JTAGARM7TDMI_GET_REGISTERS:
933         jtagarm7tdmi_resettap();
934     jtagarm7tdmi_get_registers();
935     txdata(app,verb,80);
936     break;
937   //case JTAGARM7TDMI_SET_REGISTERS:
938   case JTAGARM7TDMI_DEBUG_INSTR:
939         jtagarm7tdmi_resettap();
940     cmddataword[0] = jtagarm7tdmi_exec(cmddataword[0], cmddataword[1], cmddata[9]);
941     txdata(app,verb,80);
942     break;
943   //case JTAGARM7TDMI_STEP_INSTR:
944 /*  case JTAGARM7TDMI_READ_CODE_MEMORY:
945   case JTAGARM7TDMI_WRITE_FLASH_PAGE:
946   case JTAGARM7TDMI_READ_FLASH_PAGE:
947   case JTAGARM7TDMI_MASS_ERASE_FLASH:
948   case JTAGARM7TDMI_PROGRAM_FLASH:
949   case JTAGARM7TDMI_LOCKCHIP:
950   case JTAGARM7TDMI_CHIP_ERASE:
951   */
952 // Really ARM specific stuff
953   case JTAGARM7TDMI_GET_CPSR:
954         jtagarm7tdmi_resettap();
955     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
956     txdata(app,verb,4);
957     break;
958   case JTAGARM7TDMI_SET_CPSR:
959         jtagarm7tdmi_resettap();
960     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
961     txdata(app,verb,4);
962     break;
963   case JTAGARM7TDMI_GET_SPSR:           // FIXME: NOT CORRECT
964         jtagarm7tdmi_resettap();
965     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
966     txdata(app,verb,4);
967     break;
968   case JTAGARM7TDMI_SET_SPSR:           // FIXME: NOT CORRECT
969         jtagarm7tdmi_resettap();
970     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
971     txdata(app,verb,4);
972     break;
973   case JTAGARM7TDMI_SET_MODE_THUMB:
974   case JTAGARM7TDMI_SET_MODE_ARM:
975         jtagarm7tdmi_resettap();
976     cmddataword[0] = jtagarm7tdmi_setMode_ARM();
977     txdata(app,verb,4);
978     break;
979     
980   case 0xD0:          // loopback test
981     jtagarm7tdmi_resettap();
982     cmddatalong[0] = jtagarm7tdmi_bypass(cmddatalong[0]);
983     txdata(app,verb,4);
984     break;
985   case 0xD8:          // EICE_READ
986     jtagarm7tdmi_resettap();
987     cmddatalong[0] = eice_read(cmddatalong[0]);
988     txdata(app,verb,4);
989     break;
990   case 0xD9:          // EICE_WRITE
991     jtagarm7tdmi_resettap();
992     cmddatalong[0] = eice_write(cmddatalong[0], cmddatalong[1]);
993     txdata(app,verb,4);
994     break;
995   case 0xDA:          // TEST MSB THROUGH CHAIN0 and CHAIN1
996     jtagarm7tdmi_resettap();
997     jtagarm7tdmi_scan_intest(0);
998     cmddatalong[0] = jtagarmtransn(0x41414141, 32, LSB, NOEND, NORETIDLE);
999     cmddatalong[1] = jtagarmtransn(0x42424242, 32, MSB, NOEND, NORETIDLE);
1000     cmddatalong[2] = jtagarmtransn(0x43434343,  9, MSB, NOEND, NORETIDLE);
1001     cmddatalong[3] = jtagarmtransn(0x44444444, 32, MSB, NOEND, NORETIDLE);
1002     cmddatalong[4] = jtagarmtransn(cmddatalong[0], 32, LSB, NOEND, NORETIDLE);
1003     cmddatalong[5] = jtagarmtransn(cmddatalong[1], 32, MSB, NOEND, NORETIDLE);
1004     cmddatalong[6] = jtagarmtransn(cmddatalong[2],  9, MSB, NOEND, NORETIDLE);
1005     cmddatalong[7] = jtagarmtransn(cmddatalong[3], 32, MSB, END, RETIDLE);
1006     jtagarm7tdmi_resettap();
1007     jtagarm7tdmi_scan_intest(1);
1008     cmddatalong[8] = jtagarmtransn(0x41414141, 32, MSB, NOEND, NORETIDLE);
1009     cmddatalong[9] = jtagarmtransn(0x44444444,  1, MSB, NOEND, NORETIDLE);
1010     cmddatalong[10] = jtagarmtransn(cmddatalong[8], 32, MSB, NOEND, NORETIDLE);
1011     cmddatalong[11] = jtagarmtransn(cmddatalong[9],  1, MSB, END, RETIDLE);
1012     jtagarm7tdmi_resettap();
1013     txdata(app,verb,48);
1014     break;
1015     
1016   default:
1017     jtaghandle(app,verb,len);
1018   }
1019 }