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