armv7m: restore core mode after executing algorithm

Make sure we restore the core mode after executing any algorithm.

We also now check that we actually need to swap the core mode, we may
already be in the correct mode.

Change-Id: Ia48af2c108e0f9868aae241bf25f60323503f092
Signed-off-by: Spencer Oliver <spen@spen-soft.co.uk>
Reviewed-on: http://openocd.zylin.com/1107
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>

diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index 6814fdbc039635519cd1c57fd3a425d465be5fac..9740a28c5790e6caafe5537e8d594601e0d84a8c 100644 (file)
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -384,13 +384,23 @@ int armv7m_start_algorithm(struct target *target,
                armv7m_set_core_reg(reg, reg_params[i].value);
        }
 
-       if (armv7m_algorithm_info->core_mode != ARM_MODE_ANY) {
+       if (armv7m_algorithm_info->core_mode != ARM_MODE_ANY &&
+                       armv7m_algorithm_info->core_mode != core_mode) {
+
+               /* we cannot set ARM_MODE_HANDLER, so use ARM_MODE_THREAD instead */
+               if (armv7m_algorithm_info->core_mode == ARM_MODE_HANDLER) {
+                       armv7m_algorithm_info->core_mode = ARM_MODE_THREAD;
+                       LOG_INFO("ARM_MODE_HANDLER not currently supported, using ARM_MODE_THREAD instead");
+               }
+
                LOG_DEBUG("setting core_mode: 0x%2.2x", armv7m_algorithm_info->core_mode);
                buf_set_u32(armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].value,
                        0, 1, armv7m_algorithm_info->core_mode);
                armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].dirty = 1;
                armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].valid = 1;
        }
+
+       /* save previous core mode */
        armv7m_algorithm_info->core_mode = core_mode;
 
        retval = target_resume(target, 0, entry_point, 1, 1);
@@ -486,6 +496,15 @@ int armv7m_wait_algorithm(struct target *target,
                }
        }
 
+       /* restore previous core mode */
+       if (armv7m_algorithm_info->core_mode != armv7m->arm.core_mode) {
+               LOG_DEBUG("restoring core_mode: 0x%2.2x", armv7m_algorithm_info->core_mode);
+               buf_set_u32(armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].value,
+                       0, 1, armv7m_algorithm_info->core_mode);
+               armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].dirty = 1;
+               armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].valid = 1;
+       }
+
        armv7m->arm.core_mode = armv7m_algorithm_info->core_mode;
 
        return retval;