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