git.sur5r.net Git - openocd/blob - src/target/dsp5680xx.c

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