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   // Verify that debug mode is enabled
 556   uint16_t data_read_from_dr;
 557         retval = eonce_read_status_reg(target, &data_read_from_dr);
 558         err_check_propagate(retval);
 559         if ((data_read_from_dr&0x30) == 0x30) {
 560                 LOG_DEBUG("EOnCE successfully entered debug mode.");
 561                 dsp5680xx_context.debug_mode_enabled = true;
 562                 retval = ERROR_OK;
 563         } else {
 564                 dsp5680xx_context.debug_mode_enabled = false;
 565                 retval = ERROR_TARGET_FAILURE;
 566         /**
 567         * No error msg here, since there is still hope with full halting sequence
 568         */
 569         err_check_propagate(retval);
 570         }
 571 if (eonce_status != NULL)
 572         *eonce_status = data_read_from_dr;
 573 return retval;
 574 }
 575
 576 /**
 577  * Puts the core into debug mode, enabling the EOnCE module.
 578  *
 579  * @param target
 580  * @param eonce_status Data read from the EOnCE status register.
 581  *
 582  * @return
 583  */
 584 static int eonce_enter_debug_mode(struct target * target, uint16_t * eonce_status){
 585   int retval = ERROR_OK;
 586   uint32_t instr = JTAG_INSTR_DEBUG_REQUEST;
 587   uint32_t ir_out;//not used, just to make jtag happy.
 588   uint16_t instr_16;
 589   uint16_t read_16;
 590
 591   // First try the easy way
 592   retval = eonce_enter_debug_mode_without_reset(target,eonce_status);
 593   if(retval == ERROR_OK)
 594     return retval;
 595
 596   struct jtag_tap * tap_chp;
 597   struct jtag_tap * tap_cpu;
 598   tap_chp = jtag_tap_by_string("dsp568013.chp");
 599   if(tap_chp == NULL){
 600     retval = ERROR_FAIL;
 601         err_check(retval, DSP5680XX_ERROR_JTAG_TAP_FIND_MASTER, "Failed to get master tap.");
 602   }
 603   tap_cpu = jtag_tap_by_string("dsp568013.cpu");
 604   if(tap_cpu == NULL){
 605     retval = ERROR_FAIL;
 606         err_check(retval, DSP5680XX_ERROR_JTAG_TAP_FIND_CORE, "Failed to get master tap.");
 607   }
 608
 609   // Enable master tap
 610   tap_chp->enabled = true;
 611   tap_cpu->enabled = false;
 612
 613   instr = MASTER_TAP_CMD_IDCODE;
 614   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_MASTER_TAP_IRLEN);
 615   err_check_propagate(retval);
 616   jtag_add_sleep(TIME_DIV_FREESCALE*100*1000);
 617
 618   // Enable EOnCE module
 619   jtag_add_reset(0,1);
 620   jtag_add_sleep(TIME_DIV_FREESCALE*200*1000);
 621   instr = 0x0606ffff;// This was selected experimentally.
 622   retval =  dsp5680xx_drscan(target,(uint8_t *) & instr,(uint8_t *) & ir_out,32);
 623   err_check_propagate(retval);
 624   // ir_out now hold tap idcode
 625
 626   // Enable core tap
 627   tap_chp->enabled = true;
 628   retval = switch_tap(target,tap_chp,tap_cpu);
 629   err_check_propagate(retval);
 630
 631   instr = JTAG_INSTR_ENABLE_ONCE;
 632   //Two rounds of jtag 0x6  (enable eonce) to enable EOnCE.
 633   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
 634   err_check_propagate(retval);
 635   instr = JTAG_INSTR_DEBUG_REQUEST;
 636   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
 637   err_check_propagate(retval);
 638   instr_16 = 0x1;
 639   retval = dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,8);
 640         err_check_propagate(retval);
 641   instr_16 = 0x20;
 642   retval = dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,8);
 643         err_check_propagate(retval);
 644   jtag_add_sleep(TIME_DIV_FREESCALE*100*1000);
 645   jtag_add_reset(0,0);
 646   jtag_add_sleep(TIME_DIV_FREESCALE*300*1000);
 647
 648   instr = JTAG_INSTR_ENABLE_ONCE;
 649   //Two rounds of jtag 0x6  (enable eonce) to enable EOnCE.
 650   for(int i = 0; i<3; i++){
 651     retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
 652     err_check_propagate(retval);
 653   }
 654 if ((ir_out&JTAG_STATUS_MASK) == JTAG_STATUS_DEBUG)
 655         target->state = TARGET_HALTED;
 656 else {
 657         retval = ERROR_FAIL;
 658         err_check(retval,  DSP5680XX_ERROR_HALT, "Failed to halt target.");
 659 }
 660
 661   for(int i = 0; i<3; i++){
 662     instr_16 = 0x86;
 663     dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,16);
 664     instr_16 = 0xff;
 665     dsp5680xx_drscan(target,(uint8_t *) & instr_16,(uint8_t *) & read_16,16);
 666   }
 667
 668   // Verify that debug mode is enabled
 669   uint16_t data_read_from_dr;
 670 retval = eonce_read_status_reg(target, &data_read_from_dr);
 671   err_check_propagate(retval);
 672   if((data_read_from_dr&0x30) == 0x30){
 673     LOG_DEBUG("EOnCE successfully entered debug mode.");
 674         dsp5680xx_context.debug_mode_enabled = true;
 675     retval = ERROR_OK;
 676   }else{
 677         const char *msg =  "Failed to set EOnCE module to debug mode";
 678     retval = ERROR_TARGET_FAILURE;
 679         err_check(retval, DSP5680XX_ERROR_ENTER_DEBUG_MODE, msg);
 680   }
 681   if(eonce_status!=NULL)
 682     *eonce_status = data_read_from_dr;
 683   return retval;
 684 }
 685
 686 /**
 687  * Reads the current value of the program counter and stores it.
 688  *
 689  * @param target
 690  *
 691  * @return
 692  */
 693 static int eonce_pc_store(struct target * target){
 694   uint8_t tmp[2];
 695   int retval;
 696   retval = core_move_pc_to_r4(target);
 697   err_check_propagate(retval);
 698   retval = core_move_r4_to_y(target);
 699   err_check_propagate(retval);
 700   retval = eonce_load_TX_RX_to_r0(target);
 701   err_check_propagate(retval);
 702   retval = core_move_y0_at_r0(target);
 703   err_check_propagate(retval);
 704   retval = core_rx_lower_data(target,tmp);
 705   err_check_propagate(retval);
 706   LOG_USER("PC value: 0x%X%X\n",tmp[1],tmp[0]);
 707   dsp5680xx_context.stored_pc = (tmp[0]|(tmp[1]<<8));
 708   return ERROR_OK;
 709 }
 710
 711 static int dsp5680xx_target_create(struct target *target, Jim_Interp * interp){
 712   struct dsp5680xx_common *dsp5680xx = calloc(1, sizeof(struct dsp5680xx_common));
 713   target->arch_info = dsp5680xx;
 714   return ERROR_OK;
 715 }
 716
 717 static int dsp5680xx_init_target(struct command_context *cmd_ctx, struct target *target){
 718   dsp5680xx_context.stored_pc = 0;
 719   dsp5680xx_context.flush = 1;
 720         dsp5680xx_context.debug_mode_enabled = false;
 721   LOG_DEBUG("target initiated!");
 722   //TODO core tap must be enabled before running these commands, currently this is done in the .cfg tcl script.
 723   return ERROR_OK;
 724 }
 725
 726 static int dsp5680xx_arch_state(struct target *target){
 727 LOG_USER("%s not implemented yet.", __func__);
 728 return ERROR_OK;
 729 }
 730
 731 int dsp5680xx_target_status(struct target * target, uint8_t * jtag_st, uint16_t * eonce_st){
 732   return target->state;
 733 }
 734
 735 static int dsp5680xx_assert_reset(struct target *target){
 736   target->state = TARGET_RESET;
 737   return ERROR_OK;
 738 }
 739
 740 static int dsp5680xx_deassert_reset(struct target *target){
 741   target->state = TARGET_RUNNING;
 742   return ERROR_OK;
 743 }
 744
 745 static int dsp5680xx_halt(struct target *target){
 746   int retval;
 747   uint16_t eonce_status = 0xbeef;
 748 if ((target->state == TARGET_HALTED) && (dsp5680xx_context.debug_mode_enabled)) {
 749         LOG_USER("Target already halted and in debug mode.");
 750         return ERROR_OK;
 751 } else {
 752         if (target->state == TARGET_HALTED)
 753                 LOG_USER("Target already halted, re attempting to enter debug mode.");
 754 }
 755   retval = eonce_enter_debug_mode(target,&eonce_status);
 756         err_check_propagate(retval);
 757   retval = eonce_pc_store(target);
 758   err_check_propagate(retval);
 759 if (dsp5680xx_context.debug_mode_enabled) {
 760         retval = eonce_pc_store(target);
 761         err_check_propagate(retval);
 762 }
 763   return retval;
 764 }
 765
 766 static int dsp5680xx_poll(struct target *target)
 767 {
 768   int retval;
 769   uint8_t jtag_status;
 770   uint8_t eonce_status;
 771   uint16_t read_tmp;
 772 retval = dsp5680xx_jtag_status(target, &jtag_status);
 773 err_check_propagate(retval);
 774 if (jtag_status == JTAG_STATUS_DEBUG)
 775         if (target->state != TARGET_HALTED) {
 776                 retval = eonce_enter_debug_mode(target, &read_tmp);
 777                 err_check_propagate(retval);
 778                 eonce_status = (uint8_t) read_tmp;
 779                 if ((eonce_status&EONCE_STAT_MASK) != DSP5680XX_ONCE_OSCR_DEBUG_M) {
 780                         const char *msg = "%s: Failed to put EOnCE in debug mode.Flash locked?...";
 781                         LOG_WARNING(msg, __func__);
 782                         return ERROR_TARGET_FAILURE;
 783                 } else {
 784                         target->state = TARGET_HALTED;
 785                         return ERROR_OK;
 786                 }
 787         }
 788 if (jtag_status == JTAG_STATUS_NORMAL) {
 789         if (target->state == TARGET_RESET) {
 790                 retval = dsp5680xx_halt(target);
 791                 err_check_propagate(retval);
 792                 retval = eonce_exit_debug_mode(target, &eonce_status);
 793                 err_check_propagate(retval);
 794                 if ((eonce_status&EONCE_STAT_MASK) != DSP5680XX_ONCE_OSCR_NORMAL_M) {
 795                         const char *msg = "%s: JTAG running, but EOnCE run failed.Try resetting..";
 796                         LOG_WARNING(msg, __func__);
 797                         return ERROR_TARGET_FAILURE;
 798                 } else {
 799                         target->state = TARGET_RUNNING;
 800                         return ERROR_OK;
 801                 }
 802         }
 803         if (target->state != TARGET_RUNNING) {
 804                 retval = eonce_read_status_reg(target, &read_tmp);
 805                 err_check_propagate(retval);
 806                 eonce_status = (uint8_t) read_tmp;
 807                 if ((eonce_status&EONCE_STAT_MASK) != DSP5680XX_ONCE_OSCR_NORMAL_M) {
 808                         LOG_WARNING("Inconsistent target status. Restart!");
 809                         return ERROR_TARGET_FAILURE;
 810                 }
 811         }
 812         target->state = TARGET_RUNNING;
 813         return ERROR_OK;
 814 }
 815 if (jtag_status == JTAG_STATUS_DEAD) {
 816         LOG_ERROR("%s: Cannot communicate with JTAG. Check connection...", __func__);
 817         target->state = TARGET_UNKNOWN;
 818         return ERROR_TARGET_FAILURE;
 819 };
 820 if (target->state == TARGET_UNKNOWN) {
 821         LOG_ERROR("%s: Target status invalid - communication failure", __func__);
 822         return ERROR_TARGET_FAILURE;
 823 };
 824 return ERROR_OK;
 825 }
 826
 827 static int dsp5680xx_resume(struct target *target, int current, uint32_t address, int hb, int d)
 828 {
 829 if (target->state == TARGET_RUNNING) {
 830         LOG_USER("Target already running.");
 831         return ERROR_OK;
 832 }
 833   int retval;
 834   uint8_t eonce_status;
 835 uint8_t jtag_status;
 836 if (dsp5680xx_context.debug_mode_enabled) {
 837         if (!current) {
 838                 retval = core_move_value_to_pc(target, address);
 839                 err_check_propagate(retval);
 840         }
 841
 842         int retry = 20;
 843         while (retry-- > 1) {
 844                 retval = eonce_exit_debug_mode(target, &eonce_status);
 845                 err_check_propagate(retval);
 846                 if (eonce_status == DSP5680XX_ONCE_OSCR_NORMAL_M)
 847                         break;
 848         }
 849         if (retry == 0) {
 850                 retval = ERROR_TARGET_FAILURE;
 851                 err_check(retval, DSP5680XX_ERROR_EXIT_DEBUG_MODE, "Failed to exit debug mode...");
 852         } else {
 853                 target->state = TARGET_RUNNING;
 854                 dsp5680xx_context.debug_mode_enabled = false;
 855         }
 856         LOG_DEBUG("EOnCE status: 0x%02X.", eonce_status);
 857 } else {
 858       /**
 859        * If debug mode was not enabled but target was halted, then it is most likely that
 860        * access to eonce registers is locked.
 861        * Reset target to make it run again.
 862        */
 863         jtag_add_reset(0, 1);
 864         jtag_add_sleep(TIME_DIV_FREESCALE*200*1000);
 865
 866 retval = reset_jtag();
 867 err_check(retval, DSP5680XX_ERROR_JTAG_RESET, "Failed to reset JTAG state machine");
 868 jtag_add_sleep(TIME_DIV_FREESCALE*100*1000);
 869 jtag_add_reset(0, 0);
 870 jtag_add_sleep(TIME_DIV_FREESCALE*300*1000);
 871 retval = dsp5680xx_jtag_status(target, &jtag_status);
 872 err_check_propagate(retval);
 873 if ((jtag_status&JTAG_STATUS_MASK) == JTAG_STATUS_NORMAL) {
 874         target->state = TARGET_RUNNING;
 875         dsp5680xx_context.debug_mode_enabled = false;
 876         } else {
 877                 retval = ERROR_TARGET_FAILURE;
 878                 err_check(retval, DSP5680XX_ERROR_RESUME, "Failed to resume target");
 879         }
 880 }
 881 return ERROR_OK;
 882 }
 883
 884
 885
 886
 887
 888
 889 /**
 890  * 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.
 891  * The special case of 0xFFXXXX is not modified, since it allows to read out the memory mapped EOnCE registers.
 892  *
 893  * @param address
 894  * @param pmem
 895  *
 896  * @return
 897  */
 898 static int dsp5680xx_convert_address(uint32_t *address, int *pmem)
 899 {
 900     /**
 901      * Distinguish data memory (x) from program memory (p) by the address.
 902      * Addresses over S_FILE_DATA_OFFSET are considered (x) memory.
 903      */
 904 if (*address >= S_FILE_DATA_OFFSET) {
 905         *pmem = 0;
 906         if (((*address)&0xff0000) != 0xff0000)
 907                 *address -= S_FILE_DATA_OFFSET;
 908 }
 909 return ERROR_OK;
 910 }
 911
 912 static int dsp5680xx_read_16_single(struct target *t, uint32_t a, uint8_t *data_read, int r_pmem)
 913 {
 914 struct target *target = t;
 915 uint32_t address = a;
 916   int retval;
 917   retval = core_move_long_to_r0(target,address);
 918   err_check_propagate(retval);
 919 if (r_pmem)
 920         retval = core_move_at_pr0_inc_to_y0(target);
 921 else
 922         retval = core_move_at_r0_to_y0(target);
 923   err_check_propagate(retval);
 924   retval = eonce_load_TX_RX_to_r0(target);
 925   err_check_propagate(retval);
 926   retval = core_move_y0_at_r0(target);
 927   err_check_propagate(retval);
 928   // at this point the data i want is at the reg eonce can read
 929   retval = core_rx_lower_data(target,data_read);
 930   err_check_propagate(retval);
 931 LOG_DEBUG("%s:Data read from 0x%06X: 0x%02X%02X", __func__, address, data_read[1], data_read[0]);
 932   return retval;
 933 }
 934
 935 static int dsp5680xx_read_32_single(struct target *t, uint32_t a, uint8_t *data_read, int r_pmem)
 936 {
 937 struct target *target = t;
 938 uint32_t address = a;
 939   int retval;
 940 address = (address & 0xFFFFF);
 941   // Get data to an intermediate register
 942   retval = core_move_long_to_r0(target,address);
 943   err_check_propagate(retval);
 944 if (r_pmem) {
 945         retval = core_move_at_pr0_inc_to_y0(target);
 946         err_check_propagate(retval);
 947         retval = core_move_at_pr0_inc_to_y1(target);
 948         err_check_propagate(retval);
 949 } else {
 950         retval = core_move_at_r0_inc_to_y0(target);
 951         err_check_propagate(retval);
 952         retval = core_move_at_r0_to_y1(target);
 953         err_check_propagate(retval);
 954 }
 955   // Get lower part of data to TX/RX
 956   retval = eonce_load_TX_RX_to_r0(target);
 957   err_check_propagate(retval);
 958   retval = core_move_y0_at_r0_inc(target); // This also load TX/RX high to r0
 959   err_check_propagate(retval);
 960   // Get upper part of data to TX/RX
 961   retval = core_move_y1_at_r0(target);
 962   err_check_propagate(retval);
 963   // at this point the data i want is at the reg eonce can read
 964   retval = core_rx_lower_data(target,data_read);
 965   err_check_propagate(retval);
 966   retval = core_rx_upper_data(target,data_read+2);
 967   err_check_propagate(retval);
 968   return retval;
 969 }
 970
 971 static int dsp5680xx_read(struct target *t, uint32_t a, unsigned size, unsigned count, uint8_t *buf)
 972 {
 973 struct target *target = t;
 974 uint32_t address = a;
 975 uint8_t *buffer = buf;
 976 check_halt_and_debug(target);
 977
 978   int retval = ERROR_OK;
 979   int pmem = 1;
 980
 981   retval = dsp5680xx_convert_address(&address, &pmem);
 982   err_check_propagate(retval);
 983
 984   dsp5680xx_context.flush = 0;
 985   int counter = FLUSH_COUNT_READ_WRITE;
 986
 987 for (unsigned i = 0; i < count; i++) {
 988         if (--counter == 0) {
 989                 dsp5680xx_context.flush = 1;
 990                 counter = FLUSH_COUNT_READ_WRITE;
 991         }
 992         switch (size) {
 993         case 1:
 994                 if (!(i%2))
 995                         retval = dsp5680xx_read_16_single(target, address + i/2, buffer + i, pmem);
 996                 break;
 997         case 2:
 998                 retval = dsp5680xx_read_16_single(target, address + i, buffer+2*i, pmem);
 999                 break;
1000         case 4:
1001                 retval = dsp5680xx_read_32_single(target, address + 2*i, buffer + 4*i, pmem);
1002                 break;
1003         default:
1004                 LOG_USER("%s: Invalid read size.", __func__);
1005                 break;
1006         }
1007         err_check_propagate(retval);
1008         dsp5680xx_context.flush = 0;
1009 }
1010
1011 dsp5680xx_context.flush = 1;
1012 retval = dsp5680xx_execute_queue();
1013 err_check_propagate(retval);
1014
1015 return retval;
1016 }
1017
1018 static int dsp5680xx_write_16_single(struct target *t, uint32_t a, uint16_t data, uint8_t w_pmem)
1019 {
1020 struct target *target = t;
1021 uint32_t address = a;
1022   int retval = 0;
1023   retval = core_move_long_to_r0(target,address);
1024   err_check_propagate(retval);
1025 if (w_pmem) {
1026         retval = core_move_value_to_y0(target, data);
1027         err_check_propagate(retval);
1028         retval = core_move_y0_at_pr0_inc(target);
1029         err_check_propagate(retval);
1030 } else {
1031         retval = core_move_value_at_r0(target, data);
1032         err_check_propagate(retval);
1033 }
1034 return retval;
1035 }
1036
1037 static int dsp5680xx_write_32_single(struct target *t, uint32_t a, uint32_t data, int w_pmem)
1038 {
1039 struct target *target = t;
1040 uint32_t address = a;
1041 int retval = ERROR_OK;
1042 retval = core_move_long_to_r0(target, address);
1043 err_check_propagate(retval);
1044 retval = core_move_long_to_y(target, data);
1045 err_check_propagate(retval);
1046 if (w_pmem)
1047         retval = core_move_y0_at_pr0_inc(target);
1048 else
1049         retval = core_move_y0_at_r0_inc(target);
1050 err_check_propagate(retval);
1051 if (w_pmem)
1052         retval = core_move_y1_at_pr0_inc(target);
1053 else
1054         retval = core_move_y1_at_r0_inc(target);
1055 err_check_propagate(retval);
1056 return retval;
1057 }
1058
1059 static int dsp5680xx_write_8(struct target *t, uint32_t a, uint32_t c, const uint8_t *d, int pmem)
1060 {
1061 struct target *target = t;
1062 uint32_t address = a;
1063 uint32_t count = c;
1064 const uint8_t *data = d;
1065   int retval = 0;
1066   uint16_t data_16;
1067   uint32_t iter;
1068
1069   int counter = FLUSH_COUNT_READ_WRITE;
1070 for (iter = 0; iter < count/2; iter++) {
1071         if (--counter == 0) {
1072                 dsp5680xx_context.flush = 1;
1073                 counter = FLUSH_COUNT_READ_WRITE;
1074         }
1075         data_16 = (data[2*iter]|(data[2*iter+1] << 8));
1076         retval = dsp5680xx_write_16_single(target, address+iter, data_16, pmem);
1077         if (retval != ERROR_OK) {
1078                 LOG_ERROR("%s: Could not write to p:0x%04X", __func__, address);
1079         dsp5680xx_context.flush = 1;
1080         return retval;
1081         }
1082         dsp5680xx_context.flush = 0;
1083 }
1084 dsp5680xx_context.flush = 1;
1085
1086   // Only one byte left, let's not overwrite the other byte (mem is 16bit)
1087   // Need to retrieve the part we do not want to overwrite.
1088   uint16_t data_old;
1089 if ((count == 1) || (count%2)) {
1090         retval = dsp5680xx_read(target, address+iter, 1, 1, (uint8_t *)&data_old);
1091         err_check_propagate(retval);
1092         if (count == 1)
1093                 data_old = (((data_old&0xff)<<8)|data[0]);/* preserve upper byte */
1094         else
1095                 data_old = (((data_old&0xff)<<8)|data[2*iter+1]);
1096         retval = dsp5680xx_write_16_single(target, address+iter, data_old, pmem);
1097         err_check_propagate(retval);
1098 }
1099 return retval;
1100 }
1101
1102 static int dsp5680xx_write_16(struct target *t, uint32_t a, uint32_t c, const uint8_t *d, int pmem)
1103 {
1104 struct target *target = t;
1105 uint32_t address = a;
1106 uint32_t count = c;
1107 const uint8_t *data = d;
1108   int retval = ERROR_OK;
1109   uint32_t iter;
1110   int counter = FLUSH_COUNT_READ_WRITE;
1111
1112 for (iter = 0; iter < count; iter++) {
1113         if (--counter == 0) {
1114                 dsp5680xx_context.flush = 1;
1115                 counter = FLUSH_COUNT_READ_WRITE;
1116         }
1117         retval = dsp5680xx_write_16_single(target, address+iter, data[iter], pmem);
1118         if (retval != ERROR_OK) {
1119                 LOG_ERROR("%s: Could not write to p:0x%04X", __func__, address);
1120                 dsp5680xx_context.flush = 1;
1121                 return retval;
1122         }
1123         dsp5680xx_context.flush = 0;
1124 }
1125 dsp5680xx_context.flush = 1;
1126 return retval;
1127 }
1128
1129 static int dsp5680xx_write_32(struct target *t, uint32_t a, uint32_t c, const uint8_t *d, int pmem)
1130 {
1131 struct target *target = t;
1132 uint32_t address = a;
1133 uint32_t count = c;
1134 const uint8_t *data = d;
1135   int retval = ERROR_OK;
1136   uint32_t iter;
1137   int counter = FLUSH_COUNT_READ_WRITE;
1138
1139 for (iter = 0; iter < count; iter++) {
1140         if (--counter == 0) {
1141                 dsp5680xx_context.flush = 1;
1142                 counter = FLUSH_COUNT_READ_WRITE;
1143         }
1144         retval = dsp5680xx_write_32_single(target, address+(iter<<1), data[iter], pmem);
1145         if (retval != ERROR_OK) {
1146                 LOG_ERROR("%s: Could not write to p:0x%04X", __func__, address);
1147                 dsp5680xx_context.flush = 1;
1148                 return retval;
1149         }
1150         dsp5680xx_context.flush = 0;
1151 }
1152 dsp5680xx_context.flush = 1;
1153 return retval;
1154 }
1155
1156 /**
1157  * Writes @buffer to memory.
1158  * The parameter @address determines whether @buffer should be written to
1159  * P: (program) memory or X: (dat) memory.
1160  *
1161  * @param target
1162  * @param address
1163  * @param size Bytes (1), Half words (2), Words (4).
1164  * @param count In bytes.
1165  * @param buffer
1166  *
1167  * @return
1168  */
1169 static int dsp5680xx_write(struct target *t, uint32_t a, uint32_t s, uint32_t c, const uint8_t *b)
1170 {
1171     /* TODO Cannot write 32bit to odd address, will write 0x12345678  as 0x5678 0x0012 */
1172 struct target *target = t;
1173 uint32_t address = a;
1174 uint32_t count = c;
1175 uint8_t const *buffer = b;
1176 uint32_t size = s;
1177 check_halt_and_debug(target);
1178
1179   int retval = 0;
1180   int p_mem = 1;
1181   retval = dsp5680xx_convert_address(&address, &p_mem);
1182   err_check_propagate(retval);
1183
1184 switch (size) {
1185   case 1:
1186     retval = dsp5680xx_write_8(target, address, count, buffer, p_mem);
1187     break;
1188   case 2:
1189     retval = dsp5680xx_write_16(target, address, count, buffer, p_mem);
1190       break;
1191   case 4:
1192     retval = dsp5680xx_write_32(target, address, count, buffer, p_mem);
1193     break;
1194   default:
1195         retval = ERROR_TARGET_DATA_ABORT;
1196         err_check(retval, DSP5680XX_ERROR_INVALID_DATA_SIZE_UNIT, "Invalid data size.");
1197         break;
1198   }
1199   return retval;
1200 }
1201
1202 static int dsp5680xx_bulk_write_memory(struct target *t, uint32_t a, uint32_t al, const uint8_t *b)
1203 {
1204   LOG_ERROR("Not implemented yet.");
1205   return ERROR_FAIL;
1206 }
1207
1208 static int dsp5680xx_write_buffer(struct target *t, uint32_t a, uint32_t size, const uint8_t *b)
1209 {
1210 check_halt_and_debug(t);
1211 return dsp5680xx_write(t, a, 1, size, b);
1212 }
1213
1214 /**
1215  * This function is called by verify_image, it is used to read data from memory.
1216  *
1217  * @param target
1218  * @param address Word addressing.
1219  * @param size In bytes.
1220  * @param buffer
1221  *
1222  * @return
1223  */
1224 static int dsp5680xx_read_buffer(struct target *t, uint32_t a, uint32_t size, uint8_t *buf)
1225 {
1226 check_halt_and_debug(t);
1227 /* The "/2" solves the byte/word addressing issue.*/
1228 return dsp5680xx_read(t, a, 2, size/2, buf);
1229 }
1230
1231 /**
1232  * This function is not implemented.
1233  * It returns an error in order to get OpenOCD to do read out the data and calculate the CRC, or try a binary comparison.
1234  *
1235  * @param target
1236  * @param address Start address of the image.
1237  * @param size In bytes.
1238  * @param checksum
1239  *
1240  * @return
1241  */
1242 static int dsp5680xx_checksum_memory(struct target *t, uint32_t a, uint32_t s, uint32_t *checksum)
1243 {
1244   return ERROR_FAIL;
1245 }
1246
1247 /**
1248  * 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.
1249  * This algorithm is based on the perl script available from the Freescale website at FAQ 25630.
1250  *
1251  * @param buff16
1252  * @param word_count
1253  *
1254  * @return
1255  */
1256 static int perl_crc(uint8_t *buff8, uint32_t  word_count)
1257 {
1258   uint16_t checksum = 0xffff;
1259   uint16_t data,fbmisr;
1260   uint32_t i;
1261 for (i = 0; i < word_count; i++) {
1262         data = (buff8[2*i]|(buff8[2*i+1]<<8));
1263         fbmisr = (checksum & 2)>>1^(checksum & 4)>>2^(checksum & 16)>>4^(checksum & 0x8000)>>15;
1264         checksum = (data ^ ((checksum << 1) | fbmisr));
1265 }
1266   i--;
1267 for (; !(i&0x80000000); i--) {
1268         data = (buff8[2*i]|(buff8[2*i+1]<<8));
1269         fbmisr = (checksum & 2)>>1^(checksum & 4)>>2^(checksum & 16)>>4^(checksum & 0x8000)>>15;
1270         checksum = (data ^ ((checksum << 1) | fbmisr));
1271 }
1272   return checksum;
1273 }
1274
1275 /**
1276  * Resets the SIM. (System Integration Modul).
1277  *
1278  * @param target
1279  *
1280  * @return
1281  */
1282 int dsp5680xx_f_SIM_reset(struct target *target)
1283 {
1284   int retval = ERROR_OK;
1285   uint16_t sim_cmd = SIM_CMD_RESET;
1286   uint32_t sim_addr;
1287 if (strcmp(target->tap->chip, "dsp568013") == 0) {
1288         sim_addr = MC568013_SIM_BASE_ADDR+S_FILE_DATA_OFFSET;
1289         retval = dsp5680xx_write(target,sim_addr,1,2,(const uint8_t *)&sim_cmd);
1290         err_check_propagate(retval);
1291 }
1292   return retval;
1293 }
1294
1295 /**
1296  * Halts the core and resets the SIM. (System Integration Modul).
1297  *
1298  * @param target
1299  *
1300  * @return
1301  */
1302 static int dsp5680xx_soft_reset_halt(struct target *target)
1303 {
1304   //TODO is this what this function is expected to do...?
1305   int retval;
1306   retval = dsp5680xx_halt(target);
1307   err_check_propagate(retval);
1308   retval = dsp5680xx_f_SIM_reset(target);
1309 err_check_propagate(retval);
1310   return retval;
1311 }
1312
1313 int dsp5680xx_f_protect_check(struct target * target, uint16_t * protected) {
1314   int retval;
1315 check_halt_and_debug(target);
1316 if (protected == NULL) {
1317         const char *msg = "NULL pointer not valid.";
1318         err_check(ERROR_FAIL, DSP5680XX_ERROR_PROTECT_CHECK_INVALID_ARGS, msg);
1319 }
1320   retval = dsp5680xx_read_16_single(target,HFM_BASE_ADDR|HFM_PROT,(uint8_t *)protected,0);
1321   err_check_propagate(retval);
1322   return retval;
1323 }
1324
1325 /**
1326  * Executes a command on the FM module.
1327  * Some commands use the parameters @address and @data, others ignore them.
1328  *
1329  * @param target
1330  * @param command Command to execute.
1331  * @param address Command parameter.
1332  * @param data Command parameter.
1333  * @param hfm_ustat FM status register.
1334  * @param pmem Address is P: (program) memory (@pmem==1) or X: (dat) memory (@pmem==0)
1335  *
1336  * @return
1337  */
1338 static int dsp5680xx_f_ex(struct target *t, uint16_t c, uint32_t a, uint32_t d, uint16_t *h, int p)
1339 {
1340 struct target *target = t;
1341 uint32_t command = c;
1342 uint32_t address = a;
1343 uint32_t data = d;
1344 uint16_t *hfm_ustat = h;
1345 int pmem = p;
1346   int retval;
1347   retval = core_load_TX_RX_high_addr_to_r0(target);
1348   err_check_propagate(retval);
1349   retval = core_move_long_to_r2(target,HFM_BASE_ADDR);
1350   err_check_propagate(retval);
1351   uint8_t i[2];
1352   int watchdog = 100;
1353   do{
1354     retval = core_move_at_r2_disp_to_y0(target,HFM_USTAT);      // read HMF_USTAT
1355         err_check_propagate(retval);
1356     retval = core_move_y0_at_r0(target);
1357         err_check_propagate(retval);
1358     retval = core_rx_upper_data(target,i);
1359         err_check_propagate(retval);
1360 if ((watchdog--) == 1) {
1361         retval = ERROR_TARGET_FAILURE;
1362         const char *msg = "Timed out waiting for FM to finish old command.";
1363         err_check(retval, DSP5680XX_ERROR_FM_BUSY, msg);
1364     }
1365   }while (!(i[0]&0x40));                                // wait until current command is complete
1366
1367   dsp5680xx_context.flush = 0;
1368
1369   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 ???
1370   err_check_propagate(retval);
1371   retval = core_move_value_at_r2_disp(target,0x04,HFM_USTAT);           // write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1372   err_check_propagate(retval);
1373   retval = core_move_value_at_r2_disp(target,0x10,HFM_USTAT);           // clear only one bit at a time
1374   err_check_propagate(retval);
1375   retval = core_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1376   err_check_propagate(retval);
1377   retval = core_move_value_at_r2_disp(target,0x00,HFM_PROT);            // write to HMF_PROT, clear protection
1378   err_check_propagate(retval);
1379   retval = core_move_value_at_r2_disp(target,0x00,HFM_PROTB);           // write to HMF_PROTB, clear protection
1380   err_check_propagate(retval);
1381   retval = core_move_value_to_y0(target,data);
1382   err_check_propagate(retval);
1383   retval = core_move_long_to_r3(target,address);                        // write to the flash block
1384   err_check_propagate(retval);
1385 if (pmem) {
1386         retval = core_move_y0_at_pr3_inc(target);
1387         err_check_propagate(retval);
1388 } else {
1389         retval = core_move_y0_at_r3(target);
1390         err_check_propagate(retval);
1391 }
1392   retval = core_move_value_at_r2_disp(target,command,HFM_CMD);  // write command to the HFM_CMD reg
1393   err_check_propagate(retval);
1394   retval = core_move_value_at_r2_disp(target,0x80,HFM_USTAT);           // start the command
1395   err_check_propagate(retval);
1396
1397   dsp5680xx_context.flush = 1;
1398   retval = dsp5680xx_execute_queue();
1399   err_check_propagate(retval);
1400
1401   watchdog = 100;
1402   do{
1403     retval = core_move_at_r2_disp_to_y0(target,HFM_USTAT);      // read HMF_USTAT
1404         err_check_propagate(retval);
1405     retval = core_move_y0_at_r0(target);
1406         err_check_propagate(retval);
1407         retval = core_rx_upper_data(target,i);
1408         err_check_propagate(retval);
1409         if ((watchdog--) == 1) {
1410                 retval = ERROR_TARGET_FAILURE;
1411                 err_check(retval, DSP5680XX_ERROR_FM_CMD_TIMED_OUT, "FM execution did not finish.");
1412     }
1413   }while (!(i[0]&0x40));            // wait until the command is complete
1414   *hfm_ustat = ((i[0]<<8)|(i[1]));
1415   if (i[0]&HFM_USTAT_MASK_PVIOL_ACCER){
1416         retval = ERROR_TARGET_FAILURE;
1417         const char *msg = "pviol and/or accer bits set. HFM command execution error";
1418         err_check(retval, DSP5680XX_ERROR_FM_EXEC, msg);
1419   }
1420   return ERROR_OK;
1421 }
1422
1423 /**
1424  * 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.)
1425  *
1426  * @param target
1427  *
1428  * @return
1429  */
1430 static int set_fm_ck_div(struct target *target)
1431 {
1432   uint8_t i[2];
1433   int retval;
1434   retval = core_move_long_to_r2(target,HFM_BASE_ADDR);
1435   err_check_propagate(retval);
1436   retval = core_load_TX_RX_high_addr_to_r0(target);
1437   err_check_propagate(retval);
1438   retval = core_move_at_r2_to_y0(target);// read HFM_CLKD
1439   err_check_propagate(retval);
1440   retval = core_move_y0_at_r0(target);
1441   err_check_propagate(retval);
1442   retval = core_rx_upper_data(target,i);
1443   err_check_propagate(retval);
1444   unsigned int hfm_at_wrong_value = 0;
1445   if ((i[0]&0x7f)!=HFM_CLK_DEFAULT) {
1446     LOG_DEBUG("HFM CLK divisor contained incorrect value (0x%02X).",i[0]&0x7f);
1447     hfm_at_wrong_value = 1;
1448 } else {
1449     LOG_DEBUG("HFM CLK divisor was already set to correct value (0x%02X).",i[0]&0x7f);
1450     return ERROR_OK;
1451   }
1452   retval = core_move_value_at_r2(target,HFM_CLK_DEFAULT);       // write HFM_CLKD
1453   err_check_propagate(retval);
1454   retval = core_move_at_r2_to_y0(target); // verify HFM_CLKD
1455   err_check_propagate(retval);
1456   retval = core_move_y0_at_r0(target);
1457   err_check_propagate(retval);
1458   retval = core_rx_upper_data(target,i);
1459   err_check_propagate(retval);
1460   if (i[0]!=(0x80|(HFM_CLK_DEFAULT&0x7f))) {
1461         retval = ERROR_TARGET_FAILURE;
1462         err_check(retval, DSP5680XX_ERROR_FM_SET_CLK, "Unable to set HFM CLK divisor.");
1463   }
1464 if (hfm_at_wrong_value)
1465         LOG_DEBUG("HFM CLK divisor set to 0x%02x.", i[0]&0x7f);
1466 return ERROR_OK;
1467 }
1468
1469 /**
1470  * 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.
1471  *
1472  * @param target
1473  * @param address Start of flash array where the signature should be calculated.
1474  * @param words Number of words over which the signature should be calculated.
1475  * @param signature Value calculated by the FM.
1476  *
1477  * @return
1478  */
1479 static int dsp5680xx_f_signature(struct target *t, uint32_t a, uint32_t words, uint16_t * signature)
1480 {
1481 struct target *target = t;
1482 uint32_t address = a;
1483   int retval;
1484   uint16_t hfm_ustat;
1485 if (!dsp5680xx_context.debug_mode_enabled) {
1486         retval = eonce_enter_debug_mode_without_reset(target, NULL);
1487         /**
1488         * Generate error here, since it is not done in eonce_enter_debug_mode_without_reset
1489         */
1490         err_check(retval, DSP5680XX_ERROR_HALT, "Failed to halt target.");
1491 }
1492 retval = dsp5680xx_f_ex(target, HFM_CALCULATE_DATA_SIGNATURE, address, words, &hfm_ustat, 1);
1493   err_check_propagate(retval);
1494   retval = dsp5680xx_read_16_single(target, HFM_BASE_ADDR|HFM_DATA, (uint8_t *)signature, 0);
1495   return retval;
1496 }
1497
1498 int dsp5680xx_f_erase_check(struct target *target, uint8_t *erased, uint32_t sector)
1499 {
1500   int retval;
1501   uint16_t hfm_ustat;
1502 uint32_t tmp;
1503 if (!dsp5680xx_context.debug_mode_enabled) {
1504         retval = dsp5680xx_halt(target);
1505         err_check_propagate(retval);
1506 }
1507   retval = set_fm_ck_div(target);
1508   err_check_propagate(retval);
1509   /**
1510    * Check if chip is already erased.
1511    */
1512 tmp = HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2;
1513 retval = dsp5680xx_f_ex(target, HFM_ERASE_VERIFY, tmp, 0, &hfm_ustat, 1);
1514   err_check_propagate(retval);
1515 if (erased != NULL)
1516         *erased = (uint8_t)(hfm_ustat&HFM_USTAT_MASK_BLANK);
1517         return retval;
1518 }
1519
1520 /**
1521  * Executes the FM page erase command.
1522  *
1523  * @param target
1524  * @param sector Page to erase.
1525  * @param hfm_ustat FM module status register.
1526  *
1527  * @return
1528  */
1529 static int erase_sector(struct target *target, int sector, uint16_t *hfm_ustat)
1530 {
1531 int retval;
1532 uint32_t tmp = HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2;
1533 retval = dsp5680xx_f_ex(target, HFM_PAGE_ERASE, tmp, 0, hfm_ustat, 1);
1534   err_check_propagate(retval);
1535   return retval;
1536 }
1537
1538 /**
1539  * Executes the FM mass erase command. Erases the flash array completely.
1540  *
1541  * @param target
1542  * @param hfm_ustat FM module status register.
1543  *
1544  * @return
1545  */
1546 static int mass_erase(struct target *target, uint16_t *hfm_ustat)
1547 {
1548   int retval;
1549 retval = dsp5680xx_f_ex(target, HFM_MASS_ERASE, 0, 0, hfm_ustat, 1);
1550   return retval;
1551 }
1552
1553 int dsp5680xx_f_erase(struct target *target, int first, int last)
1554 {
1555   int retval;
1556 if (!dsp5680xx_context.debug_mode_enabled) {
1557         retval = dsp5680xx_halt(target);
1558         err_check_propagate(retval);
1559 }
1560   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1561   // Reset SIM
1562   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1563   retval = dsp5680xx_f_SIM_reset(target);
1564   err_check_propagate(retval);
1565   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1566   // Set hfmdiv
1567   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1568   retval = set_fm_ck_div(target);
1569   err_check_propagate(retval);
1570
1571   uint16_t hfm_ustat;
1572   int do_mass_erase = ((!(first|last)) || ((first==0)&&(last == (HFM_SECTOR_COUNT-1))));
1573 if (do_mass_erase) {
1574         /* Mass erase */
1575         retval = mass_erase(target, &hfm_ustat);
1576         err_check_propagate(retval);
1577 } else {
1578         for (int i = first; i <= last; i++) {
1579                 retval = erase_sector(target, i, &hfm_ustat);
1580                 err_check_propagate(retval);
1581         }
1582 }
1583   return ERROR_OK;
1584 }
1585
1586 /**
1587  * Algorithm for programming normal p: flash
1588  * Follow state machine from "56F801x Peripheral Reference Manual"@163.
1589  * Registers to set up before calling:
1590 *  r0: TX/RX high address.
1591 *  r2: FM module base address.
1592 *  r3: Destination address in flash.
1593 *
1594 *               hfm_wait:                                           // wait for buffer empty
1595 *                       brclr   #0x80,x:(r2+0x13),hfm_wait
1596 *               rx_check:                                           // wait for input buffer full
1597 *                       brclr   #0x01,x:(r0-2),rx_check
1598 *                       move.w  x:(r0),y0                           // read from Rx buffer
1599 *                       move.w  y0,p:(r3)+
1600 *                       move.w  #0x20,x:(r2+0x14)                   // write PGM command
1601 *                       move.w  #0x80,x:(r2+0x13)                   // start the command
1602 *                       move.w  X:(R2+0x13),A                       // Read USTAT register
1603 *                      brclr       #0x20,A,accerr_check             // protection violation check
1604 *                      bfset       #0x20,X:(R2+0x13)                // clear pviol
1605 *                      bra         hfm_wait
1606 *              accerr_check:
1607 *                      brclr       #0x10,A,hfm_wait                 // access error check
1608 *                      bfset       #0x10,X:(R2+0x13)                // clear accerr
1609 *                       bra         hfm_wait                        // loop
1610 *0x00000000  0x8A460013807D         brclr       #0x80,X:(R2+0x13),*+0
1611 *0x00000003  0xE700                 nop
1612 *0x00000004  0xE700                 nop
1613 *0x00000005  0x8A44FFFE017B         brclr       #1,X:(R0-2),*-2
1614 *0x00000008  0xE700                 nop
1615 *0x00000009  0xF514                 move.w      X:(R0),Y0
1616 *0x0000000A  0x8563                 move.w      Y0,P:(R3)+
1617 *0x0000000B  0x864600200014         move.w      #32,X:(R2+0x14)
1618 *0x0000000E  0x864600800013         move.w      #128,X:(R2+0x13)
1619 *0x00000011  0xF0420013             move.w      X:(R2+0x13),A
1620 *0x00000013  0x8B402004             brclr       #0x20,A,*+6
1621 *0x00000015  0x824600130020         bfset       #0x20,X:(R2+0x13)
1622 *0x00000018  0xA967                 bra         *-24
1623 *0x00000019  0x8B401065             brclr       #0x10,A,*-25
1624 *0x0000001B  0x824600130010         bfset       #0x10,X:(R2+0x13)
1625 *0x0000001E  0xA961                 bra         *-30
1626 */
1627
1628 const uint16_t pgm_write_pflash[] = {0x8A46, 0x0013, 0x807D, 0xE700,\
1629                                      0xE700, 0x8A44, 0xFFFE, 0x017B,\
1630                                      0xE700, 0xF514, 0x8563, 0x8646,\
1631                                      0x0020, 0x0014, 0x8646, 0x0080,\
1632                                      0x0013, 0xF042, 0x0013, 0x8B40,\
1633                                      0x2004, 0x8246, 0x0013, 0x0020,\
1634                                      0xA967, 0x8B40, 0x1065, 0x8246,\
1635                                      0x0013, 0x0010, 0xA961};
1636 const uint32_t pgm_write_pflash_length = 31;
1637
1638 int dsp5680xx_f_wr(struct target *t, uint8_t *b, uint32_t a, uint32_t count, int is_flash_lock)
1639 {
1640 struct target *target = t;
1641 uint32_t address = a;
1642 uint8_t *buffer = b;
1643   int retval = ERROR_OK;
1644 if (!dsp5680xx_context.debug_mode_enabled) {
1645         retval = eonce_enter_debug_mode(target, NULL);
1646         err_check_propagate(retval);
1647 }
1648   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1649   // Download the pgm that flashes.
1650   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1651 const uint32_t len = pgm_write_pflash_length;
1652 uint32_t ram_addr = 0x8700;
1653 /**
1654  *  This seems to be a safe address.
1655  * This one is the one used by codewarrior in 56801x_flash.cfg
1656  */
1657 if (!is_flash_lock) {
1658         retval = dsp5680xx_write(target, ram_addr, 1, len*2, (uint8_t *) pgm_write_pflash);
1659         err_check_propagate(retval);
1660         retval = dsp5680xx_execute_queue();
1661         err_check_propagate(retval);
1662 }
1663   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1664   // Set hfmdiv
1665   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1666   retval = set_fm_ck_div(target);
1667   err_check_propagate(retval);
1668   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1669   // Setup registers needed by pgm_write_pflash
1670   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1671
1672   dsp5680xx_context.flush = 0;
1673
1674   retval = core_move_long_to_r3(target,address);  // Destination address to r3
1675   err_check_propagate(retval);
1676   core_load_TX_RX_high_addr_to_r0(target);  // TX/RX reg address to r0
1677   err_check_propagate(retval);
1678   retval = core_move_long_to_r2(target,HFM_BASE_ADDR);// FM base address to r2
1679   err_check_propagate(retval);
1680   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1681   // Run flashing program.
1682   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1683   retval = core_move_value_at_r2_disp(target,0x00,HFM_CNFG); // write to HFM_CNFG (lock=0, select bank)
1684   err_check_propagate(retval);
1685   retval = core_move_value_at_r2_disp(target,0x04,HFM_USTAT);// write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1686   err_check_propagate(retval);
1687   retval = core_move_value_at_r2_disp(target,0x10,HFM_USTAT);// clear only one bit at a time
1688   err_check_propagate(retval);
1689   retval = core_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1690   err_check_propagate(retval);
1691   retval = core_move_value_at_r2_disp(target,0x00,HFM_PROT);// write to HMF_PROT, clear protection
1692   err_check_propagate(retval);
1693   retval = core_move_value_at_r2_disp(target,0x00,HFM_PROTB);// write to HMF_PROTB, clear protection
1694   err_check_propagate(retval);
1695 if (count%2) {
1696         /* TODO implement handling of odd number of words. */
1697         retval = ERROR_FAIL;
1698         const char *msg = "Cannot handle odd number of words.";
1699         err_check(retval, DSP5680XX_ERROR_FLASHING_INVALID_WORD_COUNT, msg);
1700 }
1701
1702   dsp5680xx_context.flush = 1;
1703   retval = dsp5680xx_execute_queue();
1704   err_check_propagate(retval);
1705
1706   uint32_t drscan_data;
1707   uint16_t tmp = (buffer[0]|(buffer[1]<<8));
1708 retval = core_tx_upper_data(target, tmp, &drscan_data);
1709   err_check_propagate(retval);
1710
1711 retval = dsp5680xx_resume(target, 0, ram_addr, 0, 0);
1712   err_check_propagate(retval);
1713
1714   int counter = FLUSH_COUNT_FLASH;
1715   dsp5680xx_context.flush = 0;
1716   uint32_t i;
1717 for (i = 1; (i < count/2) && (i < HFM_SIZE_WORDS); i++) {
1718         if (--counter == 0) {
1719                 dsp5680xx_context.flush = 1;
1720                 counter = FLUSH_COUNT_FLASH;
1721         }
1722         tmp = (buffer[2*i]|(buffer[2*i+1]<<8));
1723         retval = core_tx_upper_data(target, tmp, &drscan_data);
1724         if (retval != ERROR_OK) {
1725                 dsp5680xx_context.flush = 1;
1726                 err_check_propagate(retval);
1727         }
1728         dsp5680xx_context.flush = 0;
1729   }
1730   dsp5680xx_context.flush = 1;
1731 if (!is_flash_lock) {
1732         /** -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1733         * Verify flash (skip when exec lock sequence)
1734         *  -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1735         */
1736         uint16_t signature;
1737         uint16_t pc_crc;
1738         retval =  dsp5680xx_f_signature(target, address, i, &signature);
1739         err_check_propagate(retval);
1740         pc_crc = perl_crc(buffer, i);
1741         if (pc_crc != signature) {
1742                 retval = ERROR_FAIL;
1743                 const char *msg = "Flashed data failed CRC check, flash again!";
1744                 err_check(retval, DSP5680XX_ERROR_FLASHING_CRC, msg);
1745         }
1746 }
1747 return retval;
1748 }
1749
1750 int dsp5680xx_f_unlock(struct target *target)
1751 {
1752   int retval = ERROR_OK;
1753   uint16_t eonce_status;
1754   uint32_t instr;
1755   uint32_t ir_out;
1756   struct jtag_tap * tap_chp;
1757   struct jtag_tap * tap_cpu;
1758   tap_chp = jtag_tap_by_string("dsp568013.chp");
1759 if (tap_chp == NULL) {
1760         retval = ERROR_FAIL;
1761         err_check(retval, DSP5680XX_ERROR_JTAG_TAP_ENABLE_MASTER, "Failed to get master tap.");
1762 }
1763   tap_cpu = jtag_tap_by_string("dsp568013.cpu");
1764 if (tap_cpu == NULL) {
1765         retval = ERROR_FAIL;
1766         err_check(retval, DSP5680XX_ERROR_JTAG_TAP_ENABLE_CORE, "Failed to get master tap.");
1767 }
1768
1769 retval = eonce_enter_debug_mode_without_reset(target, &eonce_status);
1770 if (retval == ERROR_OK)
1771         LOG_WARNING("Memory was not locked.");
1772
1773   jtag_add_reset(0,1);
1774   jtag_add_sleep(TIME_DIV_FREESCALE*200*1000);
1775
1776   retval = reset_jtag();
1777         err_check(retval, DSP5680XX_ERROR_JTAG_RESET, "Failed to reset JTAG state machine");
1778   jtag_add_sleep(150);
1779
1780   // Enable core tap
1781   tap_chp->enabled = true;
1782   retval = switch_tap(target,tap_chp,tap_cpu);
1783   err_check_propagate(retval);
1784
1785   instr = JTAG_INSTR_DEBUG_REQUEST;
1786   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
1787   err_check_propagate(retval);
1788   jtag_add_sleep(TIME_DIV_FREESCALE*100*1000);
1789   jtag_add_reset(0,0);
1790   jtag_add_sleep(TIME_DIV_FREESCALE*300*1000);
1791
1792   // Enable master tap
1793   tap_chp->enabled = false;
1794   retval = switch_tap(target,tap_chp,tap_cpu);
1795   err_check_propagate(retval);
1796
1797   // Execute mass erase to unlock
1798   instr = MASTER_TAP_CMD_FLASH_ERASE;
1799   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_MASTER_TAP_IRLEN);
1800   err_check_propagate(retval);
1801
1802   instr = HFM_CLK_DEFAULT;
1803   retval =  dsp5680xx_drscan(target,(uint8_t *) & instr,(uint8_t *) & ir_out,16);
1804   err_check_propagate(retval);
1805
1806   jtag_add_sleep(TIME_DIV_FREESCALE*150*1000);
1807   jtag_add_reset(0,1);
1808   jtag_add_sleep(TIME_DIV_FREESCALE*200*1000);
1809
1810   retval = reset_jtag();
1811         err_check(retval, DSP5680XX_ERROR_JTAG_RESET, "Failed to reset JTAG state machine");
1812   jtag_add_sleep(150);
1813
1814   instr = 0x0606ffff;
1815   retval =  dsp5680xx_drscan(target,(uint8_t *) & instr,(uint8_t *) & ir_out,32);
1816   err_check_propagate(retval);
1817
1818   // enable core tap
1819   instr = 0x5;
1820   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_MASTER_TAP_IRLEN);
1821   err_check_propagate(retval);
1822   instr = 0x2;
1823   retval =  dsp5680xx_drscan(target,(uint8_t *) & instr,(uint8_t *) & ir_out,4);
1824   err_check_propagate(retval);
1825
1826   tap_cpu->enabled = true;
1827   tap_chp->enabled = false;
1828 target->state = TARGET_RUNNING;
1829 dsp5680xx_context.debug_mode_enabled = false;
1830   return retval;
1831 }
1832
1833 int dsp5680xx_f_lock(struct target *target)
1834 {
1835   int retval;
1836 struct jtag_tap *tap_chp;
1837 struct jtag_tap *tap_cpu;
1838   uint16_t lock_word[] = {HFM_LOCK_FLASH};
1839   retval = dsp5680xx_f_wr(target,(uint8_t *)(lock_word),HFM_LOCK_ADDR_L,2,1);
1840   err_check_propagate(retval);
1841
1842   jtag_add_reset(0,1);
1843   jtag_add_sleep(TIME_DIV_FREESCALE*200*1000);
1844
1845   retval = reset_jtag();
1846         err_check(retval, DSP5680XX_ERROR_JTAG_RESET, "Failed to reset JTAG state machine");
1847   jtag_add_sleep(TIME_DIV_FREESCALE*100*1000);
1848   jtag_add_reset(0,0);
1849   jtag_add_sleep(TIME_DIV_FREESCALE*300*1000);
1850
1851 tap_chp = jtag_tap_by_string("dsp568013.chp");
1852 if (tap_chp == NULL) {
1853         retval = ERROR_FAIL;
1854         err_check(retval, DSP5680XX_ERROR_JTAG_TAP_ENABLE_MASTER, "Failed to get master tap.");
1855 }
1856 tap_cpu = jtag_tap_by_string("dsp568013.cpu");
1857 if (tap_cpu == NULL) {
1858         retval = ERROR_FAIL;
1859         err_check(retval, DSP5680XX_ERROR_JTAG_TAP_ENABLE_CORE, "Failed to get master tap.");
1860 }
1861 target->state = TARGET_RUNNING;
1862 dsp5680xx_context.debug_mode_enabled = false;
1863 tap_cpu->enabled = false;
1864 tap_chp->enabled = true;
1865 retval = switch_tap(target, tap_chp, tap_cpu);
1866 return retval;
1867 }
1868
1869 static int dsp5680xx_step(struct target * target,int current, uint32_t address, int handle_breakpoints){
1870         err_check(ERROR_FAIL, DSP5680XX_ERROR_NOT_IMPLEMENTED_STEP, "Not implemented yet.");
1871 }
1872
1873 /** Holds methods for dsp5680xx targets. */
1874 struct target_type dsp5680xx_target = {
1875   .name = "dsp5680xx",
1876
1877   .poll = dsp5680xx_poll,
1878   .arch_state = dsp5680xx_arch_state,
1879
1880   .target_request_data = NULL,
1881
1882   .halt = dsp5680xx_halt,
1883   .resume = dsp5680xx_resume,
1884   .step = dsp5680xx_step,
1885
1886   .write_buffer = dsp5680xx_write_buffer,
1887   .read_buffer = dsp5680xx_read_buffer,
1888
1889   .assert_reset = dsp5680xx_assert_reset,
1890   .deassert_reset = dsp5680xx_deassert_reset,
1891   .soft_reset_halt = dsp5680xx_soft_reset_halt,
1892
1893   .read_memory = dsp5680xx_read,
1894   .write_memory = dsp5680xx_write,
1895   .bulk_write_memory = dsp5680xx_bulk_write_memory,
1896
1897   .checksum_memory = dsp5680xx_checksum_memory,
1898
1899   .target_create = dsp5680xx_target_create,
1900   .init_target = dsp5680xx_init_target,
1901 };