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

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