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