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