X-Git-Url: http://git.rot13.org/?p=goodfet;a=blobdiff_plain;f=firmware%2Fapps%2Fjtag%2Fjtagarm7tdmi.c;h=176ba0bb3cea33b36ff949ad9585813789b12313;hp=73f560bb2f48b7c8cc4b00b30ed8d5fb598e1220;hb=5c2a6eba496141f211f252be1d82d5964b23115b;hpb=e1fac2e62056841eeb482a001563ce981dd54d36

diff --git a/firmware/apps/jtag/jtagarm7tdmi.c b/firmware/apps/jtag/jtagarm7tdmi.c
index 73f560b..176ba0b 100644
--- a/firmware/apps/jtag/jtagarm7tdmi.c
+++ b/firmware/apps/jtag/jtagarm7tdmi.c
@@ -126,18 +126,30 @@ void jtag_arm_tcktock() {
   PLEDOUT^=PLEDPIN;
 }
 
+//! Set up the pins for JTAG mode.
+void armjtagsetup(){
+  P5DIR|=MOSI+SCK+TMS;
+  P5DIR&=~MISO;
+  P5OUT|=0xFFFF;
+  P5OUT=0;
+  P4DIR|=TST;
+  P2DIR|=RST;
+  msdelay(10);
+}
+
+
 // ! Start JTAG, setup pins, reset TAP and return IDCODE
 unsigned long jtagarm7tdmi_start() {
-  jtagsetup();
+  armjtagsetup();
   //Known-good starting position.
   //Might be unnecessary.
   SETTST;
   SETRST;
   
-  delay(0xF);
+  delay(0x2);
   
   CLRRST;
-  delay(20);
+  delay(2);
   CLRTST;
 
   msdelay(10);
@@ -145,7 +157,7 @@ unsigned long jtagarm7tdmi_start() {
   /*
   P5DIR &=~RST;
   */
-  delay(0xF);
+  delay(0x2);
   jtagarm7tdmi_resettap();
   return jtagarm7tdmi_idcode();
 }
@@ -167,9 +179,9 @@ unsigned long jtagarmtransn(unsigned long word, unsigned char bitcount, unsigned
   unsigned long high = 1;
   unsigned long mask;
 
-  for (bit=(bitcount-1)/16; bit>0; bit--)
-    high <<= 16;
-  high <<= ((bitcount-1)%16);
+  for (bit=(bitcount-1)/8; bit>0; bit--)
+    high <<= 8;
+  high <<= ((bitcount-1)%8);
 
   mask = high-1;
 
@@ -193,7 +205,7 @@ unsigned long jtagarmtransn(unsigned long word, unsigned char bitcount, unsigned
       }
     }
   } else {
-    for (bit=bitcount; bit>0; bit++) {
+    for (bit = bitcount; bit > 0; bit--) {
       /* write MOSI on trailing edge of previous clock */
       if (word & high)
         {SETMOSI;}
@@ -342,6 +354,7 @@ unsigned long jtagarm7tdmi_scan_intest(int chain) {               // PROVEN
 //! push an instruction into the pipeline  - Assumes scan-chain 1 is already INTEST
 unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){
   unsigned long retval;
+  //jtagarm7tdmi_resettap();                  // FIXME: DEBUG: seems necessary for some reason.  ugh.
   jtagarm7tdmi_scan_intest(1);
 
   SHIFT_DR;
@@ -367,6 +380,9 @@ unsigned long jtagarm7tdmi_instr_primitive(unsigned long instr, char breakpt){
 
 
 unsigned long jtagarm7tdmi_nop(char breakpt){
+  //jtagarm7tdmi_scan_intest(1);
+  //SHIFT_DR
+  //return jtagarmtransn(ARM_INSTR_NOP, 32, LSB, END, NORETIDLE);
   return jtagarm7tdmi_instr_primitive(ARM_INSTR_NOP, breakpt);
 }
 
@@ -392,7 +408,6 @@ unsigned long jtagarm7tdmi_setMode_ARM(){               // PROVEN
     cmddataword[4] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_BX_PC,0);
     cmddataword[5] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
     cmddataword[6] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0);
-    delay(10);
     jtagarm7tdmi_resettap();                  // seems necessary for some reason.  ugh.
   }
   return(retval);
@@ -557,6 +572,25 @@ unsigned long jtagarm7tdmi_get_register(unsigned char reg) {
   return retval;
 }
 
+//! Retrieve a 32-bit Register value
+unsigned long test_get_register(unsigned char reg) {
+  unsigned long retval = 0, instr;
+  // push nop into pipeline - clean out the pipeline...
+  cmddatalong[2] = jtagarm7tdmi_nop( 0);
+
+  instr = ARM_WRITE_REG | (reg<<12);                     // push STR Rx, [R14] into pipeline
+  cmddatalong[1] = jtagarm7tdmi_instr_primitive(instr, 0);
+  cmddatalong[2] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - fetched
+  cmddatalong[3] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - decoded
+  cmddatalong[4] = jtagarm7tdmi_nop( 0);                // push nop into pipeline - executed 
+  retval = jtagarm7tdmi_nop( 0);                        // recover 32-bit word
+  cmddatalong[5] = retval;
+  cmddatalong[6] = jtagarm7tdmi_nop( 0);
+  cmddatalong[7] = jtagarm7tdmi_nop( 0);
+  cmddatalong[8] = jtagarm7tdmi_nop( 0);
+  return retval;
+}
+
 //! Set a 32-bit Register value
 unsigned long jtagarm7tdmi_set_register(unsigned char reg, unsigned long val) {
   unsigned long retval = 0, instr;
@@ -580,6 +614,29 @@ unsigned long jtagarm7tdmi_set_register(unsigned char reg, unsigned long val) {
   return(retval);
 }
 
+//! Set a 32-bit Register value
+unsigned long test_set_register(unsigned char reg, unsigned long val) {
+  unsigned long retval = 0, instr;
+  cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline...
+
+  instr = ARM_READ_REG | (reg<<12);     // push LDR Rx, [R14] into pipeline
+  cmddatalong[2] = jtagarm7tdmi_instr_primitive(instr, 0);
+  
+  cmddatalong[3] = jtagarm7tdmi_instr_primitive(val+32, 0); // push 32-bit word on data bus - execute state
+  cmddatalong[4] = jtagarm7tdmi_instr_primitive(val+16, 0); // push 32-bit word on data bus - execute state
+  cmddatalong[5] = jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus - execute state
+  cmddatalong[6] = jtagarm7tdmi_instr_primitive(val-16, 0); // push 32-bit word on data bus - execute state
+
+  if (reg == ARM_REG_PC){
+    cmddatalong[7] = jtagarm7tdmi_nop( 0);
+    cmddatalong[8] = jtagarm7tdmi_nop( 0);
+  }
+  cmddatalong[9] = jtagarm7tdmi_instr_primitive(val-32, 0); // push 32-bit word on data bus - execute state
+
+  retval = cmddatalong[5];
+  return(retval);
+}
+
 
 
 //! Get all registers.  Return an array
@@ -676,12 +733,12 @@ unsigned long jtagarm7tdmi_readmem(unsigned long adr){
     delay(1);
     waitcount --;
   }
-  if (waitcount == 0xffff){
+  if (waitcount == 0){
     return (-1);
   } else {
     retval = jtagarm7tdmi_get_register(1);  // read memory value from R1 register
-  jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1 
-  jtagarm7tdmi_set_register(0, r0);
+    jtagarm7tdmi_set_register(1, r1);         // restore R0 and R1 
+    jtagarm7tdmi_set_register(0, r0);
   }
   return retval;
 }
@@ -711,7 +768,7 @@ unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
 
   // poll until debug status says the cpu is in debug mode
   while (!(jtagarm7tdmi_get_dbgstate() & 0x1)   && waitcount-- > 0){
-    delay(5);
+    delay(1);
   }
   eice_write(EICE_WP1CTRL, 0x0);            // write 0 in watchpoint 0 control value - disables watchpoint 0
 
@@ -725,6 +782,7 @@ unsigned long jtagarm7tdmi_haltcpu(){                   //  PROVEN
   while (jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT && waitcount-- > 0) {
     jtagarm7tdmi_setMode_ARM();
   }
+  jtagarm7tdmi_resettap();
   return waitcount;
 }
 
@@ -739,7 +797,7 @@ unsigned long jtagarm7tdmi_releasecpu(){
     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?
     jtagarm7tdmi_instr_primitive(instr,0);
   } else {
-    instr = ARM_INSTR_B_PC + 0x1000000 - (count_dbgspd_instr_since_debug) - (count_sysspd_instr_since_debug*3);  //FIXME: make this right  - can't we just do an absolute b/bx?
+    instr = ARM_INSTR_B_PC + 0x1000000 - (count_dbgspd_instr_since_debug*4) - (count_sysspd_instr_since_debug*12);
     jtagarm7tdmi_instr_primitive(instr,0);
   }
 
@@ -748,7 +806,7 @@ unsigned long jtagarm7tdmi_releasecpu(){
 
   // wait until restart-bit set in debug state register
   while ((jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_DBGACK) && waitcount > 0){
-    //delay(1);
+    msdelay(1);
     waitcount --;
   }
   last_halt_debug_state = -1;
@@ -764,7 +822,7 @@ unsigned long jtagarm7tdmi_releasecpu(){
 void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len){
   register char blocks;
   
-  unsigned int i,val;
+  unsigned int i,val,mlop;
   unsigned long at;
   
   jtagarm7tdmi_resettap();
@@ -776,7 +834,12 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
     cmddatalong[2] = jtagarm7tdmi_haltcpu();
     //jtagarm7tdmi_resettap();
     cmddatalong[1] = jtagarm7tdmi_get_dbgstate();
-
+    
+    // DEBUG: FIXME: NOT PART OF OPERATIONAL CODE
+    //for (mlop=2;mlop<4;mlop++){
+    //  jtagarm7tdmi_set_register(mlop, 0x43424140);
+    //} 
+    /////////////////////////////////////////////
     txdata(app,verb,0xc);
     break;
   case JTAGARM7TDMI_READMEM:
@@ -855,13 +918,15 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
   //case JTAGARM7TDMI_SET_WATCHPOINT:
   case JTAGARM7TDMI_GET_REGISTER:
 	jtagarm7tdmi_resettap();
-    cmddatalong[0] = jtagarm7tdmi_get_register(cmddata[0]);
+    //cmddatalong[0] = jtagarm7tdmi_get_register(cmddata[0]);
+    cmddatalong[0] = test_get_register(cmddata[0]);
     txdata(app,verb,96);
     break;
   case JTAGARM7TDMI_SET_REGISTER:
 	jtagarm7tdmi_resettap();
     cmddatalong[0] = cmddatalong[1];
-    jtagarm7tdmi_set_register(cmddata[0], cmddatalong[1]);
+    test_set_register(cmddata[0], cmddatalong[1]);
+    //jtagarm7tdmi_set_register(cmddata[0], cmddatalong[1]);
     txdata(app,verb,96);
     break;
   case JTAGARM7TDMI_GET_REGISTERS:
@@ -875,7 +940,7 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
     cmddataword[0] = jtagarm7tdmi_exec(cmddataword[0], cmddataword[1], cmddata[9]);
     txdata(app,verb,80);
     break;
-  case JTAGARM7TDMI_STEP_INSTR:
+  //case JTAGARM7TDMI_STEP_INSTR:
 /*  case JTAGARM7TDMI_READ_CODE_MEMORY:
   case JTAGARM7TDMI_WRITE_FLASH_PAGE:
   case JTAGARM7TDMI_READ_FLASH_PAGE:
@@ -944,6 +1009,7 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len
     cmddatalong[9] = jtagarmtransn(0x44444444,  1, MSB, NOEND, NORETIDLE);
     cmddatalong[10] = jtagarmtransn(cmddatalong[8], 32, MSB, NOEND, NORETIDLE);
     cmddatalong[11] = jtagarmtransn(cmddatalong[9],  1, MSB, END, RETIDLE);
+    jtagarm7tdmi_resettap();
     txdata(app,verb,48);
     break;