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