ETM: start cleaning up ETM_CTRL bit handling

[openocd.git] / src / target / arm_semihosting.c

diff --git a/src/target/arm_semihosting.c b/src/target/arm_semihosting.c
index d71fbaef02bd0544f6ad0cceec5021604507ec1d..f4244c84d11c15985590e2abda59317f1db34fb3 100644 (file)
--- a/src/target/arm_semihosting.c
+++ b/src/target/arm_semihosting.c
@@ -34,6 +34,7 @@
 #include "config.h"
 #endif
 
+#include "arm.h"
 #include "armv4_5.h"
 #include "register.h"
 #include "arm_semihosting.h"
@@ -379,15 +380,22 @@ static int do_semihosting(struct target *target)
        }
 
        /* resume execution to the original mode */
+
+       /* return value in R0 */
        buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, result);
        armv4_5->core_cache->reg_list[0].dirty = 1;
+
+       /* LR --> PC */
        buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, lr);
        armv4_5->core_cache->reg_list[15].dirty = 1;
-       buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, spsr);
-       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
+
+       /* saved PSR --> current PSR */
+       buf_set_u32(armv4_5->cpsr->value, 0, 32, spsr);
+       armv4_5->cpsr->dirty = 1;
        armv4_5->core_mode = spsr & 0x1f;
        if (spsr & 0x20)
                armv4_5->core_state = ARM_STATE_THUMB;
+
        return target_resume(target, 1, 0, 0, 0);
 }
 
@@ -407,18 +415,16 @@ static int do_semihosting(struct target *target)
 int arm_semihosting(struct target *target, int *retval)
 {
        struct arm *arm = target_to_arm(target);
-       uint32_t lr, spsr;
+       uint32_t pc, lr, spsr;
        struct reg *r;
 
        if (!arm->is_semihosting || arm->core_mode != ARM_MODE_SVC)
                return 0;
 
-       /* Check for PC == 8:  Supervisor Call vector
-        * REVISIT:  assumes low exception vectors, not hivecs...
-        * safer to test "was this entry from a vector catch".
-        */
+       /* Check for PC == 0x00000008 or 0xffff0008: Supervisor Call vector. */
        r = arm->core_cache->reg_list + 15;
-       if (buf_get_u32(r->value, 0, 32) != 0x08)
+       pc = buf_get_u32(r->value, 0, 32);
+       if (pc != 0x00000008 && pc != 0xffff0008)
                return 0;
 
        r = arm_reg_current(arm, 14);