+static int stlink_debug_entry(struct target *target)
+{
+ struct armv7m_common *armv7m = target_to_armv7m(target);
+ struct arm *arm = &armv7m->arm;
+ struct reg *r;
+ uint32_t xPSR;
+ int retval;
+
+ retval = armv7m->examine_debug_reason(target);
+ if (retval != ERROR_OK)
+ return retval;
+
+ stm32_stlink_load_context(target);
+
+ r = armv7m->core_cache->reg_list + ARMV7M_xPSR;
+ xPSR = buf_get_u32(r->value, 0, 32);
+
+ /* Are we in an exception handler */
+ if (xPSR & 0x1FF) {
+ armv7m->core_mode = ARMV7M_MODE_HANDLER;
+ armv7m->exception_number = (xPSR & 0x1FF);
+
+ arm->core_mode = ARM_MODE_HANDLER;
+ arm->map = armv7m_msp_reg_map;
+ } else {
+ unsigned control = buf_get_u32(armv7m->core_cache
+ ->reg_list[ARMV7M_CONTROL].value, 0, 2);
+
+ /* is this thread privileged? */
+ armv7m->core_mode = control & 1;
+ arm->core_mode = armv7m->core_mode
+ ? ARM_MODE_USER_THREAD
+ : ARM_MODE_THREAD;
+
+ /* which stack is it using? */
+ if (control & 2)
+ arm->map = armv7m_psp_reg_map;
+ else
+ arm->map = armv7m_msp_reg_map;
+
+ armv7m->exception_number = 0;
+ }
+
+ LOG_DEBUG("entered debug state in core mode: %s at PC 0x%" PRIx32 ", target->state: %s",
+ armv7m_mode_strings[armv7m->core_mode],
+ *(uint32_t *)(arm->pc->value),
+ target_state_name(target));
+
+ return retval;
+}
+