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

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