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