git.gag.com Git - fw/openocd/blob - src/target/dsp5680xx.c

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