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

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