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

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