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

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