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