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

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