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

   1 /***************************************************************************
   2  *   Copyright (C) 2011 by Rodrigo L. Rosa                                 *
   3  *   rodrigorosa.LG@gmail.com                                              *
   4  *                                                                         *
   5  *   Based on dsp563xx_once.h written by Mathias Kuester                   *
   6  *   mkdorg@users.sourceforge.net                                          *
   7  *                                                                         *
   8  *   This program is free software; you can redistribute it and/or modify  *
   9  *   it under the terms of the GNU General Public License as published by  *
  10  *   the Free Software Foundation; either version 2 of the License, or     *
  11  *   (at your option) any later version.                                   *
  12  *                                                                         *
  13  *   This program is distributed in the hope that it will be useful,       *
  14  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  15  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  16  *   GNU General Public License for more details.                          *
  17  *                                                                         *
  18  *   You should have received a copy of the GNU General Public License     *
  19  *   along with this program; if not, write to the                         *
  20  *   Free Software Foundation, Inc.,                                       *
  21  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  22  ***************************************************************************/
  23 #ifdef HAVE_CONFIG_H
  24 #include "config.h"
  25 #endif
  26
  27 #include "target.h"
  28 #include "target_type.h"
  29 #include "dsp5680xx.h"
  30
  31 struct dsp5680xx_common dsp5680xx_context;
  32
  33
  34 #define err_check(retval,err_msg) if(retval != ERROR_OK){LOG_ERROR("%s: %d %s.",__FUNCTION__,__LINE__,err_msg);return retval;}
  35 #define err_check_propagate(retval) if(retval!=ERROR_OK){return retval;}
  36
  37 int dsp5680xx_execute_queue(void){
  38   int retval;
  39   retval = jtag_execute_queue();
  40   err_check_propagate(retval);
  41   return retval;
  42 }
  43
  44 static int dsp5680xx_drscan(struct target * target, uint8_t * data_to_shift_into_dr, uint8_t * data_shifted_out_of_dr, int len){
  45 // -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
  46 //
  47 // Inputs:
  48 //     - data_to_shift_into_dr: This is the data that will be shifted into the JTAG DR reg.
  49 //     - data_shifted_out_of_dr: The data that will be shifted out of the JTAG DR reg will stored here
  50 //     - len: Length of the data to be shifted to JTAG DR.
  51 //
  52 // Note:  If  data_shifted_out_of_dr  == NULL, discard incoming bits.
  53 //
  54 // -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
  55   int retval = ERROR_OK;
  56   if (NULL == target->tap){
  57         retval = ERROR_FAIL;
  58         err_check(retval,"Invalid tap");
  59   }
  60   if (len > 32){
  61         retval = ERROR_FAIL;
  62         err_check(retval,"dr_len overflow, maxium is 32");
  63   }
  64   //TODO what values of len are valid for jtag_add_plain_dr_scan?
  65   //can i send as many bits as i want?
  66   //is the casting necessary?
  67   jtag_add_plain_dr_scan(len,data_to_shift_into_dr,data_shifted_out_of_dr, TAP_IDLE);
  68   if(dsp5680xx_context.flush){
  69         retval = dsp5680xx_execute_queue();
  70         err_check_propagate(retval);
  71   }
  72   if(data_shifted_out_of_dr!=NULL){
  73     LOG_DEBUG("Data read (%d bits): 0x%04X",len,*data_shifted_out_of_dr);
  74   }else
  75     LOG_DEBUG("Data read was discarded.");
  76   return retval;
  77 }
  78
  79 static int dsp5680xx_irscan(struct target * target, uint32_t * data_to_shift_into_ir, uint32_t * data_shifted_out_of_ir, uint8_t ir_len){
  80 // -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
  81 // Inputs:
  82 //     - data_to_shift_into_ir: This is the data that will be shifted into the JTAG IR reg.
  83 //     - data_shifted_out_of_ir: The data that will be shifted out of the JTAG IR reg will stored here
  84 //     - len: Length of the data to be shifted to JTAG IR.
  85 //
  86 // -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
  87   int retval = ERROR_OK;
  88   if (NULL == target->tap){
  89         retval = ERROR_FAIL;
  90         err_check(retval,"Invalid tap");
  91   }
  92   if (ir_len != target->tap->ir_length){
  93     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, uint32_t * data_read, int num_bits){
 120   uint32_t bogus_instr;
 121   int retval = dsp5680xx_drscan(target,(uint8_t *) & bogus_instr,(uint8_t *) 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 int jtag_data_write(struct target * target, uint32_t instr,int num_bits, uint32_t * data_read){
 131   int retval;
 132   uint32_t data_read_dummy;
 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, uint16_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,(uint32_t *)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,uint16_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,(uint32_t *)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   uint32_t tmp = 0;
 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,(uint16_t *)&tmp);
 509   err_check_propagate(retval);
 510   LOG_USER("PC value: 0x%06X\n",tmp);
 511   dsp5680xx_context.stored_pc = (uint32_t)tmp;
 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;
 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, uint16_t * data_read, int r_pmem){
 676   //TODO add error control!
 677   int retval;
 678   retval = eonce_move_long_to_r0(target,address);
 679   err_check_propagate(retval);
 680   if(r_pmem)
 681     retval = eonce_move_at_pr0_inc_to_y0(target);
 682   else
 683     retval = eonce_move_at_r0_to_y0(target);
 684   err_check_propagate(retval);
 685   retval = eonce_load_TX_RX_to_r0(target);
 686   err_check_propagate(retval);
 687   retval = eonce_move_y0_at_r0(target);
 688   err_check_propagate(retval);
 689   // at this point the data i want is at the reg eonce can read
 690   retval = eonce_rx_lower_data(target,data_read);
 691   err_check_propagate(retval);
 692   LOG_DEBUG("%s: Data read from 0x%06X: 0x%04X",__FUNCTION__, address,*data_read);
 693   return retval;
 694 }
 695
 696 static int dsp5680xx_read_32_single(struct target * target, uint32_t address, uint32_t * data_read, int r_pmem){
 697   int retval;
 698   address = (address & 0xFFFFFE);
 699   // Get data to an intermediate register
 700   retval = eonce_move_long_to_r0(target,address);
 701   err_check_propagate(retval);
 702   if(r_pmem){
 703     retval = eonce_move_at_pr0_inc_to_y0(target);
 704         err_check_propagate(retval);
 705     retval = eonce_move_at_pr0_inc_to_y1(target);
 706         err_check_propagate(retval);
 707   }else{
 708     retval = eonce_move_at_r0_inc_to_y0(target);
 709         err_check_propagate(retval);
 710     retval = eonce_move_at_r0_to_y1(target);
 711         err_check_propagate(retval);
 712   }
 713   // Get lower part of data to TX/RX
 714   retval = eonce_load_TX_RX_to_r0(target);
 715   err_check_propagate(retval);
 716   retval = eonce_move_y0_at_r0_inc(target); // This also load TX/RX high to r0
 717   err_check_propagate(retval);
 718   // Get upper part of data to TX/RX
 719   retval = eonce_move_y1_at_r0(target);
 720   err_check_propagate(retval);
 721   // at this point the data i want is at the reg eonce can read
 722   retval = eonce_rx_lower_data(target,(uint16_t * )data_read);
 723   err_check_propagate(retval);
 724   uint16_t tmp;
 725   retval = eonce_rx_upper_data(target,&tmp);
 726   err_check_propagate(retval);
 727   *data_read = ((tmp<<16) | (*data_read));//This enables OpenOCD crc to succeed (when it should)
 728   return retval;
 729 }
 730
 731 static int dsp5680xx_read(struct target * target, uint32_t address, unsigned size, unsigned count, uint8_t * buffer){
 732   if(target->state != TARGET_HALTED){
 733     LOG_USER("Target must be halted.");
 734     return ERROR_OK;
 735   }
 736   uint32_t * buff32 = (uint32_t *) buffer;
 737   uint16_t * buff16 = (uint16_t *) buffer;
 738   int retval = ERROR_OK;
 739   int pmem = 1;
 740   uint16_t tmp_wrd;
 741
 742   retval = dsp5680xx_convert_address(&address, &pmem);
 743   err_check_propagate(retval);
 744
 745   dsp5680xx_context.flush = 0;
 746   int counter = FLUSH_COUNT_READ_WRITE;
 747
 748   for (unsigned i=0; i<count; i++){
 749     if(--counter==0){
 750       dsp5680xx_context.flush = 1;
 751       counter = FLUSH_COUNT_FLASH;
 752     }
 753     switch (size){
 754     case 1:
 755       if(!(i%2)){
 756                 retval = dsp5680xx_read_16_single(target, address + i/2, &tmp_wrd, pmem);
 757                 buffer[i] = (uint8_t) (tmp_wrd>>8);
 758                 buffer[i+1] = (uint8_t) (tmp_wrd&0xff);
 759       }
 760       break;
 761     case 2:
 762       retval = dsp5680xx_read_16_single(target, address + i, buff16 + i, pmem);
 763       break;
 764     case 4:
 765       retval = dsp5680xx_read_32_single(target, address + 2*i, buff32 + i, pmem);
 766       break;
 767     default:
 768       LOG_USER("%s: Invalid read size.",__FUNCTION__);
 769       break;
 770     }
 771         err_check_propagate(retval);
 772     dsp5680xx_context.flush = 0;
 773   }
 774
 775   dsp5680xx_context.flush = 1;
 776   retval = dsp5680xx_execute_queue();
 777   err_check_propagate(retval);
 778
 779   return retval;
 780 }
 781
 782 static int dsp5680xx_write_16_single(struct target *target, uint32_t address, uint16_t data, uint8_t w_pmem){
 783   int retval = 0;
 784   retval = eonce_move_long_to_r0(target,address);
 785   err_check_propagate(retval);
 786   if(w_pmem){
 787     retval = eonce_move_value_to_y0(target,data);
 788         err_check_propagate(retval);
 789     retval = eonce_move_y0_at_pr0_inc(target);
 790         err_check_propagate(retval);
 791   }else{
 792     retval = eonce_move_value_at_r0(target,data);
 793         err_check_propagate(retval);
 794   }
 795   return retval;
 796 }
 797
 798 static int dsp5680xx_write_32_single(struct target *target, uint32_t address, uint32_t data, int w_pmem){
 799   int retval = 0;
 800   retval = eonce_move_long_to_r0(target,address);
 801   err_check_propagate(retval);
 802   retval = eonce_move_long_to_y(target,data);
 803   err_check_propagate(retval);
 804   if(w_pmem)
 805     retval = eonce_move_y0_at_pr0_inc(target);
 806   else
 807     retval = eonce_move_y0_at_r0_inc(target);
 808   err_check_propagate(retval);
 809   if(w_pmem)
 810     retval = eonce_move_y1_at_pr0_inc(target);
 811   else
 812     retval = eonce_move_y1_at_r0_inc(target);
 813   err_check_propagate(retval);
 814   return retval;
 815 }
 816
 817 static int dsp5680xx_write_8(struct target * target, uint32_t address, uint32_t count, uint8_t * data, int pmem){
 818   if(target->state != TARGET_HALTED){
 819     LOG_ERROR("%s: Target must be halted.",__FUNCTION__);
 820     return ERROR_OK;
 821   };
 822   int retval = 0;
 823   uint16_t * data_w = (uint16_t *)data;
 824   uint32_t iter;
 825
 826   int counter = FLUSH_COUNT_READ_WRITE;
 827   for(iter = 0; iter<count/2; iter++){
 828     if(--counter==0){
 829       dsp5680xx_context.flush = 1;
 830       counter = FLUSH_COUNT_READ_WRITE;
 831     }
 832     retval = dsp5680xx_write_16_single(target,address+iter,data_w[iter], pmem);
 833     if(retval != ERROR_OK){
 834       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 835       dsp5680xx_context.flush = 1;
 836       return retval;
 837     }
 838     dsp5680xx_context.flush = 0;
 839   }
 840   dsp5680xx_context.flush = 1;
 841
 842   // Only one byte left, let's not overwrite the other byte (mem is 16bit)
 843   // Need to retrieve the part we do not want to overwrite.
 844   uint16_t data_old;
 845   if((count==1)||(count%2)){
 846     retval = dsp5680xx_read(target,address+iter,1,1,(uint8_t *)&data_old);
 847         err_check_propagate(retval);
 848     if(count==1)
 849       data_old=(((data_old&0xff)<<8)|data[0]);// preserve upper byte
 850     else
 851       data_old=(((data_old&0xff)<<8)|data[2*iter+1]);
 852     retval = dsp5680xx_write_16_single(target,address+iter,data_old, pmem);
 853         err_check_propagate(retval);
 854   }
 855   return retval;
 856 }
 857
 858 static int dsp5680xx_write_16(struct target * target, uint32_t address, uint32_t count, uint16_t * data, int pmem){
 859   int retval = ERROR_OK;
 860   if(target->state != TARGET_HALTED){
 861         retval = ERROR_TARGET_NOT_HALTED;
 862         err_check(retval,"Target must be halted.");
 863   };
 864   uint32_t iter;
 865   int counter = FLUSH_COUNT_READ_WRITE;
 866
 867   for(iter = 0; iter<count; iter++){
 868         if(--counter==0){
 869           dsp5680xx_context.flush = 1;
 870       counter = FLUSH_COUNT_READ_WRITE;
 871         }
 872     retval = dsp5680xx_write_16_single(target,address+iter,data[iter], pmem);
 873     if(retval != ERROR_OK){
 874       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 875           dsp5680xx_context.flush = 1;
 876       return retval;
 877     }
 878         dsp5680xx_context.flush = 0;
 879   }
 880   dsp5680xx_context.flush = 1;
 881   return retval;
 882 }
 883
 884 static int dsp5680xx_write_32(struct target * target, uint32_t address, uint32_t count, uint32_t * data, int pmem){
 885   int retval = ERROR_OK;
 886   if(target->state != TARGET_HALTED){
 887         retval = ERROR_TARGET_NOT_HALTED;
 888         err_check(retval,"Target must be halted.");
 889   };
 890   uint32_t iter;
 891   int counter = FLUSH_COUNT_READ_WRITE;
 892
 893   for(iter = 0; iter<count; iter++){
 894         if(--counter==0){
 895           dsp5680xx_context.flush = 1;
 896       counter = FLUSH_COUNT_READ_WRITE;
 897         }
 898     retval = dsp5680xx_write_32_single(target,address+(iter<<1),data[iter], pmem);
 899     if(retval != ERROR_OK){
 900       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 901           dsp5680xx_context.flush = 1;
 902       return retval;
 903     }
 904         dsp5680xx_context.flush = 0;
 905   }
 906   dsp5680xx_context.flush = 1;
 907   return retval;
 908 }
 909
 910 /**
 911  * Writes @buffer to memory.
 912  * The parameter @address determines whether @buffer should be written to P: (program) memory or X: (data) memory.
 913  *
 914  * @param target
 915  * @param address
 916  * @param size Bytes (1), Half words (2), Words (4).
 917  * @param count In bytes.
 918  * @param buffer
 919  *
 920  * @return
 921  */
 922 static int dsp5680xx_write(struct target *target, uint32_t address, uint32_t size, uint32_t count, const uint8_t * buffer){
 923   //TODO Cannot write 32bit to odd address, will write 0x12345678  as 0x5678 0x0012
 924   if(target->state != TARGET_HALTED){
 925     LOG_USER("Target must be halted.");
 926     return ERROR_OK;
 927   }
 928   int retval = 0;
 929   int p_mem = 1;
 930   retval = dsp5680xx_convert_address(&address, &p_mem);
 931   err_check_propagate(retval);
 932
 933   switch (size){
 934   case 1:
 935     retval = dsp5680xx_write_8(target, address, count,(uint8_t *) buffer, p_mem);
 936     break;
 937   case 2:
 938     retval = dsp5680xx_write_16(target, address, count, (uint16_t *)buffer, p_mem);
 939       break;
 940   case 4:
 941     retval = dsp5680xx_write_32(target, address, count, (uint32_t *)buffer, p_mem);
 942     break;
 943   default:
 944         retval = ERROR_TARGET_DATA_ABORT;
 945         err_check(retval,"Invalid data size.");
 946         break;
 947   }
 948   return retval;
 949 }
 950
 951 static int dsp5680xx_bulk_write_memory(struct target * target,uint32_t address, uint32_t aligned, const uint8_t * buffer){
 952   LOG_ERROR("Not implemented yet.");
 953   return ERROR_FAIL;
 954 }
 955
 956 static int dsp5680xx_write_buffer(struct target * target, uint32_t address, uint32_t size, const uint8_t * buffer){
 957   if(target->state != TARGET_HALTED){
 958     LOG_USER("Target must be halted.");
 959     return ERROR_OK;
 960   }
 961   return dsp5680xx_write(target, address, 1, size, buffer);
 962 }
 963
 964 /**
 965  * This function is called by verify_image, it is used to read data from memory.
 966  *
 967  * @param target
 968  * @param address Word addressing.
 969  * @param size In bytes.
 970  * @param buffer
 971  *
 972  * @return
 973  */
 974 static int dsp5680xx_read_buffer(struct target * target, uint32_t address, uint32_t size, uint8_t * buffer){
 975   if(target->state != TARGET_HALTED){
 976     LOG_USER("Target must be halted.");
 977     return ERROR_OK;
 978   }
 979   // The "/2" solves the byte/word addressing issue.
 980   return dsp5680xx_read(target,address,2,size/2,buffer);
 981 }
 982
 983 /**
 984  * This function is not implemented.
 985  * It returns an error in order to get OpenOCD to do read out the data and calculate the CRC, or try a binary comparison.
 986  *
 987  * @param target
 988  * @param address Start address of the image.
 989  * @param size In bytes.
 990  * @param checksum
 991  *
 992  * @return
 993  */
 994 static int dsp5680xx_checksum_memory(struct target * target, uint32_t address, uint32_t size, uint32_t * checksum){
 995   return ERROR_FAIL;
 996 }
 997
 998 /**
 999  * 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.
1000  * This algorithm is based on the perl script available from the Freescale website at FAQ 25630.
1001  *
1002  * @param buff16
1003  * @param word_count
1004  *
1005  * @return
1006  */
1007 static int perl_crc(uint16_t * buff16,uint32_t  word_count){
1008   uint16_t checksum = 0xffff;
1009   uint16_t data,fbmisr;
1010   uint32_t i;
1011   for(i=0;i<word_count;i++){
1012     data = buff16[i];
1013     fbmisr = (checksum & 2)>>1 ^ (checksum & 4)>>2 ^ (checksum & 16)>>4 ^ (checksum & 0x8000)>>15;
1014     checksum = (data ^ ((checksum << 1) | fbmisr));
1015   }
1016   i--;
1017   for(;!(i&0x80000000);i--){
1018     data = buff16[i];
1019     fbmisr = (checksum & 2)>>1 ^ (checksum & 4)>>2 ^ (checksum & 16)>>4 ^ (checksum & 0x8000)>>15;
1020     checksum = (data ^ ((checksum << 1) | fbmisr));
1021   }
1022   return checksum;
1023 }
1024
1025 /**
1026  * Resets the SIM. (System Integration Module).
1027  *
1028  * @param target
1029  *
1030  * @return
1031  */
1032 int dsp5680xx_f_SIM_reset(struct target * target){
1033   int retval = ERROR_OK;
1034   uint16_t sim_cmd = SIM_CMD_RESET;
1035   uint32_t sim_addr;
1036   if(strcmp(target->tap->chip,"dsp568013")==0){
1037         sim_addr = MC568013_SIM_BASE_ADDR+S_FILE_DATA_OFFSET;
1038         retval = dsp5680xx_write(target,sim_addr,1,2,(const uint8_t *)&sim_cmd);
1039         err_check_propagate(retval);
1040   }
1041   return retval;
1042 }
1043
1044 /**
1045  * Halts the core and resets the SIM. (System Integration Module).
1046  *
1047  * @param target
1048  *
1049  * @return
1050  */
1051 static int dsp5680xx_soft_reset_halt(struct target *target){
1052   //TODO is this what this function is expected to do...?
1053   int retval;
1054   retval = dsp5680xx_halt(target);
1055   err_check_propagate(retval);
1056   retval = dsp5680xx_f_SIM_reset(target);
1057   err_check_propagate(retval);
1058   return retval;
1059 }
1060
1061 int dsp5680xx_f_protect_check(struct target * target, uint16_t * protected) {
1062   uint16_t aux;
1063   int retval;
1064   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1065     retval = dsp5680xx_halt(target);
1066         err_check_propagate(retval);
1067   }
1068   if(protected == NULL){
1069     err_check(ERROR_FAIL,"NULL pointer not valid.");
1070   }
1071   retval = dsp5680xx_read_16_single(target,HFM_BASE_ADDR|HFM_PROT,&aux,0);
1072   err_check_propagate(retval);
1073   *protected = aux;
1074   return retval;
1075 }
1076
1077 /**
1078  * Executes a command on the FM module. Some commands use the parameters @address and @data, others ignore them.
1079  *
1080  * @param target
1081  * @param command Command to execute.
1082  * @param address Command parameter.
1083  * @param data Command parameter.
1084  * @param hfm_ustat FM status register.
1085  * @param pmem Address is P: (program) memory (@pmem==1) or X: (data) memory (@pmem==0)
1086  *
1087  * @return
1088  */
1089 static int dsp5680xx_f_execute_command(struct target * target, uint16_t command, uint32_t address, uint32_t data, uint16_t * hfm_ustat, int pmem){
1090   int retval;
1091   retval = eonce_load_TX_RX_high_to_r0(target);
1092   err_check_propagate(retval);
1093   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);
1094   err_check_propagate(retval);
1095   uint16_t i;
1096   int watchdog = 100;
1097   do{
1098     retval = eonce_move_at_r2_disp_to_y0(target,HFM_USTAT);     // read HMF_USTAT
1099         err_check_propagate(retval);
1100     retval = eonce_move_y0_at_r0(target);
1101         err_check_propagate(retval);
1102     retval = eonce_rx_upper_data(target,&i);
1103         err_check_propagate(retval);
1104     if((watchdog--)==1){
1105       retval = ERROR_TARGET_FAILURE;
1106       err_check(retval,"FM execute command failed.");
1107     }
1108   }while (!(i&0x40));                           // wait until current command is complete
1109
1110   dsp5680xx_context.flush = 0;
1111
1112   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 ???
1113   err_check_propagate(retval);
1114   retval = eonce_move_value_at_r2_disp(target,0x04,HFM_USTAT);          // write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1115   err_check_propagate(retval);
1116   retval = eonce_move_value_at_r2_disp(target,0x10,HFM_USTAT);          // clear only one bit at a time
1117   err_check_propagate(retval);
1118   retval = eonce_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1119   err_check_propagate(retval);
1120   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROT);           // write to HMF_PROT, clear protection
1121   err_check_propagate(retval);
1122   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROTB);          // write to HMF_PROTB, clear protection
1123   err_check_propagate(retval);
1124   retval = eonce_move_value_to_y0(target,data);
1125   err_check_propagate(retval);
1126   retval = eonce_move_long_to_r3(target,address);                       // write to the flash block
1127   err_check_propagate(retval);
1128   if (pmem){
1129     retval = eonce_move_y0_at_pr3_inc(target);
1130         err_check_propagate(retval);
1131   }else{
1132     retval = eonce_move_y0_at_r3(target);
1133         err_check_propagate(retval);
1134   }
1135   retval = eonce_move_value_at_r2_disp(target,command,HFM_CMD); // write command to the HFM_CMD reg
1136   err_check_propagate(retval);
1137   retval = eonce_move_value_at_r2_disp(target,0x80,HFM_USTAT);          // start the command
1138   err_check_propagate(retval);
1139
1140   dsp5680xx_context.flush = 1;
1141   retval = dsp5680xx_execute_queue();
1142   err_check_propagate(retval);
1143
1144   watchdog = 100;
1145   do{
1146     retval = eonce_move_at_r2_disp_to_y0(target,HFM_USTAT);     // read HMF_USTAT
1147         err_check_propagate(retval);
1148     retval = eonce_move_y0_at_r0(target);
1149         err_check_propagate(retval);
1150         retval = eonce_rx_upper_data(target,&i);
1151         err_check_propagate(retval);
1152     if((watchdog--)==1){
1153           retval = ERROR_TARGET_FAILURE;
1154       err_check(retval,"FM execution did not finish.");
1155     }
1156   }while (!(i&0x40));       // wait until the command is complete
1157   *hfm_ustat = i;
1158   if (i&HFM_USTAT_MASK_PVIOL_ACCER){
1159     retval = ERROR_TARGET_FAILURE;
1160     err_check(retval,"pviol and/or accer bits set. HFM command execution error");
1161   }
1162   return ERROR_OK;
1163 }
1164
1165 /**
1166  * 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.)
1167  *
1168  * @param target
1169  *
1170  * @return
1171  */
1172 static int eonce_set_hfmdiv(struct target * target){
1173   uint16_t i;
1174   int retval;
1175   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);
1176   err_check_propagate(retval);
1177   retval = eonce_load_TX_RX_high_to_r0(target);
1178   err_check_propagate(retval);
1179   retval = eonce_move_at_r2_to_y0(target);// read HFM_CLKD
1180   err_check_propagate(retval);
1181   retval = eonce_move_y0_at_r0(target);
1182   err_check_propagate(retval);
1183   retval = eonce_rx_upper_data(target,&i);
1184   err_check_propagate(retval);
1185   unsigned int hfm_at_wrong_value = 0;
1186   if ((i&0x7f)!=HFM_CLK_DEFAULT) {
1187     LOG_DEBUG("HFM CLK divisor contained incorrect value (0x%02X).",i&0x7f);
1188     hfm_at_wrong_value = 1;
1189   }else{
1190     LOG_DEBUG("HFM CLK divisor was already set to correct value (0x%02X).",i&0x7f);
1191     return ERROR_OK;
1192   }
1193   retval = eonce_move_value_at_r2(target,HFM_CLK_DEFAULT);      // write HFM_CLKD
1194   err_check_propagate(retval);
1195   retval = eonce_move_at_r2_to_y0(target); // verify HFM_CLKD
1196   err_check_propagate(retval);
1197   retval = eonce_move_y0_at_r0(target);
1198   err_check_propagate(retval);
1199   retval = eonce_rx_upper_data(target,&i);
1200   err_check_propagate(retval);
1201   if (i!=(0x80|(HFM_CLK_DEFAULT&0x7f))) {
1202         retval = ERROR_TARGET_FAILURE;
1203         err_check(retval,"Unable to set HFM CLK divisor.");
1204   }
1205   if(hfm_at_wrong_value)
1206     LOG_DEBUG("HFM CLK divisor set to 0x%02x.",i&0x7f);
1207   return ERROR_OK;
1208 }
1209
1210 /**
1211  * 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.
1212  *
1213  * @param target
1214  * @param address Start of flash array where the signature should be calculated.
1215  * @param words Number of words over which the signature should be calculated.
1216  * @param signature Value calculated by the FM.
1217  *
1218  * @return
1219  */
1220 static int dsp5680xx_f_signature(struct target * target, uint32_t address, uint32_t words, uint16_t * signature){
1221   int retval;
1222   uint16_t hfm_ustat;
1223   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1224     retval = eonce_enter_debug_mode(target,NULL);
1225     err_check_propagate(retval);
1226   }
1227   retval = dsp5680xx_f_execute_command(target,HFM_CALCULATE_DATA_SIGNATURE,address,words,&hfm_ustat,1);
1228   err_check_propagate(retval);
1229   retval = dsp5680xx_read_16_single(target, HFM_BASE_ADDR|HFM_DATA, signature, 0);
1230   return retval;
1231 }
1232
1233 int dsp5680xx_f_erase_check(struct target * target, uint8_t * erased,uint32_t sector){
1234   int retval;
1235   uint16_t hfm_ustat;
1236   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1237     retval = dsp5680xx_halt(target);
1238     err_check_propagate(retval);
1239   }
1240   // Check if chip is already erased.
1241   retval = dsp5680xx_f_execute_command(target,HFM_ERASE_VERIFY,HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2,0,&hfm_ustat,1); // blank check
1242   err_check_propagate(retval);
1243   if(erased!=NULL)
1244     *erased = (uint8_t)(hfm_ustat&HFM_USTAT_MASK_BLANK);
1245   return retval;
1246 }
1247
1248 /**
1249  * Executes the FM page erase command.
1250  *
1251  * @param target
1252  * @param sector Page to erase.
1253  * @param hfm_ustat FM module status register.
1254  *
1255  * @return
1256  */
1257 static int erase_sector(struct target * target, int sector, uint16_t * hfm_ustat){
1258   int retval;
1259   retval = dsp5680xx_f_execute_command(target,HFM_PAGE_ERASE,HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2,0,hfm_ustat,1);
1260   err_check_propagate(retval);
1261   return retval;
1262 }
1263
1264 /**
1265  * Executes the FM mass erase command. Erases the flash array completely.
1266  *
1267  * @param target
1268  * @param hfm_ustat FM module status register.
1269  *
1270  * @return
1271  */
1272 static int mass_erase(struct target * target, uint16_t * hfm_ustat){
1273   int retval;
1274   retval = dsp5680xx_f_execute_command(target,HFM_MASS_ERASE,0,0,hfm_ustat,1);
1275   return retval;
1276 }
1277
1278 int dsp5680xx_f_erase(struct target * target, int first, int last){
1279   int retval;
1280   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1281     retval = dsp5680xx_halt(target);
1282     err_check_propagate(retval);
1283   }
1284   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1285   // Reset SIM
1286   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1287   retval = dsp5680xx_f_SIM_reset(target);
1288   err_check_propagate(retval);
1289   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1290   // Set hfmdiv
1291   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1292   retval = eonce_set_hfmdiv(target);
1293   err_check_propagate(retval);
1294
1295   uint16_t hfm_ustat;
1296   int do_mass_erase = ((!(first|last)) || ((first==0)&&(last == (HFM_SECTOR_COUNT-1))));
1297   if(do_mass_erase){
1298     //Mass erase
1299     retval = mass_erase(target,&hfm_ustat);
1300     err_check_propagate(retval);
1301     last = HFM_SECTOR_COUNT-1;
1302   }else{
1303     for(int i = first;i<=last;i++){
1304       retval = erase_sector(target,i,&hfm_ustat);
1305       err_check_propagate(retval);
1306     }
1307   }
1308   return ERROR_OK;
1309 }
1310
1311 /**
1312  * Algorithm for programming normal p: flash
1313  * Follow state machine from "56F801x Peripheral Reference Manual"@163.
1314  * Registers to set up before calling:
1315 *  r0: TX/RX high address.
1316 *  r2: FM module base address.
1317 *  r3: Destination address in flash.
1318 *
1319 *               hfm_wait:                                           // wait for command to finish
1320 *                       brclr   #0x40,x:(r2+0x13),hfm_wait
1321 *               rx_check:                                           // wait for input buffer full
1322 *                       brclr   #0x01,x:(r0-2),rx_check
1323 *                       move.w  x:(r0),y0                           // read from Rx buffer
1324 *                       move.w  y0,p:(r3)+
1325 *                       move.w  #0x20,x:(r2+0x14)                   // write PGM command
1326 *                       move.w  #0x80,x:(r2+0x13)                   // start the command
1327 *                      brclr       #0x20,X:(R2+0x13),accerr_check  // protection violation check
1328 *                      bfset       #0x20,X:(R2+0x13)               // clear pviol
1329 *                      bra         hfm_wait
1330 *              accerr_check:
1331 *                      brclr       #0x10,X:(R2+0x13),hfm_wait      // access error check
1332 *                      bfset       #0x10,X:(R2+0x13)               // clear accerr
1333 *                       bra         hfm_wait                        // loop
1334 *0x00000073  0x8A460013407D         brclr       #0x40,X:(R2+0x13),*+0
1335 *0x00000076  0xE700                 nop
1336 *0x00000077  0xE700                 nop
1337 *0x00000078  0x8A44FFFE017B         brclr       #1,X:(R0-2),*-2
1338 *0x0000007B  0xE700                 nop
1339 *0x0000007C  0xF514                 move.w      X:(R0),Y0
1340 *0x0000007D  0x8563                 move.w      Y0,P:(R3)+
1341 *0x0000007E  0x864600200014         move.w      #0x20,X:(R2+0x14)
1342 *0x00000081  0x864600800013         move.w      #0x80,X:(R2+0x13)
1343 *0x00000084  0x8A4600132004         brclr       #0x20,X:(R2+0x13),*+7
1344 *0x00000087  0x824600130020         bfset       #0x20,X:(R2+0x13)
1345 *0x0000008A  0xA968                 bra         *-23
1346 *0x0000008B  0x8A4600131065         brclr       #0x10,X:(R2+0x13),*-24
1347 *0x0000008E  0x824600130010         bfset       #0x10,X:(R2+0x13)
1348 *0x00000091  0xA961                 bra         *-30
1349 */
1350 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};
1351 const uint32_t pgm_write_pflash_length = 31;
1352
1353 int dsp5680xx_f_wr(struct target * target, uint8_t *buffer, uint32_t address, uint32_t count){
1354   int retval = ERROR_OK;
1355   uint16_t* buff16 = (uint16_t *) buffer;
1356   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1357     retval = eonce_enter_debug_mode(target,NULL);
1358     err_check_propagate(retval);
1359   }
1360   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1361   // Download the pgm that flashes.
1362   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1363   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
1364   retval = dsp5680xx_write(target, my_favourite_ram_address, 1, pgm_write_pflash_length*2,(uint8_t *) pgm_write_pflash);
1365   err_check_propagate(retval);
1366   retval = dsp5680xx_execute_queue();
1367   err_check_propagate(retval);
1368   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1369   // Set hfmdiv
1370   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1371   retval = eonce_set_hfmdiv(target);
1372   err_check_propagate(retval);
1373   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1374   // Setup registers needed by pgm_write_pflash
1375   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1376
1377   dsp5680xx_context.flush = 0;
1378
1379   retval = eonce_move_long_to_r3(target,address);  // Destination address to r3
1380   err_check_propagate(retval);
1381   eonce_load_TX_RX_high_to_r0(target);  // TX/RX reg address to r0
1382   err_check_propagate(retval);
1383   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);// FM base address to r2
1384   err_check_propagate(retval);
1385   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1386   // Run flashing program.
1387   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1388   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_CNFG); // write to HFM_CNFG (lock=0, select bank)
1389   err_check_propagate(retval);
1390   retval = eonce_move_value_at_r2_disp(target,0x04,HFM_USTAT);// write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1391   err_check_propagate(retval);
1392   retval = eonce_move_value_at_r2_disp(target,0x10,HFM_USTAT);// clear only one bit at a time
1393   err_check_propagate(retval);
1394   retval = eonce_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1395   err_check_propagate(retval);
1396   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROT);// write to HMF_PROT, clear protection
1397   err_check_propagate(retval);
1398   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROTB);// write to HMF_PROTB, clear protection
1399   err_check_propagate(retval);
1400   if(count%2){
1401     //TODO implement handling of odd number of words.
1402         retval = ERROR_FAIL;
1403         err_check(retval,"Cannot handle odd number of words.");
1404   }
1405
1406   dsp5680xx_context.flush = 1;
1407   retval = dsp5680xx_execute_queue();
1408   err_check_propagate(retval);
1409
1410   uint32_t drscan_data;
1411   retval = eonce_tx_upper_data(target,buff16[0],&drscan_data);
1412   err_check_propagate(retval);
1413
1414   retval = dsp5680xx_resume(target,0,my_favourite_ram_address,0,0);
1415   err_check_propagate(retval);
1416
1417   int counter = FLUSH_COUNT_FLASH;
1418   dsp5680xx_context.flush = 0;
1419   uint32_t i;
1420   for(i=1; (i<count/2)&&(i<HFM_SIZE_WORDS); i++){
1421     if(--counter==0){
1422       dsp5680xx_context.flush = 1;
1423       counter = FLUSH_COUNT_FLASH;
1424     }
1425     retval = eonce_tx_upper_data(target,buff16[i],&drscan_data);
1426         if(retval!=ERROR_OK){
1427           dsp5680xx_context.flush = 1;
1428           err_check_propagate(retval);
1429         }
1430         dsp5680xx_context.flush = 0;
1431   }
1432   dsp5680xx_context.flush = 1;
1433   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1434   // Verify flash
1435   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1436   uint16_t signature;
1437   uint16_t pc_crc;
1438   retval =  dsp5680xx_f_signature(target,address,i,&signature);
1439   err_check_propagate(retval);
1440   pc_crc = perl_crc(buff16,i);
1441   if(pc_crc != signature){
1442     retval = ERROR_FAIL;
1443     err_check(retval,"Flashed data failed CRC check, flash again!");
1444   }
1445   return retval;
1446 }
1447
1448 int dsp5680xx_f_unlock(struct target * target){
1449   int retval;
1450   if(target->tap->enabled){
1451     //TODO find a way to switch to the master tap here.
1452     LOG_ERROR("Master tap must be enabled to unlock flash.");
1453     return ERROR_TARGET_FAILURE;
1454   }
1455   uint32_t data_to_shift_in = MASTER_TAP_CMD_FLASH_ERASE;
1456   uint32_t data_shifted_out;
1457   retval = dsp5680xx_irscan(target,&data_to_shift_in,&data_shifted_out,8);
1458   err_check_propagate(retval);
1459   data_to_shift_in = HFM_CLK_DEFAULT;
1460   retval = dsp5680xx_drscan(target,((uint8_t *) & data_to_shift_in),((uint8_t *)&data_shifted_out),8);
1461   err_check_propagate(retval);
1462   return retval;
1463 }
1464
1465 int dsp5680xx_f_lock(struct target * target){
1466   int retval;
1467   uint16_t lock_word[] = {HFM_LOCK_FLASH,HFM_LOCK_FLASH};
1468   retval = dsp5680xx_f_wr(target,(uint8_t *)(lock_word),HFM_LOCK_ADDR_L,4);
1469   err_check_propagate(retval);
1470   return retval;
1471 }
1472
1473 static int dsp5680xx_step(struct target * target,int current, uint32_t address, int handle_breakpoints){
1474   err_check(ERROR_FAIL,"Not implemented yet.");
1475 }
1476
1477 /** Holds methods for dsp5680xx targets. */
1478 struct target_type dsp5680xx_target = {
1479   .name = "dsp5680xx",
1480
1481   .poll = dsp5680xx_poll,
1482   .arch_state = dsp5680xx_arch_state,
1483
1484   .target_request_data = NULL,
1485
1486   .halt = dsp5680xx_halt,
1487   .resume = dsp5680xx_resume,
1488   .step = dsp5680xx_step,
1489
1490   .write_buffer = dsp5680xx_write_buffer,
1491   .read_buffer = dsp5680xx_read_buffer,
1492
1493   .assert_reset = dsp5680xx_assert_reset,
1494   .deassert_reset = dsp5680xx_deassert_reset,
1495   .soft_reset_halt = dsp5680xx_soft_reset_halt,
1496
1497   .read_memory = dsp5680xx_read,
1498   .write_memory = dsp5680xx_write,
1499   .bulk_write_memory = dsp5680xx_bulk_write_memory,
1500
1501   .checksum_memory = dsp5680xx_checksum_memory,
1502
1503   .target_create = dsp5680xx_target_create,
1504   .init_target = dsp5680xx_init_target,
1505 };