Diff: Target2.cpp

Revision:

4:5e4107edcbdb

Parent:

3:d7a7cde0bfb8

Child:

5:2774358f5e4f

--- a/Target2.cpp	Sun Sep 08 14:13:15 2013 +0000
+++ b/Target2.cpp	Wed Sep 11 14:00:40 2013 +0000
@@ -1,4 +1,4 @@
-// Target2.cpp 2013/9/8
+// Target2.cpp 2013/9/11
 #include "Target2.h"
 #include "mydebug.h"
 
@@ -12,6 +12,11 @@
 
 #define NVIC_AIRCR 0xE000ED0C
 
+// FPB (breakpoint)
+#define FP_CTRL (0xE0002000)
+#define FP_CTRL_KEY (1 << 1)
+#define FP_COMP0 (0xE0002008)
+
 Target2::Target2(PinName swdio, PinName swclk, PinName reset, Serial* usbpc)
     : _swd(swdio, swclk, reset), _pc(usbpc)
 {
@@ -253,6 +258,45 @@
     return false;
 }
 
+bool Target2::prog_status()
+{
+    writeMemory(DEMCR, 1);
+    int status = getStatus();
+    TEST_ASSERT(status == TARGET_HALTED);
+    if (status == TARGET_RUNNING) {
+        halt();
+    }
+    bool st = wait_status(TARGET_HALTED);
+    TEST_ASSERT(st == true);
+    writeMemory(DEMCR, 0);
+    writeMemory(SYSMEMREMAP, 2); // user flash page
+    uint32_t reset_handler = readMemory(4);
+    breakpoint0(reset_handler);
+    writeMemory(NVIC_AIRCR, 0x05fa0004); // SYSRESETREQ software reset
+    st = wait_status(TARGET_HALTED);
+    TEST_ASSERT(st == true);
+    TEST_ASSERT((reset_handler&0xfffffffe) == pc);
+    breakpoint0(0); // breakpoint clear
+    return true;
+}
+
+void Target2::breakpoint0(uint32_t addr)
+{
+    if (addr) {
+        uint32_t data = (addr&0x1ffffffc) | 0xc0000001;
+        if (addr&0x00000002) {
+            data |= 0x80000000;
+        } else {
+            data |= 0x40000000;
+        }
+        writeMemory(FP_COMP0, data); // set breakpoint
+        writeMemory(FP_CTRL, 3); // enable FPB
+    } else {
+        writeMemory(FP_COMP0, 0); // breakpoint clear
+        writeMemory(FP_CTRL, 2); // desable FPB
+    }
+}
+
 void Target2::halt()
 {
     writeMemory(DHCSR, 0xa05f0003);