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