lib: utils/serial: add semihosting support

We add RISC-V semihosting based serial console for JTAG based early
debugging.

The RISC-V semihosting specification is available at:
https://github.com/riscv/riscv-semihosting-spec/blob/main/riscv-semihosting-spec.adoc

Signed-off-by: Anup Patel <apatel@ventanamicro.com>
Signed-off-by: Kautuk Consul <kconsul@ventanamicro.com>
Reviewed-by: Anup Patel <anup@brainfault.org>

3 files changed, 183 insertions, 0 deletions

diff --git a/lib/utils/serial/Kconfig b/lib/utils/serial/Kconfig
index 6e425f2..da549a7 100644
--- a/lib/utils/serial/Kconfig
+++ b/lib/utils/serial/Kconfig
@@ -79,4 +79,8 @@ config SERIAL_XILINX_UARTLITE
 	bool "Xilinx UART Lite support"
 	default n
 
+config SERIAL_SEMIHOSTING
+	bool "Semihosting support"
+	default n
+
 endmenu
diff --git a/lib/utils/serial/objects.mk b/lib/utils/serial/objects.mk
index efb1d9e..98f3f9a 100644
--- a/lib/utils/serial/objects.mk
+++ b/lib/utils/serial/objects.mk
@@ -41,3 +41,4 @@ libsbiutils-objs-$(CONFIG_SERIAL_SIFIVE) += serial/sifive-uart.o
 libsbiutils-objs-$(CONFIG_SERIAL_LITEX) += serial/litex-uart.o
 libsbiutils-objs-$(CONFIG_SERIAL_UART8250) += serial/uart8250.o
 libsbiutils-objs-$(CONFIG_SERIAL_XILINX_UARTLITE) += serial/xlnx-uartlite.o
+libsbiutils-objs-$(CONFIG_SERIAL_SEMIHOSTING) += serial/semihosting.o
diff --git a/lib/utils/serial/semihosting.c b/lib/utils/serial/semihosting.c
new file mode 100644
index 0000000..5012fa1
--- /dev/null
+++ b/lib/utils/serial/semihosting.c
@@ -0,0 +1,178 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2022 Ventana Micro Systems Inc.
+ *
+ * Authors:
+ *   Anup Patel <apatel@ventanamicro.com>
+ *   Kautuk Consul <kconsul@ventanamicro.com>
+ */
+
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_string.h>
+#include <sbi/sbi_error.h>
+#include <sbi_utils/serial/semihosting.h>
+
+#define SYSOPEN     0x01
+#define SYSWRITEC   0x03
+#define SYSREAD     0x06
+#define SYSREADC    0x07
+#define SYSERRNO	0x13
+
+static long semihosting_trap(int sysnum, void *addr)
+{
+	register int ret asm ("a0") = sysnum;
+	register void *param0 asm ("a1") = addr;
+
+	asm volatile (
+		"	.align 4\n"
+		"	.option push\n"
+		"	.option norvc\n"
+
+		"	slli zero, zero, 0x1f\n"
+		"	ebreak\n"
+		"	srai zero, zero, 7\n"
+
+		"	.option pop\n"
+		: "+r" (ret) : "r" (param0) : "memory");
+
+	return ret;
+}
+
+static bool _semihosting_enabled = true;
+static bool try_semihosting = true;
+
+bool semihosting_enabled(void)
+{
+	register int ret asm ("a0") = SYSERRNO;
+	register void *param0 asm ("a1") = NULL;
+	unsigned long tmp = 0;
+
+	if (!try_semihosting)
+		return _semihosting_enabled;
+
+	asm volatile (
+		"	.align 4\n"
+		"	.option push\n"
+		"	.option norvc\n"
+
+		"	j _semihost_test_vector_next\n"
+		"	.align 4\n"
+		"_semihost_test_vector:\n"
+		"	csrr %[en], mepc\n"
+		"	addi %[en], %[en], 4\n"
+		"	csrw mepc, %[en]\n"
+		"	add %[en], zero, zero\n"
+		"	mret\n"
+		"_semihost_test_vector_next:\n"
+
+		"	la %[tmp], _semihost_test_vector\n"
+		"	csrrw %[tmp], mtvec, %[tmp]\n"
+		"	.align 4\n"
+		"	slli zero, zero, 0x1f\n"
+		"	ebreak\n"
+		"	srai zero, zero, 7\n"
+		"	csrw mtvec, %[tmp]\n"
+
+		"	.option pop\n"
+		: [tmp] "+r" (tmp), [en] "+r" (_semihosting_enabled),
+		  [ret] "+r" (ret)
+		: "r" (param0) : "memory");
+
+	try_semihosting = false;
+	return _semihosting_enabled;
+}
+
+static int semihosting_errno(void)
+{
+	long ret = semihosting_trap(SYSERRNO, NULL);
+
+	if (ret > 0)
+		return -ret;
+	return SBI_EIO;
+}
+
+static int semihosting_infd = SBI_ENODEV;
+
+static long semihosting_open(const char *fname, enum semihosting_open_mode mode)
+{
+	long fd;
+	struct semihosting_open_s {
+		const char *fname;
+		unsigned long mode;
+		size_t len;
+	} open;
+
+	open.fname = fname;
+	open.len = sbi_strlen(fname);
+	open.mode = mode;
+
+	/* Open the file on the host */
+	fd = semihosting_trap(SYSOPEN, &open);
+	if (fd == -1)
+		return semihosting_errno();
+	return fd;
+}
+
+/**
+ * struct semihosting_rdwr_s - Arguments for read and write
+ * @fd: A file descriptor returned from semihosting_open()
+ * @memp: Pointer to a buffer of memory of at least @len bytes
+ * @len: The number of bytes to read or write
+ */
+struct semihosting_rdwr_s {
+	long fd;
+	void *memp;
+	size_t len;
+};
+
+static long semihosting_read(long fd, void *memp, size_t len)
+{
+	long ret;
+	struct semihosting_rdwr_s read;
+
+	read.fd = fd;
+	read.memp = memp;
+	read.len = len;
+
+	ret = semihosting_trap(SYSREAD, &read);
+	if (ret < 0)
+		return semihosting_errno();
+	return len - ret;
+}
+
+/* clang-format on */
+
+static void semihosting_putc(char ch)
+{
+	semihosting_trap(SYSWRITEC, &ch);
+}
+
+static int semihosting_getc(void)
+{
+	char ch = 0;
+	int ret;
+
+	if (semihosting_infd < 0)  {
+		ch = semihosting_trap(SYSREADC, NULL);
+		ret = ch > -1 ? ch : -1;
+	} else
+		ret = semihosting_read(semihosting_infd, &ch, 1) > 0 ? ch : -1;
+
+	return ret;
+}
+
+static struct sbi_console_device semihosting_console = {
+	.name = "semihosting",
+	.console_putc = semihosting_putc,
+	.console_getc = semihosting_getc
+};
+
+int semihosting_init(void)
+{
+	semihosting_infd = semihosting_open(":tt", MODE_READ);
+
+	sbi_console_set_device(&semihosting_console);
+
+	return 0;
+}