61d851fc74e2bc8e542339f0d4b4d651a65141ed
[goodfet] / firmware / apps / jtag / jtagarm7tdmi.c
1 /*! \file jtagarm7tdmi.c
2   \brief ARM7TDMI JTAG (AT91R40008, AT91SAM7xxx)
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 -----------  SAMPLE TIMES  -----------
88
89 TDI and TMS are sampled on the rising edge of TCK and TDO transitions appear on the falling edge of TCK. Therefore, TDI and TMS must be written after the falling edge of TCK and TDO must be read after the rising edge of TCK.
90
91 for this module, we keep tck high for all changes/sampling, and then bounce it.
92 ****************************************************************/
93
94
95
96 /************************** JTAGARM7TDMI Primitives ****************************/
97 /*void jtag_goto_shift_ir() {
98   SETTMS;
99   jtag_arm_tcktock();
100   jtag_arm_tcktock();
101   CLRTMS;
102   jtag_arm_tcktock();
103   jtag_arm_tcktock();
104
105 }
106 void jtag_goto_shift_dr() {
107   SETTMS;
108   jtag_arm_tcktock();
109   CLRTMS;
110   jtag_arm_tcktock();
111   jtag_arm_tcktock();
112 }
113 */
114
115 void jtag_reset_to_runtest_idle() {
116   SETTMS;
117   jtag_arm_tcktock();
118   jtag_arm_tcktock();
119   jtag_arm_tcktock();
120   jtag_arm_tcktock();
121   jtag_arm_tcktock();  // now in Reset state
122   CLRTMS;
123   jtag_arm_tcktock();  // now in Run-Test/Idle state
124 }
125
126 void jtag_arm_tcktock() {
127   delay(1);  // FIXME: Should never wait this long...
128   CLRTCK; 
129   PLEDOUT^=PLEDPIN; 
130   delay(1);  // FIXME: Should never wait this long...
131   SETTCK; 
132   PLEDOUT^=PLEDPIN;
133 }
134
135
136 // ! Start JTAG, setup pins, reset TAP and return IDCODE
137 unsigned long jtagarm7tdmi_start() {
138   jtagsetup();
139   jtagarm7tdmi_resettap();
140   return jtagarm7tdmi_idcode();
141 }
142
143
144 //! Reset TAP State Machine       
145 void jtagarm7tdmi_resettap(){               // PROVEN
146   current_chain = -1;
147   jtag_reset_to_runtest_idle();
148 }
149
150
151 //  NOTE: important: THIS MODULE REVOLVES AROUND RETURNING TO RUNTEST/IDLE, OR THE FUNCTIONAL EQUIVALENT
152
153
154 //! 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
155 unsigned long jtagarmtransn(unsigned long word, unsigned char bitcount, unsigned char lsb, unsigned char end, unsigned char retidle){               // PROVEN
156   unsigned char bit;
157   unsigned long high = 1L;
158   unsigned long mask;
159
160   //for (bit=(bitcount-1)/8; bit>0; bit--)
161   //  high <<= 8;
162   //high <<= ((bitcount-1)%8);
163   high <<= (bitcount-1);
164
165   mask = high-1;
166
167   if (lsb) {
168     for (bit = bitcount; bit > 0; bit--) {
169       /* write MOSI on trailing edge of previous clock */
170       if (word & 1)
171         {SETMOSI;}
172       else
173         {CLRMOSI;}
174       word >>= 1;
175
176       if (bit==1 && end)
177         SETTMS;//TMS high on last bit to exit.
178        
179       jtag_arm_tcktock();
180
181       /* read MISO on trailing edge */
182       if (READMISO){
183         word += (high);
184       }
185     }
186   } else {
187     for (bit = bitcount; bit > 0; bit--) {
188       /* write MOSI on trailing edge of previous clock */
189       if (word & high)
190         {SETMOSI;}
191       else
192         {CLRMOSI;}
193       word = (word & mask) << 1;
194
195       if (bit==1 && end)
196         SETTMS;//TMS high on last bit to exit.
197
198       jtag_arm_tcktock();
199
200       /* read MISO on trailing edge */
201       word |= (READMISO);
202     }
203   }
204  
205
206   SETMOSI;
207
208   if (end){
209     // exit state
210     jtag_arm_tcktock();
211     // update state
212     if (retidle){
213       CLRTMS;
214       jtag_arm_tcktock();
215     }
216   }
217   return word;
218 }
219
220
221
222 /************************************************************************
223 * ARM7TDMI core has 6 primary registers to be connected between TDI/TDO
224 *   * Bypass Register
225 *   * ID Code Register
226 *   * Scan Chain Select Register    (4 bits_lsb)
227 *   * Scan Chain 0                  (105 bits: 32_databits_lsb + ctrlbits + 32_addrbits_msb)
228 *   * Scan Chain 1                  (33 bits: 32_bits + BREAKPT)
229 *   * Scan Chain 2                  (38 bits: rw + 5_regbits_msb + 32_databits_msb)
230 ************************************************************************/
231
232
233
234 /************************** Basic JTAG Verb Commands *******************************/
235 //! Grab the core ID.
236 unsigned long jtagarm7tdmi_idcode(){               // PROVEN
237   jtagarm7tdmi_resettap();
238   SHIFT_IR;
239   jtagarmtransn(ARM7TDMI_IR_IDCODE, 4, LSB, END, RETIDLE);
240   SHIFT_DR;
241   return jtagarmtransn(0,32, LSB, END, RETIDLE);
242 }
243
244 //!  Connect Bypass Register to TDO/TDI
245 unsigned char jtagarm7tdmi_bypass(){               // PROVEN
246   jtagarm7tdmi_resettap();
247   SHIFT_IR;
248   return jtagarmtransn(ARM7TDMI_IR_BYPASS, 4, LSB, END, NORETIDLE);
249 }
250 //!  INTEST verb - do internal test
251 unsigned char jtagarm7tdmi_intest() { 
252   SHIFT_IR;
253   return jtagarmtransn(ARM7TDMI_IR_INTEST, 4, LSB, END, NORETIDLE); 
254 }
255
256 //!  EXTEST verb - act like the processor to external components
257 unsigned char jtagarm7tdmi_extest() { 
258   SHIFT_IR;
259   return jtagarmtransn(ARM7TDMI_IR_EXTEST, 4, LSB, END, NORETIDLE);
260 }
261
262 //!  SAMPLE verb
263 //unsigned long jtagarm7tdmi_sample() { 
264 //  jtagarm7tdmi_ir_shift4(ARM7TDMI_IR_SAMPLE);        // ???? same here.
265 //  return jtagtransn(0,32);
266 //}
267
268 //!  RESTART verb
269 unsigned char jtagarm7tdmi_restart() { 
270   jtagarm7tdmi_resettap();
271   SHIFT_IR;
272   return jtagarmtransn(ARM7TDMI_IR_RESTART, 4, LSB, END, RETIDLE); 
273 }
274
275 //!  ARM7TDMI_IR_CLAMP               0x5
276 //unsigned long jtagarm7tdmi_clamp() { 
277 //  jtagarm7tdmi_resettap();
278 //  SHIFT_IR;
279 //  jtagarmtransn(ARM7TDMI_IR_CLAMP, 4, LSB, END, NORETIDLE);
280 //  SHIFT_DR;
281 //  return jtagarmtransn(0, 32, LSB, END, RETIDLE);
282 //}
283
284 //!  ARM7TDMI_IR_HIGHZ               0x7
285 //unsigned char jtagarm7tdmi_highz() { 
286 //  jtagarm7tdmi_resettap();
287 //  SHIFT_IR;
288 //  return jtagarmtransn(ARM7TDMI_IR_HIGHZ, 4, LSB, END, NORETIDLE);
289 //}
290
291 //! define ARM7TDMI_IR_CLAMPZ              0x9
292 //unsigned char jtagarm7tdmi_clampz() { 
293 //  jtagarm7tdmi_resettap();
294 //  SHIFT_IR;
295 //  return jtagarmtransn(ARM7TDMI_IR_CLAMPZ, 4, LSB, END, NORETIDLE);
296 //}
297
298
299 //!  Connect the appropriate scan chain to TDO/TDI.  SCAN_N, INTEST, ENDS IN SHIFT_DR!!!!!
300 unsigned long jtagarm7tdmi_scan(int chain, int testmode) {               // PROVEN
301 /*
302 When selecting a scan chain the “Run Test/Idle” state should never be reached, other-
303 wise, when in debug state, the core will not be correctly isolated and intrusive
304 commands occur. Therefore, it is recommended to pass directly from the “Update”
305 state” to the “Select DR” state each time the “Update” state is reached.
306 */
307   unsigned long retval;
308   if (current_chain != chain) {
309     //debugstr("===change chains===");
310     SHIFT_IR;
311     jtagarmtransn(ARM7TDMI_IR_SCAN_N, 4, LSB, END, NORETIDLE);
312     SHIFT_DR;
313     retval = jtagarmtransn(chain, 4, LSB, END, NORETIDLE);
314     current_chain = chain;
315   }    else
316     //debugstr("===NOT change chains===");
317     retval = current_chain;
318   // put in test mode...
319   SHIFT_IR;
320   jtagarmtransn(testmode, 4, LSB, END, RETIDLE); 
321   return(retval);
322 }
323
324
325 //!  Connect the appropriate scan chain to TDO/TDI.  SCAN_N, INTEST, ENDS IN SHIFT_DR!!!!!
326 unsigned long jtagarm7tdmi_scan_intest(int chain) {               // PROVEN
327   return jtagarm7tdmi_scan(chain, ARM7TDMI_IR_INTEST);
328 }
329
330
331
332
333 //! push an instruction into the pipeline
334 unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){  // PROVEN
335   unsigned long retval;
336   jtagarm7tdmi_scan_intest(1);
337
338   SHIFT_DR;
339   // if the next instruction is to run using MCLK (master clock), set TDI
340   if (breakpt)
341     {
342     SETMOSI;
343     count_sysspd_instr_since_debug++;
344     } 
345   else
346     {
347     CLRMOSI; 
348     count_dbgspd_instr_since_debug++;
349     }
350   jtag_arm_tcktock();
351   
352   // Now shift in the 32 bits
353   retval = jtagarmtransn(instr, 32, MSB, END, RETIDLE);    // Must return to RUN-TEST/IDLE state for instruction to enter pipeline, and causes debug clock.
354   return(retval);
355   
356 }
357
358 //! push NOP into the instruction pipeline
359 unsigned long jtagarm7tdmi_nop(char breakpt){  // PROVEN
360   if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT) 
361     return jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP, breakpt);
362   return jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP, breakpt);
363 }
364
365 /*    stolen from ARM DDI0029G documentation, translated using ARM Architecture Reference Manual (14128.pdf)
366 STR R0, [R0]; Save R0 before use
367 MOV R0, PC ; Copy PC into R0
368 STR R0, [R0]; Now save the PC in R0
369 BX PC ; Jump into ARM state
370 MOV R8, R8 ;
371 MOV R8, R8 ;
372 NOP
373 NOP
374
375 */
376
377 //! set the current mode to ARM, returns PC (FIXME).  Should be used by haltcpu(), which should also store PC and the THUMB state, for use by releasecpu();
378 unsigned long jtagarm7tdmi_setMode_ARM(unsigned char restart){               // PROVEN  BUT FUGLY! FIXME: clean up and store and replace clobbered r0
379   debugstr("=== Switching to ARM mode ===");
380   unsigned long retval = 0xffL;
381   while ((current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
382     cmddataword[9] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
383     cmddataword[1] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
384     cmddataword[2] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_MOV_R0_PC,0);
385     cmddataword[3] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0);
386     cmddataword[4] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_BX_PC,0);
387     cmddataword[5] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
388     cmddataword[6] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
389     cmddataword[7] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
390     jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
391     current_dbgstate = jtagarm7tdmi_get_dbgstate();
392     jtagarm7tdmi_set_register(0,cmddataword[4]);
393     debugstr("PC:");
394     debughex32(cmddataword[6]);
395     debughex32(cmddataword[7]);
396     debughex32(cmddataword[9]);
397   }
398   return(retval);
399 }
400
401
402 //! 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();
403 unsigned long jtagarm7tdmi_setMode_THUMB(unsigned char restart){               // PROVEN
404   debugstr("=== Switching to THUMB mode ===");
405   unsigned long retval = 0xffL;
406   while (!(current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){
407     last_halt_pc |= 1;
408     cmddataword[9] = jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
409     jtagarm7tdmi_set_register(0, last_halt_pc);
410     cmddataword[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_BX_R0,0);
411     if (restart) {
412       jtagarm7tdmi_restart();
413     } else {
414       jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
415       jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP,0);
416       jtagarm7tdmi_resettap();                  // seems necessary for some reason.
417     }
418     current_dbgstate = jtagarm7tdmi_get_dbgstate();
419   }
420   return(retval);
421 }
422
423
424
425
426 /************************* EmbeddedICE Primitives ****************************/
427 //! shifter for writing to chain2 (EmbeddedICE). 
428 unsigned long eice_write(unsigned char reg, unsigned long data){
429   unsigned long retval, temp;
430   jtagarm7tdmi_scan_intest(2);
431   // Now shift in the 32 bits
432   SHIFT_DR;
433   retval = jtagarmtransn(data, 32, LSB, NOEND, NORETIDLE);          // send in the data - 32-bits lsb
434   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);              // send in the register address - 5 bits lsb
435   jtagarmtransn(1, 1, LSB, END, RETIDLE);                           // send in the WRITE bit
436   
437   return(retval); 
438 }
439
440 //! shifter for reading from chain2 (EmbeddedICE).
441 unsigned long eice_read(unsigned char reg){               // PROVEN
442   unsigned long temp, retval;
443   //debugstr("eice_read");
444   //debughex(reg);
445   jtagarm7tdmi_scan_intest(2);
446
447   // send in the register address - 5 bits LSB
448   SHIFT_DR;
449   temp = jtagarmtransn(reg, 5, LSB, NOEND, NORETIDLE);
450   
451   // clear TDI to select "read only"
452   jtagarmtransn(0L, 1, LSB, END, RETIDLE);
453   
454   SHIFT_DR;
455   // Now shift out the 32 bits
456   retval = jtagarmtransn(0L, 32, LSB, END, RETIDLE);   // atmel arm jtag docs pp.10-11: LSB first
457   //debughex32(retval);
458   return(retval);   // atmel arm jtag docs pp.10-11: LSB first
459   
460 }
461
462
463
464
465 /************************* ICEBreaker/EmbeddedICE Stuff ******************************/
466 //! Grab debug register
467 unsigned long jtagarm7tdmi_get_dbgstate() {       // ICE Debug register, *NOT* ARM register DSCR    //    PROVEN
468   //jtagarm7tdmi_resettap();
469   return eice_read(EICE_DBGSTATUS);
470 }
471
472 //! Grab debug register
473 unsigned long jtagarm7tdmi_get_dbgctrl() {
474   return eice_read(EICE_DBGCTRL);
475 }
476
477 //! Update debug register
478 unsigned long jtagarm7tdmi_set_dbgctrl(unsigned long bits) {
479   return eice_write(EICE_DBGCTRL, bits);
480 }
481
482
483
484 //!  Set and Enable Watchpoint 0
485 void jtagarm7tdmi_set_watchpoint0(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
486   // store watchpoint info?  - not right now
487     // FIXME: store info
488
489   eice_write(EICE_WP0ADDR, addr);           // write 0 in watchpoint 0 address
490   eice_write(EICE_WP0ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 0 address mask
491   eice_write(EICE_WP0DATA, data);           // write 0 in watchpoint 0 data
492   eice_write(EICE_WP0DATAMASK, datamask);   // write 0xffffffff in watchpoint 0 data mask
493   eice_write(EICE_WP0CTRL, ctrlmask);       // write 0x00000100 in watchpoint 0 control value register (enables watchpoint)
494   eice_write(EICE_WP0CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 0 control mask - only detect the fetch instruction
495 }
496
497 //!  Set and Enable Watchpoint 1
498 void jtagarm7tdmi_set_watchpoint1(unsigned long addr, unsigned long addrmask, unsigned long data, unsigned long datamask, unsigned long ctrl, unsigned long ctrlmask){
499   // store watchpoint info?  - not right now
500     // FIXME: store info
501
502   eice_write(EICE_WP1ADDR, addr);           // write 0 in watchpoint 1 address
503   eice_write(EICE_WP1ADDRMASK, addrmask);   // write 0xffffffff in watchpoint 1 address mask
504   eice_write(EICE_WP1DATA, data);           // write 0 in watchpoint 1 data
505   eice_write(EICE_WP1DATAMASK, datamask);   // write 0xffffffff in watchpoint 1 data mask
506   eice_write(EICE_WP1CTRL, ctrl);           // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
507   eice_write(EICE_WP1CTRLMASK, ctrlmask);   // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
508 }
509
510 //!  Disable Watchpoint 0
511 void jtagarm7tdmi_disable_watchpoint0(){
512   eice_write(EICE_WP0CTRL, 0x0L); // write 0 in watchpoint 0 control value - disables watchpoint 0
513 }
514   
515 //!  Disable Watchpoint 1
516 void jtagarm7tdmi_disable_watchpoint1(){
517   eice_write(EICE_WP1CTRL, 0x0L);            // write 0 in watchpoint 0 control value - disables watchpoint 0
518 }
519
520
521
522 /******************** Complex Commands **************************/
523
524 //! Push an instruction into the CPU pipeline
525 //  NOTE!  Must provide EXECNOPARM for parameter if no parm is required.
526 unsigned long jtagarm7tdmi_exec(unsigned long instr, unsigned long parameter, unsigned char systemspeed) {
527   unsigned long retval,waitcount=0xff;
528
529   debughex32(jtagarm7tdmi_nop( 0));
530   debughex32(jtagarm7tdmi_nop(systemspeed));
531   debughex32(jtagarm7tdmi_instr_primitive(instr, 0));      // write 32-bit instruction code into DR
532   debughex32(jtagarm7tdmi_nop( 0));
533   if (systemspeed){
534     jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
535
536     // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
537     while ((jtagarm7tdmi_get_dbgstate() & 9L) == 0  && waitcount > 0){
538       delay(1);
539       waitcount --;
540     }
541     if (waitcount == 0)
542       return (-1);
543     retval = 0x12345678;
544   } else {
545     debughex32(jtagarm7tdmi_nop( 0));
546     debughex32(jtagarm7tdmi_instr_primitive(parameter, 0));  // inject long
547     retval = jtagarm7tdmi_nop( 0);
548     debughex32(retval);
549     debughex32(jtagarm7tdmi_nop( 0));
550     debughex32(jtagarm7tdmi_nop( 0));
551   }
552   return(retval);
553 }
554
555 //! Retrieve a 32-bit Register value
556 unsigned long jtagarm7tdmi_get_register(unsigned long reg) {
557   unsigned long retval=0L, instr;
558   if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
559     instr = THUMB_INSTR_STR_R0_r0 | reg | (reg<<16);
560   else
561     instr = (unsigned long)(reg<<12L) | (unsigned long)ARM_READ_REG;   // STR Rx, [R14] 
562
563   jtagarm7tdmi_nop( 0);
564   jtagarm7tdmi_nop( 0);
565   jtagarm7tdmi_instr_primitive(instr, 0);
566   jtagarm7tdmi_nop( 0);
567   jtagarm7tdmi_nop( 0);
568   jtagarm7tdmi_nop( 0);
569   retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
570   return retval;
571 }
572
573 //! Set a 32-bit Register value
574 void jtagarm7tdmi_set_register(unsigned long reg, unsigned long val) {
575   unsigned long instr;
576   instr = (unsigned long)(((unsigned long)reg<<12L) | ARM_WRITE_REG); //  LDR Rx, [R14]
577   
578   jtagarm7tdmi_nop( 0);            // push nop into pipeline - clean out the pipeline...
579   jtagarm7tdmi_nop( 0);            // push nop into pipeline - clean out the pipeline...
580   jtagarm7tdmi_instr_primitive(instr, 0); // push instr into pipeline - fetch
581   if (reg == ARM_REG_PC){
582     jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
583     jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
584     jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
585   } else {
586     jtagarm7tdmi_nop( 0);            // push nop into pipeline - decode
587     jtagarm7tdmi_nop( 0);            // push nop into pipeline - execute
588     jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus
589   }
590   jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
591   jtagarm7tdmi_nop( 0);            // push nop into pipeline - executed 
592   jtagarm7tdmi_nop( 0);
593 }
594
595
596
597 //! Get all registers, placing them into cmddatalong[0-14]
598 void jtagarm7tdmi_get_registers() {
599   jtagarm7tdmi_nop( 0);
600   jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0);
601   jtagarm7tdmi_nop( 0);
602   jtagarm7tdmi_nop( 0);
603   cmddatalong[ 0] = jtagarm7tdmi_nop( 0);
604   cmddatalong[ 1] = jtagarm7tdmi_nop( 0);
605   cmddatalong[ 2] = jtagarm7tdmi_nop( 0);
606   cmddatalong[ 3] = jtagarm7tdmi_nop( 0);
607   cmddatalong[ 4] = jtagarm7tdmi_nop( 0);
608   cmddatalong[ 5] = jtagarm7tdmi_nop( 0);
609   cmddatalong[ 6] = jtagarm7tdmi_nop( 0);
610   cmddatalong[ 7] = jtagarm7tdmi_nop( 0);
611   cmddatalong[ 8] = jtagarm7tdmi_nop( 0);
612   cmddatalong[ 9] = jtagarm7tdmi_nop( 0);
613   cmddatalong[10] = jtagarm7tdmi_nop( 0);
614   cmddatalong[11] = jtagarm7tdmi_nop( 0);
615   cmddatalong[12] = jtagarm7tdmi_nop( 0);
616   cmddatalong[13] = jtagarm7tdmi_nop( 0);
617   cmddatalong[14] = jtagarm7tdmi_nop( 0);
618   cmddatalong[15] = jtagarm7tdmi_nop( 0);
619   jtagarm7tdmi_nop( 0);
620 }
621
622 //! Set all registers from cmddatalong[0-14]
623 void jtagarm7tdmi_set_registers() {   // using r15 to write through.  not including it.  use set_pc
624   jtagarm7tdmi_nop( 0);
625   debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_CLOBBEREGS,0));
626   jtagarm7tdmi_nop( 0);
627   jtagarm7tdmi_nop( 0);
628   jtagarm7tdmi_instr_primitive(cmddatalong[0],0);
629   jtagarm7tdmi_instr_primitive(cmddatalong[1],0);
630   jtagarm7tdmi_instr_primitive(cmddatalong[2],0);
631   jtagarm7tdmi_instr_primitive(cmddatalong[3],0);
632   jtagarm7tdmi_instr_primitive(cmddatalong[4],0);
633   jtagarm7tdmi_instr_primitive(cmddatalong[5],0);
634   jtagarm7tdmi_instr_primitive(cmddatalong[6],0);
635   jtagarm7tdmi_instr_primitive(cmddatalong[7],0);
636   jtagarm7tdmi_instr_primitive(cmddatalong[8],0);
637   jtagarm7tdmi_instr_primitive(cmddatalong[9],0);
638   jtagarm7tdmi_instr_primitive(cmddatalong[10],0);
639   jtagarm7tdmi_instr_primitive(cmddatalong[11],0);
640   jtagarm7tdmi_instr_primitive(cmddatalong[12],0);
641   jtagarm7tdmi_instr_primitive(cmddatalong[13],0);
642   jtagarm7tdmi_instr_primitive(cmddatalong[14],0);
643   jtagarm7tdmi_nop( 0);
644 }
645
646 //! Retrieve the CPSR Register value
647 unsigned long jtagarm7tdmi_get_regCPSR() {
648   unsigned long retval = 0L, r0;
649
650   r0 = jtagarm7tdmi_get_register(0);
651   jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
652   jtagarm7tdmi_instr_primitive(ARM_INSTR_MRS_R0_CPSR, 0); // push MRS_R0, CPSR into pipeline - fetch
653   jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded
654   jtagarm7tdmi_nop( 0); // push nop into pipeline - execute
655   retval = jtagarm7tdmi_get_register(0);
656   jtagarm7tdmi_set_register(0, r0);
657   return retval;
658 }
659
660 //! Retrieve the CPSR Register value
661 unsigned long jtagarm7tdmi_set_regCPSR(unsigned long val) {
662   unsigned long r0;
663
664   r0 = jtagarm7tdmi_get_register(0);
665   jtagarm7tdmi_set_register(0, val);
666   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - clean out the pipeline...
667   debughex32(jtagarm7tdmi_instr_primitive(ARM_INSTR_MSR_cpsr_cxsf_R0, 0)); // push MSR cpsr_cxsf, R0 into pipeline - fetch
668   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - decoded
669   debughex32(jtagarm7tdmi_nop( 0));        // push nop into pipeline - execute
670   jtagarm7tdmi_set_register(0, r0);
671   return(val);
672 }
673
674 //! Write data to address - Assume TAP in run-test/idle state
675 unsigned long jtagarm7tdmi_writemem(unsigned long adr, unsigned long data){
676   unsigned long retval = 0xffL;
677   unsigned long r0=0L, r1=-1L;
678
679   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
680   r1 = jtagarm7tdmi_get_register(1);
681   jtagarm7tdmi_set_register(0, adr);        // write address into R0
682   jtagarm7tdmi_set_register(1, data);       // write data in R1
683   debughex32(jtagarm7tdmi_get_register(0));
684   debughex32(jtagarm7tdmi_get_register(1));
685   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
686   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
687   jtagarm7tdmi_instr_primitive(ARM_INSTR_STR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline
688   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
689   jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
690
691   // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
692   current_dbgstate = jtagarm7tdmi_get_dbgstate();
693   while ((!(current_dbgstate & 9L) == 9)  && retval > 0){
694     delay(1);
695     retval --;
696     current_dbgstate = jtagarm7tdmi_get_dbgstate();
697   }
698   if (retval == 0){
699     debugstr("FAILED TO WRITE MEMORY/RE-ENTER DEBUG MODE");
700     return (-1);
701   } else {
702     retval = jtagarm7tdmi_get_register(1);  // read memory value from R1 register
703     jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1 
704     jtagarm7tdmi_set_register(0, r0);
705   }
706   return retval;
707 }
708
709
710
711
712 //! Read data from address
713 unsigned long jtagarm7tdmi_readmem(unsigned long adr){
714   unsigned long retval = 0xffL;
715   unsigned long r0=0L, r1=-1L;
716
717   r0 = jtagarm7tdmi_get_register(0);        // store R0 and R1
718   r1 = jtagarm7tdmi_get_register(1);
719   jtagarm7tdmi_set_register(0, adr);        // write address into R0
720   jtagarm7tdmi_nop( 0);                     // push nop into pipeline to "clean" it ???
721   jtagarm7tdmi_nop( 1);                     // push nop into pipeline with BREAKPT set
722   jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, [R0], #4 into instruction pipeline  (autoincrements for consecutive reads)
723   jtagarm7tdmi_nop( 0);                     // push nop into pipeline
724   jtagarm7tdmi_restart();                   // SHIFT_IR with RESTART instruction
725
726   // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH
727   current_dbgstate = jtagarm7tdmi_get_dbgstate();
728   debughex(current_dbgstate);
729   while ((!(current_dbgstate & 9L) == 9)  && retval > 0){
730     delay(1);
731     retval --;
732     current_dbgstate = jtagarm7tdmi_get_dbgstate();
733   }
734   // FIXME: this may end up changing te current debug-state.  should we compare to current_dbgstate?
735   if (retval == 0){
736     debugstr("FAILED TO READ MEMORY/RE-ENTER DEBUG MODE");
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   unsigned long val;
750   val = jtagarm7tdmi_get_register(ARM_REG_PC);
751   if (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT)
752     val -= (4*2);                           // thumb uses 2 bytes per instruction.
753   else
754     val -= (6*4);                           // assume 6 instructions at 4 bytes a piece.
755   return val;
756 }
757
758 //! Set Program Counter - if setting it to non-word-aligned anything, crap may not like you.  you've been warned
759 void jtagarm7tdmi_setpc(unsigned long adr){
760   jtagarm7tdmi_set_register(ARM_REG_PC, adr);
761 }
762
763 //! Halt CPU - returns 0xffff if the operation fails to complete within 
764 unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
765   int waitcount = 0xfffL;
766
767   // store the debug state
768   last_halt_debug_state = jtagarm7tdmi_get_dbgstate();
769   // store watchpoint info?  - not right now
770   eice_write(EICE_WP1ADDR, 0L);              // write 0 in watchpoint 1 address
771   eice_write(EICE_WP1ADDRMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 address mask
772   eice_write(EICE_WP1DATA, 0L);              // write 0 in watchpoint 1 data
773   eice_write(EICE_WP1DATAMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 data mask
774   eice_write(EICE_WP1CTRL, 0x100L);          // write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
775   eice_write(EICE_WP1CTRLMASK, 0xfffffff7); // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
776
777   // poll until debug status says the cpu is in debug mode
778   while (!(current_dbgstate & 0x1L)   && waitcount-- > 0){
779     current_dbgstate = jtagarm7tdmi_get_dbgstate();
780     delay(1);
781   }
782
783   eice_write(EICE_WP1CTRL, 0x0L);            // write 0 in watchpoint 0 control value - disables watchpoint 0
784
785   // store the debug state program counter.
786   last_halt_pc = jtagarm7tdmi_getpc();
787   count_dbgspd_instr_since_debug = 0L;          // should be able to clean this up and remove all this tracking nonsense.
788   count_sysspd_instr_since_debug = 0L;          // should be able to clean this up and remove all this tracking nonsense.
789
790   //FIXME: is this necessary?  for now, yes... but perhaps make the rest of the module arm/thumb impervious.
791   // get into ARM mode if the T flag is set (Thumb mode)
792   while (current_dbgstate & JTAG_ARM7TDMI_DBG_TBIT && waitcount-- > 0) {
793     jtagarm7tdmi_setMode_ARM(0);
794     current_dbgstate = jtagarm7tdmi_get_dbgstate();
795   }
796   jtagarm7tdmi_resettap();
797   jtagarm7tdmi_set_register(ARM_REG_PC, last_halt_pc & 0xfffffffc);     // make sure PC is word-aligned.  otherwise all other register accesses get all wonky.
798   return waitcount;
799 }
800
801 unsigned long jtagarm7tdmi_releasecpu(){
802   int waitcount = 0xfff;
803   unsigned long instr;
804   // somehow determine what PC should be (a couple ways possible, calculations required)
805   jtagarm7tdmi_nop(0);                          // NOP
806   jtagarm7tdmi_nop(1);                          // NOP/BREAKPT
807
808
809   // four possible states.  arm mode needing arm mode, arm mode needing thumb mode, thumb mode needing arm mode, and thumb mode needing thumb mode
810   // FIXME:  BX is bs.  it requires the clobbering of at least one register.... this is not acceptable.  
811   // FIXME:  so we either switch modes, then correct the register before restarting with bx, or find the way to use SPSR
812   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?
813     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?
814     jtagarm7tdmi_instr_primitive(instr,0);
815   } else {
816     instr = ARM_INSTR_B_PC + 0x1000000 - (count_dbgspd_instr_since_debug*4) - (count_sysspd_instr_since_debug*12);
817     jtagarm7tdmi_instr_primitive(instr,0);
818   }
819
820   jtagarm7tdmi_restart();
821   //SHIFT_IR;
822   //jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); // VERB_RESTART
823
824   // wait until restart-bit set in debug state register
825   while ((current_dbgstate & JTAG_ARM7TDMI_DBG_DBGACK) && waitcount > 0){
826     msdelay(1);
827     waitcount --;
828     current_dbgstate = jtagarm7tdmi_get_dbgstate();
829   }
830   last_halt_debug_state = -1;
831   last_halt_pc = -1;
832   return 0;
833 }
834  
835
836
837
838 ///////////////////////////////////////////////////////////////////////////////////////////////////
839 //! Handles ARM7TDMI JTAG commands.  Forwards others to JTAG.
840 void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len){
841   register char blocks;
842   
843   unsigned int i,val;
844   unsigned long at;
845   current_dbgstate = jtagarm7tdmi_get_dbgstate();
846   
847   jtagarm7tdmi_resettap();
848  
849   switch(verb){
850   case START:
851     //Enter JTAG mode.
852     debughex32(jtagarm7tdmi_start());
853     debughex32(jtagarm7tdmi_haltcpu());
854     //jtagarm7tdmi_resettap();
855     cmddatalong[0] = jtagarm7tdmi_get_dbgstate();
856     txdata(app,verb,0x36);
857     break;
858   case JTAGARM7TDMI_READMEM:
859     at     = cmddatalong[0];
860     blocks = cmddatalong[1];
861     
862     txhead(app,verb,len);
863     
864         jtagarm7tdmi_resettap();
865         delay(1);
866         
867     for(i=0;i<blocks;i++){
868           val=jtagarm7tdmi_readmem(at);
869                 
870           serial_tx(val&0xFFL);
871           serial_tx((val&0xFF00L)>>8);
872           serial_tx((val&0xFF0000L)>>8);
873           serial_tx((val&0xFF000000L)>>8);
874           at+=4;
875       }
876     
877     
878     break;
879   case PEEK:
880         jtagarm7tdmi_resettap();
881         delay(1);
882         cmddatalong[0] = jtagarm7tdmi_readmem(cmddatalong[0]);
883     txdata(app,verb,4);
884     break;
885   case JTAGARM7TDMI_GET_CHIP_ID:
886         jtagarm7tdmi_resettap();
887     cmddatalong[0] = jtagarm7tdmi_idcode();
888     txdata(app,verb,4);
889     break;
890
891
892   case JTAGARM7TDMI_WRITEMEM:
893   case POKE:
894         jtagarm7tdmi_resettap();
895     jtagarm7tdmi_writemem(cmddatalong[0],
896                        cmddataword[2]);
897     cmddataword[0]=jtagarm7tdmi_readmem(cmddatalong[0]);
898     txdata(app,verb,4);
899     break;
900
901   case JTAGARM7TDMI_HALTCPU:  
902     cmddatalong[0] = jtagarm7tdmi_haltcpu();
903     txdata(app,verb,4);
904     break;
905   case JTAGARM7TDMI_RELEASECPU:
906         jtagarm7tdmi_resettap();
907     cmddatalong[0] = jtagarm7tdmi_releasecpu();
908     txdata(app,verb,4);
909     break;
910   //unimplemented functions
911   //case JTAGARM7TDMI_SETINSTRFETCH:
912   //case JTAGARM7TDMI_WRITEFLASH:
913   //case JTAGARM7TDMI_ERASEFLASH:
914   case JTAGARM7TDMI_SET_PC:
915     jtagarm7tdmi_setpc(cmddatalong[0]);
916     txdata(app,verb,0);
917     break;
918   case JTAGARM7TDMI_GET_DEBUG_CTRL:
919     cmddatalong[0] = jtagarm7tdmi_get_dbgctrl();
920     txdata(app,verb,1);
921     break;
922   case JTAGARM7TDMI_SET_DEBUG_CTRL:
923     cmddatalong[0] = jtagarm7tdmi_set_dbgctrl(cmddata[0]);
924     txdata(app,verb,4);
925     break;
926   case JTAGARM7TDMI_GET_PC:
927     cmddatalong[0] = jtagarm7tdmi_getpc();
928     txdata(app,verb,4);
929     break;
930   case JTAGARM7TDMI_GET_DEBUG_STATE:
931     //jtagarm7tdmi_resettap();            // Shouldn't need this, but currently do.  FIXME!
932     cmddatalong[0] = current_dbgstate;
933     txdata(app,verb,4);
934     break;
935   //case JTAGARM7TDMI_GET_WATCHPOINT:
936   //case JTAGARM7TDMI_SET_WATCHPOINT:
937   case JTAGARM7TDMI_GET_REGISTER:
938         jtagarm7tdmi_resettap();
939     val = cmddata[0];
940     cmddatalong[0] = jtagarm7tdmi_get_register(val);
941     txdata(app,verb,4);
942     break;
943   case JTAGARM7TDMI_SET_REGISTER:
944         jtagarm7tdmi_resettap();
945     jtagarm7tdmi_set_register(cmddatalong[1], cmddatalong[0]);
946     txdata(app,verb,4);
947     break;
948   case JTAGARM7TDMI_GET_REGISTERS:
949         jtagarm7tdmi_resettap();
950     jtagarm7tdmi_get_registers();
951     txdata(app,verb,64);
952     break;
953   case JTAGARM7TDMI_SET_REGISTERS:
954         jtagarm7tdmi_resettap();
955     jtagarm7tdmi_set_registers();
956     txdata(app,verb,64);
957     break;
958   case JTAGARM7TDMI_DEBUG_INSTR:
959         jtagarm7tdmi_resettap();
960     cmddataword[0] = jtagarm7tdmi_exec(cmddataword[0], cmddataword[1], cmddata[9]);
961     txdata(app,verb,80);
962     break;
963   //case JTAGARM7TDMI_STEP_INSTR:
964 /*  case JTAGARM7TDMI_READ_CODE_MEMORY:
965   case JTAGARM7TDMI_WRITE_FLASH_PAGE:
966   case JTAGARM7TDMI_READ_FLASH_PAGE:
967   case JTAGARM7TDMI_MASS_ERASE_FLASH:
968   case JTAGARM7TDMI_PROGRAM_FLASH:
969   case JTAGARM7TDMI_LOCKCHIP:
970   case JTAGARM7TDMI_CHIP_ERASE:
971   */
972 // Really ARM specific stuff
973   case JTAGARM7TDMI_GET_CPSR:
974         jtagarm7tdmi_resettap();
975     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
976     txdata(app,verb,4);
977     break;
978   case JTAGARM7TDMI_SET_CPSR:
979         jtagarm7tdmi_resettap();
980     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
981     txdata(app,verb,4);
982     break;
983   case JTAGARM7TDMI_GET_SPSR:           // FIXME: NOT CORRECT
984         jtagarm7tdmi_resettap();
985     cmddatalong[0] = jtagarm7tdmi_get_regCPSR();
986     txdata(app,verb,4);
987     break;
988   case JTAGARM7TDMI_SET_SPSR:           // FIXME: NOT CORRECT
989         jtagarm7tdmi_resettap();
990     cmddatalong[0] = jtagarm7tdmi_set_regCPSR(cmddatalong[0]);
991     txdata(app,verb,4);
992     break;
993   case JTAGARM7TDMI_SET_MODE_THUMB:
994   case JTAGARM7TDMI_SET_MODE_ARM:
995         jtagarm7tdmi_resettap();
996     cmddataword[0] = jtagarm7tdmi_setMode_ARM(0);
997     txdata(app,verb,4);
998     break;
999     
1000   case 0xD0:          // loopback test
1001     jtagarm7tdmi_resettap();
1002     cmddatalong[0] = jtagarm7tdmi_bypass(cmddatalong[0]);
1003     txdata(app,verb,4);
1004     break;
1005   case 0xD8:          // EICE_READ
1006     jtagarm7tdmi_resettap();
1007     cmddatalong[0] = eice_read(cmddatalong[0]);
1008     txdata(app,verb,4);
1009     break;
1010   case 0xD9:          // EICE_WRITE
1011     jtagarm7tdmi_resettap();
1012     cmddatalong[0] = eice_write(cmddatalong[0], cmddatalong[1]);
1013     txdata(app,verb,4);
1014     break;
1015   case 0xDA:          // TEST MSB THROUGH CHAIN0 and CHAIN1
1016     jtagarm7tdmi_resettap();
1017     jtagarm7tdmi_scan_intest(0);
1018     cmddatalong[0] = jtagarmtransn(0x41414141, 32, LSB, NOEND, NORETIDLE);
1019     cmddatalong[1] = jtagarmtransn(0x42424242, 32, MSB, NOEND, NORETIDLE);
1020     cmddatalong[2] = jtagarmtransn(0x43434343,  9, MSB, NOEND, NORETIDLE);
1021     cmddatalong[3] = jtagarmtransn(0x44444444, 32, MSB, NOEND, NORETIDLE);
1022     cmddatalong[4] = jtagarmtransn(cmddatalong[0], 32, LSB, NOEND, NORETIDLE);
1023     cmddatalong[5] = jtagarmtransn(cmddatalong[1], 32, MSB, NOEND, NORETIDLE);
1024     cmddatalong[6] = jtagarmtransn(cmddatalong[2],  9, MSB, NOEND, NORETIDLE);
1025     cmddatalong[7] = jtagarmtransn(cmddatalong[3], 32, MSB, END, RETIDLE);
1026     jtagarm7tdmi_resettap();
1027     jtagarm7tdmi_scan_intest(1);
1028     cmddatalong[8] = jtagarmtransn(0x41414141, 32, MSB, NOEND, NORETIDLE);
1029     cmddatalong[9] = jtagarmtransn(0x44444444,  1, MSB, NOEND, NORETIDLE);
1030     cmddatalong[10] = jtagarmtransn(cmddatalong[8], 32, MSB, NOEND, NORETIDLE);
1031     cmddatalong[11] = jtagarmtransn(cmddatalong[9],  1, MSB, END, RETIDLE);
1032     jtagarm7tdmi_resettap();
1033     txdata(app,verb,48);
1034     break;
1035     
1036   default:
1037     jtaghandle(app,verb,len);
1038   }
1039 }
1040
1041
1042
1043
1044 /*****************************
1045 Captured from FlySwatter against AT91SAM7S, to be used by me for testing.  ignore
1046
1047 > arm reg
1048 System and User mode registers
1049       r0: 300000df       r1: 00000000       r2: 58000000       r3: 00200a75
1050       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1051       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1052      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 000000fc
1053     cpsr: 00000093
1054
1055 FIQ mode shadow registers
1056   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1057  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1058
1059 Supervisor mode shadow registers
1060   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1061
1062 Abort mode shadow registers
1063   sp_abt: 00000000   lr_abt: 00000000 spsr_abt: e00000ff
1064
1065 IRQ mode shadow registers
1066   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1067
1068 Undefined instruction mode shadow registers
1069   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1070
1071 > step;arm reg
1072 target state: halted
1073 target halted in ARM state due to single-step, current mode: Supervisor
1074 cpsr: 0x00000093 pc: 0x00000100
1075 System and User mode registers
1076       r0: 300000df       r1: 00000000       r2: 00200000       r3: 00200a75 
1077       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c 
1078       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000 
1079      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000100 
1080     cpsr: 00000093 
1081
1082 FIQ mode shadow registers
1083   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000 
1084  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb 
1085
1086 Supervisor mode shadow registers
1087   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3 
1088
1089 Abort mode shadow registers
1090   sp_abt: 00000000   lr_abt: 00000000 spsr_abt: e00000ff 
1091
1092 IRQ mode shadow registers
1093   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b 
1094
1095 Undefined instruction mode shadow registers
1096   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df 
1097
1098  step;arm reg
1099 target state: halted
1100 target halted in ARM state due to single-step, current mode: Abort
1101 cpsr: 0x00000097 pc: 0x00000010
1102 System and User mode registers
1103       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75 
1104       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c 
1105       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000 
1106      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010 
1107     cpsr: 00000097 
1108
1109 FIQ mode shadow registers
1110   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000 
1111  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb 
1112
1113 Supervisor mode shadow registers
1114   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3 
1115
1116 Abort mode shadow registers
1117   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093 
1118
1119 IRQ mode shadow registers
1120   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b 
1121
1122 Undefined instruction mode shadow registers
1123   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df 
1124 > step;arm reg
1125 target state: halted
1126 target halted in ARM state due to single-step, current mode: Abort
1127 cpsr: 0x00000097 pc: 0x00000010
1128 System and User mode registers
1129       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75 
1130       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c 
1131       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000 
1132      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010 
1133     cpsr: 00000097 
1134
1135 FIQ mode shadow registers
1136   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000 
1137  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb 
1138
1139 Supervisor mode shadow registers
1140   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3 
1141
1142 Abort mode shadow registers
1143   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093 
1144
1145 IRQ mode shadow registers
1146   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b 
1147
1148 Undefined instruction mode shadow registers
1149   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df 
1150 > step;arm reg
1151 target state: halted
1152 target halted in ARM state due to single-step, current mode: Abort
1153 cpsr: 0x00000097 pc: 0x00000010
1154 System and User mode registers
1155       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1156       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1157       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1158      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1159     cpsr: 00000097
1160
1161 FIQ mode shadow registers
1162   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1163  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1164
1165 Supervisor mode shadow registers
1166   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1167
1168 Abort mode shadow registers
1169   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1170
1171 IRQ mode shadow registers
1172   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1173
1174 Undefined instruction mode shadow registers
1175   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1176 > step;arm reg
1177 target state: halted
1178 target halted in ARM state due to single-step, current mode: Abort
1179 cpsr: 0x00000097 pc: 0x00000010
1180 System and User mode registers
1181       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1182       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1183       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1184      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1185     cpsr: 00000097
1186
1187 FIQ mode shadow registers
1188   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1189  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1190
1191 Supervisor mode shadow registers
1192   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1193
1194 Abort mode shadow registers
1195   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1196
1197 IRQ mode shadow registers
1198   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1199
1200 Undefined instruction mode shadow registers
1201   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1202 > step;arm reg
1203 target state: halted
1204 target halted in ARM state due to single-step, current mode: Abort
1205 cpsr: 0x00000097 pc: 0x00000010
1206 System and User mode registers
1207       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1208       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1209       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1210      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1211     cpsr: 00000097
1212
1213 FIQ mode shadow registers
1214   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1215  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1216
1217 Supervisor mode shadow registers
1218   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1219
1220 Abort mode shadow registers
1221   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1222
1223 IRQ mode shadow registers
1224   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1225
1226 Undefined instruction mode shadow registers
1227   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1228 > step;arm reg
1229 target state: halted
1230 target halted in ARM state due to single-step, current mode: Abort
1231 cpsr: 0x00000097 pc: 0x00000010
1232 System and User mode registers
1233       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1234       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1235       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1236      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1237     cpsr: 00000097
1238
1239 FIQ mode shadow registers
1240   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1241  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1242
1243 Supervisor mode shadow registers
1244   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1245
1246 Abort mode shadow registers
1247   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1248
1249 IRQ mode shadow registers
1250   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1251
1252 Undefined instruction mode shadow registers
1253   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1254 > step;arm reg
1255 target state: halted
1256 target halted in ARM state due to single-step, current mode: Abort
1257 cpsr: 0x00000097 pc: 0x00000010
1258 System and User mode registers
1259       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1260       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1261       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1262      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1263     cpsr: 00000097
1264
1265 FIQ mode shadow registers
1266   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1267  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1268
1269 Supervisor mode shadow registers
1270   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1271
1272 Abort mode shadow registers
1273   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1274
1275 IRQ mode shadow registers
1276   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1277
1278 Undefined instruction mode shadow registers
1279   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1280 > step;arm reg
1281 target state: halted
1282 target halted in ARM state due to single-step, current mode: Abort
1283 cpsr: 0x00000097 pc: 0x00000010
1284 System and User mode registers
1285       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1286       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1287       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1288      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1289     cpsr: 00000097
1290
1291 FIQ mode shadow registers
1292   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1293  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1294
1295 Supervisor mode shadow registers
1296   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1297
1298 Abort mode shadow registers
1299   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1300
1301 IRQ mode shadow registers
1302   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1303
1304 Undefined instruction mode shadow registers
1305   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1306 > step;arm reg
1307 target state: halted
1308 target halted in ARM state due to single-step, current mode: Abort
1309 cpsr: 0x00000097 pc: 0x00000010
1310 System and User mode registers
1311       r0: 300000e3       r1: 00000000       r2: 00200000       r3: 00200a75
1312       r4: fffb0000       r5: 00000002       r6: 00000000       r7: 00200f6c
1313       r8: 00000000       r9: 00000000      r10: ffffffff      r11: 00000000
1314      r12: 00000009   sp_usr: 00000000   lr_usr: 00000000       pc: 00000010
1315     cpsr: 00000097
1316
1317 FIQ mode shadow registers
1318   r8_fiq: 00000000   r9_fiq: fffcc000  r10_fiq: fffff400  r11_fiq: fffff000
1319  r12_fiq: 00200f44   sp_fiq: 00000000   lr_fiq: 00000000 spsr_fiq: f00000fb
1320
1321 Supervisor mode shadow registers
1322   sp_svc: 00201f78   lr_svc: 00200a75 spsr_svc: 400000b3
1323
1324 Abort mode shadow registers
1325   sp_abt: 00000000   lr_abt: 00000108 spsr_abt: 00000093
1326
1327 IRQ mode shadow registers
1328   sp_irq: 00000000   lr_irq: 00000000 spsr_irq: f000003b
1329
1330 Undefined instruction mode shadow registers
1331   sp_und: 00000000   lr_und: 00000000 spsr_und: 300000df
1332 >
1333 */