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

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