jtag/drivers: dmem: Add Emulated AP mode

This emulation mode supports software translation of an AP request
into an address mapped transaction that does not rely on physical AP
hardware. This is necessary in some hardware such as K3 SoCs since the
hardware architecture anticipates a potential race condition between
AP doing direct memory access generating transactions back to system
bus and firewalls that data path out.

This emulation mode allows direct memory driver to emulate CoreSight
Access Port (AP) and reuse the SoC configuration meant for JTAG
debuggers.

Since the address ranges are flat in nature, the requisite memory base
and size will need to be provided a-priori to the driver for mapping.
The other design alternative would be to map requested memory map for
every register operation, but, that would defeat our intent of getting
max debug performance.

Signed-off-by: Nishanth Menon <nm@ti.com>
Signed-off-by: Jason Peck <jpeck@ti.com>
Change-Id: I2d3c5f7833f1973e90b4f6b247827f62fc2905d0
Reviewed-on: https://review.openocd.org/c/openocd/+/7089
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

2 files changed, 299 insertions, 6 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 6f16c9f..d07188c 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -3626,6 +3626,19 @@ Set the address offset between Access Ports (APs).
 Set the maximum number of valid access ports on the SoC.
 @end deffn
 
+@deffn {Config Command} {dmem emu_ap_list} n
+Set the list of Access Ports (APs) that need to be emulated. This
+emulation mode supports software translation of an AP request into an
+address mapped transaction that does not rely on physical AP hardware.
+This maybe needed if the AP is either denied access via memory map or
+protected using other SoC mechanisms.
+@end deffn
+
+@deffn {Config Command} {dmem emu_base_address_range} base_address address_window_size
+Set the emulated address and address window size. Both of these
+parameters must be aligned to page size.
+@end deffn
+
 @end deffn
 
 @section Transport Configuration
diff --git a/src/jtag/drivers/dmem.c b/src/jtag/drivers/dmem.c
index 8d603ad..4dc5821 100644
--- a/src/jtag/drivers/dmem.c
+++ b/src/jtag/drivers/dmem.c
@@ -25,6 +25,19 @@
 #include <target/arm_adi_v5.h>
 #include <transport/transport.h>
 
+struct dmem_emu_ap_info {
+	uint64_t ap_num;
+	/* Emulation mode AP state variables */
+	uint32_t apbap_tar;
+	uint32_t apbap_csw;
+};
+
+/*
+ * This bit tells if the transaction is coming in from jtag or not
+ * we just mask this out to emulate direct address access
+ */
+#define ARM_APB_PADDR31  BIT(31)
+
 static void *dmem_map_base, *dmem_virt_base_addr;
 static size_t dmem_mapped_size;
 
@@ -35,6 +48,176 @@ static uint64_t dmem_dap_base_address;
 static unsigned int dmem_dap_max_aps = 1;
 static uint32_t dmem_dap_ap_offset = 0x100;
 
+/* DAP error code. */
+static int dmem_dap_retval = ERROR_OK;
+
+/* AP Emulation Mode */
+static uint64_t dmem_emu_base_address;
+static uint64_t dmem_emu_size;
+static void *dmem_emu_map_base, *dmem_emu_virt_base_addr;
+static size_t dmem_emu_mapped_size;
+#define DMEM_MAX_EMULATE_APS 5
+static unsigned int dmem_emu_ap_count;
+static struct dmem_emu_ap_info dmem_emu_ap_list[DMEM_MAX_EMULATE_APS];
+
+/*
+ * This helper is used to determine the TAR increment size in bytes.  The AP's
+ * CSW encoding for SIZE supports byte count decode using "1 << SIZE".
+ */
+static uint32_t dmem_memap_tar_inc(uint32_t csw)
+{
+	if ((csw & CSW_ADDRINC_MASK) != 0)
+		return 1 << (csw & CSW_SIZE_MASK);
+	return 0;
+}
+
+/*
+ * EMULATION MODE: In Emulation MODE, we assume the following:
+ * TCL still describes as system is operational from the view of AP (ex. jtag)
+ * However, the hardware doesn't permit direct memory access to these APs
+ * (only permitted via JTAG).
+ *
+ * So, the access to these APs have to be decoded to a memory map
+ * access which we can directly access.
+ *
+ * A few TI processors have this issue.
+ */
+static bool dmem_is_emulated_ap(struct adiv5_ap *ap, unsigned int *idx)
+{
+	for (unsigned int i = 0; i < dmem_emu_ap_count; i++) {
+		if (ap->ap_num == dmem_emu_ap_list[i].ap_num) {
+			*idx = i;
+			return true;
+		}
+	}
+	return false;
+}
+
+static void dmem_emu_set_ap_reg(uint64_t addr, uint32_t val)
+{
+	addr &= ~ARM_APB_PADDR31;
+
+	*(volatile uint32_t *)((uintptr_t)dmem_emu_virt_base_addr + addr) = val;
+}
+
+static uint32_t dmem_emu_get_ap_reg(uint64_t addr)
+{
+	uint32_t val;
+
+	addr &= ~ARM_APB_PADDR31;
+
+	val = *(volatile uint32_t *)((uintptr_t)dmem_emu_virt_base_addr + addr);
+
+	return val;
+}
+
+static int dmem_emu_ap_q_read(unsigned int ap_idx, unsigned int reg, uint32_t *data)
+{
+	uint64_t addr;
+	int ret = ERROR_OK;
+	struct dmem_emu_ap_info *ap_info = &dmem_emu_ap_list[ap_idx];
+
+	switch (reg) {
+		case ADIV5_MEM_AP_REG_CSW:
+			*data = ap_info->apbap_csw;
+			break;
+		case ADIV5_MEM_AP_REG_TAR:
+			*data = ap_info->apbap_tar;
+			break;
+		case ADIV5_MEM_AP_REG_CFG:
+			*data = 0;
+			break;
+		case ADIV5_MEM_AP_REG_BASE:
+			*data = 0;
+			break;
+		case ADIV5_AP_REG_IDR:
+			*data = 0;
+			break;
+		case ADIV5_MEM_AP_REG_BD0:
+		case ADIV5_MEM_AP_REG_BD1:
+		case ADIV5_MEM_AP_REG_BD2:
+		case ADIV5_MEM_AP_REG_BD3:
+			addr = (ap_info->apbap_tar & ~0xf) + (reg & 0x0C);
+
+			*data = dmem_emu_get_ap_reg(addr);
+
+			break;
+		case ADIV5_MEM_AP_REG_DRW:
+			addr = ap_info->apbap_tar;
+
+			*data = dmem_emu_get_ap_reg(addr);
+
+			ap_info->apbap_tar += dmem_memap_tar_inc(ap_info->apbap_csw);
+			break;
+		default:
+			LOG_INFO("%s: Unknown reg: 0x%02x", __func__, reg);
+			ret = ERROR_FAIL;
+			break;
+	}
+
+	/* Track the last error code. */
+	if (ret != ERROR_OK)
+		dmem_dap_retval = ret;
+
+	return ret;
+}
+
+static int dmem_emu_ap_q_write(unsigned int ap_idx, unsigned int reg, uint32_t data)
+{
+	uint64_t addr;
+	int ret = ERROR_OK;
+	struct dmem_emu_ap_info *ap_info = &dmem_emu_ap_list[ap_idx];
+
+	switch (reg) {
+		case ADIV5_MEM_AP_REG_CSW:
+			/*
+			 * This implementation only supports 32-bit accesses.
+			 * Force this by ensuring CSW_SIZE field indicates 32-BIT.
+			 */
+			ap_info->apbap_csw = ((data & ~CSW_SIZE_MASK) | CSW_32BIT);
+			break;
+		case ADIV5_MEM_AP_REG_TAR:
+			/*
+			 * This implementation only supports 32-bit accesses.
+			 * Force LS 2-bits of TAR to 00b
+			 */
+			ap_info->apbap_tar = (data & ~0x3);
+			break;
+
+		case ADIV5_MEM_AP_REG_CFG:
+		case ADIV5_MEM_AP_REG_BASE:
+		case ADIV5_AP_REG_IDR:
+			/* We don't use this, so we don't need to store */
+			break;
+
+		case ADIV5_MEM_AP_REG_BD0:
+		case ADIV5_MEM_AP_REG_BD1:
+		case ADIV5_MEM_AP_REG_BD2:
+		case ADIV5_MEM_AP_REG_BD3:
+			addr = (ap_info->apbap_tar & ~0xf) + (reg & 0x0C);
+
+			dmem_emu_set_ap_reg(addr, data);
+
+			break;
+		case ADIV5_MEM_AP_REG_DRW:
+			addr = ap_info->apbap_tar;
+			dmem_emu_set_ap_reg(addr, data);
+
+			ap_info->apbap_tar += dmem_memap_tar_inc(ap_info->apbap_csw);
+			break;
+		default:
+			LOG_INFO("%s: Unknown reg: 0x%02x", __func__, reg);
+			ret = EINVAL;
+			break;
+	}
+
+	/* Track the last error code. */
+	if (ret != ERROR_OK)
+		dmem_dap_retval = ret;
+
+	return ret;
+}
+
 /* AP MODE */
 static uint32_t dmem_get_ap_reg_offset(struct adiv5_ap *ap, unsigned int reg)
 {
@@ -78,6 +261,8 @@ static int dmem_dp_q_write(struct adiv5_dap *dap, unsigned int reg, uint32_t dat
 
 static int dmem_ap_q_read(struct adiv5_ap *ap, unsigned int reg, uint32_t *data)
 {
+	unsigned int idx;
+
 	if (is_adiv6(ap->dap)) {
 		static bool error_flagged;
 
@@ -89,6 +274,9 @@ static int dmem_ap_q_read(struct adiv5_ap *ap, unsigned int reg, uint32_t *data)
 		return ERROR_FAIL;
 	}
 
+	if (dmem_is_emulated_ap(ap, &idx))
+		return dmem_emu_ap_q_read(idx, reg, data);
+
 	*data = dmem_get_ap_reg(ap, reg);
 
 	return ERROR_OK;
@@ -96,6 +284,8 @@ static int dmem_ap_q_read(struct adiv5_ap *ap, unsigned int reg, uint32_t *data)
 
 static int dmem_ap_q_write(struct adiv5_ap *ap, unsigned int reg, uint32_t data)
 {
+	unsigned int idx;
+
 	if (is_adiv6(ap->dap)) {
 		static bool error_flagged;
 
@@ -107,6 +297,9 @@ static int dmem_ap_q_write(struct adiv5_ap *ap, unsigned int reg, uint32_t data)
 		return ERROR_FAIL;
 	}
 
+	if (dmem_is_emulated_ap(ap, &idx))
+		return dmem_emu_ap_q_write(idx, reg, data);
+
 	dmem_set_ap_reg(ap, reg, data);
 
 	return ERROR_OK;
@@ -119,7 +312,12 @@ static int dmem_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack)
 
 static int dmem_dp_run(struct adiv5_dap *dap)
 {
-	return ERROR_OK;
+	int retval = dmem_dap_retval;
+
+	/* Clear the error code. */
+	dmem_dap_retval = ERROR_OK;
+
+	return retval;
 }
 
 static int dmem_connect(struct adiv5_dap *dap)
@@ -168,6 +366,34 @@ COMMAND_HANDLER(dmem_dap_ap_offset_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(dmem_emu_base_address_command)
+{
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(u64, CMD_ARGV[0], dmem_emu_base_address);
+	COMMAND_PARSE_NUMBER(u64, CMD_ARGV[1], dmem_emu_size);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(dmem_emu_ap_list_command)
+{
+	uint64_t em_ap;
+
+	if (CMD_ARGC < 1 || CMD_ARGC > DMEM_MAX_EMULATE_APS)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	for (unsigned int i = 0; i < CMD_ARGC; i++) {
+		COMMAND_PARSE_NUMBER(u64, CMD_ARGV[i], em_ap);
+		dmem_emu_ap_list[i].ap_num = em_ap;
+	}
+
+	dmem_emu_ap_count = CMD_ARGC;
+
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(dmem_dap_config_info_command)
 {
 	if (CMD_ARGC != 0)
@@ -179,7 +405,16 @@ COMMAND_HANDLER(dmem_dap_config_info_command)
 	command_print(CMD, " Base Address : 0x%" PRIx64, dmem_dap_base_address);
 	command_print(CMD, " Max APs      : %u", dmem_dap_max_aps);
 	command_print(CMD, " AP offset    : 0x%08" PRIx32, dmem_dap_ap_offset);
-
+	command_print(CMD, " Emulated AP Count : %u", dmem_emu_ap_count);
+
+	if (dmem_emu_ap_count) {
+		command_print(CMD, " Emulated AP details:");
+		command_print(CMD, " Emulated address  : 0x%" PRIx64, dmem_emu_base_address);
+		command_print(CMD, " Emulated size     : 0x%" PRIx64, dmem_emu_size);
+		for (unsigned int i = 0; i < dmem_emu_ap_count; i++)
+			command_print(CMD, " Emulated AP [%u]  : %" PRIx64, i,
+				      dmem_emu_ap_list[i].ap_num);
+	}
 	return ERROR_OK;
 }
 
@@ -219,6 +454,20 @@ static const struct command_registration dmem_dap_subcommand_handlers[] = {
 		.help = "set the maximum number of APs this will support",
 		.usage = "n",
 	},
+	{
+		.name = "emu_ap_list",
+		.handler = dmem_emu_ap_list_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the list of AP indices to be emulated (upto max)",
+		.usage = "n",
+	},
+	{
+		.name = "emu_base_address_range",
+		.handler = dmem_emu_base_address_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the base address and size of emulated AP range (all emulated APs access this range)",
+		.usage = "base_address address_window_size",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -269,18 +518,45 @@ static int dmem_dap_init(void)
 						 (PROT_READ | PROT_WRITE),
 						 MAP_SHARED, dmem_fd,
 						 dmem_mapped_start);
-
-	close(dmem_fd);
-
 	if (dmem_map_base == MAP_FAILED) {
 		LOG_ERROR("Mapping address 0x%lx for 0x%lx bytes failed!",
 			dmem_mapped_start, dmem_mapped_size);
-		return ERROR_FAIL;
+		goto error_fail;
 	}
 
 	dmem_virt_base_addr = (void *)((uintptr_t)dmem_map_base + start_delta);
 
+	/* Lets Map the emulated address if necessary */
+	if (dmem_emu_ap_count) {
+		dmem_mapped_start = dmem_emu_base_address;
+		dmem_mapped_end = dmem_emu_base_address + dmem_emu_size;
+		/* mmap() requires page aligned offsets */
+		dmem_mapped_start = ALIGN_DOWN(dmem_mapped_start, page_size);
+		dmem_mapped_end = ALIGN_UP(dmem_mapped_end, page_size);
+
+		dmem_emu_mapped_size = dmem_mapped_end - dmem_mapped_start;
+		start_delta = dmem_mapped_start - dmem_emu_base_address;
+
+		dmem_emu_map_base = mmap(NULL,
+									   dmem_emu_mapped_size,
+									   (PROT_READ | PROT_WRITE),
+									   MAP_SHARED, dmem_fd,
+									   dmem_mapped_start);
+		if (dmem_emu_map_base == MAP_FAILED) {
+			LOG_ERROR("Mapping EMU address 0x%lx for 0x%lx bytes failed!",
+					  dmem_emu_base_address, dmem_emu_size);
+			goto error_fail;
+		}
+		dmem_emu_virt_base_addr = (void *)((uintptr_t)dmem_emu_map_base +
+										   start_delta);
+	}
+
+	close(dmem_fd);
 	return ERROR_OK;
+
+error_fail:
+	close(dmem_fd);
+	return ERROR_FAIL;
 }
 
 static int dmem_dap_quit(void)
@@ -288,6 +564,10 @@ static int dmem_dap_quit(void)
 	if (munmap(dmem_map_base, dmem_mapped_size) == -1)
 		LOG_ERROR("%s: Failed to unmap mapped memory!", __func__);
 
+	if (dmem_emu_ap_count
+		&& munmap(dmem_emu_map_base, dmem_emu_mapped_size) == -1)
+		LOG_ERROR("%s: Failed to unmap emu mapped memory!", __func__);
+
 	return ERROR_OK;
 }