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