git.sur5r.net Git - 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 ASM_REG_W_R0    0x60F400
  33 #define ASM_REG_W_R1    0x61F400
  34 #define ASM_REG_W_R2    0x62F400
  35 #define ASM_REG_W_R3    0x63F400
  36 #define ASM_REG_W_R4    0x64F400
  37 #define ASM_REG_W_R5    0x65F400
  38 #define ASM_REG_W_R6    0x66F400
  39 #define ASM_REG_W_R7    0x67F400
  40
  41 #define ASM_REG_W_N0    0x70F400
  42 #define ASM_REG_W_N1    0x71F400
  43 #define ASM_REG_W_N2    0x72F400
  44 #define ASM_REG_W_N3    0x73F400
  45 #define ASM_REG_W_N4    0x74F400
  46 #define ASM_REG_W_N5    0x75F400
  47 #define ASM_REG_W_N6    0x76F400
  48 #define ASM_REG_W_N7    0x77F400
  49
  50 #define ASM_REG_W_M0    0x05F420
  51 #define ASM_REG_W_M1    0x05F421
  52 #define ASM_REG_W_M2    0x05F422
  53 #define ASM_REG_W_M3    0x05F423
  54 #define ASM_REG_W_M4    0x05F424
  55 #define ASM_REG_W_M5    0x05F425
  56 #define ASM_REG_W_M6    0x05F426
  57 #define ASM_REG_W_M7    0x05F427
  58
  59 #define ASM_REG_W_X0    0x44F400
  60 #define ASM_REG_W_X1    0x45F400
  61
  62 #define ASM_REG_W_Y0    0x46F400
  63 #define ASM_REG_W_Y1    0x47F400
  64
  65 #define ASM_REG_W_A0    0x50F400
  66 #define ASM_REG_W_A1    0x54F400
  67 #define ASM_REG_W_A2    0x52F400
  68
  69 #define ASM_REG_W_B0    0x51F400
  70 #define ASM_REG_W_B1    0x55F400
  71 #define ASM_REG_W_B2    0x53F400
  72
  73 #define ASM_REG_W_VBA   0x05F430
  74 #define ASM_REG_W_OMR   0x05F43A
  75 #define ASM_REG_W_EP    0x05F42A
  76 #define ASM_REG_W_SC    0x05F431
  77 #define ASM_REG_W_SZ    0x05F438
  78 #define ASM_REG_W_SR    0x05F439
  79 #define ASM_REG_W_SP    0x05F43B
  80 #define ASM_REG_W_SSH   0x05F43C
  81 #define ASM_REG_W_SSL   0x05F43D
  82 #define ASM_REG_W_LA    0x05F43E
  83 #define ASM_REG_W_LC    0x05F43F
  84 #define ASM_REG_W_PC    0x000000
  85 #define ASM_REG_W_IPRC  0xFFFFFF
  86 #define ASM_REG_W_IPRP  0xFFFFFE
  87
  88 #define ASM_REG_W_BCR   0xFFFFFB
  89 #define ASM_REG_W_DCR   0xFFFFFA
  90 #define ASM_REG_W_AAR0  0xFFFFF9
  91 #define ASM_REG_W_AAR1  0xFFFFF8
  92 #define ASM_REG_W_AAR2  0xFFFFF7
  93 #define ASM_REG_W_AAR3  0xFFFFF6
  94
  95 static struct once_reg once_regs[] = {
  96         {0, 0x00, 24, "OSCR", 0},
  97         {1, 0x01, 24, "OMBC", 0},
  98         {2, 0x02, 24, "OBCR", 0},
  99         {3, 0x05, 24, "OMLR0", 0},
 100         {4, 0x06, 24, "OMLR1", 0},
 101         {5, 0x09, 24, "OGDBR", 0},
 102         {6, 0x0a, 24, "OPDBR", 0},
 103         {7, 0x0b, 24, "OPILR", 0},
 104         {8, 0x0c, 24, "PDB", 0},
 105         {9, 0x0d, 24, "OTC", 0},
 106         {10, 0x0f, 24, "OPABFR", 0},
 107         {11, 0x10, 24, "OPABDR", 0},
 108         {12, 0x11, 24, "OPABEX", 0},
 109         {13, 0x12, 25, "OPABF0", 0},
 110         {14, 0x12, 25, "OPABF1", 0},
 111         {15, 0x12, 25, "OPABF2", 0},
 112         {16, 0x12, 25, "OPABF3", 0},
 113         {17, 0x12, 25, "OPABF4", 0},
 114         {18, 0x12, 25, "OPABF5", 0},
 115         {19, 0x12, 25, "OPABF6", 0},
 116         {20, 0x12, 25, "OPABF7", 0},
 117         {21, 0x12, 25, "OPABF8", 0},
 118         {22, 0x12, 25, "OPABF9", 0},
 119         {23, 0x12, 25, "OPABF10", 0},
 120         {24, 0x12, 25, "OPABF11", 0},
 121 //      {25,0x1f,24,"NRSEL",0},
 122 };
 123
 124 static const struct
 125 {
 126         unsigned id;
 127         const char *name;
 128         unsigned bits;
 129         /* effective addressing mode encoding */
 130         uint8_t eame;
 131         uint32_t instr_mask;
 132 } dsp563xx_regs[] =
 133 {
 134         /* *INDENT-OFF* */
 135         /* address registers */
 136         { 0, "r0", 24, 0x10, ASM_REG_W_R0},
 137         { 1, "r1", 24, 0x11, ASM_REG_W_R1},
 138         { 2, "r2", 24, 0x12, ASM_REG_W_R2},
 139         { 3, "r3", 24, 0x13, ASM_REG_W_R3},
 140         { 4, "r4", 24, 0x14, ASM_REG_W_R4},
 141         { 5, "r5", 24, 0x15, ASM_REG_W_R5},
 142         { 6, "r6", 24, 0x16, ASM_REG_W_R6},
 143         { 7, "r7", 24, 0x17, ASM_REG_W_R7},
 144         /* offset registers */
 145         { 8, "n0", 24, 0x18, ASM_REG_W_N0},
 146         { 9, "n1", 24, 0x19, ASM_REG_W_N1},
 147         {10, "n2", 24, 0x1a, ASM_REG_W_N2},
 148         {11, "n3", 24, 0x1b, ASM_REG_W_N3},
 149         {12, "n4", 24, 0x1c, ASM_REG_W_N4},
 150         {13, "n5", 24, 0x1d, ASM_REG_W_N5},
 151         {14, "n6", 24, 0x1e, ASM_REG_W_N6},
 152         {15, "n7", 24, 0x1f, ASM_REG_W_N7},
 153         /* modifier registers */
 154         {16, "m0", 24, 0x20, ASM_REG_W_M0},
 155         {17, "m1", 24, 0x21, ASM_REG_W_M1},
 156         {18, "m2", 24, 0x22, ASM_REG_W_M2},
 157         {19, "m3", 24, 0x23, ASM_REG_W_M3},
 158         {20, "m4", 24, 0x24, ASM_REG_W_M4},
 159         {21, "m5", 24, 0x25, ASM_REG_W_M5},
 160         {22, "m6", 24, 0x26, ASM_REG_W_M6},
 161         {23, "m7", 24, 0x27, ASM_REG_W_M7},
 162         /* data alu input register */
 163         {24, "x0", 24, 0x04, ASM_REG_W_X0},
 164         {25, "x1", 24, 0x05, ASM_REG_W_X1},
 165         {26, "y0", 24, 0x06, ASM_REG_W_Y0},
 166         {27, "y1", 24, 0x07, ASM_REG_W_Y1},
 167         /* data alu accumulator register */
 168         {28, "a0", 24, 0x08, ASM_REG_W_A0},
 169         {29, "a1", 24, 0x0c, ASM_REG_W_A1},
 170         {30, "a2",  8, 0x0a, ASM_REG_W_A2},
 171         {31, "b0", 24, 0x09, ASM_REG_W_B0},
 172         {32, "b1", 24, 0x0d, ASM_REG_W_B1},
 173         {33, "b2",  8, 0x0b, ASM_REG_W_B2},
 174         /* stack */
 175         {34, "ssh",24, 0x3c, ASM_REG_W_SSH},
 176         {35, "ssl",24, 0x3d, ASM_REG_W_SSL},
 177         {36, "sp", 24, 0x3b, ASM_REG_W_SP},
 178         {37, "ep", 24, 0x2a, ASM_REG_W_EP},
 179         {38, "sz", 24, 0x38, ASM_REG_W_SZ},
 180         {39, "sc", 24, 0x31, ASM_REG_W_SC},
 181         /* system */
 182         {40, "pc", 24, 0x00, ASM_REG_W_PC},
 183         {41, "sr", 24, 0x39, ASM_REG_W_SR},
 184         {42, "omr",24, 0x3a, ASM_REG_W_OMR},
 185         {43, "la", 24, 0x3e, ASM_REG_W_LA},
 186         {44, "lc", 24, 0x3f, ASM_REG_W_LC},
 187         /* interrupt */
 188         {45, "vba", 24, 0x30, ASM_REG_W_VBA},
 189         {46, "iprc",24, 0x00, ASM_REG_W_IPRC},
 190         {47, "iprp",24, 0x00, ASM_REG_W_IPRP},
 191         /* port a */
 192         {48, "bcr", 24, 0x00, ASM_REG_W_BCR},
 193         {49, "dcr", 24, 0x00, ASM_REG_W_DCR},
 194         {50, "aar0",24, 0x00, ASM_REG_W_AAR0},
 195         {51, "aar1",24, 0x00, ASM_REG_W_AAR1},
 196         {52, "aar2",24, 0x00, ASM_REG_W_AAR2},
 197         {53, "aar3",24, 0x00, ASM_REG_W_AAR3},
 198         /* *INDENT-ON* */
 199 };
 200
 201 #define REG_NUM_R0      0
 202 #define REG_NUM_R1      1
 203 #define REG_NUM_N0      8
 204 #define REG_NUM_N1      9
 205 #define REG_NUM_M0      16
 206 #define REG_NUM_M1      17
 207 #define REG_NUM_SSH     34
 208 #define REG_NUM_SSL     35
 209 #define REG_NUM_SP      36
 210 #define REG_NUM_EP      37
 211 #define REG_NUM_SC      39
 212 #define REG_NUM_PC      40
 213 #define REG_NUM_SR      41
 214 #define REG_NUM_IPRC    46
 215 #define REG_NUM_IPRP    47
 216 #define REG_NUM_BCR     48
 217 #define REG_NUM_DCR     49
 218 #define REG_NUM_AAR0    50
 219 #define REG_NUM_AAR1    51
 220 #define REG_NUM_AAR2    52
 221 #define REG_NUM_AAR3    53
 222
 223 enum memory_type
 224 {
 225         MEM_X = 0,
 226         MEM_Y = 1,
 227         MEM_P = 2,
 228 };
 229
 230 #define INSTR_JUMP      0x0AF080
 231 /* Effective Addressing Mode Encoding */
 232 #define EAME_R0         0x10
 233 /* instrcution encoder */
 234 /* movep
 235  * s - peripheral space X/Y (X=0,Y=1)
 236  * w - write/read
 237  * d - source/destination register
 238  * p - IO short address
 239  */
 240 #define INSTR_MOVEP_REG_HIO(s,w,d,p)   (0x084000 | ((s & 1)<<16) | ((w&1)<<15) | ((d & 0x3f)<<8) | (p & 0x3f))
 241
 242 static int dsp563xx_get_gdb_reg_list(struct target *target, struct reg **reg_list[], int *reg_list_size)
 243 {
 244         int i;
 245         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 246
 247         if (target->state != TARGET_HALTED)
 248         {
 249                 return ERROR_TARGET_NOT_HALTED;
 250         }
 251
 252         *reg_list_size = DSP563XX_NUMCOREREGS;
 253         *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
 254
 255         if (!*reg_list)
 256                 return ERROR_INVALID_ARGUMENTS;
 257
 258         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 259         {
 260                 (*reg_list)[i] = &dsp563xx->core_cache->reg_list[i];
 261         }
 262
 263         return ERROR_OK;
 264
 265 }
 266
 267 static int dsp563xx_read_core_reg(struct target *target, int num)
 268 {
 269         uint32_t reg_value;
 270         struct dsp563xx_core_reg *dsp563xx_core_reg;
 271         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 272
 273         if ((num < 0) || (num >= DSP563XX_NUMCOREREGS))
 274                 return ERROR_INVALID_ARGUMENTS;
 275
 276         dsp563xx_core_reg = dsp563xx->core_cache->reg_list[num].arch_info;
 277         reg_value = dsp563xx->core_regs[num];
 278         buf_set_u32(dsp563xx->core_cache->reg_list[num].value, 0, 32, reg_value);
 279         dsp563xx->core_cache->reg_list[num].valid = 1;
 280         dsp563xx->core_cache->reg_list[num].dirty = 0;
 281
 282         return ERROR_OK;
 283 }
 284
 285 static int dsp563xx_write_core_reg(struct target *target, int num)
 286 {
 287         uint32_t reg_value;
 288         struct dsp563xx_core_reg *dsp563xx_core_reg;
 289         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 290
 291         if ((num < 0) || (num >= DSP563XX_NUMCOREREGS))
 292                 return ERROR_INVALID_ARGUMENTS;
 293
 294         reg_value = buf_get_u32(dsp563xx->core_cache->reg_list[num].value, 0, 32);
 295         dsp563xx_core_reg = dsp563xx->core_cache->reg_list[num].arch_info;
 296         dsp563xx->core_regs[num] = reg_value;
 297         dsp563xx->core_cache->reg_list[num].valid = 1;
 298         dsp563xx->core_cache->reg_list[num].dirty = 0;
 299
 300         return ERROR_OK;
 301 }
 302
 303 static int dsp563xx_target_create(struct target *target, Jim_Interp * interp)
 304 {
 305         struct dsp563xx_common *dsp563xx = calloc(1, sizeof(struct dsp563xx_common));
 306
 307         if (!dsp563xx)
 308                 return ERROR_INVALID_ARGUMENTS;
 309
 310         dsp563xx->jtag_info.tap = target->tap;
 311         target->arch_info = dsp563xx;
 312         dsp563xx->read_core_reg = dsp563xx_read_core_reg;
 313         dsp563xx->write_core_reg = dsp563xx_write_core_reg;
 314
 315         return ERROR_OK;
 316 }
 317
 318 static int dsp563xx_get_core_reg(struct reg *reg)
 319 {
 320         struct dsp563xx_core_reg *dsp563xx_reg = reg->arch_info;
 321         struct target *target = dsp563xx_reg->target;
 322         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 323
 324         LOG_DEBUG("%s", __FUNCTION__);
 325
 326         if (target->state != TARGET_HALTED)
 327         {
 328                 return ERROR_TARGET_NOT_HALTED;
 329         }
 330
 331         return dsp563xx->read_core_reg(target, dsp563xx_reg->num);
 332 }
 333
 334 static int dsp563xx_set_core_reg(struct reg *reg, uint8_t * buf)
 335 {
 336         LOG_DEBUG("%s", __FUNCTION__);
 337
 338         struct dsp563xx_core_reg *dsp563xx_reg = reg->arch_info;
 339         struct target *target = dsp563xx_reg->target;
 340         uint32_t value = buf_get_u32(buf, 0, 32);
 341
 342         if (target->state != TARGET_HALTED)
 343         {
 344                 return ERROR_TARGET_NOT_HALTED;
 345         }
 346
 347         buf_set_u32(reg->value, 0, reg->size, value);
 348         reg->dirty = 1;
 349         reg->valid = 1;
 350
 351         return ERROR_OK;
 352 }
 353
 354 static int dsp563xx_read_register(struct target *target, int num, int force);
 355 static int dsp563xx_write_register(struct target *target, int num, int force);
 356
 357 static int dsp563xx_reg_read_high_io(struct target *target, uint32_t instr_mask, uint32_t * data)
 358 {
 359         int err;
 360         uint32_t instr;
 361         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 362
 363         /* we use r0 to store temporary data */
 364         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
 365                 dsp563xx->read_core_reg(target, REG_NUM_R0);
 366
 367         /* move source memory to r0 */
 368         instr = INSTR_MOVEP_REG_HIO(MEM_X, 0, EAME_R0, instr_mask);
 369         if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, instr)) != ERROR_OK)
 370                 return err;
 371         /* move r0 to debug register */
 372         instr = INSTR_MOVEP_REG_HIO(MEM_X, 1, EAME_R0, 0xfffffc);
 373         if ((err = dsp563xx_once_execute_sw_ir(target->tap, instr)) != ERROR_OK)
 374                 return err;
 375         /* read debug register */
 376         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OGDBR, data)) != ERROR_OK)
 377                 return err;
 378         /* r0 is no longer valid on target */
 379         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
 380
 381         return ERROR_OK;
 382 }
 383
 384 static int dsp563xx_reg_write_high_io(struct target *target, uint32_t instr_mask, uint32_t data)
 385 {
 386         int err;
 387         uint32_t instr;
 388         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 389
 390         /* we use r0 to store temporary data */
 391         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
 392                 dsp563xx->read_core_reg(target, REG_NUM_R0);
 393
 394         /* move data to r0 */
 395         if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, 0x60F400, data)) != ERROR_OK)
 396                 return err;
 397         /* move r0 to destination memory */
 398         instr = INSTR_MOVEP_REG_HIO(MEM_X, 1, EAME_R0, instr_mask);
 399         if ((err = dsp563xx_once_execute_sw_ir(target->tap, instr)) != ERROR_OK)
 400                 return err;
 401
 402         /* r0 is no longer valid on target */
 403         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
 404
 405         return ERROR_OK;
 406 }
 407
 408 static int dsp563xx_reg_read(struct target *target, uint32_t eame, uint32_t * data)
 409 {
 410         int err;
 411         uint32_t instr;
 412
 413         instr = INSTR_MOVEP_REG_HIO(MEM_X, 1, eame, 0xfffffc);
 414         if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, instr)) != ERROR_OK)
 415                 return err;
 416         /* nop */
 417         if ((err = dsp563xx_once_execute_sw_ir(target->tap, 0x000000)) != ERROR_OK)
 418                 return err;
 419         /* read debug register */
 420         return dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OGDBR, data);
 421 }
 422
 423 static int dsp563xx_reg_write(struct target *target, uint32_t instr_mask, uint32_t data)
 424 {
 425         int err;
 426
 427         if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, instr_mask, data)) != ERROR_OK)
 428                 return err;
 429         /* nop */
 430         return dsp563xx_once_execute_sw_ir(target->tap, 0x000000);
 431 }
 432
 433 static int dsp563xx_reg_pc_read(struct target *target)
 434 {
 435         int err;
 436         uint32_t opabdr, opabex;
 437         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 438
 439         /* pc was changed, nothing todo */
 440         if (dsp563xx->core_cache->reg_list[REG_NUM_PC].dirty)
 441                 return ERROR_OK;
 442
 443         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABDR, &opabdr)) != ERROR_OK)
 444                 return err;
 445         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABEX, &opabex)) != ERROR_OK)
 446                 return err;
 447
 448         /* conditional branch check */
 449         if (opabdr == opabex)
 450         {
 451                 /* TODO: check the trace buffer and if a
 452                  * conditional branch is detected then decode
 453                  * the branch command and add the relative
 454                  * address to the current pc
 455                  */
 456                 LOG_DEBUG("%s conditional branch not supported yet", __FUNCTION__);
 457         }
 458         else
 459         {
 460                 dsp563xx->core_regs[REG_NUM_PC] = opabex;
 461                 dsp563xx->read_core_reg(target, REG_NUM_PC);
 462         }
 463
 464         return ERROR_OK;
 465 }
 466
 467 static int dsp563xx_reg_ssh_read(struct target *target)
 468 {
 469         int err;
 470         uint32_t sp, sc, ep;
 471         struct dsp563xx_core_reg *arch_info;
 472         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 473
 474         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSH].arch_info;
 475
 476         /* get a valid stack pointer */
 477         if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 478                 return err;
 479         sp = dsp563xx->core_regs[REG_NUM_SP];
 480         if ((err = dsp563xx_write_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 481                 return err;
 482
 483         /* get a valid stack count */
 484         if ((err = dsp563xx_read_register(target, REG_NUM_SC, 0)) != ERROR_OK)
 485                 return err;
 486         sc = dsp563xx->core_regs[REG_NUM_SC];
 487         if ((err = dsp563xx_write_register(target, REG_NUM_SC, 0)) != ERROR_OK)
 488                 return err;
 489
 490         /* get a valid extended pointer */
 491         if ((err = dsp563xx_read_register(target, REG_NUM_EP, 0)) != ERROR_OK)
 492                 return err;
 493         ep = dsp563xx->core_regs[REG_NUM_EP];
 494         if ((err = dsp563xx_write_register(target, REG_NUM_EP, 0)) != ERROR_OK)
 495                 return err;
 496
 497         if (!sp)
 498         {
 499                 sp = 0x00FFFFFF;
 500         }
 501         else
 502         {
 503                 if ((err = dsp563xx_reg_read(target, arch_info->eame, &sp)) != ERROR_OK)
 504                         return err;
 505
 506                 if ((err = dsp563xx_write_register(target, REG_NUM_SC, 1)) != ERROR_OK)
 507                         return err;
 508                 if ((err = dsp563xx_write_register(target, REG_NUM_SP, 1)) != ERROR_OK)
 509                         return err;
 510                 if ((err = dsp563xx_write_register(target, REG_NUM_EP, 1)) != ERROR_OK)
 511                         return err;
 512         }
 513
 514         dsp563xx->core_regs[REG_NUM_SSH] = sp;
 515         dsp563xx->read_core_reg(target, REG_NUM_SSH);
 516
 517         return ERROR_OK;
 518 }
 519
 520 static int dsp563xx_reg_ssh_write(struct target *target)
 521 {
 522         int err;
 523         uint32_t sp;
 524         struct dsp563xx_core_reg *arch_info;
 525         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 526
 527         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSH].arch_info;
 528
 529         /* get a valid stack pointer */
 530         if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 531                 return err;
 532         sp = dsp563xx->core_regs[REG_NUM_SP];
 533
 534         if (sp)
 535         {
 536                 sp--;
 537                 /* write new stackpointer */
 538                 dsp563xx->core_regs[REG_NUM_SP] = sp;
 539                 if ((err = dsp563xx->read_core_reg(target, REG_NUM_SP)) != ERROR_OK)
 540                         return err;
 541                 if ((err = dsp563xx_write_register(target, REG_NUM_SP, 1)) != ERROR_OK)
 542                         return err;
 543
 544                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, dsp563xx->core_regs[REG_NUM_SSH])) != ERROR_OK)
 545                         return err;
 546
 547                 if ((err = dsp563xx_read_register(target, REG_NUM_SP, 1)) != ERROR_OK)
 548                         return err;
 549                 if ((err = dsp563xx_read_register(target, REG_NUM_SSH, 1)) != ERROR_OK)
 550                         return err;
 551         }
 552
 553         return ERROR_OK;
 554 }
 555
 556 static int dsp563xx_reg_ssl_read(struct target *target)
 557 {
 558         int err;
 559         uint32_t sp;
 560         struct dsp563xx_core_reg *arch_info;
 561         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 562
 563         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SSL].arch_info;
 564
 565         /* get a valid stack pointer */
 566         if ((err = dsp563xx_read_register(target, REG_NUM_SP, 0)) != ERROR_OK)
 567                 return err;
 568         sp = dsp563xx->core_regs[REG_NUM_SP];
 569
 570         if (!sp)
 571         {
 572                 sp = 0x00FFFFFF;
 573         }
 574         else
 575         {
 576                 if ((err = dsp563xx_reg_read(target, arch_info->eame, &sp)) != ERROR_OK)
 577                         return err;
 578         }
 579
 580         dsp563xx->core_regs[REG_NUM_SSL] = sp;
 581         dsp563xx->read_core_reg(target, REG_NUM_SSL);
 582
 583         return ERROR_OK;
 584 }
 585
 586 static int dsp563xx_read_register(struct target *target, int num, int force)
 587 {
 588         int err = ERROR_OK;
 589         uint32_t data = 0;
 590         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 591         struct dsp563xx_core_reg *arch_info;
 592
 593         if (force)
 594                 dsp563xx->core_cache->reg_list[num].valid = 0;
 595
 596         if (!dsp563xx->core_cache->reg_list[num].valid)
 597         {
 598                 arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
 599
 600                 switch (arch_info->num)
 601                 {
 602                         case REG_NUM_SSH:
 603                                 err = dsp563xx_reg_ssh_read(target);
 604                                 break;
 605                         case REG_NUM_SSL:
 606                                 err = dsp563xx_reg_ssl_read(target);
 607                                 break;
 608                         case REG_NUM_PC:
 609                                 err = dsp563xx_reg_pc_read(target);
 610                                 break;
 611                         case REG_NUM_IPRC:
 612                         case REG_NUM_IPRP:
 613                         case REG_NUM_BCR:
 614                         case REG_NUM_DCR:
 615                         case REG_NUM_AAR0:
 616                         case REG_NUM_AAR1:
 617                         case REG_NUM_AAR2:
 618                         case REG_NUM_AAR3:
 619                                 err = dsp563xx_reg_read_high_io(target, arch_info->instr_mask, &data);
 620                                 if (err == ERROR_OK)
 621                                 {
 622                                         dsp563xx->core_regs[num] = data;
 623                                         dsp563xx->read_core_reg(target, num);
 624                                 }
 625                                 break;
 626                         default:
 627                                 err = dsp563xx_reg_read(target, arch_info->eame, &data);
 628                                 if (err == ERROR_OK)
 629                                 {
 630                                         dsp563xx->core_regs[num] = data;
 631                                         dsp563xx->read_core_reg(target, num);
 632                                 }
 633                                 break;
 634                 }
 635
 636         }
 637
 638         return err;
 639 }
 640
 641 static int dsp563xx_write_register(struct target *target, int num, int force)
 642 {
 643         int err = ERROR_OK;
 644         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 645         struct dsp563xx_core_reg *arch_info;
 646
 647         if (force)
 648                 dsp563xx->core_cache->reg_list[num].dirty = 1;
 649
 650         if (dsp563xx->core_cache->reg_list[num].dirty)
 651         {
 652                 arch_info = dsp563xx->core_cache->reg_list[num].arch_info;
 653
 654                 dsp563xx->write_core_reg(target, num);
 655
 656                 switch (arch_info->num)
 657                 {
 658                         case REG_NUM_SSH:
 659                                 err = dsp563xx_reg_ssh_write(target);
 660                                 break;
 661                         case REG_NUM_PC:
 662                                 /* pc is updated on resume, no need to write it here */
 663                                 break;
 664                         case REG_NUM_IPRC:
 665                         case REG_NUM_IPRP:
 666                         case REG_NUM_BCR:
 667                         case REG_NUM_DCR:
 668                         case REG_NUM_AAR0:
 669                         case REG_NUM_AAR1:
 670                         case REG_NUM_AAR2:
 671                         case REG_NUM_AAR3:
 672                                 err = dsp563xx_reg_write_high_io(target, arch_info->instr_mask, dsp563xx->core_regs[num]);
 673                                 break;
 674                         default:
 675                                 err = dsp563xx_reg_write(target, arch_info->instr_mask, dsp563xx->core_regs[num]);
 676
 677                                 if ((err == ERROR_OK) && (arch_info->num == REG_NUM_SP))
 678                                 {
 679                                         dsp563xx->core_cache->reg_list[REG_NUM_SSH].valid = 0;
 680                                         dsp563xx->core_cache->reg_list[REG_NUM_SSL].valid = 0;
 681                                 }
 682
 683                                 break;
 684                 }
 685         }
 686
 687         return err;
 688 }
 689
 690 static int dsp563xx_save_context(struct target *target)
 691 {
 692         int i, err = ERROR_OK;
 693
 694         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 695         {
 696                 if ((err = dsp563xx_read_register(target, i, 0)) != ERROR_OK)
 697                         break;
 698         }
 699
 700         return err;
 701 }
 702
 703 static int dsp563xx_restore_context(struct target *target)
 704 {
 705         int i, err = ERROR_OK;
 706
 707         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 708         {
 709                 if ((err = dsp563xx_write_register(target, i, 0)) != ERROR_OK)
 710                         break;
 711         }
 712
 713         return err;
 714 }
 715
 716 static const struct reg_arch_type dsp563xx_reg_type = {
 717         .get = dsp563xx_get_core_reg,
 718         .set = dsp563xx_set_core_reg,
 719 };
 720
 721 static int dsp563xx_init_target(struct command_context *cmd_ctx, struct target *target)
 722 {
 723         /* get pointers to arch-specific information */
 724         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 725
 726         struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
 727         struct reg_cache *cache = malloc(sizeof(struct reg_cache));
 728         struct reg *reg_list = malloc(sizeof(struct reg) * DSP563XX_NUMCOREREGS);
 729         struct dsp563xx_core_reg *arch_info = malloc(sizeof(struct dsp563xx_core_reg) * DSP563XX_NUMCOREREGS);
 730         int i;
 731
 732         LOG_DEBUG("%s", __FUNCTION__);
 733
 734         /* Build the process context cache */
 735         cache->name = "dsp563xx registers";
 736         cache->next = NULL;
 737         cache->reg_list = reg_list;
 738         cache->num_regs = DSP563XX_NUMCOREREGS;
 739         (*cache_p) = cache;
 740         dsp563xx->core_cache = cache;
 741
 742         for (i = 0; i < DSP563XX_NUMCOREREGS; i++)
 743         {
 744                 arch_info[i].num = dsp563xx_regs[i].id;
 745                 arch_info[i].name = dsp563xx_regs[i].name;
 746                 arch_info[i].size = dsp563xx_regs[i].bits;
 747                 arch_info[i].eame = dsp563xx_regs[i].eame;
 748                 arch_info[i].instr_mask = dsp563xx_regs[i].instr_mask;
 749                 arch_info[i].target = target;
 750                 arch_info[i].dsp563xx_common = dsp563xx;
 751                 reg_list[i].name = dsp563xx_regs[i].name;
 752                 reg_list[i].size = dsp563xx_regs[i].bits;
 753                 reg_list[i].value = calloc(1, 4);
 754                 reg_list[i].dirty = 0;
 755                 reg_list[i].valid = 0;
 756                 reg_list[i].type = &dsp563xx_reg_type;
 757                 reg_list[i].arch_info = &arch_info[i];
 758         }
 759
 760         return ERROR_OK;
 761 }
 762
 763 static int dsp563xx_arch_state(struct target *target)
 764 {
 765         LOG_DEBUG("%s", __FUNCTION__);
 766         return ERROR_OK;
 767 }
 768
 769 #define DSP563XX_SR_SA  (1<<17)
 770 #define DSP563XX_SR_SC  (1<<13)
 771
 772 static int dsp563xx_debug_once_init(struct target *target)
 773 {
 774         return dsp563xx_once_read_register(target->tap, once_regs, DSP563XX_NUMONCEREGS);
 775 }
 776
 777 static int dsp563xx_debug_init(struct target *target)
 778 {
 779         int err;
 780         uint32_t sr;
 781         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 782         struct dsp563xx_core_reg *arch_info;
 783
 784         if ((err = dsp563xx_debug_once_init(target)) != ERROR_OK)
 785                 return err;
 786
 787         arch_info = dsp563xx->core_cache->reg_list[REG_NUM_SR].arch_info;
 788
 789         /* check 24bit mode */
 790         if ((err = dsp563xx_read_register(target, REG_NUM_SR, 0)) != ERROR_OK)
 791                 return err;
 792
 793         sr = dsp563xx->core_regs[REG_NUM_SR];
 794
 795         if (sr & (DSP563XX_SR_SA | DSP563XX_SR_SC))
 796         {
 797                 sr &= ~(DSP563XX_SR_SA | DSP563XX_SR_SC);
 798
 799                 if ((err = dsp563xx_once_execute_dw_ir(target->tap, arch_info->instr_mask, sr)) != ERROR_OK)
 800                         return err;
 801                 dsp563xx->core_cache->reg_list[REG_NUM_SR].dirty = 1;
 802         }
 803
 804         if ((err = dsp563xx_read_register(target, REG_NUM_N0, 0)) != ERROR_OK)
 805                 return err;
 806         if ((err = dsp563xx_read_register(target, REG_NUM_N1, 0)) != ERROR_OK)
 807                 return err;
 808         if ((err = dsp563xx_read_register(target, REG_NUM_M0, 0)) != ERROR_OK)
 809                 return err;
 810         if ((err = dsp563xx_read_register(target, REG_NUM_M1, 0)) != ERROR_OK)
 811                 return err;
 812
 813         if (dsp563xx->core_regs[REG_NUM_N0] != 0x000000)
 814         {
 815                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_N0].arch_info;
 816                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0x000000)) != ERROR_OK)
 817                         return err;
 818         }
 819         dsp563xx->core_cache->reg_list[REG_NUM_N0].dirty = 1;
 820
 821         if (dsp563xx->core_regs[REG_NUM_N1] != 0x000000)
 822         {
 823                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_N1].arch_info;
 824                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0x000000)) != ERROR_OK)
 825                         return err;
 826         }
 827         dsp563xx->core_cache->reg_list[REG_NUM_N1].dirty = 1;
 828
 829         if (dsp563xx->core_regs[REG_NUM_M0] != 0xffffff)
 830         {
 831                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_M0].arch_info;
 832                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0xffffff)) != ERROR_OK)
 833                         return err;
 834         }
 835         dsp563xx->core_cache->reg_list[REG_NUM_M0].dirty = 1;
 836
 837         if (dsp563xx->core_regs[REG_NUM_M1] != 0xffffff)
 838         {
 839                 arch_info = dsp563xx->core_cache->reg_list[REG_NUM_M1].arch_info;
 840                 if ((err = dsp563xx_reg_write(target, arch_info->instr_mask, 0xffffff)) != ERROR_OK)
 841                         return err;
 842         }
 843         dsp563xx->core_cache->reg_list[REG_NUM_M1].dirty = 1;
 844
 845         if ((err = dsp563xx_save_context(target)) != ERROR_OK)
 846                 return err;
 847
 848         return ERROR_OK;
 849 }
 850
 851 static int dsp563xx_jtag_debug_request(struct target *target)
 852 {
 853         return dsp563xx_once_request_debug(target->tap, target->state == TARGET_RESET);
 854 }
 855
 856 static int dsp563xx_poll(struct target *target)
 857 {
 858         int err;
 859         uint32_t once_status;
 860         int state;
 861
 862         state = dsp563xx_once_target_status(target->tap);
 863
 864         if (state == TARGET_UNKNOWN)
 865         {
 866                 target->state = state;
 867                 LOG_ERROR("jtag status contains invalid mode value - communication failure");
 868                 return ERROR_TARGET_FAILURE;
 869         }
 870
 871         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OSCR, &once_status)) != ERROR_OK)
 872                 return err;
 873
 874         if ((once_status & DSP563XX_ONCE_OSCR_DEBUG_M) == DSP563XX_ONCE_OSCR_DEBUG_M)
 875         {
 876                 if (target->state != TARGET_HALTED)
 877                 {
 878                         target->state = TARGET_HALTED;
 879                         if ((err = dsp563xx_debug_init(target)) != ERROR_OK)
 880                                 return err;
 881
 882                         LOG_DEBUG("target->state: %s", target_state_name(target));
 883                 }
 884         }
 885
 886         return ERROR_OK;
 887 }
 888
 889 static int dsp563xx_halt(struct target *target)
 890 {
 891         int err;
 892         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 893
 894         if (target->state == TARGET_HALTED)
 895         {
 896                 LOG_DEBUG("target was already halted");
 897                 return ERROR_OK;
 898         }
 899
 900         if (target->state == TARGET_UNKNOWN)
 901         {
 902                 LOG_WARNING("target was in unknown state when halt was requested");
 903         }
 904
 905         if ((err = dsp563xx_jtag_debug_request(target)) != ERROR_OK)
 906                 return err;
 907
 908         /* store pipeline register */
 909         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPILR, &dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
 910                 return err;
 911         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPDBR, &dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
 912                 return err;
 913
 914         LOG_DEBUG("%s", __FUNCTION__);
 915
 916         return ERROR_OK;
 917 }
 918
 919 static int dsp563xx_resume(struct target *target, int current, uint32_t address, int handle_breakpoints, int debug_execution)
 920 {
 921         int err;
 922         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 923
 924         LOG_DEBUG("%s", __FUNCTION__);
 925
 926         if ((err = dsp563xx_restore_context(target)) != ERROR_OK)
 927                 return err;
 928         register_cache_invalidate(dsp563xx->core_cache);
 929
 930         if (current)
 931         {
 932                 /* restore pipeline registers and go */
 933                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPILR, dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
 934                         return err;
 935                 if ((err =
 936                      dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO,
 937                                              dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
 938                         return err;
 939         }
 940         else
 941         {
 942                 /* set to go register and jump */
 943                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR, INSTR_JUMP)) != ERROR_OK)
 944                         return err;
 945                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_PDBGOTO | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO, address)) != ERROR_OK)
 946                         return err;
 947         }
 948
 949         target->state = TARGET_RUNNING;
 950
 951         return ERROR_OK;
 952 }
 953
 954 static int dsp563xx_step_ex(struct target *target, int current, uint32_t address, int handle_breakpoints, int steps)
 955 {
 956         int err;
 957         uint32_t once_status;
 958         uint32_t dr_in, cnt;
 959         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 960
 961         if (target->state != TARGET_HALTED)
 962         {
 963                 LOG_DEBUG("target was not halted");
 964                 return ERROR_OK;
 965         }
 966
 967         LOG_DEBUG("%s %08X %08X", __FUNCTION__, current, (unsigned) address);
 968
 969         if ((err = dsp563xx_jtag_debug_request(target)) != ERROR_OK)
 970                 return err;
 971         if ((err = dsp563xx_restore_context(target)) != ERROR_OK)
 972                 return err;
 973
 974         /* reset trace mode */
 975         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OSCR, 0x000000)) != ERROR_OK)
 976                 return err;
 977         /* enable trace mode */
 978         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OSCR, DSP563XX_ONCE_OSCR_TME)) != ERROR_OK)
 979                 return err;
 980
 981         cnt = steps;
 982
 983         /* on JUMP we need one extra cycle */
 984         if (!current)
 985                 cnt++;
 986
 987         /* load step counter with N-1 */
 988         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OTC, cnt)) != ERROR_OK)
 989                 return err;
 990
 991         if (current)
 992         {
 993                 /* restore pipeline registers and go */
 994                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPILR, dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
 995                         return err;
 996                 if ((err =
 997                      dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO,
 998                                              dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
 999                         return err;
1000         }
1001         else
1002         {
1003                 /* set to go register and jump */
1004                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OPDBR, INSTR_JUMP)) != ERROR_OK)
1005                         return err;
1006                 if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_PDBGOTO | DSP563XX_ONCE_OCR_EX | DSP563XX_ONCE_OCR_GO, address)) != ERROR_OK)
1007                         return err;
1008         }
1009
1010         while (1)
1011         {
1012                 if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OSCR, &once_status)) != ERROR_OK)
1013                         return err;
1014
1015                 if (once_status & DSP563XX_ONCE_OSCR_TO)
1016                 {
1017                         /* store pipeline register */
1018                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPILR, &dsp563xx->pipeline_context.once_opilr)) != ERROR_OK)
1019                                 return err;
1020                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPDBR, &dsp563xx->pipeline_context.once_opdbr)) != ERROR_OK)
1021                                 return err;
1022
1023                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABFR, &dr_in)) != ERROR_OK)
1024                                 return err;
1025                         LOG_DEBUG("fetch: %08X", (unsigned) dr_in);
1026                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABDR, &dr_in)) != ERROR_OK)
1027                                 return err;
1028                         LOG_DEBUG("decode: %08X", (unsigned) dr_in);
1029                         if ((err = dsp563xx_once_reg_read(target->tap, DSP563XX_ONCE_OPABEX, &dr_in)) != ERROR_OK)
1030                                 return err;
1031                         LOG_DEBUG("execute: %08X", (unsigned) dr_in);
1032
1033                         /* reset trace mode */
1034                         if ((err = dsp563xx_once_reg_write(target->tap, DSP563XX_ONCE_OSCR, 0x000000)) != ERROR_OK)
1035                                 return err;
1036
1037                         register_cache_invalidate(dsp563xx->core_cache);
1038                         if ((err = dsp563xx_debug_init(target)) != ERROR_OK)
1039                                 return err;
1040
1041                         break;
1042                 }
1043         }
1044
1045         return ERROR_OK;
1046 }
1047
1048 static int dsp563xx_step(struct target *target, int current, uint32_t address, int handle_breakpoints)
1049 {
1050         return dsp563xx_step_ex(target, current, address, handle_breakpoints, 0);
1051 }
1052
1053 static int dsp563xx_assert_reset(struct target *target)
1054 {
1055         int retval = 0;
1056         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
1057         enum reset_types jtag_reset_config = jtag_get_reset_config();
1058
1059         if (jtag_reset_config & RESET_HAS_SRST)
1060         {
1061                 /* default to asserting srst */
1062                 if (jtag_reset_config & RESET_SRST_PULLS_TRST)
1063                 {
1064                         jtag_add_reset(1, 1);
1065                 }
1066                 else
1067                 {
1068                         jtag_add_reset(0, 1);
1069                 }
1070         }
1071
1072         target->state = TARGET_RESET;
1073         jtag_add_sleep(5000);
1074
1075         /* registers are now invalid */
1076         register_cache_invalidate(dsp563xx->core_cache);
1077
1078         if (target->reset_halt)
1079         {
1080                 if ((retval = target_halt(target)) != ERROR_OK)
1081                         return retval;
1082         }
1083
1084         LOG_DEBUG("%s", __FUNCTION__);
1085         return ERROR_OK;
1086 }
1087
1088 static int dsp563xx_deassert_reset(struct target *target)
1089 {
1090         int err;
1091
1092         /* deassert reset lines */
1093         jtag_add_reset(0, 0);
1094
1095         if ((err = dsp563xx_poll(target)) != ERROR_OK)
1096                 return err;
1097
1098         if (target->reset_halt)
1099         {
1100                 if (target->state == TARGET_HALTED)
1101                 {
1102                         /* after a reset the cpu jmp to the
1103                          * reset vector and need 2 cycles to fill
1104                          * the cache (fetch,decode,excecute)
1105                          */
1106                         if ((err = dsp563xx_step_ex(target, 1, 0, 1, 1)) != ERROR_OK)
1107                                 return err;
1108                 }
1109         }
1110
1111 //      target->state = TARGET_RUNNING;
1112
1113         LOG_DEBUG("%s", __FUNCTION__);
1114         return ERROR_OK;
1115 }
1116
1117 static int dsp563xx_soft_reset_halt(struct target *target)
1118 {
1119         LOG_DEBUG("%s", __FUNCTION__);
1120         return ERROR_OK;
1121 }
1122
1123 static int dsp563xx_read_memory(struct target *target, int mem_type, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1124 {
1125         int err;
1126         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
1127         uint32_t i, x;
1128         uint32_t data, move_cmd;
1129         uint8_t *b;
1130
1131         LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32, address, size, count);
1132
1133         if (target->state != TARGET_HALTED)
1134         {
1135                 LOG_WARNING("target not halted");
1136                 return ERROR_TARGET_NOT_HALTED;
1137         }
1138
1139         /* we only support 4 byte aligned data */
1140         if ( size != 4 )
1141         {
1142                 return ERROR_INVALID_ARGUMENTS;
1143         }
1144
1145         switch (mem_type)
1146         {
1147                 case MEM_X:
1148                         /* TODO: mark effected queued registers */
1149                         move_cmd = 0x61d800;
1150                         break;
1151                 case MEM_Y:
1152                         move_cmd = 0x69d800;
1153                         break;
1154                 case MEM_P:
1155                         move_cmd = 0x07d891;
1156                         break;
1157                 default:
1158                         return ERROR_INVALID_ARGUMENTS;
1159         }
1160
1161         /* we use r0 to store temporary data */
1162         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
1163                 dsp563xx->read_core_reg(target, REG_NUM_R0);
1164         /* we use r1 to store temporary data */
1165         if (!dsp563xx->core_cache->reg_list[REG_NUM_R1].valid)
1166                 dsp563xx->read_core_reg(target, REG_NUM_R1);
1167
1168         /* r0 is no longer valid on target */
1169         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
1170         /* r1 is no longer valid on target */
1171         dsp563xx->core_cache->reg_list[REG_NUM_R1].dirty = 1;
1172
1173         x = count;
1174         b = buffer;
1175
1176         if ((err = dsp563xx_once_execute_dw_ir(target->tap, 0x60F400, address)) != ERROR_OK)
1177                 return err;
1178
1179         for (i = 0; i < x; i++)
1180         {
1181                 if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, move_cmd)) != ERROR_OK)
1182                         return err;
1183                 if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, 0x08D13C)) != ERROR_OK)
1184                         return err;
1185                 if ((err = dsp563xx_once_reg_read_nq(target->tap, DSP563XX_ONCE_OGDBR, (uint32_t*)b)) != ERROR_OK)
1186                         return err;
1187                 b += 4;
1188         }
1189
1190         /* flush the jtag queue */
1191         if ((err = jtag_execute_queue()) != ERROR_OK)
1192         {
1193                 return err;
1194         }
1195
1196         /* walk over the buffer and fix target endianness */
1197         b = buffer;
1198
1199         for (i = 0; i < x; i++)
1200         {
1201                 data = *((uint32_t*)b) & 0x00FFFFFF;
1202 //              LOG_DEBUG("R: %08X", *((uint32_t*)b));
1203                 target_buffer_set_u32(target, b, data);
1204                 b += 4;
1205         }
1206
1207         return ERROR_OK;
1208 }
1209
1210 static int dsp563xx_read_memory_p(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1211 {
1212         return dsp563xx_read_memory(target, MEM_P, address, size, count, buffer);
1213 }
1214
1215 static int dsp563xx_write_memory(struct target *target, int mem_type, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1216 {
1217         int err;
1218         struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
1219         uint32_t i, x;
1220         uint32_t data, move_cmd;
1221         uint8_t *b;
1222
1223         LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "", address, size, count);
1224
1225         if (target->state != TARGET_HALTED)
1226         {
1227                 LOG_WARNING("target not halted");
1228                 return ERROR_TARGET_NOT_HALTED;
1229         }
1230
1231         /* we only support 4 byte aligned data */
1232         if ( size != 4 )
1233         {
1234                 return ERROR_INVALID_ARGUMENTS;
1235         }
1236
1237         switch (mem_type)
1238         {
1239                 case MEM_X:
1240                         move_cmd = 0x615800;
1241                         break;
1242                 case MEM_Y:
1243                         move_cmd = 0x695800;
1244                         break;
1245                 case MEM_P:
1246                         move_cmd = 0x075891;
1247                         break;
1248                 default:
1249                         return ERROR_INVALID_ARGUMENTS;
1250         }
1251
1252         /* we use r0 to store temporary data */
1253         if (!dsp563xx->core_cache->reg_list[REG_NUM_R0].valid)
1254                 dsp563xx->read_core_reg(target, REG_NUM_R0);
1255         /* we use r1 to store temporary data */
1256         if (!dsp563xx->core_cache->reg_list[REG_NUM_R1].valid)
1257                 dsp563xx->read_core_reg(target, REG_NUM_R1);
1258
1259         /* r0 is no longer valid on target */
1260         dsp563xx->core_cache->reg_list[REG_NUM_R0].dirty = 1;
1261         /* r1 is no longer valid on target */
1262         dsp563xx->core_cache->reg_list[REG_NUM_R1].dirty = 1;
1263
1264         x = count;
1265         b = buffer;
1266
1267         if ((err = dsp563xx_once_execute_dw_ir(target->tap, 0x60F400, address)) != ERROR_OK)
1268                 return err;
1269
1270         for (i = 0; i < x; i++)
1271         {
1272                 data = target_buffer_get_u32(target, b);
1273
1274 //              LOG_DEBUG("W: %08X", data);
1275
1276                 data &= 0x00ffffff;
1277
1278                 if ((err = dsp563xx_once_execute_dw_ir_nq(target->tap, 0x61F400, data)) != ERROR_OK)
1279                         return err;
1280                 if ((err = dsp563xx_once_execute_sw_ir_nq(target->tap, move_cmd)) != ERROR_OK)
1281                         return err;
1282                 b += 4;
1283         }
1284
1285         /* flush the jtag queue */
1286         if ((err = jtag_execute_queue()) != ERROR_OK)
1287         {
1288                 return err;
1289         }
1290
1291         return ERROR_OK;
1292 }
1293
1294 static int dsp563xx_write_memory_p(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t * buffer)
1295 {
1296         return dsp563xx_write_memory(target, MEM_P, address, size, count, buffer);
1297 }
1298
1299 static int dsp563xx_bulk_write_memory_p(struct target *target, uint32_t address, uint32_t count, uint8_t *buffer)
1300 {
1301         return dsp563xx_write_memory(target, MEM_P, address, 4, count, buffer);
1302 }
1303
1304 static void handle_md_output(struct command_context *cmd_ctx, struct target *target, uint32_t address, unsigned size, unsigned count, const uint8_t * buffer)
1305 {
1306         const unsigned line_bytecnt = 32;
1307         unsigned line_modulo = line_bytecnt / size;
1308
1309         char output[line_bytecnt * 4 + 1];
1310         unsigned output_len = 0;
1311
1312         const char *value_fmt;
1313         switch (size)
1314         {
1315                 case 4:
1316                         value_fmt = "%8.8x ";
1317                         break;
1318                 case 2:
1319                         value_fmt = "%4.4x ";
1320                         break;
1321                 case 1:
1322                         value_fmt = "%2.2x ";
1323                         break;
1324                 default:
1325                         /* "can't happen", caller checked */
1326                         LOG_ERROR("invalid memory read size: %u", size);
1327                         return;
1328         }
1329
1330         for (unsigned i = 0; i < count; i++)
1331         {
1332                 if (i % line_modulo == 0)
1333                 {
1334                         output_len += snprintf(output + output_len, sizeof(output) - output_len, "0x%8.8x: ", (unsigned) (address + (i * size)));
1335                 }
1336
1337                 uint32_t value = 0;
1338                 const uint8_t *value_ptr = buffer + i * size;
1339                 switch (size)
1340                 {
1341                         case 4:
1342                                 value = target_buffer_get_u32(target, value_ptr);
1343                                 break;
1344                         case 2:
1345                                 value = target_buffer_get_u16(target, value_ptr);
1346                                 break;
1347                         case 1:
1348                                 value = *value_ptr;
1349                 }
1350                 output_len += snprintf(output + output_len, sizeof(output) - output_len, value_fmt, value);
1351
1352                 if ((i % line_modulo == line_modulo - 1) || (i == count - 1))
1353                 {
1354                         command_print(cmd_ctx, "%s", output);
1355                         output_len = 0;
1356                 }
1357         }
1358 }
1359
1360 COMMAND_HANDLER(dsp563xx_mem_command)
1361 {
1362         struct target *target = get_current_target(CMD_CTX);
1363         int err = ERROR_OK;
1364         int read_mem;
1365         uint32_t address = 0;
1366         uint32_t count = 1, i;
1367         uint32_t pattern = 0;
1368         uint32_t mem_type;
1369         uint8_t *buffer, *b;
1370
1371         switch (CMD_NAME[1])
1372         {
1373                 case 'w':
1374                         read_mem = 0;
1375                         break;
1376                 case 'd':
1377                         read_mem = 1;
1378                         break;
1379                 default:
1380                         return ERROR_COMMAND_SYNTAX_ERROR;
1381         }
1382
1383         switch (CMD_NAME[3])
1384         {
1385                 case 'x':
1386                         mem_type = MEM_X;
1387                         break;
1388                 case 'y':
1389                         mem_type = MEM_Y;
1390                         break;
1391                 case 'p':
1392                         mem_type = MEM_P;
1393                         break;
1394                 default:
1395                         return ERROR_COMMAND_SYNTAX_ERROR;
1396         }
1397
1398         if (CMD_ARGC > 0)
1399         {
1400                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
1401         }
1402
1403         if (read_mem == 0)
1404         {
1405                 if (CMD_ARGC < 2)
1406                 {
1407                         return ERROR_COMMAND_SYNTAX_ERROR;
1408                 }
1409                 if (CMD_ARGC > 1)
1410                 {
1411                         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], pattern);
1412                 }
1413                 if (CMD_ARGC > 2)
1414                 {
1415                         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], count);
1416                 }
1417         }
1418
1419         if (read_mem == 1)
1420         {
1421                 if (CMD_ARGC < 1)
1422                 {
1423                         return ERROR_COMMAND_SYNTAX_ERROR;
1424                 }
1425                 if (CMD_ARGC > 1)
1426                 {
1427                         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], count);
1428                 }
1429         }
1430
1431         buffer = calloc(count, sizeof(uint32_t));
1432
1433         if (read_mem == 1)
1434         {
1435                 if ((err = dsp563xx_read_memory(target, mem_type, address, sizeof(uint32_t), count, buffer)) == ERROR_OK)
1436                         handle_md_output(CMD_CTX, target, address, sizeof(uint32_t), count, buffer);
1437         }
1438         else
1439         {
1440                 b = buffer;
1441
1442                 for (i = 0; i < count; i++)
1443                 {
1444                         target_buffer_set_u32(target, b, pattern);
1445                         b += 4;
1446                 }
1447
1448                 err = dsp563xx_write_memory(target, mem_type, address, sizeof(uint32_t), count, buffer);
1449         }
1450
1451         free(buffer);
1452
1453         return err;
1454 }
1455
1456 static const struct command_registration dsp563xx_command_handlers[] = {
1457         {
1458          .name = "mwwx",
1459          .handler = dsp563xx_mem_command,
1460          .mode = COMMAND_EXEC,
1461          .help = "write x memory words",
1462          .usage = "mwwx address value [count]",
1463          },
1464         {
1465          .name = "mwwy",
1466          .handler = dsp563xx_mem_command,
1467          .mode = COMMAND_EXEC,
1468          .help = "write y memory words",
1469          .usage = "mwwy address value [count]",
1470          },
1471         {
1472          .name = "mwwp",
1473          .handler = dsp563xx_mem_command,
1474          .mode = COMMAND_EXEC,
1475          .help = "write p memory words",
1476          .usage = "mwwp address value [count]",
1477          },
1478         {
1479          .name = "mdwx",
1480          .handler = dsp563xx_mem_command,
1481          .mode = COMMAND_EXEC,
1482          .help = "display x memory words",
1483          .usage = "mdwx address [count]",
1484          },
1485         {
1486          .name = "mdwy",
1487          .handler = dsp563xx_mem_command,
1488          .mode = COMMAND_EXEC,
1489          .help = "display y memory words",
1490          .usage = "mdwy address [count]",
1491          },
1492         {
1493          .name = "mdwp",
1494          .handler = dsp563xx_mem_command,
1495          .mode = COMMAND_EXEC,
1496          .help = "display p memory words",
1497          .usage = "mdwp address [count]",
1498          },
1499         COMMAND_REGISTRATION_DONE
1500 };
1501
1502 /** Holds methods for DSP563XX targets. */
1503 struct target_type dsp563xx_target = {
1504         .name = "dsp563xx",
1505
1506         .poll = dsp563xx_poll,
1507         .arch_state = dsp563xx_arch_state,
1508
1509         .target_request_data = NULL,
1510
1511         .get_gdb_reg_list = dsp563xx_get_gdb_reg_list,
1512
1513         .halt = dsp563xx_halt,
1514         .resume = dsp563xx_resume,
1515         .step = dsp563xx_step,
1516
1517         .assert_reset = dsp563xx_assert_reset,
1518         .deassert_reset = dsp563xx_deassert_reset,
1519         .soft_reset_halt = dsp563xx_soft_reset_halt,
1520
1521         .read_memory = dsp563xx_read_memory_p,
1522         .write_memory = dsp563xx_write_memory_p,
1523         .bulk_write_memory = dsp563xx_bulk_write_memory_p,
1524
1525         .commands = dsp563xx_command_handlers,
1526         .target_create = dsp563xx_target_create,
1527         .init_target = dsp563xx_init_target,
1528 };