From aca421d0bbd021c4b0e320195692d9760d9ecdcd Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Sun, 14 Apr 2013 21:58:49 +0400
Subject: [PATCH] Basic semihosting support

Implement bare minimum necessary to support console IO.

This works with standard newlib builds and is based on the reference
documentation:
http://infocenter.arm.com/help/topic/com.arm.doc.dui0471c/CHDJHHDI.html

Tested using gcc-arm-none-eabi-4_7-2013q1-20130313 for both
stm32f1-based BMP and stm32f0 target.

Signed-off-by: Paul Fertser <fercerpav@gmail.com>
---
 README         | 12 +++++++++
 src/gdb_main.c | 70 ++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 82 insertions(+)

diff --git a/README b/README
index 52835f9..b47e22b 100644
--- a/README
+++ b/README
@@ -45,6 +45,18 @@ your '.gdbinit'.  That will allow you to access addresses outside of
 the memory map.  It will treat anything outside of the memory map as
 RAM.
 
+Semihosting support
+===================
+Basic armv6m/armv7m semihosting is supported, i.e. you can build your
+application in a special way to use host's stdin and stdout right
+inside gdb simply by calling printf(), scanf() and other input-output
+functions. To make use of it, add --specs=rdimon.specs and -lrdimon to
+the linker flags for your firmware.
+
+All writes are treated as writes to the stdout, all reads are done
+from stdin; SYS_ISTTY always returns true, SYS_ERRNO -- EINTR;
+SYS_OPEN and SYS_FLEN return dummy non-zero values.
+
 Project layout
 ==============
 driver/ - Windows drivers for the Black Magic probe hardware.
diff --git a/src/gdb_main.c b/src/gdb_main.c
index 6be6d0f..887b9ac 100644
--- a/src/gdb_main.c
+++ b/src/gdb_main.c
@@ -75,6 +75,8 @@ gdb_main(void)
 {
 	int size;
 	bool single_step = false;
+	char last_activity = 0;
+	uint32_t semihost_read_bytes = 0;
 
 	DEBUG("Entring GDB protocol main loop\n");
 	/* GDB protocol main loop */
@@ -82,6 +84,7 @@ gdb_main(void)
 		SET_IDLE_STATE(1);
 		size = gdb_getpacket(pbuf, BUF_SIZE);
 		SET_IDLE_STATE(0);
+	continue_activity:
 		switch(pbuf[0]) {
 		/* Implementation of these is mandatory! */
 		case 'g': { /* 'g': Read general registers */
@@ -163,9 +166,48 @@ gdb_main(void)
 				unsigned char c = gdb_if_getchar_to(0);
 				if((c == '\x03') || (c == '\x04')) {
 					target_halt_request(cur_target);
+					last_activity = 's';
 				}
 			}
 			SET_RUN_STATE(0);
+
+			uint32_t arm_regs[cur_target->regs_size];
+			uint32_t semihost_buf[4];
+			target_regs_read(cur_target, arm_regs);
+			target_mem_read_bytes(cur_target, (uint8_t *)semihost_buf, arm_regs[15], 2);
+			/* Is this a semihosting breakpoint? */
+			if ((semihost_buf[0] & 0xFFFF) == 0xBEAB) {
+				last_activity = pbuf[0];
+				semihost_read_bytes = 0;
+				target_mem_read_words(cur_target, semihost_buf, arm_regs[1], sizeof(semihost_buf));
+
+				switch (arm_regs[0]) {
+				case 0x09: /* SYS_ISTTY */
+					arm_regs[0] = 1; /* it's a tty */
+					target_regs_write(cur_target, arm_regs);
+					/* fall-through */
+				case 0x01: /* SYS_OPEN */
+				case 0x0C: /* SYS_FLEN */
+					/* pretend it's successful, r0 is non-zero already */
+					goto continue_activity;
+				case 0x13: /* SYS_ERRNO */
+					arm_regs[0] = 4; /* EINTR */
+					target_regs_write(cur_target, arm_regs);
+					goto continue_activity;
+				case 0x05: /* SYS_WRITE */
+					gdb_putpacket_f("Fwrite,1,%08X,%08X",
+							semihost_buf[1], semihost_buf[2]);
+					arm_regs[0] = 0; /* pretend it's always successful */
+					target_regs_write(cur_target, arm_regs);
+					continue;
+				case 0x06: /* SYS_READ */
+					gdb_putpacket_f("Fread,0,%08X,%08X",
+							semihost_buf[1], semihost_buf[2]);
+					semihost_read_bytes = semihost_buf[2];
+					continue;
+				}
+			}
+
 			/* Report reason for halt */
 			if(target_check_hw_wp(cur_target, &watch_addr)) {
 				/* Watchpoint hit */
@@ -175,6 +217,34 @@ gdb_main(void)
 			}
 			break;
 			}
+		case 'F': {	/* Semihosting call finished */
+			int bytes, errcode, items;
+			char c, *p;
+			if (pbuf[1] == '-')
+				p = &pbuf[2];
+			else
+				p = &pbuf[1];
+			items = sscanf(p, "%x,%x,%c", &bytes, &errcode, &c);
+
+			if (semihost_read_bytes) {
+				uint32_t arm_regs[cur_target->regs_size];
+				target_regs_read(cur_target, arm_regs);
+				if (items == 3 && c == 'C')
+					arm_regs[0] = -1;
+				else
+					arm_regs[0] = semihost_read_bytes - bytes;
+				target_regs_write(cur_target, arm_regs);
+			}
+
+			/* if break is requested */
+			if (items == 3 && c == 'C') {
+				gdb_putpacketz("T02");
+				break;
+			}
+
+			pbuf[0] = last_activity;
+			goto continue_activity;
+		}
 
 		/* Optional GDB packet support */
 		case '!':	/* Enable Extended GDB Protocol. */