git.sur5r.net Git - 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 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 #define TIME_DIV_FREESCALE 0.3
 499 /**
 500  * Puts the core into debug mode, enabling the EOnCE module.
 501  *
 502  * @param target
 503  * @param eonce_status Data read from the EOnCE status register.
 504  *
 505  * @return
 506  */
 507 static int eonce_enter_debug_mode(struct target * target, uint16_t * eonce_status){
 508   int retval = ERROR_OK;
 509   uint32_t instr = JTAG_INSTR_DEBUG_REQUEST;
 510   uint32_t ir_out;//not used, just to make jtag happy.
 511   uint16_t instr_16;
 512   uint16_t read_16;
 513
 514   struct jtag_tap * tap_chp;
 515   struct jtag_tap * tap_cpu;
 516   tap_chp = jtag_tap_by_string("dsp568013.chp");
 517   if(tap_chp == NULL){
 518     retval = ERROR_FAIL;
 519     err_check(retval,"Failed to get master tap.");
 520   }
 521   tap_cpu = jtag_tap_by_string("dsp568013.cpu");
 522   if(tap_cpu == NULL){
 523     retval = ERROR_FAIL;
 524     err_check(retval,"Failed to get master tap.");
 525   }
 526
 527   tap_chp->enabled = false;
 528   retval = switch_tap(target,tap_chp,tap_cpu);
 529   err_check_propagate(retval);
 530
 531   instr = MASTER_TAP_CMD_IDCODE;
 532   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_MASTER_TAP_IRLEN);
 533   err_check_propagate(retval);
 534   usleep(TIME_DIV_FREESCALE*100*1000);
 535
 536   // Enable EOnCE module
 537   jtag_add_reset(0,1);
 538   usleep(TIME_DIV_FREESCALE*200*1000);
 539   instr = 0x0606ffff;// This was selected experimentally.
 540   retval =  dsp5680xx_drscan(target,(uint8_t *) & instr,(uint8_t *) & ir_out,32);
 541   err_check_propagate(retval);
 542   // ir_out now hold tap idcode
 543
 544   // Enable core tap
 545   retval = switch_tap(target,tap_chp,tap_cpu);
 546   err_check_propagate(retval);
 547
 548   instr = JTAG_INSTR_ENABLE_ONCE;
 549   //Two rounds of jtag 0x6  (enable eonce) to enable EOnCE.
 550   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
 551   err_check_propagate(retval);
 552   instr = JTAG_INSTR_DEBUG_REQUEST;
 553   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
 554   err_check_propagate(retval);
 555   instr_16 = 0x1;
 556   retval = dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,8);
 557   instr_16 = 0x20;
 558   retval = dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,8);
 559   usleep(TIME_DIV_FREESCALE*100*1000);
 560   jtag_add_reset(0,0);
 561   usleep(TIME_DIV_FREESCALE*300*1000);
 562
 563   instr = JTAG_INSTR_ENABLE_ONCE;
 564   //Two rounds of jtag 0x6  (enable eonce) to enable EOnCE.
 565   for(int i = 0; i<3; i++){
 566     retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
 567     err_check_propagate(retval);
 568   }
 569
 570   for(int i = 0; i<3; i++){
 571     instr_16 = 0x86;
 572     dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,16);
 573     instr_16 = 0xff;
 574     dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,16);
 575   }
 576
 577   // Verify that debug mode is enabled
 578   uint16_t data_read_from_dr;
 579   retval = eonce_read_status_reg(target,&data_read_from_dr);
 580   err_check_propagate(retval);
 581   if((data_read_from_dr&0x30) == 0x30){
 582     LOG_DEBUG("EOnCE successfully entered debug mode.");
 583     target->state = TARGET_HALTED;
 584     retval = ERROR_OK;
 585   }else{
 586     LOG_DEBUG("Failed to set EOnCE module to debug mode.");
 587     retval = ERROR_TARGET_FAILURE;
 588   }
 589   if(eonce_status!=NULL)
 590     *eonce_status = data_read_from_dr;
 591   return retval;
 592 }
 593
 594 /**
 595  * Reads the current value of the program counter and stores it.
 596  *
 597  * @param target
 598  *
 599  * @return
 600  */
 601 static int eonce_pc_store(struct target * target){
 602   uint8_t tmp[2];
 603   int retval;
 604   retval = core_move_pc_to_r4(target);
 605   err_check_propagate(retval);
 606   retval = core_move_r4_to_y(target);
 607   err_check_propagate(retval);
 608   retval = eonce_load_TX_RX_to_r0(target);
 609   err_check_propagate(retval);
 610   retval = core_move_y0_at_r0(target);
 611   err_check_propagate(retval);
 612   retval = core_rx_lower_data(target,tmp);
 613   err_check_propagate(retval);
 614   LOG_USER("PC value: 0x%X%X\n",tmp[1],tmp[0]);
 615   dsp5680xx_context.stored_pc = (tmp[0]|(tmp[1]<<8));
 616   return ERROR_OK;
 617 }
 618
 619 static int dsp5680xx_target_create(struct target *target, Jim_Interp * interp){
 620   struct dsp5680xx_common *dsp5680xx = calloc(1, sizeof(struct dsp5680xx_common));
 621   target->arch_info = dsp5680xx;
 622   return ERROR_OK;
 623 }
 624
 625 static int dsp5680xx_init_target(struct command_context *cmd_ctx, struct target *target){
 626   dsp5680xx_context.stored_pc = 0;
 627   dsp5680xx_context.flush = 1;
 628   LOG_DEBUG("target initiated!");
 629   //TODO core tap must be enabled before running these commands, currently this is done in the .cfg tcl script.
 630   return ERROR_OK;
 631 }
 632
 633 static int dsp5680xx_arch_state(struct target *target){
 634   LOG_USER("%s not implemented yet.",__FUNCTION__);
 635   return ERROR_OK;
 636 }
 637
 638 int dsp5680xx_target_status(struct target * target, uint8_t * jtag_st, uint16_t * eonce_st){
 639   return target->state;
 640 }
 641
 642 static int dsp5680xx_assert_reset(struct target *target){
 643   target->state = TARGET_RESET;
 644   return ERROR_OK;
 645 }
 646
 647 static int dsp5680xx_deassert_reset(struct target *target){
 648   target->state = TARGET_RUNNING;
 649   return ERROR_OK;
 650 }
 651
 652 static int dsp5680xx_halt(struct target *target){
 653   int retval;
 654   uint16_t eonce_status = 0xbeef;
 655   if(target->state == TARGET_HALTED){
 656     LOG_USER("Target already halted.");
 657     return ERROR_OK;
 658   }
 659   retval = eonce_enter_debug_mode(target,&eonce_status);
 660   err_check(retval,"Failed to halt target.");
 661   retval = eonce_pc_store(target);
 662   err_check_propagate(retval);
 663   //TODO is it useful to store the pc?
 664   return retval;
 665 }
 666
 667 static int dsp5680xx_poll(struct target *target){
 668   int retval;
 669   uint8_t jtag_status;
 670   uint8_t eonce_status;
 671   uint16_t read_tmp;
 672   retval = dsp5680xx_jtag_status(target,&jtag_status);
 673   err_check_propagate(retval);
 674   if (jtag_status == JTAG_STATUS_DEBUG)
 675     if (target->state != TARGET_HALTED){
 676       retval = eonce_enter_debug_mode(target,&read_tmp);
 677           err_check_propagate(retval);
 678       eonce_status = (uint8_t) read_tmp;
 679       if((eonce_status&EONCE_STAT_MASK) != DSP5680XX_ONCE_OSCR_DEBUG_M){
 680                 LOG_WARNING("%s: Failed to put EOnCE in debug mode. Is flash locked?...",__FUNCTION__);
 681                 return ERROR_TARGET_FAILURE;
 682       }else{
 683                 target->state = TARGET_HALTED;
 684                 return ERROR_OK;
 685       }
 686     }
 687   if (jtag_status == JTAG_STATUS_NORMAL){
 688     if(target->state == TARGET_RESET){
 689       retval = dsp5680xx_halt(target);
 690           err_check_propagate(retval);
 691       retval = eonce_exit_debug_mode(target,&eonce_status);
 692           err_check_propagate(retval);
 693       if((eonce_status&EONCE_STAT_MASK) != DSP5680XX_ONCE_OSCR_NORMAL_M){
 694                 LOG_WARNING("%s: JTAG running, but cannot make EOnCE run. Try resetting...",__FUNCTION__);
 695                 return ERROR_TARGET_FAILURE;
 696       }else{
 697                 target->state = TARGET_RUNNING;
 698                 return ERROR_OK;
 699       }
 700     }
 701     if(target->state != TARGET_RUNNING){
 702       retval = eonce_read_status_reg(target,&read_tmp);
 703           err_check_propagate(retval);
 704       eonce_status = (uint8_t) read_tmp;
 705       if((eonce_status&EONCE_STAT_MASK) != DSP5680XX_ONCE_OSCR_NORMAL_M){
 706                 LOG_WARNING("Inconsistent target status. Restart!");
 707                 return ERROR_TARGET_FAILURE;
 708       }
 709     }
 710     target->state = TARGET_RUNNING;
 711     return ERROR_OK;
 712   }
 713   if(jtag_status == JTAG_STATUS_DEAD){
 714     LOG_ERROR("%s: Cannot communicate with JTAG. Check connection...",__FUNCTION__);
 715     target->state = TARGET_UNKNOWN;
 716     return ERROR_TARGET_FAILURE;
 717   };
 718   if (target->state == TARGET_UNKNOWN){
 719     LOG_ERROR("%s: Target status invalid - communication failure",__FUNCTION__);
 720     return ERROR_TARGET_FAILURE;
 721   };
 722   return ERROR_OK;
 723 }
 724
 725 static int dsp5680xx_resume(struct target *target, int current, uint32_t address,int handle_breakpoints, int debug_execution){
 726   if(target->state == TARGET_RUNNING){
 727     LOG_USER("Target already running.");
 728     return ERROR_OK;
 729   }
 730   int retval;
 731   uint8_t eonce_status;
 732   if(!current){
 733     retval = core_move_value_to_pc(target,address);
 734     err_check_propagate(retval);
 735   }
 736
 737   int retry = 20;
 738   while(retry-- > 1){
 739     retval = eonce_exit_debug_mode(target,&eonce_status );
 740         err_check_propagate(retval);
 741     if(eonce_status == DSP5680XX_ONCE_OSCR_NORMAL_M)
 742       break;
 743   }
 744   if(retry == 0){
 745     retval = ERROR_TARGET_FAILURE;
 746         err_check(retval,"Failed to resume...");
 747   }else{
 748     target->state = TARGET_RUNNING;
 749   }
 750   LOG_DEBUG("EOnCE status: 0x%02X.",eonce_status);
 751   return ERROR_OK;
 752 }
 753
 754
 755
 756
 757
 758
 759 /**
 760  * 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.
 761  * The special case of 0xFFXXXX is not modified, since it allows to read out the memory mapped EOnCE registers.
 762  *
 763  * @param address
 764  * @param pmem
 765  *
 766  * @return
 767  */
 768 static int dsp5680xx_convert_address(uint32_t * address, int * pmem){
 769   // Distinguish data memory (x:) from program memory (p:) by the address.
 770   // Addresses over S_FILE_DATA_OFFSET are considered (x:) memory.
 771   if(*address >= S_FILE_DATA_OFFSET){
 772     *pmem = 0;
 773     if(((*address)&0xff0000)!=0xff0000)
 774       *address -= S_FILE_DATA_OFFSET;
 775   }
 776   return ERROR_OK;
 777 }
 778
 779 static int dsp5680xx_read_16_single(struct target * target, uint32_t address, uint8_t * data_read, int r_pmem){
 780   int retval;
 781   retval = core_move_long_to_r0(target,address);
 782   err_check_propagate(retval);
 783   if(r_pmem)
 784     retval = core_move_at_pr0_inc_to_y0(target);
 785   else
 786     retval = core_move_at_r0_to_y0(target);
 787   err_check_propagate(retval);
 788   retval = eonce_load_TX_RX_to_r0(target);
 789   err_check_propagate(retval);
 790   retval = core_move_y0_at_r0(target);
 791   err_check_propagate(retval);
 792   // at this point the data i want is at the reg eonce can read
 793   retval = core_rx_lower_data(target,data_read);
 794   err_check_propagate(retval);
 795   LOG_DEBUG("%s: Data read from 0x%06X: 0x%02X%02X",__FUNCTION__, address,data_read[1],data_read[0]);
 796   return retval;
 797 }
 798
 799 static int dsp5680xx_read_32_single(struct target * target, uint32_t address, uint8_t * data_read, int r_pmem){
 800   int retval;
 801   address = (address & 0xFFFFFE);
 802   // Get data to an intermediate register
 803   retval = core_move_long_to_r0(target,address);
 804   err_check_propagate(retval);
 805   if(r_pmem){
 806     retval = core_move_at_pr0_inc_to_y0(target);
 807         err_check_propagate(retval);
 808     retval = core_move_at_pr0_inc_to_y1(target);
 809         err_check_propagate(retval);
 810   }else{
 811     retval = core_move_at_r0_inc_to_y0(target);
 812         err_check_propagate(retval);
 813     retval = core_move_at_r0_to_y1(target);
 814         err_check_propagate(retval);
 815   }
 816   // Get lower part of data to TX/RX
 817   retval = eonce_load_TX_RX_to_r0(target);
 818   err_check_propagate(retval);
 819   retval = core_move_y0_at_r0_inc(target); // This also load TX/RX high to r0
 820   err_check_propagate(retval);
 821   // Get upper part of data to TX/RX
 822   retval = core_move_y1_at_r0(target);
 823   err_check_propagate(retval);
 824   // at this point the data i want is at the reg eonce can read
 825   retval = core_rx_lower_data(target,data_read);
 826   err_check_propagate(retval);
 827   retval = core_rx_upper_data(target,data_read+2);
 828   err_check_propagate(retval);
 829   return retval;
 830 }
 831
 832 static int dsp5680xx_read(struct target * target, uint32_t address, unsigned size, unsigned count, uint8_t * buffer){
 833   if(target->state != TARGET_HALTED){
 834     LOG_USER("Target must be halted.");
 835     return ERROR_FAIL;
 836   }
 837   int retval = ERROR_OK;
 838   int pmem = 1;
 839
 840   retval = dsp5680xx_convert_address(&address, &pmem);
 841   err_check_propagate(retval);
 842
 843   dsp5680xx_context.flush = 0;
 844   int counter = FLUSH_COUNT_READ_WRITE;
 845
 846   for (unsigned i=0; i<count; i++){
 847     if(--counter==0){
 848       dsp5680xx_context.flush = 1;
 849       counter = FLUSH_COUNT_READ_WRITE;
 850     }
 851     switch (size){
 852     case 1:
 853       if(!(i%2)){
 854                 retval = dsp5680xx_read_16_single(target, address + i/2, buffer + i, pmem);
 855       }
 856       break;
 857     case 2:
 858       retval = dsp5680xx_read_16_single(target, address + i, buffer+2*i, pmem);
 859       break;
 860     case 4:
 861       retval = dsp5680xx_read_32_single(target, address + 2*i, buffer + 4*i, pmem);
 862       break;
 863     default:
 864       LOG_USER("%s: Invalid read size.",__FUNCTION__);
 865       break;
 866     }
 867         err_check_propagate(retval);
 868     dsp5680xx_context.flush = 0;
 869   }
 870
 871   dsp5680xx_context.flush = 1;
 872   retval = dsp5680xx_execute_queue();
 873   err_check_propagate(retval);
 874
 875   return retval;
 876 }
 877
 878 static int dsp5680xx_write_16_single(struct target *target, uint32_t address, uint16_t data, uint8_t w_pmem){
 879   int retval = 0;
 880   retval = core_move_long_to_r0(target,address);
 881   err_check_propagate(retval);
 882   if(w_pmem){
 883     retval = core_move_value_to_y0(target,data);
 884         err_check_propagate(retval);
 885     retval = core_move_y0_at_pr0_inc(target);
 886         err_check_propagate(retval);
 887   }else{
 888     retval = core_move_value_at_r0(target,data);
 889         err_check_propagate(retval);
 890   }
 891   return retval;
 892 }
 893
 894 static int dsp5680xx_write_32_single(struct target *target, uint32_t address, uint32_t data, int w_pmem){
 895   int retval = 0;
 896   retval = core_move_long_to_r0(target,address);
 897   err_check_propagate(retval);
 898   retval = core_move_long_to_y(target,data);
 899   err_check_propagate(retval);
 900   if(w_pmem)
 901     retval = core_move_y0_at_pr0_inc(target);
 902   else
 903     retval = core_move_y0_at_r0_inc(target);
 904   err_check_propagate(retval);
 905   if(w_pmem)
 906     retval = core_move_y1_at_pr0_inc(target);
 907   else
 908     retval = core_move_y1_at_r0_inc(target);
 909   err_check_propagate(retval);
 910   return retval;
 911 }
 912
 913 static int dsp5680xx_write_8(struct target * target, uint32_t address, uint32_t count, const uint8_t * data, int pmem){
 914   if(target->state != TARGET_HALTED){
 915     LOG_ERROR("%s: Target must be halted.",__FUNCTION__);
 916     return ERROR_OK;
 917   };
 918   int retval = 0;
 919   uint16_t data_16;
 920   uint32_t iter;
 921
 922   int counter = FLUSH_COUNT_READ_WRITE;
 923   for(iter = 0; iter<count/2; iter++){
 924     if(--counter==0){
 925       dsp5680xx_context.flush = 1;
 926       counter = FLUSH_COUNT_READ_WRITE;
 927     }
 928     data_16=(data[2*iter]|(data[2*iter+1]<<8));
 929     retval = dsp5680xx_write_16_single(target,address+iter,data_16, pmem);
 930     if(retval != ERROR_OK){
 931       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 932       dsp5680xx_context.flush = 1;
 933       return retval;
 934     }
 935     dsp5680xx_context.flush = 0;
 936   }
 937   dsp5680xx_context.flush = 1;
 938
 939   // Only one byte left, let's not overwrite the other byte (mem is 16bit)
 940   // Need to retrieve the part we do not want to overwrite.
 941   uint16_t data_old;
 942   if((count==1)||(count%2)){
 943     retval = dsp5680xx_read(target,address+iter,1,1,(uint8_t *)&data_old);
 944         err_check_propagate(retval);
 945     if(count==1)
 946       data_old=(((data_old&0xff)<<8)|data[0]);// preserve upper byte
 947     else
 948       data_old=(((data_old&0xff)<<8)|data[2*iter+1]);
 949     retval = dsp5680xx_write_16_single(target,address+iter,data_old, pmem);
 950         err_check_propagate(retval);
 951   }
 952   return retval;
 953 }
 954
 955 static int dsp5680xx_write_16(struct target * target, uint32_t address, uint32_t count, const uint8_t * data, int pmem){
 956   int retval = ERROR_OK;
 957   if(target->state != TARGET_HALTED){
 958         retval = ERROR_TARGET_NOT_HALTED;
 959         err_check(retval,"Target must be halted.");
 960   };
 961   uint32_t iter;
 962   int counter = FLUSH_COUNT_READ_WRITE;
 963
 964   for(iter = 0; iter<count; iter++){
 965         if(--counter==0){
 966           dsp5680xx_context.flush = 1;
 967       counter = FLUSH_COUNT_READ_WRITE;
 968         }
 969     retval = dsp5680xx_write_16_single(target,address+iter,data[iter], pmem);
 970     if(retval != ERROR_OK){
 971       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 972           dsp5680xx_context.flush = 1;
 973       return retval;
 974     }
 975         dsp5680xx_context.flush = 0;
 976   }
 977   dsp5680xx_context.flush = 1;
 978   return retval;
 979 }
 980
 981 static int dsp5680xx_write_32(struct target * target, uint32_t address, uint32_t count, const uint8_t * data, int pmem){
 982   int retval = ERROR_OK;
 983   if(target->state != TARGET_HALTED){
 984         retval = ERROR_TARGET_NOT_HALTED;
 985         err_check(retval,"Target must be halted.");
 986   };
 987   uint32_t iter;
 988   int counter = FLUSH_COUNT_READ_WRITE;
 989
 990   for(iter = 0; iter<count; iter++){
 991         if(--counter==0){
 992           dsp5680xx_context.flush = 1;
 993       counter = FLUSH_COUNT_READ_WRITE;
 994         }
 995     retval = dsp5680xx_write_32_single(target,address+(iter<<1),data[iter], pmem);
 996     if(retval != ERROR_OK){
 997       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 998           dsp5680xx_context.flush = 1;
 999       return retval;
1000     }
1001         dsp5680xx_context.flush = 0;
1002   }
1003   dsp5680xx_context.flush = 1;
1004   return retval;
1005 }
1006
1007 /**
1008  * Writes @buffer to memory.
1009  * The parameter @address determines whether @buffer should be written to P: (program) memory or X: (data) memory.
1010  *
1011  * @param target
1012  * @param address
1013  * @param size Bytes (1), Half words (2), Words (4).
1014  * @param count In bytes.
1015  * @param buffer
1016  *
1017  * @return
1018  */
1019 static int dsp5680xx_write(struct target *target, uint32_t address, uint32_t size, uint32_t count, const uint8_t * buffer){
1020   //TODO Cannot write 32bit to odd address, will write 0x12345678  as 0x5678 0x0012
1021   if(target->state != TARGET_HALTED){
1022     LOG_USER("Target must be halted.");
1023     return ERROR_OK;
1024   }
1025   int retval = 0;
1026   int p_mem = 1;
1027   retval = dsp5680xx_convert_address(&address, &p_mem);
1028   err_check_propagate(retval);
1029
1030   switch (size){
1031   case 1:
1032     retval = dsp5680xx_write_8(target, address, count, buffer, p_mem);
1033     break;
1034   case 2:
1035     retval = dsp5680xx_write_16(target, address, count, buffer, p_mem);
1036       break;
1037   case 4:
1038     retval = dsp5680xx_write_32(target, address, count, buffer, p_mem);
1039     break;
1040   default:
1041         retval = ERROR_TARGET_DATA_ABORT;
1042         err_check(retval,"Invalid data size.");
1043         break;
1044   }
1045   return retval;
1046 }
1047
1048 static int dsp5680xx_bulk_write_memory(struct target * target,uint32_t address, uint32_t aligned, const uint8_t * buffer){
1049   LOG_ERROR("Not implemented yet.");
1050   return ERROR_FAIL;
1051 }
1052
1053 static int dsp5680xx_write_buffer(struct target * target, uint32_t address, uint32_t size, const uint8_t * buffer){
1054   if(target->state != TARGET_HALTED){
1055     LOG_USER("Target must be halted.");
1056     return ERROR_OK;
1057   }
1058   return dsp5680xx_write(target, address, 1, size, buffer);
1059 }
1060
1061 /**
1062  * This function is called by verify_image, it is used to read data from memory.
1063  *
1064  * @param target
1065  * @param address Word addressing.
1066  * @param size In bytes.
1067  * @param buffer
1068  *
1069  * @return
1070  */
1071 static int dsp5680xx_read_buffer(struct target * target, uint32_t address, uint32_t size, uint8_t * buffer){
1072   if(target->state != TARGET_HALTED){
1073     LOG_USER("Target must be halted.");
1074     return ERROR_OK;
1075   }
1076   // The "/2" solves the byte/word addressing issue.
1077   return dsp5680xx_read(target,address,2,size/2,buffer);
1078 }
1079
1080 /**
1081  * This function is not implemented.
1082  * It returns an error in order to get OpenOCD to do read out the data and calculate the CRC, or try a binary comparison.
1083  *
1084  * @param target
1085  * @param address Start address of the image.
1086  * @param size In bytes.
1087  * @param checksum
1088  *
1089  * @return
1090  */
1091 static int dsp5680xx_checksum_memory(struct target * target, uint32_t address, uint32_t size, uint32_t * checksum){
1092   return ERROR_FAIL;
1093 }
1094
1095 /**
1096  * 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.
1097  * This algorithm is based on the perl script available from the Freescale website at FAQ 25630.
1098  *
1099  * @param buff16
1100  * @param word_count
1101  *
1102  * @return
1103  */
1104 static int perl_crc(uint8_t * buff8,uint32_t  word_count){
1105   uint16_t checksum = 0xffff;
1106   uint16_t data,fbmisr;
1107   uint32_t i;
1108   for(i=0;i<word_count;i++){
1109     data = (buff8[2*i]|(buff8[2*i+1]<<8));
1110     fbmisr = (checksum & 2)>>1 ^ (checksum & 4)>>2 ^ (checksum & 16)>>4 ^ (checksum & 0x8000)>>15;
1111     checksum = (data ^ ((checksum << 1) | fbmisr));
1112   }
1113   i--;
1114   for(;!(i&0x80000000);i--){
1115     data = (buff8[2*i]|(buff8[2*i+1]<<8));
1116     fbmisr = (checksum & 2)>>1 ^ (checksum & 4)>>2 ^ (checksum & 16)>>4 ^ (checksum & 0x8000)>>15;
1117     checksum = (data ^ ((checksum << 1) | fbmisr));
1118   }
1119   return checksum;
1120 }
1121
1122 /**
1123  * Resets the SIM. (System Integration Module).
1124  *
1125  * @param target
1126  *
1127  * @return
1128  */
1129 int dsp5680xx_f_SIM_reset(struct target * target){
1130   int retval = ERROR_OK;
1131   uint16_t sim_cmd = SIM_CMD_RESET;
1132   uint32_t sim_addr;
1133   if(strcmp(target->tap->chip,"dsp568013")==0){
1134         sim_addr = MC568013_SIM_BASE_ADDR+S_FILE_DATA_OFFSET;
1135         retval = dsp5680xx_write(target,sim_addr,1,2,(const uint8_t *)&sim_cmd);
1136         err_check_propagate(retval);
1137   }
1138   return retval;
1139 }
1140
1141 /**
1142  * Halts the core and resets the SIM. (System Integration Module).
1143  *
1144  * @param target
1145  *
1146  * @return
1147  */
1148 static int dsp5680xx_soft_reset_halt(struct target *target){
1149   //TODO is this what this function is expected to do...?
1150   int retval;
1151   retval = dsp5680xx_halt(target);
1152   err_check_propagate(retval);
1153   retval = dsp5680xx_f_SIM_reset(target);
1154   err_check_propagate(retval);
1155   return retval;
1156 }
1157
1158 int dsp5680xx_f_protect_check(struct target * target, uint16_t * protected) {
1159   int retval;
1160   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1161     retval = dsp5680xx_halt(target);
1162         err_check_propagate(retval);
1163   }
1164   if(protected == NULL){
1165     err_check(ERROR_FAIL,"NULL pointer not valid.");
1166   }
1167   retval = dsp5680xx_read_16_single(target,HFM_BASE_ADDR|HFM_PROT,(uint8_t *)protected,0);
1168   err_check_propagate(retval);
1169   return retval;
1170 }
1171
1172 /**
1173  * Executes a command on the FM module. Some commands use the parameters @address and @data, others ignore them.
1174  *
1175  * @param target
1176  * @param command Command to execute.
1177  * @param address Command parameter.
1178  * @param data Command parameter.
1179  * @param hfm_ustat FM status register.
1180  * @param pmem Address is P: (program) memory (@pmem==1) or X: (data) memory (@pmem==0)
1181  *
1182  * @return
1183  */
1184 static int dsp5680xx_f_execute_command(struct target * target, uint16_t command, uint32_t address, uint32_t data, uint16_t * hfm_ustat, int pmem){
1185   int retval;
1186   retval = core_load_TX_RX_high_addr_to_r0(target);
1187   err_check_propagate(retval);
1188   retval = core_move_long_to_r2(target,HFM_BASE_ADDR);
1189   err_check_propagate(retval);
1190   uint8_t i[2];
1191   int watchdog = 100;
1192   do{
1193     retval = core_move_at_r2_disp_to_y0(target,HFM_USTAT);      // read HMF_USTAT
1194         err_check_propagate(retval);
1195     retval = core_move_y0_at_r0(target);
1196         err_check_propagate(retval);
1197     retval = core_rx_upper_data(target,i);
1198         err_check_propagate(retval);
1199     if((watchdog--)==1){
1200       retval = ERROR_TARGET_FAILURE;
1201       err_check(retval,"FM execute command failed.");
1202     }
1203   }while (!(i[0]&0x40));                                // wait until current command is complete
1204
1205   dsp5680xx_context.flush = 0;
1206
1207   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 ???
1208   err_check_propagate(retval);
1209   retval = core_move_value_at_r2_disp(target,0x04,HFM_USTAT);           // write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1210   err_check_propagate(retval);
1211   retval = core_move_value_at_r2_disp(target,0x10,HFM_USTAT);           // clear only one bit at a time
1212   err_check_propagate(retval);
1213   retval = core_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1214   err_check_propagate(retval);
1215   retval = core_move_value_at_r2_disp(target,0x00,HFM_PROT);            // write to HMF_PROT, clear protection
1216   err_check_propagate(retval);
1217   retval = core_move_value_at_r2_disp(target,0x00,HFM_PROTB);           // write to HMF_PROTB, clear protection
1218   err_check_propagate(retval);
1219   retval = core_move_value_to_y0(target,data);
1220   err_check_propagate(retval);
1221   retval = core_move_long_to_r3(target,address);                        // write to the flash block
1222   err_check_propagate(retval);
1223   if (pmem){
1224     retval = core_move_y0_at_pr3_inc(target);
1225         err_check_propagate(retval);
1226   }else{
1227     retval = core_move_y0_at_r3(target);
1228         err_check_propagate(retval);
1229   }
1230   retval = core_move_value_at_r2_disp(target,command,HFM_CMD);  // write command to the HFM_CMD reg
1231   err_check_propagate(retval);
1232   retval = core_move_value_at_r2_disp(target,0x80,HFM_USTAT);           // start the command
1233   err_check_propagate(retval);
1234
1235   dsp5680xx_context.flush = 1;
1236   retval = dsp5680xx_execute_queue();
1237   err_check_propagate(retval);
1238
1239   watchdog = 100;
1240   do{
1241     retval = core_move_at_r2_disp_to_y0(target,HFM_USTAT);      // read HMF_USTAT
1242         err_check_propagate(retval);
1243     retval = core_move_y0_at_r0(target);
1244         err_check_propagate(retval);
1245         retval = core_rx_upper_data(target,i);
1246         err_check_propagate(retval);
1247     if((watchdog--)==1){
1248           retval = ERROR_TARGET_FAILURE;
1249       err_check(retval,"FM execution did not finish.");
1250     }
1251   }while (!(i[0]&0x40));            // wait until the command is complete
1252   *hfm_ustat = ((i[0]<<8)|(i[1]));
1253   if (i[0]&HFM_USTAT_MASK_PVIOL_ACCER){
1254     retval = ERROR_TARGET_FAILURE;
1255     err_check(retval,"pviol and/or accer bits set. HFM command execution error");
1256   }
1257   return ERROR_OK;
1258 }
1259
1260 /**
1261  * 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.)
1262  *
1263  * @param target
1264  *
1265  * @return
1266  */
1267 static int set_fm_ck_div(struct target * target){
1268   uint8_t i[2];
1269   int retval;
1270   retval = core_move_long_to_r2(target,HFM_BASE_ADDR);
1271   err_check_propagate(retval);
1272   retval = core_load_TX_RX_high_addr_to_r0(target);
1273   err_check_propagate(retval);
1274   retval = core_move_at_r2_to_y0(target);// read HFM_CLKD
1275   err_check_propagate(retval);
1276   retval = core_move_y0_at_r0(target);
1277   err_check_propagate(retval);
1278   retval = core_rx_upper_data(target,i);
1279   err_check_propagate(retval);
1280   unsigned int hfm_at_wrong_value = 0;
1281   if ((i[0]&0x7f)!=HFM_CLK_DEFAULT) {
1282     LOG_DEBUG("HFM CLK divisor contained incorrect value (0x%02X).",i[0]&0x7f);
1283     hfm_at_wrong_value = 1;
1284   }else{
1285     LOG_DEBUG("HFM CLK divisor was already set to correct value (0x%02X).",i[0]&0x7f);
1286     return ERROR_OK;
1287   }
1288   retval = core_move_value_at_r2(target,HFM_CLK_DEFAULT);       // write HFM_CLKD
1289   err_check_propagate(retval);
1290   retval = core_move_at_r2_to_y0(target); // verify HFM_CLKD
1291   err_check_propagate(retval);
1292   retval = core_move_y0_at_r0(target);
1293   err_check_propagate(retval);
1294   retval = core_rx_upper_data(target,i);
1295   err_check_propagate(retval);
1296   if (i[0]!=(0x80|(HFM_CLK_DEFAULT&0x7f))) {
1297         retval = ERROR_TARGET_FAILURE;
1298         err_check(retval,"Unable to set HFM CLK divisor.");
1299   }
1300   if(hfm_at_wrong_value)
1301     LOG_DEBUG("HFM CLK divisor set to 0x%02x.",i[0]&0x7f);
1302   return ERROR_OK;
1303 }
1304
1305 /**
1306  * 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.
1307  *
1308  * @param target
1309  * @param address Start of flash array where the signature should be calculated.
1310  * @param words Number of words over which the signature should be calculated.
1311  * @param signature Value calculated by the FM.
1312  *
1313  * @return
1314  */
1315 static int dsp5680xx_f_signature(struct target * target, uint32_t address, uint32_t words, uint16_t * signature){
1316   int retval;
1317   uint16_t hfm_ustat;
1318   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1319     retval = eonce_enter_debug_mode(target,NULL);
1320     err_check_propagate(retval);
1321     // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1322     // Set hfmdiv
1323     // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1324     retval = set_fm_ck_div(target);
1325     err_check_propagate(retval);
1326   }
1327   retval = dsp5680xx_f_execute_command(target,HFM_CALCULATE_DATA_SIGNATURE,address,words,&hfm_ustat,1);
1328   err_check_propagate(retval);
1329   retval = dsp5680xx_read_16_single(target, HFM_BASE_ADDR|HFM_DATA, (uint8_t *)signature, 0);
1330   return retval;
1331 }
1332
1333 int dsp5680xx_f_erase_check(struct target * target, uint8_t * erased,uint32_t sector){
1334   int retval;
1335   uint16_t hfm_ustat;
1336   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1337     retval = dsp5680xx_halt(target);
1338     err_check_propagate(retval);
1339   }
1340   retval = set_fm_ck_div(target);
1341   err_check_propagate(retval);
1342   // Check if chip is already erased.
1343   retval = dsp5680xx_f_execute_command(target,HFM_ERASE_VERIFY,HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2,0,&hfm_ustat,1); // blank check
1344   err_check_propagate(retval);
1345   if(erased!=NULL)
1346     *erased = (uint8_t)(hfm_ustat&HFM_USTAT_MASK_BLANK);
1347   return retval;
1348 }
1349
1350 /**
1351  * Executes the FM page erase command.
1352  *
1353  * @param target
1354  * @param sector Page to erase.
1355  * @param hfm_ustat FM module status register.
1356  *
1357  * @return
1358  */
1359 static int erase_sector(struct target * target, int sector, uint16_t * hfm_ustat){
1360   int retval;
1361   retval = dsp5680xx_f_execute_command(target,HFM_PAGE_ERASE,HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2,0,hfm_ustat,1);
1362   err_check_propagate(retval);
1363   return retval;
1364 }
1365
1366 /**
1367  * Executes the FM mass erase command. Erases the flash array completely.
1368  *
1369  * @param target
1370  * @param hfm_ustat FM module status register.
1371  *
1372  * @return
1373  */
1374 static int mass_erase(struct target * target, uint16_t * hfm_ustat){
1375   int retval;
1376   retval = dsp5680xx_f_execute_command(target,HFM_MASS_ERASE,0,0,hfm_ustat,1);
1377   return retval;
1378 }
1379
1380 int dsp5680xx_f_erase(struct target * target, int first, int last){
1381   int retval;
1382   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1383     retval = dsp5680xx_halt(target);
1384     err_check_propagate(retval);
1385   }
1386   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1387   // Reset SIM
1388   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1389   retval = dsp5680xx_f_SIM_reset(target);
1390   err_check_propagate(retval);
1391   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1392   // Set hfmdiv
1393   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1394   retval = set_fm_ck_div(target);
1395   err_check_propagate(retval);
1396
1397   uint16_t hfm_ustat;
1398   int do_mass_erase = ((!(first|last)) || ((first==0)&&(last == (HFM_SECTOR_COUNT-1))));
1399   if(do_mass_erase){
1400     //Mass erase
1401     retval = mass_erase(target,&hfm_ustat);
1402     err_check_propagate(retval);
1403     last = HFM_SECTOR_COUNT-1;
1404   }else{
1405     for(int i = first;i<=last;i++){
1406       retval = erase_sector(target,i,&hfm_ustat);
1407       err_check_propagate(retval);
1408     }
1409   }
1410   return ERROR_OK;
1411 }
1412
1413 /**
1414  * Algorithm for programming normal p: flash
1415  * Follow state machine from "56F801x Peripheral Reference Manual"@163.
1416  * Registers to set up before calling:
1417 *  r0: TX/RX high address.
1418 *  r2: FM module base address.
1419 *  r3: Destination address in flash.
1420 *
1421 *               hfm_wait:                                           // wait for command to finish
1422 *                       brclr   #0x40,x:(r2+0x13),hfm_wait
1423 *               rx_check:                                           // wait for input buffer full
1424 *                       brclr   #0x01,x:(r0-2),rx_check
1425 *                       move.w  x:(r0),y0                           // read from Rx buffer
1426 *                       move.w  y0,p:(r3)+
1427 *                       move.w  #0x20,x:(r2+0x14)                   // write PGM command
1428 *                       move.w  #0x80,x:(r2+0x13)                   // start the command
1429 *                      brclr       #0x20,X:(R2+0x13),accerr_check  // protection violation check
1430 *                      bfset       #0x20,X:(R2+0x13)               // clear pviol
1431 *                      bra         hfm_wait
1432 *              accerr_check:
1433 *                      brclr       #0x10,X:(R2+0x13),hfm_wait      // access error check
1434 *                      bfset       #0x10,X:(R2+0x13)               // clear accerr
1435 *                       bra         hfm_wait                        // loop
1436 *0x00000073  0x8A460013407D         brclr       #0x40,X:(R2+0x13),*+0
1437 *0x00000076  0xE700                 nop
1438 *0x00000077  0xE700                 nop
1439 *0x00000078  0x8A44FFFE017B         brclr       #1,X:(R0-2),*-2
1440 *0x0000007B  0xE700                 nop
1441 *0x0000007C  0xF514                 move.w      X:(R0),Y0
1442 *0x0000007D  0x8563                 move.w      Y0,P:(R3)+
1443 *0x0000007E  0x864600200014         move.w      #0x20,X:(R2+0x14)
1444 *0x00000081  0x864600800013         move.w      #0x80,X:(R2+0x13)
1445 *0x00000084  0x8A4600132004         brclr       #0x20,X:(R2+0x13),*+7
1446 *0x00000087  0x824600130020         bfset       #0x20,X:(R2+0x13)
1447 *0x0000008A  0xA968                 bra         *-23
1448 *0x0000008B  0x8A4600131065         brclr       #0x10,X:(R2+0x13),*-24
1449 *0x0000008E  0x824600130010         bfset       #0x10,X:(R2+0x13)
1450 *0x00000091  0xA961                 bra         *-30
1451 */
1452 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};
1453 const uint32_t pgm_write_pflash_length = 31;
1454
1455 int dsp5680xx_f_wr(struct target * target, uint8_t *buffer, uint32_t address, uint32_t count, int is_flash_lock){
1456   int retval = ERROR_OK;
1457   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1458     retval = eonce_enter_debug_mode(target,NULL);
1459     err_check_propagate(retval);
1460   }
1461   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1462   // Download the pgm that flashes.
1463   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1464   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
1465   if(!is_flash_lock){
1466     retval = dsp5680xx_write(target, my_favourite_ram_address, 1, pgm_write_pflash_length*2,(uint8_t *) pgm_write_pflash);
1467     err_check_propagate(retval);
1468     retval = dsp5680xx_execute_queue();
1469     err_check_propagate(retval);
1470   }
1471   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1472   // Set hfmdiv
1473   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1474   retval = set_fm_ck_div(target);
1475   err_check_propagate(retval);
1476   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1477   // Setup registers needed by pgm_write_pflash
1478   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1479
1480   dsp5680xx_context.flush = 0;
1481
1482   retval = core_move_long_to_r3(target,address);  // Destination address to r3
1483   err_check_propagate(retval);
1484   core_load_TX_RX_high_addr_to_r0(target);  // TX/RX reg address to r0
1485   err_check_propagate(retval);
1486   retval = core_move_long_to_r2(target,HFM_BASE_ADDR);// FM base address to r2
1487   err_check_propagate(retval);
1488   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1489   // Run flashing program.
1490   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1491   retval = core_move_value_at_r2_disp(target,0x00,HFM_CNFG); // write to HFM_CNFG (lock=0, select bank)
1492   err_check_propagate(retval);
1493   retval = core_move_value_at_r2_disp(target,0x04,HFM_USTAT);// write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1494   err_check_propagate(retval);
1495   retval = core_move_value_at_r2_disp(target,0x10,HFM_USTAT);// clear only one bit at a time
1496   err_check_propagate(retval);
1497   retval = core_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1498   err_check_propagate(retval);
1499   retval = core_move_value_at_r2_disp(target,0x00,HFM_PROT);// write to HMF_PROT, clear protection
1500   err_check_propagate(retval);
1501   retval = core_move_value_at_r2_disp(target,0x00,HFM_PROTB);// write to HMF_PROTB, clear protection
1502   err_check_propagate(retval);
1503   if(count%2){
1504     //TODO implement handling of odd number of words.
1505         retval = ERROR_FAIL;
1506         err_check(retval,"Cannot handle odd number of words.");
1507   }
1508
1509   dsp5680xx_context.flush = 1;
1510   retval = dsp5680xx_execute_queue();
1511   err_check_propagate(retval);
1512
1513   uint32_t drscan_data;
1514   uint16_t tmp = (buffer[0]|(buffer[1]<<8));
1515   retval = core_tx_upper_data(target,tmp,&drscan_data);
1516   err_check_propagate(retval);
1517
1518   retval = dsp5680xx_resume(target,0,my_favourite_ram_address,0,0);
1519   err_check_propagate(retval);
1520
1521   int counter = FLUSH_COUNT_FLASH;
1522   dsp5680xx_context.flush = 0;
1523   uint32_t i;
1524   for(i=1; (i<count/2)&&(i<HFM_SIZE_WORDS); i++){
1525     if(--counter==0){
1526       dsp5680xx_context.flush = 1;
1527       counter = FLUSH_COUNT_FLASH;
1528     }
1529     tmp = (buffer[2*i]|(buffer[2*i+1]<<8));
1530     retval = core_tx_upper_data(target,tmp,&drscan_data);
1531         if(retval!=ERROR_OK){
1532           dsp5680xx_context.flush = 1;
1533           err_check_propagate(retval);
1534         }
1535         dsp5680xx_context.flush = 0;
1536   }
1537   dsp5680xx_context.flush = 1;
1538   if(!is_flash_lock){
1539     // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1540     // Verify flash (skip when exec lock sequence)
1541     // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1542     uint16_t signature;
1543     uint16_t pc_crc;
1544     retval =  dsp5680xx_f_signature(target,address,i,&signature);
1545     err_check_propagate(retval);
1546     pc_crc = perl_crc(buffer,i);
1547     if(pc_crc != signature){
1548       retval = ERROR_FAIL;
1549       err_check(retval,"Flashed data failed CRC check, flash again!");
1550     }
1551   }
1552   return retval;
1553 }
1554
1555 // Reset state machine
1556 int reset_jtag(void){
1557   int retval;
1558   tap_state_t states[2];
1559   const char *cp = "RESET";
1560   states[0] = tap_state_by_name(cp);
1561   retval = jtag_add_statemove(states[0]);
1562   err_check_propagate(retval);
1563   retval = jtag_execute_queue();
1564   err_check_propagate(retval);
1565   jtag_add_pathmove(0, states + 1);
1566   retval = jtag_execute_queue();
1567   return retval;
1568 }
1569
1570 int dsp5680xx_f_unlock(struct target * target){
1571   int retval = ERROR_OK;
1572   uint16_t eonce_status;
1573   uint32_t instr;
1574   uint32_t ir_out;
1575   uint16_t instr_16;
1576   uint16_t read_16;
1577   struct jtag_tap * tap_chp;
1578   struct jtag_tap * tap_cpu;
1579   tap_chp = jtag_tap_by_string("dsp568013.chp");
1580   if(tap_chp == NULL){
1581     retval = ERROR_FAIL;
1582     err_check(retval,"Failed to get master tap.");
1583   }
1584   tap_cpu = jtag_tap_by_string("dsp568013.cpu");
1585   if(tap_cpu == NULL){
1586     retval = ERROR_FAIL;
1587     err_check(retval,"Failed to get master tap.");
1588   }
1589
1590   retval = eonce_enter_debug_mode(target,&eonce_status);
1591   if(retval == ERROR_OK){
1592     LOG_WARNING("Memory was not locked.");
1593     return retval;
1594   }
1595
1596   jtag_add_reset(0,1);
1597   usleep(TIME_DIV_FREESCALE*200*1000);
1598
1599   retval = reset_jtag();
1600   err_check(retval,"Failed to reset JTAG state machine");
1601   usleep(150);
1602
1603   // Enable core tap
1604   tap_chp->enabled = true;
1605   retval = switch_tap(target,tap_chp,tap_cpu);
1606   err_check_propagate(retval);
1607
1608   instr = JTAG_INSTR_DEBUG_REQUEST;
1609   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
1610   err_check_propagate(retval);
1611   usleep(TIME_DIV_FREESCALE*100*1000);
1612   jtag_add_reset(0,0);
1613   usleep(TIME_DIV_FREESCALE*300*1000);
1614
1615   // Enable master tap
1616   retval = switch_tap(target,tap_chp,tap_cpu);
1617   err_check_propagate(retval);
1618
1619   // Execute mass erase to unlock
1620   instr = MASTER_TAP_CMD_FLASH_ERASE;
1621   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_MASTER_TAP_IRLEN);
1622   err_check_propagate(retval);
1623
1624   instr = HFM_CLK_DEFAULT;
1625   retval =  dsp5680xx_drscan(target,(uint8_t *) & instr,(uint8_t *) & ir_out,16);
1626   err_check_propagate(retval);
1627
1628   usleep(TIME_DIV_FREESCALE*150*1000);
1629   jtag_add_reset(0,1);
1630   usleep(TIME_DIV_FREESCALE*200*1000);
1631
1632   retval = reset_jtag();
1633   err_check(retval,"Failed to reset JTAG state machine");
1634   usleep(150);
1635
1636   instr = 0x0606ffff;
1637   retval =  dsp5680xx_drscan(target,(uint8_t *) & instr,(uint8_t *) & ir_out,32);
1638   err_check_propagate(retval);
1639
1640   // enable core tap
1641   instr = 0x5;
1642   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_MASTER_TAP_IRLEN);
1643   err_check_propagate(retval);
1644   instr = 0x2;
1645   retval =  dsp5680xx_drscan(target,(uint8_t *) & instr,(uint8_t *) & ir_out,4);
1646   err_check_propagate(retval);
1647
1648   tap_cpu->enabled = true;
1649   tap_chp->enabled = false;
1650
1651   instr = JTAG_INSTR_ENABLE_ONCE;
1652   //Two rounds of jtag 0x6  (enable eonce) to enable EOnCE.
1653   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
1654   err_check_propagate(retval);
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   instr_16 = 0x1;
1659   retval = dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,8);
1660   instr_16 = 0x20;
1661   retval = dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,8);
1662   usleep(TIME_DIV_FREESCALE*100*1000);
1663   jtag_add_reset(0,0);
1664   usleep(TIME_DIV_FREESCALE*300*1000);
1665   return retval;
1666 }
1667
1668 int dsp5680xx_f_lock(struct target * target){
1669   int retval;
1670   uint16_t lock_word[] = {HFM_LOCK_FLASH,HFM_LOCK_FLASH};
1671   retval = dsp5680xx_f_wr(target,(uint8_t *)(lock_word),HFM_LOCK_ADDR_L,4,1);
1672   err_check_propagate(retval);
1673   return retval;
1674   jtag_add_reset(0,1);
1675   usleep(TIME_DIV_FREESCALE*200*1000);
1676
1677   retval = reset_jtag();
1678   err_check(retval,"Failed to reset JTAG state machine");
1679   usleep(TIME_DIV_FREESCALE*100*1000);
1680   jtag_add_reset(0,0);
1681   usleep(TIME_DIV_FREESCALE*300*1000);
1682
1683   return retval;
1684 }
1685
1686 static int dsp5680xx_step(struct target * target,int current, uint32_t address, int handle_breakpoints){
1687   err_check(ERROR_FAIL,"Not implemented yet.");
1688 }
1689
1690 /** Holds methods for dsp5680xx targets. */
1691 struct target_type dsp5680xx_target = {
1692   .name = "dsp5680xx",
1693
1694   .poll = dsp5680xx_poll,
1695   .arch_state = dsp5680xx_arch_state,
1696
1697   .target_request_data = NULL,
1698
1699   .halt = dsp5680xx_halt,
1700   .resume = dsp5680xx_resume,
1701   .step = dsp5680xx_step,
1702
1703   .write_buffer = dsp5680xx_write_buffer,
1704   .read_buffer = dsp5680xx_read_buffer,
1705
1706   .assert_reset = dsp5680xx_assert_reset,
1707   .deassert_reset = dsp5680xx_deassert_reset,
1708   .soft_reset_halt = dsp5680xx_soft_reset_halt,
1709
1710   .read_memory = dsp5680xx_read,
1711   .write_memory = dsp5680xx_write,
1712   .bulk_write_memory = dsp5680xx_bulk_write_memory,
1713
1714   .checksum_memory = dsp5680xx_checksum_memory,
1715
1716   .target_create = dsp5680xx_target_create,
1717   .init_target = dsp5680xx_init_target,
1718 };