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_R1      1
 205 #define REG_NUM_N0      8
 206 #define REG_NUM_N1      9
 207 #define REG_NUM_M0      16
 208 #define REG_NUM_M1      17
 209 #define REG_NUM_SSH     34
 210 #define REG_NUM_SSL     35
 211 #define REG_NUM_SP      36
 212 #define REG_NUM_EP      37
 213 #define REG_NUM_SC      39
 214 #define REG_NUM_PC      40
 215 #define REG_NUM_SR      41
 216 #define REG_NUM_IPRC    46
 217 #define REG_NUM_IPRP    47
 218 #define REG_NUM_BCR     48
 219 #define REG_NUM_DCR     49
 220 #define REG_NUM_AAR0    50
 221 #define REG_NUM_AAR1    51
 222 #define REG_NUM_AAR2    52
 223 #define REG_NUM_AAR3    53
 224
 225 enum memory_type
 226 {
 227         MEM_X = 0,
 228         MEM_Y = 1,
 229         MEM_P = 2,
 230 };
 231
 232 #define INSTR_JUMP      0x0AF080
 233 /* Effective Addressing Mode Encoding */
 234 #define EAME_R0         0x10
 235 /* instrcution encoder */
 236 /* movep
 237  * s - peripheral space X/Y (X=0,Y=1)
 238  * w - write/read
 239  * d - source/destination register
 240  * p - IO short address
 241  */
 242 #define INSTR_MOVEP_REG_HIO(s,w,d,p)   (0x084000 | ((s & 1)<<16) | ((w&1)<<15) | ((d & 0x3f)<<8) | (p & 0x3f))
 243
 244 static int dsp563xx_get_gdb_reg_list(struct target *target, struct reg **reg_list[], int *reg_list_size)
 245 {
 246         int i;
 247         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 248
 249         if (target->state != TARGET_HALTED)
 250         {
 251                 return ERROR_TARGET_NOT_HALTED;
 252         }
 253
 254         *reg_list_size = DSP563XX_NUMCOREREGS;
 255         *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
 256
 257         if (!*reg_list)
 258                 return ERROR_INVALID_ARGUMENTS;
 259
 260         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 261         {
 262                 (*reg_list)[i] = &dsp563xx->core_cache->reg_list[i];
 263         }
 264
 265         return ERROR_OK;
 266
 267 }
 268
 269 static int dsp563xx_read_core_reg(struct target *target, int num)
 270 {
 271         uint32_t reg_value;
 272         struct dsp563xx_core_reg *dsp563xx_core_reg;
 273         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 274
 275         if ((num < 0) || (num >= DSP563XX_NUMCOREREGS))
 276                 return ERROR_INVALID_ARGUMENTS;
 277
 278         dsp563xx_core_reg = dsp563xx->core_cache->reg_list[num].arch_info;
 279         reg_value = dsp563xx->core_regs[num];
 280         buf_set_u32(dsp563xx->core_cache->reg_list[num].value, 0, 32, reg_value);
 281         dsp563xx->core_cache->reg_list[num].valid = 1;
 282         dsp563xx->core_cache->reg_list[num].dirty = 0;
 283
 284         return ERROR_OK;
 285 }
 286
 287 static int dsp563xx_write_core_reg(struct target *target, int num)
 288 {
 289         uint32_t reg_value;
 290         struct dsp563xx_core_reg *dsp563xx_core_reg;
 291         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 292
 293         if ((num < 0) || (num >= DSP563XX_NUMCOREREGS))
 294                 return ERROR_INVALID_ARGUMENTS;
 295
 296         reg_value = buf_get_u32(dsp563xx->core_cache->reg_list[num].value, 0, 32);
 297         dsp563xx_core_reg = dsp563xx->core_cache->reg_list[num].arch_info;
 298         dsp563xx->core_regs[num] = reg_value;
 299         dsp563xx->core_cache->reg_list[num].valid = 1;
 300         dsp563xx->core_cache->reg_list[num].dirty = 0;
 301
 302         return ERROR_OK;
 303 }
 304
 305 static int dsp563xx_target_create(struct target *target, Jim_Interp * interp)
 306 {
 307         struct dsp563xx_common *dsp563xx = calloc(1, sizeof(struct dsp563xx_common));
 308
 309         if (!dsp563xx)
 310                 return ERROR_INVALID_ARGUMENTS;
 311
 312         dsp563xx->jtag_info.tap = target->tap;
 313         target->arch_info = dsp563xx;
 314         dsp563xx->read_core_reg = dsp563xx_read_core_reg;
 315         dsp563xx->write_core_reg = dsp563xx_write_core_reg;
 316
 317         return ERROR_OK;
 318 }
 319
 320 static int dsp563xx_get_core_reg(struct reg *reg)
 321 {
 322         struct dsp563xx_core_reg *dsp563xx_reg = reg->arch_info;
 323         struct target *target = dsp563xx_reg->target;
 324         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 325
 326         LOG_DEBUG("%s", __FUNCTION__);
 327
 328         if (target->state != TARGET_HALTED)
 329         {
 330                 return ERROR_TARGET_NOT_HALTED;
 331         }
 332
 333         return dsp563xx->read_core_reg(target, dsp563xx_reg->num);
 334 }
 335
 336 static int dsp563xx_set_core_reg(struct reg *reg, uint8_t * buf)
 337 {
 338         LOG_DEBUG("%s", __FUNCTION__);
 339
 340         struct dsp563xx_core_reg *dsp563xx_reg = reg->arch_info;
 341         struct target *target = dsp563xx_reg->target;
 342         uint32_t value = buf_get_u32(buf, 0, 32);
 343
 344         if (target->state != TARGET_HALTED)
 345         {
 346                 return ERROR_TARGET_NOT_HALTED;
 347         }
 348
 349         buf_set_u32(reg->value, 0, reg->size, value);
 350         reg->dirty = 1;
 351         reg->valid = 1;
 352
 353         return ERROR_OK;
 354 }
 355
 356 static int dsp563xx_read_register(struct target *target, int num, int force);
 357 static int dsp563xx_write_register(struct target *target, int num, int force);
 358
 359 static int dsp563xx_reg_read_high_io(struct target *target, uint32_t instr_mask, uint32_t * data)
 360 {
 361         int err;
 362         uint32_t instr;
 363         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 364
 365         /* we use r0 to store temporary data */
 366         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
 367                 dsp563xx->read_core_reg(target, REG_NUM_R0);
 368
 369         /* move source memory to r0 */
 370         instr = INSTR_MOVEP_REG_HIO(MEM_X, 0, EAME_R0, instr_mask);
 371         if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, instr)) != ERROR_OK)
 372                 return err;
 373         /* move r0 to debug register */
 374         instr = INSTR_MOVEP_REG_HIO(MEM_X, 1, EAME_R0, 0xfffffc);
 375         if ((err = dsp563xx_once_execute_sw_ir(target->tap, instr)) != ERROR_OK)
 376                 return err;
 377         /* read debug register */
 378         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OGDBR, data)) != ERROR_OK)
 379                 return err;
 380         /* r0 is no longer valid on target */
 381         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
 382
 383         return ERROR_OK;
 384 }
 385
 386 static int dsp563xx_reg_write_high_io(struct target *target, uint32_t instr_mask, uint32_t data)
 387 {
 388         int err;
 389         uint32_t instr;
 390         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 391
 392         /* we use r0 to store temporary data */
 393         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
 394                 dsp563xx->read_core_reg(target, REG_NUM_R0);
 395
 396         /* move data to r0 */
 397         if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, 0x60F400, data)) != ERROR_OK)
 398                 return err;
 399         /* move r0 to destination memory */
 400         instr = INSTR_MOVEP_REG_HIO(MEM_X, 1, EAME_R0, instr_mask);
 401         if ((err = dsp563xx_once_execute_sw_ir(target->tap, instr)) != ERROR_OK)
 402                 return err;
 403
 404         /* r0 is no longer valid on target */
 405         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
 406
 407         return ERROR_OK;
 408 }
 409
 410 static int dsp563xx_reg_read(struct target *target, uint32_t eame, uint32_t * data)
 411 {
 412         int err;
 413         uint32_t instr;
 414
 415         instr = INSTR_MOVEP_REG_HIO(MEM_X, 1, eame, 0xfffffc);
 416         if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, instr)) != ERROR_OK)
 417                 return err;
 418         /* nop */
 419         if ((err = dsp563xx_once_execute_sw_ir(target->tap, 0x000000)) != ERROR_OK)
 420                 return err;
 421         /* read debug register */
 422         return dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OGDBR, data);
 423 }
 424
 425 static int dsp563xx_reg_write(struct target *target, uint32_t instr_mask, uint32_t data)
 426 {
 427         int err;
 428
 429         if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, instr_mask, data)) != ERROR_OK)
 430                 return err;
 431         /* nop */
 432         return dsp563xx_once_execute_sw_ir(target->tap, 0x000000);
 433 }
 434
 435 static int dsp563xx_reg_pc_read(struct target *target)
 436 {
 437         int err;
 438         uint32_t opabdr, opabex;
 439         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 440
 441         /* pc was changed, nothing todo */
 442         if (dsp563xx->core_cache->reg_list[REG_NUM_PC].dirty)
 443                 return ERROR_OK;
 444
 445         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABDR, &opabdr)) != ERROR_OK)
 446                 return err;
 447         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABEX, &opabex)) != ERROR_OK)
 448                 return err;
 449
 450         /* conditional branch check */
 451         if (opabdr == opabex)
 452         {
 453                 /* TODO: check the trace buffer and if a
 454                  * conditional branch is detected then decode
 455                  * the branch command and add the relative
 456                  * address to the current pc
 457                  */
 458                 LOG_DEBUG("%s conditional branch not supported yet", __FUNCTION__);
 459         }
 460         else
 461         {
 462                 dsp563xx->core_regs[REG_NUM_PC] = opabex;
 463                 dsp563xx->read_core_reg(target, REG_NUM_PC);
 464         }
 465
 466         return ERROR_OK;
 467 }
 468
 469 static int dsp563xx_reg_ssh_read(struct target *target)
 470 {
 471         int err;
 472         uint32_t sp, sc, ep;
 473         struct dsp563xx_core_reg *arch_info;
 474         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 475
 476         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSH].arch_info;
 477
 478         /* get a valid stack pointer */
 479         if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 480                 return err;
 481         sp = dsp563xx->core_regs[REG_NUM_SP];
 482         if ((err = dsp563xx_write_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 483                 return err;
 484
 485         /* get a valid stack count */
 486         if ((err = dsp563xx_read_register(target, REG_NUM_SC, 0)) != ERROR_OK)
 487                 return err;
 488         sc = dsp563xx->core_regs[REG_NUM_SC];
 489         if ((err = dsp563xx_write_register(target, REG_NUM_SC, 0)) != ERROR_OK)
 490                 return err;
 491
 492         /* get a valid extended pointer */
 493         if ((err = dsp563xx_read_register(target, REG_NUM_EP, 0)) != ERROR_OK)
 494                 return err;
 495         ep = dsp563xx->core_regs[REG_NUM_EP];
 496         if ((err = dsp563xx_write_register(target, REG_NUM_EP, 0)) != ERROR_OK)
 497                 return err;
 498
 499         if (!sp)
 500         {
 501                 sp = 0x00FFFFFF;
 502         }
 503         else
 504         {
 505                 if ((err = dsp563xx_reg_read(target, arch_info->eame, &sp)) != ERROR_OK)
 506                         return err;
 507
 508                 if ((err = dsp563xx_write_register(target, REG_NUM_SC, 1)) != ERROR_OK)
 509                         return err;
 510                 if ((err = dsp563xx_write_register(target, REG_NUM_SP, 1)) != ERROR_OK)
 511                         return err;
 512                 if ((err = dsp563xx_write_register(target, REG_NUM_EP, 1)) != ERROR_OK)
 513                         return err;
 514         }
 515
 516         dsp563xx->core_regs[REG_NUM_SSH] = sp;
 517         dsp563xx->read_core_reg(target, REG_NUM_SSH);
 518
 519         return ERROR_OK;
 520 }
 521
 522 static int dsp563xx_reg_ssh_write(struct target *target)
 523 {
 524         int err;
 525         uint32_t sp;
 526         struct dsp563xx_core_reg *arch_info;
 527         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 528
 529         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSH].arch_info;
 530
 531         /* get a valid stack pointer */
 532         if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 533                 return err;
 534         sp = dsp563xx->core_regs[REG_NUM_SP];
 535
 536         if (sp)
 537         {
 538                 sp--;
 539                 /* write new stackpointer */
 540                 dsp563xx->core_regs[REG_NUM_SP] = sp;
 541                 if ((err = dsp563xx->read_core_reg(target, REG_NUM_SP)) != ERROR_OK)
 542                         return err;
 543                 if ((err = dsp563xx_write_register(target, REG_NUM_SP, 1)) != ERROR_OK)
 544                         return err;
 545
 546                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, dsp563xx->core_regs[REG_NUM_SSH])) != ERROR_OK)
 547                         return err;
 548
 549                 if ((err = dsp563xx_read_register(target, REG_NUM_SP, 1)) != ERROR_OK)
 550                         return err;
 551                 if ((err = dsp563xx_read_register(target, REG_NUM_SSH, 1)) != ERROR_OK)
 552                         return err;
 553         }
 554
 555         return ERROR_OK;
 556 }
 557
 558 static int dsp563xx_reg_ssl_read(struct target *target)
 559 {
 560         int err;
 561         uint32_t sp;
 562         struct dsp563xx_core_reg *arch_info;
 563         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 564
 565         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSL].arch_info;
 566
 567         /* get a valid stack pointer */
 568         if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 569                 return err;
 570         sp = dsp563xx->core_regs[REG_NUM_SP];
 571
 572         if (!sp)
 573         {
 574                 sp = 0x00FFFFFF;
 575         }
 576         else
 577         {
 578                 if ((err = dsp563xx_reg_read(target, arch_info->eame, &sp)) != ERROR_OK)
 579                         return err;
 580         }
 581
 582         dsp563xx->core_regs[REG_NUM_SSL] = sp;
 583         dsp563xx->read_core_reg(target, REG_NUM_SSL);
 584
 585         return ERROR_OK;
 586 }
 587
 588 static int dsp563xx_read_register(struct target *target, int num, int force)
 589 {
 590         int err = ERROR_OK;
 591         uint32_t data = 0;
 592         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 593         struct dsp563xx_core_reg *arch_info;
 594
 595         if (force)
 596                 dsp563xx->core_cache->reg_list[num].valid = 0;
 597
 598         if (!dsp563xx->core_cache->reg_list[num].valid)
 599         {
 600                 arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
 601
 602                 switch (arch_info->num)
 603                 {
 604                         case REG_NUM_SSH:
 605                                 err = dsp563xx_reg_ssh_read(target);
 606                                 break;
 607                         case REG_NUM_SSL:
 608                                 err = dsp563xx_reg_ssl_read(target);
 609                                 break;
 610                         case REG_NUM_PC:
 611                                 err = dsp563xx_reg_pc_read(target);
 612                                 break;
 613                         case REG_NUM_IPRC:
 614                         case REG_NUM_IPRP:
 615                         case REG_NUM_BCR:
 616                         case REG_NUM_DCR:
 617                         case REG_NUM_AAR0:
 618                         case REG_NUM_AAR1:
 619                         case REG_NUM_AAR2:
 620                         case REG_NUM_AAR3:
 621                                 err = dsp563xx_reg_read_high_io(target, arch_info->instr_mask, &data);
 622                                 if (err == ERROR_OK)
 623                                 {
 624                                         dsp563xx->core_regs[num] = data;
 625                                         dsp563xx->read_core_reg(target, num);
 626                                 }
 627                                 break;
 628                         default:
 629                                 err = dsp563xx_reg_read(target, arch_info->eame, &data);
 630                                 if (err == ERROR_OK)
 631                                 {
 632                                         dsp563xx->core_regs[num] = data;
 633                                         dsp563xx->read_core_reg(target, num);
 634                                 }
 635                                 break;
 636                 }
 637
 638         }
 639
 640         return err;
 641 }
 642
 643 static int dsp563xx_write_register(struct target *target, int num, int force)
 644 {
 645         int err = ERROR_OK;
 646         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 647         struct dsp563xx_core_reg *arch_info;
 648
 649         if (force)
 650                 dsp563xx->core_cache->reg_list[num].dirty = 1;
 651
 652         if (dsp563xx->core_cache->reg_list[num].dirty)
 653         {
 654                 arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
 655
 656                 dsp563xx->write_core_reg(target, num);
 657
 658                 switch (arch_info->num)
 659                 {
 660                         case REG_NUM_SSH:
 661                                 err = dsp563xx_reg_ssh_write(target);
 662                                 break;
 663                         case REG_NUM_PC:
 664                                 /* pc is updated on resume, no need to write it here */
 665                                 break;
 666                         case REG_NUM_IPRC:
 667                         case REG_NUM_IPRP:
 668                         case REG_NUM_BCR:
 669                         case REG_NUM_DCR:
 670                         case REG_NUM_AAR0:
 671                         case REG_NUM_AAR1:
 672                         case REG_NUM_AAR2:
 673                         case REG_NUM_AAR3:
 674                                 err = dsp563xx_reg_write_high_io(target, arch_info->instr_mask, dsp563xx->core_regs[num]);
 675                                 break;
 676                         default:
 677                                 err = dsp563xx_reg_write(target, arch_info->instr_mask, dsp563xx->core_regs[num]);
 678
 679                                 if ((err == ERROR_OK) && (arch_info->num == REG_NUM_SP))
 680                                 {
 681                                         dsp563xx->core_cache->reg_list[REG_NUM_SSH].valid = 0;
 682                                         dsp563xx->core_cache->reg_list[REG_NUM_SSL].valid = 0;
 683                                 }
 684
 685                                 break;
 686                 }
 687         }
 688
 689         return err;
 690 }
 691
 692 static int dsp563xx_save_context(struct target *target)
 693 {
 694         int i, err = ERROR_OK;
 695
 696         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 697         {
 698                 if ((err = dsp563xx_read_register(target, i, 0)) != ERROR_OK)
 699                         break;
 700         }
 701
 702         return err;
 703 }
 704
 705 static int dsp563xx_restore_context(struct target *target)
 706 {
 707         int i, err = ERROR_OK;
 708
 709         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 710         {
 711                 if ((err = dsp563xx_write_register(target, i, 0)) != ERROR_OK)
 712                         break;
 713         }
 714
 715         return err;
 716 }
 717
 718 static const struct reg_arch_type dsp563xx_reg_type = {
 719         .get = dsp563xx_get_core_reg,
 720         .set = dsp563xx_set_core_reg,
 721 };
 722
 723 static int dsp563xx_init_target(struct command_context *cmd_ctx, struct target *target)
 724 {
 725         /* get pointers to arch-specific information */
 726         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 727
 728         struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
 729         struct reg_cache *cache = malloc(sizeof(struct reg_cache));
 730         struct reg *reg_list = malloc(sizeof(struct reg) * DSP563XX_NUMCOREREGS);
 731         struct dsp563xx_core_reg *arch_info = malloc(sizeof(struct dsp563xx_core_reg) * DSP563XX_NUMCOREREGS);
 732         int i;
 733
 734         LOG_DEBUG("%s", __FUNCTION__);
 735
 736         /* Build the process context cache */
 737         cache->name = "dsp563xx registers";
 738         cache->next = NULL;
 739         cache->reg_list = reg_list;
 740         cache->num_regs = DSP563XX_NUMCOREREGS;
 741         (*cache_p) = cache;
 742         dsp563xx->core_cache = cache;
 743
 744         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 745         {
 746                 arch_info[i].num = dsp563xx_regs[i].id;
 747                 arch_info[i].name = dsp563xx_regs[i].name;
 748                 arch_info[i].size = dsp563xx_regs[i].bits;
 749                 arch_info[i].eame = dsp563xx_regs[i].eame;
 750                 arch_info[i].instr_mask = dsp563xx_regs[i].instr_mask;
 751                 arch_info[i].target = target;
 752                 arch_info[i].dsp563xx_common = dsp563xx;
 753                 reg_list[i].name = dsp563xx_regs[i].name;
 754                 reg_list[i].size = dsp563xx_regs[i].bits;
 755                 reg_list[i].value = calloc(1, 4);
 756                 reg_list[i].dirty = 0;
 757                 reg_list[i].valid = 0;
 758                 reg_list[i].type = &dsp563xx_reg_type;
 759                 reg_list[i].arch_info = &arch_info[i];
 760         }
 761
 762         return ERROR_OK;
 763 }
 764
 765 static int dsp563xx_arch_state(struct target *target)
 766 {
 767         LOG_DEBUG("%s", __FUNCTION__);
 768         return ERROR_OK;
 769 }
 770
 771 #define DSP563XX_SR_SA  (1<<17)
 772 #define DSP563XX_SR_SC  (1<<13)
 773
 774 static int dsp563xx_debug_once_init(struct target *target)
 775 {
 776         return dsp563xx_once_read_register(target->tap, once_regs, DSP563XX_NUMONCEREGS);
 777 }
 778
 779 static int dsp563xx_debug_init(struct target *target)
 780 {
 781         int err;
 782         uint32_t sr;
 783         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 784         struct dsp563xx_core_reg *arch_info;
 785
 786         if ((err = dsp563xx_debug_once_init(target)) != ERROR_OK)
 787                 return err;
 788
 789         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SR].arch_info;
 790
 791         /* check 24bit mode */
 792         if ((err = dsp563xx_read_register(target, REG_NUM_SR, 0)) != ERROR_OK)
 793                 return err;
 794
 795         sr = dsp563xx->core_regs[REG_NUM_SR];
 796
 797         if (sr & (DSP563XX_SR_SA | DSP563XX_SR_SC))
 798         {
 799                 sr &= ~(DSP563XX_SR_SA | DSP563XX_SR_SC);
 800
 801                 if ((err = dsp563xx_once_execute_dw_ir(target->tap, arch_info->instr_mask, sr)) != ERROR_OK)
 802                         return err;
 803                 dsp563xx->core_cache->reg_list[REG_NUM_SR].dirty = 1;
 804         }
 805
 806         if ((err = dsp563xx_read_register(target, REG_NUM_N0, 0)) != ERROR_OK)
 807                 return err;
 808         if ((err = dsp563xx_read_register(target, REG_NUM_N1, 0)) != ERROR_OK)
 809                 return err;
 810         if ((err = dsp563xx_read_register(target, REG_NUM_M0, 0)) != ERROR_OK)
 811                 return err;
 812         if ((err = dsp563xx_read_register(target, REG_NUM_M1, 0)) != ERROR_OK)
 813                 return err;
 814
 815         if (dsp563xx->core_regs[REG_NUM_N0] != 0x000000)
 816         {
 817                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_N0].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_N0].dirty = 1;
 822
 823         if (dsp563xx->core_regs[REG_NUM_N1] != 0x000000)
 824         {
 825                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_N1].arch_info;
 826                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0x000000)) != ERROR_OK)
 827                         return err;
 828         }
 829         dsp563xx->core_cache->reg_list[REG_NUM_N1].dirty = 1;
 830
 831         if (dsp563xx->core_regs[REG_NUM_M0] != 0xffffff)
 832         {
 833                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_M0].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_M0].dirty = 1;
 838
 839         if (dsp563xx->core_regs[REG_NUM_M1] != 0xffffff)
 840         {
 841                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_M1].arch_info;
 842                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0xffffff)) != ERROR_OK)
 843                         return err;
 844         }
 845         dsp563xx->core_cache->reg_list[REG_NUM_M1].dirty = 1;
 846
 847         if ((err = dsp563xx_save_context(target)) != ERROR_OK)
 848                 return err;
 849
 850         return ERROR_OK;
 851 }
 852
 853 static int dsp563xx_jtag_debug_request(struct target *target)
 854 {
 855         return dsp563xx_once_request_debug(target->tap, target->state == TARGET_RESET);
 856 }
 857
 858 static int dsp563xx_poll(struct target *target)
 859 {
 860         int err;
 861         uint32_t once_status;
 862         int state;
 863
 864         state = dsp563xx_once_target_status(target->tap);
 865
 866         if (state == TARGET_UNKNOWN)
 867         {
 868                 target->state = state;
 869                 LOG_ERROR("jtag status contains invalid mode value - communication failure");
 870                 return ERROR_TARGET_FAILURE;
 871         }
 872
 873         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OSCR, &once_status)) != ERROR_OK)
 874                 return err;
 875
 876         if ((once_status & DSP563XX_ONCE_OSCR_DEBUG_M) == DSP563XX_ONCE_OSCR_DEBUG_M)
 877         {
 878                 if (target->state != TARGET_HALTED)
 879                 {
 880                         target->state = TARGET_HALTED;
 881                         if ((err = dsp563xx_debug_init(target)) != ERROR_OK)
 882                                 return err;
 883
 884                         LOG_DEBUG("target->state: %s", target_state_name(target));
 885                 }
 886         }
 887
 888         return ERROR_OK;
 889 }
 890
 891 static int dsp563xx_halt(struct target *target)
 892 {
 893         int err;
 894         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 895
 896         if (target->state == TARGET_HALTED)
 897         {
 898                 LOG_DEBUG("target was already halted");
 899                 return ERROR_OK;
 900         }
 901
 902         if (target->state == TARGET_UNKNOWN)
 903         {
 904                 LOG_WARNING("target was in unknown state when halt was requested");
 905         }
 906
 907         if ((err = dsp563xx_jtag_debug_request(target)) != ERROR_OK)
 908                 return err;
 909
 910         /* store pipeline register */
 911         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPILR, &dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
 912                 return err;
 913         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPDBR, &dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
 914                 return err;
 915
 916         LOG_DEBUG("%s", __FUNCTION__);
 917
 918         return ERROR_OK;
 919 }
 920
 921 static int dsp563xx_resume(struct target *target, int current, uint32_t address, int handle_breakpoints, int debug_execution)
 922 {
 923         int err;
 924         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 925
 926         LOG_DEBUG("%s", __FUNCTION__);
 927
 928         if ((err = dsp563xx_restore_context(target)) != ERROR_OK)
 929                 return err;
 930         register_cache_invalidate(dsp563xx->core_cache);
 931
 932         if (current)
 933         {
 934                 /* restore pipeline registers and go */
 935                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPILR, dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
 936                         return err;
 937                 if ((err =
 938                      dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO,
 939                                              dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
 940                         return err;
 941         }
 942         else
 943         {
 944                 /* set to go register and jump */
 945                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR, INSTR_JUMP)) != ERROR_OK)
 946                         return err;
 947                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_PDBGOTO | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO, address)) != ERROR_OK)
 948                         return err;
 949         }
 950
 951         target->state = TARGET_RUNNING;
 952
 953         return ERROR_OK;
 954 }
 955
 956 static int dsp563xx_step_ex(struct target *target, int current, uint32_t address, int handle_breakpoints, int steps)
 957 {
 958         int err;
 959         uint32_t once_status;
 960         uint32_t dr_in, cnt;
 961         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 962
 963         if (target->state != TARGET_HALTED)
 964         {
 965                 LOG_DEBUG("target was not halted");
 966                 return ERROR_OK;
 967         }
 968
 969         LOG_DEBUG("%s %08X %08X", __FUNCTION__, current, (unsigned) address);
 970
 971         if ((err = dsp563xx_jtag_debug_request(target)) != ERROR_OK)
 972                 return err;
 973         if ((err = dsp563xx_restore_context(target)) != ERROR_OK)
 974                 return err;
 975
 976         /* reset trace mode */
 977         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OSCR, 0x000000)) != ERROR_OK)
 978                 return err;
 979         /* enable trace mode */
 980         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OSCR, DSP563XX_ONCE_OSCR_TME)) != ERROR_OK)
 981                 return err;
 982
 983         cnt = steps;
 984
 985         /* on JUMP we need one extra cycle */
 986         if (!current)
 987                 cnt++;
 988
 989         /* load step counter with N-1 */
 990         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OTC, cnt)) != ERROR_OK)
 991                 return err;
 992
 993         if (current)
 994         {
 995                 /* restore pipeline registers and go */
 996                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPILR, dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
 997                         return err;
 998                 if ((err =
 999                      dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO,
1000                                              dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
1001                         return err;
1002         }
1003         else
1004         {
1005                 /* set to go register and jump */
1006                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR, INSTR_JUMP)) != ERROR_OK)
1007                         return err;
1008                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_PDBGOTO | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO, address)) != ERROR_OK)
1009                         return err;
1010         }
1011
1012         while (1)
1013         {
1014                 if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OSCR, &once_status)) != ERROR_OK)
1015                         return err;
1016
1017                 if (once_status & DSP563XX_ONCE_OSCR_TO)
1018                 {
1019                         /* store pipeline register */
1020                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPILR, &dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
1021                                 return err;
1022                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPDBR, &dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
1023                                 return err;
1024
1025                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABFR, &dr_in)) != ERROR_OK)
1026                                 return err;
1027                         LOG_DEBUG("fetch: %08X", (unsigned) dr_in);
1028                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABDR, &dr_in)) != ERROR_OK)
1029                                 return err;
1030                         LOG_DEBUG("decode: %08X", (unsigned) dr_in);
1031                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABEX, &dr_in)) != ERROR_OK)
1032                                 return err;
1033                         LOG_DEBUG("execute: %08X", (unsigned) dr_in);
1034
1035                         /* reset trace mode */
1036                         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OSCR, 0x000000)) != ERROR_OK)
1037                                 return err;
1038
1039                         register_cache_invalidate(dsp563xx->core_cache);
1040                         if ((err = dsp563xx_debug_init(target)) != ERROR_OK)
1041                                 return err;
1042
1043                         break;
1044                 }
1045         }
1046
1047         return ERROR_OK;
1048 }
1049
1050 static int dsp563xx_step(struct target *target, int current, uint32_t address, int handle_breakpoints)
1051 {
1052         return dsp563xx_step_ex(target, current, address, handle_breakpoints, 0);
1053 }
1054
1055 static int dsp563xx_assert_reset(struct target *target)
1056 {
1057         int retval = 0;
1058         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
1059         enum reset_types jtag_reset_config = jtag_get_reset_config();
1060
1061         if (jtag_reset_config & RESET_HAS_SRST)
1062         {
1063                 /* default to asserting srst */
1064                 if (jtag_reset_config & RESET_SRST_PULLS_TRST)
1065                 {
1066                         jtag_add_reset(1, 1);
1067                 }
1068                 else
1069                 {
1070                         jtag_add_reset(0, 1);
1071                 }
1072         }
1073
1074         target->state = TARGET_RESET;
1075         jtag_add_sleep(5000);
1076
1077         /* registers are now invalid */
1078         register_cache_invalidate(dsp563xx->core_cache);
1079
1080         if (target->reset_halt)
1081         {
1082                 if ((retval = target_halt(target)) != ERROR_OK)
1083                         return retval;
1084         }
1085
1086         LOG_DEBUG("%s", __FUNCTION__);
1087         return ERROR_OK;
1088 }
1089
1090 static int dsp563xx_deassert_reset(struct target *target)
1091 {
1092         int err;
1093
1094         /* deassert reset lines */
1095         jtag_add_reset(0, 0);
1096
1097         if ((err = dsp563xx_poll(target)) != ERROR_OK)
1098                 return err;
1099
1100         if (target->reset_halt)
1101         {
1102                 if (target->state == TARGET_HALTED)
1103                 {
1104                         /* after a reset the cpu jmp to the
1105                          * reset vector and need 2 cycles to fill
1106                          * the cache (fetch,decode,excecute)
1107                          */
1108                         if ((err = dsp563xx_step_ex(target, 1, 0, 1, 1)) != ERROR_OK)
1109                                 return err;
1110                 }
1111         }
1112
1113 //      target->state = TARGET_RUNNING;
1114
1115         LOG_DEBUG("%s", __FUNCTION__);
1116         return ERROR_OK;
1117 }
1118
1119 static int dsp563xx_soft_reset_halt(struct target *target)
1120 {
1121         LOG_DEBUG("%s", __FUNCTION__);
1122         return ERROR_OK;
1123 }
1124
1125 static int dsp563xx_read_memory(struct target *target, int mem_type, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1126 {
1127         int err;
1128         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
1129         uint32_t i, x;
1130         uint32_t data, move_cmd;
1131         uint8_t *b;
1132
1133         LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32, address, size, count);
1134
1135         if (target->state != TARGET_HALTED)
1136         {
1137                 LOG_WARNING("target not halted");
1138                 return ERROR_TARGET_NOT_HALTED;
1139         }
1140
1141         switch (mem_type)
1142         {
1143                 case MEM_X:
1144                         move_cmd = 0x61d800;
1145                         break;
1146                 case MEM_Y:
1147                         move_cmd = 0x69d800;
1148                         break;
1149                 case MEM_P:
1150                         move_cmd = 0x07d891;
1151                         break;
1152                 default:
1153                         return ERROR_INVALID_ARGUMENTS;
1154         }
1155
1156         /* we use r0 to store temporary data */
1157         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
1158                 dsp563xx->read_core_reg(target, REG_NUM_R0);
1159         /* we use r1 to store temporary data */
1160         if (!dsp563xx->core_cache->reg_list[REG_NUM_R1].valid)
1161                 dsp563xx->read_core_reg(target, REG_NUM_R1);
1162
1163         /* r0 is no longer valid on target */
1164         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
1165         /* r1 is no longer valid on target */
1166         dsp563xx->core_cache->reg_list[REG_NUM_R1].dirty = 1;
1167
1168         x = count;
1169         b = buffer;
1170
1171         if ((err = dsp563xx_once_execute_dw_ir(target->tap, 0x60F400, address)) != ERROR_OK)
1172                 return err;
1173
1174         for (i = 0; i < x; i++)
1175         {
1176                 data = 0;
1177                 if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, move_cmd)) != ERROR_OK)
1178                         return err;
1179                 if ((err = dsp563xx_once_execute_sw_ir(target->tap, 0x08D13C)) != ERROR_OK)
1180                         return err;
1181                 if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OGDBR, &data)) != ERROR_OK)
1182                         return err;
1183                 target_buffer_set_u32(target, b, data);
1184                 b += 4;
1185
1186                 LOG_DEBUG("R: %08X", data);
1187         }
1188
1189         return ERROR_OK;
1190 }
1191
1192 static int dsp563xx_read_memory_p(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1193 {
1194         return dsp563xx_read_memory(target, MEM_P, address, size, count, buffer);
1195 }
1196
1197 static int dsp563xx_write_memory(struct target *target, int mem_type, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1198 {
1199         int err;
1200         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
1201         uint32_t i, x;
1202         uint32_t data, move_cmd;
1203         uint8_t *b;
1204
1205         LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "", address, size, count);
1206
1207         if (target->state != TARGET_HALTED)
1208         {
1209                 LOG_WARNING("target not halted");
1210                 return ERROR_TARGET_NOT_HALTED;
1211         }
1212
1213         switch (mem_type)
1214         {
1215                 case MEM_X:
1216                         move_cmd = 0x615800;
1217                         break;
1218                 case MEM_Y:
1219                         move_cmd = 0x695800;
1220                         break;
1221                 case MEM_P:
1222                         move_cmd = 0x075891;
1223                         break;
1224                 default:
1225                         return ERROR_INVALID_ARGUMENTS;
1226         }
1227
1228         /* we use r0 to store temporary data */
1229         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
1230                 dsp563xx->read_core_reg(target, REG_NUM_R0);
1231         /* we use r1 to store temporary data */
1232         if (!dsp563xx->core_cache->reg_list[REG_NUM_R1].valid)
1233                 dsp563xx->read_core_reg(target, REG_NUM_R1);
1234
1235         /* r0 is no longer valid on target */
1236         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
1237         /* r1 is no longer valid on target */
1238         dsp563xx->core_cache->reg_list[REG_NUM_R1].dirty = 1;
1239
1240         x = count;
1241         b = buffer;
1242
1243         if ((err = dsp563xx_once_execute_dw_ir(target->tap, 0x60F400, address)) != ERROR_OK)
1244                 return err;
1245
1246         for (i = 0; i < x; i++)
1247         {
1248                 data = target_buffer_get_u32(target, b);
1249                 data &= 0x00ffffff;
1250
1251                 LOG_DEBUG("W: %08X", data);
1252
1253                 if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, 0x61F400, data)) != ERROR_OK)
1254                         return err;
1255                 if ((err = dsp563xx_once_execute_sw_ir(target->tap, move_cmd)) != ERROR_OK)
1256                         return err;
1257
1258                 b += 4;
1259         }
1260
1261         return ERROR_OK;
1262 }
1263
1264 static int dsp563xx_write_memory_p(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1265 {
1266         return dsp563xx_write_memory(target, MEM_P, address, size, count, buffer);
1267 }
1268
1269 static void handle_md_output(struct command_context *cmd_ctx, struct target *target, uint32_t address, unsigned size, unsigned count, const uint8_t * buffer)
1270 {
1271         const unsigned line_bytecnt = 32;
1272         unsigned line_modulo = line_bytecnt / size;
1273
1274         char output[line_bytecnt * 4 + 1];
1275         unsigned output_len = 0;
1276
1277         const char *value_fmt;
1278         switch (size)
1279         {
1280                 case 4:
1281                         value_fmt = "%8.8x ";
1282                         break;
1283                 case 2:
1284                         value_fmt = "%4.4x ";
1285                         break;
1286                 case 1:
1287                         value_fmt = "%2.2x ";
1288                         break;
1289                 default:
1290                         /* "can't happen", caller checked */
1291                         LOG_ERROR("invalid memory read size: %u", size);
1292                         return;
1293         }
1294
1295         for (unsigned i = 0; i < count; i++)
1296         {
1297                 if (i % line_modulo == 0)
1298                 {
1299                         output_len += snprintf(output + output_len, sizeof(output) - output_len, "0x%8.8x: ", (unsigned) (address + (i * size)));
1300                 }
1301
1302                 uint32_t value = 0;
1303                 const uint8_t *value_ptr = buffer + i * size;
1304                 switch (size)
1305                 {
1306                         case 4:
1307                                 value = target_buffer_get_u32(target, value_ptr);
1308                                 break;
1309                         case 2:
1310                                 value = target_buffer_get_u16(target, value_ptr);
1311                                 break;
1312                         case 1:
1313                                 value = *value_ptr;
1314                 }
1315                 output_len += snprintf(output + output_len, sizeof(output) - output_len, value_fmt, value);
1316
1317                 if ((i % line_modulo == line_modulo - 1) || (i == count - 1))
1318                 {
1319                         command_print(cmd_ctx, "%s", output);
1320                         output_len = 0;
1321                 }
1322         }
1323 }
1324
1325 COMMAND_HANDLER(dsp563xx_mem_command)
1326 {
1327         struct target *target = get_current_target(CMD_CTX);
1328         int err = ERROR_OK;
1329         int read_mem;
1330         uint32_t address = 0;
1331         uint32_t count = 1, i;
1332         uint32_t pattern = 0;
1333         uint32_t mem_type;
1334         uint8_t *buffer, *b;
1335
1336         switch (CMD_NAME[1])
1337         {
1338                 case 'w':
1339                         read_mem = 0;
1340                         break;
1341                 case 'd':
1342                         read_mem = 1;
1343                         break;
1344                 default:
1345                         return ERROR_COMMAND_SYNTAX_ERROR;
1346         }
1347
1348         switch (CMD_NAME[3])
1349         {
1350                 case 'x':
1351                         mem_type = MEM_X;
1352                         break;
1353                 case 'y':
1354                         mem_type = MEM_Y;
1355                         break;
1356                 case 'p':
1357                         mem_type = MEM_P;
1358                         break;
1359                 default:
1360                         return ERROR_COMMAND_SYNTAX_ERROR;
1361         }
1362
1363         if (CMD_ARGC > 0)
1364         {
1365                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
1366         }
1367
1368         if (read_mem == 0)
1369         {
1370                 if (CMD_ARGC < 2)
1371                 {
1372                         return ERROR_COMMAND_SYNTAX_ERROR;
1373                 }
1374                 if (CMD_ARGC > 1)
1375                 {
1376                         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], pattern);
1377                 }
1378                 if (CMD_ARGC > 2)
1379                 {
1380                         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], count);
1381                 }
1382         }
1383
1384         if (read_mem == 1)
1385         {
1386                 if (CMD_ARGC < 1)
1387                 {
1388                         return ERROR_COMMAND_SYNTAX_ERROR;
1389                 }
1390                 if (CMD_ARGC > 1)
1391                 {
1392                         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], count);
1393                 }
1394         }
1395
1396         buffer = calloc(count, sizeof(uint32_t));
1397
1398         if (read_mem == 1)
1399         {
1400                 if ((err = dsp563xx_read_memory(target, mem_type, address, sizeof(uint32_t), count, buffer)) == ERROR_OK)
1401                         handle_md_output(CMD_CTX, target, address, sizeof(uint32_t), count, buffer);
1402         }
1403         else
1404         {
1405                 b = buffer;
1406
1407                 for (i = 0; i < count; i++)
1408                 {
1409                         target_buffer_set_u32(target, b, pattern);
1410                         b += 4;
1411                 }
1412
1413                 err = dsp563xx_write_memory(target, mem_type, address, sizeof(uint32_t), count, buffer);
1414         }
1415
1416         free(buffer);
1417
1418         return err;
1419 }
1420
1421 static const struct command_registration dsp563xx_command_handlers[] = {
1422         {
1423          .name = "mwwx",
1424          .handler = dsp563xx_mem_command,
1425          .mode = COMMAND_EXEC,
1426          .help = "write x memory words",
1427          .usage = "mwwx address value [count]",
1428          },
1429         {
1430          .name = "mwwy",
1431          .handler = dsp563xx_mem_command,
1432          .mode = COMMAND_EXEC,
1433          .help = "write y memory words",
1434          .usage = "mwwy address value [count]",
1435          },
1436         {
1437          .name = "mwwp",
1438          .handler = dsp563xx_mem_command,
1439          .mode = COMMAND_EXEC,
1440          .help = "write p memory words",
1441          .usage = "mwwp address value [count]",
1442          },
1443         {
1444          .name = "mdwx",
1445          .handler = dsp563xx_mem_command,
1446          .mode = COMMAND_EXEC,
1447          .help = "display x memory words",
1448          .usage = "mdwx address [count]",
1449          },
1450         {
1451          .name = "mdwy",
1452          .handler = dsp563xx_mem_command,
1453          .mode = COMMAND_EXEC,
1454          .help = "display y memory words",
1455          .usage = "mdwy address [count]",
1456          },
1457         {
1458          .name = "mdwp",
1459          .handler = dsp563xx_mem_command,
1460          .mode = COMMAND_EXEC,
1461          .help = "display p memory words",
1462          .usage = "mdwp address [count]",
1463          },
1464         COMMAND_REGISTRATION_DONE
1465 };
1466
1467 /** Holds methods for DSP563XX targets. */
1468 struct target_type dsp563xx_target = {
1469         .name = "dsp563xx",
1470
1471         .poll = dsp563xx_poll,
1472         .arch_state = dsp563xx_arch_state,
1473
1474         .target_request_data = NULL,
1475
1476         .get_gdb_reg_list = dsp563xx_get_gdb_reg_list,
1477
1478         .halt = dsp563xx_halt,
1479         .resume = dsp563xx_resume,
1480         .step = dsp563xx_step,
1481
1482         .assert_reset = dsp563xx_assert_reset,
1483         .deassert_reset = dsp563xx_deassert_reset,
1484         .soft_reset_halt = dsp563xx_soft_reset_halt,
1485
1486         .read_memory = dsp563xx_read_memory_p,
1487         .write_memory = dsp563xx_write_memory_p,
1488
1489         .commands = dsp563xx_command_handlers,
1490         .target_create = dsp563xx_target_create,
1491         .init_target = dsp563xx_init_target,
1492 };