From 921b32cf58f0320bdfaff7bbef91d2c51e47ce69 Mon Sep 17 00:00:00 2001
From: Karl Palsson <karlp@tweak.net.au>
Date: Wed, 12 Oct 2011 19:56:19 +0000
Subject: [PATCH 1/1] Support "force debug" command, required by gdb server

usb implementation provided by Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>

Glued together again by me.  With this change, gdbserver actually enters debug and gdbserver
stays open!
---
 src/stlink-common.c |  6 ++++++
 src/stlink-common.h |  4 ++--
 src/stlink-sg.c     |  5 +++--
 src/stlink-usb.c    | 20 +++++++++++++++++++-
 4 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/src/stlink-common.c b/src/stlink-common.c
index a317007..53a359b 100644
--- a/src/stlink-common.c
+++ b/src/stlink-common.c
@@ -272,6 +272,12 @@ void stlink_enter_swd_mode(stlink_t *sl) {
     sl->backend->enter_swd_mode(sl);
 }
 
+// Force the core into the debug mode -> halted state.
+void stlink_force_debug(stlink_t *sl) {
+    D(sl, "\n*** stlink_force_debug_mode ***\n");
+    sl->backend->force_debug(sl);
+}
+
 void stlink_exit_dfu_mode(stlink_t *sl) {
     D(sl, "\n*** stlink_exit_dfu_mode ***\n");
     sl->backend->exit_dfu_mode(sl);
diff --git a/src/stlink-common.h b/src/stlink-common.h
index a716f4d..37de9fc 100644
--- a/src/stlink-common.h
+++ b/src/stlink-common.h
@@ -123,6 +123,7 @@ extern "C" {
         void (*write_reg) (stlink_t *sl, uint32_t reg, int idx);
         void (*step) (stlink_t * stl);
         int (*current_mode) (stlink_t * stl);
+        void (*force_debug) (stlink_t *sl);
     } stlink_backend_t;
 
     struct _stlink {
@@ -187,11 +188,10 @@ extern "C" {
     void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx);
     void stlink_step(stlink_t *sl);
     int stlink_current_mode(stlink_t *sl);
+    void stlink_force_debug(stlink_t *sl);
 
 
     // unprocessed
-    void stlink_force_debug(stlink_t *sl);
-
     int stlink_erase_flash_mass(stlink_t* sl);
     int stlink_write_flash(stlink_t* sl, stm32_addr_t address, uint8_t* data, unsigned length);
     
diff --git a/src/stlink-sg.c b/src/stlink-sg.c
index 891d16e..320fb78 100644
--- a/src/stlink-sg.c
+++ b/src/stlink-sg.c
@@ -403,7 +403,7 @@ void _stlink_sg_status(stlink_t *sl) {
 
 // Force the core into the debug mode -> halted state.
 
-void stlink_force_debug(stlink_t *sl) {
+void _stlink_sg_force_debug(stlink_t *sl) {
     struct stlink_libsg *sg = sl->backend_data;
     D(sl, "\n*** stlink_force_debug ***\n");
     clear_cdb(sg);
@@ -718,7 +718,8 @@ stlink_backend_t _stlink_sg_backend = {
     _stlink_sg_read_reg,
     _stlink_sg_write_reg,
     _stlink_sg_step,
-    _stlink_sg_current_mode
+    _stlink_sg_current_mode,
+    _stlink_sg_force_debug
 };
 
 stlink_t* stlink_open(const char *dev_name, const int verbose) {
diff --git a/src/stlink-usb.c b/src/stlink-usb.c
index 90d5913..16f9343 100644
--- a/src/stlink-usb.c
+++ b/src/stlink-usb.c
@@ -238,6 +238,23 @@ void _stlink_usb_status(stlink_t * sl) {
 
 }
 
+void _stlink_usb_force_debug(stlink_t *sl) {
+    struct stlink_libusb *slu = sl->backend_data;
+    unsigned char* const buf = sl->q_buf;
+    ssize_t size;
+
+    memset(buf, 0, sizeof (sl->q_buf));
+
+    buf[0] = STLINK_DEBUG_COMMAND;
+    buf[1] = STLINK_DEBUG_FORCEDEBUG;
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    if (size == -1) {
+        printf("[!] send_recv\n");
+        return;
+    }
+}
+
+
 void _stlink_usb_enter_swd_mode(stlink_t * sl) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
@@ -400,7 +417,8 @@ stlink_backend_t _stlink_usb_backend = {
     _stlink_usb_read_reg,
     _stlink_usb_write_reg,
     _stlink_usb_step,
-    _stlink_usb_current_mode
+    _stlink_usb_current_mode,
+    _stlink_usb_force_debug
 };
 
 
-- 
2.30.2