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