semihosting armv7a: Add support for ARMv7-A

Add semihosting support for ARMv7-A based processors.

Tested with custom Vybrid VF610 based board
and Pandaboard ES (Rev. B1) board (Cortex-A9).

Change-Id: I6b896a61c1c6a1c5dcf89de834486f82dd6c80a2
Signed-off-by: Andrey Smirnov <andrew.smirnov@gmail.com>
Signed-off-by: Tsung-Han Lin <tsunghan.tw@gmail.com>
Reviewed-on: http://openocd.zylin.com/2908
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins

4 files changed, 89 insertions, 4 deletions

diff --git a/src/target/arm_semihosting.c b/src/target/arm_semihosting.c
index 2fd65801..63335ff3 100644
--- a/src/target/arm_semihosting.c
+++ b/src/target/arm_semihosting.c
@@ -39,8 +39,10 @@
 #include "armv4_5.h"
 #include "arm7_9_common.h"
 #include "armv7m.h"
+#include "armv7a.h"
 #include "cortex_m.h"
 #include "register.h"
+#include "arm_opcodes.h"
 #include "arm_semihosting.h"
 #include <helper/binarybuffer.h>
 #include <helper/log.h>
@@ -415,7 +417,8 @@ static int do_semihosting(struct target *target)
 	/* REVISIT this looks wrong ... ARM11 and Cortex-A8
 	 * should work this way at least sometimes.
 	 */
-	if (is_arm7_9(target_to_arm7_9(target))) {
+	if (is_arm7_9(target_to_arm7_9(target)) ||
+	    is_armv7a(target_to_armv7a(target))) {
 		uint32_t spsr;
 
 		/* return value in R0 */
@@ -468,20 +471,42 @@ static int do_semihosting(struct target *target)
 int arm_semihosting(struct target *target, int *retval)
 {
 	struct arm *arm = target_to_arm(target);
+	struct armv7a_common *armv7a = target_to_armv7a(target);
 	uint32_t pc, lr, spsr;
 	struct reg *r;
 
 	if (!arm->is_semihosting)
 		return 0;
 
-	if (is_arm7_9(target_to_arm7_9(target))) {
+	if (is_arm7_9(target_to_arm7_9(target)) ||
+	    is_armv7a(armv7a)) {
+		uint32_t vbar = 0x00000000;
+
 		if (arm->core_mode != ARM_MODE_SVC)
 			return 0;
 
+		if (is_armv7a(armv7a)) {
+			struct arm_dpm *dpm = armv7a->arm.dpm;
+
+			*retval = dpm->prepare(dpm);
+			if (*retval == ERROR_OK) {
+				*retval = dpm->instr_read_data_r0(dpm,
+								 ARMV4_5_MRC(15, 0, 0, 12, 0, 0),
+								 &vbar);
+
+				dpm->finish(dpm);
+
+				if (*retval != ERROR_OK)
+					return 1;
+			} else {
+				return 1;
+			}
+		}
+
 		/* Check for PC == 0x00000008 or 0xffff0008: Supervisor Call vector. */
 		r = arm->pc;
 		pc = buf_get_u32(r->value, 0, 32);
-		if (pc != 0x00000008 && pc != 0xffff0008)
+		if (pc != (vbar + 0x00000008) && pc != 0xffff0008)
 			return 0;
 
 		r = arm_reg_current(arm, 14);
diff --git a/src/target/armv7a.c b/src/target/armv7a.c
index 37eb1b5f..6021def4 100644
--- a/src/target/armv7a.c
+++ b/src/target/armv7a.c
@@ -679,11 +679,40 @@ done:
 
 }
 
+static int armv7a_setup_semihosting(struct target *target, int enable)
+{
+	struct armv7a_common *armv7a = target_to_armv7a(target);
+	uint32_t vcr;
+	int ret;
+
+	ret = mem_ap_read_atomic_u32(armv7a->debug_ap,
+					 armv7a->debug_base + CPUDBG_VCR,
+					 &vcr);
+	if (ret < 0) {
+		LOG_ERROR("Failed to read VCR register\n");
+		return ret;
+	}
+
+	if (enable)
+		vcr |= DBG_VCR_SVC_MASK;
+	else
+		vcr &= ~DBG_VCR_SVC_MASK;
+
+	ret = mem_ap_write_atomic_u32(armv7a->debug_ap,
+					  armv7a->debug_base + CPUDBG_VCR,
+					  vcr);
+	if (ret < 0)
+		LOG_ERROR("Failed to write VCR register\n");
+
+	return ret;
+}
+
 int armv7a_init_arch_info(struct target *target, struct armv7a_common *armv7a)
 {
 	struct arm *arm = &armv7a->arm;
 	arm->arch_info = armv7a;
 	target->arch_info = &armv7a->arm;
+	arm->setup_semihosting = armv7a_setup_semihosting;
 	/*  target is useful in all function arm v4 5 compatible */
 	armv7a->arm.target = target;
 	armv7a->arm.common_magic = ARM_COMMON_MAGIC;
diff --git a/src/target/armv7a.h b/src/target/armv7a.h
index 6461ba90..6a62f72c 100644
--- a/src/target/armv7a.h
+++ b/src/target/armv7a.h
@@ -134,6 +134,12 @@ target_to_armv7a(struct target *target)
 	return container_of(target->arch_info, struct armv7a_common, arm);
 }
 
+static inline bool is_armv7a(struct armv7a_common *armv7a)
+{
+	return armv7a->common_magic == ARMV7_COMMON_MAGIC;
+}
+
+
 /* register offsets from armv7a.debug_base */
 
 /* See ARMv7a arch spec section C10.2 */
@@ -172,6 +178,13 @@ target_to_armv7a(struct target *target)
 /* See ARMv7a arch spec section C10.8 */
 #define CPUDBG_AUTHSTATUS	0xFB8
 
+/* Masks for Vector Catch register */
+#define DBG_VCR_FIQ_MASK	((1 << 31) | (1 << 7))
+#define DBG_VCR_IRQ_MASK	((1 << 30) | (1 << 6))
+#define DBG_VCR_DATA_ABORT_MASK	((1 << 28) | (1 << 4))
+#define DBG_VCR_PREF_ABORT_MASK	((1 << 27) | (1 << 3))
+#define DBG_VCR_SVC_MASK	((1 << 26) | (1 << 2))
+
 int armv7a_arch_state(struct target *target);
 int armv7a_identify_cache(struct target *target);
 int armv7a_init_arch_info(struct target *target, struct armv7a_common *armv7a);
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index d1590f65..0a689eaf 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -53,6 +53,7 @@
 #include "target_request.h"
 #include "target_type.h"
 #include "arm_opcodes.h"
+#include "arm_semihosting.h"
 #include <helper/time_support.h>
 
 static int cortex_a_poll(struct target *target);
@@ -915,6 +916,10 @@ static int cortex_a_poll(struct target *target)
 					if (retval != ERROR_OK)
 						return retval;
 				}
+
+				if (arm_semihosting(target, &retval) != 0)
+					return retval;
+
 				target_call_event_callbacks(target,
 					TARGET_EVENT_HALTED);
 			}
@@ -1201,7 +1206,7 @@ static int cortex_a_resume(struct target *target, int current,
 static int cortex_a_debug_entry(struct target *target)
 {
 	int i;
-	uint32_t regfile[16], cpsr, dscr;
+	uint32_t regfile[16], cpsr, spsr, dscr;
 	int retval = ERROR_OK;
 	struct working_area *regfile_working_area = NULL;
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
@@ -1250,6 +1255,7 @@ static int cortex_a_debug_entry(struct target *target)
 	if (cortex_a->fast_reg_read)
 		target_alloc_working_area(target, 64, &regfile_working_area);
 
+
 	/* First load register acessible through core debug port*/
 	if (!regfile_working_area)
 		retval = arm_dpm_read_current_registers(&armv7a->dpm);
@@ -1294,6 +1300,17 @@ static int cortex_a_debug_entry(struct target *target)
 		reg->dirty = reg->valid;
 	}
 
+	/* read Saved PSR */
+	retval = cortex_a_dap_read_coreregister_u32(target, &spsr, 17);
+	/*  store current spsr */
+	if (retval != ERROR_OK)
+		return retval;
+
+	reg = arm->spsr;
+	buf_set_u32(reg->value, 0, 32, spsr);
+	reg->valid = 1;
+	reg->dirty = 0;
+
 #if 0
 /* TODO, Move this */
 	uint32_t cp15_control_register, cp15_cacr, cp15_nacr;
@@ -2953,6 +2970,7 @@ static int cortex_a_examine_first(struct target *target)
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
 	struct armv7a_common *armv7a = &cortex_a->armv7a_common;
 	struct adiv5_dap *swjdp = armv7a->arm.dap;
+
 	int i;
 	int retval = ERROR_OK;
 	uint32_t didr, ctypr, ttypr, cpuid, dbg_osreg;