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   for (unsigned i=0; i<count; i++){
 718     switch (size){
 719     case 1:
 720       if(!(i%2)){
 721                 retval = dsp5680xx_read_16_single(target, address + i/2, &tmp_wrd, pmem);
 722                 buffer[i] = (uint8_t) (tmp_wrd>>8);
 723                 buffer[i+1] = (uint8_t) (tmp_wrd&0xff);
 724       }
 725       break;
 726     case 2:
 727       retval = dsp5680xx_read_16_single(target, address + i, buff16 + i, pmem);
 728       break;
 729     case 4:
 730       retval = dsp5680xx_read_32_single(target, address + 2*i, buff32 + i, pmem);
 731       break;
 732     default:
 733       LOG_USER("%s: Invalid read size.",__FUNCTION__);
 734       break;
 735     }
 736         err_check_propagate(retval);
 737   }
 738   return retval;
 739 }
 740
 741 //TODO doxy
 742 static int dsp5680xx_write_16_single(struct target *target, uint32_t address, uint16_t data, uint8_t w_pmem){
 743   int retval = 0;
 744   retval = eonce_move_long_to_r0(target,address);
 745   err_check_propagate(retval);
 746   if(w_pmem){
 747     retval = eonce_move_value_to_y0(target,data);
 748         err_check_propagate(retval);
 749     retval = eonce_move_y0_at_pr0_inc(target);
 750         err_check_propagate(retval);
 751   }else{
 752     retval = eonce_move_value_at_r0(target,data);
 753         err_check_propagate(retval);
 754   }
 755   return retval;
 756 }
 757
 758 //TODO doxy
 759 static int dsp5680xx_write_32_single(struct target *target, uint32_t address, uint32_t data, int w_pmem){
 760   int retval = 0;
 761   retval = eonce_move_long_to_r0(target,address);
 762   err_check_propagate(retval);
 763   retval = eonce_move_long_to_y(target,data);
 764   err_check_propagate(retval);
 765   if(w_pmem)
 766     retval = eonce_move_y0_at_pr0_inc(target);
 767   else
 768     retval = eonce_move_y0_at_r0_inc(target);
 769   err_check_propagate(retval);
 770   if(w_pmem)
 771     retval = eonce_move_y1_at_pr0_inc(target);
 772   else
 773     retval = eonce_move_y1_at_r0_inc(target);
 774   err_check_propagate(retval);
 775   return retval;
 776 }
 777
 778 static int dsp5680xx_write_8(struct target * target, uint32_t address, uint32_t count, uint8_t * data, int pmem){
 779   if(target->state != TARGET_HALTED){
 780     LOG_ERROR("%s: Target must be halted.",__FUNCTION__);
 781     return ERROR_OK;
 782   };
 783   int retval = 0;
 784   uint16_t * data_w = (uint16_t *)data;
 785   uint32_t iter;
 786
 787   int counter = FLUSH_COUNT_READ_WRITE;
 788   for(iter = 0; iter<count/2; iter++){
 789     if(--counter==0){
 790       context.flush = 1;
 791       counter = FLUSH_COUNT_READ_WRITE;
 792     }
 793     retval = dsp5680xx_write_16_single(target,address+iter,data_w[iter], pmem);
 794     if(retval != ERROR_OK){
 795       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 796       context.flush = 1;
 797       return retval;
 798     }
 799     context.flush = 0;
 800   }
 801   context.flush = 1;
 802
 803   // Only one byte left, let's not overwrite the other byte (mem is 16bit)
 804   // Need to retrieve the part we do not want to overwrite.
 805   uint16_t data_old;
 806   if((count==1)||(count%2)){
 807     retval = dsp5680xx_read(target,address+iter,1,1,(uint8_t *)&data_old);
 808         err_check_propagate(retval);
 809     if(count==1)
 810       data_old=(((data_old&0xff)<<8)|data[0]);// preserve upper byte
 811     else
 812       data_old=(((data_old&0xff)<<8)|data[2*iter+1]);
 813     retval = dsp5680xx_write_16_single(target,address+iter,data_old, pmem);
 814         err_check_propagate(retval);
 815   }
 816   return retval;
 817 }
 818
 819 static int dsp5680xx_write_16(struct target * target, uint32_t address, uint32_t count, uint16_t * data, int pmem){
 820   int retval = ERROR_OK;
 821   if(target->state != TARGET_HALTED){
 822         retval = ERROR_TARGET_NOT_HALTED;
 823         err_check(retval,"Target must be halted.");
 824   };
 825   uint32_t iter;
 826   int counter = FLUSH_COUNT_READ_WRITE;
 827
 828   for(iter = 0; iter<count; iter++){
 829         if(--counter==0){
 830           context.flush = 1;
 831       counter = FLUSH_COUNT_READ_WRITE;
 832         }
 833     retval = dsp5680xx_write_16_single(target,address+iter,data[iter], pmem);
 834     if(retval != ERROR_OK){
 835       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 836           context.flush = 1;
 837       return retval;
 838     }
 839         context.flush = 0;
 840   }
 841   context.flush = 1;
 842   return retval;
 843 }
 844
 845 static int dsp5680xx_write_32(struct target * target, uint32_t address, uint32_t count, uint32_t * data, int pmem){
 846   int retval = ERROR_OK;
 847   if(target->state != TARGET_HALTED){
 848         retval = ERROR_TARGET_NOT_HALTED;
 849         err_check(retval,"Target must be halted.");
 850   };
 851   uint32_t iter;
 852   int counter = FLUSH_COUNT_READ_WRITE;
 853
 854   for(iter = 0; iter<count; iter++){
 855         if(--counter==0){
 856           context.flush = 1;
 857       counter = FLUSH_COUNT_READ_WRITE;
 858         }
 859     retval = dsp5680xx_write_32_single(target,address+(iter<<1),data[iter], pmem);
 860     if(retval != ERROR_OK){
 861       LOG_ERROR("%s: Could not write to p:0x%04X",__FUNCTION__,address);
 862           context.flush = 1;
 863       return retval;
 864     }
 865         context.flush = 0;
 866   }
 867   context.flush = 1;
 868   return retval;
 869 }
 870
 871 //TODO doxy
 872 static int dsp5680xx_write(struct target *target, uint32_t address, uint32_t size, uint32_t count, const uint8_t * buffer){
 873   //TODO Cannot write 32bit to odd address, will write 0x12345678  as 0x5678 0x0012
 874   if(target->state != TARGET_HALTED){
 875     LOG_USER("Target must be halted.");
 876     return ERROR_OK;
 877   }
 878   int retval = 0;
 879   int p_mem = 1;
 880   retval = dsp5680xx_convert_address(&address, &p_mem);
 881   err_check_propagate(retval);
 882
 883   switch (size){
 884   case 1:
 885     retval = dsp5680xx_write_8(target, address, count,(uint8_t *) buffer, p_mem);
 886     break;
 887   case 2:
 888     retval = dsp5680xx_write_16(target, address, count, (uint16_t *)buffer, p_mem);
 889       break;
 890   case 4:
 891     retval = dsp5680xx_write_32(target, address, count, (uint32_t *)buffer, p_mem);
 892     break;
 893   default:
 894         retval = ERROR_TARGET_DATA_ABORT;
 895         err_check(retval,"Invalid data size.")
 896         break;
 897   }
 898   return retval;
 899 }
 900
 901 static int dsp5680xx_bulk_write_memory(struct target * target,uint32_t address, uint32_t aligned, const uint8_t * buffer){
 902   LOG_ERROR("Not implemented yet.");
 903   return ERROR_FAIL;
 904 }
 905
 906 // Writes to pram at address
 907 // r3 holds the destination address-> p:(r3)
 908 // r2 hold 0xf151 to flash a led (probably cannot see it due to high freq.)
 909 // r0 holds TX/RX address.
 910 //0x00000073  0x8A44FFFE017B         brclr       #1,X:(R0-2),*-2
 911 //0x00000076  0xE700                 nop
 912 //0x00000077  0xF514                 move.w      X:(R0),Y0
 913 //0x00000078  0xE700                 nop
 914 //0x00000079  0x8563                 move.w      Y0,P:(R3)+
 915 //0x0000007A  0x84420003             bfchg       #3,X:(R2)
 916 //0x0000007C  0xA976                 bra         *-9
 917 uint16_t pgm_write_pram[] = {0x8A44,0xFFFE,0x017D,0xE700,0xF514,0xE700,0x8563,0x8442,0x0003,0xA976};
 918 uint16_t pgm_write_pram_length = 10;
 919
 920 static int dsp5680xx_write_buffer(struct target * target, uint32_t address, uint32_t size, const uint8_t * buffer){
 921   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
 922   // this solution works, but it's slow. it flushes USB all the time.
 923   return dsp5680xx_write(target, address, 1, size, buffer);
 924   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
 925 }
 926
 927 static int dsp5680xx_read_buffer(struct target * target, uint32_t address, uint32_t size, uint8_t * buffer){
 928   if(target->state != TARGET_HALTED){
 929     LOG_USER("Target must be halted.");
 930     return ERROR_OK;
 931   }
 932   // byte addressing!
 933   int retval = ERROR_OK;
 934   int pmem = 1;
 935   uint16_t tmp_wrd= 0;
 936
 937   retval = dsp5680xx_convert_address(&address, &pmem);
 938   err_check_propagate(retval);
 939
 940   for (unsigned i=0; i<size; i++)
 941     if(!(i%2)){
 942       retval = dsp5680xx_read_16_single(target, address + i/2, &tmp_wrd, pmem);
 943           err_check_propagate(retval);
 944       //TODO find a better solution. endiannes differs from normal read, otherwise the openocd crc would do weird stuff.
 945       buffer[i+1] = (uint8_t) (tmp_wrd>>8);
 946       buffer[i] = (uint8_t) (tmp_wrd&0xff);
 947    }
 948   return retval;
 949 }
 950
 951 static int dsp5680xx_checksum_memory(struct target * target, uint32_t address, uint32_t size, uint32_t * checksum){
 952  //TODO implement.
 953   //This will make openocd do the work, but it will fail because of the word/byte addressing issues.
 954   int retval;
 955   struct working_area * crc_algorithm;
 956   retval = target_alloc_working_area(target, 20, &crc_algorithm);
 957   if(retval != ERROR_OK)
 958     return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 959   retval = target_free_working_area(target, crc_algorithm);
 960   return ERROR_FAIL;
 961 }
 962
 963 // Data signature algorithm used by the core FM (flash module)
 964 static int perl_crc(uint16_t * buff16,uint32_t  word_count){
 965   uint16_t checksum = 0xffff;
 966   uint16_t data,fbmisr;
 967   uint32_t i;
 968   for(i=0;i<word_count;i++){
 969     data = buff16[i];
 970     fbmisr = (checksum & 2)>>1 ^ (checksum & 4)>>2 ^ (checksum & 16)>>4 ^ (checksum & 0x8000)>>15;
 971     checksum = (data ^ ((checksum << 1) | fbmisr));
 972   }
 973   i--;
 974   for(;!(i&0x80000000);i--){
 975     data = buff16[i];
 976     fbmisr = (checksum & 2)>>1 ^ (checksum & 4)>>2 ^ (checksum & 16)>>4 ^ (checksum & 0x8000)>>15;
 977     checksum = (data ^ ((checksum << 1) | fbmisr));
 978   }
 979   return checksum;
 980 }
 981
 982 int dsp5680xx_f_SIM_reset(struct target * target){
 983   int retval = ERROR_OK;
 984   uint16_t sim_cmd = SIM_CMD_RESET;
 985   uint32_t sim_addr;
 986   if(strcmp(target->tap->chip,"dsp568013")==0){
 987         sim_addr = MC568013_SIM_BASE_ADDR+S_FILE_DATA_OFFSET;
 988         retval = dsp5680xx_write(target,sim_addr,1,2,(const uint8_t *)&sim_cmd);
 989         err_check_propagate(retval);
 990   }
 991   return retval;
 992 }
 993
 994 //TODO doxy
 995 static int dsp5680xx_soft_reset_halt(struct target *target){
 996   //TODO is this what this function is expected to do...?
 997   int retval;
 998   retval = dsp5680xx_halt(target);
 999   err_check_propagate(retval);
1000   retval = dsp5680xx_f_SIM_reset(target);
1001   err_check_propagate(retval);
1002   return retval;
1003 }
1004
1005 int dsp5680xx_f_protect_check(struct target * target, uint16_t * protected) {
1006   uint16_t aux;
1007   int retval;
1008   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1009     retval = dsp5680xx_halt(target);
1010         err_check_propagate(retval);
1011   }
1012   if(protected == NULL){
1013     err_check(ERROR_FAIL,"NULL pointer not valid.");
1014   }
1015   retval = dsp5680xx_read_16_single(target,HFM_BASE_ADDR|HFM_PROT,&aux,0);
1016   err_check_propagate(retval);
1017   *protected = aux;
1018   return retval;
1019 }
1020
1021 static int dsp5680xx_f_execute_command(struct target * target, uint16_t command, uint32_t address, uint32_t data, uint16_t * hfm_ustat, int pmem){
1022   int retval;
1023   retval = eonce_load_TX_RX_high_to_r0(target);
1024   err_check_propagate(retval);
1025   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);
1026   err_check_propagate(retval);
1027   uint16_t i;
1028   int watchdog = 100;
1029   do{
1030     retval = eonce_move_at_r2_disp_to_y0(target,HFM_USTAT);     // read HMF_USTAT
1031         err_check_propagate(retval);
1032     retval = eonce_move_y0_at_r0(target);
1033         err_check_propagate(retval);
1034     retval = eonce_rx_upper_data(target,&i);
1035         err_check_propagate(retval);
1036     if((watchdog--)==1){
1037       retval = ERROR_TARGET_FAILURE;
1038       err_check(retval,"FM execute command failed.");
1039     }
1040   }while (!(i&0x40));                           // wait until current command is complete
1041   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 ???
1042   err_check_propagate(retval);
1043   retval = eonce_move_value_at_r2_disp(target,0x04,HFM_USTAT);          // write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1044   err_check_propagate(retval);
1045   retval = eonce_move_value_at_r2_disp(target,0x10,HFM_USTAT);          // clear only one bit at a time
1046   err_check_propagate(retval);
1047   retval = eonce_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1048   err_check_propagate(retval);
1049   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROT);           // write to HMF_PROT, clear protection
1050   err_check_propagate(retval);
1051   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROTB);          // write to HMF_PROTB, clear protection
1052   err_check_propagate(retval);
1053   retval = eonce_move_value_to_y0(target,data);
1054   err_check_propagate(retval);
1055   retval = eonce_move_long_to_r3(target,address);                       // write to the flash block
1056   err_check_propagate(retval);
1057   if (pmem){
1058     retval = eonce_move_y0_at_pr3_inc(target);
1059         err_check_propagate(retval);
1060   }else{
1061     retval = eonce_move_y0_at_r3(target);
1062         err_check_propagate(retval);
1063   }
1064   retval = eonce_move_value_at_r2_disp(target,command,HFM_CMD); // write command to the HFM_CMD reg
1065   err_check_propagate(retval);
1066   retval = eonce_move_value_at_r2_disp(target,0x80,HFM_USTAT);          // start the command
1067   err_check_propagate(retval);
1068   watchdog = 100;
1069   do{
1070     retval = eonce_move_at_r2_disp_to_y0(target,HFM_USTAT);     // read HMF_USTAT
1071         err_check_propagate(retval);
1072     retval = eonce_move_y0_at_r0(target);
1073         err_check_propagate(retval);
1074         retval = eonce_rx_upper_data(target,&i);
1075         err_check_propagate(retval);
1076     if((watchdog--)==1){
1077           retval = ERROR_TARGET_FAILURE;
1078       err_check(retval,"FM execution did not finish.");
1079     }
1080   }while (!(i&0x40));       // wait until the command is complete
1081   *hfm_ustat = i;
1082   return ERROR_OK;
1083 }
1084
1085 static int eonce_set_hfmdiv(struct target * target){
1086   uint16_t i;
1087   int retval;
1088   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);
1089   err_check_propagate(retval);
1090   retval = eonce_load_TX_RX_high_to_r0(target);
1091   err_check_propagate(retval);
1092   retval = eonce_move_at_r2_to_y0(target);// read HFM_CLKD
1093   err_check_propagate(retval);
1094   retval = eonce_move_y0_at_r0(target);
1095   err_check_propagate(retval);
1096   retval = eonce_rx_upper_data(target,&i);
1097   err_check_propagate(retval);
1098   unsigned int hfm_at_wrong_value = 0;
1099   if ((i&0x7f)!=HFM_CLK_DEFAULT) {
1100     LOG_DEBUG("HFM CLK divisor contained incorrect value (0x%02X).",i&0x7f);
1101     hfm_at_wrong_value = 1;
1102   }else{
1103     LOG_DEBUG("HFM CLK divisor was already set to correct value (0x%02X).",i&0x7f);
1104     return ERROR_OK;
1105   }
1106   retval = eonce_move_value_at_r2(target,HFM_CLK_DEFAULT);      // write HFM_CLKD
1107   err_check_propagate(retval);
1108   retval = eonce_move_at_r2_to_y0(target); // verify HFM_CLKD
1109   err_check_propagate(retval);
1110   retval = eonce_move_y0_at_r0(target);
1111   err_check_propagate(retval);
1112   retval = eonce_rx_upper_data(target,&i);
1113   err_check_propagate(retval);
1114   if (i!=(0x80|(HFM_CLK_DEFAULT&0x7f))) {
1115         retval = ERROR_TARGET_FAILURE;
1116         err_check(retval,"Unable to set HFM CLK divisor.");
1117   }
1118   if(hfm_at_wrong_value)
1119     LOG_DEBUG("HFM CLK divisor set to 0x%02x.",i&0x7f);
1120   return ERROR_OK;
1121 }
1122
1123 static int dsp5680xx_f_signature(struct target * target, uint32_t address, uint32_t words, uint16_t * signature){
1124   int retval;
1125   uint16_t hfm_ustat;
1126   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1127     retval = eonce_enter_debug_mode(target,NULL);
1128     err_check_propagate(retval);
1129   }
1130   retval = dsp5680xx_f_execute_command(target,HFM_CALCULATE_DATA_SIGNATURE,address,words,&hfm_ustat,1);
1131   err_check_propagate(retval);
1132   if (hfm_ustat&HFM_USTAT_MASK_PVIOL_ACCER){
1133     retval = ERROR_TARGET_FAILURE;
1134     err_check(retval,"HFM exec error:pviol and/or accer bits set.");
1135   }
1136   retval = dsp5680xx_read_16_single(target, HFM_BASE_ADDR|HFM_DATA, signature, 0);
1137   return retval;
1138 }
1139
1140 int dsp5680xx_f_erase_check(struct target * target, uint8_t * erased,uint32_t sector){
1141   int retval;
1142   uint16_t hfm_ustat;
1143   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1144     retval = dsp5680xx_halt(target);
1145     err_check_propagate(retval);
1146   }
1147   // Check if chip is already erased.
1148   retval = dsp5680xx_f_execute_command(target,HFM_ERASE_VERIFY,HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2,0,&hfm_ustat,1); // blank check
1149   err_check_propagate(retval);
1150   if (hfm_ustat&HFM_USTAT_MASK_PVIOL_ACCER){
1151         retval = ERROR_TARGET_FAILURE;
1152         err_check(retval,"pviol and/or accer bits set. EraseVerify HFM command execution error");;
1153   }
1154   if(erased!=NULL)
1155     *erased = (uint8_t)(hfm_ustat&HFM_USTAT_MASK_BLANK);
1156   return retval;
1157 }
1158
1159 static int erase_sector(struct target * target, int sector, uint16_t * hfm_ustat){
1160   int retval;
1161   retval = dsp5680xx_f_execute_command(target,HFM_PAGE_ERASE,HFM_FLASH_BASE_ADDR+sector*HFM_SECTOR_SIZE/2,0,hfm_ustat,1);
1162   err_check_propagate(retval);
1163   return retval;
1164 }
1165
1166 static int mass_erase(struct target * target, uint16_t * hfm_ustat){
1167   int retval;
1168   retval = dsp5680xx_f_execute_command(target,HFM_MASS_ERASE,0,0,hfm_ustat,1);
1169   return retval;
1170 }
1171
1172 int dsp5680xx_f_erase(struct target * target, int first, int last){
1173   int retval;
1174   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1175     retval = dsp5680xx_halt(target);
1176     err_check_propagate(retval);
1177   }
1178   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1179   // Reset SIM
1180   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1181   retval = dsp5680xx_f_SIM_reset(target);
1182   err_check_propagate(retval);
1183   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1184   // Set hfmdiv
1185   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1186   retval = eonce_set_hfmdiv(target);
1187   err_check_propagate(retval);
1188
1189   uint16_t hfm_ustat;
1190   int do_mass_erase = ((!(first|last)) || ((first==0)&&(last == (HFM_SECTOR_COUNT-1))));
1191   if(do_mass_erase){
1192     //Mass erase
1193     retval = mass_erase(target,&hfm_ustat);
1194     err_check_propagate(retval);
1195     last = HFM_SECTOR_COUNT-1;
1196   }else{
1197     for(int i = first;i<=last;i++){
1198       retval = erase_sector(target,i,&hfm_ustat);
1199         err_check_propagate(retval);
1200   }
1201   }
1202   return ERROR_OK;
1203 }
1204
1205 // Algorithm for programming normal p: flash
1206 // Follow state machine from "56F801x Peripheral Reference Manual"@163.
1207 // Registers to set up before calling:
1208 //  r0: TX/RX high address.
1209 //  r2: FM module base address.
1210 //  r3: Destination address in flash.
1211 //
1212 //              hfm_wait:                                           // wait for command to finish
1213 //                      brclr   #0x40,x:(r2+0x13),hfm_wait
1214 //              rx_check:                                           // wait for input buffer full
1215 //                      brclr   #0x01,x:(r0-2),rx_check
1216 //                      move.w  x:(r0),y0                           // read from Rx buffer
1217 //                      move.w  y0,p:(r3)+
1218 //                      move.w  #0x20,x:(r2+0x14)                   // write PGM command
1219 //                      move.w  #0x80,x:(r2+0x13)                   // start the command
1220 //                      brclr       #0x20,X:(R2+0x13),accerr_check  // protection violation check
1221 //                      bfset       #0x20,X:(R2+0x13)               // clear pviol
1222 //                      bra         hfm_wait
1223 //              accerr_check:
1224 //                      brclr       #0x10,X:(R2+0x13),hfm_wait      // access error check
1225 //                      bfset       #0x10,X:(R2+0x13)               // clear accerr
1226 //                      bra         hfm_wait                        // loop
1227 //0x00000073  0x8A460013407D         brclr       #0x40,X:(R2+0x13),*+0
1228 //0x00000076  0xE700                 nop
1229 //0x00000077  0xE700                 nop
1230 //0x00000078  0x8A44FFFE017B         brclr       #1,X:(R0-2),*-2
1231 //0x0000007B  0xE700                 nop
1232 //0x0000007C  0xF514                 move.w      X:(R0),Y0
1233 //0x0000007D  0x8563                 move.w      Y0,P:(R3)+
1234 //0x0000007E  0x864600200014         move.w      #0x20,X:(R2+0x14)
1235 //0x00000081  0x864600800013         move.w      #0x80,X:(R2+0x13)
1236 //0x00000084  0x8A4600132004         brclr       #0x20,X:(R2+0x13),*+7
1237 //0x00000087  0x824600130020         bfset       #0x20,X:(R2+0x13)
1238 //0x0000008A  0xA968                 bra         *-23
1239 //0x0000008B  0x8A4600131065         brclr       #0x10,X:(R2+0x13),*-24
1240 //0x0000008E  0x824600130010         bfset       #0x10,X:(R2+0x13)
1241 //0x00000091  0xA961                 bra         *-30
1242 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};
1243 const uint32_t pgm_write_pflash_length = 31;
1244
1245 int dsp5680xx_f_wr(struct target * target, uint8_t *buffer, uint32_t address, uint32_t count){
1246   int retval = ERROR_OK;
1247   uint16_t* buff16 = (uint16_t *) buffer;
1248   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1249     retval = dsp5680xx_halt(target);
1250         err_check_propagate(retval);
1251   }
1252   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1253   // Download the pgm that flashes.
1254   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1255   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
1256   retval = dsp5680xx_write(target, my_favourite_ram_address, 1, pgm_write_pflash_length*2,(uint8_t *) pgm_write_pflash);
1257   err_check_propagate(retval);
1258   retval = dsp5680xx_execute_queue();
1259   err_check_propagate(retval);
1260   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1261   // Set hfmdiv
1262   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1263   retval = eonce_set_hfmdiv(target);
1264   err_check_propagate(retval);
1265   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1266   // Setup registers needed by pgm_write_pflash
1267   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1268   retval = eonce_move_long_to_r3(target,address);  // Destination address to r3
1269   err_check_propagate(retval);
1270   eonce_load_TX_RX_high_to_r0(target);  // TX/RX reg address to r0
1271   err_check_propagate(retval);
1272   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);// FM base address to r2
1273   err_check_propagate(retval);
1274   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1275   // Run flashing program.
1276   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1277   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_CNFG); // write to HFM_CNFG (lock=0, select bank)
1278   err_check_propagate(retval);
1279   retval = eonce_move_value_at_r2_disp(target,0x04,HFM_USTAT);// write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1280   err_check_propagate(retval);
1281   retval = eonce_move_value_at_r2_disp(target,0x10,HFM_USTAT);// clear only one bit at a time
1282   err_check_propagate(retval);
1283   retval = eonce_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1284   err_check_propagate(retval);
1285   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROT);// write to HMF_PROT, clear protection
1286   err_check_propagate(retval);
1287   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROTB);// write to HMF_PROTB, clear protection
1288   err_check_propagate(retval);
1289   if(count%2){
1290     //TODO implement handling of odd number of words.
1291         retval = ERROR_FAIL;
1292         err_check(retval,"Cannot handle odd number of words.");
1293   }
1294   uint32_t drscan_data;
1295   retval = eonce_tx_upper_data(target,buff16[0],&drscan_data);
1296   err_check_propagate(retval);
1297
1298   retval = dsp5680xx_resume(target,0,my_favourite_ram_address,0,0);
1299   err_check_propagate(retval);
1300
1301   int counter = FLUSH_COUNT_FLASH;
1302   context.flush = 0;
1303   uint32_t i;
1304   for(i=1; (i<count/2)&&(i<HFM_SIZE_WORDS); i++){
1305     if(--counter==0){
1306       context.flush = 1;
1307       counter = FLUSH_COUNT_FLASH;
1308     }
1309     retval = eonce_tx_upper_data(target,buff16[i],&drscan_data);
1310         if(retval!=ERROR_OK){
1311           context.flush = 1;
1312           err_check_propagate(retval);
1313         }
1314         context.flush = 0;
1315   }
1316   context.flush = 1;
1317   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1318   // Verify flash
1319   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1320   uint16_t signature;
1321   uint16_t pc_crc;
1322   retval =  dsp5680xx_f_signature(target,address,i,&signature);
1323   err_check_propagate(retval);
1324   pc_crc = perl_crc(buff16,i);
1325   if(pc_crc != signature){
1326     retval = ERROR_FAIL;
1327     err_check(retval,"Flashed data failed CRC check, flash again!");
1328   }
1329   return retval;
1330 }
1331
1332
1333
1334 int dsp5680xx_f_unlock(struct target * target){
1335   int retval;
1336   if(target->tap->enabled){
1337     //TODO find a way to switch to the master tap here.
1338     LOG_ERROR("Master tap must be enabled to unlock flash.");
1339     return ERROR_TARGET_FAILURE;
1340   }
1341   uint32_t data_to_shift_in = MASTER_TAP_CMD_FLASH_ERASE;
1342   uint32_t data_shifted_out;
1343   retval = dsp5680xx_irscan(target,&data_to_shift_in,&data_shifted_out,8);
1344   err_check_propagate(retval);
1345   data_to_shift_in = HFM_CLK_DEFAULT;
1346   retval = dsp5680xx_drscan(target,((uint8_t *) & data_to_shift_in),((uint8_t *)&data_shifted_out),8);
1347   err_check_propagate(retval);
1348   return retval;
1349 }
1350
1351 int dsp5680xx_f_lock(struct target * target){
1352   int retval;
1353   uint16_t lock_word[] = {HFM_LOCK_FLASH,HFM_LOCK_FLASH};
1354   retval = dsp5680xx_f_wr(target,(uint8_t *)(lock_word),HFM_LOCK_ADDR_L,4);
1355   err_check_propagate(retval);
1356   return retval;
1357 }
1358
1359 static int dsp5680xx_step(struct target * target,int current, uint32_t address, int handle_breakpoints){
1360   err_check(ERROR_FAIL,"Not implemented yet.");
1361 }
1362
1363 /** Holds methods for dsp5680xx targets. */
1364 struct target_type dsp5680xx_target = {
1365   .name = "dsp5680xx",
1366
1367   .poll = dsp5680xx_poll,
1368   .arch_state = dsp5680xx_arch_state,
1369
1370   .target_request_data = NULL,
1371
1372   .halt = dsp5680xx_halt,
1373   .resume = dsp5680xx_resume,
1374   .step = dsp5680xx_step,
1375
1376   .write_buffer = dsp5680xx_write_buffer,
1377   .read_buffer = dsp5680xx_read_buffer,
1378
1379   .assert_reset = dsp5680xx_assert_reset,
1380   .deassert_reset = dsp5680xx_deassert_reset,
1381   .soft_reset_halt = dsp5680xx_soft_reset_halt,
1382
1383   .read_memory = dsp5680xx_read,
1384   .write_memory = dsp5680xx_write,
1385   .bulk_write_memory = dsp5680xx_bulk_write_memory,
1386
1387   .checksum_memory = dsp5680xx_checksum_memory,
1388
1389   .target_create = dsp5680xx_target_create,
1390   .init_target = dsp5680xx_init_target,
1391 };