target/espressif: add semihosting support

ARM semihosting + some custom syscalls implemented for
Espressif chips (ESP32, ESP32-S2, ESP32-S3)

Signed-off-by: Erhan Kurubas <erhan.kurubas@espressif.com>
Change-Id: Ic8174cf1cd344fa16d619b7b8405c9650e869443
Reviewed-on: https://review.openocd.org/c/openocd/+/7074
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

12 files changed, 434 insertions, 15 deletions

diff --git a/src/target/espressif/Makefile.am b/src/target/espressif/Makefile.am
index 1b4f806..8367a38 100644
--- a/src/target/espressif/Makefile.am
+++ b/src/target/espressif/Makefile.am
@@ -6,6 +6,10 @@ noinst_LTLIBRARIES += %D%/libespressif.la
        %D%/esp_xtensa.h \
        %D%/esp_xtensa_smp.c \
        %D%/esp_xtensa_smp.h \
+       %D%/esp_xtensa_semihosting.c \
+       %D%/esp_xtensa_semihosting.h \
        %D%/esp32.c \
        %D%/esp32s2.c \
-       %D%/esp32s3.c
+       %D%/esp32s3.c \
+       %D%/esp_semihosting.c \
+       %D%/esp_semihosting.h
diff --git a/src/target/espressif/esp32.c b/src/target/espressif/esp32.c
index a083627..8ad8bad 100644
--- a/src/target/espressif/esp32.c
+++ b/src/target/espressif/esp32.c
@@ -13,6 +13,7 @@
 #include <target/target.h>
 #include <target/target_type.h>
 #include <target/smp.h>
+#include <target/semihosting_common.h>
 #include "assert.h"
 #include "esp_xtensa_smp.h"
 
@@ -329,6 +330,10 @@ static const struct esp_xtensa_smp_chip_ops esp32_chip_ops = {
 	.on_halt = esp32_on_halt
 };
 
+static const struct esp_semihost_ops esp32_semihost_ops = {
+	.prepare = esp32_disable_wdts
+};
+
 static int esp32_target_create(struct target *target, Jim_Interp *interp)
 {
 	struct xtensa_debug_module_config esp32_dm_cfg = {
@@ -346,7 +351,7 @@ static int esp32_target_create(struct target *target, Jim_Interp *interp)
 	}
 
 	int ret = esp_xtensa_smp_init_arch_info(target, &esp32->esp_xtensa_smp,
-		&esp32_dm_cfg, &esp32_chip_ops);
+		&esp32_dm_cfg, &esp32_chip_ops, &esp32_semihost_ops);
 	if (ret != ERROR_OK) {
 		LOG_ERROR("Failed to init arch info!");
 		free(esp32);
@@ -445,6 +450,13 @@ static const struct command_registration esp32_command_handlers[] = {
 		.usage = "",
 		.chain = esp32_any_command_handlers,
 	},
+	{
+		.name = "arm",
+		.mode = COMMAND_ANY,
+		.help = "ARM Command Group",
+		.usage = "",
+		.chain = semihosting_common_handlers
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/espressif/esp32s2.c b/src/target/espressif/esp32s2.c
index 0bcd20f..4aef379 100644
--- a/src/target/espressif/esp32s2.c
+++ b/src/target/espressif/esp32s2.c
@@ -13,7 +13,9 @@
 #include "assert.h"
 #include <target/target.h>
 #include <target/target_type.h>
+#include <target/semihosting_common.h>
 #include "esp_xtensa.h"
+#include "esp_xtensa_semihosting.h"
 
 /* Overall memory map
  * TODO: read memory configuration from target registers */
@@ -406,6 +408,19 @@ static int esp32s2_poll(struct target *target)
 		if (old_state == TARGET_DEBUG_RUNNING) {
 			target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 		} else {
+			if (esp_xtensa_semihosting(target, &ret) == SEMIHOSTING_HANDLED) {
+				struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
+				if (ret == ERROR_OK && esp_xtensa->semihost.need_resume) {
+					esp_xtensa->semihost.need_resume = false;
+					/* Resume xtensa_resume will handle BREAK instruction. */
+					ret = target_resume(target, 1, 0, 1, 0);
+					if (ret != ERROR_OK) {
+						LOG_ERROR("Failed to resume target");
+						return ret;
+					}
+				}
+				return ret;
+			}
 			esp32s2_on_halt(target);
 			target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 		}
@@ -423,7 +438,11 @@ static int esp32s2_virt2phys(struct target *target,
 
 static int esp32s2_target_init(struct command_context *cmd_ctx, struct target *target)
 {
-	return esp_xtensa_target_init(cmd_ctx, target);
+	int ret = esp_xtensa_target_init(cmd_ctx, target);
+	if (ret != ERROR_OK)
+		return ret;
+
+	return esp_xtensa_semihosting_init(target);
 }
 
 static const struct xtensa_debug_ops esp32s2_dbg_ops = {
@@ -437,6 +456,10 @@ static const struct xtensa_power_ops esp32s2_pwr_ops = {
 	.queue_reg_write = xtensa_dm_queue_pwr_reg_write
 };
 
+static const struct esp_semihost_ops esp32s2_semihost_ops = {
+	.prepare = esp32s2_disable_wdts
+};
+
 static int esp32s2_target_create(struct target *target, Jim_Interp *interp)
 {
 	struct xtensa_debug_module_config esp32s2_dm_cfg = {
@@ -454,7 +477,7 @@ static int esp32s2_target_create(struct target *target, Jim_Interp *interp)
 		return ERROR_FAIL;
 	}
 
-	int ret = esp_xtensa_init_arch_info(target, &esp32->esp_xtensa, &esp32s2_dm_cfg);
+	int ret = esp_xtensa_init_arch_info(target, &esp32->esp_xtensa, &esp32s2_dm_cfg, &esp32s2_semihost_ops);
 	if (ret != ERROR_OK) {
 		LOG_ERROR("Failed to init arch info!");
 		free(esp32);
@@ -471,6 +494,13 @@ static const struct command_registration esp32s2_command_handlers[] = {
 	{
 		.chain = xtensa_command_handlers,
 	},
+	{
+		.name = "arm",
+		.mode = COMMAND_ANY,
+		.help = "ARM Command Group",
+		.usage = "",
+		.chain = semihosting_common_handlers
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/espressif/esp32s3.c b/src/target/espressif/esp32s3.c
index b870059..0da8552 100644
--- a/src/target/espressif/esp32s3.c
+++ b/src/target/espressif/esp32s3.c
@@ -13,6 +13,7 @@
 #include <target/target.h>
 #include <target/target_type.h>
 #include <target/smp.h>
+#include <target/semihosting_common.h>
 #include "assert.h"
 #include "esp_xtensa_smp.h"
 
@@ -302,7 +303,7 @@ static int esp32s3_virt2phys(struct target *target,
 
 static int esp32s3_target_init(struct command_context *cmd_ctx, struct target *target)
 {
-	return esp_xtensa_target_init(cmd_ctx, target);
+	return esp_xtensa_smp_target_init(cmd_ctx, target);
 }
 
 static const struct xtensa_debug_ops esp32s3_dbg_ops = {
@@ -321,6 +322,10 @@ static const struct esp_xtensa_smp_chip_ops esp32s3_chip_ops = {
 	.on_halt = esp32s3_on_halt
 };
 
+static const struct esp_semihost_ops esp32s3_semihost_ops = {
+	.prepare = esp32s3_disable_wdts
+};
+
 static int esp32s3_target_create(struct target *target, Jim_Interp *interp)
 {
 	struct xtensa_debug_module_config esp32s3_dm_cfg = {
@@ -340,7 +345,8 @@ static int esp32s3_target_create(struct target *target, Jim_Interp *interp)
 	int ret = esp_xtensa_smp_init_arch_info(target,
 		&esp32s3->esp_xtensa_smp,
 		&esp32s3_dm_cfg,
-		&esp32s3_chip_ops);
+		&esp32s3_chip_ops,
+		&esp32s3_semihost_ops);
 	if (ret != ERROR_OK) {
 		LOG_ERROR("Failed to init arch info!");
 		free(esp32s3);
@@ -363,6 +369,13 @@ static const struct command_registration esp32s3_command_handlers[] = {
 		.usage = "",
 		.chain = smp_command_handlers,
 	},
+	{
+		.name = "arm",
+		.mode = COMMAND_ANY,
+		.help = "ARM Command Group",
+		.usage = "",
+		.chain = semihosting_common_handlers
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/espressif/esp_semihosting.c b/src/target/espressif/esp_semihosting.c
new file mode 100644
index 0000000..b1edef3
--- /dev/null
+++ b/src/target/espressif/esp_semihosting.c
@@ -0,0 +1,127 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Semihosting API for Espressif chips                                   *
+ *   Copyright (C) 2022 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/log.h>
+#include <target/target.h>
+#include <target/semihosting_common.h>
+#include "esp_semihosting.h"
+#include "esp_xtensa.h"
+
+struct esp_semihost_data *target_to_esp_semihost_data(struct target *target)
+{
+	const char *arch = target_get_gdb_arch(target);
+	if (arch) {
+		if (strncmp(arch, "xtensa", 6) == 0)
+			return &target_to_esp_xtensa(target)->semihost;
+		/* TODO: add riscv */
+	}
+	LOG_ERROR("Unknown target arch!");
+	return NULL;
+}
+
+int esp_semihosting_sys_seek(struct target *target, uint64_t fd, uint32_t pos, size_t whence)
+{
+	struct semihosting *semihosting = target->semihosting;
+
+	semihosting->result = lseek(fd, pos, whence);
+	semihosting->sys_errno = errno;
+	LOG_TARGET_DEBUG(target, "lseek(%" PRIx64 ", %" PRIu32 " %" PRId64 ")=%d", fd, pos, semihosting->result, errno);
+	return ERROR_OK;
+}
+
+int esp_semihosting_common(struct target *target)
+{
+	struct semihosting *semihosting = target->semihosting;
+	if (!semihosting)
+		/* Silently ignore if the semihosting field was not set. */
+		return ERROR_OK;
+
+	int retval = ERROR_NOT_IMPLEMENTED;
+
+	/* Enough space to hold 4 long words. */
+	uint8_t fields[4 * 8];
+
+	/*
+	 * By default return an error.
+	 * The actual result must be set by each function
+	 */
+	semihosting->result = -1;
+	semihosting->sys_errno = EIO;
+
+	LOG_TARGET_DEBUG(target, "op=0x%x, param=0x%" PRIx64, semihosting->op, semihosting->param);
+
+	switch (semihosting->op) {
+	case ESP_SEMIHOSTING_SYS_DRV_INFO:
+		/* Return success to make esp-idf application happy */
+		retval = ERROR_OK;
+		semihosting->result = 0;
+		semihosting->sys_errno = 0;
+		break;
+
+	case ESP_SEMIHOSTING_SYS_SEEK:
+		retval = semihosting_read_fields(target, 3, fields);
+		if (retval == ERROR_OK) {
+			uint64_t fd = semihosting_get_field(target, 0, fields);
+			uint32_t pos = semihosting_get_field(target, 1, fields);
+			size_t whence = semihosting_get_field(target, 2, fields);
+			retval = esp_semihosting_sys_seek(target, fd, pos, whence);
+		}
+		break;
+
+	case ESP_SEMIHOSTING_SYS_APPTRACE_INIT:
+	case ESP_SEMIHOSTING_SYS_DEBUG_STUBS_INIT:
+	case ESP_SEMIHOSTING_SYS_BREAKPOINT_SET:
+	case ESP_SEMIHOSTING_SYS_WATCHPOINT_SET:
+		/* For the time being only riscv chips support these commands
+		 * TODO: invoke riscv custom command handler */
+		break;
+	}
+
+	return retval;
+}
+
+int esp_semihosting_basedir_command(struct command_invocation *cmd)
+{
+	struct target *target = get_current_target(CMD_CTX);
+
+	if (!target) {
+		LOG_ERROR("No target selected");
+		return ERROR_FAIL;
+	}
+
+	struct semihosting *semihosting = target->semihosting;
+	if (!semihosting) {
+		command_print(CMD, "semihosting not supported for current target");
+		return ERROR_FAIL;
+	}
+
+	if (!semihosting->is_active) {
+		if (semihosting->setup(target, true) != ERROR_OK) {
+			LOG_ERROR("Failed to Configure semihosting");
+			return ERROR_FAIL;
+		}
+		semihosting->is_active = true;
+	}
+
+	if (CMD_ARGC > 0) {
+		free(semihosting->basedir);
+		semihosting->basedir = strdup(CMD_ARGV[0]);
+		if (!semihosting->basedir) {
+			command_print(CMD, "semihosting failed to allocate memory for basedir!");
+			return ERROR_FAIL;
+		}
+	}
+
+	command_print(CMD, "DEPRECATED! semihosting base dir: %s",
+		semihosting->basedir ? semihosting->basedir : "");
+
+	return ERROR_OK;
+}
diff --git a/src/target/espressif/esp_semihosting.h b/src/target/espressif/esp_semihosting.h
new file mode 100644
index 0000000..bd2c079
--- /dev/null
+++ b/src/target/espressif/esp_semihosting.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Semihosting API for Espressif chips                                   *
+ *   Copyright (C) 2022 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESP_SEMIHOSTING_H
+#define OPENOCD_TARGET_ESP_SEMIHOSTING_H
+
+/* Legacy syscalls */
+#define ESP_SYS_DRV_INFO_LEGACY                     0xE0
+
+/* syscalls compatible to ARM standard */
+#define ESP_SEMIHOSTING_SYS_DRV_INFO                0x100
+#define ESP_SEMIHOSTING_SYS_APPTRACE_INIT           0x101
+#define ESP_SEMIHOSTING_SYS_DEBUG_STUBS_INIT        0x102
+#define ESP_SEMIHOSTING_SYS_BREAKPOINT_SET          0x103
+#define ESP_SEMIHOSTING_SYS_WATCHPOINT_SET          0x104
+#define ESP_SEMIHOSTING_SYS_SEEK                    0x105	/* custom lseek with whence */
+/* not implemented yet */
+#define ESP_SEMIHOSTING_SYS_MKDIR                   0x106
+#define ESP_SEMIHOSTING_SYS_OPENDIR                 0x107
+#define ESP_SEMIHOSTING_SYS_READDIR                 0x108
+#define ESP_SEMIHOSTING_SYS_READDIR_R               0x109
+#define ESP_SEMIHOSTING_SYS_SEEKDIR                 0x10A
+#define ESP_SEMIHOSTING_SYS_TELLDIR                 0x10B
+#define ESP_SEMIHOSTING_SYS_CLOSEDIR                0x10C
+#define ESP_SEMIHOSTING_SYS_RMDIR                   0x10D
+#define ESP_SEMIHOSTING_SYS_ACCESS                  0x10E
+#define ESP_SEMIHOSTING_SYS_TRUNCATE                0x10F
+#define ESP_SEMIHOSTING_SYS_UTIME                   0x110
+#define ESP_SEMIHOSTING_SYS_FSTAT                   0x111
+#define ESP_SEMIHOSTING_SYS_STAT                    0x112
+#define ESP_SEMIHOSTING_SYS_FSYNC                   0x113
+#define ESP_SEMIHOSTING_SYS_LINK                    0x114
+#define ESP_SEMIHOSTING_SYS_UNLINK                  0x115
+
+/**
+ * Semihost calls handling operations.
+ */
+struct esp_semihost_ops {
+	/** Callback called before handling semihost call */
+	int (*prepare)(struct target *target);
+};
+
+struct esp_semihost_data {
+	bool need_resume;
+	struct esp_semihost_ops *ops;
+};
+
+int esp_semihosting_common(struct target *target);
+int esp_semihosting_basedir_command(struct command_invocation *cmd);
+
+#endif	/* OPENOCD_TARGET_ESP_SEMIHOSTING_H */
diff --git a/src/target/espressif/esp_xtensa.c b/src/target/espressif/esp_xtensa.c
index fcd42ea..6a1b72e 100644
--- a/src/target/espressif/esp_xtensa.c
+++ b/src/target/espressif/esp_xtensa.c
@@ -12,14 +12,20 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <target/smp.h>
-#include "esp_xtensa.h"
 #include <target/register.h>
+#include "esp_xtensa.h"
+#include "esp_semihosting.h"
 
 int esp_xtensa_init_arch_info(struct target *target,
 	struct esp_xtensa_common *esp_xtensa,
-	struct xtensa_debug_module_config *dm_cfg)
+	struct xtensa_debug_module_config *dm_cfg,
+	const struct esp_semihost_ops *semihost_ops)
 {
-	return xtensa_init_arch_info(target, &esp_xtensa->xtensa, dm_cfg);
+	int ret = xtensa_init_arch_info(target, &esp_xtensa->xtensa, dm_cfg);
+	if (ret != ERROR_OK)
+		return ret;
+	esp_xtensa->semihost.ops = (struct esp_semihost_ops *)semihost_ops;
+	return ERROR_OK;
 }
 
 int esp_xtensa_target_init(struct command_context *cmd_ctx, struct target *target)
diff --git a/src/target/espressif/esp_xtensa.h b/src/target/espressif/esp_xtensa.h
index 61e87c0..1ad6c37 100644
--- a/src/target/espressif/esp_xtensa.h
+++ b/src/target/espressif/esp_xtensa.h
@@ -8,12 +8,14 @@
 #ifndef OPENOCD_TARGET_ESP_XTENSA_H
 #define OPENOCD_TARGET_ESP_XTENSA_H
 
-#include <helper/command.h>
 #include <target/target.h>
 #include <target/xtensa/xtensa.h>
+#include "esp_xtensa.h"
+#include "esp_semihosting.h"
 
 struct esp_xtensa_common {
 	struct xtensa xtensa;	/* must be the first element */
+	struct esp_semihost_data semihost;
 };
 
 static inline struct esp_xtensa_common *target_to_esp_xtensa(struct target *target)
@@ -23,7 +25,8 @@ static inline struct esp_xtensa_common *target_to_esp_xtensa(struct target *targ
 
 int esp_xtensa_init_arch_info(struct target *target,
 	struct esp_xtensa_common *esp_xtensa,
-	struct xtensa_debug_module_config *dm_cfg);
+	struct xtensa_debug_module_config *dm_cfg,
+	const struct esp_semihost_ops *semihost_ops);
 int esp_xtensa_target_init(struct command_context *cmd_ctx, struct target *target);
 void esp_xtensa_target_deinit(struct target *target);
 int esp_xtensa_arch_state(struct target *target);
diff --git a/src/target/espressif/esp_xtensa_semihosting.c b/src/target/espressif/esp_xtensa_semihosting.c
new file mode 100644
index 0000000..9ea8e4d
--- /dev/null
+++ b/src/target/espressif/esp_xtensa_semihosting.c
@@ -0,0 +1,114 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Copyright (c) 2020 Espressif Systems (Shanghai) Co. Ltd.              *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <target/semihosting_common.h>
+#include <target/xtensa/xtensa_regs.h>
+#include <target/xtensa/xtensa.h>
+#include "esp_xtensa.h"
+#include "esp_xtensa_semihosting.h"
+
+#define ESP_XTENSA_SYSCALL              0x41E0	/* XT_INS_BREAK(1, 14) */
+#define ESP_XTENSA_SYSCALL_SZ           3
+
+#define XTENSA_SYSCALL_OP_REG           XT_REG_IDX_A2
+#define XTENSA_SYSCALL_RETVAL_REG       XT_REG_IDX_A2
+#define XTENSA_SYSCALL_ERRNO_REG        XT_REG_IDX_A3
+
+static int esp_xtensa_semihosting_setup(struct target *target, int enable)
+{
+	LOG_TARGET_DEBUG(target, "semihosting enable=%d", enable);
+
+	return ERROR_OK;
+}
+
+static int esp_xtensa_semihosting_post_result(struct target *target)
+{
+	/* Even with the v2 and later, errno will not retrieved from A3 reg, it is safe to set */
+	xtensa_reg_set(target, XTENSA_SYSCALL_RETVAL_REG, target->semihosting->result);
+	xtensa_reg_set(target, XTENSA_SYSCALL_ERRNO_REG, target->semihosting->sys_errno);
+	return ERROR_OK;
+}
+
+/**
+ * Checks and processes an ESP Xtensa semihosting request. This is meant
+ * to be called when the target is stopped due to a debug mode entry.
+ * If the value 0 is returned then there was nothing to process. A non-zero
+ * return value signifies that a request was processed and the target resumed,
+ * or an error was encountered, in which case the caller must return immediately.
+ *
+ * @param target Pointer to the ESP Xtensa target to process.
+ * @param retval Pointer to a location where the return code will be stored
+ * @return SEMIHOSTING_HANDLED if a request was processed or SEMIHOSTING_NONE with the proper retval
+ */
+int esp_xtensa_semihosting(struct target *target, int *retval)
+{
+	struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
+
+	xtensa_reg_val_t dbg_cause = xtensa_reg_get(target, XT_REG_IDX_DEBUGCAUSE);
+	if ((dbg_cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN)) == 0)
+		return SEMIHOSTING_NONE;
+
+	uint8_t brk_insn_buf[sizeof(uint32_t)] = { 0 };
+	xtensa_reg_val_t pc = xtensa_reg_get(target, XT_REG_IDX_PC);
+	*retval = target_read_memory(target, pc, ESP_XTENSA_SYSCALL_SZ, 1, brk_insn_buf);
+	if (*retval != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to read break instruction!");
+		return SEMIHOSTING_NONE;
+	}
+
+	uint32_t syscall_ins = buf_get_u32(brk_insn_buf, 0, 32);
+	if (syscall_ins != ESP_XTENSA_SYSCALL) {
+		*retval = ERROR_OK;
+		return SEMIHOSTING_NONE;
+	}
+
+	if (esp_xtensa->semihost.ops && esp_xtensa->semihost.ops->prepare)
+		esp_xtensa->semihost.ops->prepare(target);
+
+	xtensa_reg_val_t a2 = xtensa_reg_get(target, XT_REG_IDX_A2);
+	xtensa_reg_val_t a3 = xtensa_reg_get(target, XT_REG_IDX_A3);
+	LOG_TARGET_DEBUG(target, "Semihosting call 0x%" PRIx32 " 0x%" PRIx32 " Base dir '%s'",
+		a2,
+		a3,
+		target->semihosting->basedir ? target->semihosting->basedir : "");
+
+	target->semihosting->op = a2;
+	target->semihosting->param = a3;
+
+	*retval = semihosting_common(target);
+
+	/* Most operations are resumable, except the two exit calls. */
+	if (*retval != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Semihosting operation (op: 0x%x) error! Code: %d",
+			target->semihosting->op,
+			*retval);
+	}
+
+	/* Resume if target it is resumable and we are not waiting on a fileio operation to complete. */
+	if (target->semihosting->is_resumable && !target->semihosting->hit_fileio)
+		target_to_esp_xtensa(target)->semihost.need_resume = true;
+
+	return SEMIHOSTING_HANDLED;
+}
+
+static int xtensa_semihosting_init(struct target *target)
+{
+	return semihosting_common_init(target, esp_xtensa_semihosting_setup, esp_xtensa_semihosting_post_result);
+}
+
+int esp_xtensa_semihosting_init(struct target *target)
+{
+	int retval = xtensa_semihosting_init(target);
+	if (retval != ERROR_OK)
+		return retval;
+	target->semihosting->word_size_bytes = 4;			/* 32 bits */
+	target->semihosting->user_command_extension = esp_semihosting_common;
+	return ERROR_OK;
+}
diff --git a/src/target/espressif/esp_xtensa_semihosting.h b/src/target/espressif/esp_xtensa_semihosting.h
new file mode 100644
index 0000000..1da3115
--- /dev/null
+++ b/src/target/espressif/esp_xtensa_semihosting.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Copyright (c) 2020 Espressif Systems (Shanghai) Co. Ltd.              *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESP_XTENSA_SEMIHOSTING_H
+#define OPENOCD_TARGET_ESP_XTENSA_SEMIHOSTING_H
+
+#include <target/target.h>
+
+int esp_xtensa_semihosting_init(struct target *target);
+int esp_xtensa_semihosting(struct target *target, int *retval);
+
+#endif	/* OPENOCD_TARGET_ESP_XTENSA_SEMIHOSTING_H */
diff --git a/src/target/espressif/esp_xtensa_smp.c b/src/target/espressif/esp_xtensa_smp.c
index b109f3c..235a86e 100644
--- a/src/target/espressif/esp_xtensa_smp.c
+++ b/src/target/espressif/esp_xtensa_smp.c
@@ -13,7 +13,9 @@
 #include <target/target.h>
 #include <target/target_type.h>
 #include <target/smp.h>
+#include <target/semihosting_common.h>
 #include "esp_xtensa_smp.h"
+#include "esp_xtensa_semihosting.h"
 
 /*
 Multiprocessor stuff common:
@@ -128,6 +130,7 @@ int esp_xtensa_smp_poll(struct target *target)
 {
 	enum target_state old_state = target->state;
 	struct esp_xtensa_smp_common *esp_xtensa_smp = target_to_esp_xtensa_smp(target);
+	struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
 	struct target_list *head;
 	struct target *curr;
 	bool other_core_resume_req = false;
@@ -180,6 +183,19 @@ int esp_xtensa_smp_poll(struct target *target)
 		if (old_state == TARGET_DEBUG_RUNNING) {
 			target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 		} else {
+			if (esp_xtensa_semihosting(target, &ret) == SEMIHOSTING_HANDLED) {
+				if (ret == ERROR_OK && esp_xtensa->semihost.need_resume &&
+					!esp_xtensa_smp->other_core_does_resume) {
+					esp_xtensa->semihost.need_resume = false;
+					/* Resume xtensa_resume will handle BREAK instruction. */
+					ret = target_resume(target, 1, 0, 1, 0);
+					if (ret != ERROR_OK) {
+						LOG_ERROR("Failed to resume target");
+						return ret;
+					}
+				}
+				return ret;
+			}
 			/* check whether any core polled by esp_xtensa_smp_update_halt_gdb() requested resume */
 			if (target->smp && other_core_resume_req) {
 				/* Resume xtensa_resume will handle BREAK instruction. */
@@ -253,6 +269,11 @@ static int esp_xtensa_smp_update_halt_gdb(struct target *target, bool *need_resu
 		if (ret != ERROR_OK)
 			return ret;
 		esp_xtensa_smp->other_core_does_resume = false;
+		struct esp_xtensa_common *curr_esp_xtensa = target_to_esp_xtensa(curr);
+		if (curr_esp_xtensa->semihost.need_resume) {
+			curr_esp_xtensa->semihost.need_resume = false;
+			*need_resume = true;
+		}
 	}
 
 	/* after all targets were updated, poll the gdb serving target */
@@ -451,9 +472,10 @@ int esp_xtensa_smp_watchpoint_remove(struct target *target, struct watchpoint *w
 int esp_xtensa_smp_init_arch_info(struct target *target,
 	struct esp_xtensa_smp_common *esp_xtensa_smp,
 	struct xtensa_debug_module_config *dm_cfg,
-	const struct esp_xtensa_smp_chip_ops *chip_ops)
+	const struct esp_xtensa_smp_chip_ops *chip_ops,
+	const struct esp_semihost_ops *semihost_ops)
 {
-	int ret = esp_xtensa_init_arch_info(target, &esp_xtensa_smp->esp_xtensa, dm_cfg);
+	int ret = esp_xtensa_init_arch_info(target, &esp_xtensa_smp->esp_xtensa, dm_cfg, semihost_ops);
 	if (ret != ERROR_OK)
 		return ret;
 	esp_xtensa_smp->chip_ops = chip_ops;
@@ -463,7 +485,24 @@ int esp_xtensa_smp_init_arch_info(struct target *target,
 
 int esp_xtensa_smp_target_init(struct command_context *cmd_ctx, struct target *target)
 {
-	return esp_xtensa_target_init(cmd_ctx, target);
+	int ret = esp_xtensa_target_init(cmd_ctx, target);
+	if (ret != ERROR_OK)
+		return ret;
+
+	if (target->smp) {
+		struct target_list *head;
+		foreach_smp_target(head, target->smp_targets) {
+			struct target *curr = head->target;
+			ret = esp_xtensa_semihosting_init(curr);
+			if (ret != ERROR_OK)
+				return ret;
+		}
+	} else {
+		ret = esp_xtensa_semihosting_init(target);
+		if (ret != ERROR_OK)
+			return ret;
+	}
+	return ERROR_OK;
 }
 
 COMMAND_HANDLER(esp_xtensa_smp_cmd_xtdef)
diff --git a/src/target/espressif/esp_xtensa_smp.h b/src/target/espressif/esp_xtensa_smp.h
index bafd420..aeb1d61 100644
--- a/src/target/espressif/esp_xtensa_smp.h
+++ b/src/target/espressif/esp_xtensa_smp.h
@@ -44,7 +44,8 @@ int esp_xtensa_smp_target_init(struct command_context *cmd_ctx, struct target *t
 int esp_xtensa_smp_init_arch_info(struct target *target,
 	struct esp_xtensa_smp_common *esp_xtensa_smp,
 	struct xtensa_debug_module_config *dm_cfg,
-	const struct esp_xtensa_smp_chip_ops *chip_ops);
+	const struct esp_xtensa_smp_chip_ops *chip_ops,
+	const struct esp_semihost_ops *semihost_ops);
 
 extern const struct command_registration esp_xtensa_smp_command_handlers[];
 extern const struct command_registration esp_xtensa_smp_xtensa_command_handlers[];