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