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