+static int dsp563xx_reg_write_high_io(struct target *target, uint32_t instr_mask, uint32_t data)
+{
+ int err;
+ uint32_t instr;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ /* we use r0 to store temporary data */
+ if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
+ dsp563xx->read_core_reg(target, REG_NUM_R0);
+
+ /* move data to r0 */
+ if ((err = dsp563xx_once_execute_dw_ir(target->tap, 0, 0x60F400, data)) != ERROR_OK)
+ return err;
+ /* move r0 to destination memory */
+ instr = INSTR_MOVEP_REG_HIO(MEM_X, 1, EAME_R0, instr_mask);
+ if ((err = dsp563xx_once_execute_sw_ir(target->tap, 1, instr)) != ERROR_OK)
+ return err;
+
+ /* r0 is no longer valid on target */
+ dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_reg_read(struct target *target, uint32_t eame, uint32_t * data)
+{
+ int err;
+ uint32_t instr;
+
+ instr = INSTR_MOVEP_REG_HIO(MEM_X, 1, eame, 0xfffffc);
+ if ((err = dsp563xx_once_execute_sw_ir(target->tap, 0, instr)) != ERROR_OK)
+ return err;
+ /* nop */
+ if ((err = dsp563xx_once_execute_sw_ir(target->tap, 1, 0x000000)) != ERROR_OK)
+ return err;
+ /* read debug register */
+ return dsp563xx_once_reg_read(target->tap, 1, DSP563XX_ONCE_OGDBR, data);
+}
+
+static int dsp563xx_reg_write(struct target *target, uint32_t instr_mask, uint32_t data)
+{
+ int err;
+
+ if ((err = dsp563xx_once_execute_dw_ir(target->tap, 0, instr_mask, data)) != ERROR_OK)
+ return err;
+ /* nop */
+ return dsp563xx_once_execute_sw_ir(target->tap, 1, 0x000000);
+}
+
+static int dsp563xx_reg_pc_read(struct target *target)
+{
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ /* pc was changed, nothing todo */
+ if (dsp563xx->core_cache->reg_list[REG_NUM_PC].dirty)
+ return ERROR_OK;
+
+ /* 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", __FUNCTION__);
+
+ /* TODO: use disassembly to set correct pc offset */
+ dsp563xx->core_regs[REG_NUM_PC] = (once_regs[ONCE_REG_IDX_OPABF11].reg >> 1) & 0x00FFFFFF;
+ }
+ else
+ {
+ if ( once_regs[ONCE_REG_IDX_OPABEX].reg == once_regs[ONCE_REG_IDX_OPABFR].reg )
+ {
+ dsp563xx->core_regs[REG_NUM_PC] = once_regs[ONCE_REG_IDX_OPABEX].reg;
+ }
+ else
+ {
+ dsp563xx->core_regs[REG_NUM_PC] = once_regs[ONCE_REG_IDX_OPABEX].reg - 1;
+ }
+ }
+ }
+ else
+ {
+ dsp563xx->core_regs[REG_NUM_PC] = once_regs[ONCE_REG_IDX_OPABEX].reg;
+ }
+
+ dsp563xx->read_core_reg(target, REG_NUM_PC);
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_reg_ssh_read(struct target *target)
+{
+ int err;
+ uint32_t sp, sc, ep;
+ struct dsp563xx_core_reg *arch_info;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSH].arch_info;
+
+ /* get a valid stack pointer */
+ if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
+ return err;
+ sp = dsp563xx->core_regs[REG_NUM_SP];
+ if ((err = dsp563xx_write_register(target, REG_NUM_SP, 0)) != ERROR_OK)
+ return err;
+
+ /* get a valid stack count */
+ if ((err = dsp563xx_read_register(target, REG_NUM_SC, 0)) != ERROR_OK)
+ return err;
+ sc = dsp563xx->core_regs[REG_NUM_SC];
+ if ((err = dsp563xx_write_register(target, REG_NUM_SC, 0)) != ERROR_OK)
+ return err;
+
+ /* get a valid extended pointer */
+ if ((err = dsp563xx_read_register(target, REG_NUM_EP, 0)) != ERROR_OK)
+ return err;
+ ep = dsp563xx->core_regs[REG_NUM_EP];
+ if ((err = dsp563xx_write_register(target, REG_NUM_EP, 0)) != ERROR_OK)
+ return err;
+
+ if (!sp)
+ {
+ sp = 0x00FFFFFF;
+ }
+ else
+ {
+ if ((err = dsp563xx_reg_read(target, arch_info->eame, &sp)) != ERROR_OK)
+ return err;
+
+ if ((err = dsp563xx_write_register(target, REG_NUM_SC, 1)) != ERROR_OK)
+ return err;
+ if ((err = dsp563xx_write_register(target, REG_NUM_SP, 1)) != ERROR_OK)
+ return err;
+ if ((err = dsp563xx_write_register(target, REG_NUM_EP, 1)) != ERROR_OK)
+ return err;
+ }
+
+ dsp563xx->core_regs[REG_NUM_SSH] = sp;
+ dsp563xx->read_core_reg(target, REG_NUM_SSH);
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_reg_ssh_write(struct target *target)
+{
+ int err;
+ uint32_t sp;
+ struct dsp563xx_core_reg *arch_info;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSH].arch_info;
+
+ /* get a valid stack pointer */
+ if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
+ return err;
+ sp = dsp563xx->core_regs[REG_NUM_SP];
+
+ if (sp)
+ {
+ sp--;
+ /* write new stackpointer */
+ dsp563xx->core_regs[REG_NUM_SP] = sp;
+ if ((err = dsp563xx->read_core_reg(target, REG_NUM_SP)) != ERROR_OK)
+ return err;
+ if ((err = dsp563xx_write_register(target, REG_NUM_SP, 1)) != ERROR_OK)
+ return err;
+
+ if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, dsp563xx->core_regs[REG_NUM_SSH])) != ERROR_OK)
+ return err;
+
+ if ((err = dsp563xx_read_register(target, REG_NUM_SP, 1)) != ERROR_OK)
+ return err;
+ if ((err = dsp563xx_read_register(target, REG_NUM_SSH, 1)) != ERROR_OK)
+ return err;
+ }
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_reg_ssl_read(struct target *target)
+{
+ int err;
+ uint32_t sp;
+ struct dsp563xx_core_reg *arch_info;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSL].arch_info;
+
+ /* get a valid stack pointer */
+ if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
+ return err;
+ sp = dsp563xx->core_regs[REG_NUM_SP];
+
+ if (!sp)
+ {
+ sp = 0x00FFFFFF;
+ }
+ else
+ {
+ if ((err = dsp563xx_reg_read(target, arch_info->eame, &sp)) != ERROR_OK)
+ return err;
+ }
+
+ dsp563xx->core_regs[REG_NUM_SSL] = sp;
+ dsp563xx->read_core_reg(target, REG_NUM_SSL);
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_read_register(struct target *target, int num, int force)
+{
+ int err = ERROR_OK;
+ uint32_t data = 0;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+ struct dsp563xx_core_reg *arch_info;
+
+ if (force)
+ dsp563xx->core_cache->reg_list[num].valid = 0;
+
+ if (!dsp563xx->core_cache->reg_list[num].valid)
+ {
+ arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
+
+ switch (arch_info->num)
+ {
+ case REG_NUM_SSH:
+ err = dsp563xx_reg_ssh_read(target);
+ break;
+ case REG_NUM_SSL:
+ err = dsp563xx_reg_ssl_read(target);
+ break;
+ case REG_NUM_PC:
+ err = dsp563xx_reg_pc_read(target);
+ break;
+ case REG_NUM_IPRC:
+ case REG_NUM_IPRP:
+ case REG_NUM_BCR:
+ case REG_NUM_DCR:
+ case REG_NUM_AAR0:
+ case REG_NUM_AAR1:
+ case REG_NUM_AAR2:
+ case REG_NUM_AAR3:
+ err = dsp563xx_reg_read_high_io(target, arch_info->instr_mask, &data);
+ if (err == ERROR_OK)
+ {
+ dsp563xx->core_regs[num] = data;
+ dsp563xx->read_core_reg(target, num);
+ }
+ break;
+ default:
+ err = dsp563xx_reg_read(target, arch_info->eame, &data);
+ if (err == ERROR_OK)
+ {
+ dsp563xx->core_regs[num] = data;
+ dsp563xx->read_core_reg(target, num);
+ }
+ break;
+ }
+
+ }
+
+ return err;
+}
+
+static int dsp563xx_write_register(struct target *target, int num, int force)
+{
+ int err = ERROR_OK;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+ struct dsp563xx_core_reg *arch_info;
+
+ if (force)
+ dsp563xx->core_cache->reg_list[num].dirty = 1;
+
+ if (dsp563xx->core_cache->reg_list[num].dirty)
+ {
+ arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
+
+ dsp563xx->write_core_reg(target, num);
+
+ switch (arch_info->num)
+ {
+ case REG_NUM_SSH:
+ err = dsp563xx_reg_ssh_write(target);
+ break;
+ case REG_NUM_PC:
+ /* pc is updated on resume, no need to write it here */
+ break;
+ case REG_NUM_IPRC:
+ case REG_NUM_IPRP:
+ case REG_NUM_BCR:
+ case REG_NUM_DCR:
+ case REG_NUM_AAR0:
+ case REG_NUM_AAR1:
+ case REG_NUM_AAR2:
+ case REG_NUM_AAR3:
+ err = dsp563xx_reg_write_high_io(target, arch_info->instr_mask, dsp563xx->core_regs[num]);
+ break;
+ default:
+ err = dsp563xx_reg_write(target, arch_info->instr_mask, dsp563xx->core_regs[num]);
+
+ if ((err == ERROR_OK) && (arch_info->num == REG_NUM_SP))
+ {
+ dsp563xx->core_cache->reg_list[REG_NUM_SSH].valid = 0;
+ dsp563xx->core_cache->reg_list[REG_NUM_SSL].valid = 0;
+ }
+
+ break;
+ }
+ }
+
+ return err;
+}
+
+static int dsp563xx_save_context(struct target *target)
+{
+ int i, err = ERROR_OK;
+
+ for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
+ {
+ if ((err = dsp563xx_read_register(target, i, 0)) != ERROR_OK)
+ break;
+ }
+
+ return err;
+}
+
+static int dsp563xx_restore_context(struct target *target)
+{
+ int i, err = ERROR_OK;
+
+ for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
+ {
+ if ((err = dsp563xx_write_register(target, i, 0)) != ERROR_OK)
+ break;
+ }
+
+ return err;
+}
+
+static void dsp563xx_invalidate_x_context(struct target *target, uint32_t addr_start, uint32_t addr_end )
+{
+ int i;
+ struct dsp563xx_core_reg *arch_info;
+ struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
+
+ if ( addr_start > ASM_REG_W_IPRC )
+ return;
+ if ( addr_start < ASM_REG_W_AAR3 )
+ return;
+
+ for (i = REG_NUM_IPRC; i < DSP563XX_NUMCOREREGS; i++)
+ {
+ arch_info = dsp563xx->core_cache->reg_list[i].arch_info;
+
+ 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;
+ }
+ }
+}
+
+static int dsp563xx_target_create(struct target *target, Jim_Interp * interp)
+{
+ struct dsp563xx_common *dsp563xx = calloc(1, sizeof(struct dsp563xx_common));
+
+ if (!dsp563xx)
+ return ERROR_INVALID_ARGUMENTS;
+
+ dsp563xx->jtag_info.tap = target->tap;
+ target->arch_info = dsp563xx;
+ dsp563xx->read_core_reg = dsp563xx_read_core_reg;
+ dsp563xx->write_core_reg = dsp563xx_write_core_reg;
+
+ return ERROR_OK;
+}
+
+static int dsp563xx_init_target(struct command_context *cmd_ctx, struct target *target)