+static int dsp563xx_step_ex(struct target *target,
+ int current,
+ uint32_t address,
+ int handle_breakpoints,
+ int steps)
+{
+ int err;
+ uint32_t once_status;
+ uint32_t dr_in, cnt;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ if (target->state != TARGET_HALTED) {
+ LOG_DEBUG("target was not halted");
+ return ERROR_OK;
+ }
+
+ /* check if pc was changed and step want to execute the next address
+ * if pc was changed from gdb or other interface we will
+ * jump to this address and don't execute the next address
+ * this will not affect the step command with an address argument
+ * because current is set to zero then
+ */
+ if (current && dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_PC].dirty) {
+ dsp563xx_write_core_reg(target, DSP563XX_REG_IDX_PC);
+ address = dsp563xx->core_regs[DSP563XX_REG_IDX_PC];
+ current = 0;
+ }
+
+ LOG_DEBUG("%s %08X %08X", __func__, current, (unsigned) address);
+
+ err = dsp563xx_jtag_debug_request(target);
+ if (err != ERROR_OK)
+ return err;
+ err = dsp563xx_restore_context(target);
+ if (err != ERROR_OK)
+ return err;
+
+ /* reset trace mode */
+ err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OSCR, 0x000000);
+ if (err != ERROR_OK)
+ return err;
+ /* enable trace mode */
+ err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OSCR, DSP563XX_ONCE_OSCR_TME);
+ if (err != ERROR_OK)
+ return err;
+
+ cnt = steps;
+
+ /* on JUMP we need one extra cycle */
+ if (!current)
+ cnt++;
+
+ /* load step counter with N-1 */
+ err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OTC, cnt);
+ if (err != ERROR_OK)
+ return err;
+
+ if (current) {
+ /* restore pipeline registers and go */
+ err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OPDBR,
+ once_regs[ONCE_REG_IDX_OPILR].reg);
+ if (err != ERROR_OK)
+ return err;
+ err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OPDBR |
+ DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO,
+ once_regs[ONCE_REG_IDX_OPDBR].reg);
+ if (err != ERROR_OK)
+ return err;
+ } else {
+ /* set to go register and jump */
+ err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OPDBR, INSTR_JUMP);
+ if (err != ERROR_OK)
+ return err;
+ err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_PDBGOTO |
+ DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO,
+ address);
+ if (err != ERROR_OK)
+ return err;
+ }
+
+ while (1) {
+ err = dsp563xx_once_reg_read(target->tap, 1, DSP563XX_ONCE_OSCR, &once_status);
+ if (err != ERROR_OK)
+ return err;
+
+ if (once_status & DSP563XX_ONCE_OSCR_TO) {
+ err = dsp563xx_once_reg_read(target->tap, 1, DSP563XX_ONCE_OPABFR, &dr_in);
+ if (err != ERROR_OK)
+ return err;
+ LOG_DEBUG("fetch: %08X", (unsigned) dr_in&0x00ffffff);
+ err = dsp563xx_once_reg_read(target->tap, 1, DSP563XX_ONCE_OPABDR, &dr_in);
+ if (err != ERROR_OK)
+ return err;
+ LOG_DEBUG("decode: %08X", (unsigned) dr_in&0x00ffffff);
+ err = dsp563xx_once_reg_read(target->tap, 1, DSP563XX_ONCE_OPABEX, &dr_in);
+ if (err != ERROR_OK)
+ return err;
+ LOG_DEBUG("execute: %08X", (unsigned) dr_in&0x00ffffff);
+
+ /* reset trace mode */
+ err = dsp563xx_once_reg_write(target->tap, 1, DSP563XX_ONCE_OSCR, 0x000000);
+ if (err != ERROR_OK)
+ return err;
+
+ register_cache_invalidate(dsp563xx->core_cache);
+ err = dsp563xx_debug_init(target);
+ if (err != ERROR_OK)
+ return err;
+
+ break;
+ }
+ }
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_step(struct target *target,
+ int current,
+ uint32_t address,
+ int handle_breakpoints)
+{
+ int err;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ if (target->state != TARGET_HALTED) {
+ LOG_WARNING("target not halted");
+ return ERROR_TARGET_NOT_HALTED;
+ }
+
+ err = dsp563xx_step_ex(target, current, address, handle_breakpoints, 0);
+ if (err != ERROR_OK)
+ return err;
+
+ 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]);
+
+ return err;
+}
+
+static int dsp563xx_assert_reset(struct target *target)
+{
+ int retval = 0;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+ enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+ if (jtag_reset_config & RESET_HAS_SRST) {
+ /* default to asserting srst */
+ if (jtag_reset_config & RESET_SRST_PULLS_TRST)
+ jtag_add_reset(1, 1);
+ else
+ jtag_add_reset(0, 1);
+ }
+
+ target->state = TARGET_RESET;
+ jtag_add_sleep(5000);
+
+ /* registers are now invalid */
+ register_cache_invalidate(dsp563xx->core_cache);
+
+ if (target->reset_halt) {
+ retval = target_halt(target);
+ if (retval != ERROR_OK)
+ return retval;
+ }
+
+ LOG_DEBUG("%s", __func__);
+ return ERROR_OK;
+}
+
+static int dsp563xx_deassert_reset(struct target *target)
+{
+ int err;
+
+ /* deassert reset lines */
+ jtag_add_reset(0, 0);
+
+ err = dsp563xx_poll(target);
+ if (err != ERROR_OK)
+ return err;
+
+ if (target->reset_halt) {
+ 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)
+ */
+ err = dsp563xx_step_ex(target, 1, 0, 1, 1);
+ if (err != ERROR_OK)
+ return err;
+ }
+ } else
+ target->state = TARGET_RUNNING;
+
+ 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,
+ int timeout_ms, void *arch_info)
+{
+ int i;
+ int retval = ERROR_OK;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ if (target->state != TARGET_HALTED) {
+ LOG_WARNING("target not halted");
+ return ERROR_TARGET_NOT_HALTED;
+ }
+
+ for (i = 0; i < num_mem_params; i++) {
+ retval = target_write_buffer(target, mem_params[i].address,
+ mem_params[i].size, mem_params[i].value);
+ if (retval != ERROR_OK)
+ return retval;
+ }
+
+ for (i = 0; i < num_reg_params; i++) {
+ struct reg *reg = register_get_by_name(dsp563xx->core_cache,
+ reg_params[i].reg_name,
+ 0);
+
+ if (!reg) {
+ LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
+ continue;
+ }
+
+ if (reg->size != reg_params[i].size) {
+ LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size",
+ reg_params[i].reg_name);
+ continue;
+ }
+
+ retval = dsp563xx_set_core_reg(reg, reg_params[i].value);
+ if (retval != ERROR_OK)
+ return retval;
+ }
+
+ /* exec */
+ retval = target_resume(target, 0, entry_point, 1, 1);
+ if (retval != ERROR_OK)
+ return retval;
+
+ retval = target_wait_state(target, TARGET_HALTED, timeout_ms);
+ if (retval != ERROR_OK)
+ return retval;
+
+ for (i = 0; i < num_mem_params; i++) {
+ if (mem_params[i].direction != PARAM_OUT)
+ retval = target_read_buffer(target,
+ mem_params[i].address,
+ mem_params[i].size,
+ mem_params[i].value);
+ if (retval != ERROR_OK)
+ return retval;
+ }
+
+ for (i = 0; i < num_reg_params; i++) {
+ if (reg_params[i].direction != PARAM_OUT) {
+
+ struct reg *reg = register_get_by_name(dsp563xx->core_cache,
+ reg_params[i].reg_name,
+ 0);
+ if (!reg) {
+ LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
+ continue;
+ }
+
+ if (reg->size != reg_params[i].size) {
+ LOG_ERROR(
+ "BUG: register '%s' size doesn't match reg_params[i].size",
+ reg_params[i].reg_name);
+ continue;
+ }
+
+ buf_set_u32(reg_params[i].value, 0, 32, buf_get_u32(reg->value, 0, 32));
+ }
+ }
+
+ return ERROR_OK;
+}
+
+/* global command context from openocd.c */
+extern struct command_context *global_cmd_ctx;
+
+static int dsp563xx_get_default_memory(void)
+{
+ Jim_Interp *interp;
+ Jim_Obj *memspace;
+ char *c;
+
+ if (!global_cmd_ctx)
+ return MEM_P;
+
+ interp = global_cmd_ctx->interp;
+
+ if (!interp)
+ return MEM_P;
+
+ memspace = Jim_GetGlobalVariableStr(interp, "memspace", JIM_NONE);
+
+ if (!memspace)
+ return MEM_P;
+
+ c = (char *)Jim_GetString(memspace, NULL);
+
+ if (!c)
+ return MEM_P;
+
+ switch (c[0]) {
+ case '1':
+ return MEM_X;
+ case '2':
+ return MEM_Y;
+ case '3':
+ return MEM_L;
+ default:
+ break;
+ }
+
+ return MEM_P;
+}
+
+static int dsp563xx_read_memory_core(struct target *target,
+ int mem_type,
+ uint32_t address,
+ uint32_t size,
+ uint32_t count,
+ uint8_t *buffer)
+{
+ int err;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+ uint32_t i, x;
+ uint32_t data, move_cmd = 0;
+ uint8_t *b;
+
+ LOG_DEBUG(
+ "memtype: %d address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+ mem_type,
+ address,
+ size,
+ count);
+
+ if (target->state != TARGET_HALTED) {
+ LOG_WARNING("target not halted");
+ return ERROR_TARGET_NOT_HALTED;
+ }
+
+ switch (mem_type) {
+ case MEM_X:
+ /* TODO: mark effected queued registers */
+ move_cmd = 0x61d800;
+ break;
+ case MEM_Y:
+ move_cmd = 0x69d800;
+ break;
+ case MEM_P:
+ move_cmd = 0x07d891;
+ break;
+ default:
+ return ERROR_COMMAND_SYNTAX_ERROR;
+ }
+
+ /* we use r0 to store temporary data */
+ if (!dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].valid)
+ dsp563xx->read_core_reg(target, DSP563XX_REG_IDX_R0);
+ /* we use r1 to store temporary data */
+ if (!dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R1].valid)
+ 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;
+ /* r1 is no longer valid on target */
+ dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R1].dirty = 1;
+
+ x = count;
+ b = buffer;
+
+ err = dsp563xx_once_execute_dw_ir(target->tap, 1, 0x60F400, address);
+ if (err != ERROR_OK)
+ return err;
+
+ for (i = 0; i < x; i++) {
+ err = dsp563xx_once_execute_sw_ir(target->tap, 0, move_cmd);
+ if (err != ERROR_OK)
+ return err;
+ err = dsp563xx_once_execute_sw_ir(target->tap, 0, 0x08D13C);
+ if (err != ERROR_OK)
+ return err;
+ err = dsp563xx_once_reg_read(target->tap, 0,
+ DSP563XX_ONCE_OGDBR, (uint32_t *)(void *)b);
+ if (err != ERROR_OK)
+ return err;
+ b += 4;
+ }
+
+ /* flush the jtag queue */
+ err = jtag_execute_queue();
+ if (err != ERROR_OK)
+ return err;
+
+ /* walk over the buffer and fix target endianness */
+ b = buffer;
+
+ for (i = 0; i < x; i++) {
+ data = buf_get_u32(b, 0, 32) & 0x00FFFFFF;
+/* LOG_DEBUG("R: %08X", *((uint32_t*)b)); */
+ target_buffer_set_u32(target, b, data);
+ b += 4;
+ }
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_read_memory(struct target *target,
+ int mem_type,
+ uint32_t address,
+ uint32_t size,
+ uint32_t count,
+ uint8_t *buffer)
+{
+ int err;
+ uint32_t i, i1;
+ uint8_t *buffer_y, *buffer_x;
+
+ /* if size equals zero we are called from target read memory
+ * and have to handle the parameter here */
+ if ((size == 0) && (count != 0)) {
+ size = count % 4;
+
+ if (size)
+ LOG_DEBUG("size is not aligned to 4 byte");
+
+ count = (count - size) / 4;
+ size = 4;
+ }
+
+ /* we only support 4 byte aligned data */
+ if ((size != 4) || (!count))
+ return ERROR_COMMAND_SYNTAX_ERROR;
+
+ if (mem_type != MEM_L)
+ return dsp563xx_read_memory_core(target, mem_type, address, size, count, buffer);
+
+ buffer_y = malloc(size * count);
+ if (!buffer_y)
+ return ERROR_COMMAND_SYNTAX_ERROR;
+
+ buffer_x = malloc(size * count);
+ if (!buffer_x) {
+ free(buffer_y);
+ return ERROR_COMMAND_SYNTAX_ERROR;
+ }
+
+ err = dsp563xx_read_memory_core(target, MEM_Y, address, size, count / 2, buffer_y);
+
+ if (err != ERROR_OK) {
+ free(buffer_y);
+ free(buffer_x);
+ return err;
+ }
+
+ err = dsp563xx_read_memory_core(target, MEM_X, address, size, count / 2, buffer_x);
+
+ if (err != ERROR_OK) {
+ free(buffer_y);
+ free(buffer_x);
+ return err;
+ }
+
+ for (i = 0, i1 = 0; i < count; i += 2, i1++) {
+ buf_set_u32(buffer + i*sizeof(uint32_t), 0, 32,
+ buf_get_u32(buffer_y + i1 * sizeof(uint32_t), 0, 32));
+ buf_set_u32(buffer + (i + 1) * sizeof(uint32_t), 0, 32,
+ buf_get_u32(buffer_x + i1 * sizeof(uint32_t), 0, 32));
+ }
+
+ free(buffer_y);
+ free(buffer_x);
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_read_memory_default(struct target *target,
+ uint32_t address,
+ uint32_t size,
+ uint32_t count,
+ uint8_t *buffer)
+{
+
+ return dsp563xx_read_memory(target,
+ dsp563xx_get_default_memory(), address, size, count, buffer);
+}
+
+static int dsp563xx_read_buffer_default(struct target *target,
+ uint32_t address,
+ uint32_t size,
+ uint8_t *buffer)
+{
+
+ return dsp563xx_read_memory(target, dsp563xx_get_default_memory(), address, size, 0,
+ buffer);
+}
+
+static int dsp563xx_write_memory_core(struct target *target,
+ int mem_type,
+ uint32_t address,
+ uint32_t size,
+ uint32_t count,
+ const uint8_t *buffer)
+{
+ int err;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+ uint32_t i, x;
+ uint32_t data, move_cmd = 0;
+ const uint8_t *b;
+
+ LOG_DEBUG(
+ "memtype: %d address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+ mem_type,
+ address,
+ size,
+ count);
+
+ if (target->state != TARGET_HALTED) {
+ LOG_WARNING("target not halted");
+ return ERROR_TARGET_NOT_HALTED;
+ }
+
+ switch (mem_type) {
+ case MEM_X:
+ /* invalidate affected x registers */
+ dsp563xx_invalidate_x_context(target, address, address + count - 1);
+ move_cmd = 0x615800;
+ break;
+ case MEM_Y:
+ move_cmd = 0x695800;
+ break;
+ case MEM_P:
+ move_cmd = 0x075891;
+ break;
+ default:
+ return ERROR_COMMAND_SYNTAX_ERROR;
+ }
+
+ /* we use r0 to store temporary data */
+ if (!dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R0].valid)
+ dsp563xx->read_core_reg(target, DSP563XX_REG_IDX_R0);
+ /* we use r1 to store temporary data */
+ if (!dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R1].valid)
+ 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;
+ /* r1 is no longer valid on target */
+ dsp563xx->core_cache->reg_list[DSP563XX_REG_IDX_R1].dirty = 1;
+
+ x = count;
+ b = buffer;
+
+ err = dsp563xx_once_execute_dw_ir(target->tap, 1, 0x60F400, address);
+ if (err != ERROR_OK)
+ return err;
+
+ for (i = 0; i < x; i++) {
+ data = target_buffer_get_u32(target, b);
+
+/* LOG_DEBUG("W: %08X", data); */
+
+ data &= 0x00ffffff;
+
+ err = dsp563xx_once_execute_dw_ir(target->tap, 0, 0x61F400, data);
+ if (err != ERROR_OK)
+ return err;
+ err = dsp563xx_once_execute_sw_ir(target->tap, 0, move_cmd);
+ if (err != ERROR_OK)
+ return err;
+ b += 4;
+ }
+
+ /* flush the jtag queue */
+ err = jtag_execute_queue();
+ if (err != ERROR_OK)
+ return err;
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_write_memory(struct target *target,
+ int mem_type,
+ uint32_t address,
+ uint32_t size,
+ uint32_t count,
+ const uint8_t *buffer)
+{
+ int err;
+ uint32_t i, i1;
+ uint8_t *buffer_y, *buffer_x;
+
+ /* if size equals zero we are called from target write memory
+ * and have to handle the parameter here */
+ if ((size == 0) && (count != 0)) {
+ size = count % 4;
+
+ if (size)
+ LOG_DEBUG("size is not aligned to 4 byte");
+
+ count = (count - size) / 4;
+ size = 4;
+ }
+
+ /* we only support 4 byte aligned data */
+ if ((size != 4) || (!count))
+ return ERROR_COMMAND_SYNTAX_ERROR;
+
+ if (mem_type != MEM_L)
+ return dsp563xx_write_memory_core(target, mem_type, address, size, count, buffer);
+
+ buffer_y = malloc(size * count);
+ if (!buffer_y)
+ return ERROR_COMMAND_SYNTAX_ERROR;
+
+ buffer_x = malloc(size * count);
+ if (!buffer_x) {
+ free(buffer_y);
+ return ERROR_COMMAND_SYNTAX_ERROR;
+ }
+
+ for (i = 0, i1 = 0; i < count; i += 2, i1++) {
+ buf_set_u32(buffer_y + i1 * sizeof(uint32_t), 0, 32,
+ buf_get_u32(buffer + i * sizeof(uint32_t), 0, 32));
+ buf_set_u32(buffer_x + i1 * sizeof(uint32_t), 0, 32,
+ buf_get_u32(buffer + (i + 1) * sizeof(uint32_t), 0, 32));
+ }
+
+ err = dsp563xx_write_memory_core(target, MEM_Y, address, size, count / 2, buffer_y);
+
+ if (err != ERROR_OK) {
+ free(buffer_y);
+ free(buffer_x);
+ return err;
+ }
+
+ err = dsp563xx_write_memory_core(target, MEM_X, address, size, count / 2, buffer_x);
+
+ if (err != ERROR_OK) {
+ free(buffer_y);
+ free(buffer_x);
+ return err;
+ }
+
+ free(buffer_y);
+ free(buffer_x);
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_write_memory_default(struct target *target,
+ uint32_t address,
+ uint32_t size,
+ uint32_t count,
+ const uint8_t *buffer)
+{
+ return dsp563xx_write_memory(target,
+ dsp563xx_get_default_memory(), address, size, count, buffer);
+}
+
+static int dsp563xx_write_buffer_default(struct target *target,
+ uint32_t address,
+ uint32_t size,
+ const uint8_t *buffer)
+{
+ return dsp563xx_write_memory(target, dsp563xx_get_default_memory(), address, size, 0,
+ buffer);
+}
+
+/*
+ * 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_TARGET_RESOURCE_NOT_AVAILABLE;
+}
+
+/*
+ * @see dsp563xx_add_watchpoint
+ */
+static int dsp563xx_remove_watchpoint(struct target *target, struct watchpoint *watchpoint)
+{
+ return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+}
+
+static void handle_md_output(struct command_context *cmd_ctx,
+ struct target *target,
+ uint32_t address,
+ unsigned size,
+ unsigned count,
+ const uint8_t *buffer)
+{
+ const unsigned line_bytecnt = 32;
+ unsigned line_modulo = line_bytecnt / size;
+
+ char output[line_bytecnt * 4 + 1];
+ unsigned output_len = 0;
+
+ const char *value_fmt;
+ switch (size) {
+ case 4:
+ value_fmt = "%8.8x ";
+ break;
+ case 2:
+ value_fmt = "%4.4x ";
+ break;
+ case 1:
+ value_fmt = "%2.2x ";
+ 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;
+ }
+ }
+}
+
+static int dsp563xx_add_custom_watchpoint(struct target *target, uint32_t address, uint32_t memType,
+ enum watchpoint_rw rw, enum watchpoint_condition cond)
+{
+ int err = ERROR_OK;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ bool wasRunning = false;
+ /* Only set breakpoint when halted */
+ if (target->state != TARGET_HALTED) {
+ dsp563xx_halt(target);
+ wasRunning = 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 (memType) {
+ 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 memType parameter (%d)", memType);
+ 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 && wasRunning) {
+ /* Resume from current PC */
+ err = dsp563xx_resume(target, 1, 0x0, 0, 0);
+ }
+
+ return err;
+}
+
+static int dsp563xx_remove_custom_watchpoint(struct target *target)
+{
+ 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;
+}
+
+COMMAND_HANDLER(dsp563xx_add_watchpoint_command)
+{
+ 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;
+
+ uint32_t address = 0;
+ if (CMD_ARGC > 2)
+ COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], address);
+
+ enum watchpoint_condition cond;
+ switch (CMD_ARGV[0][0]) {
+ case '>':
+ cond = GREATER;
+ break;
+ case '<':
+ cond = LESS_THAN;
+ break;
+ case '=':
+ cond = EQUAL;
+ break;
+ case '!':
+ cond = NOT_EQUAL;
+ break;
+ default:
+ 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)
+{
+ struct target *target = get_current_target(CMD_CTX);
+ int err = ERROR_OK;
+ int read_mem;
+ uint32_t address = 0;
+ uint32_t count = 1, i;
+ uint32_t pattern = 0;
+ uint32_t mem_type;
+ uint8_t *buffer, *b;
+
+ switch (CMD_NAME[1]) {
+ case 'w':
+ read_mem = 0;
+ break;
+ case 'd':
+ read_mem = 1;
+ break;
+ default:
+ return ERROR_COMMAND_SYNTAX_ERROR;
+ }
+
+ switch (CMD_NAME[3]) {
+ 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 > 0)
+ COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+
+ if (read_mem == 0) {
+ if (CMD_ARGC < 2)
+ return ERROR_COMMAND_SYNTAX_ERROR;
+ if (CMD_ARGC > 1)
+ COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], pattern);
+ if (CMD_ARGC > 2)
+ COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], count);
+ }
+
+ if (read_mem == 1) {
+ if (CMD_ARGC < 1)
+ return ERROR_COMMAND_SYNTAX_ERROR;
+ if (CMD_ARGC > 1)
+ COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], count);
+ }
+
+ buffer = calloc(count, sizeof(uint32_t));
+
+ 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);
+
+ } else {
+ b = buffer;
+
+ for (i = 0; i < count; i++) {
+ target_buffer_set_u32(target, b, pattern);
+ b += 4;
+ }
+
+ err = dsp563xx_write_memory(target,
+ mem_type,
+ address,
+ sizeof(uint32_t),
+ count,
+ buffer);
+ }
+
+ free(buffer);
+
+ return err;
+}
+
+static const struct command_registration dsp563xx_command_handlers[] = {
+ {
+ .name = "mwwx",
+ .handler = dsp563xx_mem_command,
+ .mode = COMMAND_EXEC,
+ .help = "write x memory words",
+ .usage = "address value [count]",
+ },
+ {
+ .name = "mwwy",
+ .handler = dsp563xx_mem_command,
+ .mode = COMMAND_EXEC,
+ .help = "write y memory words",
+ .usage = "address value [count]",
+ },
+ {
+ .name = "mwwp",
+ .handler = dsp563xx_mem_command,
+ .mode = COMMAND_EXEC,
+ .help = "write p memory words",
+ .usage = "address value [count]",
+ },
+ {
+ .name = "mdwx",
+ .handler = dsp563xx_mem_command,
+ .mode = COMMAND_EXEC,
+ .help = "display x memory words",
+ .usage = "address [count]",
+ },
+ {
+ .name = "mdwy",
+ .handler = dsp563xx_mem_command,
+ .mode = COMMAND_EXEC,
+ .help = "display y memory words",
+ .usage = "address [count]",
+ },
+ {
+ .name = "mdwp",
+ .handler = dsp563xx_mem_command,
+ .mode = COMMAND_EXEC,
+ .help = "display p memory words",
+ .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
+};
+