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

   1 /***************************************************************************
   2  *  Copyright (C) 2011 by Rodrigo L. Rosa                                 *
   3  *  rodrigorosa.LG@gmail.com                                              *
   4  *                                                                        *
   5  *  Based on dsp563xx_once.h written by Mathias Kuester                   *
   6  *  mkdorg@users.sourceforge.net                                          *
   7  *                                                                        *
   8  *  This program is free software; you can redistribute it and/or modify  *
   9  *  it under the terms of the GNU General Public License as published by  *
  10  *  the Free Software Foundation; either version 2 of the License, or     *
  11  *  (at your option) any later version.                                   *
  12  *                                                                        *
  13  *  This program is distributed in the hope that it will be useful,       *
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  16  *  GNU General Public License for more details.                          *
  17  *                                                                        *
  18  *  You should have received a copy of the GNU General Public License     *
  19  *  along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  20  ***************************************************************************/
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include "config.h"
  24 #endif
  25
  26 #include "target.h"
  27 #include "target_type.h"
  28 #include "dsp5680xx.h"
  29
  30 struct dsp5680xx_common dsp5680xx_context;
  31
  32 #define _E "DSP5680XX_ERROR:%d\nAt:%s:%d:%s"
  33 #define err_check(r, c, m) if (r != ERROR_OK) {LOG_ERROR(_E, c, __func__, __LINE__, m); return r; }
  34 #define err_check_propagate(retval) if (retval != ERROR_OK) return retval;
  35 #define DEBUG_MSG "Debug mode be enabled to read mem."
  36 #define DEBUG_FAIL { err_check(ERROR_FAIL, DSP5680XX_ERROR_NOT_IN_DEBUG, DEBUG_MSG) }
  37 #define CHECK_DBG if (!dsp5680xx_context.debug_mode_enabled) DEBUG_FAIL
  38 #define HALT_MSG "Target must be halted."
  39 #define HALT_FAIL { err_check(ERROR_FAIL, DSP5680XX_ERROR_TARGET_RUNNING, HALT_MSG) }
  40 #define CHECK_HALT(target) if (target->state != TARGET_HALTED) HALT_FAIL
  41 #define check_halt_and_debug(target) { CHECK_HALT(target); CHECK_DBG; }
  42
  43 static int dsp5680xx_execute_queue(void)
  44 {
  45         int retval;
  46
  47         retval = jtag_execute_queue();
  48         return retval;
  49 }
  50
  51 /**
  52  * Reset state machine
  53  */
  54 static int reset_jtag(void)
  55 {
  56         int retval;
  57
  58         tap_state_t states[2];
  59
  60         const char *cp = "RESET";
  61
  62         states[0] = tap_state_by_name(cp);
  63         retval = jtag_add_statemove(states[0]);
  64         err_check_propagate(retval);
  65         retval = jtag_execute_queue();
  66         err_check_propagate(retval);
  67         jtag_add_pathmove(0, states + 1);
  68         retval = jtag_execute_queue();
  69         return retval;
  70 }
  71
  72 static int dsp5680xx_drscan(struct target *target, uint8_t *d_in,
  73                             uint8_t *d_out, int len)
  74 {
  75         /* -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
  76          *
  77          *Inputs:
  78          *    - d_in: This is the data that will be shifted into the JTAG DR reg.
  79          *    - d_out: The data that will be shifted out of the JTAG DR reg will stored here
  80          *    - len: Length of the data to be shifted to JTAG DR.
  81          *
  82          *Note:  If  d_out   ==  NULL, discard incoming bits.
  83          *
  84          *-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
  85          */
  86         int retval = ERROR_OK;
  87
  88         if (NULL == target->tap) {
  89                 retval = ERROR_FAIL;
  90                 err_check(retval, DSP5680XX_ERROR_JTAG_INVALID_TAP,
  91                           "Invalid tap");
  92         }
  93         if (len > 32) {
  94                 retval = ERROR_FAIL;
  95                 err_check(retval, DSP5680XX_ERROR_JTAG_DR_LEN_OVERFLOW,
  96                           "dr_len overflow, maximum is 32");
  97         }
  98         /* TODO what values of len are valid for jtag_add_plain_dr_scan? */
  99         /* can i send as many bits as i want? */
 100         /* is the casting necessary? */
 101         jtag_add_plain_dr_scan(len, d_in, d_out, TAP_IDLE);
 102         if (dsp5680xx_context.flush) {
 103                 retval = dsp5680xx_execute_queue();
 104                 err_check(retval, DSP5680XX_ERROR_JTAG_DRSCAN,
 105                           "drscan failed!");
 106         }
 107         if (d_out != NULL)
 108                 LOG_DEBUG("Data read (%d bits): 0x%04X", len, *d_out);
 109         else
 110                 LOG_DEBUG("Data read was discarded.");
 111         return retval;
 112 }
 113
 114 /**
 115  * Test func
 116  *
 117  * @param target
 118  * @param d_in This is the data that will be shifted into the JTAG IR reg.
 119  * @param d_out The data that will be shifted out of the JTAG IR reg will be stored here.
 120  * @param ir_len Length of the data to be shifted to JTAG IR.
 121  *
 122  */
 123 static int dsp5680xx_irscan(struct target *target, uint32_t *d_in,
 124                             uint32_t *d_out, uint8_t ir_len)
 125 {
 126         int retval = ERROR_OK;
 127
 128         uint16_t tap_ir_len = DSP5680XX_JTAG_MASTER_TAP_IRLEN;
 129
 130         if (NULL == target->tap) {
 131                 retval = ERROR_FAIL;
 132                 err_check(retval, DSP5680XX_ERROR_JTAG_INVALID_TAP,
 133                           "Invalid tap");
 134         }
 135         if (ir_len != target->tap->ir_length) {
 136                 if (target->tap->enabled) {
 137                         retval = ERROR_FAIL;
 138                         err_check(retval, DSP5680XX_ERROR_INVALID_IR_LEN,
 139                                   "Invalid irlen");
 140                 } else {
 141                         struct jtag_tap *t =
 142                                 jtag_tap_by_string("dsp568013.chp");
 143                         if ((t == NULL)
 144                             || ((t->enabled) && (ir_len != tap_ir_len))) {
 145                                 retval = ERROR_FAIL;
 146                                 err_check(retval,
 147                                           DSP5680XX_ERROR_INVALID_IR_LEN,
 148                                           "Invalid irlen");
 149                         }
 150                 }
 151         }
 152         jtag_add_plain_ir_scan(ir_len, (uint8_t *) d_in, (uint8_t *) d_out,
 153                                TAP_IDLE);
 154         if (dsp5680xx_context.flush) {
 155                 retval = dsp5680xx_execute_queue();
 156                 err_check(retval, DSP5680XX_ERROR_JTAG_IRSCAN,
 157                           "irscan failed!");
 158         }
 159         return retval;
 160 }
 161
 162 static int dsp5680xx_jtag_status(struct target *target, uint8_t *status)
 163 {
 164         uint32_t read_from_ir;
 165
 166         uint32_t instr;
 167
 168         int retval;
 169
 170         instr = JTAG_INSTR_ENABLE_ONCE;
 171         retval =
 172                 dsp5680xx_irscan(target, &instr, &read_from_ir,
 173                                  DSP5680XX_JTAG_CORE_TAP_IRLEN);
 174         err_check_propagate(retval);
 175         if (status != NULL)
 176                 *status = (uint8_t) read_from_ir;
 177         return ERROR_OK;
 178 }
 179
 180 static int jtag_data_read(struct target *target, uint8_t *data_read,
 181                           int num_bits)
 182 {
 183         uint32_t bogus_instr = 0;
 184
 185         int retval =
 186                 dsp5680xx_drscan(target, (uint8_t *) &bogus_instr, data_read,
 187                                  num_bits);
 188         LOG_DEBUG("Data read (%d bits): 0x%04X", num_bits, *data_read);
 189         /** TODO remove this or move to jtagio? */
 190         return retval;
 191 }
 192
 193 #define jtag_data_read8(target, data_read)  jtag_data_read(target, data_read, 8)
 194 #define jtag_data_read16(target, data_read) jtag_data_read(target, data_read, 16)
 195 #define jtag_data_read32(target, data_read) jtag_data_read(target, data_read, 32)
 196
 197 static uint32_t data_read_dummy;
 198
 199 static int jtag_data_write(struct target *target, uint32_t instr, int num_bits,
 200                            uint32_t *data_read)
 201 {
 202         int retval;
 203
 204         retval =
 205                 dsp5680xx_drscan(target, (uint8_t *) &instr,
 206                                  (uint8_t *) &data_read_dummy, num_bits);
 207         err_check_propagate(retval);
 208         if (data_read != NULL)
 209                 *data_read = data_read_dummy;
 210         return retval;
 211 }
 212
 213 #define jtag_data_write8(target, instr, data_read)  jtag_data_write(target, instr, 8, data_read)
 214 #define jtag_data_write16(target, instr, data_read) jtag_data_write(target, instr, 16, data_read)
 215 #define jtag_data_write24(target, instr, data_read) jtag_data_write(target, instr, 24, data_read)
 216 #define jtag_data_write32(target, instr, data_read) jtag_data_write(target, instr, 32, data_read)
 217
 218 /**
 219  * Executes EOnCE instruction.
 220  *
 221  * @param target
 222  * @param instr Instruction to execute.
 223  * @param rw
 224  * @param go
 225  * @param ex
 226  * @param eonce_status Value read from the EOnCE status register.
 227  *
 228  * @return
 229  */
 230 static int eonce_instruction_exec_single(struct target *target, uint8_t instr,
 231                                          uint8_t rw, uint8_t go, uint8_t ex,
 232                                          uint8_t *eonce_status)
 233 {
 234         int retval;
 235
 236         uint32_t dr_out_tmp;
 237
 238         uint8_t instr_with_flags = instr | (rw << 7) | (go << 6) | (ex << 5);
 239
 240         retval = jtag_data_write(target, instr_with_flags, 8, &dr_out_tmp);
 241         err_check_propagate(retval);
 242         if (eonce_status != NULL)
 243                 *eonce_status = (uint8_t) dr_out_tmp;
 244         return retval;
 245 }
 246
 247 /* wrappers for multi opcode instructions */
 248 #define dsp5680xx_exe_1(target, oc1, oc2, oc3)   dsp5680xx_exe1(target, oc1)
 249 #define dsp5680xx_exe_2(target, oc1, oc2, oc3)   dsp5680xx_exe2(target, oc1, oc2)
 250 #define dsp5680xx_exe_3(target, oc1, oc2, oc3)   dsp5680xx_exe3(target, oc1, oc2, oc3)
 251 #define dsp5680xx_exe_generic(t, words, oc1, oc2, oc3) dsp5680xx_exe_##words(t, oc1, oc2, oc3)
 252
 253 /* Executes one word DSP instruction */
 254 static int dsp5680xx_exe1(struct target *target, uint16_t opcode)
 255 {
 256         int retval;
 257
 258         retval = eonce_instruction_exec_single(target, 0x04, 0, 1, 0, NULL);
 259         err_check_propagate(retval);
 260         retval = jtag_data_write16(target, opcode, NULL);
 261         err_check_propagate(retval);
 262         return retval;
 263 }
 264
 265 /* Executes two word DSP instruction */
 266 static int dsp5680xx_exe2(struct target *target, uint16_t opcode1,
 267                           uint16_t opcode2)
 268 {
 269         int retval;
 270
 271         retval = eonce_instruction_exec_single(target, 0x04, 0, 0, 0, NULL);
 272         err_check_propagate(retval);
 273         retval = jtag_data_write16(target, opcode1, NULL);
 274         err_check_propagate(retval);
 275         retval = eonce_instruction_exec_single(target, 0x04, 0, 1, 0, NULL);
 276         err_check_propagate(retval);
 277         retval = jtag_data_write16(target, opcode2, NULL);
 278         err_check_propagate(retval);
 279         return retval;
 280 }
 281
 282 /* Executes three word DSP instruction */
 283 static int dsp5680xx_exe3(struct target *target, uint16_t opcode1,
 284                           uint16_t opcode2, uint16_t opcode3)
 285 {
 286         int retval;
 287
 288         retval = eonce_instruction_exec_single(target, 0x04, 0, 0, 0, NULL);
 289         err_check_propagate(retval);
 290         retval = jtag_data_write16(target, opcode1, NULL);
 291         err_check_propagate(retval);
 292         retval = eonce_instruction_exec_single(target, 0x04, 0, 0, 0, NULL);
 293         err_check_propagate(retval);
 294         retval = jtag_data_write16(target, opcode2, NULL);
 295         err_check_propagate(retval);
 296         retval = eonce_instruction_exec_single(target, 0x04, 0, 1, 0, NULL);
 297         err_check_propagate(retval);
 298         retval = jtag_data_write16(target, opcode3, NULL);
 299         err_check_propagate(retval);
 300         return retval;
 301 }
 302
 303 /*
 304  *--------------- Real-time data exchange ---------------
 305  * The EOnCE Transmit (OTX) and Receive (ORX) registers are data memory mapped, each with an upper
 306  * and lower 16 bit word.
 307  * Transmit and receive directions are defined from the core’s perspective.
 308  * The core writes to the Transmit register and reads the Receive register, and the host through
 309  * JTAG writes to the Receive register and reads the Transmit register.
 310  * Both registers have a combined data memory mapped OTXRXSR which provides indication when
 311  * each may be accessed.
 312  * ref: eonce_rev.1.0_0208081.pdf@36
 313  */
 314
 315 /* writes data into upper ORx register of the target */
 316 static int core_tx_upper_data(struct target *target, uint16_t data,
 317                               uint32_t *eonce_status_low)
 318 {
 319         int retval;
 320
 321         retval =
 322                 eonce_instruction_exec_single(target, DSP5680XX_ONCE_ORX1, 0, 0, 0,
 323                                               NULL);
 324         err_check_propagate(retval);
 325         retval = jtag_data_write16(target, data, eonce_status_low);
 326         err_check_propagate(retval);
 327         return retval;
 328 }
 329
 330 /* writes data into lower ORx register of the target */
 331 #define CMD1 eonce_instruction_exec_single(target, DSP5680XX_ONCE_ORX, 0, 0, 0, NULL);
 332 #define CMD2 jtag_data_write16((t, data)
 333 #define core_tx_lower_data(t, data) PT1\ PT2
 334
 335 /**
 336  *
 337  * @param target
 338  * @param data_read: Returns the data read from the upper OTX register via JTAG.
 339  * @return: Returns an error code (see error code documentation)
 340  */
 341 static int core_rx_upper_data(struct target *target, uint8_t *data_read)
 342 {
 343         int retval;
 344
 345         retval =
 346                 eonce_instruction_exec_single(target, DSP5680XX_ONCE_OTX1, 1, 0, 0,
 347                                               NULL);
 348         err_check_propagate(retval);
 349         retval = jtag_data_read16(target, data_read);
 350         err_check_propagate(retval);
 351         return retval;
 352 }
 353
 354 /**
 355  *
 356  * @param target
 357  * @param data_read: Returns the data read from the lower OTX register via JTAG.
 358  * @return: Returns an error code (see error code documentation)
 359  */
 360 static int core_rx_lower_data(struct target *target, uint8_t *data_read)
 361 {
 362         int retval;
 363
 364         retval =
 365                 eonce_instruction_exec_single(target, DSP5680XX_ONCE_OTX, 1, 0, 0,
 366                                               NULL);
 367         err_check_propagate(retval);
 368         retval = jtag_data_read16(target, data_read);
 369         err_check_propagate(retval);
 370         return retval;
 371 }
 372
 373 /*
 374  *-- -- -- -- --- -- -- -- --- -- -- -- --- -- -- -- --- -- -- -- --- --
 375  *-- -- -- -- --- -- -- -Core Instructions- -- -- -- --- -- -- -- --- --
 376  *-- -- -- -- --- -- -- -- --- -- -- -- --- -- -- -- --- -- -- -- --- --
 377  */
 378
 379 #define exe(a, b, c, d, e) dsp5680xx_exe_generic(a, b, c, d, e)
 380
 381 /* move.l #value, r0 */
 382 #define core_move_long_to_r0(target, value)     exe(target, 3, 0xe418, value&0xffff, value>>16)
 383
 384 /* move.l #value, n */
 385 #define core_move_long_to_n(target, value)      exe(target, 3, 0xe41e, value&0xffff, value>>16)
 386
 387 /* move x:(r0), y0 */
 388 #define core_move_at_r0_to_y0(target)   exe(target, 1, 0xF514, 0, 0)
 389
 390 /* move x:(r0), y1 */
 391 #define core_move_at_r0_to_y1(target)   exe(target, 1, 0xF714, 0, 0)
 392
 393 /* move.l x:(r0), y */
 394 #define core_move_long_at_r0_y(target) exe(target, 1, 0xF734, 0, 0)
 395
 396 /* move y0, x:(r0) */
 397 #define core_move_y0_at_r0(target)      exe(target, 1, 0xd514, 0, 0)
 398
 399 /* bfclr #value, x:(r0) */
 400 #define eonce_bfclr_at_r0(target, value)        exe(target, 2, 0x8040, value, 0)
 401
 402 /* move #value, y0 */
 403 #define core_move_value_to_y0(target, value)    exe(target, 2, 0x8745, value, 0)
 404
 405 /* move.w y0, x:(r0)+ */
 406 #define core_move_y0_at_r0_inc(target)  exe(target, 1, 0xd500, 0, 0)
 407
 408 /* move.w y0, p:(r0)+ */
 409 #define core_move_y0_at_pr0_inc(target) exe(target, 1, 0x8560, 0, 0)
 410
 411 /* move.w p:(r0)+, y0 */
 412 #define core_move_at_pr0_inc_to_y0(target)      exe(target, 1, 0x8568, 0, 0)
 413
 414 /* move.w p:(r0)+, y1 */
 415 #define core_move_at_pr0_inc_to_y1(target)      exe(target, 1, 0x8768, 0, 0)
 416
 417 /* move.l #value, r2 */
 418 #define core_move_long_to_r2(target, value)     exe(target, 3, 0xe41A, value&0xffff, value>>16)
 419
 420 /* move y0, x:(r2) */
 421 #define core_move_y0_at_r2(target)           exe(target, 1, 0xd516, 0, 0)
 422
 423 /* move.w #<value>, x:(r2) */
 424 #define core_move_value_at_r2(target, value)    exe(target, 2, 0x8642, value, 0)
 425
 426 /* move.w #<value>, x:(r0) */
 427 #define core_move_value_at_r0(target, value)    exe(target, 2, 0x8640, value, 0)
 428
 429 /* move.w #<value>, x:(R2+<disp>) */
 430 #define core_move_value_at_r2_disp(target, value, disp) exe(target, 3, 0x8646, value, disp)
 431
 432 /* move.w x:(r2), Y0 */
 433 #define core_move_at_r2_to_y0(target)   exe(target, 1, 0xF516, 0, 0)
 434
 435 /* move.w p:(r2)+, y0 */
 436 #define core_move_at_pr2_inc_to_y0(target)      exe(target, 1, 0x856A, 0, 0)
 437
 438 /* move.l #value, r3 */
 439 #define core_move_long_to_r1(target, value)     exe(target, 3, 0xE419, value&0xffff, value>>16)
 440
 441 /* move.l #value, r3 */
 442 #define core_move_long_to_r3(target, value)     exe(target, 3, 0xE41B, value&0xffff, value>>16)
 443
 444 /* move.w y0, p:(r3)+ */
 445 #define core_move_y0_at_pr3_inc(target) exe(target, 1, 0x8563, 0, 0)
 446
 447 /* move.w y0, x:(r3) */
 448 #define core_move_y0_at_r3(target)      exe(target, 1, 0xD503, 0, 0)
 449
 450 /* move.l #value, r4 */
 451 #define core_move_long_to_r4(target, value)     exe(target, 3, 0xE41C, value&0xffff, value>>16)
 452
 453 /* move pc, r4 */
 454 #define core_move_pc_to_r4(target)      exe(target, 1, 0xE716, 0, 0)
 455
 456 /* move.l r4, y */
 457 #define core_move_r4_to_y(target)       exe(target, 1, 0xe764, 0, 0)
 458
 459 /* move.w p:(r0)+, y0 */
 460 #define core_move_at_pr0_inc_to_y0(target)      exe(target, 1, 0x8568, 0, 0)
 461
 462 /* move.w x:(r0)+, y0 */
 463 #define core_move_at_r0_inc_to_y0(target)       exe(target, 1, 0xf500, 0, 0)
 464
 465 /* move x:(r0), y0 */
 466 #define core_move_at_r0_y0(target)      exe(target, 1, 0xF514, 0, 0)
 467
 468 /* nop */
 469 #define eonce_nop(target)       exe(target, 1, 0xe700, 0, 0)
 470
 471 /* move.w x:(R2+<disp>), Y0 */
 472 #define core_move_at_r2_disp_to_y0(target, disp) exe(target, 2, 0xF542, disp, 0)
 473
 474 /* move.w y1, x:(r2) */
 475 #define core_move_y1_at_r2(target) exe(target, 1, 0xd716, 0, 0)
 476
 477 /* move.w y1, x:(r0) */
 478 #define core_move_y1_at_r0(target) exe(target, 1, 0xd714, 0, 0)
 479
 480 /* move.bp y0, x:(r0)+ */
 481 #define core_move_byte_y0_at_r0(target) exe(target, 1, 0xd5a0, 0, 0)
 482
 483 /* move.w y1, p:(r0)+ */
 484 #define core_move_y1_at_pr0_inc(target) exe(target, 1, 0x8760, 0, 0)
 485
 486 /* move.w y1, x:(r0)+ */
 487 #define core_move_y1_at_r0_inc(target) exe(target, 1, 0xD700, 0, 0)
 488
 489 /* move.l #value, y */
 490 #define core_move_long_to_y(target, value) exe(target, 3, 0xe417, value&0xffff, value>>16)
 491
 492 static int core_move_value_to_pc(struct target *target, uint32_t value)
 493 {
 494         check_halt_and_debug(target);
 495         int retval;
 496
 497         retval =
 498                 dsp5680xx_exe_generic(target, 3, 0xE71E, value & 0xffff,
 499                                       value >> 16);
 500         err_check_propagate(retval);
 501         return retval;
 502 }
 503
 504 static int eonce_load_TX_RX_to_r0(struct target *target)
 505 {
 506         int retval;
 507
 508         retval =
 509                 core_move_long_to_r0(target,
 510                                      ((MC568013_EONCE_TX_RX_ADDR) +
 511                                       (MC568013_EONCE_OBASE_ADDR << 16)));
 512         return retval;
 513 }
 514
 515 static int core_load_TX_RX_high_addr_to_r0(struct target *target)
 516 {
 517         int retval = 0;
 518
 519         retval =
 520                 core_move_long_to_r0(target,
 521                                      ((MC568013_EONCE_TX1_RX1_HIGH_ADDR) +
 522                                       (MC568013_EONCE_OBASE_ADDR << 16)));
 523         return retval;
 524 }
 525
 526 static int dsp5680xx_read_core_reg(struct target *target, uint8_t reg_addr,
 527                                    uint16_t *data_read)
 528 {
 529         /* TODO implement a general version of this which matches what openocd uses. */
 530         int retval;
 531
 532         uint32_t dummy_data_to_shift_into_dr;
 533
 534         retval = eonce_instruction_exec_single(target, reg_addr, 1, 0, 0, NULL);
 535         err_check_propagate(retval);
 536         retval =
 537                 dsp5680xx_drscan(target, (uint8_t *) &dummy_data_to_shift_into_dr,
 538                                  (uint8_t *) data_read, 8);
 539         err_check_propagate(retval);
 540         LOG_DEBUG("Reg. data: 0x%02X.", *data_read);
 541         return retval;
 542 }
 543
 544 static int eonce_read_status_reg(struct target *target, uint16_t *data)
 545 {
 546         int retval;
 547
 548         retval = dsp5680xx_read_core_reg(target, DSP5680XX_ONCE_OSR, data);
 549         err_check_propagate(retval);
 550         return retval;
 551 }
 552
 553 /**
 554  * Takes the core out of debug mode.
 555  *
 556  * @param target
 557  * @param eonce_status Data read from the EOnCE status register.
 558  *
 559  * @return
 560  */
 561 static int eonce_exit_debug_mode(struct target *target, uint8_t *eonce_status)
 562 {
 563         int retval;
 564
 565         retval =
 566                 eonce_instruction_exec_single(target, 0x1F, 0, 0, 1, eonce_status);
 567         err_check_propagate(retval);
 568         return retval;
 569 }
 570
 571 static int switch_tap(struct target *target, struct jtag_tap *master_tap,
 572                       struct jtag_tap *core_tap)
 573 {
 574         int retval = ERROR_OK;
 575
 576         uint32_t instr;
 577
 578         uint32_t ir_out;        /* not used, just to make jtag happy. */
 579
 580         if (master_tap == NULL) {
 581                 master_tap = jtag_tap_by_string("dsp568013.chp");
 582                 if (master_tap == NULL) {
 583                         retval = ERROR_FAIL;
 584                         const char *msg = "Failed to get master tap.";
 585
 586                         err_check(retval, DSP5680XX_ERROR_JTAG_TAP_FIND_MASTER,
 587                                   msg);
 588                 }
 589         }
 590         if (core_tap == NULL) {
 591                 core_tap = jtag_tap_by_string("dsp568013.cpu");
 592                 if (core_tap == NULL) {
 593                         retval = ERROR_FAIL;
 594                         err_check(retval, DSP5680XX_ERROR_JTAG_TAP_FIND_CORE,
 595                                   "Failed to get core tap.");
 596                 }
 597         }
 598
 599         if (!(((int)master_tap->enabled) ^ ((int)core_tap->enabled))) {
 600                 LOG_WARNING
 601                         ("Master:%d\nCore:%d\nOnly 1 should be enabled.\n",
 602                          (int)master_tap->enabled, (int)core_tap->enabled);
 603         }
 604
 605         if (master_tap->enabled) {
 606                 instr = 0x5;
 607                 retval =
 608                         dsp5680xx_irscan(target, &instr, &ir_out,
 609                                          DSP5680XX_JTAG_MASTER_TAP_IRLEN);
 610                 err_check_propagate(retval);
 611                 instr = 0x2;
 612                 retval =
 613                         dsp5680xx_drscan(target, (uint8_t *) &instr,
 614                                          (uint8_t *) &ir_out, 4);
 615                 err_check_propagate(retval);
 616                 core_tap->enabled = true;
 617                 master_tap->enabled = false;
 618         } else {
 619                 instr = 0x08;
 620                 retval =
 621                         dsp5680xx_irscan(target, &instr, &ir_out,
 622                                          DSP5680XX_JTAG_CORE_TAP_IRLEN);
 623                 err_check_propagate(retval);
 624                 instr = 0x1;
 625                 retval =
 626                         dsp5680xx_drscan(target, (uint8_t *) &instr,
 627                                          (uint8_t *) &ir_out, 4);
 628                 err_check_propagate(retval);
 629                 core_tap->enabled = false;
 630                 master_tap->enabled = true;
 631         }
 632         return retval;
 633 }
 634
 635 /**
 636  * Puts the core into debug mode, enabling the EOnCE module.
 637  * This will not always work, eonce_enter_debug_mode executes much
 638  * more complicated routine, which is guaranteed to work, but requires
 639  * a reset. This will complicate comm with the flash module, since
 640  * after a reset clock divisors must be set again.
 641  * This implementation works most of the time, and is not accessible to the
 642  * user.
 643  *
 644  * @param target
 645  * @param eonce_status Data read from the EOnCE status register.
 646  *
 647  * @return
 648  */
 649 static int eonce_enter_debug_mode_without_reset(struct target *target,
 650                                                 uint16_t *eonce_status)
 651 {
 652         int retval;
 653
 654         uint32_t instr = JTAG_INSTR_DEBUG_REQUEST;
 655
 656         uint32_t ir_out;        /* not used, just to make jtag happy.*/
 657
 658         /* Debug request #1 */
 659         retval =
 660                 dsp5680xx_irscan(target, &instr, &ir_out,
 661                                  DSP5680XX_JTAG_CORE_TAP_IRLEN);
 662         err_check_propagate(retval);
 663
 664         /* Enable EOnCE module */
 665         instr = JTAG_INSTR_ENABLE_ONCE;
 666         /* Two rounds of jtag 0x6  (enable eonce) to enable EOnCE. */
 667         retval =
 668                 dsp5680xx_irscan(target, &instr, &ir_out,
 669                                  DSP5680XX_JTAG_CORE_TAP_IRLEN);
 670         err_check_propagate(retval);
 671         retval =
 672                 dsp5680xx_irscan(target, &instr, &ir_out,
 673                                  DSP5680XX_JTAG_CORE_TAP_IRLEN);
 674         err_check_propagate(retval);
 675         if ((ir_out & JTAG_STATUS_MASK) == JTAG_STATUS_DEBUG)
 676                 target->state = TARGET_HALTED;
 677         else {
 678                 retval = ERROR_FAIL;
 679                 err_check_propagate(retval);
 680         }
 681         /* Verify that debug mode is enabled */
 682         uint16_t data_read_from_dr;
 683
 684         retval = eonce_read_status_reg(target, &data_read_from_dr);
 685         err_check_propagate(retval);
 686         if ((data_read_from_dr & 0x30) == 0x30) {
 687                 LOG_DEBUG("EOnCE successfully entered debug mode.");
 688                 dsp5680xx_context.debug_mode_enabled = true;
 689                 retval = ERROR_OK;
 690         } else {
 691                 dsp5680xx_context.debug_mode_enabled = false;
 692                 retval = ERROR_TARGET_FAILURE;
 693                 /**
 694                  *No error msg here, since there is still hope with full halting sequence
 695                  */
 696                 err_check_propagate(retval);
 697         }
 698         if (eonce_status != NULL)
 699                 *eonce_status = data_read_from_dr;
 700         return retval;
 701 }
 702
 703 /**
 704  * Puts the core into debug mode, enabling the EOnCE module.
 705  *
 706  * @param target
 707  * @param eonce_status Data read from the EOnCE status register.
 708  *
 709  * @return
 710  */
 711 static int eonce_enter_debug_mode(struct target *target,
 712                                   uint16_t *eonce_status)
 713 {
 714         int retval = ERROR_OK;
 715
 716         uint32_t instr = JTAG_INSTR_DEBUG_REQUEST;
 717
 718         uint32_t ir_out;        /* not used, just to make jtag happy. */
 719
 720         uint16_t instr_16;
 721
 722         uint16_t read_16;
 723
 724         /* First try the easy way */
 725         retval = eonce_enter_debug_mode_without_reset(target, eonce_status);
 726         if (retval == ERROR_OK)
 727                 return retval;
 728
 729         struct jtag_tap *tap_chp;
 730
 731         struct jtag_tap *tap_cpu;
 732
 733         tap_chp = jtag_tap_by_string("dsp568013.chp");
 734         if (tap_chp == NULL) {
 735                 retval = ERROR_FAIL;
 736                 err_check(retval, DSP5680XX_ERROR_JTAG_TAP_FIND_MASTER,
 737                           "Failed to get master tap.");
 738         }
 739         tap_cpu = jtag_tap_by_string("dsp568013.cpu");
 740         if (tap_cpu == NULL) {
 741                 retval = ERROR_FAIL;
 742                 err_check(retval, DSP5680XX_ERROR_JTAG_TAP_FIND_CORE,
 743                           "Failed to get master tap.");
 744         }
 745         /* Enable master tap */
 746         tap_chp->enabled = true;
 747         tap_cpu->enabled = false;
 748
 749         instr = MASTER_TAP_CMD_IDCODE;
 750         retval =
 751                 dsp5680xx_irscan(target, &instr, &ir_out,
 752                                  DSP5680XX_JTAG_MASTER_TAP_IRLEN);
 753         err_check_propagate(retval);
 754         jtag_add_sleep(TIME_DIV_FREESCALE * 100 * 1000);
 755
 756         /* Enable EOnCE module */
 757         jtag_add_reset(0, 1);
 758         jtag_add_sleep(TIME_DIV_FREESCALE * 200 * 1000);
 759         instr = 0x0606ffff;     /* This was selected experimentally. */
 760         retval =
 761                 dsp5680xx_drscan(target, (uint8_t *) &instr, (uint8_t *) &ir_out,
 762                                  32);
 763         err_check_propagate(retval);
 764         /* ir_out now hold tap idcode */
 765
 766         /* Enable core tap */
 767         tap_chp->enabled = true;
 768         retval = switch_tap(target, tap_chp, tap_cpu);
 769         err_check_propagate(retval);
 770
 771         instr = JTAG_INSTR_ENABLE_ONCE;
 772         /* Two rounds of jtag 0x6  (enable eonce) to enable EOnCE. */
 773         retval =
 774                 dsp5680xx_irscan(target, &instr, &ir_out,
 775                                  DSP5680XX_JTAG_CORE_TAP_IRLEN);
 776         err_check_propagate(retval);
 777         instr = JTAG_INSTR_DEBUG_REQUEST;
 778         retval =
 779                 dsp5680xx_irscan(target, &instr, &ir_out,
 780                                  DSP5680XX_JTAG_CORE_TAP_IRLEN);
 781         err_check_propagate(retval);
 782         instr_16 = 0x1;
 783         retval =
 784                 dsp5680xx_drscan(target, (uint8_t *) &instr_16,
 785                                  (uint8_t *) &read_16, 8);
 786         err_check_propagate(retval);
 787         instr_16 = 0x20;
 788         retval =
 789                 dsp5680xx_drscan(target, (uint8_t *) &instr_16,
 790                                  (uint8_t *) &read_16, 8);
 791         err_check_propagate(retval);
 792         jtag_add_sleep(TIME_DIV_FREESCALE * 100 * 1000);
 793         jtag_add_reset(0, 0);
 794         jtag_add_sleep(TIME_DIV_FREESCALE * 300 * 1000);
 795
 796         instr = JTAG_INSTR_ENABLE_ONCE;
 797         /* Two rounds of jtag 0x6  (enable eonce) to enable EOnCE. */
 798         for (int i = 0; i < 3; i++) {
 799                 retval =
 800                         dsp5680xx_irscan(target, &instr, &ir_out,
 801                                          DSP5680XX_JTAG_CORE_TAP_IRLEN);
 802                 err_check_propagate(retval);
 803         }
 804         if ((ir_out & JTAG_STATUS_MASK) == JTAG_STATUS_DEBUG)
 805                 target->state = TARGET_HALTED;
 806         else {
 807                 retval = ERROR_FAIL;
 808                 err_check(retval, DSP5680XX_ERROR_HALT,
 809                           "Failed to halt target.");
 810         }
 811
 812         for (int i = 0; i < 3; i++) {
 813                 instr_16 = 0x86;
 814                 dsp5680xx_drscan(target, (uint8_t *) &instr_16,
 815                                  (uint8_t *) &read_16, 16);
 816                 instr_16 = 0xff;
 817                 dsp5680xx_drscan(target, (uint8_t *) &instr_16,
 818                                  (uint8_t *) &read_16, 16);
 819         }
 820
 821         /* Verify that debug mode is enabled */
 822         uint16_t data_read_from_dr;
 823
 824         retval = eonce_read_status_reg(target, &data_read_from_dr);
 825         err_check_propagate(retval);
 826         if ((data_read_from_dr & 0x30) == 0x30) {
 827                 LOG_DEBUG("EOnCE successfully entered debug mode.");
 828                 dsp5680xx_context.debug_mode_enabled = true;
 829                 retval = ERROR_OK;
 830         } else {
 831                 const char *msg = "Failed to set EOnCE module to debug mode";
 832
 833                 retval = ERROR_TARGET_FAILURE;
 834                 err_check(retval, DSP5680XX_ERROR_ENTER_DEBUG_MODE, msg);
 835         }
 836         if (eonce_status != NULL)
 837                 *eonce_status = data_read_from_dr;
 838         return retval;
 839 }
 840
 841 /**
 842  * Reads the current value of the program counter and stores it.
 843  *
 844  * @param target
 845  *
 846  * @return
 847  */
 848 static int eonce_pc_store(struct target *target)
 849 {
 850         uint8_t tmp[2];
 851
 852         int retval;
 853
 854         retval = core_move_pc_to_r4(target);
 855         err_check_propagate(retval);
 856         retval = core_move_r4_to_y(target);
 857         err_check_propagate(retval);
 858         retval = eonce_load_TX_RX_to_r0(target);
 859         err_check_propagate(retval);
 860         retval = core_move_y0_at_r0(target);
 861         err_check_propagate(retval);
 862         retval = core_rx_lower_data(target, tmp);
 863         err_check_propagate(retval);
 864         LOG_USER("PC value: 0x%X%X\n", tmp[1], tmp[0]);
 865         dsp5680xx_context.stored_pc = (tmp[0] | (tmp[1] << 8));
 866         return ERROR_OK;
 867 }
 868
 869 static int dsp5680xx_target_create(struct target *target, Jim_Interp *interp)
 870 {
 871         struct dsp5680xx_common *dsp5680xx =
 872                 calloc(1, sizeof(struct dsp5680xx_common));
 873         target->arch_info = dsp5680xx;
 874         return ERROR_OK;
 875 }
 876
 877 static int dsp5680xx_init_target(struct command_context *cmd_ctx,
 878                                  struct target *target)
 879 {
 880         dsp5680xx_context.stored_pc = 0;
 881         dsp5680xx_context.flush = 1;
 882         dsp5680xx_context.debug_mode_enabled = false;
 883         LOG_DEBUG("target initiated!");
 884         /* TODO core tap must be enabled before running these commands, currently
 885          * this is done in the .cfg tcl script. */
 886         return ERROR_OK;
 887 }
 888
 889 static int dsp5680xx_arch_state(struct target *target)
 890 {
 891         LOG_USER("%s not implemented yet.", __func__);
 892         return ERROR_OK;
 893 }
 894
 895 static int dsp5680xx_assert_reset(struct target *target)
 896 {
 897         target->state = TARGET_RESET;
 898         return ERROR_OK;
 899 }
 900
 901 static int dsp5680xx_deassert_reset(struct target *target)
 902 {
 903         target->state = TARGET_RUNNING;
 904         return ERROR_OK;
 905 }
 906
 907 static int dsp5680xx_halt(struct target *target)
 908 {
 909         int retval;
 910
 911         uint16_t eonce_status = 0xbeef;
 912
 913         if ((target->state == TARGET_HALTED)
 914             && (dsp5680xx_context.debug_mode_enabled)) {
 915                 LOG_USER("Target already halted and in debug mode.");
 916                 return ERROR_OK;
 917         } else {
 918                 if (target->state == TARGET_HALTED)
 919                         LOG_USER
 920                                 ("Target already halted, re attempting to enter debug mode.");
 921         }
 922         retval = eonce_enter_debug_mode(target, &eonce_status);
 923         err_check_propagate(retval);
 924         retval = eonce_pc_store(target);
 925         err_check_propagate(retval);
 926         if (dsp5680xx_context.debug_mode_enabled) {
 927                 retval = eonce_pc_store(target);
 928                 err_check_propagate(retval);
 929         }
 930         return retval;
 931 }
 932
 933 static int dsp5680xx_poll(struct target *target)
 934 {
 935         int retval;
 936
 937         uint8_t jtag_status;
 938
 939         uint8_t eonce_status;
 940
 941         uint16_t read_tmp;
 942
 943         retval = dsp5680xx_jtag_status(target, &jtag_status);
 944         err_check_propagate(retval);
 945         if (jtag_status == JTAG_STATUS_DEBUG)
 946                 if (target->state != TARGET_HALTED) {
 947                         retval = eonce_enter_debug_mode(target, &read_tmp);
 948                         err_check_propagate(retval);
 949                         eonce_status = (uint8_t) read_tmp;
 950                         if ((eonce_status & EONCE_STAT_MASK) !=
 951                             DSP5680XX_ONCE_OSCR_DEBUG_M) {
 952                                 const char *msg =
 953                                         "%s: Failed to put EOnCE in debug mode.Flash locked?...";
 954                                 LOG_WARNING(msg, __func__);
 955                                 return ERROR_TARGET_FAILURE;
 956                         } else {
 957                                 target->state = TARGET_HALTED;
 958                                 return ERROR_OK;
 959                         }
 960                 }
 961         if (jtag_status == JTAG_STATUS_NORMAL) {
 962                 if (target->state == TARGET_RESET) {
 963                         retval = dsp5680xx_halt(target);
 964                         err_check_propagate(retval);
 965                         retval = eonce_exit_debug_mode(target, &eonce_status);
 966                         err_check_propagate(retval);
 967                         if ((eonce_status & EONCE_STAT_MASK) !=
 968                             DSP5680XX_ONCE_OSCR_NORMAL_M) {
 969                                 const char *msg =
 970                                         "%s: JTAG running, but EOnCE run failed.Try resetting..";
 971                                 LOG_WARNING(msg, __func__);
 972                                 return ERROR_TARGET_FAILURE;
 973                         } else {
 974                                 target->state = TARGET_RUNNING;
 975                                 return ERROR_OK;
 976                         }
 977                 }
 978                 if (target->state != TARGET_RUNNING) {
 979                         retval = eonce_read_status_reg(target, &read_tmp);
 980                         err_check_propagate(retval);
 981                         eonce_status = (uint8_t) read_tmp;
 982                         if ((eonce_status & EONCE_STAT_MASK) !=
 983                             DSP5680XX_ONCE_OSCR_NORMAL_M) {
 984                                 LOG_WARNING
 985                                         ("Inconsistent target status. Restart!");
 986                                 return ERROR_TARGET_FAILURE;
 987                         }
 988                 }
 989                 target->state = TARGET_RUNNING;
 990                 return ERROR_OK;
 991         }
 992         if (jtag_status == JTAG_STATUS_DEAD) {
 993                 LOG_ERROR
 994                         ("%s: Cannot communicate with JTAG. Check connection...",
 995                          __func__);
 996                 target->state = TARGET_UNKNOWN;
 997                 return ERROR_TARGET_FAILURE;
 998         }
 999         if (target->state == TARGET_UNKNOWN) {
1000                 LOG_ERROR("%s: Target status invalid - communication failure",
1001                           __func__);
1002                 return ERROR_TARGET_FAILURE;
1003         }
1004         return ERROR_OK;
1005 }
1006
1007 static int dsp5680xx_resume(struct target *target, int current,
1008                             target_addr_t address, int hb, int d)
1009 {
1010         if (target->state == TARGET_RUNNING) {
1011                 LOG_USER("Target already running.");
1012                 return ERROR_OK;
1013         }
1014         int retval;
1015
1016         uint8_t eonce_status;
1017
1018         uint8_t jtag_status;
1019
1020         if (dsp5680xx_context.debug_mode_enabled) {
1021                 if (!current) {
1022                         retval = core_move_value_to_pc(target, address);
1023                         err_check_propagate(retval);
1024                 }
1025
1026                 int retry = 20;
1027
1028                 while (retry-- > 1) {
1029                         retval = eonce_exit_debug_mode(target, &eonce_status);
1030                         err_check_propagate(retval);
1031                         if (eonce_status == DSP5680XX_ONCE_OSCR_NORMAL_M)
1032                                 break;
1033                 }
1034                 if (retry == 0) {
1035                         retval = ERROR_TARGET_FAILURE;
1036                         err_check(retval, DSP5680XX_ERROR_EXIT_DEBUG_MODE,
1037                                   "Failed to exit debug mode...");
1038                 } else {
1039                         target->state = TARGET_RUNNING;
1040                         dsp5680xx_context.debug_mode_enabled = false;
1041                 }
1042                 LOG_DEBUG("EOnCE status: 0x%02X.", eonce_status);
1043         } else {
1044                 /*
1045                  * If debug mode was not enabled but target was halted, then it is most likely that
1046                  * access to eonce registers is locked.
1047                  * Reset target to make it run again.
1048                  */
1049                 jtag_add_reset(0, 1);
1050                 jtag_add_sleep(TIME_DIV_FREESCALE * 200 * 1000);
1051
1052                 retval = reset_jtag();
1053                 err_check(retval, DSP5680XX_ERROR_JTAG_RESET,
1054                           "Failed to reset JTAG state machine");
1055                 jtag_add_sleep(TIME_DIV_FREESCALE * 100 * 1000);
1056                 jtag_add_reset(0, 0);
1057                 jtag_add_sleep(TIME_DIV_FREESCALE * 300 * 1000);
1058                 retval = dsp5680xx_jtag_status(target, &jtag_status);
1059                 err_check_propagate(retval);
1060                 if ((jtag_status & JTAG_STATUS_MASK) == JTAG_STATUS_NORMAL) {
1061                         target->state = TARGET_RUNNING;
1062                         dsp5680xx_context.debug_mode_enabled = false;
1063                 } else {
1064                         retval = ERROR_TARGET_FAILURE;
1065                         err_check(retval, DSP5680XX_ERROR_RESUME,
1066                                   "Failed to resume target");
1067                 }
1068         }
1069         return ERROR_OK;
1070 }
1071
1072 /**
1073  * The value of @a address determines if it corresponds to P: (program) or X: (dat) memory.
1074  * If the address is over 0x200000 then it is considered X: memory, and @a pmem = 0.
1075  * The special case of 0xFFXXXX is not modified, since it allows to read out the
1076  * memory mapped EOnCE registers.
1077  *
1078  * @param address
1079  * @param pmem
1080  *
1081  * @return
1082  */
1083 static int dsp5680xx_convert_address(uint32_t *address, int *pmem)
1084 {
1085         /*
1086          * Distinguish data memory (x) from program memory (p) by the address.
1087          * Addresses over S_FILE_DATA_OFFSET are considered (x) memory.
1088          */
1089         if (*address >= S_FILE_DATA_OFFSET) {
1090                 *pmem = 0;
1091                 if (((*address) & 0xff0000) != 0xff0000)
1092                         *address -= S_FILE_DATA_OFFSET;
1093         }
1094         return ERROR_OK;
1095 }
1096
1097 static int dsp5680xx_read_16_single(struct target *t, uint32_t a,
1098                                     uint8_t *data_read, int r_pmem)
1099 {
1100         struct target *target = t;
1101
1102         uint32_t address = a;
1103
1104         int retval;
1105
1106         retval = core_move_long_to_r0(target, address);
1107         err_check_propagate(retval);
1108         if (r_pmem)
1109                 retval = core_move_at_pr0_inc_to_y0(target);
1110         else
1111                 retval = core_move_at_r0_to_y0(target);
1112         err_check_propagate(retval);
1113         retval = eonce_load_TX_RX_to_r0(target);
1114         err_check_propagate(retval);
1115         retval = core_move_y0_at_r0(target);
1116         err_check_propagate(retval);
1117         /* at this point the data i want is at the reg eonce can read */
1118         retval = core_rx_lower_data(target, data_read);
1119         err_check_propagate(retval);
1120         LOG_DEBUG("%s:Data read from 0x%06" PRIX32 ": 0x%02X%02X", __func__, address,
1121                   data_read[1], data_read[0]);
1122         return retval;
1123 }
1124
1125 static int dsp5680xx_read_32_single(struct target *t, uint32_t a,
1126                                     uint8_t *data_read, int r_pmem)
1127 {
1128         struct target *target = t;
1129
1130         uint32_t address = a;
1131
1132         int retval;
1133
1134         address = (address & 0xFFFFF);
1135         /* Get data to an intermediate register */
1136         retval = core_move_long_to_r0(target, address);
1137         err_check_propagate(retval);
1138         if (r_pmem) {
1139                 retval = core_move_at_pr0_inc_to_y0(target);
1140                 err_check_propagate(retval);
1141                 retval = core_move_at_pr0_inc_to_y1(target);
1142                 err_check_propagate(retval);
1143         } else {
1144                 retval = core_move_at_r0_inc_to_y0(target);
1145                 err_check_propagate(retval);
1146                 retval = core_move_at_r0_to_y1(target);
1147                 err_check_propagate(retval);
1148         }
1149         /* Get lower part of data to TX/RX */
1150         retval = eonce_load_TX_RX_to_r0(target);
1151         err_check_propagate(retval);
1152         retval = core_move_y0_at_r0_inc(target);    /* This also load TX/RX high to r0 */
1153         err_check_propagate(retval);
1154         /* Get upper part of data to TX/RX */
1155         retval = core_move_y1_at_r0(target);
1156         err_check_propagate(retval);
1157         /* at this point the data i want is at the reg eonce can read */
1158         retval = core_rx_lower_data(target, data_read);
1159         err_check_propagate(retval);
1160         retval = core_rx_upper_data(target, data_read + 2);
1161         err_check_propagate(retval);
1162         return retval;
1163 }
1164
1165 static int dsp5680xx_read(struct target *t, target_addr_t a, uint32_t size,
1166                           uint32_t count, uint8_t *buf)
1167 {
1168         struct target *target = t;
1169
1170         uint32_t address = a;
1171
1172         uint8_t *buffer = buf;
1173
1174         check_halt_and_debug(target);
1175
1176         int retval = ERROR_OK;
1177
1178         int pmem = 1;
1179
1180         retval = dsp5680xx_convert_address(&address, &pmem);
1181         err_check_propagate(retval);
1182
1183         dsp5680xx_context.flush = 0;
1184         int counter = FLUSH_COUNT_READ_WRITE;
1185
1186         for (unsigned i = 0; i < count; i++) {
1187                 if (--counter == 0) {
1188                         dsp5680xx_context.flush = 1;
1189                         counter = FLUSH_COUNT_READ_WRITE;
1190                 }
1191                 switch (size) {
1192                 case 1:
1193                         if (!(i % 2))
1194                                 retval =
1195                                         dsp5680xx_read_16_single(target,
1196                                                                  address + i / 2,
1197                                                                  buffer + i, pmem);
1198                         break;
1199                 case 2:
1200                         retval =
1201                                 dsp5680xx_read_16_single(target, address + i,
1202                                                          buffer + 2 * i, pmem);
1203                         break;
1204                 case 4:
1205                         retval =
1206                                 dsp5680xx_read_32_single(target, address + 2 * i,
1207                                                          buffer + 4 * i, pmem);
1208                         break;
1209                 default:
1210                         LOG_USER("%s: Invalid read size.", __func__);
1211                         break;
1212                 }
1213                 err_check_propagate(retval);
1214                 dsp5680xx_context.flush = 0;
1215         }
1216
1217         dsp5680xx_context.flush = 1;
1218         retval = dsp5680xx_execute_queue();
1219         err_check_propagate(retval);
1220
1221         return retval;
1222 }
1223
1224 static int dsp5680xx_write_16_single(struct target *t, uint32_t a,
1225                                      uint16_t data, uint8_t w_pmem)
1226 {
1227         struct target *target = t;
1228
1229         uint32_t address = a;
1230
1231         int retval = 0;
1232
1233         retval = core_move_long_to_r0(target, address);
1234         err_check_propagate(retval);
1235         if (w_pmem) {
1236                 retval = core_move_value_to_y0(target, data);
1237                 err_check_propagate(retval);
1238                 retval = core_move_y0_at_pr0_inc(target);
1239                 err_check_propagate(retval);
1240         } else {
1241                 retval = core_move_value_at_r0(target, data);
1242                 err_check_propagate(retval);
1243         }
1244         return retval;
1245 }
1246
1247 static int dsp5680xx_write_32_single(struct target *t, uint32_t a,
1248                                      uint32_t data, int w_pmem)
1249 {
1250         struct target *target = t;
1251
1252         uint32_t address = a;
1253
1254         int retval = ERROR_OK;
1255
1256         retval = core_move_long_to_r0(target, address);
1257         err_check_propagate(retval);
1258         retval = core_move_long_to_y(target, data);
1259         err_check_propagate(retval);
1260         if (w_pmem)
1261                 retval = core_move_y0_at_pr0_inc(target);
1262         else
1263                 retval = core_move_y0_at_r0_inc(target);
1264         err_check_propagate(retval);
1265         if (w_pmem)
1266                 retval = core_move_y1_at_pr0_inc(target);
1267         else
1268                 retval = core_move_y1_at_r0_inc(target);
1269         err_check_propagate(retval);
1270         return retval;
1271 }
1272
1273 static int dsp5680xx_write_8(struct target *t, uint32_t a, uint32_t c,
1274                              const uint8_t *d, int pmem)
1275 {
1276         struct target *target = t;
1277
1278         uint32_t address = a;
1279
1280         uint32_t count = c;
1281
1282         const uint8_t *data = d;
1283
1284         int retval = 0;
1285
1286         uint16_t data_16;
1287
1288         uint32_t iter;
1289
1290         int counter = FLUSH_COUNT_READ_WRITE;
1291
1292         for (iter = 0; iter < count / 2; iter++) {
1293                 if (--counter == 0) {
1294                         dsp5680xx_context.flush = 1;
1295                         counter = FLUSH_COUNT_READ_WRITE;
1296                 }
1297                 data_16 = (data[2 * iter] | (data[2 * iter + 1] << 8));
1298                 retval =
1299                         dsp5680xx_write_16_single(target, address + iter, data_16,
1300                                                   pmem);
1301                 if (retval != ERROR_OK) {
1302                         LOG_ERROR("%s: Could not write to p:0x%04" PRIX32, __func__,
1303                                   address);
1304                         dsp5680xx_context.flush = 1;
1305                         return retval;
1306                 }
1307                 dsp5680xx_context.flush = 0;
1308         }
1309         dsp5680xx_context.flush = 1;
1310
1311         /* Only one byte left, let's not overwrite the other byte (mem is 16bit) */
1312         /* Need to retrieve the part we do not want to overwrite. */
1313         uint16_t data_old;
1314
1315         if ((count == 1) || (count % 2)) {
1316                 retval =
1317                         dsp5680xx_read(target, address + iter, 1, 1,
1318                                        (uint8_t *) &data_old);
1319                 err_check_propagate(retval);
1320                 if (count == 1)
1321                         data_old = (((data_old & 0xff) << 8) | data[0]); /* preserve upper byte */
1322                 else
1323                         data_old =
1324                                 (((data_old & 0xff) << 8) | data[2 * iter + 1]);
1325                 retval =
1326                         dsp5680xx_write_16_single(target, address + iter, data_old,
1327                                                   pmem);
1328                 err_check_propagate(retval);
1329         }
1330         return retval;
1331 }
1332
1333 static int dsp5680xx_write_16(struct target *t, uint32_t a, uint32_t c,
1334                               const uint8_t *d, int pmem)
1335 {
1336         struct target *target = t;
1337
1338         uint32_t address = a;
1339
1340         uint32_t count = c;
1341
1342         const uint8_t *data = d;
1343
1344         int retval = ERROR_OK;
1345
1346         uint32_t iter;
1347
1348         int counter = FLUSH_COUNT_READ_WRITE;
1349
1350         for (iter = 0; iter < count; iter++) {
1351                 if (--counter == 0) {
1352                         dsp5680xx_context.flush = 1;
1353                         counter = FLUSH_COUNT_READ_WRITE;
1354                 }
1355                 retval =
1356                         dsp5680xx_write_16_single(target, address + iter,
1357                                                   data[iter], pmem);
1358                 if (retval != ERROR_OK) {
1359                         LOG_ERROR("%s: Could not write to p:0x%04" PRIX32, __func__,
1360                                   address);
1361                         dsp5680xx_context.flush = 1;
1362                         return retval;
1363                 }
1364                 dsp5680xx_context.flush = 0;
1365         }
1366         dsp5680xx_context.flush = 1;
1367         return retval;
1368 }
1369
1370 static int dsp5680xx_write_32(struct target *t, uint32_t a, uint32_t c,
1371                               const uint8_t *d, int pmem)
1372 {
1373         struct target *target = t;
1374
1375         uint32_t address = a;
1376
1377         uint32_t count = c;
1378
1379         const uint8_t *data = d;
1380
1381         int retval = ERROR_OK;
1382
1383         uint32_t iter;
1384
1385         int counter = FLUSH_COUNT_READ_WRITE;
1386
1387         for (iter = 0; iter < count; iter++) {
1388                 if (--counter == 0) {
1389                         dsp5680xx_context.flush = 1;
1390                         counter = FLUSH_COUNT_READ_WRITE;
1391                 }
1392                 retval =
1393                         dsp5680xx_write_32_single(target, address + (iter << 1),
1394                                                   data[iter], pmem);
1395                 if (retval != ERROR_OK) {
1396                         LOG_ERROR("%s: Could not write to p:0x%04" PRIX32, __func__,
1397                                   address);
1398                         dsp5680xx_context.flush = 1;
1399                         return retval;
1400                 }
1401                 dsp5680xx_context.flush = 0;
1402         }
1403         dsp5680xx_context.flush = 1;
1404         return retval;
1405 }
1406
1407 /**
1408  * Writes @a buffer to memory.
1409  * The parameter @a address determines whether @a buffer should be written to
1410  * P: (program) memory or X: (dat) memory.
1411  *
1412  * @param target
1413  * @param address
1414  * @param size Bytes (1), Half words (2), Words (4).
1415  * @param count In bytes.
1416  * @param buffer
1417  *
1418  * @return
1419  */
1420 static int dsp5680xx_write(struct target *t, target_addr_t a, uint32_t s, uint32_t c,
1421                            const uint8_t *b)
1422 {
1423         /* TODO Cannot write 32bit to odd address, will write 0x12345678  as 0x5678 0x0012 */
1424         struct target *target = t;
1425
1426         uint32_t address = a;
1427
1428         uint32_t count = c;
1429
1430         uint8_t const *buffer = b;
1431
1432         uint32_t size = s;
1433
1434         check_halt_and_debug(target);
1435
1436         int retval = 0;
1437
1438         int p_mem = 1;
1439
1440         retval = dsp5680xx_convert_address(&address, &p_mem);
1441         err_check_propagate(retval);
1442
1443         switch (size) {
1444         case 1:
1445                 retval =
1446                         dsp5680xx_write_8(target, address, count, buffer, p_mem);
1447                 break;
1448         case 2:
1449                 retval =
1450                         dsp5680xx_write_16(target, address, count, buffer, p_mem);
1451                 break;
1452         case 4:
1453                 retval =
1454                         dsp5680xx_write_32(target, address, count, buffer, p_mem);
1455                 break;
1456         default:
1457                 retval = ERROR_TARGET_DATA_ABORT;
1458                 err_check(retval, DSP5680XX_ERROR_INVALID_DATA_SIZE_UNIT,
1459                           "Invalid data size.");
1460                 break;
1461         }
1462         return retval;
1463 }
1464
1465 static int dsp5680xx_write_buffer(struct target *t, target_addr_t a, uint32_t size,
1466                                   const uint8_t *b)
1467 {
1468         check_halt_and_debug(t);
1469         return dsp5680xx_write(t, a, 1, size, b);
1470 }
1471
1472 /**
1473  * This function is called by verify_image, it is used to read data from memory.
1474  *
1475  * @param target
1476  * @param address Word addressing.
1477  * @param size In bytes.
1478  * @param buffer
1479  *
1480  * @return
1481  */
1482 static int dsp5680xx_read_buffer(struct target *t, target_addr_t a, uint32_t size,
1483                                  uint8_t *buf)
1484 {
1485         check_halt_and_debug(t);
1486         /* The "/2" solves the byte/word addressing issue.*/
1487         return dsp5680xx_read(t, a, 2, size / 2, buf);
1488 }
1489
1490 /**
1491  * This function is not implemented.
1492  * It returns an error in order to get OpenOCD to do read out the data
1493  * and calculate the CRC, or try a binary comparison.
1494  *
1495  * @param target
1496  * @param address Start address of the image.
1497  * @param size In bytes.
1498  * @param checksum
1499  *
1500  * @return
1501  */
1502 static int dsp5680xx_checksum_memory(struct target *t, target_addr_t a, uint32_t s,
1503                                      uint32_t *checksum)
1504 {
1505         return ERROR_FAIL;
1506 }
1507
1508 /**
1509  * Calculates a signature over @a word_count words in the data from @a buff16.
1510  * The algorithm used is the same the FM uses, so the @a return may be used to compare
1511  * with the one generated by the FM module, and check if flashing was successful.
1512  * This algorithm is based on the perl script available from the Freescale website at FAQ 25630.
1513  *
1514  * @param buff16
1515  * @param word_count
1516  *
1517  * @return
1518  */
1519 static int perl_crc(const uint8_t *buff8, uint32_t word_count)
1520 {
1521         uint16_t checksum = 0xffff;
1522
1523         uint16_t data, fbmisr;
1524
1525         uint32_t i;
1526
1527         for (i = 0; i < word_count; i++) {
1528                 data = (buff8[2 * i] | (buff8[2 * i + 1] << 8));
1529                 fbmisr =
1530                         (checksum & 2) >> 1 ^ (checksum & 4) >> 2 ^ (checksum & 16)
1531                                 >> 4 ^ (checksum & 0x8000) >> 15;
1532                 checksum = (data ^ ((checksum << 1) | fbmisr));
1533         }
1534         i--;
1535         for (; !(i & 0x80000000); i--) {
1536                 data = (buff8[2 * i] | (buff8[2 * i + 1] << 8));
1537                 fbmisr =
1538                         (checksum & 2) >> 1 ^ (checksum & 4) >> 2 ^ (checksum & 16)
1539                                        >> 4 ^ (checksum & 0x8000) >> 15;
1540                 checksum = (data ^ ((checksum << 1) | fbmisr));
1541         }
1542         return checksum;
1543 }
1544
1545 /**
1546  * Resets the SIM. (System Integration Modul).
1547  *
1548  * @param target
1549  *
1550  * @return
1551  */
1552 static int dsp5680xx_f_SIM_reset(struct target *target)
1553 {
1554         int retval = ERROR_OK;
1555
1556         uint16_t sim_cmd = SIM_CMD_RESET;
1557
1558         uint32_t sim_addr;
1559
1560         if (strcmp(target->tap->chip, "dsp568013") == 0) {
1561                 sim_addr = MC568013_SIM_BASE_ADDR + S_FILE_DATA_OFFSET;
1562                 retval =
1563                         dsp5680xx_write(target, sim_addr, 1, 2,
1564                                         (const uint8_t *)&sim_cmd);
1565                 err_check_propagate(retval);
1566         }
1567         return retval;
1568 }
1569
1570 /**
1571  * Halts the core and resets the SIM. (System Integration Modul).
1572  *
1573  * @param target
1574  *
1575  * @return
1576  */
1577 static int dsp5680xx_soft_reset_halt(struct target *target)
1578 {
1579         /* TODO is this what this function is expected to do...? */
1580         int retval;
1581
1582         retval = dsp5680xx_halt(target);
1583         err_check_propagate(retval);
1584         retval = dsp5680xx_f_SIM_reset(target);
1585         err_check_propagate(retval);
1586         return retval;
1587 }
1588
1589 int dsp5680xx_f_protect_check(struct target *target, uint16_t *protected)
1590 {
1591         int retval;
1592
1593         check_halt_and_debug(target);
1594         if (protected == NULL) {
1595                 const char *msg = "NULL pointer not valid.";
1596
1597                 err_check(ERROR_FAIL,
1598                           DSP5680XX_ERROR_PROTECT_CHECK_INVALID_ARGS, msg);
1599         }
1600         retval =
1601                 dsp5680xx_read_16_single(target, HFM_BASE_ADDR | HFM_PROT,
1602                                          (uint8_t *) protected, 0);
1603         err_check_propagate(retval);
1604         return retval;
1605 }
1606
1607 /**
1608  * Executes a command on the FM module.
1609  * Some commands use the parameters @a address and @a data, others ignore them.
1610  *
1611  * @param target
1612  * @param command Command to execute.
1613  * @param address Command parameter.
1614  * @param data Command parameter.
1615  * @param hfm_ustat FM status register.
1616  * @param pmem Address is P: (program) memory (@a pmem == 1) or X: (dat) memory (@a pmem == 0)
1617  *
1618  * @return
1619  */
1620 static int dsp5680xx_f_ex(struct target *t, uint16_t c, uint32_t a, uint32_t d,
1621                           uint16_t *h, int p)
1622 {
1623         struct target *target = t;
1624
1625         uint32_t command = c;
1626
1627         uint32_t address = a;
1628
1629         uint32_t data = d;
1630
1631         uint16_t *hfm_ustat = h;
1632
1633         int pmem = p;
1634
1635         int retval;
1636
1637         retval = core_load_TX_RX_high_addr_to_r0(target);
1638         err_check_propagate(retval);
1639         retval = core_move_long_to_r2(target, HFM_BASE_ADDR);
1640         err_check_propagate(retval);
1641         uint8_t i[2];
1642
1643         int watchdog = 100;
1644
1645         do {
1646                 retval = core_move_at_r2_disp_to_y0(target, HFM_USTAT); /* read HMF_USTAT */
1647                 err_check_propagate(retval);
1648                 retval = core_move_y0_at_r0(target);
1649                 err_check_propagate(retval);
1650                 retval = core_rx_upper_data(target, i);
1651                 err_check_propagate(retval);
1652                 if ((watchdog--) == 1) {
1653                         retval = ERROR_TARGET_FAILURE;
1654                         const char *msg =
1655                                 "Timed out waiting for FM to finish old command.";
1656                         err_check(retval, DSP5680XX_ERROR_FM_BUSY, msg);
1657                 }
1658         } while (!(i[0] & 0x40)); /* wait until current command is complete */
1659
1660         dsp5680xx_context.flush = 0;
1661
1662         /* write to HFM_CNFG (lock=0,select bank) - flash_desc.bank&0x03, 0x01 == 0x00, 0x01 ??? */
1663         retval = core_move_value_at_r2_disp(target, 0x00, HFM_CNFG);
1664         err_check_propagate(retval);
1665         /* write to HMF_USTAT, clear PVIOL, ACCERR &BLANK bits */
1666         retval = core_move_value_at_r2_disp(target, 0x04, HFM_USTAT);
1667         err_check_propagate(retval);
1668         /* clear only one bit at a time */
1669         retval = core_move_value_at_r2_disp(target, 0x10, HFM_USTAT);
1670         err_check_propagate(retval);
1671         retval = core_move_value_at_r2_disp(target, 0x20, HFM_USTAT);
1672         err_check_propagate(retval);
1673         /* write to HMF_PROT, clear protection */
1674         retval = core_move_value_at_r2_disp(target, 0x00, HFM_PROT);
1675         err_check_propagate(retval);
1676         /* write to HMF_PROTB, clear protection */
1677         retval = core_move_value_at_r2_disp(target, 0x00, HFM_PROTB);
1678         err_check_propagate(retval);
1679         retval = core_move_value_to_y0(target, data);
1680         err_check_propagate(retval);
1681         /* write to the flash block */
1682         retval = core_move_long_to_r3(target, address);
1683         err_check_propagate(retval);
1684         if (pmem) {
1685                 retval = core_move_y0_at_pr3_inc(target);
1686                 err_check_propagate(retval);
1687         } else {
1688                 retval = core_move_y0_at_r3(target);
1689                 err_check_propagate(retval);
1690         }
1691         /* write command to the HFM_CMD reg */
1692         retval = core_move_value_at_r2_disp(target, command, HFM_CMD);
1693         err_check_propagate(retval);
1694         /* start the command */
1695         retval = core_move_value_at_r2_disp(target, 0x80, HFM_USTAT);
1696         err_check_propagate(retval);
1697
1698         dsp5680xx_context.flush = 1;
1699         retval = dsp5680xx_execute_queue();
1700         err_check_propagate(retval);
1701
1702         watchdog = 100;
1703         do {
1704                 /* read HMF_USTAT */
1705                 retval = core_move_at_r2_disp_to_y0(target, HFM_USTAT);
1706                 err_check_propagate(retval);
1707                 retval = core_move_y0_at_r0(target);
1708                 err_check_propagate(retval);
1709                 retval = core_rx_upper_data(target, i);
1710                 err_check_propagate(retval);
1711                 if ((watchdog--) == 1) {
1712                         retval = ERROR_TARGET_FAILURE;
1713                         err_check(retval, DSP5680XX_ERROR_FM_CMD_TIMED_OUT,
1714                                   "FM execution did not finish.");
1715                 }
1716         } while (!(i[0] & 0x40)); /* wait until the command is complete */
1717         *hfm_ustat = ((i[0] << 8) | (i[1]));
1718         if (i[0] & HFM_USTAT_MASK_PVIOL_ACCER) {
1719                 retval = ERROR_TARGET_FAILURE;
1720                 const char *msg =
1721                         "pviol and/or accer bits set. HFM command execution error";
1722                 err_check(retval, DSP5680XX_ERROR_FM_EXEC, msg);
1723         }
1724         return ERROR_OK;
1725 }
1726
1727 /**
1728  * Prior to the execution of any Flash module command, the Flash module Clock
1729  * Divider (CLKDIV) register must be initialized. The values of this register
1730  * determine the speed of the internal Flash Clock (FCLK). FCLK must be in the
1731  * range of 150kHz ≤ FCLK ≤ 200kHz for proper operation of the Flash module.
1732  * (Running FCLK too slowly wears out the module, while running it too fast
1733  * under programs Flash leading to bit errors.)
1734  *
1735  * @param target
1736  *
1737  * @return
1738  */
1739 static int set_fm_ck_div(struct target *target)
1740 {
1741         uint8_t i[2];
1742
1743         int retval;
1744
1745         retval = core_move_long_to_r2(target, HFM_BASE_ADDR);
1746         err_check_propagate(retval);
1747         retval = core_load_TX_RX_high_addr_to_r0(target);
1748         err_check_propagate(retval);
1749         /* read HFM_CLKD */
1750         retval = core_move_at_r2_to_y0(target);
1751         err_check_propagate(retval);
1752         retval = core_move_y0_at_r0(target);
1753         err_check_propagate(retval);
1754         retval = core_rx_upper_data(target, i);
1755         err_check_propagate(retval);
1756         unsigned int hfm_at_wrong_value = 0;
1757
1758         if ((i[0] & 0x7f) != HFM_CLK_DEFAULT) {
1759                 LOG_DEBUG("HFM CLK divisor contained incorrect value (0x%02X).",
1760                           i[0] & 0x7f);
1761                 hfm_at_wrong_value = 1;
1762         } else {
1763                 LOG_DEBUG
1764                         ("HFM CLK divisor was already set to correct value (0x%02X).",
1765                          i[0] & 0x7f);
1766                 return ERROR_OK;
1767         }
1768         /* write HFM_CLKD */
1769         retval = core_move_value_at_r2(target, HFM_CLK_DEFAULT);
1770         err_check_propagate(retval);
1771         /* verify HFM_CLKD */
1772         retval = core_move_at_r2_to_y0(target);
1773         err_check_propagate(retval);
1774         retval = core_move_y0_at_r0(target);
1775         err_check_propagate(retval);
1776         retval = core_rx_upper_data(target, i);
1777         err_check_propagate(retval);
1778         if (i[0] != (0x80 | (HFM_CLK_DEFAULT & 0x7f))) {
1779                 retval = ERROR_TARGET_FAILURE;
1780                 err_check(retval, DSP5680XX_ERROR_FM_SET_CLK,
1781                           "Unable to set HFM CLK divisor.");
1782         }
1783         if (hfm_at_wrong_value)
1784                 LOG_DEBUG("HFM CLK divisor set to 0x%02x.", i[0] & 0x7f);
1785         return ERROR_OK;
1786 }
1787
1788 /**
1789  * Executes the FM calculate signature command. The FM will calculate over the
1790  * data from @a address to @a address + @a words -1. The result is written to a
1791  * register, then read out by this function and returned in @a signature. The
1792  * value @a signature may be compared to the one returned by perl_crc to
1793  * verify the flash was written correctly.
1794  *
1795  * @param target
1796  * @param address Start of flash array where the signature should be calculated.
1797  * @param words Number of words over which the signature should be calculated.
1798  * @param signature Value calculated by the FM.
1799  *
1800  * @return
1801  */
1802 static int dsp5680xx_f_signature(struct target *t, uint32_t a, uint32_t words,
1803                                  uint16_t *signature)
1804 {
1805         struct target *target = t;
1806
1807         uint32_t address = a;
1808
1809         int retval;
1810
1811         uint16_t hfm_ustat;
1812
1813         if (!dsp5680xx_context.debug_mode_enabled) {
1814                 retval = eonce_enter_debug_mode_without_reset(target, NULL);
1815                 /*
1816                  * Generate error here, since it is not done in eonce_enter_debug_mode_without_reset
1817                  */
1818                 err_check(retval, DSP5680XX_ERROR_HALT,
1819                           "Failed to halt target.");
1820         }
1821         retval =
1822                 dsp5680xx_f_ex(target, HFM_CALCULATE_DATA_SIGNATURE, address, words,
1823                                &hfm_ustat, 1);
1824         err_check_propagate(retval);
1825         retval =
1826                 dsp5680xx_read_16_single(target, HFM_BASE_ADDR | HFM_DATA,
1827                                          (uint8_t *) signature, 0);
1828         return retval;
1829 }
1830
1831 int dsp5680xx_f_erase_check(struct target *target, uint8_t *erased,
1832                             uint32_t sector)
1833 {
1834         int retval;
1835
1836         uint16_t hfm_ustat;
1837
1838         uint32_t tmp;
1839
1840         if (!dsp5680xx_context.debug_mode_enabled) {
1841                 retval = dsp5680xx_halt(target);
1842                 err_check_propagate(retval);
1843         }
1844         retval = set_fm_ck_div(target);
1845         err_check_propagate(retval);
1846         /*
1847          * Check if chip is already erased.
1848          */
1849         tmp = HFM_FLASH_BASE_ADDR + sector * HFM_SECTOR_SIZE / 2;
1850         retval =
1851                 dsp5680xx_f_ex(target, HFM_ERASE_VERIFY, tmp, 0, &hfm_ustat, 1);
1852         err_check_propagate(retval);
1853         if (erased != NULL)
1854                 *erased = (uint8_t) (hfm_ustat & HFM_USTAT_MASK_BLANK);
1855         return retval;
1856 }
1857
1858 /**
1859  * Executes the FM page erase command.
1860  *
1861  * @param target
1862  * @param sector Page to erase.
1863  * @param hfm_ustat FM module status register.
1864  *
1865  * @return
1866  */
1867 static int erase_sector(struct target *target, int sector, uint16_t *hfm_ustat)
1868 {
1869         int retval;
1870
1871         uint32_t tmp = HFM_FLASH_BASE_ADDR + sector * HFM_SECTOR_SIZE / 2;
1872
1873         retval = dsp5680xx_f_ex(target, HFM_PAGE_ERASE, tmp, 0, hfm_ustat, 1);
1874         err_check_propagate(retval);
1875         return retval;
1876 }
1877
1878 /**
1879  * Executes the FM mass erase command. Erases the flash array completely.
1880  *
1881  * @param target
1882  * @param hfm_ustat FM module status register.
1883  *
1884  * @return
1885  */
1886 static int mass_erase(struct target *target, uint16_t *hfm_ustat)
1887 {
1888         int retval;
1889
1890         retval = dsp5680xx_f_ex(target, HFM_MASS_ERASE, 0, 0, hfm_ustat, 1);
1891         return retval;
1892 }
1893
1894 int dsp5680xx_f_erase(struct target *target, int first, int last)
1895 {
1896         int retval;
1897
1898         if (!dsp5680xx_context.debug_mode_enabled) {
1899                 retval = dsp5680xx_halt(target);
1900                 err_check_propagate(retval);
1901         }
1902         /*
1903          * Reset SIM
1904          *
1905          */
1906         retval = dsp5680xx_f_SIM_reset(target);
1907         err_check_propagate(retval);
1908         /*
1909          * Set hfmdiv
1910          *
1911          */
1912         retval = set_fm_ck_div(target);
1913         err_check_propagate(retval);
1914
1915         uint16_t hfm_ustat;
1916
1917         int do_mass_erase = ((!(first | last))
1918                              || ((first == 0)
1919                                  && (last == (HFM_SECTOR_COUNT - 1))));
1920         if (do_mass_erase) {
1921                 /* Mass erase */
1922                 retval = mass_erase(target, &hfm_ustat);
1923                 err_check_propagate(retval);
1924         } else {
1925                 for (int i = first; i <= last; i++) {
1926                         retval = erase_sector(target, i, &hfm_ustat);
1927                         err_check_propagate(retval);
1928                 }
1929         }
1930         return ERROR_OK;
1931 }
1932
1933 /*
1934  * Algorithm for programming normal p: flash
1935  * Follow state machine from "56F801x Peripheral Reference Manual"@163.
1936  * Registers to set up before calling:
1937  * r0: TX/RX high address.
1938  * r2: FM module base address.
1939  * r3: Destination address in flash.
1940  *
1941  *              hfm_wait:                                          // wait for buffer empty
1942  *                      brclr   #0x80, x:(r2+0x13), hfm_wait
1943  *              rx_check:                                           // wait for input buffer full
1944  *                      brclr   #0x01, x:(r0-2), rx_check
1945  *                      move.w  x:(r0), y0                          // read from Rx buffer
1946  *                      move.w  y0, p:(r3)+
1947  *                      move.w  #0x20, x:(r2+0x14)                  // write PGM command
1948  *                      move.w  #0x80, x:(r2+0x13)                  // start the command
1949  *                      move.w  X:(R2+0x13), A                      // Read USTAT register
1950  *                    brclr       #0x20, A, accerr_check             // protection violation check
1951  *                    bfset       #0x20, X:(R2+0x13)            // clear pviol
1952  *                    bra        hfm_wait
1953  *            accerr_check:
1954  *                    brclr       #0x10, A, hfm_wait             // access error check
1955  *                    bfset       #0x10, X:(R2+0x13)            // clear accerr
1956  *                      bra         hfm_wait                        // loop
1957  * 0x00000000  0x8A460013807D    brclr       #0x80, X:(R2+0x13),*+0
1958  * 0x00000003  0xE700            nop
1959  * 0x00000004  0xE700            nop
1960  * 0x00000005  0x8A44FFFE017B    brclr       #1, X:(R0-2),*-2
1961  * 0x00000008  0xE700            nop
1962  * 0x00000009  0xF514            move.w      X:(R0), Y0
1963  * 0x0000000A  0x8563            move.w      Y0, P:(R3)+
1964  * 0x0000000B  0x864600200014    move.w      #32, X:(R2+0x14)
1965  * 0x0000000E  0x864600800013    move.w      #128, X:(R2+0x13)
1966  * 0x00000011  0xF0420013            move.w      X:(R2+0x13), A
1967  * 0x00000013  0x8B402004            brclr       #0x20, A,*+6
1968  * 0x00000015  0x824600130020    bfset       #0x20, X:(R2+0x13)
1969  * 0x00000018  0xA967            bra     *-24
1970  * 0x00000019  0x8B401065            brclr       #0x10, A,*-25
1971  * 0x0000001B  0x824600130010    bfset       #0x10, X:(R2+0x13)
1972  * 0x0000001E  0xA961            bra     *-30
1973  */
1974
1975 static const uint16_t pgm_write_pflash[] = {
1976                 0x8A46, 0x0013, 0x807D, 0xE700,
1977                 0xE700, 0x8A44, 0xFFFE, 0x017B,
1978                 0xE700, 0xF514, 0x8563, 0x8646,
1979                 0x0020, 0x0014, 0x8646, 0x0080,
1980                 0x0013, 0xF042, 0x0013, 0x8B40,
1981                 0x2004, 0x8246, 0x0013, 0x0020,
1982                 0xA967, 0x8B40, 0x1065, 0x8246,
1983                 0x0013, 0x0010, 0xA961
1984 };
1985
1986 static const uint32_t pgm_write_pflash_length = 31;
1987
1988 int dsp5680xx_f_wr(struct target *t, const uint8_t *b, uint32_t a, uint32_t count,
1989                    int is_flash_lock)
1990 {
1991         struct target *target = t;
1992
1993         uint32_t address = a;
1994
1995         const uint8_t *buffer = b;
1996
1997         int retval = ERROR_OK;
1998
1999         if (!dsp5680xx_context.debug_mode_enabled) {
2000                 retval = eonce_enter_debug_mode(target, NULL);
2001                 err_check_propagate(retval);
2002         }
2003         /*
2004          * Download the pgm that flashes.
2005          *
2006          */
2007         const uint32_t len = pgm_write_pflash_length;
2008
2009         uint32_t ram_addr = 0x8700;
2010
2011         /*
2012          * This seems to be a safe address.
2013          * This one is the one used by codewarrior in 56801x_flash.cfg
2014          */
2015         if (!is_flash_lock) {
2016                 retval =
2017                         dsp5680xx_write(target, ram_addr, 1, len * 2,
2018                                         (uint8_t *) pgm_write_pflash);
2019                 err_check_propagate(retval);
2020                 retval = dsp5680xx_execute_queue();
2021                 err_check_propagate(retval);
2022         }
2023         /*
2024          * Set hfmdiv
2025          *
2026          */
2027         retval = set_fm_ck_div(target);
2028         err_check_propagate(retval);
2029         /*
2030          * Setup registers needed by pgm_write_pflash
2031          *
2032          */
2033
2034         dsp5680xx_context.flush = 0;
2035
2036         retval = core_move_long_to_r3(target, address); /* Destination address to r3 */
2037         err_check_propagate(retval);
2038         core_load_TX_RX_high_addr_to_r0(target); /* TX/RX reg address to r0 */
2039         err_check_propagate(retval);
2040         retval = core_move_long_to_r2(target, HFM_BASE_ADDR); /* FM base address to r2 */
2041         err_check_propagate(retval);
2042         /*
2043          * Run flashing program.
2044          *
2045          */
2046         /* write to HFM_CNFG (lock=0, select bank) */
2047         retval = core_move_value_at_r2_disp(target, 0x00, HFM_CNFG);
2048         err_check_propagate(retval);
2049         /* write to HMF_USTAT, clear PVIOL, ACCERR &BLANK bits */
2050         retval = core_move_value_at_r2_disp(target, 0x04, HFM_USTAT);
2051         err_check_propagate(retval);
2052         /* clear only one bit at a time */
2053         retval = core_move_value_at_r2_disp(target, 0x10, HFM_USTAT);
2054         err_check_propagate(retval);
2055         retval = core_move_value_at_r2_disp(target, 0x20, HFM_USTAT);
2056         err_check_propagate(retval);
2057         /* write to HMF_PROT, clear protection */
2058         retval = core_move_value_at_r2_disp(target, 0x00, HFM_PROT);
2059         err_check_propagate(retval);
2060         /* write to HMF_PROTB, clear protection */
2061         retval = core_move_value_at_r2_disp(target, 0x00, HFM_PROTB);
2062         err_check_propagate(retval);
2063         if (count % 2) {
2064                 /* TODO implement handling of odd number of words. */
2065                 retval = ERROR_FAIL;
2066                 const char *msg = "Cannot handle odd number of words.";
2067
2068                 err_check(retval, DSP5680XX_ERROR_FLASHING_INVALID_WORD_COUNT,
2069                           msg);
2070         }
2071
2072         dsp5680xx_context.flush = 1;
2073         retval = dsp5680xx_execute_queue();
2074         err_check_propagate(retval);
2075
2076         uint32_t drscan_data;
2077
2078         uint16_t tmp = (buffer[0] | (buffer[1] << 8));
2079
2080         retval = core_tx_upper_data(target, tmp, &drscan_data);
2081         err_check_propagate(retval);
2082
2083         retval = dsp5680xx_resume(target, 0, ram_addr, 0, 0);
2084         err_check_propagate(retval);
2085
2086         int counter = FLUSH_COUNT_FLASH;
2087
2088         dsp5680xx_context.flush = 0;
2089         uint32_t i;
2090
2091         for (i = 1; (i < count / 2) && (i < HFM_SIZE_WORDS); i++) {
2092                 if (--counter == 0) {
2093                         dsp5680xx_context.flush = 1;
2094                         counter = FLUSH_COUNT_FLASH;
2095                 }
2096                 tmp = (buffer[2 * i] | (buffer[2 * i + 1] << 8));
2097                 retval = core_tx_upper_data(target, tmp, &drscan_data);
2098                 if (retval != ERROR_OK) {
2099                         dsp5680xx_context.flush = 1;
2100                         err_check_propagate(retval);
2101                 }
2102                 dsp5680xx_context.flush = 0;
2103         }
2104         dsp5680xx_context.flush = 1;
2105         if (!is_flash_lock) {
2106                 /*
2107                  *Verify flash (skip when exec lock sequence)
2108                  *
2109                  */
2110                 uint16_t signature;
2111
2112                 uint16_t pc_crc;
2113
2114                 retval = dsp5680xx_f_signature(target, address, i, &signature);
2115                 err_check_propagate(retval);
2116                 pc_crc = perl_crc(buffer, i);
2117                 if (pc_crc != signature) {
2118                         retval = ERROR_FAIL;
2119                         const char *msg =
2120                                 "Flashed data failed CRC check, flash again!";
2121                         err_check(retval, DSP5680XX_ERROR_FLASHING_CRC, msg);
2122                 }
2123         }
2124         return retval;
2125 }
2126
2127 int dsp5680xx_f_unlock(struct target *target)
2128 {
2129         int retval = ERROR_OK;
2130
2131         uint16_t eonce_status;
2132
2133         uint32_t instr;
2134
2135         uint32_t ir_out;
2136
2137         struct jtag_tap *tap_chp;
2138
2139         struct jtag_tap *tap_cpu;
2140
2141         tap_chp = jtag_tap_by_string("dsp568013.chp");
2142         if (tap_chp == NULL) {
2143                 retval = ERROR_FAIL;
2144                 err_check(retval, DSP5680XX_ERROR_JTAG_TAP_ENABLE_MASTER,
2145                           "Failed to get master tap.");
2146         }
2147         tap_cpu = jtag_tap_by_string("dsp568013.cpu");
2148         if (tap_cpu == NULL) {
2149                 retval = ERROR_FAIL;
2150                 err_check(retval, DSP5680XX_ERROR_JTAG_TAP_ENABLE_CORE,
2151                           "Failed to get master tap.");
2152         }
2153
2154         retval = eonce_enter_debug_mode_without_reset(target, &eonce_status);
2155         if (retval == ERROR_OK)
2156                 LOG_WARNING("Memory was not locked.");
2157
2158         jtag_add_reset(0, 1);
2159         jtag_add_sleep(TIME_DIV_FREESCALE * 200 * 1000);
2160
2161         retval = reset_jtag();
2162         err_check(retval, DSP5680XX_ERROR_JTAG_RESET,
2163                   "Failed to reset JTAG state machine");
2164         jtag_add_sleep(150);
2165
2166         /* Enable core tap */
2167         tap_chp->enabled = true;
2168         retval = switch_tap(target, tap_chp, tap_cpu);
2169         err_check_propagate(retval);
2170
2171         instr = JTAG_INSTR_DEBUG_REQUEST;
2172         retval =
2173                 dsp5680xx_irscan(target, &instr, &ir_out,
2174                                  DSP5680XX_JTAG_CORE_TAP_IRLEN);
2175         err_check_propagate(retval);
2176         jtag_add_sleep(TIME_DIV_FREESCALE * 100 * 1000);
2177         jtag_add_reset(0, 0);
2178         jtag_add_sleep(TIME_DIV_FREESCALE * 300 * 1000);
2179
2180         /* Enable master tap */
2181         tap_chp->enabled = false;
2182         retval = switch_tap(target, tap_chp, tap_cpu);
2183         err_check_propagate(retval);
2184
2185         /* Execute mass erase to unlock */
2186         instr = MASTER_TAP_CMD_FLASH_ERASE;
2187         retval =
2188                 dsp5680xx_irscan(target, &instr, &ir_out,
2189                                  DSP5680XX_JTAG_MASTER_TAP_IRLEN);
2190         err_check_propagate(retval);
2191
2192         instr = HFM_CLK_DEFAULT;
2193         retval = dsp5680xx_drscan(target, (uint8_t *) &instr, (uint8_t *) &ir_out, 16);
2194         err_check_propagate(retval);
2195
2196         jtag_add_sleep(TIME_DIV_FREESCALE * 150 * 1000);
2197         jtag_add_reset(0, 1);
2198         jtag_add_sleep(TIME_DIV_FREESCALE * 200 * 1000);
2199
2200         retval = reset_jtag();
2201         err_check(retval, DSP5680XX_ERROR_JTAG_RESET,
2202                   "Failed to reset JTAG state machine");
2203         jtag_add_sleep(150);
2204
2205         instr = 0x0606ffff;
2206         retval = dsp5680xx_drscan(target, (uint8_t *) &instr, (uint8_t *) &ir_out,
2207                                  32);
2208         err_check_propagate(retval);
2209
2210         /* enable core tap */
2211         instr = 0x5;
2212         retval =
2213                 dsp5680xx_irscan(target, &instr, &ir_out,
2214                                  DSP5680XX_JTAG_MASTER_TAP_IRLEN);
2215         err_check_propagate(retval);
2216         instr = 0x2;
2217         retval = dsp5680xx_drscan(target, (uint8_t *) &instr, (uint8_t *) &ir_out,
2218                                  4);
2219         err_check_propagate(retval);
2220
2221         tap_cpu->enabled = true;
2222         tap_chp->enabled = false;
2223         target->state = TARGET_RUNNING;
2224         dsp5680xx_context.debug_mode_enabled = false;
2225         return retval;
2226 }
2227
2228 int dsp5680xx_f_lock(struct target *target)
2229 {
2230         int retval;
2231
2232         struct jtag_tap *tap_chp;
2233
2234         struct jtag_tap *tap_cpu;
2235         uint16_t lock_word[] = { HFM_LOCK_FLASH };
2236         retval = dsp5680xx_f_wr(target, (uint8_t *) (lock_word), HFM_LOCK_ADDR_L, 2, 1);
2237         err_check_propagate(retval);
2238
2239         jtag_add_reset(0, 1);
2240         jtag_add_sleep(TIME_DIV_FREESCALE * 200 * 1000);
2241
2242         retval = reset_jtag();
2243         err_check(retval, DSP5680XX_ERROR_JTAG_RESET,
2244                   "Failed to reset JTAG state machine");
2245         jtag_add_sleep(TIME_DIV_FREESCALE * 100 * 1000);
2246         jtag_add_reset(0, 0);
2247         jtag_add_sleep(TIME_DIV_FREESCALE * 300 * 1000);
2248
2249         tap_chp = jtag_tap_by_string("dsp568013.chp");
2250         if (tap_chp == NULL) {
2251                 retval = ERROR_FAIL;
2252                 err_check(retval, DSP5680XX_ERROR_JTAG_TAP_ENABLE_MASTER,
2253                           "Failed to get master tap.");
2254         }
2255         tap_cpu = jtag_tap_by_string("dsp568013.cpu");
2256         if (tap_cpu == NULL) {
2257                 retval = ERROR_FAIL;
2258                 err_check(retval, DSP5680XX_ERROR_JTAG_TAP_ENABLE_CORE,
2259                           "Failed to get master tap.");
2260         }
2261         target->state = TARGET_RUNNING;
2262         dsp5680xx_context.debug_mode_enabled = false;
2263         tap_cpu->enabled = false;
2264         tap_chp->enabled = true;
2265         retval = switch_tap(target, tap_chp, tap_cpu);
2266         return retval;
2267 }
2268
2269 static int dsp5680xx_step(struct target *target, int current, target_addr_t address,
2270                           int handle_breakpoints)
2271 {
2272         err_check(ERROR_FAIL, DSP5680XX_ERROR_NOT_IMPLEMENTED_STEP,
2273                   "Not implemented yet.");
2274 }
2275
2276 /** Holds methods for dsp5680xx targets. */
2277 struct target_type dsp5680xx_target = {
2278         .name = "dsp5680xx",
2279
2280         .poll = dsp5680xx_poll,
2281         .arch_state = dsp5680xx_arch_state,
2282
2283         .halt = dsp5680xx_halt,
2284         .resume = dsp5680xx_resume,
2285         .step = dsp5680xx_step,
2286
2287         .write_buffer = dsp5680xx_write_buffer,
2288         .read_buffer = dsp5680xx_read_buffer,
2289
2290         .assert_reset = dsp5680xx_assert_reset,
2291         .deassert_reset = dsp5680xx_deassert_reset,
2292         .soft_reset_halt = dsp5680xx_soft_reset_halt,
2293
2294         .read_memory = dsp5680xx_read,
2295         .write_memory = dsp5680xx_write,
2296
2297         .checksum_memory = dsp5680xx_checksum_memory,
2298
2299         .target_create = dsp5680xx_target_create,
2300         .init_target = dsp5680xx_init_target,
2301 };