esp32: Adds support for SW breakpoints in RAM

contrib/loaders/flash/esp32/stub_flasher.c

@@ -623,11 +623,6 @@ static int stub_flash_get_map(uint32_t maps_addr)
     return ESP32_STUB_ERR_OK;
 }
 
-static inline uint8_t stub_get_insn_size(uint8_t *insn)
-{
-  return insn[0] & 0x8 ? 2 : 3;
-}
-
 /**
 * Possible BP layouts in flash:
 * 1) addr is aligned to 4 bytes (in 1 sector)
@@ -649,7 +644,7 @@ static uint8_t stub_flash_set_bp(uint32_t bp_flash_addr, uint32_t insn_buf_addr,
         return 0;
     }
     memcpy((void *)insn_buf_addr, &insn_sect[(bp_flash_addr & (ESP32_FLASH_SECTOR_SIZE-1)) & ~0x3UL], ESP32_STUB_BP_INSN_BUF_SIZE);
-    uint8_t insn_sz = stub_get_insn_size(&insn_sect[bp_flash_addr & (ESP32_FLASH_SECTOR_SIZE-1)]);
+    uint8_t insn_sz = xtensa_get_insn_size(&insn_sect[bp_flash_addr & (ESP32_FLASH_SECTOR_SIZE-1)]);
     STUB_LOGI("Read insn [%02x %02x %02x] %d bytes @ 0x%x\n", insn_sect[(bp_flash_addr & (ESP32_FLASH_SECTOR_SIZE-1)) + 0], insn_sect[(bp_flash_addr & (ESP32_FLASH_SECTOR_SIZE-1)) + 1],
       insn_sect[(bp_flash_addr & (ESP32_FLASH_SECTOR_SIZE-1)) + 2], insn_sz, bp_flash_addr);
 
@@ -691,7 +686,7 @@ static int stub_flash_clear_bp(uint32_t bp_flash_addr, uint32_t insn_buf_addr, u
         STUB_LOGE("Failed to read insn sector (%d)!\n", rc);
         return ESP32_STUB_ERR_FAIL;
     }
-    uint8_t insn_sz = stub_get_insn_size(&insn[bp_flash_addr & 0x3UL]);
+    uint8_t insn_sz = xtensa_get_insn_size(&insn[bp_flash_addr & 0x3UL]);
     // this will erase full sector or two
     if (stub_flash_erase(bp_flash_addr, insn_sz) != ESP32_STUB_ERR_OK) {
         STUB_LOGE("Failed to erase insn sector!\n");
@@ -722,6 +717,7 @@ static int stub_flash_clear_bp(uint32_t bp_flash_addr, uint32_t insn_buf_addr, u
     return ESP32_STUB_ERR_OK;
 }
 
+
 static int stub_flash_handler(int cmd, va_list ap)
 {
   int ret = ESP32_STUB_ERR_OK;
@@ -734,7 +730,7 @@ static int stub_flash_handler(int cmd, va_list ap)
   uint32_t arg4 = va_arg(ap, uint32_t);   // down buf size
   bool other_cache_enab = stub_spi_flash_cache_enabled(other_core_id);
 
-  STUB_LOGD("flash a %x, s %d\n", arg1, arg2);
+  STUB_LOGD("%s a %x, s %d\n", __func__, arg1, arg2);
 
   ets_efuse_read_op();
   uint32_t spiconfig = ets_efuse_get_spiconfig();
@@ -894,13 +890,13 @@ int stub_main(int cmd, ...)
         ret = stub_flash_handler(cmd, ap);
     } else  switch (cmd) {
 #if STUB_DEBUG
-        case ESP32_STUB_CMD_TEST:
-          STUB_LOGD("TEST %d\n", cmd);
-          break;
-        case ESP32_STUB_CMD_FLASH_TEST:
+      case ESP32_STUB_CMD_TEST:
+        STUB_LOGD("TEST %d\n", cmd);
+        break;
+      case ESP32_STUB_CMD_FLASH_TEST:
 #endif
-        default:
-          ret = ESP32_STUB_ERR_NOT_SUPPORTED;
+      default:
+        ret = ESP32_STUB_ERR_NOT_SUPPORTED;
     }
     va_end(ap);
 

contrib/loaders/flash/esp32/stub_flasher.h

@@ -25,17 +25,17 @@
 #define ESP32_STUB_ERR_FAIL                 (-1)
 #define ESP32_STUB_ERR_NOT_SUPPORTED        (-2)
 
-#define ESP32_STUB_CMD_TEST                 0
-#define ESP32_STUB_CMD_FLASH_READ           1
-#define ESP32_STUB_CMD_FLASH_WRITE          2
-#define ESP32_STUB_CMD_FLASH_ERASE          3
-#define ESP32_STUB_CMD_FLASH_ERASE_CHECK    4
-#define ESP32_STUB_CMD_FLASH_SIZE	        5
-#define ESP32_STUB_CMD_FLASH_MAP_GET        6
-#define ESP32_STUB_CMD_FLASH_BP_SET			7
-#define ESP32_STUB_CMD_FLASH_BP_CLEAR		8
+#define ESP32_STUB_CMD_FLASH_READ           0
+#define ESP32_STUB_CMD_FLASH_WRITE          1
+#define ESP32_STUB_CMD_FLASH_ERASE          2
+#define ESP32_STUB_CMD_FLASH_ERASE_CHECK    3
+#define ESP32_STUB_CMD_FLASH_SIZE	        4
+#define ESP32_STUB_CMD_FLASH_MAP_GET        5
+#define ESP32_STUB_CMD_FLASH_BP_SET			6
+#define ESP32_STUB_CMD_FLASH_BP_CLEAR		7
 #define ESP32_STUB_CMD_FLASH_MAX_ID	        ESP32_STUB_CMD_FLASH_BP_CLEAR
 #define ESP32_STUB_CMD_FLASH_TEST           (ESP32_STUB_CMD_FLASH_MAX_ID+1)
+#define ESP32_STUB_CMD_TEST                 8
 
 #define ESP32_STUB_PARTITION_TABLE_MAX_ENTRIES 	10
 #define ESP32_STUB_FLASH_MAPPINGS_MAX_NUM		2 // IROM, DROM
@@ -55,4 +55,9 @@ struct esp32_flash_mapping {
 	struct esp32_flash_region_mapping maps[ESP32_STUB_FLASH_MAPPINGS_MAX_NUM];
 };
 
-#endif //ESP32_FLASHER_STUB_H
+static inline uint8_t xtensa_get_insn_size(uint8_t *insn)
+{
+  return insn[0] & 0x8 ? 2 : 3;
+}
+
+#endif //ESP32_FLASHER_STUB_H

contrib/loaders/flash/esp32/stub_flasher_data.inc

@@ -1,5 +1,5 @@
-  0xae, 0x09, 0x09, 0x40, 0xa7, 0x0a, 0x09, 0x40, 0x8e, 0x0a, 0x09, 0x40,
-  0x9b, 0x0a, 0x09, 0x40, 0xe7, 0x0b, 0x09, 0x40, 0xf0, 0x0b, 0x09, 0x40,
-  0xfb, 0x0b, 0x09, 0x40, 0x76, 0x0c, 0x09, 0x40, 0x00, 0x00, 0x04, 0x00,
+  0xab, 0x09, 0x09, 0x40, 0xa3, 0x0a, 0x09, 0x40, 0x8a, 0x0a, 0x09, 0x40,
+  0x97, 0x0a, 0x09, 0x40, 0xe6, 0x0b, 0x09, 0x40, 0xf1, 0x0b, 0x09, 0x40,
+  0xf9, 0x0b, 0x09, 0x40, 0x76, 0x0c, 0x09, 0x40, 0x00, 0x00, 0x04, 0x00,
   0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x20, 0x00,
   0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x01

src/flash/nor/esp32.c

@@ -76,7 +76,7 @@
 #include <target/esp108_apptrace.h>
 #include <target/esp32.h>
 #include "time_support.h"
-#include <contrib/loaders/flash/esp32/stub_flasher.h>
+#include "contrib/loaders/flash/esp32/stub_flasher.h"
 #include "contrib/loaders/flash/esp32/stub_flasher_image.h"
 #include "target/esp32.h"
 
@@ -796,7 +796,7 @@ struct esp32_flash_sw_breakpoint * esp32_add_flash_breakpoint(struct target *tar
 		LOG_ERROR("Failed to alloc memory for sw breakpoint data!");
 		return NULL;
 	}
-	sw_bp->oocd_bp = breakpoint;
+	sw_bp->data.oocd_bp = breakpoint;
 	sw_bp->bank = bank;
 
 	init_mem_param(&mp, 2/*2nd usr arg*/, 3/*size in bytes*/, PARAM_IN);

src/target/esp108_common.c

@@ -253,19 +253,6 @@ int xtensa_examine(struct target *target)
 	return ERROR_OK;
 }
 
-uint32_t xtensa_read_reg_direct(struct target *target, uint8_t reg)
-{
-	uint32_t result = 0xdeadface;
-	uint8_t dsr[4];
-
-	esp108_queue_nexus_reg_read(target, reg, dsr);
-	esp108_queue_tdi_idle(target);
-	int res = jtag_execute_queue();
-	if (res != ERROR_OK) return result;
-	result = intfromchars(dsr);
-	return result;
-}
-
 int read_reg_direct(struct target *target, uint8_t addr)
 {
 	uint8_t dsr[4];
@@ -275,7 +262,6 @@ int read_reg_direct(struct target *target, uint8_t addr)
 	return intfromchars(dsr);
 }
 
-
 int xtensa_write_uint32(struct target *target, uint32_t addr, uint32_t val)
 {
 
@@ -407,7 +393,7 @@ int xtensa_wait_algorithm_generic(struct target *target,
 	for (int i = 0; i < num_mem_params; i++) {
 		LOG_DEBUG("Check mem param @ 0x%x", mem_params[i].address);
 		if (mem_params[i].direction != PARAM_OUT) {
-			LOG_USER("Read mem param @ 0x%x", mem_params[i].address);
+			LOG_DEBUG("Read mem param @ 0x%x", mem_params[i].address);
 			retval = target_read_buffer(target, mem_params[i].address,
 					mem_params[i].size,
 					mem_params[i].value);
@@ -435,8 +421,6 @@ int xtensa_wait_algorithm_generic(struct target *target,
 	for (int i = core_cache->num_regs - 1; i >= 0; i--) {
 		uint32_t regvalue;
 		regvalue = esp108_reg_get(&core_cache->reg_list[i]);
-		// LOG_DEBUG("check register %s with value 0x%x -> 0x%8.8" PRIx32,
-		// 		core_cache->reg_list[i].name, regvalue, algorithm_info->context[i]);
 		if (i == XT_REG_IDX_DEBUGCAUSE) {
 			//FIXME: restoring DEBUGCAUSE causes exception when executing corresponding instruction in DIR
 			LOG_DEBUG("Skip restoring register %s: 0x%x -> 0x%8.8" PRIx32,

src/target/esp32.c

@@ -547,7 +547,6 @@ static int xtensa_resume_cpu(struct target *target,
 			xtensa_step(target, current, address, handle_breakpoints);
 		}
 	}
-
 	//Write back hw breakpoints. Current FreeRTOS SMP code can set a hw breakpoint on an
 	//exception; we need to clear that and return to the breakpoints gdb has set on resume.
 	bpena = 0;
@@ -939,7 +938,7 @@ static int esp32_handle_target_event(struct target *target, enum target_event ev
 					int ret = esp32_remove_flash_breakpoint(target, flash_bp);
 					if (ret != ERROR_OK) {
 						LOG_ERROR("%s: Failed to remove SW flash BP @ 0x%x (%d)!",
-								target->cmd_name, flash_bp->oocd_bp->address, ret);
+								target->cmd_name, flash_bp->data.oocd_bp->address, ret);
 						return ret;
 					}
 				}
@@ -958,7 +957,50 @@ static int esp32_handle_target_event(struct target *target, enum target_event ev
 		default:
 			break;
 	}
+	return ERROR_OK;
+}
+
+static struct esp32_sw_breakpoint * esp32_add_sw_breakpoint(struct target *target, struct breakpoint *breakpoint)
+{
+	union {
+		uint32_t d32;
+		uint8_t d8[4];
+	} break_insn;
+
+	int ret = target_read_buffer(target, breakpoint->address, 3, break_insn.d8);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("%s: Faied to read insn (%d)!", target->cmd_name, ret);
+		return NULL;
+	}
+	struct esp32_sw_breakpoint *sw_bp = malloc(sizeof(struct esp32_flash_sw_breakpoint));
+	if (sw_bp == NULL) {
+		LOG_ERROR("Failed to alloc memory for sw breakpoint data!");
+		return NULL;
+	}
+	sw_bp->insn_sz = xtensa_get_insn_size(break_insn.d8);
+	memcpy(sw_bp->insn, break_insn.d8, sw_bp->insn_sz);
+	sw_bp->oocd_bp = breakpoint;
+
+	break_insn.d32 = sw_bp->insn_sz == 2 ? XT_INS_BREAKN(0) : XT_INS_BREAK(0, 0);
+
+	ret = target_write_buffer(target, breakpoint->address, sw_bp->insn_sz, break_insn.d8);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("%s: Faied to read insn (%d)!", target->cmd_name, ret);
+		free(sw_bp);
+		return NULL;
+	}
+
+	return sw_bp;
+}
 
+static int esp32_remove_sw_breakpoint(struct target *target, struct esp32_sw_breakpoint *breakpoint)
+{
+	int ret = target_write_buffer(target, breakpoint->oocd_bp->address, breakpoint->insn_sz, breakpoint->insn);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("%s: Faied to read insn (%d)!", target->cmd_name, ret);
+		return ret;
+	}
+	free(breakpoint);
 	return ERROR_OK;
 }
 
@@ -968,7 +1010,21 @@ static int xtensa_add_breakpoint(struct target *target, struct breakpoint *break
 	size_t slot;
 
 	if (breakpoint->type == BKPT_SOFT) {
-		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		for(slot = 0; slot < ESP32_SW_BREAKPOINTS_MAX_NUM; slot++) {
+			if (esp32->sw_brps[slot] == NULL || esp32->sw_brps[slot]->oocd_bp == breakpoint) break;
+		}
+		if (slot == ESP32_SW_BREAKPOINTS_MAX_NUM)
+		{
+			LOG_WARNING("%s: max SW slot reached, slot=%i", __func__, (unsigned int)slot);
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		esp32->sw_brps[slot] = esp32_add_sw_breakpoint(target, breakpoint);
+		if (esp32->sw_brps[slot] == NULL) {
+			LOG_ERROR("%s: Faied to add SW BP!", target->cmd_name);
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		LOG_DEBUG("%s: placed SW breakpoint %d at 0x%X", target->cmd_name, (int)slot, breakpoint->address);
+		return ERROR_OK;
 	}
 
 	for(slot = 0; slot < esp32->num_brps; slot++) {
@@ -978,11 +1034,11 @@ static int xtensa_add_breakpoint(struct target *target, struct breakpoint *break
 	{
 		LOG_WARNING("%s: max HW slot reached, slot=%i", __func__, (unsigned int)slot);
 		for(slot = 0; slot < ESP32_FLASH_SW_BREAKPOINTS_MAX_NUM; slot++) {
-			if (esp32->flash_sw_brps[slot] == NULL || esp32->flash_sw_brps[slot]->oocd_bp == breakpoint) break;
+			if (esp32->flash_sw_brps[slot] == NULL || esp32->flash_sw_brps[slot]->data.oocd_bp == breakpoint) break;
 		}
 		if (slot == ESP32_FLASH_SW_BREAKPOINTS_MAX_NUM)
 		{
-			LOG_WARNING("%s: max SW slot reached, slot=%i", __func__, (unsigned int)slot);
+			LOG_WARNING("%s: max SW flash slot reached, slot=%i", __func__, (unsigned int)slot);
 			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 		}
 		esp32->flash_sw_brps[slot] = esp32_add_flash_breakpoint(target, breakpoint);
@@ -1007,7 +1063,23 @@ static int xtensa_remove_breakpoint(struct target *target, struct breakpoint *br
 	LOG_DEBUG("%s: %p", __func__, breakpoint);
 
 	if (breakpoint->type == BKPT_SOFT) {
-		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		for(slot = 0; slot < ESP32_SW_BREAKPOINTS_MAX_NUM; slot++) {
+			if(esp32->sw_brps[slot] != NULL && esp32->sw_brps[slot]->oocd_bp == breakpoint)
+				break;
+		}
+		if (slot == ESP32_SW_BREAKPOINTS_MAX_NUM)
+		{
+			LOG_WARNING("%s: max SW slot reached, slot=%i", __func__, (unsigned int)slot);
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		int ret = esp32_remove_sw_breakpoint(target, esp32->sw_brps[slot]);
+		if (ret != ERROR_OK) {
+			LOG_ERROR("%s: Failed to remove SW BP (%d)!", target->cmd_name, ret);
+			return ret;
+		}
+		esp32->sw_brps[slot] = NULL;
+		LOG_DEBUG("%s: cleared SW breakpoint %d at 0x%X", target->cmd_name, (int)slot, breakpoint->address);
+		return ERROR_OK;
 	}
 
 	for(slot = 0; slot < esp32->num_brps; slot++) {
@@ -1017,7 +1089,7 @@ static int xtensa_remove_breakpoint(struct target *target, struct breakpoint *br
 	if (slot==esp32->num_brps) {
 		LOG_DEBUG("%s: HW BP not found!", target->cmd_name);
 		for(slot = 0; slot < ESP32_FLASH_SW_BREAKPOINTS_MAX_NUM; slot++) {
-			if(esp32->flash_sw_brps[slot] != NULL && esp32->flash_sw_brps[slot]->oocd_bp == breakpoint)
+			if(esp32->flash_sw_brps[slot] != NULL && esp32->flash_sw_brps[slot]->data.oocd_bp == breakpoint)
 				break;
 		}
 		if (slot == ESP32_FLASH_SW_BREAKPOINTS_MAX_NUM) {
@@ -1352,7 +1424,8 @@ static int xtensa_target_create(struct target *target, Jim_Interp *interp)
 	esp32->hw_brps = calloc(XT_NUM_BREAKPOINTS, sizeof(struct breakpoint *));
 	esp32->num_wps = XT_NUM_WATCHPOINTS;
 	esp32->hw_wps = calloc(XT_NUM_WATCHPOINTS, sizeof(struct watchpoint *));
-	esp32->flash_sw_brps = calloc(ESP32_FLASH_SW_BREAKPOINTS_MAX_NUM, sizeof(struct esp32_flash_sw_breakpoint	*));
+	esp32->flash_sw_brps = calloc(ESP32_FLASH_SW_BREAKPOINTS_MAX_NUM, sizeof(struct esp32_flash_sw_breakpoint *));
+	esp32->sw_brps = calloc(ESP32_SW_BREAKPOINTS_MAX_NUM, sizeof(struct esp32_sw_breakpoint	*));
 
 	//Create the register cache
 	cache->name = "Xtensa registers";
@@ -1470,7 +1543,7 @@ static size_t esp32_read_cores_num(struct target *target)
 	LOG_DEBUG("Read APP CPU ctrl reg 0x%x", appcpu_ctrl);
 	if (appcpu_ctrl & ESP32_DPORT_APPCPU_CLKGATE_EN) {
 		cores_num++;
-		LOG_INFO("APP CPU enabled");
+		LOG_DEBUG("APP CPU enabled");
 	}
 	LOG_DEBUG("Detected %u cores", (uint32_t)cores_num);
 	return cores_num;
@@ -1549,21 +1622,22 @@ static int xtensa_poll(struct target *target)
 				struct reg *cpu_reg_list = esp32->core_caches[i]->reg_list;
 				// volatile int temp_cause = xtensa_read_reg_direct(esp32->esp32_targets[i], XT_REG_IDX_DEBUGCAUSE);
 				int cause = esp108_reg_get(&cpu_reg_list[XT_REG_IDX_DEBUGCAUSE]);
+				int exc_cause = esp108_reg_get(&cpu_reg_list[XT_REG_IDX_EXCCAUSE]);
 
 				volatile unsigned int dsr_core = xtensa_read_dsr(esp32->esp32_targets[i]);
 				if ((dsr_core&OCDDSR_DEBUGPENDBREAK) != 0)
 				{
 					if (esp32->active_cpu != i)
 					{
-						LOG_INFO("active_cpu: %u, changed to %u, reson = 0x%08x", (uint32_t)esp32->active_cpu, (uint32_t)i, dsr_core);
+						LOG_INFO("active_cpu: %u, changed to %u, reason = 0x%08x", (uint32_t)esp32->active_cpu, (uint32_t)i, dsr_core);
 					}
 					esp32->active_cpu = i;
 				}
 
 				int dcrset = read_reg_direct(esp32->esp32_targets[i], NARADR_DCRSET);
 
-				LOG_DEBUG("%s: Halt reason =0x%08X, dsr=0x%08x, dcrset=0x%08x",
-					esp32->esp32_targets[i]->cmd_name, cause, dsr_core, dcrset);
+				LOG_DEBUG("%s: Halt reason=0x%08X, exc_cause=%d, dsr=0x%08x, dcrset=0x%08x",
+					esp32->esp32_targets[i]->cmd_name, cause, exc_cause, dsr_core, dcrset);
 				if (cause&DEBUGCAUSE_IC)
 				{
 					target->debug_reason = DBG_REASON_SINGLESTEP;