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