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

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