target/espressif: read entry addresses of pre-defined stub functions

Debug stubs functionality provided by ESP IDF allows executing
target function in any address. e.g; esp32_cmd_gcov()

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

6 files changed, 300 insertions, 21 deletions

diff --git a/src/target/espressif/Makefile.am b/src/target/espressif/Makefile.am
index 14625d4..776818f 100644
--- a/src/target/espressif/Makefile.am
+++ b/src/target/espressif/Makefile.am
@@ -2,21 +2,23 @@
 
 noinst_LTLIBRARIES += %D%/libespressif.la
 %C%_libespressif_la_SOURCES = \
-       %D%/esp_xtensa.c \
-       %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%/esp_xtensa_apptrace.c \
-       %D%/esp_xtensa_apptrace.h \
-       %D%/esp32_apptrace.c \
-       %D%/esp32_apptrace.h \
-       %D%/esp32.c \
-       %D%/esp32s2.c \
-       %D%/esp32s3.c \
-       %D%/esp32_sysview.c \
-       %D%/esp32_sysview.h \
-       %D%/segger_sysview.h \
-       %D%/esp_semihosting.c \
-       %D%/esp_semihosting.h
+	%D%/esp_xtensa.c \
+	%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%/esp_xtensa_apptrace.c \
+	%D%/esp_xtensa_apptrace.h \
+	%D%/esp32_apptrace.c \
+	%D%/esp32_apptrace.h \
+	%D%/esp32.c \
+	%D%/esp32s2.c \
+	%D%/esp32s3.c \
+	%D%/esp.c \
+	%D%/esp.h \
+	%D%/esp32_sysview.c \
+	%D%/esp32_sysview.h \
+	%D%/segger_sysview.h \
+	%D%/esp_semihosting.c \
+	%D%/esp_semihosting.h
diff --git a/src/target/espressif/esp.c b/src/target/espressif/esp.c
new file mode 100644
index 0000000..9583d64
--- /dev/null
+++ b/src/target/espressif/esp.c
@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ *   Espressif chips common target API for OpenOCD                         *
+ *   Copyright (C) 2021 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/log.h>
+#include <helper/binarybuffer.h>
+#include "target/target.h"
+#include "esp.h"
+
+int esp_dbgstubs_table_read(struct target *target, struct esp_dbg_stubs *dbg_stubs)
+{
+	uint32_t table_size, table_start_id, desc_entry_id, gcov_entry_id;
+	uint32_t entries[ESP_DBG_STUB_ENTRY_MAX] = {0};
+	uint8_t entry_buff[sizeof(entries)] = {0}; /* to avoid endiannes issues */
+
+	LOG_TARGET_DEBUG(target, "Read debug stubs info %" PRIx32 " / %d", dbg_stubs->base, dbg_stubs->entries_count);
+
+	/* First of, read 2 entries to get magic num and table size */
+	int res = target_read_buffer(target, dbg_stubs->base, sizeof(uint32_t) * 2, entry_buff);
+	if (res != ERROR_OK) {
+		LOG_ERROR("%s: Failed to read first debug stub entry!", target_name(target));
+		return res;
+	}
+	entries[0] = target_buffer_get_u32(target, entry_buff);
+	entries[1] = target_buffer_get_u32(target, entry_buff + sizeof(uint32_t));
+
+	if (entries[0] != ESP_DBG_STUB_MAGIC_NUM_VAL) {
+		/* idf with the old table entry structure */
+		table_size = 2;
+		table_start_id = 0;
+		desc_entry_id = 0;
+		gcov_entry_id = 1;
+	} else {
+		table_size = entries[1];
+		table_start_id = ESP_DBG_STUB_TABLE_START;
+		desc_entry_id = ESP_DBG_STUB_TABLE_START;
+		gcov_entry_id = ESP_DBG_STUB_ENTRY_FIRST;
+
+		/* discard unsupported entries */
+		if (table_size > ESP_DBG_STUB_ENTRY_MAX)
+			table_size = ESP_DBG_STUB_ENTRY_MAX;
+
+		/* now read the remaining entries */
+		res = target_read_buffer(target, dbg_stubs->base + 2 * sizeof(uint32_t), sizeof(uint32_t) * table_size - 2,
+			entry_buff + sizeof(uint32_t) * 2);
+		if (res != ERROR_OK) {
+			LOG_TARGET_ERROR(target, "Failed to read debug stubs info!");
+			return res;
+		}
+		for (unsigned int i = 2; i < table_size; ++i)
+			entries[i] = target_buffer_get_u32(target, entry_buff + sizeof(uint32_t) * i);
+
+		dbg_stubs->entries[ESP_DBG_STUB_CAPABILITIES] = entries[ESP_DBG_STUB_CAPABILITIES];
+	}
+
+	dbg_stubs->entries[ESP_DBG_STUB_DESC] = entries[desc_entry_id];
+	dbg_stubs->entries[ESP_DBG_STUB_ENTRY_GCOV] = entries[gcov_entry_id];
+
+	for (enum esp_dbg_stub_id i = ESP_DBG_STUB_DESC; i < ESP_DBG_STUB_ENTRY_MAX; i++) {
+		LOG_DEBUG("Check dbg stub %d - %x", i, dbg_stubs->entries[i]);
+		if (dbg_stubs->entries[i]) {
+			LOG_DEBUG("New dbg stub %d at %x", dbg_stubs->entries_count, dbg_stubs->entries[i]);
+			dbg_stubs->entries_count++;
+		}
+	}
+	if (dbg_stubs->entries_count < table_size - table_start_id)
+		LOG_WARNING("Not full dbg stub table %d of %d", dbg_stubs->entries_count, table_size - table_start_id);
+
+	return ERROR_OK;
+}
diff --git a/src/target/espressif/esp.h b/src/target/espressif/esp.h
new file mode 100644
index 0000000..3ba2b8b
--- /dev/null
+++ b/src/target/espressif/esp.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Espressif chips common target API for OpenOCD                         *
+ *   Copyright (C) 2021 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESP_H
+#define OPENOCD_TARGET_ESP_H
+
+#include <stdint.h>
+#include <helper/bits.h>
+
+/* must be in sync with ESP-IDF version */
+/** Size of the pre-compiled target buffer for stub trampoline.
+ * @note Must be in sync with ESP-IDF version */
+#define ESP_DBG_STUBS_CODE_BUF_SIZE         32	/* TODO: move this info to esp_dbg_stubs_desc */
+/** Size of the pre-compiled target buffer for stack.
+ * @note Must be in sync with ESP-IDF version */
+#define ESP_DBG_STUBS_STACK_MIN_SIZE        2048/* TODO: move this info to esp_dbg_stubs_desc */
+
+/**
+ * Debug stubs table entries IDs
+ *
+ * @note Must be in sync with ESP-IDF version
+ */
+enum esp_dbg_stub_id {
+	ESP_DBG_STUB_ENTRY_MAGIC_NUM,
+	ESP_DBG_STUB_TABLE_SIZE,
+	ESP_DBG_STUB_TABLE_START,
+	ESP_DBG_STUB_DESC = ESP_DBG_STUB_TABLE_START,	/*< Stubs descriptor ID */
+	ESP_DBG_STUB_ENTRY_FIRST,
+	ESP_DBG_STUB_ENTRY_GCOV = ESP_DBG_STUB_ENTRY_FIRST,	/*< GCOV stub ID */
+	ESP_DBG_STUB_CAPABILITIES,
+	/* add new stub entries here */
+	ESP_DBG_STUB_ENTRY_MAX,
+};
+
+#define ESP_DBG_STUB_MAGIC_NUM_VAL      0xFEEDBEEF
+#define ESP_DBG_STUB_CAP_GCOV_THREAD    BIT(0)
+
+/**
+ * Debug stubs descriptor. ID: ESP_DBG_STUB_DESC
+ *
+ * @note Must be in sync with ESP-IDF version
+ */
+struct esp_dbg_stubs_desc {
+	/** Address of pre-compiled target buffer for stub trampoline.
+	 * Size of the buffer is ESP_DBG_STUBS_CODE_BUF_SIZE
+	 */
+	uint32_t tramp_addr;
+	/** Pre-compiled target buffer's addr for stack. The size of the buffer is ESP_DBG_STUBS_STACK_MIN_SIZE.
+	 * Target has the buffer which is used for the stack of onboard algorithms.
+	 * If stack size required by algorithm exceeds ESP_DBG_STUBS_STACK_MIN_SIZE,
+	 * it should be allocated using onboard function pointed by 'data_alloc' and
+	 * freed by 'data_free'. They fit to the minimal stack. See below.
+	 */
+	uint32_t min_stack_addr;
+	/** Address of malloc-like function to allocate buffer on target. */
+	uint32_t data_alloc;
+	/** Address of free-like function to free buffer allocated with data_alloc. */
+	uint32_t data_free;
+};
+
+/**
+ * Debug stubs info.
+ */
+struct esp_dbg_stubs {
+	/** Address. */
+	uint32_t base;
+	/** Table contents. */
+	uint32_t entries[ESP_DBG_STUB_ENTRY_MAX];
+	/** Number of table entries. */
+	uint32_t entries_count;
+	/** Debug stubs decsriptor. */
+	struct esp_dbg_stubs_desc desc;
+};
+
+struct esp_common {
+	struct esp_dbg_stubs dbg_stubs;
+};
+
+int esp_dbgstubs_table_read(struct target *target, struct esp_dbg_stubs *dbg_stubs);
+
+#endif	/* OPENOCD_TARGET_ESP_H */
diff --git a/src/target/espressif/esp_xtensa.c b/src/target/espressif/esp_xtensa.c
index 3dfcc0f..0bd2cdd 100644
--- a/src/target/espressif/esp_xtensa.c
+++ b/src/target/espressif/esp_xtensa.c
@@ -17,9 +17,46 @@
 #include "esp_xtensa.h"
 #include "esp_semihosting.h"
 
+#define ESP_XTENSA_DBGSTUBS_UPDATE_DATA_ENTRY(_e_) \
+	do { \
+		uint32_t __internal_val = (_e_); \
+		if (!xtensa_data_addr_valid(target, __internal_val)) { \
+			LOG_ERROR("No valid stub data entry found (0x%" PRIx32 ")!", __internal_val); \
+			return;	\
+		} \
+	} while (0)
+
+#define ESP_XTENSA_DBGSTUBS_UPDATE_CODE_ENTRY(_e_) \
+	do { \
+		uint32_t __internal_val = (_e_); \
+		if (__internal_val == 0) { \
+			LOG_ERROR("No valid stub code entry found (0x%" PRIx32 ")!", __internal_val); \
+			return;	\
+		} \
+	} while (0)
+
+static void esp_xtensa_dbgstubs_info_update(struct target *target);
+static void esp_xtensa_dbgstubs_addr_check(struct target *target);
+
+static int esp_xtensa_dbgstubs_restore(struct target *target)
+{
+	struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
+
+	if (esp_xtensa->esp.dbg_stubs.base == 0)
+		return ERROR_OK;
+
+	LOG_TARGET_INFO(target, "Restore debug stubs address %" PRIx32, esp_xtensa->esp.dbg_stubs.base);
+	int res = esp_xtensa_apptrace_status_reg_write(target, esp_xtensa->esp.dbg_stubs.base);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write trace status (%d)!", res);
+		return res;
+	}
+	return ERROR_OK;
+}
 int esp_xtensa_on_halt(struct target *target)
 {
-	/* will be used in the next patches */
+	/* debug stubs can be used in HALTED state only, so it is OK to get info about them here */
+	esp_xtensa_dbgstubs_info_update(target);
 	return ERROR_OK;
 }
 
@@ -45,6 +82,11 @@ void esp_xtensa_target_deinit(struct target *target)
 {
 	LOG_DEBUG("start");
 
+	if (target_was_examined(target)) {
+		int ret = esp_xtensa_dbgstubs_restore(target);
+		if (ret != ERROR_OK)
+			return;
+	}
 	xtensa_target_deinit(target);
 	free(target_to_esp_xtensa(target));	/* same as free(xtensa) */
 }
@@ -56,7 +98,68 @@ int esp_xtensa_arch_state(struct target *target)
 
 int esp_xtensa_poll(struct target *target)
 {
-	return xtensa_poll(target);
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct esp_xtensa_common *esp_xtensa_common = target_to_esp_xtensa(target);
+
+	int ret = xtensa_poll(target);
+
+	if (xtensa_dm_power_status_get(&xtensa->dbg_mod) & PWRSTAT_COREWASRESET(xtensa)) {
+		LOG_TARGET_DEBUG(target, "Clear debug stubs info");
+		memset(&esp_xtensa_common->esp.dbg_stubs, 0, sizeof(esp_xtensa_common->esp.dbg_stubs));
+	}
+	if (target->state != TARGET_DEBUG_RUNNING)
+		esp_xtensa_dbgstubs_addr_check(target);
+	return ret;
+}
+
+static void esp_xtensa_dbgstubs_addr_check(struct target *target)
+{
+	struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
+	uint32_t vec_addr = 0;
+
+	if (esp_xtensa->esp.dbg_stubs.base != 0)
+		return;
+
+	int res = esp_xtensa_apptrace_status_reg_read(target, &vec_addr);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to read debug stubs address location (%d)!", res);
+		return;
+	}
+	if (xtensa_data_addr_valid(target, vec_addr)) {
+		LOG_TARGET_INFO(target, "Detected debug stubs @ %" PRIx32, vec_addr);
+		res = esp_xtensa_apptrace_status_reg_write(target, 0);
+		if (res != ERROR_OK)
+			LOG_ERROR("Failed to clear debug stubs address location (%d)!", res);
+		esp_xtensa->esp.dbg_stubs.base = vec_addr;
+	}
+}
+
+static void esp_xtensa_dbgstubs_info_update(struct target *target)
+{
+	struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
+
+	if (esp_xtensa->esp.dbg_stubs.base == 0 || esp_xtensa->esp.dbg_stubs.entries_count != 0)
+		return;
+
+	int res = esp_dbgstubs_table_read(target, &esp_xtensa->esp.dbg_stubs);
+	if (res != ERROR_OK)
+		return;
+	if (esp_xtensa->esp.dbg_stubs.entries_count == 0)
+		return;
+
+	/* read debug stubs descriptor */
+	ESP_XTENSA_DBGSTUBS_UPDATE_DATA_ENTRY(esp_xtensa->esp.dbg_stubs.entries[ESP_DBG_STUB_DESC]);
+	res = target_read_buffer(target, esp_xtensa->esp.dbg_stubs.entries[ESP_DBG_STUB_DESC],
+		sizeof(struct esp_dbg_stubs_desc),
+		(uint8_t *)&esp_xtensa->esp.dbg_stubs.desc);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to read debug stubs descriptor (%d)!", res);
+		return;
+	}
+	ESP_XTENSA_DBGSTUBS_UPDATE_CODE_ENTRY(esp_xtensa->esp.dbg_stubs.desc.tramp_addr);
+	ESP_XTENSA_DBGSTUBS_UPDATE_DATA_ENTRY(esp_xtensa->esp.dbg_stubs.desc.min_stack_addr);
+	ESP_XTENSA_DBGSTUBS_UPDATE_CODE_ENTRY(esp_xtensa->esp.dbg_stubs.desc.data_alloc);
+	ESP_XTENSA_DBGSTUBS_UPDATE_CODE_ENTRY(esp_xtensa->esp.dbg_stubs.desc.data_free);
 }
 
 int esp_xtensa_breakpoint_add(struct target *target, struct breakpoint *breakpoint)
diff --git a/src/target/espressif/esp_xtensa.h b/src/target/espressif/esp_xtensa.h
index 0b06b03..00f67a3 100644
--- a/src/target/espressif/esp_xtensa.h
+++ b/src/target/espressif/esp_xtensa.h
@@ -10,12 +10,13 @@
 
 #include <target/target.h>
 #include <target/xtensa/xtensa.h>
-#include "esp_xtensa.h"
 #include "esp_semihosting.h"
+#include "esp.h"
 #include "esp_xtensa_apptrace.h"
 
 struct esp_xtensa_common {
 	struct xtensa xtensa;	/* must be the first element */
+	struct esp_common esp;
 	struct esp_semihost_data semihost;
 	struct esp_xtensa_apptrace_info apptrace;
 };
diff --git a/src/target/espressif/esp_xtensa_smp.c b/src/target/espressif/esp_xtensa_smp.c
index 93c53f1..1d70be9 100644
--- a/src/target/espressif/esp_xtensa_smp.c
+++ b/src/target/espressif/esp_xtensa_smp.c
@@ -146,6 +146,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);
+	uint32_t old_dbg_stubs_base = esp_xtensa->esp.dbg_stubs.base;
 	struct target_list *head;
 	struct target *curr;
 	bool other_core_resume_req = false;
@@ -163,6 +164,16 @@ int esp_xtensa_smp_poll(struct target *target)
 	if (ret != ERROR_OK)
 		return ret;
 
+	if (esp_xtensa->esp.dbg_stubs.base && old_dbg_stubs_base != esp_xtensa->esp.dbg_stubs.base) {
+		/* debug stubs base is set only in PRO-CPU TRAX register, so sync this info */
+		foreach_smp_target(head, target->smp_targets) {
+			curr = head->target;
+			if (curr == target)
+				continue;
+			target_to_esp_xtensa(curr)->esp.dbg_stubs.base = esp_xtensa->esp.dbg_stubs.base;
+		}
+	}
+
 	if (target->smp) {
 		if (target->state == TARGET_RESET) {
 			esp_xtensa_smp->examine_other_cores = ESP_XTENSA_SMP_EXAMINE_OTHER_CORES;