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

diff --git a/src/target/arm_semihosting.c b/src/target/arm_semihosting.c
index 2fd658014b3dd2d341d0c76b86a3e486e0942a07..63335ff3c7abad524febfc0a31ba374bdd3a3581 100644 (file)
--- 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 37eb1b5f1b2ab31cc326138fe42293456c6cd2b0..6021def4e577656ff6967402eabf78faacf8eac8 100644 (file)
--- 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 6461ba905653d529edc93db9db2374d094ef55aa..6a62f72c6cc0240d0cf85540e3988c479d123592 100644 (file)
--- 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 d1590f65f59f77cd67cb13f447e3f8519e313079..0a689eafdfa96a5097567b862ded7be9016b1f09 100644 (file)
--- 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;