X-Git-Url: http://git.rot13.org/?p=goodfet;a=blobdiff_plain;f=firmware%2Fapps%2Fjtag%2Fjtagarm7tdmi.c;h=adbfaa12a12f402c05c5d367d7141eb9bf61f1ca;hp=0bf811c1115c2664312e98f7dff2a7ccad390de7;hb=878e4212e36ef51e882ef9fc7d936ce8949127d2;hpb=8203dc42156b3745f4f79ba7da4845da365902ff diff --git a/firmware/apps/jtag/jtagarm7tdmi.c b/firmware/apps/jtag/jtagarm7tdmi.c index 0bf811c..adbfaa1 100644 --- a/firmware/apps/jtag/jtagarm7tdmi.c +++ b/firmware/apps/jtag/jtagarm7tdmi.c @@ -34,6 +34,11 @@ GoodFET -> 7TDMI 14-pin connector http://hri.sourceforge.net/tools/jtag_faq_org.html ********************************/ + + + + + /**************************************************************** Enabling jtag likely differs with most platforms. We will attempt to enable most from here. Override jtagarm7tdmi_start() to extend for other implementations ARM7TDMI enables three different scan chains: @@ -106,6 +111,9 @@ void jtag_reset_to_runtest_idle() { jtag_arm_tcktock(); jtag_arm_tcktock(); jtag_arm_tcktock(); + jtag_arm_tcktock(); + jtag_arm_tcktock(); + jtag_arm_tcktock(); jtag_arm_tcktock(); // now in Reset state CLRTMS; jtag_arm_tcktock(); // now in Run-Test/Idle state @@ -113,11 +121,12 @@ void jtag_reset_to_runtest_idle() { void jtag_arm_tcktock() { CLRTCK; - //PLEDOUT^=PLEDPIN; + PLEDOUT^=PLEDPIN; SETTCK; - //PLEDOUT^=PLEDPIN; + PLEDOUT^=PLEDPIN; } + // ! Start JTAG, setup pins, reset TAP and return IDCODE unsigned long jtagarm7tdmi_start() { jtagsetup(); @@ -126,11 +135,10 @@ unsigned long jtagarm7tdmi_start() { SETTST; SETRST; - delay(0xFF); + delay(0x2); - //Entry sequence from Page 67 of SLAU265A for 4-wire JTAG CLRRST; - delay(20); + delay(2); CLRTST; msdelay(10); @@ -138,7 +146,7 @@ unsigned long jtagarm7tdmi_start() { /* P5DIR &=~RST; */ - delay(0xF); + delay(0x2); jtagarm7tdmi_resettap(); return jtagarm7tdmi_idcode(); } @@ -160,9 +168,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; @@ -186,7 +194,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;} @@ -245,20 +253,20 @@ unsigned long jtagarm7tdmi_idcode(){ // PROVEN //! Connect Bypass Register to TDO/TDI unsigned char jtagarm7tdmi_bypass(){ // PROVEN - jtagarm7tdmi_resettap(); + //jtagarm7tdmi_resettap(); SHIFT_IR; return jtagarmtransn(ARM7TDMI_IR_BYPASS, 4, LSB, END, NORETIDLE); } //! INTEST verb - do internal test unsigned char jtagarm7tdmi_intest() { - jtagarm7tdmi_resettap(); + //jtagarm7tdmi_resettap(); SHIFT_IR; return jtagarmtransn(ARM7TDMI_IR_INTEST, 4, LSB, END, NORETIDLE); } //! EXTEST verb unsigned char jtagarm7tdmi_extest() { - jtagarm7tdmi_resettap(); + //jtagarm7tdmi_resettap(); SHIFT_IR; return jtagarmtransn(ARM7TDMI_IR_EXTEST, 4, LSB, END, NORETIDLE); } @@ -271,7 +279,7 @@ unsigned char jtagarm7tdmi_extest() { //! RESTART verb unsigned char jtagarm7tdmi_restart() { - jtagarm7tdmi_resettap(); + //jtagarm7tdmi_resettap(); SHIFT_IR; return jtagarmtransn(ARM7TDMI_IR_RESTART, 4, LSB, END, RETIDLE); } @@ -332,9 +340,10 @@ unsigned long jtagarm7tdmi_scan_intest(int chain) { // PROVEN -//! push an instruction into the pipeline - Assumes scan-chain 1 is already INTEST +//! push an instruction into the pipeline 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; @@ -378,7 +387,6 @@ NOP unsigned long jtagarm7tdmi_setMode_ARM(){ // PROVEN unsigned long retval = 0xff; while ((jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT)&& retval-- > 0){ - jtagarm7tdmi_scan_intest(1); cmddataword[6] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_NOP,0); cmddataword[1] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_STR_R0_r0,0); cmddataword[2] = jtagarm7tdmi_instr_primitive(THUMB_INSTR_MOV_R0_PC,0); @@ -386,7 +394,7 @@ 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); } @@ -550,24 +558,68 @@ 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_READ_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; - cmddatalong[2] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline... + cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline... instr = ARM_WRITE_REG | (reg<<12); // push LDR 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[2] = jtagarm7tdmi_instr_primitive(instr, 0); // push nop into pipeline - fetch + cmddatalong[3] = jtagarm7tdmi_nop( 0); // push nop into pipeline - decode + cmddatalong[4] = jtagarm7tdmi_instr_primitive(val-16, 0); // push 32-bit word on data bus + //cmddatalong[4] = jtagarm7tdmi_nop( 0); // push nop into pipeline - execute - cmddatalong[4] = jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus - execute state - cmddatalong[5] = jtagarm7tdmi_nop( 0); // push nop into pipeline - executed + cmddatalong[5] = jtagarm7tdmi_instr_primitive(val, 0); // push 32-bit word on data bus + cmddatalong[6] = jtagarm7tdmi_instr_primitive(val+16, 0); // push 32-bit word on data bus + //cmddatalong[6] = jtagarm7tdmi_nop( 0); // push nop into pipeline - executed if (reg == ARM_REG_PC){ - cmddatalong[6] = jtagarm7tdmi_nop( 0); cmddatalong[7] = jtagarm7tdmi_nop( 0); + cmddatalong[8] = jtagarm7tdmi_nop( 0); } - cmddatalong[8] = jtagarm7tdmi_nop( 0); + cmddatalong[9] = jtagarm7tdmi_nop( 0); + + retval = cmddatalong[5]; + 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_WRITE_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); @@ -575,6 +627,7 @@ unsigned long jtagarm7tdmi_set_register(unsigned char reg, unsigned long val) { + //! Get all registers. Return an array unsigned long* jtagarm7tdmi_get_registers() { cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0); @@ -600,22 +653,40 @@ unsigned long* jtagarm7tdmi_get_registers() { return registers; } +//! Get all registers. Return an array +unsigned long* jtagarm7tdmi_set_registers() { + cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_SKANKREGS,0); + cmddatalong[2] = jtagarm7tdmi_nop( 0); + cmddatalong[3] = jtagarm7tdmi_nop( 0); + cmddatalong[4] = jtagarm7tdmi_instr_primitive(0x40,0); + cmddatalong[5] = jtagarm7tdmi_instr_primitive(0x41,0); + cmddatalong[6] = jtagarm7tdmi_instr_primitive(0x42,0); + cmddatalong[7] = jtagarm7tdmi_instr_primitive(0x43,0); + cmddatalong[8] = jtagarm7tdmi_instr_primitive(0x44,0); + cmddatalong[9] = jtagarm7tdmi_instr_primitive(0x45,0); + cmddatalong[10] = jtagarm7tdmi_instr_primitive(0x46,0); + cmddatalong[11] = jtagarm7tdmi_instr_primitive(0x47,0); + cmddatalong[12] = jtagarm7tdmi_instr_primitive(0x48,0); + cmddatalong[13] = jtagarm7tdmi_instr_primitive(0x49,0); + cmddatalong[14] = jtagarm7tdmi_instr_primitive(0x4a,0); + cmddatalong[15] = jtagarm7tdmi_instr_primitive(0x4b,0); + cmddatalong[16] = jtagarm7tdmi_instr_primitive(0x4c,0); + cmddatalong[17] = jtagarm7tdmi_instr_primitive(0x4d,0); + cmddatalong[18] = jtagarm7tdmi_instr_primitive(0x4e,0); + cmddatalong[19] = jtagarm7tdmi_instr_primitive(0x4f,0); + return registers; +} + //! Retrieve the CPSR Register value unsigned long jtagarm7tdmi_get_regCPSR() { unsigned long retval = 0; - // push nop into pipeline - clean out the pipeline... - cmddatalong[1] = jtagarm7tdmi_nop( 0); - // push MRS_R0, CPSR into pipeline - cmddatalong[2] = jtagarm7tdmi_instr_primitive(ARM_INSTR_MRS_R0_CPSR, 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 - cmddatalong[5] = jtagarm7tdmi_nop( 0); - // recover 32-bit word - retval = jtagarm7tdmi_nop( 0); + cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline... + cmddatalong[2] = jtagarm7tdmi_instr_primitive(ARM_INSTR_MRS_R0_CPSR, 0); // push MRS_R0, CPSR into pipeline + cmddatalong[3] = jtagarm7tdmi_nop( 0); // push nop into pipeline - fetched + cmddatalong[4] = jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded + cmddatalong[5] = jtagarm7tdmi_nop( 0); // push nop into pipeline - executed + retval = jtagarm7tdmi_nop( 0); // recover 32-bit word cmddatalong[6] = retval; return retval; } @@ -624,19 +695,13 @@ unsigned long jtagarm7tdmi_get_regCPSR() { unsigned long jtagarm7tdmi_set_regCPSR(unsigned long val) { unsigned long retval = 0; - // push nop into pipeline - clean out the pipeline... - cmddatalong[1] = jtagarm7tdmi_nop( 0); - // push MSR cpsr_cxsf, R0 into pipeline - cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_MSR_cpsr_cxsf_R0, 0); - // push nop into pipeline - fetched - cmddatalong[2] = jtagarm7tdmi_nop( 0); - // push nop into pipeline - decoded - cmddatalong[3] = jtagarm7tdmi_nop( 0); + cmddatalong[1] = jtagarm7tdmi_nop( 0); // push nop into pipeline - clean out the pipeline... + cmddatalong[1] = jtagarm7tdmi_instr_primitive(ARM_INSTR_MSR_cpsr_cxsf_R0, 0); // push MSR cpsr_cxsf, R0 into pipeline + cmddatalong[2] = jtagarm7tdmi_nop( 0); // push nop into pipeline - fetched + cmddatalong[3] = jtagarm7tdmi_nop( 0); // push nop into pipeline - decoded - // push 32-bit word on data bus - retval = jtagarm7tdmi_instr_primitive(val, 0); - // push nop into pipeline - executed - cmddatalong[5] = jtagarm7tdmi_nop( 0); + retval = jtagarm7tdmi_instr_primitive(val, 0);// push 32-bit word on data bus + cmddatalong[5] = jtagarm7tdmi_nop( 0); // push nop into pipeline - executed cmddatalong[4] = retval; return(retval); } @@ -645,24 +710,15 @@ unsigned long jtagarm7tdmi_set_regCPSR(unsigned long val) { unsigned long jtagarm7tdmi_writemem(unsigned long adr, unsigned long data){ unsigned long r0=0, r1=-1; - // store R0 and R1 - r0 = jtagarm7tdmi_get_register(0); + r0 = jtagarm7tdmi_get_register(0); // store R0 and R1 r1 = jtagarm7tdmi_get_register(1); - // write address into R0 - jtagarm7tdmi_set_register(0, adr); - // write data in R1 - jtagarm7tdmi_set_register(1, data); - //retval = jtagarm7tdmi_exec(ARM_INSTR_LDR_R1_r0_4,1); - // push nop into pipeline to "clean" it ??? - jtagarm7tdmi_nop( 0); - // push nop into pipeline with BREAKPT set - jtagarm7tdmi_nop( 1); - // push LDR R1, R0, #4 into instruction pipeline - jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); - // push nop into pipeline - jtagarm7tdmi_nop( 0); - // restore R0 and R1 - jtagarm7tdmi_set_register(1, r1); + jtagarm7tdmi_set_register(0, adr); // write address into R0 + jtagarm7tdmi_set_register(1, data); // write data in R1 + jtagarm7tdmi_nop( 0); // push nop into pipeline to "clean" it ??? + jtagarm7tdmi_nop( 1); // push nop into pipeline with BREAKPT set + jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline + jtagarm7tdmi_nop( 0); // push nop into pipeline + jtagarm7tdmi_set_register(1, r1); // restore R0 and R1 jtagarm7tdmi_set_register(0, r0); return(-1); } @@ -676,35 +732,26 @@ unsigned long jtagarm7tdmi_readmem(unsigned long adr){ unsigned long r0=0, r1=-1; int waitcount = 0xfff; - // select chain 2 - // store R0 and R1 - not yet... - r0 = jtagarm7tdmi_get_register(0); + r0 = jtagarm7tdmi_get_register(0); // store R0 and R1 r1 = jtagarm7tdmi_get_register(1); - // write address into R0 - jtagarm7tdmi_set_register(0, adr); - // push nop into pipeline to "clean" it ??? - jtagarm7tdmi_nop( 0); - // push nop into pipeline with BREAKPT set - jtagarm7tdmi_nop( 1); - // push LDR R1, R0, #4 into instruction pipeline - jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); - // push nop into pipeline - jtagarm7tdmi_nop( 0); - // SHIFT_IR with RESTART instruction - jtagarm7tdmi_restart(); + jtagarm7tdmi_set_register(0, adr); // write address into R0 + jtagarm7tdmi_nop( 0); // push nop into pipeline to "clean" it ??? + jtagarm7tdmi_nop( 1); // push nop into pipeline with BREAKPT set + jtagarm7tdmi_instr_primitive(ARM_INSTR_LDR_R1_r0_4, 0); // push LDR R1, R0, #4 into instruction pipeline + jtagarm7tdmi_nop( 0); // push nop into pipeline + jtagarm7tdmi_restart(); // SHIFT_IR with RESTART instruction + // Poll the Debug Status Register for DBGACK and nMREQ to be HIGH - while ((jtagarm7tdmi_get_dbgstate() & 1) == 0 && waitcount > 0){ + while ((jtagarm7tdmi_get_dbgstate() & 9) == 0 && waitcount > 0){ delay(1); waitcount --; } - if (waitcount == 0xffff){ + if (waitcount == 0){ return (-1); } else { - retval = jtagarm7tdmi_get_register(1); - // read memory value from R1 register - // restore R0 and R1 - jtagarm7tdmi_set_register(1, r1); - jtagarm7tdmi_set_register(0, r0); + 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); } return retval; } @@ -725,34 +772,30 @@ unsigned long jtagarm7tdmi_haltcpu(){ // PROVEN int waitcount = 0xfff; // store watchpoint info? - not right now - // write 0 in watchpoint 1 address - eice_write(EICE_WP1ADDR, 0); - // write 0xffffffff in watchpoint 1 address mask - eice_write(EICE_WP1ADDRMASK, 0xffffffff); - // write 0 in watchpoint 1 data - eice_write(EICE_WP1DATA, 0); - // write 0xffffffff in watchpoint 1 data mask - eice_write(EICE_WP1DATAMASK, 0xffffffff); - // write 0x00000100 in watchpoint 1 control value register (enables watchpoint) - eice_write(EICE_WP1CTRL, 0x100); //!!!!! WTF! THIS IS SUPPOSED TO BE 9 bits wide?!? - // write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction - eice_write(EICE_WP1CTRLMASK, 0xfffffff7); //!!!!! WTF! THIS IS SUPPOSED TO BE 8 bits wide?!? + eice_write(EICE_WP1ADDR, 0); // write 0 in watchpoint 1 address + eice_write(EICE_WP1ADDRMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 address mask + eice_write(EICE_WP1DATA, 0); // write 0 in watchpoint 1 data + eice_write(EICE_WP1DATAMASK, 0xffffffff); // write 0xffffffff in watchpoint 1 data mask + eice_write(EICE_WP1CTRL, 0x100); //!!!!! WTF! THIS IS SUPPOSED TO BE 9 bits wide?!? // write 0x00000100 in watchpoint 1 control value register (enables watchpoint) + 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 + // poll until debug status says the cpu is in debug mode - while (!(jtagarm7tdmi_get_dbgstate() & 0x1) && waitcount > 0){ - delay(5); - waitcount --; + while (!(jtagarm7tdmi_get_dbgstate() & 0x1) && waitcount-- > 0){ + delay(1); } - // write 0 in watchpoint 0 control value - disables watchpoint 0 - eice_write(EICE_WP1CTRL, 0x0); + eice_write(EICE_WP1CTRL, 0x0); // write 0 in watchpoint 0 control value - disables watchpoint 0 // store the debug state last_halt_debug_state = jtagarm7tdmi_get_dbgstate(); - last_halt_pc = jtagarm7tdmi_getpc() - 4; // assume -4 for entering debug mode via watchpoint. + last_halt_pc = jtagarm7tdmi_getpc() - 4; // assume -4 for entering debug mode via watchpoint. count_dbgspd_instr_since_debug = 0; count_sysspd_instr_since_debug = 0; - if (last_halt_debug_state & JTAG_ARM7TDMI_DBG_TBIT){ + + // get into ARM mode if the T flag is set (Thumb mode) + while (jtagarm7tdmi_get_dbgstate() & JTAG_ARM7TDMI_DBG_TBIT && waitcount-- > 0) { jtagarm7tdmi_setMode_ARM(); } + jtagarm7tdmi_resettap(); return waitcount; } @@ -760,25 +803,23 @@ unsigned long jtagarm7tdmi_releasecpu(){ int waitcount = 0xfff; unsigned long instr; // somehow determine what PC should be (a couple ways possible, calculations required) - // NOP - jtagarm7tdmi_nop(0); - // NOP/BREAKPT - jtagarm7tdmi_nop(1); + jtagarm7tdmi_nop(0); // NOP + jtagarm7tdmi_nop(1); // NOP/BREAKPT - if (last_halt_debug_state & JTAG_ARM7TDMI_DBG_TBIT){ // FIXME: FORKED. - instr = ARM_INSTR_BX_PC + 0x1000000 - (count_dbgspd_instr_since_debug) - (count_sysspd_instr_since_debug*3); //FIXME: make this right - can't we just do an a7solute b/bx? + 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? + 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); } - // VERB_RESTART + SHIFT_IR; - jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); + jtagarmtransn(ARM7TDMI_IR_RESTART,4,LSB,END,RETIDLE); // VERB_RESTART // 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; @@ -794,7 +835,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(); @@ -804,9 +845,14 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len //Enter JTAG mode. cmddatalong[0] = jtagarm7tdmi_start(); cmddatalong[2] = jtagarm7tdmi_haltcpu(); - cmddatalong[1] = jtagarm7tdmi_get_dbgstate(); - //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: @@ -886,26 +932,32 @@ void jtagarm7tdmihandle(unsigned char app, unsigned char verb, unsigned long len case JTAGARM7TDMI_GET_REGISTER: jtagarm7tdmi_resettap(); cmddatalong[0] = jtagarm7tdmi_get_register(cmddata[0]); + //cmddatalong[0] = test_get_register(cmddata[0]); txdata(app,verb,96); break; - case JTAGARM7TDMI_SET_REGISTER: + case JTAGARM7TDMI_SET_REGISTER: // FIXME: NOT AT ALL CORRECT, THIS IS TESTING CODE ONLY jtagarm7tdmi_resettap(); cmddatalong[0] = cmddatalong[1]; jtagarm7tdmi_set_register(cmddata[0], cmddatalong[1]); + //test_set_register(cmddata[0], cmddatalong[1]); txdata(app,verb,96); break; case JTAGARM7TDMI_GET_REGISTERS: jtagarm7tdmi_resettap(); jtagarm7tdmi_get_registers(); - txdata(app,verb,80); + txdata(app,verb,200); + break; + case JTAGARM7TDMI_SET_REGISTERS: + jtagarm7tdmi_resettap(); + jtagarm7tdmi_set_registers(); + txdata(app,verb,200); break; - //case JTAGARM7TDMI_SET_REGISTERS: case JTAGARM7TDMI_DEBUG_INSTR: jtagarm7tdmi_resettap(); 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: @@ -974,6 +1026,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;