+ int err;
+
+ LOG_DEBUG("%s", __func__);
+
+ if (target->state == TARGET_HALTED) {
+ LOG_DEBUG("target was already halted");
+ return ERROR_OK;
+ }
+
+ if (target->state == TARGET_UNKNOWN)
+ LOG_WARNING("target was in unknown state when halt was requested");
+
+ err = dsp563xx_jtag_debug_request(target);
+ if (err != ERROR_OK)
+ return err;
+
+ target->debug_reason = DBG_REASON_DBGRQ;
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_resume(struct target *target,
+ int current,
+ target_addr_t address,
+ int handle_breakpoints,
+ int debug_execution)
+{
+ int err;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ /* check if pc was changed and resume 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 resume 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_restore_context(target);
+ if (err != ERROR_OK)
+ return err;
+ register_cache_invalidate(dsp563xx->core_cache);
+
+ 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;
+ }
+
+ target->state = TARGET_RUNNING;
+
+ target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
+
+ return ERROR_OK;
+}
+
+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,
+ target_addr_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%" 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++) {
+ if (reg_params[i].direction == PARAM_IN)
+ continue;
+
+ 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;