flash/nor/at91samd: Use 32-bit register writes for ST-Link compat
[fw/openocd] / src / target / dsp563xx.c
index a121e8a3fa47984791b728347170e5bcf77ec631..36ee8537135f5574ae6cbf0a30e3124220641794 100644 (file)
@@ -1,21 +1,8 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
 /***************************************************************************
  *   Copyright (C) 2009-2011 by Mathias Kuester                            *
  *   mkdorg@users.sourceforge.net                                          *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
@@ -25,6 +12,7 @@
 #include <jim.h>
 
 #include "target.h"
+#include "breakpoints.h"
 #include "target_type.h"
 #include "algorithm.h"
 #include "register.h"
 #define ASM_REG_W_AAR2  0xFFFFF7
 #define ASM_REG_W_AAR3  0xFFFFF6
 
+/*
+ * OBCR Register bit definitions
+ */
+#define OBCR_B0_AND_B1            ((0x0) << 10)
+#define OBCR_B0_OR_B1             ((0x1) << 10)
+#define OBCR_B1_AFTER_B0          ((0x2) << 10)
+#define OBCR_B0_AFTER_B1          ((0x3) << 10)
+
+#define OBCR_BP_DISABLED          (0x0)
+#define OBCR_BP_MEM_P             (0x1)
+#define OBCR_BP_MEM_X             (0x2)
+#define OBCR_BP_MEM_Y             (0x3)
+#define OBCR_BP_ON_READ           ((0x2) << 0)
+#define OBCR_BP_ON_WRITE          ((0x1) << 0)
+#define OBCR_BP_CC_NOT_EQUAL      ((0x0) << 2)
+#define OBCR_BP_CC_EQUAL          ((0x1) << 2)
+#define OBCR_BP_CC_LESS_THAN      ((0x2) << 2)
+#define OBCR_BP_CC_GREATER_THAN   ((0x3) << 2)
+
+#define OBCR_BP_0(x)              ((x)<<2)
+#define OBCR_BP_1(x)              ((x)<<6)
+
+
 enum once_reg_idx {
        ONCE_REG_IDX_OSCR = 0,
        ONCE_REG_IDX_OMBC = 1,
@@ -290,10 +301,17 @@ enum memory_type {
        MEM_L = 3,
 };
 
+enum watchpoint_condition {
+       EQUAL,
+       NOT_EQUAL,
+       GREATER,
+       LESS_THAN
+};
+
 #define INSTR_JUMP      0x0AF080
 /* Effective Addressing Mode Encoding */
 #define EAME_R0         0x10
-/* instrcution encoder */
+/* instruction encoder */
 /* movep
  * s - peripheral space X/Y (X=0,Y=1)
  * w - write/read
@@ -304,7 +322,7 @@ enum memory_type {
        ((s & 1) << 16) | ((w & 1) << 15) | ((d & 0x3f) << 8) | (p & 0x3f))
 
 /* the gdb register list is send in this order */
-uint8_t gdb_reg_list_idx[] = {
+static const uint8_t gdb_reg_list_idx[] = {
        DSP563XX_REG_IDX_X1, DSP563XX_REG_IDX_X0, DSP563XX_REG_IDX_Y1, DSP563XX_REG_IDX_Y0,
        DSP563XX_REG_IDX_A2, DSP563XX_REG_IDX_A1, DSP563XX_REG_IDX_A0, DSP563XX_REG_IDX_B2,
        DSP563XX_REG_IDX_B1, DSP563XX_REG_IDX_B0, DSP563XX_REG_IDX_PC, DSP563XX_REG_IDX_SR,
@@ -323,7 +341,8 @@ uint8_t gdb_reg_list_idx[] = {
 
 static int dsp563xx_get_gdb_reg_list(struct target *target,
        struct reg **reg_list[],
-       int *reg_list_size)
+       int *reg_list_size,
+       enum target_register_class reg_class)
 {
        int i;
        struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
@@ -354,8 +373,8 @@ static int dsp563xx_read_core_reg(struct target *target, int num)
 
        reg_value = dsp563xx->core_regs[num];
        buf_set_u32(dsp563xx->core_cache->reg_list[num].value, 0, 32, reg_value);
-       dsp563xx->core_cache->reg_list[num].valid = 1;
-       dsp563xx->core_cache->reg_list[num].dirty = 0;
+       dsp563xx->core_cache->reg_list[num].valid = true;
+       dsp563xx->core_cache->reg_list[num].dirty = false;
 
        return ERROR_OK;
 }
@@ -370,8 +389,8 @@ static int dsp563xx_write_core_reg(struct target *target, int num)
 
        reg_value = buf_get_u32(dsp563xx->core_cache->reg_list[num].value, 0, 32);
        dsp563xx->core_regs[num] = reg_value;
-       dsp563xx->core_cache->reg_list[num].valid = 1;
-       dsp563xx->core_cache->reg_list[num].dirty = 0;
+       dsp563xx->core_cache->reg_list[num].valid = true;
+       dsp563xx->core_cache->reg_list[num].dirty = false;
 
        return ERROR_OK;
 }
@@ -402,8 +421,8 @@ static int dsp563xx_set_core_reg(struct reg *reg, uint8_t *buf)
                return ERROR_TARGET_NOT_HALTED;
 
        buf_set_u32(reg->value, 0, reg->size, value);
-       reg->dirty = 1;
-       reg->valid = 1;
+       reg->dirty = true;
+       reg->valid = true;
 
        return ERROR_OK;
 }
@@ -419,7 +438,7 @@ static void dsp563xx_build_reg_cache(struct target *target)
 
        struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
        struct reg_cache *cache = malloc(sizeof(struct reg_cache));
-       struct reg *reg_list = malloc(sizeof(struct reg) * DSP563XX_NUMCOREREGS);
+       struct reg *reg_list = calloc(DSP563XX_NUMCOREREGS, sizeof(struct reg));
        struct dsp563xx_core_reg *arch_info = malloc(
                        sizeof(struct dsp563xx_core_reg) * DSP563XX_NUMCOREREGS);
        int i;
@@ -443,8 +462,9 @@ static void dsp563xx_build_reg_cache(struct target *target)
                reg_list[i].name = dsp563xx_regs[i].name;
                reg_list[i].size = 32;  /* dsp563xx_regs[i].bits; */
                reg_list[i].value = calloc(1, 4);
-               reg_list[i].dirty = 0;
-               reg_list[i].valid = 0;
+               reg_list[i].dirty = false;
+               reg_list[i].valid = false;
+               reg_list[i].exist = true;
                reg_list[i].type = &dsp563xx_reg_type;
                reg_list[i].arch_info = &arch_info[i];
        }
@@ -478,7 +498,7 @@ static int dsp563xx_reg_read_high_io(struct target *target, uint32_t instr_mask,
        if (err != ERROR_OK)
                return err;
        /* r0 is no longer valid on target */
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].dirty = true;
 
        return ERROR_OK;
 }
@@ -504,7 +524,7 @@ static int dsp563xx_reg_write_high_io(struct target *target, uint32_t instr_mask
                return err;
 
        /* r0 is no longer valid on target */
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].dirty = true;
 
        return ERROR_OK;
 }
@@ -548,7 +568,7 @@ static int dsp563xx_reg_pc_read(struct target *target)
        /* conditional branch check */
        if (once_regs[ONCE_REG_IDX_OPABDR].reg == once_regs[ONCE_REG_IDX_OPABEX].reg) {
                if ((once_regs[ONCE_REG_IDX_OPABF11].reg & 1) == 0) {
-                       LOG_DEBUG("%s conditional branch not supported yet (0x%x 0x%x 0x%x)",
+                       LOG_DEBUG("%s conditional branch not supported yet (0x%" PRIx32 " 0x%" PRIx32 " 0x%" PRIx32 ")",
                                __func__,
                                (once_regs[ONCE_REG_IDX_OPABF11].reg >> 1),
                                once_regs[ONCE_REG_IDX_OPABDR].reg,
@@ -715,7 +735,7 @@ static int dsp563xx_read_register(struct target *target, int num, int force)
        struct dsp563xx_core_reg *arch_info;
 
        if (force)
-               dsp563xx->core_cache->reg_list[num].valid = 0;
+               dsp563xx->core_cache->reg_list[num].valid = false;
 
        if (!dsp563xx->core_cache->reg_list[num].valid) {
                arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
@@ -765,7 +785,7 @@ static int dsp563xx_write_register(struct target *target, int num, int force)
        struct dsp563xx_core_reg *arch_info;
 
        if (force)
-               dsp563xx->core_cache->reg_list[num].dirty = 1;
+               dsp563xx->core_cache->reg_list[num].dirty = true;
 
        if (dsp563xx->core_cache->reg_list[num].dirty) {
                arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
@@ -854,8 +874,8 @@ static void dsp563xx_invalidate_x_context(struct target *target,
 
                if ((arch_info->instr_mask >= addr_start) &&
                        (arch_info->instr_mask <= addr_end)) {
-                       dsp563xx->core_cache->reg_list[i].valid = 0;
-                       dsp563xx->core_cache->reg_list[i].dirty = 0;
+                       dsp563xx->core_cache->reg_list[i].valid = false;
+                       dsp563xx->core_cache->reg_list[i].dirty = false;
                }
        }
 }
@@ -880,6 +900,10 @@ static int dsp563xx_init_target(struct command_context *cmd_ctx, struct target *
        LOG_DEBUG("%s", __func__);
 
        dsp563xx_build_reg_cache(target);
+       struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+       dsp563xx->hardware_breakpoints_cleared = false;
+       dsp563xx->hardware_breakpoint[0].used = BPU_NONE;
 
        return ERROR_OK;
 }
@@ -902,7 +926,10 @@ static int dsp563xx_examine(struct target *target)
                if (((chip>>5)&0x1f) == 0)
                        chip += 300;
 
-               LOG_INFO("DSP56%03d device found", chip);
+               LOG_INFO("DSP56%03" PRIu32 " device found", chip);
+
+               /* Clear all breakpoints */
+               dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OBCR, 0);
        }
 
        return ERROR_OK;
@@ -948,7 +975,7 @@ static int dsp563xx_debug_init(struct target *target)
                err = dsp563xx_once_execute_dw_ir(target->tap, 1, arch_info->instr_mask, sr);
                if (err != ERROR_OK)
                        return err;
-               dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_SR].dirty = 1;
+               dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_SR].dirty = true;
        }
 
        err = dsp563xx_read_register(target, DSP563XX_REG_IDX_N0, 0);
@@ -970,7 +997,7 @@ static int dsp563xx_debug_init(struct target *target)
                if (err != ERROR_OK)
                        return err;
        }
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_N0].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_N0].dirty = true;
 
        if (dsp563xx->core_regs[DSP563XX_REG_IDX_N1] != 0x000000) {
                arch_info = dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_N1].arch_info;
@@ -978,7 +1005,7 @@ static int dsp563xx_debug_init(struct target *target)
                if (err != ERROR_OK)
                        return err;
        }
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_N1].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_N1].dirty = true;
 
        if (dsp563xx->core_regs[DSP563XX_REG_IDX_M0] != 0xffffff) {
                arch_info = dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_M0].arch_info;
@@ -986,7 +1013,7 @@ static int dsp563xx_debug_init(struct target *target)
                if (err != ERROR_OK)
                        return err;
        }
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_M0].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_M0].dirty = true;
 
        if (dsp563xx->core_regs[DSP563XX_REG_IDX_M1] != 0xffffff) {
                arch_info = dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_M1].arch_info;
@@ -994,7 +1021,7 @@ static int dsp563xx_debug_init(struct target *target)
                if (err != ERROR_OK)
                        return err;
        }
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_M1].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_M1].dirty = true;
 
        err = dsp563xx_save_context(target);
        if (err != ERROR_OK)
@@ -1040,11 +1067,27 @@ static int dsp563xx_poll(struct target *target)
                        else
                                target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 
-                       LOG_DEBUG("target->state: %s (%x)", target_state_name(target), once_status);
-                       LOG_INFO("halted: PC: 0x%x", dsp563xx->core_regs[DSP563XX_REG_IDX_PC]);
+                       LOG_DEBUG("target->state: %s (%" PRIx32 ")", target_state_name(target), once_status);
+                       LOG_INFO("halted: PC: 0x%" PRIx32, dsp563xx->core_regs[DSP563XX_REG_IDX_PC]);
                }
        }
 
+       if (!dsp563xx->hardware_breakpoints_cleared) {
+               err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OBCR, 0);
+               if (err != ERROR_OK)
+                       return err;
+
+               err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OMLR0, 0);
+               if (err != ERROR_OK)
+                       return err;
+
+               err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OMLR1, 0);
+               if (err != ERROR_OK)
+                       return err;
+
+               dsp563xx->hardware_breakpoints_cleared = true;
+       }
+
        return ERROR_OK;
 }
 
@@ -1073,7 +1116,7 @@ static int dsp563xx_halt(struct target *target)
 
 static int dsp563xx_resume(struct target *target,
        int current,
-       uint32_t address,
+       target_addr_t address,
        int handle_breakpoints,
        int debug_execution)
 {
@@ -1246,7 +1289,7 @@ static int dsp563xx_step_ex(struct target *target,
 
 static int dsp563xx_step(struct target *target,
        int current,
-       uint32_t address,
+       target_addr_t address,
        int handle_breakpoints)
 {
        int err;
@@ -1264,7 +1307,7 @@ static int dsp563xx_step(struct target *target,
        target->debug_reason = DBG_REASON_SINGLESTEP;
        target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 
-       LOG_INFO("halted: PC: 0x%x", dsp563xx->core_regs[DSP563XX_REG_IDX_PC]);
+       LOG_INFO("halted: PC: 0x%" PRIx32, dsp563xx->core_regs[DSP563XX_REG_IDX_PC]);
 
        return err;
 }
@@ -1314,7 +1357,7 @@ static int dsp563xx_deassert_reset(struct target *target)
                if (target->state == TARGET_HALTED) {
                        /* after a reset the cpu jmp to the
                         * reset vector and need 2 cycles to fill
-                        * the cache (fetch,decode,excecute)
+                        * the cache (fetch,decode,execute)
                         */
                        err = dsp563xx_step_ex(target, 1, 0, 1, 1);
                        if (err != ERROR_OK)
@@ -1327,16 +1370,10 @@ static int dsp563xx_deassert_reset(struct target *target)
        return ERROR_OK;
 }
 
-static int dsp563xx_soft_reset_halt(struct target *target)
-{
-       LOG_DEBUG("%s", __func__);
-       return ERROR_OK;
-}
-
 static int dsp563xx_run_algorithm(struct target *target,
        int num_mem_params, struct mem_param *mem_params,
        int num_reg_params, struct reg_param *reg_params,
-       uint32_t entry_point, uint32_t exit_point,
+       target_addr_t entry_point, target_addr_t exit_point,
        int timeout_ms, void *arch_info)
 {
        int i;
@@ -1349,6 +1386,8 @@ static int dsp563xx_run_algorithm(struct target *target,
        }
 
        for (i = 0; i < num_mem_params; i++) {
+               if (mem_params[i].direction == PARAM_IN)
+                       continue;
                retval = target_write_buffer(target, mem_params[i].address,
                                mem_params[i].size, mem_params[i].value);
                if (retval != ERROR_OK)
@@ -1356,9 +1395,12 @@ static int dsp563xx_run_algorithm(struct target *target,
        }
 
        for (i = 0; i < num_reg_params; i++) {
+               if (reg_params[i].direction == PARAM_IN)
+                       continue;
+
                struct reg *reg = register_get_by_name(dsp563xx->core_cache,
                                reg_params[i].reg_name,
-                               0);
+                               false);
 
                if (!reg) {
                        LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
@@ -1400,7 +1442,7 @@ static int dsp563xx_run_algorithm(struct target *target,
 
                        struct reg *reg = register_get_by_name(dsp563xx->core_cache,
                                        reg_params[i].reg_name,
-                                       0);
+                                       false);
                        if (!reg) {
                                LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
                                continue;
@@ -1509,9 +1551,9 @@ static int dsp563xx_read_memory_core(struct target *target,
                dsp563xx->read_core_reg(target, DSP563XX_REG_IDX_R1);
 
        /* r0 is no longer valid on target */
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].dirty = true;
        /* r1 is no longer valid on target */
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R1].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R1].dirty = true;
 
        x = count;
        b = buffer;
@@ -1554,7 +1596,7 @@ static int dsp563xx_read_memory_core(struct target *target,
 
 static int dsp563xx_read_memory(struct target *target,
        int mem_type,
-       uint32_t address,
+       target_addr_t address,
        uint32_t size,
        uint32_t count,
        uint8_t *buffer)
@@ -1622,7 +1664,7 @@ static int dsp563xx_read_memory(struct target *target,
 }
 
 static int dsp563xx_read_memory_default(struct target *target,
-       uint32_t address,
+       target_addr_t address,
        uint32_t size,
        uint32_t count,
        uint8_t *buffer)
@@ -1633,7 +1675,7 @@ static int dsp563xx_read_memory_default(struct target *target,
 }
 
 static int dsp563xx_read_buffer_default(struct target *target,
-       uint32_t address,
+       target_addr_t address,
        uint32_t size,
        uint8_t *buffer)
 {
@@ -1644,7 +1686,7 @@ static int dsp563xx_read_buffer_default(struct target *target,
 
 static int dsp563xx_write_memory_core(struct target *target,
        int mem_type,
-       uint32_t address,
+       target_addr_t address,
        uint32_t size,
        uint32_t count,
        const uint8_t *buffer)
@@ -1656,7 +1698,7 @@ static int dsp563xx_write_memory_core(struct target *target,
        const uint8_t *b;
 
        LOG_DEBUG(
-               "memtype: %d address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+               "memtype: %d address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
                mem_type,
                address,
                size,
@@ -1691,9 +1733,9 @@ static int dsp563xx_write_memory_core(struct target *target,
                dsp563xx->read_core_reg(target, DSP563XX_REG_IDX_R1);
 
        /* r0 is no longer valid on target */
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].dirty = true;
        /* r1 is no longer valid on target */
-       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R1].dirty = 1;
+       dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R1].dirty = true;
 
        x = count;
        b = buffer;
@@ -1728,7 +1770,7 @@ static int dsp563xx_write_memory_core(struct target *target,
 
 static int dsp563xx_write_memory(struct target *target,
        int mem_type,
-       uint32_t address,
+       target_addr_t address,
        uint32_t size,
        uint32_t count,
        const uint8_t *buffer)
@@ -1796,7 +1838,7 @@ static int dsp563xx_write_memory(struct target *target,
 }
 
 static int dsp563xx_write_memory_default(struct target *target,
-       uint32_t address,
+       target_addr_t address,
        uint32_t size,
        uint32_t count,
        const uint8_t *buffer)
@@ -1806,7 +1848,7 @@ static int dsp563xx_write_memory_default(struct target *target,
 }
 
 static int dsp563xx_write_buffer_default(struct target *target,
-       uint32_t address,
+       target_addr_t address,
        uint32_t size,
        const uint8_t *buffer)
 {
@@ -1814,85 +1856,233 @@ static int dsp563xx_write_buffer_default(struct target *target,
                        buffer);
 }
 
-static int dsp563xx_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
+/*
+ * Exit with error here, because we support watchpoints over a custom command.
+ * This is because the DSP has separate X,Y,P memspace which is not compatible to the
+ * traditional watchpoint logic.
+ */
+static int dsp563xx_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
-       return ERROR_OK;
+       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 }
 
-static int dsp563xx_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
+/*
+ * @see dsp563xx_add_watchpoint
+ */
+static int dsp563xx_remove_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
-       return ERROR_OK;
+       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 }
 
-static int dsp563xx_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
+static int dsp563xx_add_custom_watchpoint(struct target *target, uint32_t address, uint32_t mem_type,
+               enum watchpoint_rw rw, enum watchpoint_condition cond)
 {
-       return ERROR_OK;
+       int err = ERROR_OK;
+       struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+       bool was_running = false;
+       /* Only set breakpoint when halted */
+       if (target->state != TARGET_HALTED) {
+               dsp563xx_halt(target);
+               was_running = true;
+       }
+
+       if (dsp563xx->hardware_breakpoint[0].used) {
+               LOG_ERROR("Cannot add watchpoint. Hardware resource already used.");
+               err = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+       }
+
+       uint32_t obcr_value = 0;
+       if      (err == ERROR_OK) {
+               obcr_value |= OBCR_B0_OR_B1;
+               switch (mem_type) {
+                       case MEM_X:
+                               obcr_value |= OBCR_BP_MEM_X;
+                               break;
+                       case MEM_Y:
+                               obcr_value |= OBCR_BP_MEM_Y;
+                               break;
+                       case MEM_P:
+                               obcr_value |= OBCR_BP_MEM_P;
+                               break;
+                       default:
+                               LOG_ERROR("Unknown mem_type parameter (%" PRIu32 ")", mem_type);
+                               err = ERROR_TARGET_INVALID;
+               }
+       }
+
+       if (err == ERROR_OK) {
+               switch (rw) {
+                       case WPT_READ:
+                               obcr_value |= OBCR_BP_0(OBCR_BP_ON_READ);
+                               break;
+                       case WPT_WRITE:
+                               obcr_value |= OBCR_BP_0(OBCR_BP_ON_WRITE);
+                               break;
+                       case WPT_ACCESS:
+                               obcr_value |= OBCR_BP_0(OBCR_BP_ON_READ|OBCR_BP_ON_WRITE);
+                               break;
+                       default:
+                               LOG_ERROR("Unsupported write mode (%d)", rw);
+                               err = ERROR_TARGET_INVALID;
+               }
+       }
+
+       if (err == ERROR_OK) {
+               switch (cond) {
+                       case EQUAL:
+                               obcr_value |= OBCR_BP_0(OBCR_BP_CC_EQUAL);
+                               break;
+                       case NOT_EQUAL:
+                               obcr_value |= OBCR_BP_0(OBCR_BP_CC_NOT_EQUAL);
+                               break;
+                       case LESS_THAN:
+                               obcr_value |= OBCR_BP_0(OBCR_BP_CC_LESS_THAN);
+                               break;
+                       case GREATER:
+                               obcr_value |= OBCR_BP_0(OBCR_BP_CC_GREATER_THAN);
+                               break;
+                       default:
+                               LOG_ERROR("Unsupported condition code (%d)", cond);
+                               err = ERROR_TARGET_INVALID;
+               }
+       }
+
+       if (err == ERROR_OK)
+               err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OMLR0, address);
+
+       if (err == ERROR_OK)
+               err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OMLR1, 0x0);
+
+       if (err == ERROR_OK)
+               err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OBCR, obcr_value);
+
+       if (err == ERROR_OK) {
+               /* You should write the memory breakpoint counter to 0 */
+               err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OMBC, 0);
+       }
+
+       if (err == ERROR_OK) {
+               /* You should write the memory breakpoint counter to 0 */
+               err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OTC, 0);
+       }
+
+       if (err == ERROR_OK)
+               dsp563xx->hardware_breakpoint[0].used = BPU_WATCHPOINT;
+
+       if (err == ERROR_OK && was_running) {
+               /* Resume from current PC */
+               err = dsp563xx_resume(target, 1, 0x0, 0, 0);
+       }
+
+       return err;
 }
 
-static int dsp563xx_remove_watchpoint(struct target *target, struct watchpoint *watchpoint)
+static int dsp563xx_remove_custom_watchpoint(struct target *target)
 {
-       return ERROR_OK;
+       int err = ERROR_OK;
+       struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+       if (dsp563xx->hardware_breakpoint[0].used != BPU_WATCHPOINT) {
+               LOG_ERROR("Cannot remove watchpoint, as no watchpoint is currently configured!");
+               err = ERROR_TARGET_INVALID;
+       }
+
+       if (err == ERROR_OK) {
+               /* Clear watchpoint by clearing OBCR. */
+               err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OBCR, 0);
+       }
+
+       if (err == ERROR_OK)
+               dsp563xx->hardware_breakpoint[0].used = BPU_NONE;
+
+       return err;
 }
 
-static void handle_md_output(struct command_context *cmd_ctx,
-       struct target *target,
-       uint32_t address,
-       unsigned size,
-       unsigned count,
-       const uint8_t *buffer)
+COMMAND_HANDLER(dsp563xx_add_watchpoint_command)
 {
-       const unsigned line_bytecnt = 32;
-       unsigned line_modulo = line_bytecnt / size;
+       int err = ERROR_OK;
+       struct target *target = get_current_target(CMD_CTX);
+
+       uint32_t mem_type = 0;
+       switch (CMD_NAME[2]) {
+               case 'x':
+                       mem_type = MEM_X;
+                       break;
+               case 'y':
+                       mem_type = MEM_Y;
+                       break;
+               case 'p':
+                       mem_type = MEM_P;
+                       break;
+               default:
+                       return ERROR_COMMAND_SYNTAX_ERROR;
+       }
+
+       if (CMD_ARGC < 2)
+               return ERROR_COMMAND_SYNTAX_ERROR;
 
-       char output[line_bytecnt * 4 + 1];
-       unsigned output_len = 0;
+       uint32_t address = 0;
+       if (CMD_ARGC > 2)
+               COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], address);
 
-       const char *value_fmt;
-       switch (size) {
-               case 4:
-                       value_fmt = "%8.8x ";
+       enum watchpoint_condition cond;
+       switch (CMD_ARGV[0][0]) {
+               case '>':
+                       cond = GREATER;
+                       break;
+               case '<':
+                       cond = LESS_THAN;
                        break;
-               case 2:
-                       value_fmt = "%4.4x ";
+               case '=':
+                       cond = EQUAL;
                        break;
-               case 1:
-                       value_fmt = "%2.2x ";
+               case '!':
+                       cond = NOT_EQUAL;
                        break;
                default:
-                       /* "can't happen", caller checked */
-                       LOG_ERROR("invalid memory read size: %u", size);
-                       return;
-       }
-
-       for (unsigned i = 0; i < count; i++) {
-               if (i % line_modulo == 0)
-                       output_len += snprintf(output + output_len,
-                                       sizeof(output) - output_len,
-                                       "0x%8.8x: ",
-                                       (unsigned) (address + i));
-
-               uint32_t value = 0;
-               const uint8_t *value_ptr = buffer + i * size;
-               switch (size) {
-                       case 4:
-                               value = target_buffer_get_u32(target, value_ptr);
-                               break;
-                       case 2:
-                               value = target_buffer_get_u16(target, value_ptr);
-                               break;
-                       case 1:
-                               value = *value_ptr;
-               }
-               output_len += snprintf(output + output_len,
-                               sizeof(output) - output_len,
-                               value_fmt,
-                               value);
-
-               if ((i % line_modulo == line_modulo - 1) || (i == count - 1)) {
-                       command_print(cmd_ctx, "%s", output);
-                       output_len = 0;
-               }
+                       return ERROR_COMMAND_SYNTAX_ERROR;
+       }
+
+       enum watchpoint_rw rw;
+       switch (CMD_ARGV[1][0]) {
+               case 'r':
+                       rw = WPT_READ;
+                       break;
+               case 'w':
+                       rw = WPT_WRITE;
+                       break;
+               case 'a':
+                       rw = WPT_ACCESS;
+                       break;
+               default:
+                       return ERROR_COMMAND_SYNTAX_ERROR;
        }
+
+       err = dsp563xx_add_custom_watchpoint(target, address, mem_type, rw, cond);
+
+       return err;
+}
+
+/* Adding a breakpoint using the once breakpoint logic.
+ * Note that this mechanism is a true hw breakpoint and is share between the watchpoint logic.
+ * This means, you can only have one breakpoint/watchpoint at any time.
+ */
+static int dsp563xx_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
+{
+       return dsp563xx_add_custom_watchpoint(target, breakpoint->address, MEM_P, WPT_READ, EQUAL);
+}
+
+static int dsp563xx_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
+{
+       return dsp563xx_remove_custom_watchpoint(target);
+}
+
+COMMAND_HANDLER(dsp563xx_remove_watchpoint_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+
+       return dsp563xx_remove_custom_watchpoint(target);
 }
 
 COMMAND_HANDLER(dsp563xx_mem_command)
@@ -1950,13 +2140,13 @@ COMMAND_HANDLER(dsp563xx_mem_command)
                        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], count);
        }
 
-       buffer = calloc(count, sizeof(uint32_t));
+       buffer = calloc(count, 4);
 
        if (read_mem == 1) {
                err = dsp563xx_read_memory(target, mem_type, address, sizeof(uint32_t),
                                count, buffer);
                if (err == ERROR_OK)
-                       handle_md_output(CMD_CTX, target, address, sizeof(uint32_t), count, buffer);
+                       target_handle_md_output(CMD, target, address, sizeof(uint32_t), count, buffer);
 
        } else {
                b = buffer;
@@ -1985,42 +2175,73 @@ static const struct command_registration dsp563xx_command_handlers[] = {
                .handler = dsp563xx_mem_command,
                .mode = COMMAND_EXEC,
                .help = "write x memory words",
-               .usage = "mwwx address value [count]",
+               .usage = "address value [count]",
        },
        {
                .name = "mwwy",
                .handler = dsp563xx_mem_command,
                .mode = COMMAND_EXEC,
                .help = "write y memory words",
-               .usage = "mwwy address value [count]",
+               .usage = "address value [count]",
        },
        {
                .name = "mwwp",
                .handler = dsp563xx_mem_command,
                .mode = COMMAND_EXEC,
                .help = "write p memory words",
-               .usage = "mwwp address value [count]",
+               .usage = "address value [count]",
        },
        {
                .name = "mdwx",
                .handler = dsp563xx_mem_command,
                .mode = COMMAND_EXEC,
                .help = "display x memory words",
-               .usage = "mdwx address [count]",
+               .usage = "address [count]",
        },
        {
                .name = "mdwy",
                .handler = dsp563xx_mem_command,
                .mode = COMMAND_EXEC,
                .help = "display y memory words",
-               .usage = "mdwy address [count]",
+               .usage = "address [count]",
        },
        {
                .name = "mdwp",
                .handler = dsp563xx_mem_command,
                .mode = COMMAND_EXEC,
                .help = "display p memory words",
-               .usage = "mdwp address [count]",
+               .usage = "address [count]",
+       },
+  /*
+   * Watchpoint commands
+   */
+       {
+               .name = "wpp",
+               .handler = dsp563xx_add_watchpoint_command,
+               .mode = COMMAND_EXEC,
+               .help = "Create p memspace watchpoint",
+               .usage = "(>|<|=|!) (r|w|a) address",
+       },
+       {
+               .name = "wpx",
+               .handler = dsp563xx_add_watchpoint_command,
+               .mode = COMMAND_EXEC,
+               .help = "Create x memspace watchpoint",
+               .usage = "(>|<|=|!) (r|w|a) address",
+       },
+       {
+               .name = "wpy",
+               .handler = dsp563xx_add_watchpoint_command,
+               .mode = COMMAND_EXEC,
+               .help = "Create y memspace watchpoint",
+               .usage = "(>|<|=|!) (r|w|a) address",
+       },
+       {
+               .name = "rwpc",
+               .handler = dsp563xx_remove_watchpoint_command,
+               .mode = COMMAND_EXEC,
+               .help = "remove watchpoint custom",
+               .usage = "",
        },
        COMMAND_REGISTRATION_DONE
 };
@@ -2032,8 +2253,6 @@ struct target_type dsp563xx_target = {
        .poll = dsp563xx_poll,
        .arch_state = dsp563xx_arch_state,
 
-       .target_request_data = NULL,
-
        .get_gdb_reg_list = dsp563xx_get_gdb_reg_list,
 
        .halt = dsp563xx_halt,
@@ -2042,7 +2261,6 @@ struct target_type dsp563xx_target = {
 
        .assert_reset = dsp563xx_assert_reset,
        .deassert_reset = dsp563xx_deassert_reset,
-       .soft_reset_halt = dsp563xx_soft_reset_halt,
 
        .read_memory = dsp563xx_read_memory_default,
        .write_memory = dsp563xx_write_memory_default,