Add ARM v8 AArch64 semihosting support

This patch implements semihosting support for AArch64. This picks
code from previously submitted AArch64 semihosting support patch
and rebases on top of reworked semihosting code. Tested in AArch64
mode on a Lemaker Hikey Board with NewLib and GDB.

Change-Id: I228a38f1de24f79e49ba99d8514d822a28c2950b
Signed-off-by: Omair Javaid <omair.javaid@linaro.org>
Reviewed-on: http://openocd.zylin.com/4537
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>

src/target/aarch64.c

@@ -28,6 +28,7 @@
 #include "target_type.h"
 #include "armv8_opcodes.h"
 #include "armv8_cache.h"
+#include "arm_semihosting.h"
 #include <helper/time_support.h>
 
 enum restart_mode {
@@ -522,6 +523,9 @@ static int aarch64_poll(struct target *target)
 			if (target->smp)
 				update_halt_gdb(target, debug_reason);
 
+			if (arm_semihosting(target, &retval) != 0)
+				return retval;
+
 			switch (prev_target_state) {
 			case TARGET_RUNNING:
 			case TARGET_UNKNOWN:
@@ -543,6 +547,9 @@ static int aarch64_poll(struct target *target)
 
 static int aarch64_halt(struct target *target)
 {
+	struct armv8_common *armv8 = target_to_armv8(target);
+	armv8->last_run_control_op = ARMV8_RUNCONTROL_HALT;
+
 	if (target->smp)
 		return aarch64_halt_smp(target, false);
 
@@ -831,6 +838,9 @@ static int aarch64_resume(struct target *target, int current,
 	int retval = 0;
 	uint64_t addr = address;
 
+	struct armv8_common *armv8 = target_to_armv8(target);
+	armv8->last_run_control_op = ARMV8_RUNCONTROL_RESUME;
+
 	if (target->state != TARGET_HALTED)
 		return ERROR_TARGET_NOT_HALTED;
 
@@ -1069,6 +1079,8 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	int retval;
 	uint32_t edecr;
 
+	armv8->last_run_control_op = ARMV8_RUNCONTROL_STEP;
+
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
@@ -2351,6 +2363,7 @@ static int aarch64_init_target(struct command_context *cmd_ctx,
 	struct target *target)
 {
 	/* examine_first() does a bunch of this */
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 

src/target/arm_semihosting.c

@@ -46,6 +46,7 @@
 #include "arm7_9_common.h"
 #include "armv7m.h"
 #include "armv7a.h"
+#include "armv8.h"
 #include "cortex_m.h"
 #include "register.h"
 #include "arm_opcodes.h"
@@ -55,6 +56,28 @@
 #include <helper/log.h>
 #include <sys/stat.h>
 
+static int arm_semihosting_resume(struct target *target, int *retval)
+{
+	if (is_armv8(target_to_armv8(target))) {
+		struct armv8_common *armv8 = target_to_armv8(target);
+		if (armv8->last_run_control_op == ARMV8_RUNCONTROL_RESUME) {
+			*retval = target_resume(target, 1, 0, 0, 0);
+			if (*retval != ERROR_OK) {
+				LOG_ERROR("Failed to resume target");
+				return 0;
+			}
+		} else if (armv8->last_run_control_op == ARMV8_RUNCONTROL_STEP)
+			target->debug_reason = DBG_REASON_SINGLESTEP;
+	} else {
+		*retval = target_resume(target, 1, 0, 0, 0);
+		if (*retval != ERROR_OK) {
+			LOG_ERROR("Failed to resume target");
+			return 0;
+		}
+	}
+	return 1;
+}
+
 static int post_result(struct target *target)
 {
 	struct arm *arm = target_to_arm(target);
@@ -88,6 +111,16 @@ static int post_result(struct target *target)
 		if (spsr & 0x20)
 			arm->core_state = ARM_STATE_THUMB;
 
+	} else if (is_armv8(target_to_armv8(target))) {
+		if (arm->core_state == ARM_STATE_AARCH64) {
+			/* return value in R0 */
+			buf_set_u64(arm->core_cache->reg_list[0].value, 0, 64, target->semihosting->result);
+			arm->core_cache->reg_list[0].dirty = 1;
+
+			uint64_t pc = buf_get_u64(arm->core_cache->reg_list[32].value, 0, 64);
+			buf_set_u64(arm->pc->value, 0, 64, pc + 4);
+			arm->pc->dirty = 1;
+		}
 	} else {
 		/* resume execution, this will be pc+2 to skip over the
 		 * bkpt instruction */
@@ -235,6 +268,24 @@ int arm_semihosting(struct target *target, int *retval)
 		/* bkpt 0xAB */
 		if (insn != 0xBEAB)
 			return 0;
+	} else if (is_armv8(target_to_armv8(target))) {
+		if (target->debug_reason != DBG_REASON_BREAKPOINT)
+			return 0;
+
+		if (arm->core_state == ARM_STATE_AARCH64) {
+			uint32_t insn = 0;
+			r = arm->pc;
+			uint64_t pc64 = buf_get_u64(r->value, 0, 64);
+			*retval = target_read_u32(target, pc64, &insn);
+
+			if (*retval != ERROR_OK)
+				return 1;
+
+			/* bkpt 0xAB */
+			if (insn != 0xD45E0000)
+				return 0;
+		} else
+			return 1;
 	} else {
 		LOG_ERROR("Unsupported semi-hosting Target");
 		return 0;
@@ -244,13 +295,18 @@ int arm_semihosting(struct target *target, int *retval)
 	 * operation to complete.
 	 */
 	if (!semihosting->hit_fileio) {
-		/* TODO: update for 64-bits */
+		if (is_armv8(target_to_armv8(target)) &&
-		uint32_t r0 = buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32);
+				arm->core_state == ARM_STATE_AARCH64) {
-		uint32_t r1 = buf_get_u32(arm->core_cache->reg_list[1].value, 0, 32);
+			/* Read op and param from register x0 and x1 respectively. */
-
+			semihosting->op = buf_get_u64(arm->core_cache->reg_list[0].value, 0, 64);
-		semihosting->op = r0;
+			semihosting->param = buf_get_u64(arm->core_cache->reg_list[1].value, 0, 64);
-		semihosting->param = r1;
+			semihosting->word_size_bytes = 8;
+		} else {
+			/* Read op and param from register r0 and r1 respectively. */
+			semihosting->op = buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32);
+			semihosting->param = buf_get_u32(arm->core_cache->reg_list[1].value, 0, 32);
 			semihosting->word_size_bytes = 4;
+		}
 
 		/* Check for ARM operation numbers. */
 		if (0 <= semihosting->op && semihosting->op <= 0x31) {
@@ -265,19 +321,11 @@ int arm_semihosting(struct target *target, int *retval)
 		}
 	}
 
-	/* Post result to target if we are not waiting on a fileio
+	/* Resume if target it is resumable and we are not waiting on a fileio
 	 * operation to complete:
 	 */
-	if (semihosting->is_resumable && !semihosting->hit_fileio) {
+	if (semihosting->is_resumable && !semihosting->hit_fileio)
-		/* Resume right after the BRK instruction. */
+		return arm_semihosting_resume(target, retval);
-		*retval = target_resume(target, 1, 0, 0, 0);
-		if (*retval != ERROR_OK) {
-			LOG_ERROR("Failed to resume target");
-			return 0;
-		}
-
-		return 1;
-	}
 
 	return 0;
 }

src/target/armv8.c

@@ -1017,11 +1017,24 @@ int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
 	return ERROR_OK;
 }
 
+static int armv8_setup_semihosting(struct target *target, int enable)
+{
+	struct arm *arm = target_to_arm(target);
+
+	if (arm->core_state != ARM_STATE_AARCH64) {
+		LOG_ERROR("semihosting only supported in AArch64 state\n");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
 int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
 {
 	struct arm *arm = &armv8->arm;
 	arm->arch_info = armv8;
 	target->arch_info = &armv8->arm;
+	arm->setup_semihosting = armv8_setup_semihosting;
 	/*  target is useful in all function arm v4 5 compatible */
 	armv8->arm.target = target;
 	armv8->arm.common_magic = ARM_COMMON_MAGIC;

src/target/armv8.h

@@ -113,6 +113,12 @@ enum {
 	ARMV8_LAST_REG,
 };
 
+enum run_control_op {
+	ARMV8_RUNCONTROL_UNKNOWN = 0,
+	ARMV8_RUNCONTROL_RESUME = 1,
+	ARMV8_RUNCONTROL_HALT = 2,
+	ARMV8_RUNCONTROL_STEP = 3,
+};
 
 #define ARMV8_COMMON_MAGIC 0x0A450AAA
 
@@ -210,6 +216,9 @@ struct armv8_common {
 
 	struct arm_cti *cti;
 
+	/* last run-control command issued to this target (resume, halt, step) */
+	enum run_control_op last_run_control_op;
+
 	/* Direct processor core register read and writes */
 	int (*read_reg_u64)(struct armv8_common *armv8, int num, uint64_t *value);
 	int (*write_reg_u64)(struct armv8_common *armv8, int num, uint64_t value);
@@ -232,6 +241,11 @@ target_to_armv8(struct target *target)
 	return container_of(target->arch_info, struct armv8_common, arm);
 }
 
+static inline bool is_armv8(struct armv8_common *armv8)
+{
+	return armv8->common_magic == ARMV8_COMMON_MAGIC;
+}
+
 /* register offsets from armv8.debug_base */
 #define CPUV8_DBG_MAINID0		0xD00
 #define CPUV8_DBG_CPUFEATURE0	0xD20

src/target/semihosting_common.c

@@ -1397,8 +1397,9 @@ static int semihosting_read_fields(struct target *target, size_t number,
 	uint8_t *fields)
 {
 	struct semihosting *semihosting = target->semihosting;
-	return target_read_memory(target, semihosting->param,
+	/* Use 4-byte multiples to trigger fast memory access. */
-			semihosting->word_size_bytes, number, fields);
+	return target_read_memory(target, semihosting->param, 4,
+			number * (semihosting->word_size_bytes / 4), fields);
 }
 
 /**
@@ -1408,8 +1409,9 @@ static int semihosting_write_fields(struct target *target, size_t number,
 	uint8_t *fields)
 {
 	struct semihosting *semihosting = target->semihosting;
-	return target_write_memory(target, semihosting->param,
+	/* Use 4-byte multiples to trigger fast memory access. */
-			semihosting->word_size_bytes, number, fields);
+	return target_write_memory(target, semihosting->param, 4,
+			number * (semihosting->word_size_bytes / 4), fields);
 }
 
 /**