+ 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%" PRIx32, 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,
+ target_addr_t entry_point, target_addr_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,
+ target_addr_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,
+ target_addr_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,
+ target_addr_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,
+ target_addr_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" TARGET_PRIxADDR ", 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,
+ target_addr_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;