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   uint8_t jtag_status;
 250   uint16_t eonce_status;
 251   if(target->state == TARGET_HALTED){
 252     LOG_USER("Target already halted.");
 253     return ERROR_OK;
 254   }
 255   retval = eonce_enter_debug_mode(target,&eonce_status);
 256   err_check_propagate(retval);
 257   retval = dsp5680xx_jtag_status(target,&jtag_status);
 258   err_check_propagate(retval);
 259   retval = eonce_pc_store(target);
 260   err_check_propagate(retval);
 261   //TODO is it useful to store the pc?
 262   return retval;
 263 }
 264
 265 static int dsp5680xx_resume(struct target *target, int current, uint32_t address,int handle_breakpoints, int debug_execution){
 266   if(target->state == TARGET_RUNNING){
 267     LOG_USER("Target already running.");
 268     return ERROR_OK;
 269   }
 270   int retval;
 271   uint8_t jtag_status;
 272   uint16_t eonce_status;
 273
 274   // Verify that EOnCE is enabled (enable it if necessary)
 275   uint16_t data_read_from_dr = 0;
 276   retval = eonce_read_status_reg(target,&data_read_from_dr);
 277   err_check_propagate(retval);
 278   if((data_read_from_dr&DSP5680XX_ONCE_OSCR_DEBUG_M) != DSP5680XX_ONCE_OSCR_DEBUG_M){
 279     retval = eonce_enter_debug_mode(target,NULL);
 280         err_check_propagate(retval);
 281   }
 282   if(!current){
 283     retval = eonce_move_value_to_pc(target,address);
 284     err_check_propagate(retval);
 285   }
 286
 287   int retry = 20;
 288   while(retry-- > 1){
 289     retval = eonce_exit_debug_mode(target,(uint8_t *)&eonce_status );
 290         err_check_propagate(retval);
 291     retval = dsp5680xx_jtag_status(target,&jtag_status);
 292         err_check_propagate(retval);
 293     if((jtag_status & 0xff) == JTAG_STATUS_NORMAL){
 294       break;
 295     }
 296   }
 297   if(retry == 0){
 298     retval = ERROR_TARGET_FAILURE;
 299         err_check(retval,"Failed to resume...");
 300   }else{
 301     target->state = TARGET_RUNNING;
 302   };
 303   LOG_DEBUG("JTAG status: 0x%02X.",jtag_status);
 304   LOG_DEBUG("EOnCE status: 0x%02X.",eonce_status);
 305   return ERROR_OK;
 306 }
 307
 308 static int jtag_data_read(struct target * target, uint32_t * data_read, int num_bits){
 309   uint32_t bogus_instr;
 310   int retval = dsp5680xx_drscan(target,(uint8_t *) & bogus_instr,(uint8_t *) data_read,num_bits);
 311   LOG_DEBUG("Data read (%d bits): 0x%04X",num_bits,*data_read);//TODO remove this or move to jtagio?
 312   return retval;
 313 }
 314
 315 #define jtag_data_read8(target,data_read)  jtag_data_read(target,data_read,8)
 316 #define jtag_data_read16(target,data_read) jtag_data_read(target,data_read,16)
 317 #define jtag_data_read32(target,data_read) jtag_data_read(target,data_read,32)
 318
 319 static int jtag_data_write(struct target * target, uint32_t instr,int num_bits, uint32_t * data_read){
 320   int retval;
 321   uint32_t data_read_dummy;
 322   retval = dsp5680xx_drscan(target,(uint8_t *) & instr,(uint8_t *) & data_read_dummy,num_bits);
 323   err_check_propagate(retval);
 324   if(data_read != NULL)
 325     *data_read = data_read_dummy;
 326   return retval;
 327 }
 328
 329 #define jtag_data_write8(target,instr,data_read)  jtag_data_write(target,instr,8,data_read)
 330 #define jtag_data_write16(target,instr,data_read) jtag_data_write(target,instr,16,data_read)
 331 #define jtag_data_write24(target,instr,data_read) jtag_data_write(target,instr,24,data_read)
 332 #define jtag_data_write32(target,instr,data_read) jtag_data_write(target,instr,32,data_read)
 333
 334 static int eonce_enter_debug_mode(struct target * target, uint16_t * eonce_status){
 335   int retval;
 336   uint32_t instr = JTAG_INSTR_DEBUG_REQUEST;
 337   uint32_t ir_out;//not used, just to make jtag happy.
 338   // Debug request #1
 339   retval = dsp5680xx_irscan(target,& instr,& ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
 340   err_check_propagate(retval);
 341
 342   // Enable EOnCE module
 343   instr = JTAG_INSTR_ENABLE_ONCE;
 344   //Two rounds of jtag 0x6  (enable eonce) to enable EOnCE.
 345   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
 346   err_check_propagate(retval);
 347   retval =  dsp5680xx_irscan(target, & instr, & ir_out,DSP5680XX_JTAG_CORE_TAP_IRLEN);
 348   err_check_propagate(retval);
 349   // Verify that debug mode is enabled
 350   uint16_t data_read_from_dr;
 351   retval = eonce_read_status_reg(target,&data_read_from_dr);
 352   err_check_propagate(retval);
 353   if((data_read_from_dr&0x30) == 0x30){
 354     LOG_DEBUG("EOnCE successfully entered debug mode.");
 355     target->state = TARGET_HALTED;
 356     return ERROR_OK;
 357   }else{
 358         retval = ERROR_TARGET_FAILURE;
 359         err_check(retval,"Failed to set EOnCE module to debug mode.");
 360   }
 361   if(eonce_status!=NULL)
 362     *eonce_status = data_read_from_dr;
 363   return ERROR_OK;
 364 }
 365
 366 static int eonce_instruction_exec(struct target * target, uint8_t instr, uint8_t rw, uint8_t go, uint8_t ex,uint8_t * eonce_status){
 367   int retval;
 368   uint32_t dr_out_tmp;
 369   uint8_t instr_with_flags = instr|(rw<<7)|(go<<6)|(ex<<5);
 370   retval = jtag_data_write(target,instr_with_flags,8,&dr_out_tmp);
 371   err_check_propagate(retval);
 372   if(eonce_status != NULL)
 373     *eonce_status =  (uint8_t) dr_out_tmp;
 374   return retval;
 375 }
 376
 377 /* Executes DSP instruction */
 378 /* wrappers for parameter conversion between eonce_execute_instruction and eonce_execute_instructionX */
 379 #define eonce_execute_instruction_1(target,opcode1,opcode2,opcode3)      eonce_execute_instruction1(target,opcode1)
 380 #define eonce_execute_instruction_2(target,opcode1,opcode2,opcode3)      eonce_execute_instruction2(target,opcode1,opcode2)
 381 #define eonce_execute_instruction_3(target,opcode1,opcode2,opcode3)      eonce_execute_instruction3(target,opcode1,opcode2,opcode3)
 382 /* the macro itself */
 383 #define eonce_execute_instruction(target,words,opcode1,opcode2,opcode3) eonce_execute_instruction_##words(target,opcode1,opcode2,opcode3)
 384
 385 /* Executes one word DSP instruction */
 386 static int eonce_execute_instruction1(struct target * target, uint16_t opcode){
 387   int retval;
 388   retval = eonce_instruction_exec(target,0x04,0,1,0,NULL);
 389   err_check_propagate(retval);
 390   retval = jtag_data_write16(target,opcode,NULL);
 391   err_check_propagate(retval);
 392   return retval;
 393 }
 394
 395 /* Executes two word DSP instruction */
 396 static int eonce_execute_instruction2(struct target * target,uint16_t opcode1, uint16_t opcode2){
 397   int retval;
 398   retval = eonce_instruction_exec(target,0x04,0,0,0,NULL);
 399   err_check_propagate(retval);
 400   retval = jtag_data_write16(target,opcode1,NULL);
 401   err_check_propagate(retval);
 402   retval = eonce_instruction_exec(target,0x04,0,1,0,NULL);
 403   err_check_propagate(retval);
 404   retval = jtag_data_write16(target,opcode2,NULL);
 405   err_check_propagate(retval);
 406   return retval;
 407 }
 408
 409 /* Executes three word DSP instruction */
 410 static int eonce_execute_instruction3(struct target * target, uint16_t opcode1,uint16_t opcode2,uint16_t opcode3){
 411   int retval;
 412   retval = eonce_instruction_exec(target,0x04,0,0,0,NULL);
 413   err_check_propagate(retval);
 414   retval = jtag_data_write16(target,opcode1,NULL);
 415   err_check_propagate(retval);
 416   retval = eonce_instruction_exec(target,0x04,0,0,0,NULL);
 417   err_check_propagate(retval);
 418   retval = jtag_data_write16(target,opcode2,NULL);
 419   err_check_propagate(retval);
 420   retval = eonce_instruction_exec(target,0x04,0,1,0,NULL);
 421   err_check_propagate(retval);
 422   retval = jtag_data_write16(target,opcode3,NULL);
 423   err_check_propagate(retval);
 424   return retval;
 425 }
 426
 427 /* --------------- Real-time data exchange --------------- */
 428 /*
 429   The EOnCE Transmit (OTX) and Receive (ORX) registers are data memory mapped, each with an upper and lower 16 bit word.
 430   Transmit and receive directions are defined from the core’s perspective.
 431   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.
 432   Both registers have a combined data memory mapped OTXRXSR which provides indication when each may be accessed.
 433 ref: eonce_rev.1.0_0208081.pdf@36
 434 */
 435
 436 static int eonce_tx_upper_data(struct target * target, uint16_t data, uint32_t * eonce_status_low){
 437   int retval;
 438   retval = eonce_instruction_exec(target,DSP5680XX_ONCE_ORX1,0,0,0,NULL);
 439   err_check_propagate(retval);
 440   retval = jtag_data_write16(target,data,eonce_status_low);
 441   err_check_propagate(retval);
 442   return retval;
 443 }
 444
 445 /* writes data into lower ORx register of the target */
 446 #define eonce_tx_lower_data(target,data) eonce_instruction_exec(target,DSP5680XX_ONCE_ORX,0,0,0,NULL);\
 447                                                                   jtag_data_write16(target,data)
 448
 449 /**
 450  *
 451  * @param target
 452  * @param data_read: Returns the data read from the upper OTX register via JTAG.
 453  * @return: Returns an error code (see error code documentation)
 454  */
 455 static int eonce_rx_upper_data(struct target * target, uint16_t * data_read)
 456 {
 457   int retval;
 458   retval = eonce_instruction_exec(target,DSP5680XX_ONCE_OTX1,1,0,0,NULL);
 459   err_check_propagate(retval);
 460   retval = jtag_data_read16(target,(uint32_t *)data_read);
 461   err_check_propagate(retval);
 462   return retval;
 463 }
 464
 465 /**
 466  *
 467  * @param target
 468  * @param data_read: Returns the data read from the lower OTX register via JTAG.
 469  * @return: Returns an error code (see error code documentation)
 470  */
 471 static int eonce_rx_lower_data(struct target * target,uint16_t * data_read)
 472 {
 473   int retval;
 474   retval = eonce_instruction_exec(target,DSP5680XX_ONCE_OTX,1,0,0,NULL);
 475   err_check_propagate(retval);
 476   retval = jtag_data_read16(target,(uint32_t *)data_read);
 477   err_check_propagate(retval);
 478   return retval;
 479 }
 480
 481 /* -- -- -- -- --- -- -- -- --- -- -- -- --- -- -- -- --- -- -- -- --- -- -*/
 482 /* -- -- -- -- --- -- -- -Core Instructions- -- -- -- --- -- -- -- --- -- -*/
 483 /* -- -- -- -- --- -- -- -- --- -- -- -- --- -- -- -- --- -- -- -- --- -- -*/
 484 /* move.l #value,r0 */
 485 #define eonce_move_long_to_r0(target,value)     eonce_execute_instruction(target,3,0xe418,value&0xffff,value>>16)
 486
 487 /* move.l #value,n */
 488 #define eonce_move_long_to_n(target,value)              eonce_execute_instruction(target,3,0xe41e,value&0xffff,value>>16)
 489
 490 /* move x:(r0),y0 */
 491 #define eonce_move_at_r0_to_y0(target)                  eonce_execute_instruction(target,1,0xF514,0,0)
 492
 493 /* move x:(r0),y1 */
 494 #define eonce_move_at_r0_to_y1(target)                  eonce_execute_instruction(target,1,0xF714,0,0)
 495
 496 /* move.l x:(r0),y */
 497 #define eonce_move_long_at_r0_y(target) eonce_execute_instruction(target,1,0xF734,0,0)
 498
 499 /* move y0,x:(r0) */
 500 #define eonce_move_y0_at_r0(target)                     eonce_execute_instruction(target,1,0xd514,0,0)
 501
 502 /* bfclr #value,x:(r0) */
 503 #define eonce_bfclr_at_r0(target,value)         eonce_execute_instruction(target,2,0x8040,value,0)
 504
 505 /* move #value,y0 */
 506 #define eonce_move_value_to_y0(target,value)    eonce_execute_instruction(target,2,0x8745,value,0)
 507
 508 /* move.w y0,x:(r0)+ */
 509 #define eonce_move_y0_at_r0_inc(target)         eonce_execute_instruction(target,1,0xd500,0,0)
 510
 511 /* move.w y0,p:(r0)+ */
 512 #define eonce_move_y0_at_pr0_inc(target)                eonce_execute_instruction(target,1,0x8560,0,0)
 513
 514 /* move.w p:(r0)+,y0 */
 515 #define eonce_move_at_pr0_inc_to_y0(target)     eonce_execute_instruction(target,1,0x8568,0,0)
 516
 517 /* move.w p:(r0)+,y1 */
 518 #define eonce_move_at_pr0_inc_to_y1(target)     eonce_execute_instruction(target,1,0x8768,0,0)
 519
 520 /* move.l #value,r2 */
 521 #define eonce_move_long_to_r2(target,value)     eonce_execute_instruction(target,3,0xe41A,value&0xffff,value>>16)
 522
 523 /* move y0,x:(r2) */
 524 #define eonce_move_y0_at_r2(target)             eonce_execute_instruction(target,1,0xd516,0,0)
 525
 526 /* move.w #<value>,x:(r2) */
 527 #define eonce_move_value_at_r2(target,value)    eonce_execute_instruction(target,2,0x8642,value,0)
 528
 529 /* move.w #<value>,x:(r0) */
 530 #define eonce_move_value_at_r0(target,value)    eonce_execute_instruction(target,2,0x8640,value,0)
 531
 532 /* move.w #<value>,x:(R2+<disp>) */
 533 #define eonce_move_value_at_r2_disp(target,value,disp)  eonce_execute_instruction(target,3,0x8646,value,disp)
 534
 535 /* move.w x:(r2),Y0 */
 536 #define eonce_move_at_r2_to_y0(target)          eonce_execute_instruction(target,1,0xF516,0,0)
 537
 538 /* move.w p:(r2)+,y0 */
 539 #define eonce_move_at_pr2_inc_to_y0(target)     eonce_execute_instruction(target,1,0x856A,0,0)
 540
 541 /* move.l #value,r3 */
 542 #define eonce_move_long_to_r1(target,value)     eonce_execute_instruction(target,3,0xE419,value&0xffff,value>>16)
 543
 544 /* move.l #value,r3 */
 545 #define eonce_move_long_to_r3(target,value)     eonce_execute_instruction(target,3,0xE41B,value&0xffff,value>>16)
 546
 547 /* move.w y0,p:(r3)+ */
 548 #define eonce_move_y0_at_pr3_inc(target)                eonce_execute_instruction(target,1,0x8563,0,0)
 549
 550 /* move.w y0,x:(r3) */
 551 #define eonce_move_y0_at_r3(target)                     eonce_execute_instruction(target,1,0xD503,0,0)
 552
 553 /* move pc,r4 */
 554 #define eonce_move_pc_to_r4(target)                     eonce_execute_instruction(target,1,0xE716,0,0)
 555
 556 /* move.l r4,y */
 557 #define eonce_move_r4_to_y(target)                      eonce_execute_instruction(target,1,0xe764,0,0)
 558
 559 /* move.w p:(r0)+,y0 */
 560 #define eonce_move_at_pr0_inc_to_y0(target)     eonce_execute_instruction(target,1,0x8568,0,0)
 561
 562 /* move.w x:(r0)+,y0 */
 563 #define eonce_move_at_r0_inc_to_y0(target)      eonce_execute_instruction(target,1,0xf500,0,0)
 564
 565 /* move x:(r0),y0 */
 566 #define eonce_move_at_r0_y0(target)                     eonce_execute_instruction(target,1,0xF514,0,0)
 567
 568 /* nop */
 569 #define eonce_nop(target)               eonce_execute_instruction(target,1,0xe700,0,0)
 570
 571 /* move.w x:(R2+<disp>),Y0 */
 572 #define eonce_move_at_r2_disp_to_y0(target,disp) eonce_execute_instruction(target,2,0xF542,disp,0)
 573
 574 /* move.w y1,x:(r2) */
 575 #define eonce_move_y1_at_r2(target) eonce_execute_instruction(target,1,0xd716,0,0)
 576
 577 /* move.w y1,x:(r0) */
 578 #define eonce_move_y1_at_r0(target) eonce_execute_instruction(target,1,0xd714,0,0)
 579
 580 /* move.bp y0,x:(r0)+ */
 581 #define eonce_move_byte_y0_at_r0(target) eonce_execute_instruction(target,1,0xd5a0,0,0)
 582
 583 /* move.w y1,p:(r0)+ */
 584 #define eonce_move_y1_at_pr0_inc(target) eonce_execute_instruction(target,1,0x8760,0,0)
 585
 586 /* move.w y1,x:(r0)+ */
 587 #define eonce_move_y1_at_r0_inc(target) eonce_execute_instruction(target,1,0xD700,0,0)
 588
 589 /* move.l #value,y */
 590 #define eonce_move_long_to_y(target,value) eonce_execute_instruction(target,3,0xe417,value&0xffff,value>>16)
 591
 592 static int eonce_move_value_to_pc(struct target * target, uint32_t value)
 593 {
 594   if (!(target->state == TARGET_HALTED)){
 595     LOG_ERROR("Target must be halted to move PC. Target state = %d.",target->state);
 596     return ERROR_TARGET_NOT_HALTED;
 597   };
 598   int retval;
 599   retval = eonce_execute_instruction(target,3,0xE71E,value&0xffff,value>>16);
 600   err_check_propagate(retval);
 601   return retval;
 602 }
 603
 604 static int eonce_load_TX_RX_to_r0(struct target * target)
 605 {
 606   int retval;
 607   retval = eonce_move_long_to_r0(target,((MC568013_EONCE_TX_RX_ADDR)+(MC568013_EONCE_OBASE_ADDR<<16)));
 608   return retval;
 609 }
 610
 611 static int eonce_load_TX_RX_high_to_r0(struct target * target)
 612 {
 613   int retval = 0;
 614   retval = eonce_move_long_to_r0(target,((MC568013_EONCE_TX1_RX1_HIGH_ADDR)+(MC568013_EONCE_OBASE_ADDR<<16)));
 615   return retval;
 616 }
 617
 618 static int eonce_pc_store(struct target * target){
 619   uint32_t tmp = 0;
 620   int retval;
 621   retval = eonce_move_pc_to_r4(target);
 622   err_check_propagate(retval);
 623   retval = eonce_move_r4_to_y(target);
 624   err_check_propagate(retval);
 625   retval = eonce_load_TX_RX_to_r0(target);
 626   err_check_propagate(retval);
 627   retval = eonce_move_y0_at_r0(target);
 628   err_check_propagate(retval);
 629   retval = eonce_rx_lower_data(target,(uint16_t *)&tmp);
 630   err_check_propagate(retval);
 631   LOG_USER("PC value: 0x%06X\n",tmp);
 632   context.stored_pc = (uint32_t)tmp;
 633   return ERROR_OK;
 634 }
 635
 636 static int dsp5680xx_convert_address(uint32_t * address, int * pmem){
 637   // Distinguish data memory (x:) from program memory (p:) by the address.
 638   // Addresses over S_FILE_DATA_OFFSET are considered (x:) memory.
 639   if(*address >= S_FILE_DATA_OFFSET){
 640     *pmem = 0;
 641     if(((*address)&0xff0000)!=0xff0000)
 642       *address -= S_FILE_DATA_OFFSET;
 643   }
 644   return ERROR_OK;
 645 }
 646
 647 static int dsp5680xx_read_16_single(struct target * target, uint32_t address, uint16_t * data_read, int r_pmem){
 648   //TODO add error control!
 649   int retval;
 650   retval = eonce_move_long_to_r0(target,address);
 651   err_check_propagate(retval);
 652   if(r_pmem)
 653     retval = eonce_move_at_pr0_inc_to_y0(target);
 654   else
 655     retval = eonce_move_at_r0_to_y0(target);
 656   err_check_propagate(retval);
 657   retval = eonce_load_TX_RX_to_r0(target);
 658   err_check_propagate(retval);
 659   retval = eonce_move_y0_at_r0(target);
 660   err_check_propagate(retval);
 661   // at this point the data i want is at the reg eonce can read
 662   retval = eonce_rx_lower_data(target,data_read);
 663   err_check_propagate(retval);
 664   LOG_DEBUG("%s: Data read from 0x%06X: 0x%04X",__FUNCTION__, address,*data_read);
 665   return retval;
 666 }
 667
 668 static int dsp5680xx_read_32_single(struct target * target, uint32_t address, uint32_t * data_read, int r_pmem){
 669   int retval;
 670   address = (address & 0xFFFFFE);
 671   // Get data to an intermediate register
 672   retval = eonce_move_long_to_r0(target,address);
 673   err_check_propagate(retval);
 674   if(r_pmem){
 675     retval = eonce_move_at_pr0_inc_to_y0(target);
 676         err_check_propagate(retval);
 677     retval = eonce_move_at_pr0_inc_to_y1(target);
 678         err_check_propagate(retval);
 679   }else{
 680     retval = eonce_move_at_r0_inc_to_y0(target);
 681         err_check_propagate(retval);
 682     retval = eonce_move_at_r0_to_y1(target);
 683         err_check_propagate(retval);
 684   }
 685   // Get lower part of data to TX/RX
 686   retval = eonce_load_TX_RX_to_r0(target);
 687   err_check_propagate(retval);
 688   retval = eonce_move_y0_at_r0_inc(target); // This also load TX/RX high to r0
 689   err_check_propagate(retval);
 690   // Get upper part of data to TX/RX
 691   retval = eonce_move_y1_at_r0(target);
 692   err_check_propagate(retval);
 693   // at this point the data i want is at the reg eonce can read
 694   retval = eonce_rx_lower_data(target,(uint16_t * )data_read);
 695   err_check_propagate(retval);
 696   uint16_t tmp;
 697   retval = eonce_rx_upper_data(target,&tmp);
 698   err_check_propagate(retval);
 699   *data_read = ((tmp<<16) | (*data_read));//This enables opencd crc to succeed, even though it's very slow.
 700   return retval;
 701 }
 702
 703 static int dsp5680xx_read(struct target * target, uint32_t address, unsigned size, unsigned count, uint8_t * buffer){
 704   if(target->state != TARGET_HALTED){
 705     LOG_USER("Target must be halted.");
 706     return ERROR_OK;
 707   }
 708   uint32_t * buff32 = (uint32_t *) buffer;
 709   uint16_t * buff16 = (uint16_t *) buffer;
 710   int retval = ERROR_OK;
 711   int pmem = 1;
 712   uint16_t tmp_wrd;
 713
 714   retval = dsp5680xx_convert_address(&address, &pmem);
 715   err_check_propagate(retval);
 716
 717   context.flush = 0;
 718   int counter = FLUSH_COUNT_READ_WRITE;
 719
 720   for (unsigned i=0; i<count; i++){
 721     if(--counter==0){
 722       context.flush = 1;
 723       counter = FLUSH_COUNT_FLASH;
 724     }
 725     switch (size){
 726     case 1:
 727       if(!(i%2)){
 728                 retval = dsp5680xx_read_16_single(target, address + i/2, &tmp_wrd, pmem);
 729                 buffer[i] = (uint8_t) (tmp_wrd>>8);
 730                 buffer[i+1] = (uint8_t) (tmp_wrd&0xff);
 731       }
 732       break;
 733     case 2:
 734       retval = dsp5680xx_read_16_single(target, address + i, buff16 + i, pmem);
 735       break;
 736     case 4:
 737       retval = dsp5680xx_read_32_single(target, address + 2*i, buff32 + i, pmem);
 738       break;
 739     default:
 740       LOG_USER("%s: Invalid read size.",__FUNCTION__);
 741       break;
 742     }
 743         err_check_propagate(retval);
 744     context.flush = 0;
 745   }
 746
 747   context.flush = 1;
 748   retval = dsp5680xx_execute_queue();
 749   err_check_propagate(retval);
 750
 751   return retval;
 752 }
 753
 754 //TODO doxy
 755 static int dsp5680xx_write_16_single(struct target *target, uint32_t address, uint16_t data, uint8_t w_pmem){
 756   int retval = 0;
 757   retval = eonce_move_long_to_r0(target,address);
 758   err_check_propagate(retval);
 759   if(w_pmem){
 760     retval = eonce_move_value_to_y0(target,data);
 761         err_check_propagate(retval);
 762     retval = eonce_move_y0_at_pr0_inc(target);
 763         err_check_propagate(retval);
 764   }else{
 765     retval = eonce_move_value_at_r0(target,data);
 766         err_check_propagate(retval);
 767   }
 768   return retval;
 769 }
 770
 771 //TODO doxy
 772 static int dsp5680xx_write_32_single(struct target *target, uint32_t address, uint32_t data, int w_pmem){
 773   int retval = 0;
 774   retval = eonce_move_long_to_r0(target,address);
 775   err_check_propagate(retval);
 776   retval = eonce_move_long_to_y(target,data);
 777   err_check_propagate(retval);
 778   if(w_pmem)
 779     retval = eonce_move_y0_at_pr0_inc(target);
 780   else
 781     retval = eonce_move_y0_at_r0_inc(target);
 782   err_check_propagate(retval);
 783   if(w_pmem)
 784     retval = eonce_move_y1_at_pr0_inc(target);
 785   else
 786     retval = eonce_move_y1_at_r0_inc(target);
 787   err_check_propagate(retval);
 788   return retval;
 789 }
 790
 791 static int dsp5680xx_write_8(struct target * target, uint32_t address, uint32_t count, uint8_t * data, int pmem){
 792   if(target->state != TARGET_HALTED){
 793     LOG_ERROR("%s: Target must be halted.",__FUNCTION__);
 794     return ERROR_OK;
 795   };
 796   int retval = 0;
 797   uint16_t * data_w = (uint16_t *)data;
 798   uint32_t iter;
 799
 800   int counter = FLUSH_COUNT_READ_WRITE;
 801   for(iter = 0; iter<count/2; iter++){
 802     if(--counter==0){
 803       context.flush = 1;
 804       counter = FLUSH_COUNT_READ_WRITE;
 805     }
 806     retval = dsp5680xx_write_16_single(target,address+iter,data_w[iter], pmem);
 807     if(retval != ERROR_OK){
 808       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 809       context.flush = 1;
 810       return retval;
 811     }
 812     context.flush = 0;
 813   }
 814   context.flush = 1;
 815
 816   // Only one byte left, let's not overwrite the other byte (mem is 16bit)
 817   // Need to retrieve the part we do not want to overwrite.
 818   uint16_t data_old;
 819   if((count==1)||(count%2)){
 820     retval = dsp5680xx_read(target,address+iter,1,1,(uint8_t *)&data_old);
 821         err_check_propagate(retval);
 822     if(count==1)
 823       data_old=(((data_old&0xff)<<8)|data[0]);// preserve upper byte
 824     else
 825       data_old=(((data_old&0xff)<<8)|data[2*iter+1]);
 826     retval = dsp5680xx_write_16_single(target,address+iter,data_old, pmem);
 827         err_check_propagate(retval);
 828   }
 829   return retval;
 830 }
 831
 832 static int dsp5680xx_write_16(struct target * target, uint32_t address, uint32_t count, uint16_t * data, int pmem){
 833   int retval = ERROR_OK;
 834   if(target->state != TARGET_HALTED){
 835         retval = ERROR_TARGET_NOT_HALTED;
 836         err_check(retval,"Target must be halted.");
 837   };
 838   uint32_t iter;
 839   int counter = FLUSH_COUNT_READ_WRITE;
 840
 841   for(iter = 0; iter<count; iter++){
 842         if(--counter==0){
 843           context.flush = 1;
 844       counter = FLUSH_COUNT_READ_WRITE;
 845         }
 846     retval = dsp5680xx_write_16_single(target,address+iter,data[iter], pmem);
 847     if(retval != ERROR_OK){
 848       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 849           context.flush = 1;
 850       return retval;
 851     }
 852         context.flush = 0;
 853   }
 854   context.flush = 1;
 855   return retval;
 856 }
 857
 858 static int dsp5680xx_write_32(struct target * target, uint32_t address, uint32_t count, uint32_t * data, int pmem){
 859   int retval = ERROR_OK;
 860   if(target->state != TARGET_HALTED){
 861         retval = ERROR_TARGET_NOT_HALTED;
 862         err_check(retval,"Target must be halted.");
 863   };
 864   uint32_t iter;
 865   int counter = FLUSH_COUNT_READ_WRITE;
 866
 867   for(iter = 0; iter<count; iter++){
 868         if(--counter==0){
 869           context.flush = 1;
 870       counter = FLUSH_COUNT_READ_WRITE;
 871         }
 872     retval = dsp5680xx_write_32_single(target,address+(iter<<1),data[iter], pmem);
 873     if(retval != ERROR_OK){
 874       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 875           context.flush = 1;
 876       return retval;
 877     }
 878         context.flush = 0;
 879   }
 880   context.flush = 1;
 881   return retval;
 882 }
 883
 884 //TODO doxy
 885 static int dsp5680xx_write(struct target *target, uint32_t address, uint32_t size, uint32_t count, const uint8_t * buffer){
 886   //TODO Cannot write 32bit to odd address, will write 0x12345678  as 0x5678 0x0012
 887   if(target->state != TARGET_HALTED){
 888     LOG_USER("Target must be halted.");
 889     return ERROR_OK;
 890   }
 891   int retval = 0;
 892   int p_mem = 1;
 893   retval = dsp5680xx_convert_address(&address, &p_mem);
 894   err_check_propagate(retval);
 895
 896   switch (size){
 897   case 1:
 898     retval = dsp5680xx_write_8(target, address, count,(uint8_t *) buffer, p_mem);
 899     break;
 900   case 2:
 901     retval = dsp5680xx_write_16(target, address, count, (uint16_t *)buffer, p_mem);
 902       break;
 903   case 4:
 904     retval = dsp5680xx_write_32(target, address, count, (uint32_t *)buffer, p_mem);
 905     break;
 906   default:
 907         retval = ERROR_TARGET_DATA_ABORT;
 908         err_check(retval,"Invalid data size.")
 909         break;
 910   }
 911   return retval;
 912 }
 913
 914 static int dsp5680xx_bulk_write_memory(struct target * target,uint32_t address, uint32_t aligned, const uint8_t * buffer){
 915   LOG_ERROR("Not implemented yet.");
 916   return ERROR_FAIL;
 917 }
 918
 919 // Writes to pram at address
 920 // r3 holds the destination address-> p:(r3)
 921 // r2 hold 0xf151 to flash a led (probably cannot see it due to high freq.)
 922 // r0 holds TX/RX address.
 923 //0x00000073  0x8A44FFFE017B         brclr       #1,X:(R0-2),*-2
 924 //0x00000076  0xE700                 nop
 925 //0x00000077  0xF514                 move.w      X:(R0),Y0
 926 //0x00000078  0xE700                 nop
 927 //0x00000079  0x8563                 move.w      Y0,P:(R3)+
 928 //0x0000007A  0x84420003             bfchg       #3,X:(R2)
 929 //0x0000007C  0xA976                 bra         *-9
 930 uint16_t pgm_write_pram[] = {0x8A44,0xFFFE,0x017D,0xE700,0xF514,0xE700,0x8563,0x8442,0x0003,0xA976};
 931 uint16_t pgm_write_pram_length = 10;
 932
 933 static int dsp5680xx_write_buffer(struct target * target, uint32_t address, uint32_t size, const uint8_t * buffer){
 934   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
 935   // this solution works, but it's slow. it flushes USB all the time.
 936   return dsp5680xx_write(target, address, 1, size, buffer);
 937   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
 938 }
 939
 940 static int dsp5680xx_read_buffer(struct target * target, uint32_t address, uint32_t size, uint8_t * buffer){
 941   if(target->state != TARGET_HALTED){
 942     LOG_USER("Target must be halted.");
 943     return ERROR_OK;
 944   }
 945   // read_buffer is called when the verify_image command is executed.
 946   // The "/2" solves the byte/word addressing issue.
 947   return dsp5680xx_read(target,address,2,size/2,buffer);
 948 }
 949
 950 static int dsp5680xx_checksum_memory(struct target * target, uint32_t address, uint32_t size, uint32_t * checksum){
 951   return ERROR_FAIL;// This will make OpenOCD do the read out the data and verify it.
 952 }
 953
 954 // Data signature algorithm used by the core FM (flash module)
 955 static int perl_crc(uint16_t * buff16,uint32_t  word_count){
 956   uint16_t checksum = 0xffff;
 957   uint16_t data,fbmisr;
 958   uint32_t i;
 959   for(i=0;i<word_count;i++){
 960     data = buff16[i];
 961     fbmisr = (checksum & 2)>>1 ^ (checksum & 4)>>2 ^ (checksum & 16)>>4 ^ (checksum & 0x8000)>>15;
 962     checksum = (data ^ ((checksum << 1) | fbmisr));
 963   }
 964   i--;
 965   for(;!(i&0x80000000);i--){
 966     data = buff16[i];
 967     fbmisr = (checksum & 2)>>1 ^ (checksum & 4)>>2 ^ (checksum & 16)>>4 ^ (checksum & 0x8000)>>15;
 968     checksum = (data ^ ((checksum << 1) | fbmisr));
 969   }
 970   return checksum;
 971 }
 972
 973 int dsp5680xx_f_SIM_reset(struct target * target){
 974   int retval = ERROR_OK;
 975   uint16_t sim_cmd = SIM_CMD_RESET;
 976   uint32_t sim_addr;
 977   if(strcmp(target->tap->chip,"dsp568013")==0){
 978         sim_addr = MC568013_SIM_BASE_ADDR+S_FILE_DATA_OFFSET;
 979         retval = dsp5680xx_write(target,sim_addr,1,2,(const uint8_t *)&sim_cmd);
 980         err_check_propagate(retval);
 981   }
 982   return retval;
 983 }
 984
 985 //TODO doxy
 986 static int dsp5680xx_soft_reset_halt(struct target *target){
 987   //TODO is this what this function is expected to do...?
 988   int retval;
 989   retval = dsp5680xx_halt(target);
 990   err_check_propagate(retval);
 991   retval = dsp5680xx_f_SIM_reset(target);
 992   err_check_propagate(retval);
 993   return retval;
 994 }
 995
 996 int dsp5680xx_f_protect_check(struct target * target, uint16_t * protected) {
 997   uint16_t aux;
 998   int retval;
 999   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1000     retval = dsp5680xx_halt(target);
1001         err_check_propagate(retval);
1002   }
1003   if(protected == NULL){
1004     err_check(ERROR_FAIL,"NULL pointer not valid.");
1005   }
1006   retval = dsp5680xx_read_16_single(target,HFM_BASE_ADDR|HFM_PROT,&aux,0);
1007   err_check_propagate(retval);
1008   *protected = aux;
1009   return retval;
1010 }
1011
1012 static int dsp5680xx_f_execute_command(struct target * target, uint16_t command, uint32_t address, uint32_t data, uint16_t * hfm_ustat, int pmem){
1013   int retval;
1014   retval = eonce_load_TX_RX_high_to_r0(target);
1015   err_check_propagate(retval);
1016   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);
1017   err_check_propagate(retval);
1018   uint16_t i;
1019   int watchdog = 100;
1020   do{
1021     retval = eonce_move_at_r2_disp_to_y0(target,HFM_USTAT);     // read HMF_USTAT
1022         err_check_propagate(retval);
1023     retval = eonce_move_y0_at_r0(target);
1024         err_check_propagate(retval);
1025     retval = eonce_rx_upper_data(target,&i);
1026         err_check_propagate(retval);
1027     if((watchdog--)==1){
1028       retval = ERROR_TARGET_FAILURE;
1029       err_check(retval,"FM execute command failed.");
1030     }
1031   }while (!(i&0x40));                           // wait until current command is complete
1032
1033   context.flush = 0;
1034
1035   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 ???
1036   err_check_propagate(retval);
1037   retval = eonce_move_value_at_r2_disp(target,0x04,HFM_USTAT);          // write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1038   err_check_propagate(retval);
1039   retval = eonce_move_value_at_r2_disp(target,0x10,HFM_USTAT);          // clear only one bit at a time
1040   err_check_propagate(retval);
1041   retval = eonce_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1042   err_check_propagate(retval);
1043   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROT);           // write to HMF_PROT, clear protection
1044   err_check_propagate(retval);
1045   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROTB);          // write to HMF_PROTB, clear protection
1046   err_check_propagate(retval);
1047   retval = eonce_move_value_to_y0(target,data);
1048   err_check_propagate(retval);
1049   retval = eonce_move_long_to_r3(target,address);                       // write to the flash block
1050   err_check_propagate(retval);
1051   if (pmem){
1052     retval = eonce_move_y0_at_pr3_inc(target);
1053         err_check_propagate(retval);
1054   }else{
1055     retval = eonce_move_y0_at_r3(target);
1056         err_check_propagate(retval);
1057   }
1058   retval = eonce_move_value_at_r2_disp(target,command,HFM_CMD); // write command to the HFM_CMD reg
1059   err_check_propagate(retval);
1060   retval = eonce_move_value_at_r2_disp(target,0x80,HFM_USTAT);          // start the command
1061   err_check_propagate(retval);
1062
1063   context.flush = 1;
1064   retval = dsp5680xx_execute_queue();
1065   err_check_propagate(retval);
1066
1067   watchdog = 100;
1068   do{
1069     retval = eonce_move_at_r2_disp_to_y0(target,HFM_USTAT);     // read HMF_USTAT
1070         err_check_propagate(retval);
1071     retval = eonce_move_y0_at_r0(target);
1072         err_check_propagate(retval);
1073         retval = eonce_rx_upper_data(target,&i);
1074         err_check_propagate(retval);
1075     if((watchdog--)==1){
1076           retval = ERROR_TARGET_FAILURE;
1077       err_check(retval,"FM execution did not finish.");
1078     }
1079   }while (!(i&0x40));       // wait until the command is complete
1080   *hfm_ustat = i;
1081   if (i&HFM_USTAT_MASK_PVIOL_ACCER){
1082     retval = ERROR_TARGET_FAILURE;
1083     err_check(retval,"pviol and/or accer bits set. HFM command execution error");
1084   }
1085   return ERROR_OK;
1086 }
1087
1088 static int eonce_set_hfmdiv(struct target * target){
1089   uint16_t i;
1090   int retval;
1091   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);
1092   err_check_propagate(retval);
1093   retval = eonce_load_TX_RX_high_to_r0(target);
1094   err_check_propagate(retval);
1095   retval = eonce_move_at_r2_to_y0(target);// read HFM_CLKD
1096   err_check_propagate(retval);
1097   retval = eonce_move_y0_at_r0(target);
1098   err_check_propagate(retval);
1099   retval = eonce_rx_upper_data(target,&i);
1100   err_check_propagate(retval);
1101   unsigned int hfm_at_wrong_value = 0;
1102   if ((i&0x7f)!=HFM_CLK_DEFAULT) {
1103     LOG_DEBUG("HFM CLK divisor contained incorrect value (0x%02X).",i&0x7f);
1104     hfm_at_wrong_value = 1;
1105   }else{
1106     LOG_DEBUG("HFM CLK divisor was already set to correct value (0x%02X).",i&0x7f);
1107     return ERROR_OK;
1108   }
1109   retval = eonce_move_value_at_r2(target,HFM_CLK_DEFAULT);      // write HFM_CLKD
1110   err_check_propagate(retval);
1111   retval = eonce_move_at_r2_to_y0(target); // verify HFM_CLKD
1112   err_check_propagate(retval);
1113   retval = eonce_move_y0_at_r0(target);
1114   err_check_propagate(retval);
1115   retval = eonce_rx_upper_data(target,&i);
1116   err_check_propagate(retval);
1117   if (i!=(0x80|(HFM_CLK_DEFAULT&0x7f))) {
1118         retval = ERROR_TARGET_FAILURE;
1119         err_check(retval,"Unable to set HFM CLK divisor.");
1120   }
1121   if(hfm_at_wrong_value)
1122     LOG_DEBUG("HFM CLK divisor set to 0x%02x.",i&0x7f);
1123   return ERROR_OK;
1124 }
1125
1126 static int dsp5680xx_f_signature(struct target * target, uint32_t address, uint32_t words, uint16_t * signature){
1127   int retval;
1128   uint16_t hfm_ustat;
1129   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1130     retval = eonce_enter_debug_mode(target,NULL);
1131     err_check_propagate(retval);
1132   }
1133   retval = dsp5680xx_f_execute_command(target,HFM_CALCULATE_DATA_SIGNATURE,address,words,&hfm_ustat,1);
1134   err_check_propagate(retval);
1135   retval = dsp5680xx_read_16_single(target, HFM_BASE_ADDR|HFM_DATA, signature, 0);
1136   return retval;
1137 }
1138
1139 int dsp5680xx_f_erase_check(struct target * target, uint8_t * erased,uint32_t sector){
1140   int retval;
1141   uint16_t hfm_ustat;
1142   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1143     retval = dsp5680xx_halt(target);
1144     err_check_propagate(retval);
1145   }
1146   // Check if chip is already erased.
1147   retval = dsp5680xx_f_execute_command(target,HFM_ERASE_VERIFY,HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2,0,&hfm_ustat,1); // blank check
1148   err_check_propagate(retval);
1149   if(erased!=NULL)
1150     *erased = (uint8_t)(hfm_ustat&HFM_USTAT_MASK_BLANK);
1151   return retval;
1152 }
1153
1154 static int erase_sector(struct target * target, int sector, uint16_t * hfm_ustat){
1155   int retval;
1156   retval = dsp5680xx_f_execute_command(target,HFM_PAGE_ERASE,HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2,0,hfm_ustat,1);
1157   err_check_propagate(retval);
1158   return retval;
1159 }
1160
1161 static int mass_erase(struct target * target, uint16_t * hfm_ustat){
1162   int retval;
1163   retval = dsp5680xx_f_execute_command(target,HFM_MASS_ERASE,0,0,hfm_ustat,1);
1164   return retval;
1165 }
1166
1167 int dsp5680xx_f_erase(struct target * target, int first, int last){
1168   int retval;
1169   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1170     retval = dsp5680xx_halt(target);
1171     err_check_propagate(retval);
1172   }
1173   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1174   // Reset SIM
1175   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1176   retval = dsp5680xx_f_SIM_reset(target);
1177   err_check_propagate(retval);
1178   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1179   // Set hfmdiv
1180   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1181   retval = eonce_set_hfmdiv(target);
1182   err_check_propagate(retval);
1183
1184   uint16_t hfm_ustat;
1185   int do_mass_erase = ((!(first|last)) || ((first==0)&&(last == (HFM_SECTOR_COUNT-1))));
1186   if(do_mass_erase){
1187     //Mass erase
1188     retval = mass_erase(target,&hfm_ustat);
1189     err_check_propagate(retval);
1190     last = HFM_SECTOR_COUNT-1;
1191   }else{
1192     for(int i = first;i<=last;i++){
1193       retval = erase_sector(target,i,&hfm_ustat);
1194         err_check_propagate(retval);
1195   }
1196   }
1197   return ERROR_OK;
1198 }
1199
1200 // Algorithm for programming normal p: flash
1201 // Follow state machine from "56F801x Peripheral Reference Manual"@163.
1202 // Registers to set up before calling:
1203 //  r0: TX/RX high address.
1204 //  r2: FM module base address.
1205 //  r3: Destination address in flash.
1206 //
1207 //              hfm_wait:                                           // wait for command to finish
1208 //                      brclr   #0x40,x:(r2+0x13),hfm_wait
1209 //              rx_check:                                           // wait for input buffer full
1210 //                      brclr   #0x01,x:(r0-2),rx_check
1211 //                      move.w  x:(r0),y0                           // read from Rx buffer
1212 //                      move.w  y0,p:(r3)+
1213 //                      move.w  #0x20,x:(r2+0x14)                   // write PGM command
1214 //                      move.w  #0x80,x:(r2+0x13)                   // start the command
1215 //                      brclr       #0x20,X:(R2+0x13),accerr_check  // protection violation check
1216 //                      bfset       #0x20,X:(R2+0x13)               // clear pviol
1217 //                      bra         hfm_wait
1218 //              accerr_check:
1219 //                      brclr       #0x10,X:(R2+0x13),hfm_wait      // access error check
1220 //                      bfset       #0x10,X:(R2+0x13)               // clear accerr
1221 //                      bra         hfm_wait                        // loop
1222 //0x00000073  0x8A460013407D         brclr       #0x40,X:(R2+0x13),*+0
1223 //0x00000076  0xE700                 nop
1224 //0x00000077  0xE700                 nop
1225 //0x00000078  0x8A44FFFE017B         brclr       #1,X:(R0-2),*-2
1226 //0x0000007B  0xE700                 nop
1227 //0x0000007C  0xF514                 move.w      X:(R0),Y0
1228 //0x0000007D  0x8563                 move.w      Y0,P:(R3)+
1229 //0x0000007E  0x864600200014         move.w      #0x20,X:(R2+0x14)
1230 //0x00000081  0x864600800013         move.w      #0x80,X:(R2+0x13)
1231 //0x00000084  0x8A4600132004         brclr       #0x20,X:(R2+0x13),*+7
1232 //0x00000087  0x824600130020         bfset       #0x20,X:(R2+0x13)
1233 //0x0000008A  0xA968                 bra         *-23
1234 //0x0000008B  0x8A4600131065         brclr       #0x10,X:(R2+0x13),*-24
1235 //0x0000008E  0x824600130010         bfset       #0x10,X:(R2+0x13)
1236 //0x00000091  0xA961                 bra         *-30
1237 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};
1238 const uint32_t pgm_write_pflash_length = 31;
1239
1240 int dsp5680xx_f_wr(struct target * target, uint8_t *buffer, uint32_t address, uint32_t count){
1241   int retval = ERROR_OK;
1242   uint16_t* buff16 = (uint16_t *) buffer;
1243   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1244     retval = dsp5680xx_halt(target);
1245         err_check_propagate(retval);
1246   }
1247   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1248   // Download the pgm that flashes.
1249   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1250   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
1251   retval = dsp5680xx_write(target, my_favourite_ram_address, 1, pgm_write_pflash_length*2,(uint8_t *) pgm_write_pflash);
1252   err_check_propagate(retval);
1253   retval = dsp5680xx_execute_queue();
1254   err_check_propagate(retval);
1255   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1256   // Set hfmdiv
1257   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1258   retval = eonce_set_hfmdiv(target);
1259   err_check_propagate(retval);
1260   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1261   // Setup registers needed by pgm_write_pflash
1262   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1263   retval = eonce_move_long_to_r3(target,address);  // Destination address to r3
1264   err_check_propagate(retval);
1265   eonce_load_TX_RX_high_to_r0(target);  // TX/RX reg address to r0
1266   err_check_propagate(retval);
1267   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);// FM base address to r2
1268   err_check_propagate(retval);
1269   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1270   // Run flashing program.
1271   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1272   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_CNFG); // write to HFM_CNFG (lock=0, select bank)
1273   err_check_propagate(retval);
1274   retval = eonce_move_value_at_r2_disp(target,0x04,HFM_USTAT);// write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1275   err_check_propagate(retval);
1276   retval = eonce_move_value_at_r2_disp(target,0x10,HFM_USTAT);// clear only one bit at a time
1277   err_check_propagate(retval);
1278   retval = eonce_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1279   err_check_propagate(retval);
1280   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROT);// write to HMF_PROT, clear protection
1281   err_check_propagate(retval);
1282   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROTB);// write to HMF_PROTB, clear protection
1283   err_check_propagate(retval);
1284   if(count%2){
1285     //TODO implement handling of odd number of words.
1286         retval = ERROR_FAIL;
1287         err_check(retval,"Cannot handle odd number of words.");
1288   }
1289   uint32_t drscan_data;
1290   retval = eonce_tx_upper_data(target,buff16[0],&drscan_data);
1291   err_check_propagate(retval);
1292
1293   retval = dsp5680xx_resume(target,0,my_favourite_ram_address,0,0);
1294   err_check_propagate(retval);
1295
1296   int counter = FLUSH_COUNT_FLASH;
1297   context.flush = 0;
1298   uint32_t i;
1299   for(i=1; (i<count/2)&&(i<HFM_SIZE_WORDS); i++){
1300     if(--counter==0){
1301       context.flush = 1;
1302       counter = FLUSH_COUNT_FLASH;
1303     }
1304     retval = eonce_tx_upper_data(target,buff16[i],&drscan_data);
1305         if(retval!=ERROR_OK){
1306           context.flush = 1;
1307           err_check_propagate(retval);
1308         }
1309         context.flush = 0;
1310   }
1311   context.flush = 1;
1312   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1313   // Verify flash
1314   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1315   uint16_t signature;
1316   uint16_t pc_crc;
1317   retval =  dsp5680xx_f_signature(target,address,i,&signature);
1318   err_check_propagate(retval);
1319   pc_crc = perl_crc(buff16,i);
1320   if(pc_crc != signature){
1321     retval = ERROR_FAIL;
1322     err_check(retval,"Flashed data failed CRC check, flash again!");
1323   }
1324   return retval;
1325 }
1326
1327
1328
1329 int dsp5680xx_f_unlock(struct target * target){
1330   int retval;
1331   if(target->tap->enabled){
1332     //TODO find a way to switch to the master tap here.
1333     LOG_ERROR("Master tap must be enabled to unlock flash.");
1334     return ERROR_TARGET_FAILURE;
1335   }
1336   uint32_t data_to_shift_in = MASTER_TAP_CMD_FLASH_ERASE;
1337   uint32_t data_shifted_out;
1338   retval = dsp5680xx_irscan(target,&data_to_shift_in,&data_shifted_out,8);
1339   err_check_propagate(retval);
1340   data_to_shift_in = HFM_CLK_DEFAULT;
1341   retval = dsp5680xx_drscan(target,((uint8_t *) & data_to_shift_in),((uint8_t *)&data_shifted_out),8);
1342   err_check_propagate(retval);
1343   return retval;
1344 }
1345
1346 int dsp5680xx_f_lock(struct target * target){
1347   int retval;
1348   uint16_t lock_word[] = {HFM_LOCK_FLASH,HFM_LOCK_FLASH};
1349   retval = dsp5680xx_f_wr(target,(uint8_t *)(lock_word),HFM_LOCK_ADDR_L,4);
1350   err_check_propagate(retval);
1351   return retval;
1352 }
1353
1354 static int dsp5680xx_step(struct target * target,int current, uint32_t address, int handle_breakpoints){
1355   err_check(ERROR_FAIL,"Not implemented yet.");
1356 }
1357
1358 /** Holds methods for dsp5680xx targets. */
1359 struct target_type dsp5680xx_target = {
1360   .name = "dsp5680xx",
1361
1362   .poll = dsp5680xx_poll,
1363   .arch_state = dsp5680xx_arch_state,
1364
1365   .target_request_data = NULL,
1366
1367   .halt = dsp5680xx_halt,
1368   .resume = dsp5680xx_resume,
1369   .step = dsp5680xx_step,
1370
1371   .write_buffer = dsp5680xx_write_buffer,
1372   .read_buffer = dsp5680xx_read_buffer,
1373
1374   .assert_reset = dsp5680xx_assert_reset,
1375   .deassert_reset = dsp5680xx_deassert_reset,
1376   .soft_reset_halt = dsp5680xx_soft_reset_halt,
1377
1378   .read_memory = dsp5680xx_read,
1379   .write_memory = dsp5680xx_write,
1380   .bulk_write_memory = dsp5680xx_bulk_write_memory,
1381
1382   .checksum_memory = dsp5680xx_checksum_memory,
1383
1384   .target_create = dsp5680xx_target_create,
1385   .init_target = dsp5680xx_init_target,
1386 };