Support version decoding on both platforms

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

diff --git a/src/stlink-sg.c b/src/stlink-sg.c
index 06d55e25870a714f866f8e8053579df4740a01ff..891d16e1b7c08cbeef6d62100774f0f8fd7e88ef 100644 (file)
--- a/src/stlink-sg.c
+++ b/src/stlink-sg.c
@@ -246,71 +246,6 @@ void stlink_q(stlink_t *sl) {
 
 // TODO thinking, cleanup
 
-void stlink_parse_version(stlink_t *stl) {
-    struct stlink_libsg *sl = stl->backend_data;
-    sl->st_vid = 0;
-    sl->stlink_pid = 0;
-    if (stl->q_len <= 0) {
-        fprintf(stderr, "Error: could not parse the stlink version");
-        return;
-    }
-    stlink_print_data(stl);
-    uint32_t b0 = stl->q_buf[0]; //lsb
-    uint32_t b1 = stl->q_buf[1];
-    uint32_t b2 = stl->q_buf[2];
-    uint32_t b3 = stl->q_buf[3];
-    uint32_t b4 = stl->q_buf[4];
-    uint32_t b5 = stl->q_buf[5]; //msb
-
-    // b0 b1                       || b2 b3  | b4 b5
-    // 4b        | 6b     | 6b     || 2B     | 2B
-    // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
-
-    sl->stlink_v = (b0 & 0xf0) >> 4;
-    sl->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
-    sl->swim_v = b1 & 0x3f;
-    sl->st_vid = (b3 << 8) | b2;
-    sl->stlink_pid = (b5 << 8) | b4;
-
-    if (stl->verbose < 2)
-        return;
-
-    DD(stl, "st vid         = 0x%04x (expect 0x%04x)\n",
-            sl->st_vid, USB_ST_VID);
-    DD(stl, "stlink pid     = 0x%04x (expect 0x%04x)\n",
-            sl->stlink_pid, USB_STLINK_PID);
-    DD(stl, "stlink version = 0x%x\n", sl->stlink_v);
-    DD(stl, "jtag version   = 0x%x\n", sl->jtag_v);
-    DD(stl, "swim version   = 0x%x\n", sl->swim_v);
-    if (sl->jtag_v == 0)
-        DD(stl,
-            "    notice: the firmware doesn't support a jtag/swd interface\n");
-    if (sl->swim_v == 0)
-        DD(stl,
-            "    notice: the firmware doesn't support a swim interface\n");
-
-}
-
-int stlink_mode(stlink_t *stl) {
-    if (stl->q_len <= 0)
-        return STLINK_DEV_UNKNOWN_MODE;
-
-    stlink_print_data(stl);
-
-    switch (stl->q_buf[0]) {
-        case STLINK_DEV_DFU_MODE:
-            DD(stl, "stlink mode: dfu\n");
-            return STLINK_DEV_DFU_MODE;
-        case STLINK_DEV_DEBUG_MODE:
-            DD(stl, "stlink mode: debug (jtag or swd)\n");
-            return STLINK_DEV_DEBUG_MODE;
-        case STLINK_DEV_MASS_MODE:
-            DD(stl, "stlink mode: mass\n");
-            return STLINK_DEV_MASS_MODE;
-    }
-    return STLINK_DEV_UNKNOWN_MODE;
-}
-
 void stlink_stat(stlink_t *stl, char *txt) {
     if (stl->q_len <= 0)
         return;
@@ -337,7 +272,6 @@ void _stlink_sg_version(stlink_t *stl) {
     stl->q_len = 6;
     sl->q_addr = 0;
     stlink_q(stl);
-    stlink_parse_version(stl);
 }
 
 // Get stlink mode:
@@ -346,13 +280,12 @@ void _stlink_sg_version(stlink_t *stl) {
 
 int _stlink_sg_current_mode(stlink_t *stl) {
     struct stlink_libsg *sl = stl->backend_data;
-    D(stl, "\n*** stlink_current_mode ***\n");
     clear_cdb(sl);
     sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
     stl->q_len = 2;
     sl->q_addr = 0;
     stlink_q(stl);
-    return stlink_mode(stl);
+    return stl->q_buf[0];
 }
 
 // Exit the mass mode and enter the swd debug mode.
@@ -442,9 +375,6 @@ void _stlink_sg_core_id(stlink_t *sl) {
     sg->q_addr = 0;
     stlink_q(sl);
     sl->core_id = read_uint32(sl->q_buf, 0);
-    if (sl->verbose < 2)
-        return;
-    stlink_print_data(sl);
 }
 
 // Arm-core reset -> halted state.
@@ -486,9 +416,8 @@ void stlink_force_debug(stlink_t *sl) {
 
 // Read all arm-core registers.
 
-void _stlink_sg_read_all_regs(stlink_t *sl) {
+void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
     struct stlink_libsg *sg = sl->backend_data;
-    D(sl, "\n*** stlink_read_all_regs ***\n");
     clear_cdb(sg);
     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
     sl->q_len = 84;
@@ -788,7 +717,8 @@ stlink_backend_t _stlink_sg_backend = {
     _stlink_sg_read_all_regs,
     _stlink_sg_read_reg,
     _stlink_sg_write_reg,
-    _stlink_sg_step
+    _stlink_sg_step,
+    _stlink_sg_current_mode
 };
 
 stlink_t* stlink_open(const char *dev_name, const int verbose) {