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 = (((*data_read)<<16) | tmp);
 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   return ERROR_FAIL; //this makes openocd do the crc
 977 }
 978
 979 int dsp5680xx_f_SIM_reset(struct target * target){
 980   int retval = ERROR_OK;
 981   uint16_t sim_cmd = SIM_CMD_RESET;
 982   uint32_t sim_addr;
 983   if(strcmp(target->tap->chip,"dsp568013")==0){
 984         sim_addr = MC568013_SIM_BASE_ADDR+S_FILE_DATA_OFFSET;
 985         retval = dsp5680xx_write(target,sim_addr,1,2,(const uint8_t *)&sim_cmd);
 986         err_check_propagate(retval);
 987   }
 988   return retval;
 989 }
 990
 991 //TODO doxy
 992 static int dsp5680xx_soft_reset_halt(struct target *target){
 993   //TODO is this what this function is expected to do...?
 994   int retval;
 995   retval = dsp5680xx_halt(target);
 996   err_check_propagate(retval);
 997   retval = dsp5680xx_f_SIM_reset(target);
 998   err_check_propagate(retval);
 999   return retval;
1000 }
1001
1002 int dsp5680xx_f_protect_check(struct target * target, uint8_t * protected) {
1003   uint16_t i,j;
1004   int retval;
1005   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1006     retval = dsp5680xx_halt(target);
1007         err_check_propagate(retval);
1008   }
1009   retval = eonce_load_TX_RX_high_to_r0(target);
1010   err_check_propagate(retval);
1011   retval = eonce_move_value_to_y0(target,0x1234);
1012   err_check_propagate(retval);
1013   retval = eonce_move_y0_at_r0(target);
1014   err_check_propagate(retval);
1015   retval = eonce_rx_upper_data(target,&i);
1016   err_check_propagate(retval);
1017   retval = eonce_move_value_to_y0(target,0x4321);
1018   err_check_propagate(retval);
1019   retval = eonce_move_y0_at_r0(target);
1020   err_check_propagate(retval);
1021   retval = eonce_rx_upper_data(target,&j);
1022   err_check_propagate(retval);
1023   if(protected!=NULL)
1024     *protected = (uint8_t) ((i!=0x1234)||(j!=0x4321));
1025   return retval;
1026 }
1027
1028 static int dsp5680xx_f_execute_command(struct target * target, uint16_t command, uint32_t address, uint16_t * hfm_ustat, int pmem){
1029   int retval;
1030   retval = eonce_load_TX_RX_high_to_r0(target);
1031   err_check_propagate(retval);
1032   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);
1033   err_check_propagate(retval);
1034   uint16_t i;
1035   int watchdog = 100;
1036   do{
1037     retval = eonce_move_at_r2_disp_to_y0(target,HFM_USTAT);     // read HMF_USTAT
1038         err_check_propagate(retval);
1039     retval = eonce_move_y0_at_r0(target);
1040         err_check_propagate(retval);
1041     retval = eonce_rx_upper_data(target,&i);
1042         err_check_propagate(retval);
1043     if((watchdog--)==1){
1044       retval = ERROR_TARGET_FAILURE;
1045       err_check(retval,"FM execute command failed.");
1046     }
1047   }while (!(i&0x40));                           // wait until current command is complete
1048   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 ???
1049   err_check_propagate(retval);
1050   retval = eonce_move_value_at_r2_disp(target,0x04,HFM_USTAT);          // write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1051   err_check_propagate(retval);
1052   retval = eonce_move_value_at_r2_disp(target,0x10,HFM_USTAT);          // clear only one bit at a time
1053   err_check_propagate(retval);
1054   retval = eonce_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1055   err_check_propagate(retval);
1056   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROT);           // write to HMF_PROT, clear protection
1057   err_check_propagate(retval);
1058   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROTB);          // write to HMF_PROTB, clear protection
1059   err_check_propagate(retval);
1060   retval = eonce_move_long_to_r3(target,address);                       // write to the flash block
1061   err_check_propagate(retval);
1062   if (pmem){
1063     retval = eonce_move_y0_at_pr3_inc(target);
1064         err_check_propagate(retval);
1065   }else{
1066     retval = eonce_move_y0_at_r3(target);
1067         err_check_propagate(retval);
1068   }
1069   retval = eonce_move_value_at_r2_disp(target,command,HFM_CMD); // write command to the HFM_CMD reg
1070   err_check_propagate(retval);
1071   retval = eonce_move_value_at_r2_disp(target,0x80,HFM_USTAT);          // start the command
1072   err_check_propagate(retval);
1073   watchdog = 100;
1074   do{
1075     retval = eonce_move_at_r2_disp_to_y0(target,HFM_USTAT);     // read HMF_USTAT
1076         err_check_propagate(retval);
1077     retval = eonce_move_y0_at_r0(target);
1078         err_check_propagate(retval);
1079         retval = eonce_rx_upper_data(target,&i);
1080         err_check_propagate(retval);
1081     if((watchdog--)==1){
1082           retval = ERROR_TARGET_FAILURE;
1083       err_check(retval,"FM execution did not finish.");
1084     }
1085   }while (!(i&0x40));       // wait until the command is complete
1086   *hfm_ustat = i;
1087   return ERROR_OK;
1088 }
1089
1090 static int eonce_set_hfmdiv(struct target * target){
1091   uint16_t i;
1092   int retval;
1093   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);
1094   err_check_propagate(retval);
1095   retval = eonce_load_TX_RX_high_to_r0(target);
1096   err_check_propagate(retval);
1097   retval = eonce_move_at_r2_to_y0(target);// read HFM_CLKD
1098   err_check_propagate(retval);
1099   retval = eonce_move_y0_at_r0(target);
1100   err_check_propagate(retval);
1101   retval = eonce_rx_upper_data(target,&i);
1102   err_check_propagate(retval);
1103   unsigned int hfm_at_wrong_value = 0;
1104   if ((i&0x7f)!=HFM_CLK_DEFAULT) {
1105     LOG_DEBUG("HFM CLK divisor contained incorrect value (0x%02X).",i&0x7f);
1106     hfm_at_wrong_value = 1;
1107   }else{
1108     LOG_DEBUG("HFM CLK divisor was already set to correct value (0x%02X).",i&0x7f);
1109     return ERROR_OK;
1110   }
1111   retval = eonce_move_value_at_r2(target,HFM_CLK_DEFAULT);      // write HFM_CLKD
1112   err_check_propagate(retval);
1113   retval = eonce_move_at_r2_to_y0(target); // verify HFM_CLKD
1114   err_check_propagate(retval);
1115   retval = eonce_move_y0_at_r0(target);
1116   err_check_propagate(retval);
1117   retval = eonce_rx_upper_data(target,&i);
1118   err_check_propagate(retval);
1119   if (i!=(0x80|(HFM_CLK_DEFAULT&0x7f))) {
1120         retval = ERROR_TARGET_FAILURE;
1121         err_check(retval,"Unable to set HFM CLK divisor.");
1122   }
1123   if(hfm_at_wrong_value)
1124     LOG_DEBUG("HFM CLK divisor set to 0x%02x.",i&0x7f);
1125   return ERROR_OK;
1126 }
1127
1128 int dsp5680xx_f_erase_check(struct target * target, uint8_t * erased){
1129   int retval;
1130   uint16_t hfm_ustat;
1131   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1132     retval = dsp5680xx_halt(target);
1133     err_check_propagate(retval);
1134   }
1135   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1136   // Check security
1137   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1138   uint8_t protected;
1139   retval = dsp5680xx_f_protect_check(target,&protected);
1140   err_check_propagate(retval);
1141   if(protected){
1142         retval = ERROR_TARGET_FAILURE;
1143         err_check(retval,"Failed to erase, flash is still protected.");
1144   }
1145   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1146   // Set hfmdiv
1147   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1148   retval = eonce_set_hfmdiv(target);
1149   err_check_propagate(retval);
1150
1151   // Check if chip is already erased.
1152   // Since only mass erase is currently implemented, only the first sector is checked (assuming no code will leave it unused)
1153   retval = dsp5680xx_f_execute_command(target,HFM_ERASE_VERIFY,HFM_FLASH_BASE_ADDR+0*HFM_SECTOR_SIZE,&hfm_ustat,1); // blank check
1154   err_check_propagate(retval);
1155   if (hfm_ustat&HFM_USTAT_MASK_PVIOL_ACCER){
1156         retval = ERROR_TARGET_FAILURE;
1157         err_check(retval,"pviol and/or accer bits set. EraseVerify HFM command execution error");;
1158   }
1159   if(erased!=NULL)
1160     *erased = (uint8_t)(hfm_ustat&HFM_USTAT_MASK_BLANK);
1161   return retval;
1162 }
1163
1164 int dsp5680xx_f_erase(struct target * target, int first, int last){
1165   //TODO implement erasing individual sectors.
1166   int retval;
1167   if(first||last){
1168         retval = ERROR_FAIL;
1169         err_check(retval,"Sector erasing not implemented. Call with first=last=0.");
1170   }
1171   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1172     retval = dsp5680xx_halt(target);
1173         err_check_propagate(retval);
1174   }
1175   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1176   // Reset SIM
1177   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1178   retval = dsp5680xx_f_SIM_reset(target);
1179   err_check_propagate(retval);
1180   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1181   // Check security
1182   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1183   uint8_t protected;
1184   retval = dsp5680xx_f_protect_check(target,&protected);
1185   err_check_propagate(retval);
1186   if(protected){
1187         retval = ERROR_TARGET_FAILURE;
1188         err_check(retval,"Cannot flash, security is still enabled.");
1189   }
1190   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1191   // Set hfmdiv
1192   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1193   retval = eonce_set_hfmdiv(target);
1194   err_check_propagate(retval);
1195
1196   // Check if chip is already erased.
1197   // Since only mass erase is currently implemented, only the first sector is checked (assuming no code will leave it unused)
1198   uint8_t erased;
1199   retval = dsp5680xx_f_erase_check(target,&erased);
1200   err_check_propagate(retval);
1201   if (erased)
1202     LOG_USER("Flash blank - mass erase skipped.");
1203   else{
1204     // Execute mass erase command.
1205         uint16_t hfm_ustat;
1206         uint16_t hfm_cmd = HFM_MASS_ERASE;
1207     retval = dsp5680xx_f_execute_command(target,hfm_cmd,HFM_FLASH_BASE_ADDR+0*HFM_SECTOR_SIZE,&hfm_ustat,1);
1208         err_check_propagate(retval);
1209     if (hfm_ustat&HFM_USTAT_MASK_PVIOL_ACCER){
1210           retval = ERROR_TARGET_FAILURE;
1211           err_check(retval,"pviol and/or accer bits set. HFM command execution error");
1212     }
1213     // Verify flash was successfully erased.
1214     retval = dsp5680xx_f_erase_check(target,&erased);
1215         err_check_propagate(retval);
1216         if(retval == ERROR_OK){
1217       if (erased)
1218                 LOG_USER("Flash mass erased and checked blank.");
1219       else
1220                 LOG_WARNING("Flash mass erased, but still not blank!");
1221     }
1222   }
1223   return retval;
1224 }
1225
1226 // Algorithm for programming normal p: flash
1227 // Follow state machine from "56F801x Peripheral Reference Manual"@163.
1228 // Registers to set up before calling:
1229 //  r0: TX/RX high address.
1230 //  r2: FM module base address.
1231 //  r3: Destination address in flash.
1232 //
1233 //              hfm_wait:                                           // wait for command to finish
1234 //                      brclr   #0x40,x:(r2+0x13),hfm_wait
1235 //              rx_check:                                           // wait for input buffer full
1236 //                      brclr   #0x01,x:(r0-2),rx_check
1237 //                      move.w  x:(r0),y0                           // read from Rx buffer
1238 //                      move.w  y0,p:(r3)+
1239 //                      move.w  #0x20,x:(r2+0x14)                   // write PGM command
1240 //                      move.w  #0x80,x:(r2+0x13)                   // start the command
1241 //                      brclr       #0x20,X:(R2+0x13),accerr_check  // protection violation check
1242 //                      bfset       #0x20,X:(R2+0x13)               // clear pviol
1243 //                      bra         hfm_wait
1244 //              accerr_check:
1245 //                      brclr       #0x10,X:(R2+0x13),hfm_wait      // access error check
1246 //                      bfset       #0x10,X:(R2+0x13)               // clear accerr
1247 //                      bra         hfm_wait                        // loop
1248 //0x00000073  0x8A460013407D         brclr       #0x40,X:(R2+0x13),*+0
1249 //0x00000076  0xE700                 nop
1250 //0x00000077  0xE700                 nop
1251 //0x00000078  0x8A44FFFE017B         brclr       #1,X:(R0-2),*-2
1252 //0x0000007B  0xE700                 nop
1253 //0x0000007C  0xF514                 move.w      X:(R0),Y0
1254 //0x0000007D  0x8563                 move.w      Y0,P:(R3)+
1255 //0x0000007E  0x864600200014         move.w      #0x20,X:(R2+0x14)
1256 //0x00000081  0x864600800013         move.w      #0x80,X:(R2+0x13)
1257 //0x00000084  0x8A4600132004         brclr       #0x20,X:(R2+0x13),*+7
1258 //0x00000087  0x824600130020         bfset       #0x20,X:(R2+0x13)
1259 //0x0000008A  0xA968                 bra         *-23
1260 //0x0000008B  0x8A4600131065         brclr       #0x10,X:(R2+0x13),*-24
1261 //0x0000008E  0x824600130010         bfset       #0x10,X:(R2+0x13)
1262 //0x00000091  0xA961                 bra         *-30
1263 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};
1264 const uint32_t pgm_write_pflash_length = 31;
1265
1266 int dsp5680xx_f_wr(struct target * target, uint8_t *buffer, uint32_t address, uint32_t count){
1267   int retval = ERROR_OK;
1268   uint16_t* buff16 = (uint16_t *) buffer;
1269   if (dsp5680xx_target_status(target,NULL,NULL) != TARGET_HALTED){
1270     retval = dsp5680xx_halt(target);
1271         err_check_propagate(retval);
1272   }
1273   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1274   // Check if flash is erased
1275   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1276   uint8_t erased;
1277   retval = dsp5680xx_f_erase_check(target,&erased);
1278   err_check_propagate(retval);
1279   if(!erased){
1280         retval = ERROR_FAIL;
1281         err_check(retval,"Flash must be erased before flashing.");
1282   }
1283   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1284   // Download the pgm that flashes.
1285   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1286   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
1287   retval = dsp5680xx_write(target, my_favourite_ram_address, 1, pgm_write_pflash_length*2,(uint8_t *) pgm_write_pflash);
1288   err_check_propagate(retval);
1289   retval = dsp5680xx_execute_queue();
1290   err_check_propagate(retval);
1291   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1292   // Set hfmdiv
1293   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1294   retval = eonce_set_hfmdiv(target);
1295   err_check_propagate(retval);
1296   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1297   // Setup registers needed by pgm_write_pflash
1298   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1299   retval = eonce_move_long_to_r3(target,address);  // Destination address to r3
1300   err_check_propagate(retval);
1301   eonce_load_TX_RX_high_to_r0(target);  // TX/RX reg address to r0
1302   err_check_propagate(retval);
1303   retval = eonce_move_long_to_r2(target,HFM_BASE_ADDR);// FM base address to r2
1304   err_check_propagate(retval);
1305   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1306   // Run flashing program.
1307   // -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
1308   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_CNFG); // write to HFM_CNFG (lock=0, select bank)
1309   err_check_propagate(retval);
1310   retval = eonce_move_value_at_r2_disp(target,0x04,HFM_USTAT);// write to HMF_USTAT, clear PVIOL, ACCERR & BLANK bits
1311   err_check_propagate(retval);
1312   retval = eonce_move_value_at_r2_disp(target,0x10,HFM_USTAT);// clear only one bit at a time
1313   err_check_propagate(retval);
1314   retval = eonce_move_value_at_r2_disp(target,0x20,HFM_USTAT);
1315   err_check_propagate(retval);
1316   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROT);// write to HMF_PROT, clear protection
1317   err_check_propagate(retval);
1318   retval = eonce_move_value_at_r2_disp(target,0x00,HFM_PROTB);// write to HMF_PROTB, clear protection
1319   err_check_propagate(retval);
1320   if(count%2){
1321     //TODO implement handling of odd number of words.
1322         retval = ERROR_FAIL;
1323         err_check(retval,"Cannot handle odd number of words.");
1324   }
1325   uint32_t drscan_data;
1326   retval = eonce_tx_upper_data(target,buff16[0],&drscan_data);
1327   err_check_propagate(retval);
1328
1329   retval = dsp5680xx_resume(target,0,my_favourite_ram_address,0,0);
1330   err_check_propagate(retval);
1331
1332   int counter_reset = FLUSH_COUNT_FLASH;
1333   int counter = counter_reset;
1334   context.flush = 0;
1335   for(uint32_t i=1; (i<count/2)&&(i<HFM_SIZE_REAL); i++){
1336         if(--counter==0){
1337           context.flush = 1;
1338           counter = counter_reset;
1339         }
1340     retval = eonce_tx_upper_data(target,buff16[i],&drscan_data);
1341         if(retval!=ERROR_OK){
1342           context.flush = 1;
1343           err_check_propagate(retval);
1344         }
1345         context.flush = 0;
1346   }
1347   context.flush = 1;
1348   return retval;
1349 }
1350
1351 int dsp5680xx_f_unlock(struct target * target){
1352   int retval;
1353   if(target->tap->enabled){
1354     //TODO find a way to switch to the master tap here.
1355     LOG_ERROR("Master tap must be enabled to unlock flash.");
1356     return ERROR_TARGET_FAILURE;
1357   }
1358   uint32_t data_to_shift_in = MASTER_TAP_CMD_FLASH_ERASE;
1359   uint32_t data_shifted_out;
1360   retval = dsp5680xx_irscan(target,&data_to_shift_in,&data_shifted_out,8);
1361   err_check_propagate(retval);
1362   data_to_shift_in = HFM_CLK_DEFAULT;
1363   retval = dsp5680xx_drscan(target,((uint8_t *) & data_to_shift_in),((uint8_t *)&data_shifted_out),8);
1364   err_check_propagate(retval);
1365   return retval;
1366 }
1367
1368 int dsp5680xx_f_lock(struct target * target){
1369   int retval;
1370   uint16_t lock_word[] = {HFM_LOCK_FLASH,HFM_LOCK_FLASH};
1371   retval = dsp5680xx_f_wr(target,(uint8_t *)(lock_word),HFM_LOCK_ADDR_L,4);
1372   err_check_propagate(retval);
1373   return retval;
1374 }
1375
1376 static int dsp5680xx_step(struct target * target,int current, uint32_t address, int handle_breakpoints){
1377   err_check(ERROR_FAIL,"Not implemented yet.");
1378 }
1379
1380 /** Holds methods for dsp5680xx targets. */
1381 struct target_type dsp5680xx_target = {
1382   .name = "dsp5680xx",
1383
1384   .poll = dsp5680xx_poll,
1385   .arch_state = dsp5680xx_arch_state,
1386
1387   .target_request_data = NULL,
1388
1389   .halt = dsp5680xx_halt,
1390   .resume = dsp5680xx_resume,
1391   .step = dsp5680xx_step,
1392
1393   .write_buffer = dsp5680xx_write_buffer,
1394   .read_buffer = dsp5680xx_read_buffer,
1395
1396   .assert_reset = dsp5680xx_assert_reset,
1397   .deassert_reset = dsp5680xx_deassert_reset,
1398   .soft_reset_halt = dsp5680xx_soft_reset_halt,
1399
1400   .read_memory = dsp5680xx_read,
1401   .write_memory = dsp5680xx_write,
1402   .bulk_write_memory = dsp5680xx_bulk_write_memory,
1403
1404   .checksum_memory = dsp5680xx_checksum_memory,
1405
1406   .target_create = dsp5680xx_target_create,
1407   .init_target = dsp5680xx_init_target,
1408 };