git.gag.com Git - fw/openocd/blob - src/target/dsp5680xx.c

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