Target2.cpp

00001 // Target2.cpp 2013/9/23
00002 #include "Target2.h"
00003 #include "mydebug.h"
00004 
00005 #define SYSMEMREMAP  0x40048000
00006 
00007 #define CoreDebug_BASE      (0xE000EDF0UL)
00008 #define DHCSR (CoreDebug_BASE+0)
00009 #define DCRSR (CoreDebug_BASE+4) 
00010 #define DCRDR (CoreDebug_BASE+8)
00011 #define DEMCR (CoreDebug_BASE+12)
00012 
00013 #define NVIC_AIRCR 0xE000ED0C
00014 
00015 // FPB (breakpoint)
00016 #define FP_CTRL (0xE0002000)
00017 #define FP_CTRL_KEY (1 << 1)
00018 #define FP_COMP0 (0xE0002008)
00019 
00020 Target2::Target2(PinName swdio, PinName swclk, PinName reset)
00021 {
00022     _swd = new SWD(swdio, swclk, reset);
00023     inst();
00024 }
00025 
00026 Target2::Target2(SWD* swd) : _swd(swd)
00027 {
00028     inst();
00029 }
00030 
00031 void Target2::inst()
00032 {
00033     r0.setup(this, 0);
00034     r1.setup(this, 1);
00035     r2.setup(this, 2);
00036     r3.setup(this, 3);
00037     r4.setup(this, 4);
00038     r5.setup(this, 5);
00039     r6.setup(this, 6);
00040     r7.setup(this, 7);
00041     r8.setup(this, 8);
00042     r9.setup(this, 9);
00043     r10.setup(this, 10);
00044     r11.setup(this, 11);
00045     r12.setup(this, 12);
00046     sp.setup(this, 13);
00047     lr.setup(this, 14);
00048     pc.setup(this, 15);
00049     xpsr.setup(this, 16);
00050 }
00051 
00052 bool Target2::setup()
00053 {
00054     _swd->Setup();
00055     JTAG2SWD();
00056     
00057     uint32_t data;
00058     uint8_t ack = _swd->Transfer(DP_IDCODE, &data);
00059     TEST_ASSERT(ack == SWD_OK);
00060     if (ack != SWD_OK) {
00061         return false;
00062     }
00063     idcode = data;
00064 
00065     Abort();
00066 
00067     data = 0x0;
00068     ack = _swd->Transfer(DP_SELECT, &data);
00069     TEST_ASSERT(ack == SWD_OK);
00070     if (ack != SWD_OK) {
00071         return false;
00072     }
00073 
00074     ack = _swd->Transfer(DP_RDBUFF, &data);
00075     TEST_ASSERT(ack == SWD_OK);
00076     if (ack != SWD_OK) {
00077         return false;
00078     }
00079 
00080     data = CSYSPWRUPREQ | CDBGPWRUPREQ;
00081     ack = _swd->Transfer(DP_CTRL_STAT, &data);
00082     TEST_ASSERT(ack == SWD_OK);
00083     if (ack != SWD_OK) {
00084         return false;
00085     }
00086 
00087     ack = _swd->Transfer(DP_RDBUFF, &data);
00088     TEST_ASSERT(ack == SWD_OK);
00089     if (ack != SWD_OK) {
00090         return false;
00091     }
00092 
00093     ack = _swd->Transfer(DP_CTRL_STAT_R, &data);
00094     TEST_ASSERT(ack == SWD_OK);
00095     if (ack != SWD_OK) {
00096         return false;
00097     }
00098     TEST_ASSERT(data == 0xf0000040);
00099 
00100     data = CSYSPWRUPREQ | CDBGPWRUPREQ | 0x04000000;
00101     ack = _swd->Transfer(DP_CTRL_STAT, &data);
00102     TEST_ASSERT(ack == SWD_OK);
00103     if (ack != SWD_OK) {
00104         return false;
00105     }
00106 
00107     ack = _swd->Transfer(DP_RDBUFF, &data);
00108     TEST_ASSERT(ack == SWD_OK);
00109     if (ack != SWD_OK) {
00110         return false;
00111     }
00112 
00113     data = CSYSPWRUPREQ | CDBGPWRUPREQ | MASKLANE;
00114     ack = _swd->Transfer(DP_CTRL_STAT, &data);
00115     TEST_ASSERT(ack == SWD_OK);
00116     if (ack != SWD_OK) {
00117         return false;
00118     }
00119 
00120     ack = _swd->Transfer(DP_RDBUFF, &data);
00121     TEST_ASSERT(ack == SWD_OK);
00122     if (ack != SWD_OK) {
00123         return false;
00124     }
00125     return true;
00126 }
00127 
00128 void Target2::SWJClock(uint32_t clock_hz)
00129 {
00130     _swd->SWJClock(clock_hz);
00131 }
00132 
00133 void Target2::JTAG2SWD()
00134 {
00135     const uint8_t data1[] = {0xff,0xff,0xff,0xff,0xff,0xff,0xff};
00136     const uint8_t data2[] = {0x9e,0xe7};
00137     const uint8_t data3[] = {0x00};
00138     _swd->SWJSequence(sizeof(data1)*8, data1);
00139     _swd->SWJSequence(sizeof(data2)*8, data2);
00140     _swd->SWJSequence(sizeof(data1)*8, data1);
00141     _swd->SWJSequence(sizeof(data3)*8, data3);
00142 }
00143 
00144 void Target2::HardwareReset()
00145 {
00146     _swd->SWJPins(0x00, 0x80); // nReset off
00147     _swd->SWJPins(0x80, 0x80); // nReset on
00148 }
00149 
00150 void Target2::SoftwareReset()
00151 {
00152     writeMemory(NVIC_AIRCR, 0x05fa0004);
00153 }
00154 
00155 uint32_t Target2::readMemory(uint32_t addr)
00156 {
00157     _setaddr(addr);
00158 
00159     uint32_t data;
00160     uint8_t ack = _swd->Transfer(AP_DRW_R, &data); // dummy read
00161     TEST_ASSERT(ack == SWD_OK);
00162 
00163     ack = _swd->Transfer(DP_RDBUFF, &data);
00164     TEST_ASSERT(ack == SWD_OK);
00165     return data;
00166 }
00167 
00168 void Target2::readMemory(uint32_t addr, uint32_t* data, int count)
00169 {
00170     if (count == 0) {
00171         return;
00172     }
00173 
00174     _setaddr(addr);
00175     
00176     uint8_t ack = _swd->Transfer(AP_DRW_R, NULL); // dummy read
00177     TEST_ASSERT(ack == SWD_OK);
00178 
00179     for(int i = 0; i < count-1; i++) {
00180         ack = _swd->Transfer(AP_DRW_R, data++);
00181         TEST_ASSERT(ack == SWD_OK);
00182     }
00183     ack = _swd->Transfer(DP_RDBUFF, data);
00184     TEST_ASSERT(ack == SWD_OK);
00185 }
00186 
00187 void Target2::writeMemory(uint32_t addr, uint32_t data)
00188 {
00189     writeMemory(addr, &data, 1);
00190 }
00191 
00192 void Target2::writeMemory(uint32_t addr, uint32_t* data, int count)
00193 {
00194     _setaddr(addr);
00195 
00196     while(count-- > 0) {
00197         uint8_t ack = _swd->Transfer(AP_DRW_W, data);
00198         TEST_ASSERT(ack == SWD_OK);
00199         data++;
00200     }
00201 }
00202 
00203 uint8_t Target2::readMemory8(uint32_t addr)
00204 {
00205     _setaddr8(addr);
00206 
00207     uint32_t data32;
00208     uint8_t ack = _swd->Transfer(AP_DRW_R, &data32); // dummy read
00209     TEST_ASSERT(ack == SWD_OK);
00210 
00211     ack = _swd->Transfer(DP_RDBUFF, &data32);
00212     TEST_ASSERT(ack == SWD_OK);
00213     return (data32 >> ((addr & 0x03) << 3)) & 0xff;
00214 }
00215 
00216 void Target2::writeMemory8(uint32_t addr, uint8_t data)
00217 {
00218     _setaddr8(addr);
00219 
00220     uint32_t data32 = data;
00221     data32 <<= ((addr & 0x03) << 3);
00222     uint8_t ack = _swd->Transfer(AP_DRW_W, &data32);
00223     TEST_ASSERT(ack == SWD_OK);
00224 }
00225 
00226 void Target2::_setaddr(uint32_t addr)
00227 {
00228     uint32_t ctl = CSW_VALUE|CSW_SIZE32;
00229     uint8_t ack = _swd->Transfer(AP_CSW, &ctl);
00230     TEST_ASSERT(ack == SWD_OK);
00231 
00232     ack = _swd->Transfer(DP_RDBUFF, NULL);
00233     TEST_ASSERT(ack == SWD_OK);
00234 
00235     ack = _swd->Transfer(AP_TAR, &addr);
00236     TEST_ASSERT(ack == SWD_OK);
00237 
00238     ack = _swd->Transfer(DP_RDBUFF, NULL);
00239     TEST_ASSERT(ack == SWD_OK);
00240 } 
00241 
00242 void Target2::_setaddr8(uint32_t addr)
00243 {
00244     uint32_t ctl = CSW_VALUE|CSW_SIZE8;
00245     uint8_t ack = _swd->Transfer(AP_CSW, &ctl);
00246     TEST_ASSERT(ack == SWD_OK);
00247 
00248     ack = _swd->Transfer(DP_RDBUFF, NULL);
00249     TEST_ASSERT(ack == SWD_OK);
00250 
00251     ack = _swd->Transfer(AP_TAR, &addr);
00252     TEST_ASSERT(ack == SWD_OK);
00253 
00254     ack = _swd->Transfer(DP_RDBUFF, NULL);
00255     TEST_ASSERT(ack == SWD_OK);
00256 } 
00257 
00258 void Target2::Abort()
00259 {
00260     uint32_t data = 0x1e;
00261     uint8_t ack = _swd->Transfer(DP_ABORT, &data);
00262     TEST_ASSERT(ack == SWD_OK);
00263 }
00264 
00265 int Target2::getStatus()
00266 {
00267     return readMemory(DHCSR) & 6 ? TARGET_HALTED : TARGET_RUNNING;
00268 }
00269 
00270 bool Target2::wait_status(int status, int timeout_ms)
00271 {
00272     Timer t;
00273     t.reset();
00274     t.start();
00275     while(t.read_ms() < timeout_ms) {
00276         if (getStatus() == status) {
00277             return true;
00278         }
00279     }
00280     return false;
00281 }
00282 
00283 bool Target2::prog_status()
00284 {
00285     writeMemory(DEMCR, 1);
00286     int status = getStatus();
00287     TEST_ASSERT(status == TARGET_HALTED);
00288     if (status == TARGET_RUNNING) {
00289         halt();
00290     }
00291     bool st = wait_status(TARGET_HALTED);
00292     TEST_ASSERT(st == true);
00293     writeMemory(DEMCR, 0);
00294     writeMemory(SYSMEMREMAP, 2); // user flash page
00295     uint32_t reset_handler = readMemory(4);
00296     if (setBreakpoint0(reset_handler)) {
00297         writeMemory(NVIC_AIRCR, 0x05fa0004); // SYSRESETREQ software reset
00298         st = wait_status(TARGET_HALTED);
00299         TEST_ASSERT(st == true);
00300         TEST_ASSERT((reset_handler&0xfffffffe) == pc);
00301         removeBreakpoint0(0);
00302     }    
00303     return true;
00304 }
00305 
00306 bool Target2::setBreakpoint0(uint32_t addr)
00307 {
00308     if ((addr&1) == 0 || addr >= 0x20000000) {
00309         return false;
00310     }
00311     uint32_t data = (addr&0x1ffffffc) | 0xc0000001;
00312     if (addr&0x00000002) {
00313         data |= 0x80000000;
00314     } else {
00315         data |= 0x40000000;
00316     }
00317     writeMemory(FP_COMP0, data); // set breakpoint
00318     writeMemory(FP_CTRL, 3); // enable FPB
00319     return true;
00320 }
00321 
00322 void Target2::removeBreakpoint0(uint32_t addr)
00323 {
00324     writeMemory(FP_COMP0, 0); // breakpoint clear
00325     writeMemory(FP_CTRL, 2); // desable FPB
00326 }
00327 
00328 void Target2::halt()
00329 {
00330     writeMemory(DHCSR, 0xa05f0003);     
00331 }
00332 
00333 void Target2::resume()
00334 {
00335     writeMemory(DHCSR, 0xa05f0001);     
00336 }
00337 
00338 void Target2::step()
00339 {
00340     writeMemory(DHCSR, 0xa05f0005);     
00341 }
00342 
00343 uint32_t CoreReg::read()
00344 {
00345     _target->writeMemory(DCRSR, _reg);
00346     return _target->readMemory(DCRDR);
00347 }
00348 
00349 void CoreReg::write(uint32_t value)
00350 {
00351     _target->writeMemory(DCRDR, value);
00352     _target->writeMemory(DCRSR, _reg|0x10000);
00353 }
00354 
00355 void CoreReg::setup(Target2* target, uint8_t reg)
00356 {
00357     _target = target;
00358     _reg = reg;    
00359 }