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

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