+/* Setup hardware Watchpoint Register Pair */
+static int aarch64_set_watchpoint(struct target *target,
+ struct watchpoint *watchpoint)
+{
+ int retval;
+ int wp_i = 0;
+ uint32_t control, offset, length;
+ struct aarch64_common *aarch64 = target_to_aarch64(target);
+ struct armv8_common *armv8 = &aarch64->armv8_common;
+ struct aarch64_brp *wp_list = aarch64->wp_list;
+
+ if (watchpoint->set) {
+ LOG_WARNING("watchpoint already set");
+ return ERROR_OK;
+ }
+
+ while (wp_list[wp_i].used && (wp_i < aarch64->wp_num))
+ wp_i++;
+ if (wp_i >= aarch64->wp_num) {
+ LOG_ERROR("ERROR Can not find free Watchpoint Register Pair");
+ return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+ }
+
+ control = (1 << 0) /* enable */
+ | (3 << 1) /* both user and privileged access */
+ | (1 << 13); /* higher mode control */
+
+ switch (watchpoint->rw) {
+ case WPT_READ:
+ control |= 1 << 3;
+ break;
+ case WPT_WRITE:
+ control |= 2 << 3;
+ break;
+ case WPT_ACCESS:
+ control |= 3 << 3;
+ break;
+ }
+
+ /* Match up to 8 bytes. */
+ offset = watchpoint->address & 7;
+ length = watchpoint->length;
+ if (offset + length > sizeof(uint64_t)) {
+ length = sizeof(uint64_t) - offset;
+ LOG_WARNING("Adjust watchpoint match inside 8-byte boundary");
+ }
+ for (; length > 0; offset++, length--)
+ control |= (1 << offset) << 5;
+
+ wp_list[wp_i].value = watchpoint->address & 0xFFFFFFFFFFFFFFF8ULL;
+ wp_list[wp_i].control = control;
+
+ retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+ + CPUV8_DBG_WVR_BASE + 16 * wp_list[wp_i].brpn,
+ (uint32_t)(wp_list[wp_i].value & 0xFFFFFFFF));
+ if (retval != ERROR_OK)
+ return retval;
+ retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+ + CPUV8_DBG_WVR_BASE + 4 + 16 * wp_list[wp_i].brpn,
+ (uint32_t)(wp_list[wp_i].value >> 32));
+ if (retval != ERROR_OK)
+ return retval;
+
+ retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+ + CPUV8_DBG_WCR_BASE + 16 * wp_list[wp_i].brpn,
+ control);
+ if (retval != ERROR_OK)
+ return retval;
+ LOG_DEBUG("wp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, wp_i,
+ wp_list[wp_i].control, wp_list[wp_i].value);
+
+ /* Ensure that halting debug mode is enable */
+ retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
+ if (retval != ERROR_OK) {
+ LOG_DEBUG("Failed to set DSCR.HDE");
+ return retval;
+ }
+
+ wp_list[wp_i].used = 1;
+ watchpoint->set = wp_i + 1;
+
+ return ERROR_OK;
+}
+
+/* Clear hardware Watchpoint Register Pair */
+static int aarch64_unset_watchpoint(struct target *target,
+ struct watchpoint *watchpoint)
+{
+ int retval, wp_i;
+ struct aarch64_common *aarch64 = target_to_aarch64(target);
+ struct armv8_common *armv8 = &aarch64->armv8_common;
+ struct aarch64_brp *wp_list = aarch64->wp_list;
+
+ if (!watchpoint->set) {
+ LOG_WARNING("watchpoint not set");
+ return ERROR_OK;
+ }
+
+ wp_i = watchpoint->set - 1;
+ if ((wp_i < 0) || (wp_i >= aarch64->wp_num)) {
+ LOG_DEBUG("Invalid WP number in watchpoint");
+ return ERROR_OK;
+ }
+ LOG_DEBUG("rwp %i control 0x%0" PRIx32 " value 0x%0" PRIx64, wp_i,
+ wp_list[wp_i].control, wp_list[wp_i].value);
+ wp_list[wp_i].used = 0;
+ wp_list[wp_i].value = 0;
+ wp_list[wp_i].control = 0;
+ retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+ + CPUV8_DBG_WCR_BASE + 16 * wp_list[wp_i].brpn,
+ wp_list[wp_i].control);
+ if (retval != ERROR_OK)
+ return retval;
+ retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+ + CPUV8_DBG_WVR_BASE + 16 * wp_list[wp_i].brpn,
+ wp_list[wp_i].value);
+ if (retval != ERROR_OK)
+ return retval;
+
+ retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+ + CPUV8_DBG_WVR_BASE + 4 + 16 * wp_list[wp_i].brpn,
+ (uint32_t)wp_list[wp_i].value);
+ if (retval != ERROR_OK)
+ return retval;
+ watchpoint->set = 0;
+
+ return ERROR_OK;
+}
+
+static int aarch64_add_watchpoint(struct target *target,
+ struct watchpoint *watchpoint)
+{
+ int retval;
+ struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+ if (aarch64->wp_num_available < 1) {
+ LOG_INFO("no hardware watchpoint available");
+ return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+ }
+
+ retval = aarch64_set_watchpoint(target, watchpoint);
+ if (retval == ERROR_OK)
+ aarch64->wp_num_available--;
+
+ return retval;
+}
+
+static int aarch64_remove_watchpoint(struct target *target,
+ struct watchpoint *watchpoint)
+{
+ struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+ if (watchpoint->set) {
+ aarch64_unset_watchpoint(target, watchpoint);
+ aarch64->wp_num_available++;
+ }
+
+ return ERROR_OK;
+}
+
+/**
+ * find out which watchpoint hits
+ * get exception address and compare the address to watchpoints
+ */
+int aarch64_hit_watchpoint(struct target *target,
+ struct watchpoint **hit_watchpoint)
+{
+ if (target->debug_reason != DBG_REASON_WATCHPOINT)
+ return ERROR_FAIL;
+
+ struct armv8_common *armv8 = target_to_armv8(target);
+
+ target_addr_t exception_address;
+ struct watchpoint *wp;
+
+ exception_address = armv8->dpm.wp_addr;
+
+ if (exception_address == 0xFFFFFFFF)
+ return ERROR_FAIL;
+
+ for (wp = target->watchpoints; wp; wp = wp->next)
+ if (exception_address >= wp->address && exception_address < (wp->address + wp->length)) {
+ *hit_watchpoint = wp;
+ return ERROR_OK;
+ }
+
+ return ERROR_FAIL;
+}
+