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