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