static void init_code_breakpoints(stlink_t *sl) {
memset(sl->q_buf, 0, 4);
stlink_write_debug32(sl, CM3_REG_FP_CTRL, 0x03 /*KEY | ENABLE4*/);
printf("KARL - should read back as 0x03, not 60 02 00 00\n");
stlink_read_debug32(sl, CM3_REG_FP_CTRL);
for(int i = 0; i < CODE_BREAK_NUM; i++) {
code_breaks[i].type = 0;
stlink_write_debug32(sl, CM3_REG_FP_COMP0 + i * 4, 0);
}
}
static void init_data_watchpoints(stlink_t *sl) {
DLOG("init watchpoints\n");
// set trcena in debug command to turn on dwt unit
stlink_write_debug32(sl, 0xE000EDFC,
stlink_read_debug32(sl, 0xE000EDFC) | (1<<24));
// make sure all watchpoints are cleared
for(int i = 0; i < DATA_WATCH_NUM; i++) {
data_watches[i].fun = WATCHDISABLED;
stlink_write_debug32(sl, 0xe0001028 + i * 16, 0);
}
}
static int add_data_watchpoint(stlink_t *sl, enum watchfun wf, stm32_addr_t addr, unsigned int len)
{
int i = 0;
uint32_t mask;
// computer mask
// find a free watchpoint
// configure
mask = -1;
i = len;
while(i) {
i >>= 1;
mask++;
}
if((mask != (uint32_t)-1) && (mask < 16)) {
for(i = 0; i < DATA_WATCH_NUM; i++) {
// is this an empty slot ?
if(data_watches[i].fun == WATCHDISABLED) {
#ifdef DEBUG
printf("insert watchpoint %d addr %x wf %u mask %u len %d\n", i, addr, wf, mask, len);
#endif
data_watches[i].fun = wf;
data_watches[i].addr = addr;
data_watches[i].mask = mask;
// insert comparator address
stlink_write_debug32(sl, 0xE0001020 + i * 16, addr);
// insert mask
stlink_write_debug32(sl, 0xE0001024 + i * 16, mask);
// insert function
stlink_write_debug32(sl, 0xE0001028 + i * 16, wf);
// just to make sure the matched bit is clear !
stlink_read_debug32(sl, 0xE0001028 + i * 16);
return 0;
}
}
}
#ifdef DEBUG
printf("failure: add watchpoints addr %x wf %u len %u\n", addr, wf, len);
#endif
return -1;
}