git.gag.com Git - fw/openocd/blob - src/target/dsp563xx.c

   1 /***************************************************************************
   2  *   Copyright (C) 2009-2011 by Mathias Kuester                            *
   3  *   mkdorg@users.sourceforge.net                                          *
   4  *                                                                         *
   5  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program; if not, write to the                         *
  17  *   Free Software Foundation, Inc.,                                       *
  18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  19  ***************************************************************************/
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include <jim.h>
  25
  26 #include "target.h"
  27 #include "target_type.h"
  28 #include "register.h"
  29 #include "dsp563xx.h"
  30 #include "dsp563xx_once.h"
  31
  32 //#define DSP563XX_JTAG_INS_LEN         4
  33
  34 #define ASM_REG_W_R0    0x60F400
  35 #define ASM_REG_W_R1    0x61F400
  36 #define ASM_REG_W_R2    0x62F400
  37 #define ASM_REG_W_R3    0x63F400
  38 #define ASM_REG_W_R4    0x64F400
  39 #define ASM_REG_W_R5    0x65F400
  40 #define ASM_REG_W_R6    0x66F400
  41 #define ASM_REG_W_R7    0x67F400
  42
  43 #define ASM_REG_W_N0    0x70F400
  44 #define ASM_REG_W_N1    0x71F400
  45 #define ASM_REG_W_N2    0x72F400
  46 #define ASM_REG_W_N3    0x73F400
  47 #define ASM_REG_W_N4    0x74F400
  48 #define ASM_REG_W_N5    0x75F400
  49 #define ASM_REG_W_N6    0x76F400
  50 #define ASM_REG_W_N7    0x77F400
  51
  52 #define ASM_REG_W_M0    0x05F420
  53 #define ASM_REG_W_M1    0x05F421
  54 #define ASM_REG_W_M2    0x05F422
  55 #define ASM_REG_W_M3    0x05F423
  56 #define ASM_REG_W_M4    0x05F424
  57 #define ASM_REG_W_M5    0x05F425
  58 #define ASM_REG_W_M6    0x05F426
  59 #define ASM_REG_W_M7    0x05F427
  60
  61 #define ASM_REG_W_X0    0x44F400
  62 #define ASM_REG_W_X1    0x45F400
  63
  64 #define ASM_REG_W_Y0    0x46F400
  65 #define ASM_REG_W_Y1    0x47F400
  66
  67 #define ASM_REG_W_A0    0x50F400
  68 #define ASM_REG_W_A1    0x54F400
  69 #define ASM_REG_W_A2    0x52F400
  70
  71 #define ASM_REG_W_B0    0x51F400
  72 #define ASM_REG_W_B1    0x55F400
  73 #define ASM_REG_W_B2    0x53F400
  74
  75 #define ASM_REG_W_VBA   0x05F430
  76 #define ASM_REG_W_OMR   0x05F43A
  77 #define ASM_REG_W_EP    0x05F42A
  78 #define ASM_REG_W_SC    0x05F431
  79 #define ASM_REG_W_SZ    0x05F438
  80 #define ASM_REG_W_SR    0x05F439
  81 #define ASM_REG_W_SP    0x05F43B
  82 #define ASM_REG_W_SSH   0x05F43C
  83 #define ASM_REG_W_SSL   0x05F43D
  84 #define ASM_REG_W_LA    0x05F43E
  85 #define ASM_REG_W_LC    0x05F43F
  86 #define ASM_REG_W_PC    0x000000
  87 #define ASM_REG_W_IPRC  0xFFFFFF
  88 #define ASM_REG_W_IPRP  0xFFFFFE
  89
  90 #define ASM_REG_W_BCR   0xFFFFFB
  91 #define ASM_REG_W_DCR   0xFFFFFA
  92 #define ASM_REG_W_AAR0  0xFFFFF9
  93 #define ASM_REG_W_AAR1  0xFFFFF8
  94 #define ASM_REG_W_AAR2  0xFFFFF7
  95 #define ASM_REG_W_AAR3  0xFFFFF6
  96
  97 static struct once_reg once_regs[] = {
  98         {0, 0x00, 24, "OSCR", 0},
  99         {1, 0x01, 24, "OMBC", 0},
 100         {2, 0x02, 24, "OBCR", 0},
 101         {3, 0x05, 24, "OMLR0", 0},
 102         {4, 0x06, 24, "OMLR1", 0},
 103         {5, 0x09, 24, "OGDBR", 0},
 104         {6, 0x0a, 24, "OPDBR", 0},
 105         {7, 0x0b, 24, "OPILR", 0},
 106         {8, 0x0c, 24, "PDB", 0},
 107         {9, 0x0d, 24, "OTC", 0},
 108         {10, 0x0f, 24, "OPABFR", 0},
 109         {11, 0x10, 24, "OPABDR", 0},
 110         {12, 0x11, 24, "OPABEX", 0},
 111         {13, 0x12, 25, "OPABF0", 0},
 112         {14, 0x12, 25, "OPABF1", 0},
 113         {15, 0x12, 25, "OPABF2", 0},
 114         {16, 0x12, 25, "OPABF3", 0},
 115         {17, 0x12, 25, "OPABF4", 0},
 116         {18, 0x12, 25, "OPABF5", 0},
 117         {19, 0x12, 25, "OPABF6", 0},
 118         {20, 0x12, 25, "OPABF7", 0},
 119         {21, 0x12, 25, "OPABF8", 0},
 120         {22, 0x12, 25, "OPABF9", 0},
 121         {23, 0x12, 25, "OPABF10", 0},
 122         {24, 0x12, 25, "OPABF11", 0},
 123 //      {25,0x1f,24,"NRSEL",0},
 124 };
 125
 126 static const struct
 127 {
 128         unsigned id;
 129         const char *name;
 130         unsigned bits;
 131         /* effective addressing mode encoding */
 132         uint8_t eame;
 133         uint32_t instr_mask;
 134 } dsp563xx_regs[] =
 135 {
 136         /* *INDENT-OFF* */
 137         /* address registers */
 138         { 0, "r0", 24, 0x10, ASM_REG_W_R0},
 139         { 1, "r1", 24, 0x11, ASM_REG_W_R1},
 140         { 2, "r2", 24, 0x12, ASM_REG_W_R2},
 141         { 3, "r3", 24, 0x13, ASM_REG_W_R3},
 142         { 4, "r4", 24, 0x14, ASM_REG_W_R4},
 143         { 5, "r5", 24, 0x15, ASM_REG_W_R5},
 144         { 6, "r6", 24, 0x16, ASM_REG_W_R6},
 145         { 7, "r7", 24, 0x17, ASM_REG_W_R7},
 146         /* offset registers */
 147         { 8, "n0", 24, 0x18, ASM_REG_W_N0},
 148         { 9, "n1", 24, 0x19, ASM_REG_W_N1},
 149         {10, "n2", 24, 0x1a, ASM_REG_W_N2},
 150         {11, "n3", 24, 0x1b, ASM_REG_W_N3},
 151         {12, "n4", 24, 0x1c, ASM_REG_W_N4},
 152         {13, "n5", 24, 0x1d, ASM_REG_W_N5},
 153         {14, "n6", 24, 0x1e, ASM_REG_W_N6},
 154         {15, "n7", 24, 0x1f, ASM_REG_W_N7},
 155         /* modifier registers */
 156         {16, "m0", 24, 0x20, ASM_REG_W_M0},
 157         {17, "m1", 24, 0x21, ASM_REG_W_M1},
 158         {18, "m2", 24, 0x22, ASM_REG_W_M2},
 159         {19, "m3", 24, 0x23, ASM_REG_W_M3},
 160         {20, "m4", 24, 0x24, ASM_REG_W_M4},
 161         {21, "m5", 24, 0x25, ASM_REG_W_M5},
 162         {22, "m6", 24, 0x26, ASM_REG_W_M6},
 163         {23, "m7", 24, 0x27, ASM_REG_W_M7},
 164         /* data alu input register */
 165         {24, "x0", 24, 0x04, ASM_REG_W_X0},
 166         {25, "x1", 24, 0x05, ASM_REG_W_X1},
 167         {26, "y0", 24, 0x06, ASM_REG_W_Y0},
 168         {27, "y1", 24, 0x07, ASM_REG_W_Y1},
 169         /* data alu accumulator register */
 170         {28, "a0", 24, 0x08, ASM_REG_W_A0},
 171         {29, "a1", 24, 0x0c, ASM_REG_W_A1},
 172         {30, "a2",  8, 0x0a, ASM_REG_W_A2},
 173         {31, "b0", 24, 0x09, ASM_REG_W_B0},
 174         {32, "b1", 24, 0x0d, ASM_REG_W_B1},
 175         {33, "b2",  8, 0x0b, ASM_REG_W_B2},
 176         /* stack */
 177         {34, "ssh",24, 0x3c, ASM_REG_W_SSH},
 178         {35, "ssl",24, 0x3d, ASM_REG_W_SSL},
 179         {36, "sp", 24, 0x3b, ASM_REG_W_SP},
 180         {37, "ep", 24, 0x2a, ASM_REG_W_EP},
 181         {38, "sz", 24, 0x38, ASM_REG_W_SZ},
 182         {39, "sc", 24, 0x31, ASM_REG_W_SC},
 183         /* system */
 184         {40, "pc", 24, 0x00, ASM_REG_W_PC},
 185         {41, "sr", 24, 0x39, ASM_REG_W_SR},
 186         {42, "omr",24, 0x3a, ASM_REG_W_OMR},
 187         {43, "la", 24, 0x3e, ASM_REG_W_LA},
 188         {44, "lc", 24, 0x3f, ASM_REG_W_LC},
 189         /* interrupt */
 190         {45, "vba", 24, 0x30, ASM_REG_W_VBA},
 191         {46, "iprc",24, 0x00, ASM_REG_W_IPRC},
 192         {47, "iprp",24, 0x00, ASM_REG_W_IPRP},
 193         /* port a */
 194         {48, "bcr", 24, 0x00, ASM_REG_W_BCR},
 195         {49, "dcr", 24, 0x00, ASM_REG_W_DCR},
 196         {50, "aar0",24, 0x00, ASM_REG_W_AAR0},
 197         {51, "aar1",24, 0x00, ASM_REG_W_AAR1},
 198         {52, "aar2",24, 0x00, ASM_REG_W_AAR2},
 199         {53, "aar3",24, 0x00, ASM_REG_W_AAR3},
 200         /* *INDENT-ON* */
 201 };
 202
 203 #define REG_NUM_R0      0
 204 #define REG_NUM_N0      8
 205 #define REG_NUM_N1      9
 206 #define REG_NUM_M0      16
 207 #define REG_NUM_M1      17
 208 #define REG_NUM_SSH     34
 209 #define REG_NUM_SSL     35
 210 #define REG_NUM_SP      36
 211 #define REG_NUM_EP      37
 212 #define REG_NUM_SC      39
 213 #define REG_NUM_PC      40
 214 #define REG_NUM_SR      41
 215 #define REG_NUM_IPRC    46
 216 #define REG_NUM_IPRP    47
 217 #define REG_NUM_BCR     48
 218 #define REG_NUM_DCR     49
 219 #define REG_NUM_AAR0    50
 220 #define REG_NUM_AAR1    51
 221 #define REG_NUM_AAR2    52
 222 #define REG_NUM_AAR3    53
 223
 224 #define INSTR_JUMP      0x0AF080
 225 /* Effective Addressing Mode Encoding */
 226 #define EAME_R0         0x10
 227 /* instrcution encoder */
 228 /* movep
 229  * s - peripheral space X/Y (X=0,Y=1)
 230  * w - write/read
 231  * d - source/destination register
 232  * p - IO short address
 233  */
 234 #define INSTR_MOVEP_REG_HIO(s,w,d,p)   (0x084000 | ((s & 1)<<16) | ((w&1)<<15) | ((d & 0x3f)<<8) | (p & 0x3f))
 235
 236 static int dsp563xx_get_gdb_reg_list(struct target *target, struct reg **reg_list[], int *reg_list_size)
 237 {
 238         int i;
 239         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 240
 241         if (target->state != TARGET_HALTED)
 242         {
 243                 return ERROR_TARGET_NOT_HALTED;
 244         }
 245
 246         *reg_list_size = DSP563XX_NUMCOREREGS;
 247         *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
 248
 249         if (!*reg_list)
 250                 return ERROR_INVALID_ARGUMENTS;
 251
 252         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 253         {
 254                 (*reg_list)[i] = &dsp563xx->core_cache->reg_list[i];
 255         }
 256
 257         return ERROR_OK;
 258
 259 }
 260
 261 static int dsp563xx_read_core_reg(struct target *target, int num)
 262 {
 263         uint32_t reg_value;
 264         struct dsp563xx_core_reg *dsp563xx_core_reg;
 265         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 266
 267         if ((num < 0) || (num >= DSP563XX_NUMCOREREGS))
 268                 return ERROR_INVALID_ARGUMENTS;
 269
 270         dsp563xx_core_reg = dsp563xx->core_cache->reg_list[num].arch_info;
 271         reg_value = dsp563xx->core_regs[num];
 272         buf_set_u32(dsp563xx->core_cache->reg_list[num].value, 0, 32, reg_value);
 273         dsp563xx->core_cache->reg_list[num].valid = 1;
 274         dsp563xx->core_cache->reg_list[num].dirty = 0;
 275
 276         return ERROR_OK;
 277 }
 278
 279 static int dsp563xx_write_core_reg(struct target *target, int num)
 280 {
 281         uint32_t reg_value;
 282         struct dsp563xx_core_reg *dsp563xx_core_reg;
 283         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 284
 285         if ((num < 0) || (num >= DSP563XX_NUMCOREREGS))
 286                 return ERROR_INVALID_ARGUMENTS;
 287
 288         reg_value = buf_get_u32(dsp563xx->core_cache->reg_list[num].value, 0, 32);
 289         dsp563xx_core_reg = dsp563xx->core_cache->reg_list[num].arch_info;
 290         dsp563xx->core_regs[num] = reg_value;
 291         dsp563xx->core_cache->reg_list[num].valid = 1;
 292         dsp563xx->core_cache->reg_list[num].dirty = 0;
 293
 294         return ERROR_OK;
 295 }
 296
 297 static int dsp563xx_target_create(struct target *target, Jim_Interp * interp)
 298 {
 299         struct dsp563xx_common *dsp563xx = calloc(1, sizeof(struct dsp563xx_common));
 300
 301         if (!dsp563xx)
 302                 return ERROR_INVALID_ARGUMENTS;
 303
 304         dsp563xx->jtag_info.tap = target->tap;
 305         target->arch_info = dsp563xx;
 306         dsp563xx->read_core_reg = dsp563xx_read_core_reg;
 307         dsp563xx->write_core_reg = dsp563xx_write_core_reg;
 308
 309         return ERROR_OK;
 310 }
 311
 312 static int dsp563xx_get_core_reg(struct reg *reg)
 313 {
 314         struct dsp563xx_core_reg *dsp563xx_reg = reg->arch_info;
 315         struct target *target = dsp563xx_reg->target;
 316         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 317
 318         LOG_DEBUG("%s", __FUNCTION__);
 319
 320         if (target->state != TARGET_HALTED)
 321         {
 322                 return ERROR_TARGET_NOT_HALTED;
 323         }
 324
 325         return dsp563xx->read_core_reg(target, dsp563xx_reg->num);
 326 }
 327
 328 static int dsp563xx_set_core_reg(struct reg *reg, uint8_t * buf)
 329 {
 330         LOG_DEBUG("%s", __FUNCTION__);
 331
 332         struct dsp563xx_core_reg *dsp563xx_reg = reg->arch_info;
 333         struct target *target = dsp563xx_reg->target;
 334         uint32_t value = buf_get_u32(buf, 0, 32);
 335
 336         if (target->state != TARGET_HALTED)
 337         {
 338                 return ERROR_TARGET_NOT_HALTED;
 339         }
 340
 341         buf_set_u32(reg->value, 0, reg->size, value);
 342         reg->dirty = 1;
 343         reg->valid = 1;
 344
 345         return ERROR_OK;
 346 }
 347
 348 static int dsp563xx_read_register(struct target *target, int num, int force);
 349 static int dsp563xx_write_register(struct target *target, int num, int force);
 350
 351 static int dsp563xx_reg_read_high_io(struct target *target, uint32_t instr_mask, uint32_t * data)
 352 {
 353         int err;
 354         uint32_t instr;
 355         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 356
 357         /* we use r0 to store temporary data */
 358         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
 359                 dsp563xx->read_core_reg(target, REG_NUM_R0);
 360
 361         /* move source memory to r0 */
 362         instr = INSTR_MOVEP_REG_HIO(0, 0, EAME_R0, instr_mask);
 363         if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, instr)) != ERROR_OK)
 364                 return err;
 365         /* move r0 to debug register */
 366         instr = INSTR_MOVEP_REG_HIO(0, 1, EAME_R0, 0xfffffc);
 367         if ((err = dsp563xx_once_execute_sw_ir(target->tap, instr)) != ERROR_OK)
 368                 return err;
 369         /* read debug register */
 370         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OGDBR, data)) != ERROR_OK)
 371                 return err;
 372         /* r0 is no longer valid on target */
 373         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
 374
 375         return ERROR_OK;
 376 }
 377
 378 static int dsp563xx_reg_write_high_io(struct target *target, uint32_t instr_mask, uint32_t data)
 379 {
 380         int err;
 381         uint32_t instr;
 382         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 383
 384         /* we use r0 to store temporary data */
 385         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
 386                 dsp563xx->read_core_reg(target, REG_NUM_R0);
 387
 388         /* move data to r0 */
 389         if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, 0x60F400, data)) != ERROR_OK)
 390                 return err;
 391         /* move r0 to destination memory */
 392         instr = INSTR_MOVEP_REG_HIO(0, 1, EAME_R0, instr_mask);
 393         if ((err = dsp563xx_once_execute_sw_ir(target->tap, instr)) != ERROR_OK)
 394                 return err;
 395
 396         /* r0 is no longer valid on target */
 397         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
 398
 399         return ERROR_OK;
 400 }
 401
 402 static int dsp563xx_reg_read(struct target *target, uint32_t eame, uint32_t * data)
 403 {
 404         int err;
 405         uint32_t instr;
 406
 407         instr = INSTR_MOVEP_REG_HIO(0, 1, eame, 0xfffffc);
 408         if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, instr)) != ERROR_OK)
 409                 return err;
 410         /* nop */
 411         if ((err = dsp563xx_once_execute_sw_ir(target->tap, 0x000000)) != ERROR_OK)
 412                 return err;
 413         /* read debug register */
 414         return dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OGDBR, data);
 415 }
 416
 417 static int dsp563xx_reg_write(struct target *target, uint32_t instr_mask, uint32_t data)
 418 {
 419         int err;
 420
 421         if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, instr_mask, data)) != ERROR_OK)
 422                 return err;
 423         /* nop */
 424         return dsp563xx_once_execute_sw_ir(target->tap, 0x000000);
 425 }
 426
 427 static int dsp563xx_reg_pc_read(struct target *target)
 428 {
 429         int err;
 430         uint32_t opabdr, opabex;
 431         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 432
 433         /* pc was changed, nothing todo */
 434         if (dsp563xx->core_cache->reg_list[REG_NUM_PC].dirty)
 435                 return ERROR_OK;
 436
 437         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABDR, &opabdr)) != ERROR_OK)
 438                 return err;
 439         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABEX, &opabex)) != ERROR_OK)
 440                 return err;
 441
 442         /* conditional branch check */
 443         if (opabdr == opabex)
 444         {
 445                 /* TODO: check the trace buffer and if a
 446                  * conditional branch is detected then decode
 447                  * the branch command and add the relative
 448                  * address to the current pc
 449                  */
 450                 LOG_DEBUG("%s conditional branch not supported yet", __FUNCTION__);
 451         }
 452         else
 453         {
 454                 dsp563xx->core_regs[REG_NUM_PC] = opabex;
 455                 dsp563xx->read_core_reg(target, REG_NUM_PC);
 456         }
 457
 458         return ERROR_OK;
 459 }
 460
 461 static int dsp563xx_reg_ssh_read(struct target *target)
 462 {
 463         int err;
 464         uint32_t sp, sc, ep;
 465         struct dsp563xx_core_reg *arch_info;
 466         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 467
 468         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSH].arch_info;
 469
 470         /* get a valid stack pointer */
 471         if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 472                 return err;
 473         sp = dsp563xx->core_regs[REG_NUM_SP];
 474         if ((err = dsp563xx_write_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 475                 return err;
 476
 477         /* get a valid stack count */
 478         if ((err = dsp563xx_read_register(target, REG_NUM_SC, 0)) != ERROR_OK)
 479                 return err;
 480         sc = dsp563xx->core_regs[REG_NUM_SC];
 481         if ((err = dsp563xx_write_register(target, REG_NUM_SC, 0)) != ERROR_OK)
 482                 return err;
 483
 484         /* get a valid extended pointer */
 485         if ((err = dsp563xx_read_register(target, REG_NUM_EP, 0)) != ERROR_OK)
 486                 return err;
 487         ep = dsp563xx->core_regs[REG_NUM_EP];
 488         if ((err = dsp563xx_write_register(target, REG_NUM_EP, 0)) != ERROR_OK)
 489                 return err;
 490
 491         if (!sp)
 492         {
 493                 sp = 0x00FFFFFF;
 494         }
 495         else
 496         {
 497                 if ((err = dsp563xx_reg_read(target, arch_info->eame, &sp)) != ERROR_OK)
 498                         return err;
 499
 500                 if ((err = dsp563xx_write_register(target, REG_NUM_SC, 1)) != ERROR_OK)
 501                         return err;
 502                 if ((err = dsp563xx_write_register(target, REG_NUM_SP, 1)) != ERROR_OK)
 503                         return err;
 504                 if ((err = dsp563xx_write_register(target, REG_NUM_EP, 1)) != ERROR_OK)
 505                         return err;
 506         }
 507
 508         dsp563xx->core_regs[REG_NUM_SSH] = sp;
 509         dsp563xx->read_core_reg(target, REG_NUM_SSH);
 510
 511         return ERROR_OK;
 512 }
 513
 514 static int dsp563xx_reg_ssh_write(struct target *target)
 515 {
 516         int err;
 517         uint32_t sp;
 518         struct dsp563xx_core_reg *arch_info;
 519         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 520
 521         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSH].arch_info;
 522
 523         /* get a valid stack pointer */
 524         if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 525                 return err;
 526         sp = dsp563xx->core_regs[REG_NUM_SP];
 527
 528         if (sp)
 529         {
 530                 sp--;
 531                 /* write new stackpointer */
 532                 dsp563xx->core_regs[REG_NUM_SP] = sp;
 533                 if ((err = dsp563xx->read_core_reg(target, REG_NUM_SP)) != ERROR_OK)
 534                         return err;
 535                 if ((err = dsp563xx_write_register(target, REG_NUM_SP, 1)) != ERROR_OK)
 536                         return err;
 537
 538                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, dsp563xx->core_regs[REG_NUM_SSH])) != ERROR_OK)
 539                         return err;
 540
 541                 if ((err = dsp563xx_read_register(target, REG_NUM_SP, 1)) != ERROR_OK)
 542                         return err;
 543                 if ((err = dsp563xx_read_register(target, REG_NUM_SSH, 1)) != ERROR_OK)
 544                         return err;
 545         }
 546
 547         return ERROR_OK;
 548 }
 549
 550 static int dsp563xx_reg_ssl_read(struct target *target)
 551 {
 552         int err;
 553         uint32_t sp;
 554         struct dsp563xx_core_reg *arch_info;
 555         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 556
 557         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSL].arch_info;
 558
 559         /* get a valid stack pointer */
 560         if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 561                 return err;
 562         sp = dsp563xx->core_regs[REG_NUM_SP];
 563
 564         if (!sp)
 565         {
 566                 sp = 0x00FFFFFF;
 567         }
 568         else
 569         {
 570                 if ((err = dsp563xx_reg_read(target, arch_info->eame, &sp)) != ERROR_OK)
 571                         return err;
 572         }
 573
 574         dsp563xx->core_regs[REG_NUM_SSL] = sp;
 575         dsp563xx->read_core_reg(target, REG_NUM_SSL);
 576
 577         return ERROR_OK;
 578 }
 579
 580 static int dsp563xx_read_register(struct target *target, int num, int force)
 581 {
 582         int err = ERROR_OK;
 583         uint32_t data = 0;
 584         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 585         struct dsp563xx_core_reg *arch_info;
 586
 587         if (force)
 588                 dsp563xx->core_cache->reg_list[num].valid = 0;
 589
 590         if (!dsp563xx->core_cache->reg_list[num].valid)
 591         {
 592                 arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
 593
 594                 switch (arch_info->num)
 595                 {
 596                         case REG_NUM_SSH:
 597                                 err = dsp563xx_reg_ssh_read(target);
 598                                 break;
 599                         case REG_NUM_SSL:
 600                                 err = dsp563xx_reg_ssl_read(target);
 601                                 break;
 602                         case REG_NUM_PC:
 603                                 err = dsp563xx_reg_pc_read(target);
 604                                 break;
 605                         case REG_NUM_IPRC:
 606                         case REG_NUM_IPRP:
 607                         case REG_NUM_BCR:
 608                         case REG_NUM_DCR:
 609                         case REG_NUM_AAR0:
 610                         case REG_NUM_AAR1:
 611                         case REG_NUM_AAR2:
 612                         case REG_NUM_AAR3:
 613                                 err = dsp563xx_reg_read_high_io(target, arch_info->instr_mask, &data);
 614                                 if (err == ERROR_OK)
 615                                 {
 616                                         dsp563xx->core_regs[num] = data;
 617                                         dsp563xx->read_core_reg(target, num);
 618                                 }
 619                                 break;
 620                         default:
 621                                 err = dsp563xx_reg_read(target, arch_info->eame, &data);
 622                                 if (err == ERROR_OK)
 623                                 {
 624                                         dsp563xx->core_regs[num] = data;
 625                                         dsp563xx->read_core_reg(target, num);
 626                                 }
 627                                 break;
 628                 }
 629
 630         }
 631
 632         return err;
 633 }
 634
 635 static int dsp563xx_write_register(struct target *target, int num, int force)
 636 {
 637         int err = ERROR_OK;
 638         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 639         struct dsp563xx_core_reg *arch_info;
 640
 641         if (force)
 642                 dsp563xx->core_cache->reg_list[num].dirty = 1;
 643
 644         if (dsp563xx->core_cache->reg_list[num].dirty)
 645         {
 646                 arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
 647
 648                 dsp563xx->write_core_reg(target, num);
 649
 650                 switch (arch_info->num)
 651                 {
 652                         case REG_NUM_SSH:
 653                                 err = dsp563xx_reg_ssh_write(target);
 654                                 break;
 655                         case REG_NUM_PC:
 656                                 /* pc is updated on resume, no need to write it here */
 657                                 break;
 658                         case REG_NUM_IPRC:
 659                         case REG_NUM_IPRP:
 660                         case REG_NUM_BCR:
 661                         case REG_NUM_DCR:
 662                         case REG_NUM_AAR0:
 663                         case REG_NUM_AAR1:
 664                         case REG_NUM_AAR2:
 665                         case REG_NUM_AAR3:
 666                                 err = dsp563xx_reg_write_high_io(target, arch_info->instr_mask, dsp563xx->core_regs[num]);
 667                                 break;
 668                         default:
 669                                 err = dsp563xx_reg_write(target, arch_info->instr_mask, dsp563xx->core_regs[num]);
 670
 671                                 if ((err == ERROR_OK) && (arch_info->num == REG_NUM_SP))
 672                                 {
 673                                         dsp563xx->core_cache->reg_list[REG_NUM_SSH].valid = 0;
 674                                         dsp563xx->core_cache->reg_list[REG_NUM_SSL].valid = 0;
 675                                 }
 676
 677                                 break;
 678                 }
 679         }
 680
 681         return err;
 682 }
 683
 684 static int dsp563xx_save_context(struct target *target)
 685 {
 686         int i, err = ERROR_OK;
 687
 688         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 689         {
 690                 if ((err = dsp563xx_read_register(target, i, 0)) != ERROR_OK)
 691                         break;
 692         }
 693
 694         return err;
 695 }
 696
 697 static int dsp563xx_restore_context(struct target *target)
 698 {
 699         int i, err = ERROR_OK;
 700
 701         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 702         {
 703                 if ((err = dsp563xx_write_register(target, i, 0)) != ERROR_OK)
 704                         break;
 705         }
 706
 707         return err;
 708 }
 709
 710 static const struct reg_arch_type dsp563xx_reg_type = {
 711         .get = dsp563xx_get_core_reg,
 712         .set = dsp563xx_set_core_reg,
 713 };
 714
 715 static int dsp563xx_init_target(struct command_context *cmd_ctx, struct target *target)
 716 {
 717         /* get pointers to arch-specific information */
 718         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 719
 720         struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
 721         struct reg_cache *cache = malloc(sizeof(struct reg_cache));
 722         struct reg *reg_list = malloc(sizeof(struct reg) * DSP563XX_NUMCOREREGS);
 723         struct dsp563xx_core_reg *arch_info = malloc(sizeof(struct dsp563xx_core_reg) * DSP563XX_NUMCOREREGS);
 724         int i;
 725
 726         LOG_DEBUG("%s", __FUNCTION__);
 727
 728         /* Build the process context cache */
 729         cache->name = "dsp563xx registers";
 730         cache->next = NULL;
 731         cache->reg_list = reg_list;
 732         cache->num_regs = DSP563XX_NUMCOREREGS;
 733         (*cache_p) = cache;
 734         dsp563xx->core_cache = cache;
 735
 736         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 737         {
 738                 arch_info[i].num = dsp563xx_regs[i].id;
 739                 arch_info[i].name = dsp563xx_regs[i].name;
 740                 arch_info[i].size = dsp563xx_regs[i].bits;
 741                 arch_info[i].eame = dsp563xx_regs[i].eame;
 742                 arch_info[i].instr_mask = dsp563xx_regs[i].instr_mask;
 743                 arch_info[i].target = target;
 744                 arch_info[i].dsp563xx_common = dsp563xx;
 745                 reg_list[i].name = dsp563xx_regs[i].name;
 746                 reg_list[i].size = dsp563xx_regs[i].bits;
 747                 reg_list[i].value = calloc(1, 4);
 748                 reg_list[i].dirty = 0;
 749                 reg_list[i].valid = 0;
 750                 reg_list[i].type = &dsp563xx_reg_type;
 751                 reg_list[i].arch_info = &arch_info[i];
 752         }
 753
 754         return ERROR_OK;
 755 }
 756
 757 static int dsp563xx_arch_state(struct target *target)
 758 {
 759         LOG_DEBUG("%s", __FUNCTION__);
 760         return ERROR_OK;
 761 }
 762
 763 #define DSP563XX_SR_SA  (1<<17)
 764 #define DSP563XX_SR_SC  (1<<13)
 765
 766 static int dsp563xx_debug_once_init(struct target *target)
 767 {
 768         return dsp563xx_once_read_register(target->tap, once_regs, DSP563XX_NUMONCEREGS);
 769 }
 770
 771 static int dsp563xx_debug_init(struct target *target)
 772 {
 773         int err;
 774         uint32_t sr;
 775         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 776         struct dsp563xx_core_reg *arch_info;
 777
 778         if ((err = dsp563xx_debug_once_init(target)) != ERROR_OK)
 779                 return err;
 780
 781         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SR].arch_info;
 782
 783         /* check 24bit mode */
 784         if ((err = dsp563xx_read_register(target, REG_NUM_SR, 0)) != ERROR_OK)
 785                 return err;
 786
 787         sr = dsp563xx->core_regs[REG_NUM_SR];
 788
 789         if (sr & (DSP563XX_SR_SA | DSP563XX_SR_SC))
 790         {
 791                 sr &= ~(DSP563XX_SR_SA | DSP563XX_SR_SC);
 792
 793                 if ((err = dsp563xx_once_execute_dw_ir(target->tap, arch_info->instr_mask, sr)) != ERROR_OK)
 794                         return err;
 795                 dsp563xx->core_cache->reg_list[REG_NUM_SR].dirty = 1;
 796         }
 797
 798         if ((err = dsp563xx_read_register(target, REG_NUM_N0, 0)) != ERROR_OK)
 799                 return err;
 800         if ((err = dsp563xx_read_register(target, REG_NUM_N1, 0)) != ERROR_OK)
 801                 return err;
 802         if ((err = dsp563xx_read_register(target, REG_NUM_M0, 0)) != ERROR_OK)
 803                 return err;
 804         if ((err = dsp563xx_read_register(target, REG_NUM_M1, 0)) != ERROR_OK)
 805                 return err;
 806
 807         if (dsp563xx->core_regs[REG_NUM_N0] != 0x000000)
 808         {
 809                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_N0].arch_info;
 810                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0x000000)) != ERROR_OK)
 811                         return err;
 812         }
 813         dsp563xx->core_cache->reg_list[REG_NUM_N0].dirty = 1;
 814
 815         if (dsp563xx->core_regs[REG_NUM_N1] != 0x000000)
 816         {
 817                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_N1].arch_info;
 818                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0x000000)) != ERROR_OK)
 819                         return err;
 820         }
 821         dsp563xx->core_cache->reg_list[REG_NUM_N1].dirty = 1;
 822
 823         if (dsp563xx->core_regs[REG_NUM_M0] != 0xffffff)
 824         {
 825                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_M0].arch_info;
 826                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0xffffff)) != ERROR_OK)
 827                         return err;
 828         }
 829         dsp563xx->core_cache->reg_list[REG_NUM_M0].dirty = 1;
 830
 831         if (dsp563xx->core_regs[REG_NUM_M1] != 0xffffff)
 832         {
 833                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_M1].arch_info;
 834                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0xffffff)) != ERROR_OK)
 835                         return err;
 836         }
 837         dsp563xx->core_cache->reg_list[REG_NUM_M1].dirty = 1;
 838
 839         if ((err = dsp563xx_save_context(target)) != ERROR_OK)
 840                 return err;
 841
 842         return ERROR_OK;
 843 }
 844
 845 static int dsp563xx_jtag_debug_request(struct target *target)
 846 {
 847         return dsp563xx_once_request_debug(target->tap, target->state == TARGET_RESET);
 848 }
 849
 850 static int dsp563xx_poll(struct target *target)
 851 {
 852         int err;
 853         uint32_t once_status;
 854         int state;
 855
 856         state = dsp563xx_once_target_status(target->tap);
 857
 858         if (state == TARGET_UNKNOWN)
 859         {
 860                 target->state = state;
 861                 LOG_ERROR("jtag status contains invalid mode value - communication failure");
 862                 return ERROR_TARGET_FAILURE;
 863         }
 864
 865         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OSCR, &once_status)) != ERROR_OK)
 866                 return err;
 867
 868         if ((once_status & DSP563XX_ONCE_OSCR_DEBUG_M) == DSP563XX_ONCE_OSCR_DEBUG_M)
 869         {
 870                 if (target->state != TARGET_HALTED)
 871                 {
 872                         target->state = TARGET_HALTED;
 873                         if ((err = dsp563xx_debug_init(target)) != ERROR_OK)
 874                                 return err;
 875
 876                         LOG_DEBUG("target->state: %s", target_state_name(target));
 877                 }
 878         }
 879
 880         return ERROR_OK;
 881 }
 882
 883 static int dsp563xx_halt(struct target *target)
 884 {
 885         int err;
 886         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 887
 888         if (target->state == TARGET_HALTED)
 889         {
 890                 LOG_DEBUG("target was already halted");
 891                 return ERROR_OK;
 892         }
 893
 894         if (target->state == TARGET_UNKNOWN)
 895         {
 896                 LOG_WARNING("target was in unknown state when halt was requested");
 897         }
 898
 899         if ((err = dsp563xx_jtag_debug_request(target)) != ERROR_OK)
 900                 return err;
 901
 902         /* store pipeline register */
 903         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPILR, &dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
 904                 return err;
 905         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPDBR, &dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
 906                 return err;
 907
 908         LOG_DEBUG("%s", __FUNCTION__);
 909
 910         return ERROR_OK;
 911 }
 912
 913 static int dsp563xx_resume(struct target *target, int current, uint32_t address, int handle_breakpoints, int debug_execution)
 914 {
 915         int err;
 916         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 917
 918         LOG_DEBUG("%s", __FUNCTION__);
 919
 920         if ((err = dsp563xx_restore_context(target)) != ERROR_OK)
 921                 return err;
 922         register_cache_invalidate(dsp563xx->core_cache);
 923
 924         if (current)
 925         {
 926                 /* restore pipeline registers and go */
 927                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPILR, dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
 928                         return err;
 929                 if ((err =
 930                      dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO,
 931                                              dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
 932                         return err;
 933         }
 934         else
 935         {
 936                 /* set to go register and jump */
 937                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR, INSTR_JUMP)) != ERROR_OK)
 938                         return err;
 939                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_PDBGOTO | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO, address)) != ERROR_OK)
 940                         return err;
 941         }
 942
 943         target->state = TARGET_RUNNING;
 944
 945         return ERROR_OK;
 946 }
 947
 948 static int dsp563xx_step_ex(struct target *target, int current, uint32_t address, int handle_breakpoints, int steps)
 949 {
 950         int err;
 951         uint32_t once_status;
 952         uint32_t dr_in, cnt;
 953         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 954
 955         if (target->state != TARGET_HALTED)
 956         {
 957                 LOG_DEBUG("target was not halted");
 958                 return ERROR_OK;
 959         }
 960
 961         LOG_DEBUG("%s %08X %08X", __FUNCTION__, current, (unsigned) address);
 962
 963         if ((err = dsp563xx_jtag_debug_request(target)) != ERROR_OK)
 964                 return err;
 965         if ((err = dsp563xx_restore_context(target)) != ERROR_OK)
 966                 return err;
 967
 968         /* reset trace mode */
 969         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OSCR, 0x000000)) != ERROR_OK)
 970                 return err;
 971         /* enable trace mode */
 972         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OSCR, DSP563XX_ONCE_OSCR_TME)) != ERROR_OK)
 973                 return err;
 974
 975         cnt = steps;
 976
 977         /* on JUMP we need one extra cycle */
 978         if (!current)
 979                 cnt++;
 980
 981         /* load step counter with N-1 */
 982         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OTC, cnt)) != ERROR_OK)
 983                 return err;
 984
 985         if (current)
 986         {
 987                 /* restore pipeline registers and go */
 988                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPILR, dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
 989                         return err;
 990                 if ((err =
 991                      dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO,
 992                                              dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
 993                         return err;
 994         }
 995         else
 996         {
 997                 /* set to go register and jump */
 998                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR, INSTR_JUMP)) != ERROR_OK)
 999                         return err;
1000                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_PDBGOTO | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO, address)) != ERROR_OK)
1001                         return err;
1002         }
1003
1004         while (1)
1005         {
1006                 if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OSCR, &once_status)) != ERROR_OK)
1007                         return err;
1008
1009                 if (once_status & DSP563XX_ONCE_OSCR_TO)
1010                 {
1011                         /* store pipeline register */
1012                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPILR, &dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
1013                                 return err;
1014                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPDBR, &dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
1015                                 return err;
1016
1017                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABFR, &dr_in)) != ERROR_OK)
1018                                 return err;
1019                         LOG_DEBUG("fetch: %08X", (unsigned) dr_in);
1020                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABDR, &dr_in)) != ERROR_OK)
1021                                 return err;
1022                         LOG_DEBUG("decode: %08X", (unsigned) dr_in);
1023                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABEX, &dr_in)) != ERROR_OK)
1024                                 return err;
1025                         LOG_DEBUG("execute: %08X", (unsigned) dr_in);
1026
1027                         /* reset trace mode */
1028                         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OSCR, 0x000000)) != ERROR_OK)
1029                                 return err;
1030
1031                         register_cache_invalidate(dsp563xx->core_cache);
1032                         if ((err = dsp563xx_debug_init(target)) != ERROR_OK)
1033                                 return err;
1034
1035                         break;
1036                 }
1037         }
1038
1039         return ERROR_OK;
1040 }
1041
1042 static int dsp563xx_step(struct target *target, int current, uint32_t address, int handle_breakpoints)
1043 {
1044         return dsp563xx_step_ex(target, current, address, handle_breakpoints, 0);
1045 }
1046
1047 static int dsp563xx_assert_reset(struct target *target)
1048 {
1049         int retval = 0;
1050         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
1051         enum reset_types jtag_reset_config = jtag_get_reset_config();
1052
1053         if (jtag_reset_config & RESET_HAS_SRST)
1054         {
1055                 /* default to asserting srst */
1056                 if (jtag_reset_config & RESET_SRST_PULLS_TRST)
1057                 {
1058                         jtag_add_reset(1, 1);
1059                 }
1060                 else
1061                 {
1062                         jtag_add_reset(0, 1);
1063                 }
1064         }
1065
1066         target->state = TARGET_RESET;
1067         jtag_add_sleep(5000);
1068
1069         /* registers are now invalid */
1070         register_cache_invalidate(dsp563xx->core_cache);
1071
1072         if (target->reset_halt)
1073         {
1074                 if ((retval = target_halt(target)) != ERROR_OK)
1075                         return retval;
1076         }
1077
1078         LOG_DEBUG("%s", __FUNCTION__);
1079         return ERROR_OK;
1080 }
1081
1082 static int dsp563xx_deassert_reset(struct target *target)
1083 {
1084         int err;
1085
1086         /* deassert reset lines */
1087         jtag_add_reset(0, 0);
1088
1089         if ((err = dsp563xx_poll(target)) != ERROR_OK)
1090                 return err;
1091
1092         if (target->reset_halt)
1093         {
1094                 if (target->state == TARGET_HALTED)
1095                 {
1096                         /* after a reset the cpu jmp to the
1097                          * reset vector and need 2 cycles to fill
1098                          * the cache (fetch,decode,excecute)
1099                          */
1100                         if ((err = dsp563xx_step_ex(target, 1, 0, 1, 1)) != ERROR_OK)
1101                                 return err;
1102                 }
1103         }
1104
1105 //      target->state = TARGET_RUNNING;
1106
1107         LOG_DEBUG("%s", __FUNCTION__);
1108         return ERROR_OK;
1109 }
1110
1111 static int dsp563xx_soft_reset_halt(struct target *target)
1112 {
1113         LOG_DEBUG("%s", __FUNCTION__);
1114         return ERROR_OK;
1115 }
1116
1117 /*
1118 * 000000                        nop
1119 * 46F400 AABBCC         move              #$aabbcc,y0
1120 * 60F400 AABBCC         move              #$aabbcc,r0
1121 * 467000 AABBCC         move              y0,x:AABBCC
1122 * 607000 AABBCC         move              r0,x:AABBCC
1123
1124 * 46E000                move              x:(r0),y0
1125 * 4EE000                move              y:(r0),y0
1126 * 07E086                move              p:(r0),y0
1127
1128 * 0450B9                move              sr,r0
1129 * 0446BA                move              omr,y0
1130 * 0446BC                move              ssh,y0
1131 * 0446BD                move              ssl,y0
1132 * 0446BE                move              la,y0
1133 * 0446BF                move              lc,y0
1134 *
1135 * 61F000 AABBCC         move              x:AABBCC,r1
1136 * 076190                movem             r0,p:(r1)
1137 *
1138 */
1139 static int dsp563xx_read_memory_p(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1140 {
1141         int err;
1142         uint32_t i, x;
1143         uint32_t data;
1144         uint8_t *b;
1145
1146         LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32, address, size, count);
1147
1148         if (target->state != TARGET_HALTED)
1149         {
1150                 LOG_WARNING("target not halted");
1151                 return ERROR_TARGET_NOT_HALTED;
1152         }
1153
1154         x = count;
1155
1156         for (i = 0; i < x; i++)
1157         {
1158                 if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, 0x60F400, address + i)) != ERROR_OK)
1159                         return err;
1160                 if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, 0x07E086)) != ERROR_OK)
1161                         return err;
1162                 if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, 0x467000, 0xfffffc)) != ERROR_OK)
1163                         return err;
1164                 if ((err = jtag_execute_queue()) != ERROR_OK)
1165                         return err;
1166
1167                 if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OGDBR, &data)) != ERROR_OK)
1168                         return err;
1169
1170                 b = buffer + 4 * i;
1171                 if (size > 0)
1172                         *b++ = data >> 0;
1173                 if (size > 1)
1174                         *b++ = data >> 8;
1175                 if (size > 2)
1176                         *b++ = data >> 16;
1177                 if (size > 3)
1178                         *b++ = 0x00;
1179         }
1180
1181         return ERROR_OK;
1182 }
1183
1184 static int dsp563xx_write_memory_p(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1185 {
1186         int err;
1187         uint32_t i, x;
1188         uint32_t data;
1189         uint8_t *b;
1190
1191         LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "", address, size, count);
1192
1193         if (target->state != TARGET_HALTED)
1194         {
1195                 LOG_WARNING("target not halted");
1196                 return ERROR_TARGET_NOT_HALTED;
1197         }
1198
1199         x = count;
1200
1201         for (i = 0; i < x; i++)
1202         {
1203                 b = buffer + 4 * i;
1204
1205                 data = 0;
1206                 if (size > 0)
1207                         data = *buffer++;
1208                 if (size > 1)
1209                         data |= (*buffer++) << 8;
1210                 if (size > 2)
1211                         data |= (*buffer++) << 16;
1212                 if (size > 3)
1213                         data |= (*buffer++) << 24;
1214
1215 //              LOG_DEBUG("%08X", data);
1216
1217                 if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, 0x61F400, address + i)) != ERROR_OK)
1218                         return err;
1219                 if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, 0x60F400, data)) != ERROR_OK)
1220                         return err;
1221                 if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, 0x076190)) != ERROR_OK)
1222                         return err;
1223                 if ((err = jtag_execute_queue()) != ERROR_OK)
1224                         return err;
1225         }
1226
1227         return ERROR_OK;
1228 }
1229
1230 /** Holds methods for DSP563XX targets. */
1231 struct target_type dsp563xx_target = {
1232         .name = "dsp563xx",
1233
1234         .poll = dsp563xx_poll,
1235         .arch_state = dsp563xx_arch_state,
1236
1237         .target_request_data = NULL,
1238
1239         .get_gdb_reg_list = dsp563xx_get_gdb_reg_list,
1240
1241         .halt = dsp563xx_halt,
1242         .resume = dsp563xx_resume,
1243         .step = dsp563xx_step,
1244
1245         .assert_reset = dsp563xx_assert_reset,
1246         .deassert_reset = dsp563xx_deassert_reset,
1247         .soft_reset_halt = dsp563xx_soft_reset_halt,
1248
1249         .read_memory = dsp563xx_read_memory_p,
1250         .write_memory = dsp563xx_write_memory_p,
1251
1252         .target_create = dsp563xx_target_create,
1253         .init_target = dsp563xx_init_target,
1254 };