From 13d7162b0e2d5c148d0548eeaa99a6b18baef37c Mon Sep 17 00:00:00 2001
From: Peter Zotov <whitequark@whitequark.org>
Date: Wed, 16 Feb 2011 00:21:38 +0300
Subject: [PATCH 1/1] Fix continue command.

---
 README           | 10 +++++-----
 src/gdb-remote.c | 19 +++++++++++++++----
 src/gdb-remote.h |  2 +-
 src/gdb-server.c | 26 +++++++++++++++++++-------
 4 files changed, 40 insertions(+), 17 deletions(-)

diff --git a/README b/README
index 47c7c2f..a09cd44 100644
--- a/README
+++ b/README
@@ -31,10 +31,6 @@ then it would be written to the memory.
 Caveats
 =======
 
-`continue' GDB command does not work: target does not step at
-all or steps with a turtle speed. Looks like there's something
-wrong with SCSI requests.
-
 GDB sends requests for a multi-sectioned ELF files (most ones;
 having both .text and .rodata is enough) in a quite strange way which
 absolutely does not conform to flash page boundaries. Which is even more
@@ -42,4 +38,8 @@ weird when you think about FlashErase requests which it sends correctly.
 And I couldn't think of a way which will resolve this correctly now.
 
 Hardware breakpoints are not supported yet. You can still run your code from
-RAM, and then GDB will insert bkpt opcodes automagically.
+RAM, and then GDB will insert `bkpt' opcodes automagically.
+
+Sometimes when you will try to use GDB `next' command to skip a loop,
+it will use a rather inefficient single-stepping way of doing that.
+Set up a breakpoint manually in that case and do `continue'.
diff --git a/src/gdb-remote.c b/src/gdb-remote.c
index 8ed132f..474af9d 100644
--- a/src/gdb-remote.c
+++ b/src/gdb-remote.c
@@ -11,6 +11,7 @@
 #include <stdlib.h>
 #include <unistd.h>
 #include <stdint.h>
+#include <sys/poll.h>
 
 static const char hex[] = "0123456789abcdef";
 
@@ -132,14 +133,24 @@ start:
 	return packet_size;
 }
 
-int gdb_wait_for_interrupt(int fd) {
-	char c;
-	while(1) {
+// Here we skip any characters which are not \x03, GDB interrupt.
+// As we use the mode with ACK, in a (very unlikely) situation of a packet
+// lost because of this skipping, it will be resent anyway.
+int gdb_check_for_interrupt(int fd) {
+	struct pollfd pfd;
+	pfd.fd = fd;
+	pfd.events = POLLIN;
+
+	if(poll(&pfd, 1, 0) != 0) {
+		char c;
+
 		if(read(fd, &c, 1) != 1)
 			return -2;
 
 		if(c == '\x03') // ^C
-			return 0;
+			return 1;
 	}
+
+	return 0;
 }
 
diff --git a/src/gdb-remote.h b/src/gdb-remote.h
index d4c7d7a..bfa0104 100644
--- a/src/gdb-remote.h
+++ b/src/gdb-remote.h
@@ -3,6 +3,6 @@
 
 int gdb_send_packet(int fd, char* data);
 int gdb_recv_packet(int fd, char** buffer);
-int gdb_wait_for_interrupt(int fd);
+int gdb_check_for_interrupt(int fd);
 
 #endif
diff --git a/src/gdb-server.c b/src/gdb-server.c
index 615bf06..e98f780 100644
--- a/src/gdb-server.c
+++ b/src/gdb-server.c
@@ -271,15 +271,27 @@ int serve(struct stlink* sl, int port) {
 		case 'c':
 			stlink_run(sl);
 
-			printf("Core running, waiting for interrupt.\n");
+			printf("Core running, waiting for interrupt (either in chip or GDB).\n");
 
-			int status = gdb_wait_for_interrupt(client);
-			if(status < 0) {
-				fprintf(stderr, "cannot wait for int: %d\n", status);
-				return 1;
-			}
+			while(1) {
+				int status = gdb_check_for_interrupt(client);
+				if(status < 0) {
+					fprintf(stderr, "cannot check for int: %d\n", status);
+					return 1;
+				}
+
+				if(status == 1) {
+					stlink_force_debug(sl);
+					break;
+				}
+
+				stlink_status(sl);
+				if(sl->core_stat == STLINK_CORE_HALTED) {
+					break;
+				}
 
-			stlink_force_debug(sl);
+				usleep(200000);
+			}
 
 			reply = strdup("S05"); // TRAP
 			break;
-- 
2.39.5