Merge pull request #176 from prattmic/reset

[fw/stlink] / src / stlink-sg.c

diff --git a/src/stlink-sg.c b/src/stlink-sg.c
index 0e4504c7f9ca7d6d91e22546b38fb7a03e71b024..be1f7526b8fcee4cdf125702742a5060c50f487d 100644 (file)
--- a/src/stlink-sg.c
+++ b/src/stlink-sg.c
@@ -85,7 +85,7 @@
 #include <fcntl.h>
 #include <sys/types.h>
 #include <sys/stat.h>
-#include <sys/mman.h>
+#include "mmap.h"
 
 #include "stlink-common.h"
 #include "stlink-sg.h"
@@ -805,6 +805,34 @@ void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     stlink_print_data(sl);
 }
 
+// Write one DWORD data to memory
+
+void _stlink_sg_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
+    struct stlink_libsg *sg = sl->backend_data;
+    clear_cdb(sg);
+    sg->cdb_cmd_blk[1] = STLINK_JTAG_WRITEDEBUG_32BIT;
+    // 2-5: addr
+    write_uint32(sg->cdb_cmd_blk + 2, addr);
+    write_uint32(sg->cdb_cmd_blk + 6, data);
+    sl->q_len = 2;
+    stlink_q(sl);
+
+}
+
+// Read one DWORD data from memory
+
+uint32_t _stlink_sg_read_debug32(stlink_t *sl, uint32_t addr) {
+    struct stlink_libsg *sg = sl->backend_data;
+    clear_cdb(sg);
+    sg->cdb_cmd_blk[1] = STLINK_JTAG_READDEBUG_32BIT;
+    // 2-5: addr
+    write_uint32(sg->cdb_cmd_blk + 2, addr);
+    sl->q_len = 8;
+    stlink_q(sl);
+
+    return read_uint32(sl->q_buf, 4);
+}
+
 // Exit the jtag or swd mode and enter the mass mode.
 
 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
@@ -837,11 +865,16 @@ stlink_backend_t _stlink_sg_backend = {
     _stlink_sg_run,
     _stlink_sg_status,
     _stlink_sg_version,
+    _stlink_sg_read_debug32,
     _stlink_sg_read_mem32,
+    _stlink_sg_write_debug32,
     _stlink_sg_write_mem32,
     _stlink_sg_write_mem8,
     _stlink_sg_read_all_regs,
     _stlink_sg_read_reg,
+    NULL,                   /* read_all_unsupported_regs */
+    NULL,                   /* read_unsupported_regs */
+    NULL,                   /* write_unsupported_regs */
     _stlink_sg_write_reg,
     _stlink_sg_step,
     _stlink_sg_current_mode,
@@ -984,10 +1017,11 @@ stlink_t* stlink_v1_open_inner(const int verbose) {
             "WTF? successfully opened, but unable to read version details. BROKEN!\n");
         return NULL;
     }
+
     return sl;
 }
 
-stlink_t* stlink_v1_open(const int verbose) {
+stlink_t* stlink_v1_open(const int verbose, int reset) {
     stlink_t *sl = stlink_v1_open_inner(verbose);
     if (sl == NULL) {
         fputs("Error: could not open stlink device\n", stderr);
@@ -995,6 +1029,10 @@ stlink_t* stlink_v1_open(const int verbose) {
     }
     // by now, it _must_ be fully open and in a useful mode....
        stlink_enter_swd_mode(sl);
+    /* Now we are ready to read the parameters  */
+    if (reset) {
+        stlink_reset(sl);
+    }
     stlink_load_device_params(sl);
     ILOG("Successfully opened a stlink v1 debugger\n");
     return sl;