Target2.cpp

00001 // Target2.cpp 2014/6/22
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     TRACE();
00055     _swd->Setup();
00056     JTAG2SWD();
00057     
00058     uint32_t data;
00059     uint8_t ack = _swd->Transfer(DP_IDCODE, &data);
00060     TEST_ASSERT(ack == SWD_OK);
00061     if (ack != SWD_OK) {
00062         return false;
00063     }
00064     idcode = data;
00065 
00066     Abort();
00067 
00068     data = 0x0;
00069     ack = _swd->Transfer(DP_SELECT, &data);
00070     TEST_ASSERT(ack == SWD_OK);
00071     if (ack != SWD_OK) {
00072         return false;
00073     }
00074 
00075     ack = _swd->Transfer(DP_RDBUFF, &data);
00076     TEST_ASSERT(ack == SWD_OK);
00077     if (ack != SWD_OK) {
00078         return false;
00079     }
00080 
00081     data = CSYSPWRUPREQ | CDBGPWRUPREQ;
00082     ack = _swd->Transfer(DP_CTRL_STAT, &data);
00083     TEST_ASSERT(ack == SWD_OK);
00084     if (ack != SWD_OK) {
00085         return false;
00086     }
00087 
00088     ack = _swd->Transfer(DP_RDBUFF, &data);
00089     TEST_ASSERT(ack == SWD_OK);
00090     if (ack != SWD_OK) {
00091         return false;
00092     }
00093 
00094     ack = _swd->Transfer(DP_CTRL_STAT_R, &data);
00095     TEST_ASSERT(ack == SWD_OK);
00096     if (ack != SWD_OK) {
00097         return false;
00098     }
00099     TEST_ASSERT(data == 0xf0000040);
00100 
00101     data = CSYSPWRUPREQ | CDBGPWRUPREQ | 0x04000000;
00102     ack = _swd->Transfer(DP_CTRL_STAT, &data);
00103     TEST_ASSERT(ack == SWD_OK);
00104     if (ack != SWD_OK) {
00105         return false;
00106     }
00107 
00108     ack = _swd->Transfer(DP_RDBUFF, &data);
00109     TEST_ASSERT(ack == SWD_OK);
00110     if (ack != SWD_OK) {
00111         return false;
00112     }
00113 
00114     data = CSYSPWRUPREQ | CDBGPWRUPREQ | MASKLANE;
00115     ack = _swd->Transfer(DP_CTRL_STAT, &data);
00116     TEST_ASSERT(ack == SWD_OK);
00117     if (ack != SWD_OK) {
00118         return false;
00119     }
00120 
00121     ack = _swd->Transfer(DP_RDBUFF, &data);
00122     TEST_ASSERT(ack == SWD_OK);
00123     if (ack != SWD_OK) {
00124         return false;
00125     }
00126     return true;
00127 }
00128 
00129 void Target2::SWJClock(uint32_t clock_hz)
00130 {
00131     _swd->SWJClock(clock_hz);
00132 }
00133 
00134 void Target2::JTAG2SWD()
00135 {
00136     const uint8_t data1[] = {0xff,0xff,0xff,0xff,0xff,0xff,0xff};
00137     const uint8_t data2[] = {0x9e,0xe7};
00138     const uint8_t data3[] = {0x00};
00139     _swd->SWJSequence(sizeof(data1)*8, data1);
00140     _swd->SWJSequence(sizeof(data2)*8, data2);
00141     _swd->SWJSequence(sizeof(data1)*8, data1);
00142     _swd->SWJSequence(sizeof(data3)*8, data3);
00143 }
00144 
00145 void Target2::HardwareReset()
00146 {
00147     _swd->SWJPins(0x00, 0x80); // nReset off
00148     _swd->SWJPins(0x80, 0x80); // nReset on
00149 }
00150 
00151 void Target2::SoftwareReset()
00152 {
00153     TRACE();
00154     writeMemory(NVIC_AIRCR, 0x05fa0004);
00155 }
00156 
00157 uint32_t Target2::readMemory(uint32_t addr)
00158 {
00159     TRACE1(addr);
00160     _setaddr(addr);
00161 
00162     uint32_t data;
00163     uint8_t ack = _swd->Transfer(AP_DRW_R, &data); // dummy read
00164     TRACE1(ack);
00165     TEST_ASSERT(ack == SWD_OK);
00166 
00167     ack = _swd->Transfer(DP_RDBUFF, &data);
00168     TRACE1(ack);
00169     TEST_ASSERT(ack == SWD_OK);
00170     return data;
00171 }
00172 
00173 void Target2::readMemory(uint32_t addr, uint32_t* data, int count)
00174 {
00175     TRACE1(addr);
00176     if (count == 0) {
00177         return;
00178     }
00179 
00180     _setaddr(addr);
00181     
00182     uint8_t ack = _swd->Transfer(AP_DRW_R, NULL); // dummy read
00183     TEST_ASSERT(ack == SWD_OK);
00184 
00185     for(int i = 0; i < count-1; i++) {
00186         ack = _swd->Transfer(AP_DRW_R, data++);
00187         TEST_ASSERT(ack == SWD_OK);
00188     }
00189     ack = _swd->Transfer(DP_RDBUFF, data);
00190     TEST_ASSERT(ack == SWD_OK);
00191 }
00192 
00193 void Target2::writeMemory(uint32_t addr, uint32_t data)
00194 {
00195     writeMemory(addr, &data, 1);
00196 }
00197 
00198 void Target2::writeMemory(uint32_t addr, uint32_t* data, int count)
00199 {
00200     _setaddr(addr);
00201 
00202     while(count-- > 0) {
00203         uint8_t ack = _swd->Transfer(AP_DRW_W, data);
00204         TEST_ASSERT(ack == SWD_OK);
00205         data++;
00206     }
00207 }
00208 
00209 uint8_t Target2::readMemory8(uint32_t addr)
00210 {
00211     TRACE1(addr);
00212     _setaddr8(addr);
00213 
00214     uint32_t data32;
00215     uint8_t ack = _swd->Transfer(AP_DRW_R, &data32); // dummy read
00216     TEST_ASSERT(ack == SWD_OK);
00217 
00218     ack = _swd->Transfer(DP_RDBUFF, &data32);
00219     TEST_ASSERT(ack == SWD_OK);
00220     return (data32 >> ((addr & 0x03) << 3)) & 0xff;
00221 }
00222 
00223 void Target2::writeMemory8(uint32_t addr, uint8_t data)
00224 {
00225     _setaddr8(addr);
00226 
00227     uint32_t data32 = data;
00228     data32 <<= ((addr & 0x03) << 3);
00229     uint8_t ack = _swd->Transfer(AP_DRW_W, &data32);
00230     TEST_ASSERT(ack == SWD_OK);
00231 }
00232 
00233 void Target2::_setaddr(uint32_t addr)
00234 {
00235     TRACE1(addr);
00236     uint32_t ctl = CSW_VALUE|CSW_SIZE32;
00237     uint8_t ack = _swd->Transfer(AP_CSW, &ctl);
00238     TRACE1(ack);
00239     TEST_ASSERT(ack == SWD_OK);
00240 
00241     ack = _swd->Transfer(DP_RDBUFF, NULL);
00242     TRACE1(ack);
00243     TEST_ASSERT(ack == SWD_OK);
00244 
00245     ack = _swd->Transfer(AP_TAR, &addr);
00246     TRACE1(ack);
00247     TEST_ASSERT(ack == SWD_OK);
00248 
00249     ack = _swd->Transfer(DP_RDBUFF, NULL);
00250     TRACE1(ack);
00251     TEST_ASSERT(ack == SWD_OK);
00252 } 
00253 
00254 void Target2::_setaddr8(uint32_t addr)
00255 {
00256     TRACE1(addr);
00257     uint32_t ctl = CSW_VALUE|CSW_SIZE8;
00258     uint8_t ack = _swd->Transfer(AP_CSW, &ctl);
00259     TEST_ASSERT(ack == SWD_OK);
00260 
00261     ack = _swd->Transfer(DP_RDBUFF, NULL);
00262     TEST_ASSERT(ack == SWD_OK);
00263 
00264     ack = _swd->Transfer(AP_TAR, &addr);
00265     TEST_ASSERT(ack == SWD_OK);
00266 
00267     ack = _swd->Transfer(DP_RDBUFF, NULL);
00268     TEST_ASSERT(ack == SWD_OK);
00269 } 
00270 
00271 void Target2::Abort()
00272 {
00273     uint32_t data = 0x1e;
00274     uint8_t ack = _swd->Transfer(DP_ABORT, &data);
00275     TEST_ASSERT(ack == SWD_OK);
00276 }
00277 
00278 int Target2::getStatus()
00279 {
00280     TRACE();
00281     return readMemory(DHCSR) & 6 ? TARGET_HALTED : TARGET_RUNNING;
00282 }
00283 
00284 bool Target2::wait_status(int status, int timeout_ms)
00285 {
00286     TRACE1(status);
00287     Timer t;
00288     t.reset();
00289     t.start();
00290     while(t.read_ms() < timeout_ms) {
00291         if (getStatus() == status) {
00292             return true;
00293         }
00294     }
00295     return false;
00296 }
00297 
00298 bool Target2::prog_status()
00299 {
00300     TRACE();
00301     writeMemory(DEMCR, 1);
00302     int status = getStatus();
00303     TEST_ASSERT(status == TARGET_HALTED);
00304     if (status == TARGET_RUNNING) {
00305         halt();
00306     }
00307     bool st = wait_status(TARGET_HALTED);
00308     TEST_ASSERT(st == true);
00309     writeMemory(DEMCR, 0);
00310     writeMemory(SYSMEMREMAP, 2); // user flash page
00311     uint32_t reset_handler = readMemory(4);
00312     if (setBreakpoint0(reset_handler)) {
00313         writeMemory(NVIC_AIRCR, 0x05fa0004); // SYSRESETREQ software reset
00314         st = wait_status(TARGET_HALTED);
00315         TEST_ASSERT(st == true);
00316         TEST_ASSERT((reset_handler&0xfffffffe) == pc);
00317         removeBreakpoint0(0);
00318     }    
00319     return true;
00320 }
00321 
00322 bool Target2::setBreakpoint0(uint32_t addr)
00323 {
00324     TRACE1(addr);
00325     if ((addr&1) == 0 || addr >= 0x20000000) {
00326         return false;
00327     }
00328     uint32_t data = (addr&0x1ffffffc) | 0xc0000001;
00329     if (addr&0x00000002) {
00330         data |= 0x80000000;
00331     } else {
00332         data |= 0x40000000;
00333     }
00334     writeMemory(FP_COMP0, data); // set breakpoint
00335     writeMemory(FP_CTRL, 3); // enable FPB
00336     return true;
00337 }
00338 
00339 void Target2::removeBreakpoint0(uint32_t addr)
00340 {
00341     TRACE1(addr);
00342     writeMemory(FP_COMP0, 0); // breakpoint clear
00343     writeMemory(FP_CTRL, 2); // desable FPB
00344 }
00345 
00346 void Target2::halt()
00347 {
00348     TRACE();
00349     writeMemory(DHCSR, 0xa05f0003);     
00350 }
00351 
00352 void Target2::resume()
00353 {
00354     TRACE();
00355     writeMemory(DHCSR, 0xa05f0001);     
00356 }
00357 
00358 void Target2::step()
00359 {
00360     TRACE();
00361     writeMemory(DHCSR, 0xa05f0005);     
00362 }
00363 
00364 uint32_t CoreReg::read()
00365 {
00366     _target->writeMemory(DCRSR, _reg);
00367     return _target->readMemory(DCRDR);
00368 }
00369 
00370 void CoreReg::write(uint32_t value)
00371 {
00372     _target->writeMemory(DCRDR, value);
00373     _target->writeMemory(DCRSR, _reg|0x10000);
00374 }
00375 
00376 void CoreReg::setup(Target2* target, uint8_t reg)
00377 {
00378     _target = target;
00379     _reg = reg;    
00380 }