aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/adiv5.c11
-rw-r--r--src/cortexm.c28
-rw-r--r--src/include/adiv5.h1
3 files changed, 35 insertions, 5 deletions
diff --git a/src/adiv5.c b/src/adiv5.c
index 0adc97b..13ea449 100644
--- a/src/adiv5.c
+++ b/src/adiv5.c
@@ -276,6 +276,17 @@ void adiv5_ap_mem_write(ADIv5_AP_t *ap, uint32_t addr, uint32_t value)
adiv5_ap_write(ap, ADIV5_AP_DRW, value);
}
+uint16_t adiv5_ap_mem_read_halfword(ADIv5_AP_t *ap, uint32_t addr)
+{
+ adiv5_ap_write(ap, ADIV5_AP_CSW, 0xA2000051);
+ adiv5_ap_write(ap, ADIV5_AP_TAR, addr);
+ uint32_t v = adiv5_ap_read(ap, ADIV5_AP_DRW);
+ if (addr & 2)
+ return v >> 16;
+ else
+ return v & 0xFFFF;
+}
+
void adiv5_ap_mem_write_halfword(ADIv5_AP_t *ap, uint32_t addr, uint16_t value)
{
uint32_t v = value;
diff --git a/src/cortexm.c b/src/cortexm.c
index 03dbd7a..9b8b180 100644
--- a/src/cortexm.c
+++ b/src/cortexm.c
@@ -129,6 +129,7 @@ const struct command_s cortexm_cmd_list[] = {
#define CORTEXM_DHCSR_C_DEBUGEN (1 << 0)
/* Debug Core Register Selector Register (DCRSR) */
+#define CORTEXM_DCRSR_REGWnR 0x00010000
#define CORTEXM_DCRSR_REGSEL_MASK 0x0000001F
#define CORTEXM_DCRSR_REGSEL_XPSR 0x00000010
#define CORTEXM_DCRSR_REGSEL_MSP 0x00000011
@@ -205,6 +206,7 @@ static int cortexm_check_hw_wp(struct target_s *target, uint32_t *addr);
struct cortexm_priv {
bool stepping;
+ bool on_bkpt;
/* Watchpoint unit status */
struct wp_unit_s {
uint32_t addr;
@@ -500,16 +502,24 @@ cortexm_regs_write(struct target_s *target, const void *data)
return 0;
}
+static uint32_t
+cortexm_pc_read(struct target_s *target)
+{
+ ADIv5_AP_t *ap = adiv5_target_ap(target);
+
+ adiv5_ap_mem_write(ap, CORTEXM_DCRSR, 0x0F);
+ return adiv5_ap_mem_read(ap, CORTEXM_DCRDR);
+
+ return 0;
+}
+
static int
cortexm_pc_write(struct target_s *target, const uint32_t val)
{
ADIv5_AP_t *ap = adiv5_target_ap(target);
- adiv5_ap_write(ap, ADIV5_AP_CSW, 0xA2000052);
- adiv5_dp_low_access(ap->dp, 1, 0, ADIV5_AP_TAR, CORTEXM_DHCSR);
-
- adiv5_ap_write(ap, ADIV5_AP_DB(2), val); /* Required to switch banks */
- adiv5_dp_low_access(ap->dp, 1, 0, ADIV5_AP_DB(1), 0x1000F);
+ adiv5_ap_mem_write(ap, CORTEXM_DCRDR, val);
+ adiv5_ap_mem_write(ap, CORTEXM_DCRSR, CORTEXM_DCRSR_REGWnR | 0x0F);
return 0;
}
@@ -564,6 +574,8 @@ cortexm_halt_wait(struct target_s *target)
if ((dfsr & CORTEXM_DFSR_VCATCH) && cortexm_fault_unwind(target))
return SIGSEGV;
+ /* Remember if we stopped on a breakpoint */
+ priv->on_bkpt = dfsr & (CORTEXM_DFSR_BKPT);
if (dfsr & (CORTEXM_DFSR_BKPT | CORTEXM_DFSR_DWTTRAP))
return SIGTRAP;
@@ -589,6 +601,12 @@ cortexm_halt_resume(struct target_s *target, bool step)
priv->stepping = step;
}
+ if (priv->on_bkpt) {
+ uint32_t pc = cortexm_pc_read(target);
+ if ((adiv5_ap_mem_read_halfword(ap, pc) & 0xFF00) == 0xBE00)
+ cortexm_pc_write(target, pc + 2);
+ }
+
adiv5_ap_mem_write(ap, CORTEXM_DHCSR, dhcsr);
}
diff --git a/src/include/adiv5.h b/src/include/adiv5.h
index b010a60..0f51bb1 100644
--- a/src/include/adiv5.h
+++ b/src/include/adiv5.h
@@ -167,6 +167,7 @@ uint32_t adiv5_dp_read_ap(ADIv5_DP_t *dp, uint8_t addr);
uint32_t adiv5_ap_mem_read(ADIv5_AP_t *ap, uint32_t addr);
void adiv5_ap_mem_write(ADIv5_AP_t *ap, uint32_t addr, uint32_t value);
+uint16_t adiv5_ap_mem_read_halfword(ADIv5_AP_t *ap, uint32_t addr);
void adiv5_ap_mem_write_halfword(ADIv5_AP_t *ap, uint32_t addr, uint16_t value);
void adiv5_ap_write(ADIv5_AP_t *ap, uint8_t addr, uint32_t value);