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

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