hla: Get stlink device back to known state at startup

Figure out the current mode and leave it so that the device is in a
known state.

Some devices appear to not return data from the first request, so
retry it a couple of times to see if we can't smack the device back
into shape.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index e9d13d566da44ca389864f3991dff28de6c24e91..47a9b1233fd91f3addbe029a18e8e41309688b4b 100644 (file)
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -1589,6 +1589,55 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
                        break;
                }
 
+               /* Get the device back to a known mode
+                */
+               int times = 0;
+               uint8_t mode;
+
+               /* In some modes, the device fails to return the current
+                * mode when requested, so try a couple of times
+                */
+               for (;;) {
+                       err = stlink_usb_current_mode(h, &mode);
+
+                       if (err == ERROR_OK) {
+                               LOG_DEBUG("mode %d", mode);
+                               break;
+                       }
+                       LOG_ERROR("current mode failed");
+                       if (++times == 3)
+                               goto error_open;
+               }
+
+               enum stlink_mode emode;
+
+               /* try to exit current mode */
+               switch (mode) {
+               case STLINK_DEV_DFU_MODE:
+                       emode = STLINK_MODE_DFU;
+                       break;
+               case STLINK_DEV_DEBUG_MODE:
+                       emode = STLINK_MODE_DEBUG_SWD;
+                       break;
+               case STLINK_DEV_SWIM_MODE:
+                       emode = STLINK_MODE_DEBUG_SWIM;
+                       break;
+               case STLINK_DEV_BOOTLOADER_MODE:
+               case STLINK_DEV_MASS_MODE:
+               default:
+                       emode = STLINK_MODE_UNKNOWN;
+                       break;
+               }
+
+               LOG_DEBUG("exit mode %d", emode);
+               if (emode != STLINK_MODE_UNKNOWN) {
+
+                       err = stlink_usb_mode_leave(h, emode);
+
+                       if (err != ERROR_OK)
+                               goto error_open;
+               }
+
                /* get the device version */
                err = stlink_usb_version(h);