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

   1 /***************************************************************************
   2  *   Copyright (C) 2008 by Spencer Oliver                                  *
   3  *   spen@spen-soft.co.uk                                                  *
   4  *                                                                         *
   5  *   Copyright (C) 2008 by David T.L. Wong                                 *
   6  *                                                                         *
   7  *   Copyright (C) 2009 by David N. Claffey <dnclaffey@gmail.com>          *
   8  *                                                                         *
   9  *   Copyright (C) 2011 by Drasko DRASKOVIC                                *
  10  *   drasko.draskovic@gmail.com                                            *
  11  *                                                                         *
  12  *   This program is free software; you can redistribute it and/or modify  *
  13  *   it under the terms of the GNU General Public License as published by  *
  14  *   the Free Software Foundation; either version 2 of the License, or     *
  15  *   (at your option) any later version.                                   *
  16  *                                                                         *
  17  *   This program is distributed in the hope that it will be useful,       *
  18  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  19  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  20  *   GNU General Public License for more details.                          *
  21  *                                                                         *
  22  *   You should have received a copy of the GNU General Public License     *
  23  *   along with this program; if not, write to the                         *
  24  *   Free Software Foundation, Inc.,                                       *
  25  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  26  ***************************************************************************/
  27
  28 /*
  29 This version has optimized assembly routines for 32 bit operations:
  30 - read word
  31 - write word
  32 - write array of words
  33
  34 One thing to be aware of is that the MIPS32 cpu will execute the
  35 instruction after a branch instruction (one delay slot).
  36
  37 For example:
  38
  39     LW $2, ($5 +10)
  40     B foo
  41     LW $1, ($2 +100)
  42
  43 The LW $1, ($2 +100) instruction is also executed. If this is
  44 not wanted a NOP can be inserted:
  45
  46     LW $2, ($5 +10)
  47     B foo
  48     NOP
  49     LW $1, ($2 +100)
  50
  51 or the code can be changed to:
  52
  53     B foo
  54     LW $2, ($5 +10)
  55     LW $1, ($2 +100)
  56
  57 The original code contained NOPs. I have removed these and moved
  58 the branches.
  59
  60 I also moved the PRACC_STACK to 0xFF204000. This allows
  61 the use of 16 bits offsets to get pointers to the input
  62 and output area relative to the stack. Note that the stack
  63 isn't really a stack (the stack pointer is not 'moving')
  64 but a FIFO simulated in software.
  65
  66 These changes result in a 35% speed increase when programming an
  67 external flash.
  68
  69 More improvement could be gained if the registers do no need
  70 to be preserved but in that case the routines should be aware
  71 OpenOCD is used as a flash programmer or as a debug tool.
  72
  73 Nico Coesel
  74 */
  75
  76 #ifdef HAVE_CONFIG_H
  77 #include "config.h"
  78 #endif
  79
  80 #include <helper/time_support.h>
  81
  82 #include "mips32.h"
  83 #include "mips32_pracc.h"
  84
  85 struct mips32_pracc_context
  86 {
  87         uint32_t *local_iparam;
  88         int num_iparam;
  89         uint32_t *local_oparam;
  90         int num_oparam;
  91         const uint32_t *code;
  92         int code_len;
  93         uint32_t stack[32];
  94         int stack_offset;
  95         struct mips_ejtag *ejtag_info;
  96 };
  97
  98 static int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info,
  99                 uint32_t addr, int count, uint8_t *buf);
 100 static int mips32_pracc_read_mem16(struct mips_ejtag *ejtag_info,
 101                 uint32_t addr, int count, uint16_t *buf);
 102 static int mips32_pracc_read_mem32(struct mips_ejtag *ejtag_info,
 103                 uint32_t addr, int count, uint32_t *buf);
 104 static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info,
 105                 uint32_t addr, uint32_t *buf);
 106
 107 static int mips32_pracc_write_mem8(struct mips_ejtag *ejtag_info,
 108                 uint32_t addr, int count, uint8_t *buf);
 109 static int mips32_pracc_write_mem16(struct mips_ejtag *ejtag_info,
 110                 uint32_t addr, int count, uint16_t *buf);
 111 static int mips32_pracc_write_mem32(struct mips_ejtag *ejtag_info,
 112                 uint32_t addr, int count, uint32_t *buf);
 113 static int mips32_pracc_write_u32(struct mips_ejtag *ejtag_info,
 114                 uint32_t addr, uint32_t *buf);
 115
 116 static int mips32_pracc_sync_cache(struct mips_ejtag *ejtag_info,
 117                 uint32_t start_addr, uint32_t end_addr);
 118 static int mips32_pracc_clean_invalidate_cache(struct mips_ejtag *ejtag_info,
 119                                                                                                         uint32_t start_addr, uint32_t end_addr);
 120
 121 static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
 122 {
 123         uint32_t ejtag_ctrl;
 124         long long then = timeval_ms();
 125         int timeout;
 126         int retval;
 127
 128         /* wait for the PrAcc to become "1" */
 129         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
 130         ejtag_ctrl = ejtag_info->ejtag_ctrl;
 131
 132         while (1)
 133         {
 134                 retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
 135                 if (retval != ERROR_OK)
 136                         return retval;
 137
 138                 if (ejtag_ctrl & EJTAG_CTRL_PRACC)
 139                         break;
 140
 141                 if ( (timeout = timeval_ms()-then) > 1000 )
 142                 {
 143                         LOG_DEBUG("DEBUGMODULE: No memory access in progress!");
 144                         return ERROR_JTAG_DEVICE_ERROR;
 145                 }
 146         }
 147
 148         *ctrl = ejtag_ctrl;
 149         return ERROR_OK;
 150 }
 151
 152 static int mips32_pracc_exec_read(struct mips32_pracc_context *ctx, uint32_t address)
 153 {
 154         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
 155         int offset;
 156         uint32_t ejtag_ctrl, data;
 157
 158         if ((address >= MIPS32_PRACC_PARAM_IN)
 159                 && (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
 160         {
 161                 offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
 162                 data = ctx->local_iparam[offset];
 163         }
 164         else if ((address >= MIPS32_PRACC_PARAM_OUT)
 165                 && (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
 166         {
 167                 offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
 168                 data = ctx->local_oparam[offset];
 169         }
 170         else if ((address >= MIPS32_PRACC_TEXT)
 171                 && (address <= MIPS32_PRACC_TEXT + ctx->code_len * 4))
 172         {
 173                 offset = (address - MIPS32_PRACC_TEXT) / 4;
 174                 data = ctx->code[offset];
 175         }
 176         else if (address == MIPS32_PRACC_STACK)
 177         {
 178                 /* save to our debug stack */
 179                 data = ctx->stack[--ctx->stack_offset];
 180         }
 181         else
 182         {
 183                 /* TODO: send JMP 0xFF200000 instruction. Hopefully processor jump back
 184                  * to start of debug vector */
 185
 186                 LOG_ERROR("Error reading unexpected address 0x%8.8" PRIx32 "", address);
 187                 return ERROR_JTAG_DEVICE_ERROR;
 188         }
 189
 190         /* Send the data out */
 191         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA);
 192         mips_ejtag_drscan_32_out(ctx->ejtag_info, data);
 193
 194         /* Clear the access pending bit (let the processor eat!) */
 195         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 196         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL);
 197         mips_ejtag_drscan_32_out(ctx->ejtag_info, ejtag_ctrl);
 198
 199         return jtag_execute_queue();
 200 }
 201
 202 static int mips32_pracc_exec_write(struct mips32_pracc_context *ctx, uint32_t address)
 203 {
 204         uint32_t ejtag_ctrl,data;
 205         int offset;
 206         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
 207         int retval;
 208
 209         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA);
 210         retval = mips_ejtag_drscan_32(ctx->ejtag_info, &data);
 211         if (retval != ERROR_OK)
 212                 return retval;
 213
 214         /* Clear access pending bit */
 215         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 216         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL);
 217         mips_ejtag_drscan_32_out(ctx->ejtag_info, ejtag_ctrl);
 218
 219         retval = jtag_execute_queue();
 220         if (retval != ERROR_OK)
 221                 return retval;
 222
 223         if ((address >= MIPS32_PRACC_PARAM_IN)
 224                 && (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
 225         {
 226                 offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
 227                 ctx->local_iparam[offset] = data;
 228         }
 229         else if ((address >= MIPS32_PRACC_PARAM_OUT)
 230                 && (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
 231         {
 232                 offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
 233                 ctx->local_oparam[offset] = data;
 234         }
 235         else if (address == MIPS32_PRACC_STACK)
 236         {
 237                 /* save data onto our stack */
 238                 ctx->stack[ctx->stack_offset++] = data;
 239         }
 240         else
 241         {
 242                 LOG_ERROR("Error writing unexpected address 0x%8.8" PRIx32 "", address);
 243                 return ERROR_JTAG_DEVICE_ERROR;
 244         }
 245
 246         return ERROR_OK;
 247 }
 248
 249 int mips32_pracc_exec(struct mips_ejtag *ejtag_info, int code_len, const uint32_t *code,
 250                 int num_param_in, uint32_t *param_in, int num_param_out, uint32_t *param_out, int cycle)
 251 {
 252         uint32_t ejtag_ctrl;
 253         uint32_t address, data;
 254         struct mips32_pracc_context ctx;
 255         int retval;
 256         int pass = 0;
 257
 258         ctx.local_iparam = param_in;
 259         ctx.local_oparam = param_out;
 260         ctx.num_iparam = num_param_in;
 261         ctx.num_oparam = num_param_out;
 262         ctx.code = code;
 263         ctx.code_len = code_len;
 264         ctx.ejtag_info = ejtag_info;
 265         ctx.stack_offset = 0;
 266
 267         while (1)
 268         {
 269                 if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
 270                         return retval;
 271
 272                 address = data = 0;
 273                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
 274                 retval = mips_ejtag_drscan_32(ejtag_info, &address);
 275                 if (retval != ERROR_OK)
 276                         return retval;
 277
 278                 /* Check for read or write */
 279                 if (ejtag_ctrl & EJTAG_CTRL_PRNW)
 280                 {
 281                         if ((retval = mips32_pracc_exec_write(&ctx, address)) != ERROR_OK)
 282                                 return retval;
 283                 }
 284                 else
 285                 {
 286                         /* Check to see if its reading at the debug vector. The first pass through
 287                          * the module is always read at the vector, so the first one we allow.  When
 288                          * the second read from the vector occurs we are done and just exit. */
 289                         if ((address == MIPS32_PRACC_TEXT) && (pass++))
 290                         {
 291                                 break;
 292                         }
 293
 294                         if ((retval = mips32_pracc_exec_read(&ctx, address)) != ERROR_OK)
 295                                 return retval;
 296                 }
 297
 298                 if (cycle == 0)
 299                         break;
 300         }
 301
 302         /* stack sanity check */
 303         if (ctx.stack_offset != 0)
 304         {
 305                 LOG_DEBUG("Pracc Stack not zero");
 306         }
 307
 308         return ERROR_OK;
 309 }
 310
 311 int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
 312 {
 313         switch (size)
 314         {
 315                 case 1:
 316                         return mips32_pracc_read_mem8(ejtag_info, addr, count, (uint8_t*)buf);
 317                 case 2:
 318                         return mips32_pracc_read_mem16(ejtag_info, addr, count, (uint16_t*)buf);
 319                 case 4:
 320                         if (count == 1)
 321                                 return mips32_pracc_read_u32(ejtag_info, addr, (uint32_t*)buf);
 322                         else
 323                                 return mips32_pracc_read_mem32(ejtag_info, addr, count, (uint32_t*)buf);
 324         }
 325
 326         return ERROR_OK;
 327 }
 328
 329 static int mips32_pracc_read_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
 330 {
 331         static const uint32_t code[] = {
 332                                                                                                                         /* start: */
 333                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 334                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 335                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 336                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 337                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 338                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 339                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 340
 341                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 342                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 343                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 344                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 345                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 346                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 347                                                                                                                         /* loop: */
 348                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
 349                 MIPS32_NOP,
 350
 351                 MIPS32_LW(8,0,9),                                                                       /* lw $8,0($9), Load $8 with the word @mem[$9] */
 352                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 353
 354                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 355                 MIPS32_ADDI(9,9,4),                                                                     /* $1 += 4 */
 356                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 357
 358                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 359                 MIPS32_NOP,
 360                                                                                                                         /* end: */
 361                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 362                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 363                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 364                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 365                 MIPS32_B(NEG16(27)),                                                            /* b start */
 366                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 367         };
 368
 369         int retval = ERROR_OK;
 370         int blocksize;
 371         int bytesread;
 372         uint32_t param_in[2];
 373
 374         bytesread = 0;
 375
 376         while (count > 0)
 377         {
 378                 blocksize = count;
 379                 if (count > 0x400)
 380                         blocksize = 0x400;
 381
 382                 param_in[0] = addr;
 383                 param_in[1] = blocksize;
 384
 385                 if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 386                         ARRAY_SIZE(param_in), param_in, blocksize, &buf[bytesread], 1)) != ERROR_OK)
 387                 {
 388                         return retval;
 389                 }
 390
 391                 count -= blocksize;
 392                 addr += blocksize;
 393                 bytesread += blocksize;
 394         }
 395
 396         return retval;
 397 }
 398
 399 static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 400 {
 401         static const uint32_t code[] = {
 402                                                                                                                         /* start: */
 403                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 404                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 405                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 406                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 407
 408                 MIPS32_LW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN),15), /* load R8 @ param_in[0] = address */
 409
 410                 MIPS32_LW(8,0,8),                                                                       /* lw $8,0($8), Load $8 with the word @mem[$8] */
 411                 MIPS32_SW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_OUT),15), /* store R8 @ param_out[0] */
 412
 413                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 414                 MIPS32_B(NEG16(9)),                                                                     /* b start */
 415                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 416         };
 417
 418         int retval = ERROR_OK;
 419         uint32_t param_in[1];
 420
 421         param_in[0] = addr;
 422
 423         if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 424                 ARRAY_SIZE(param_in), param_in, 1, buf, 1)) != ERROR_OK)
 425         {
 426                 return retval;
 427         }
 428
 429         return retval;
 430 }
 431
 432 static int mips32_pracc_read_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
 433 {
 434         static const uint32_t code[] = {
 435                                                                                                                         /* start: */
 436                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 437                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 438                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 439                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 440                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 441                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 442                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 443
 444                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 445                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 446                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 447                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 448                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 449                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 450                                                                                                                         /* loop: */
 451                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
 452                 MIPS32_NOP,
 453
 454                 MIPS32_LHU(8,0,9),                                                                      /* lw $8,0($9), Load $8 with the halfword @mem[$9] */
 455                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 456
 457                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 458                 MIPS32_ADDI(9,9,2),                                                                     /* $9 += 2 */
 459                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 460                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 461                 MIPS32_NOP,
 462                                                                                                                         /* end: */
 463                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 464                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 465                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 466                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 467                 MIPS32_B(NEG16(27)),                                                            /* b start */
 468                 MIPS32_MFC0(15,30,0),                                                           /* move COP0 DeSave to $15 */
 469         };
 470
 471         /* TODO remove array */
 472         uint32_t *param_out = malloc(count * sizeof(uint32_t));
 473         int i;
 474
 475         int retval = ERROR_OK;
 476         int blocksize;
 477         uint32_t param_in[2];
 478
 479         //while (count > 0)
 480         {
 481                 blocksize = count;
 482                 if (count > 0x400)
 483                         blocksize = 0x400;
 484
 485                 param_in[0] = addr;
 486                 param_in[1] = blocksize;
 487
 488                 retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 489                         ARRAY_SIZE(param_in), param_in, count, param_out, 1);
 490
 491 //              count -= blocksize;
 492 //              addr += blocksize;
 493         }
 494
 495         for (i = 0; i < count; i++)
 496         {
 497                 buf[i] = param_out[i];
 498         }
 499
 500         free(param_out);
 501
 502         return retval;
 503 }
 504
 505 static int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
 506 {
 507         static const uint32_t code[] = {
 508                                                                                                                         /* start: */
 509                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 510                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 511                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 512                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 513                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 514                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 515                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 516
 517                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 518                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 519                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 520                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 521                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 522                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 523                                                                                                                         /* loop: */
 524                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
 525                 MIPS32_NOP,
 526
 527                 MIPS32_LBU(8,0,9),                                                                      /* lw $8,0($9), Load t4 with the byte @mem[t1] */
 528                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 529
 530                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 531                 MIPS32_ADDI(9,9,1),                                                                     /* $9 += 1 */
 532                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 533                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 534                 MIPS32_NOP,
 535                                                                                                                         /* end: */
 536                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 537                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 538                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 539                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 540                 MIPS32_B(NEG16(27)),                                                            /* b start */
 541                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 542         };
 543
 544         /* TODO remove array */
 545         uint32_t *param_out = malloc(count * sizeof(uint32_t));
 546         int i;
 547
 548         int retval = ERROR_OK;
 549         int blocksize;
 550         uint32_t param_in[2];
 551
 552 //      while (count > 0)
 553         {
 554                 blocksize = count;
 555                 if (count > 0x400)
 556                         blocksize = 0x400;
 557
 558                 param_in[0] = addr;
 559                 param_in[1] = blocksize;
 560
 561                 retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 562                         ARRAY_SIZE(param_in), param_in, count, param_out, 1);
 563
 564 //              count -= blocksize;
 565 //              addr += blocksize;
 566         }
 567
 568         for (i = 0; i < count; i++)
 569         {
 570                 buf[i] = param_out[i];
 571         }
 572
 573         free(param_out);
 574
 575         return retval;
 576 }
 577
 578 int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_reg, uint32_t cp0_sel)
 579 {
 580         /**
 581          * Do not make this code static, but regenerate it every time,
 582          * as 5th element has to be changed to add parameters
 583          */
 584         uint32_t code[] = {
 585                                                                                                                         /* start: */
 586                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 587                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 588                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 589                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 590                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 591
 592                 /* 5 */ MIPS32_MFC0(8,0,0),                                                     /* move COP0 [cp0_reg select] to $8 */
 593
 594                 MIPS32_LUI(9,UPPER16(MIPS32_PRACC_PARAM_OUT)),          /* $11 = MIPS32_PRACC_PARAM_OUT */
 595                 MIPS32_ORI(9,9,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 596                 MIPS32_SW(8,0,9),                                                                       /* sw $8,0($9) */
 597
 598                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 599                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 600                 MIPS32_B(NEG16(12)),                                                            /* b start */
 601                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 602         };
 603
 604         /**
 605          * Note that our input parametes cp0_reg and cp0_sel
 606          * are numbers (not gprs) which make part of mfc0 instruction opcode.
 607          *
 608          * These are not fix, but can be different for each mips32_cp0_read() function call,
 609          * and that is why we must insert them directly into opcode,
 610          * i.e. we can not pass it on EJTAG microprogram stack (via param_in),
 611          * and put them into the gprs later from MIPS32_PRACC_STACK
 612          * because mfc0 do not use gpr as a parameter for the cp0_reg and select part,
 613          * but plain (immediate) number.
 614          *
 615          * MIPS32_MTC0 is implemented via MIPS32_R_INST macro.
 616          * In order to insert our parameters, we must change rd and funct fields.
 617          */
 618         code[5] |= (cp0_reg << 11) | cp0_sel;  /* change rd and funct of MIPS32_R_INST macro */
 619
 620         /* TODO remove array */
 621         uint32_t *param_out = val;
 622         int retval;
 623
 624         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, 0, NULL, 1, param_out, 1);
 625
 626         return retval;
 627 }
 628
 629 int mips32_cp0_write(struct mips_ejtag *ejtag_info,
 630                                                                                         uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel)
 631 {
 632         uint32_t code[] = {
 633                                                                                                                         /* start: */
 634                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 635                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 636                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 637                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 638                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 639
 640                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 641                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 642                 MIPS32_LW(9,0,8),                                                                       /* Load write val to $9 */
 643
 644                 /* 8 */ MIPS32_MTC0(9,0,0),                                                     /* move $9 to COP0 [cp0_reg select] */
 645
 646                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 647                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 648                 MIPS32_B(NEG16(12)),                                                            /* b start */
 649                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 650         };
 651
 652         /**
 653          * Note that MIPS32_MTC0 macro is implemented via MIPS32_R_INST macro.
 654          * In order to insert our parameters, we must change rd and funct fields.
 655          */
 656         code[8] |= (cp0_reg << 11) | cp0_sel;  /* change rd and funct fields of MIPS32_R_INST macro */
 657
 658         /* TODO remove array */
 659         uint32_t *param_in = malloc(1 * sizeof(uint32_t));
 660         int retval;
 661         param_in[0] = val;
 662
 663         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, 1, param_in, 0, NULL, 1);
 664
 665         free(param_in);
 666
 667         return retval;
 668 }
 669
 670 /**
 671  * \b mips32_pracc_sync_cache
 672  *
 673  * Synchronize Caches to Make Instruction Writes Effective
 674  * (ref. doc. MIPS32 Architecture For Programmers Volume II: The MIPS32 Instruction Set,
 675  *  Document Number: MD00086, Revision 2.00, June 9, 2003)
 676  *
 677  * When the instruction stream is written, the SYNCI instruction should be used
 678  * in conjunction with other instructions to make the newly-written instructions effective.
 679  *
 680  * Explanation :
 681  * A program that loads another program into memory is actually writing the D- side cache.
 682  * The instructions it has loaded can't be executed until they reach the I-cache.
 683  *
 684  * After the instructions have been written, the loader should arrange
 685  * to write back any containing D-cache line and invalidate any locations
 686  * already in the I-cache.
 687  *
 688  * You can do that with cache instructions, but those instructions are only available in kernel mode,
 689  * and a loader writing instructions for the use of its own process need not be privileged software.
 690  *
 691  * In the latest MIPS32/64 CPUs, MIPS provides the synci instruction,
 692  * which does the whole job for a cache-line-sized chunk of the memory you just loaded:
 693  * That is, it arranges a D-cache write-back and an I-cache invalidate.
 694  *
 695  * To employ synci at user level, you need to know the size of a cache line,
 696  * and that can be obtained with a rdhwr SYNCI_Step
 697  * from one of the standard “hardware registers”.
 698  */
 699 static int mips32_pracc_sync_cache(struct mips_ejtag *ejtag_info,
 700                                                                                         uint32_t start_addr, uint32_t end_addr)
 701 {
 702         static const uint32_t code[] = {
 703                                                                                                                         /* start: */
 704                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 705                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 706                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 707                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 708                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 709                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 710                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 711
 712                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 713                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 714                 MIPS32_LW(9,0,8),                                                                       /* Load write start_addr to $9 */
 715                 MIPS32_LW(10,4,8),                                                                      /* Load write end_addr to $10 */
 716
 717                 MIPS32_RDHWR(11, MIPS32_SYNCI_STEP),                            /* $11 = MIPS32_SYNCI_STEP */
 718                 MIPS32_BEQ(11,0,6),                                                                     /* beq $11, $0, end */
 719                 MIPS32_NOP,
 720                                                                                                                         /* synci_loop : */
 721                 MIPS32_SYNCI(0,9),                                                                      /* synci 0($9) */
 722                 MIPS32_SLTU(8,10,9),                                                            /* sltu $8, $10, $9  # $8 = $10 < $9 ? 1 : 0 */
 723                 MIPS32_BNE(8,0,NEG16(3)),                                                       /* bne $8, $0, synci_loop */
 724                 MIPS32_ADDU(9, 9, 11),                                                          /* $9 += MIPS32_SYNCI_STEP */
 725                 MIPS32_SYNC,
 726                                                                                                                         /* end: */
 727                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 728                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 729                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 730                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 731                 MIPS32_B(NEG16(24)),                                                            /* b start */
 732                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 733         };
 734
 735         /* TODO remove array */
 736         uint32_t *param_in = malloc(2 * sizeof(uint32_t));
 737         int retval;
 738         param_in[0] = start_addr;
 739         param_in[1] = end_addr;
 740
 741         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, 2, param_in, 0, NULL, 1);
 742
 743         free(param_in);
 744
 745         return retval;
 746 }
 747
 748 /**
 749  * \b mips32_pracc_clean_invalidate_cache
 750  *
 751  * Writeback D$ and Invalidate I$
 752  * so that the instructions written can be visible to CPU
 753  */
 754 static int mips32_pracc_clean_invalidate_cache(struct mips_ejtag *ejtag_info,
 755                                                                                                         uint32_t start_addr, uint32_t end_addr)
 756 {
 757         static const uint32_t code[] = {
 758                                                                                                                         /* start: */
 759                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 760                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 761                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 762                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 763                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 764                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 765                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 766
 767                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 768                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 769                 MIPS32_LW(9,0,8),                                                                       /* Load write start_addr to $9 */
 770                 MIPS32_LW(10,4,8),                                                                      /* Load write end_addr to $10 */
 771                 MIPS32_LW(11,8,8),                                                                      /* Load write clsiz to $11 */
 772
 773                                                                                                                         /* cache_loop: */
 774                 MIPS32_SLTU(8,10,9),                                                            /* sltu $8, $10, $9  :  $8 <- $10 < $9 ? */
 775                 MIPS32_BGTZ(8,6),                                                                       /* bgtz $8, end */
 776                 MIPS32_NOP,
 777
 778                 MIPS32_CACHE(MIPS32_CACHE_D_HIT_WRITEBACK,0,9),         /* cache Hit_Writeback_D, 0($9) */
 779                 MIPS32_CACHE(MIPS32_CACHE_I_HIT_INVALIDATE,0,9),        /* cache Hit_Invalidate_I, 0($9) */
 780
 781                 MIPS32_ADDU(9,9,11),                                                            /* $9 += $11 */
 782
 783                 MIPS32_B(NEG16(7)),                                                                     /* b cache_loop */
 784                 MIPS32_NOP,
 785                                                                                                                         /* end: */
 786                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 787                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 788                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 789                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 790                 MIPS32_B(NEG16(25)),                                                            /* b start */
 791                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 792         };
 793
 794         /**
 795          * Find cache line size in bytes
 796          */
 797         uint32_t conf;
 798         uint32_t dl, clsiz;
 799
 800         mips32_cp0_read(ejtag_info, &conf, 16, 1);
 801         dl = (conf & MIPS32_CONFIG1_DL_MASK) >> MIPS32_CONFIG1_DL_SHIFT;
 802
 803         /* dl encoding : dl=1 => 4 bytes, dl=2 => 8 bytes, etc... */
 804         clsiz = 0x2 << dl;
 805
 806         /* TODO remove array */
 807         uint32_t *param_in = malloc(3 * sizeof(uint32_t));
 808         int retval;
 809         param_in[0] = start_addr;
 810         param_in[1] = end_addr;
 811         param_in[2] = clsiz;
 812
 813         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, 3, param_in, 0, NULL, 1);
 814
 815         free(param_in);
 816
 817         return retval;
 818 }
 819
 820
 821 int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
 822 {
 823         int retval;
 824
 825         switch (size)
 826         {
 827                 case 1:
 828                         retval = mips32_pracc_write_mem8(ejtag_info, addr, count, (uint8_t*)buf);
 829                         break;
 830                 case 2:
 831                         retval = mips32_pracc_write_mem16(ejtag_info, addr, count,(uint16_t*)buf);
 832                         break;
 833                 case 4:
 834                         if (count == 1)
 835                         {
 836                                 retval = mips32_pracc_write_u32(ejtag_info, addr, (uint32_t*)buf);
 837                         }
 838                         else
 839                         {
 840                                 retval = mips32_pracc_write_mem32(ejtag_info, addr, count, (uint32_t*)buf);
 841                         }
 842                         break;
 843                 default:
 844                         retval = ERROR_FAIL;
 845         }
 846
 847         /**
 848          * If we are in the cachable regoion and cache is activated,
 849          * we must clean D$ + invalidate I$ after we did the write,
 850          * so that changes do not continue to live only in D$, but to be
 851          * replicated in I$ also (maybe we wrote the istructions)
 852          */
 853         uint32_t conf = 0;
 854         int cached = 0;
 855
 856         mips32_cp0_read(ejtag_info, &conf, 16, 0);
 857
 858         switch (KSEGX(addr))
 859         {
 860                 case KUSEG:
 861                         cached = (conf & MIPS32_CONFIG0_KU_MASK) >> MIPS32_CONFIG0_KU_SHIFT;
 862                         break;
 863                 case KSEG0 :
 864                         cached = (conf & MIPS32_CONFIG0_K0_MASK) >> MIPS32_CONFIG0_K0_SHIFT;
 865                         break;
 866                 case KSEG1:
 867                         /* uncachable segment - nothing to do */
 868                         break;
 869                 case KSEG2:
 870                 case KSEG3:
 871                         cached = (conf & MIPS32_CONFIG0_K23_MASK) >> MIPS32_CONFIG0_K23_SHIFT;
 872                         break;
 873                 default:
 874                         /* what ? */
 875                         break;
 876         }
 877
 878         /**
 879          * Check cachablitiy bits coherency algorithm -
 880          * is the region cacheable or uncached.
 881          * If cacheable we have to synchronize the cache
 882          */
 883         if (cached == 0x3)
 884         {
 885                 uint32_t start_addr, end_addr;
 886                 uint32_t rel;
 887
 888                 start_addr = addr;
 889                 end_addr = addr + count * size;
 890
 891                 /** select cache synchronisation mechanism based on Architecture Release */
 892                 rel = (conf & MIPS32_CONFIG0_AR_MASK) >> MIPS32_CONFIG0_AR_SHIFT;
 893                 switch (rel)
 894                 {
 895                         case MIPS32_ARCH_REL1 :
 896                                 /* MIPS32/64 Release 1 - we must use cache instruction */
 897                                 mips32_pracc_clean_invalidate_cache(ejtag_info, start_addr, end_addr);
 898                                 break;
 899                         case MIPS32_ARCH_REL2 :
 900                                 /* MIPS32/64 Release 2 - we can use synci instruction */
 901                                 mips32_pracc_sync_cache(ejtag_info, start_addr, end_addr);
 902                                 break;
 903                         default :
 904                                 /* what ? */
 905                                 break;
 906                 }
 907         }
 908
 909         return retval;
 910 }
 911
 912 static int mips32_pracc_write_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
 913 {
 914         static const uint32_t code[] = {
 915                                                                                                                         /* start: */
 916                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 917                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 918                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 919                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 920                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 921                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 922                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 923
 924                 MIPS32_ADDI(8,15,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)),  /* $8= MIPS32_PRACC_PARAM_IN */
 925                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
 926                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
 927                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 beginning of data */
 928
 929                                                                                                                         /* loop: */
 930                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
 931                 MIPS32_SW(11,0,9),                                                                      /* sw $11,0($9) */
 932
 933                 MIPS32_ADDI(9,9,4),                                                                     /* $9 += 4 */
 934                 MIPS32_BNE(10,9,NEG16(4)),                                                      /* bne $10, $9, loop */
 935                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
 936
 937                                                                                                                         /* end: */
 938                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 939                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 940                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 941                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 942                 MIPS32_B(NEG16(21)),                                                            /* b start */
 943                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 944         };
 945
 946         /* TODO remove array */
 947         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
 948         param_in[0] = addr;
 949         param_in[1] = addr + (count * sizeof(uint32_t));        /* last address */
 950
 951         memcpy(&param_in[2], buf, count * sizeof(uint32_t));
 952
 953         int retval;
 954         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 955                 count + 2, param_in, 0, NULL, 1);
 956
 957         free(param_in);
 958
 959         return retval;
 960 }
 961
 962 static int mips32_pracc_write_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 963 {
 964         static const uint32_t code[] = {
 965                                                                                                                         /* start: */
 966                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 967                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 968                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 969                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 970                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 971
 972                 MIPS32_LW(8,NEG16((MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)-4), 15),   /* load R8 @ param_in[1] = data */
 973                 MIPS32_LW(9,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN), 15),               /* load R9 @ param_in[0] = address */
 974
 975                 MIPS32_SW(8,0,9),                                                                       /* sw $8,0($9) */
 976
 977                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 978                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 979                 MIPS32_B(NEG16(11)),                                                            /* b start */
 980                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 981         };
 982
 983         /* TODO remove array */
 984         uint32_t param_in[1 + 1];
 985         param_in[0] = addr;
 986         param_in[1] = *buf;
 987
 988         return mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 989                 ARRAY_SIZE(param_in), param_in, 0, NULL, 1);
 990 }
 991
 992 static int mips32_pracc_write_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
 993 {
 994         static const uint32_t code[] = {
 995                                                                                                                         /* start: */
 996                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 997                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 998                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 999                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
1000                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
1001                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
1002                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
1003
1004                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
1005                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
1006                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
1007                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
1008                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 */
1009                                                                                                                         /* loop: */
1010                 MIPS32_BEQ(0,10,8),                                                                     /* beq $0, $10, end */
1011                 MIPS32_NOP,
1012
1013                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
1014                 MIPS32_SH(11,0,9),                                                                      /* sh $11,0($9) */
1015
1016                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
1017                 MIPS32_ADDI(9,9,2),                                                                     /* $9 += 2 */
1018                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
1019
1020                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
1021                 MIPS32_NOP,
1022                                                                                                                         /* end: */
1023                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
1024                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
1025                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
1026                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
1027                 MIPS32_B(NEG16(26)),                                                            /* b start */
1028                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
1029         };
1030
1031         /* TODO remove array */
1032         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
1033         int i;
1034         param_in[0] = addr;
1035         param_in[1] = count;
1036
1037         for (i = 0; i < count; i++)
1038         {
1039                 param_in[i + 2] = buf[i];
1040         }
1041
1042         int retval;
1043         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
1044                 count + 2, param_in, 0, NULL, 1);
1045
1046         free(param_in);
1047
1048         return retval;
1049 }
1050
1051 static int mips32_pracc_write_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
1052 {
1053         static const uint32_t code[] = {
1054                                                                                                                         /* start: */
1055                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
1056                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
1057                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
1058                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
1059                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
1060                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
1061                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
1062
1063                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
1064                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
1065                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
1066                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
1067                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 */
1068                                                                                                                         /* loop: */
1069                 MIPS32_BEQ(0,10,8),                                                                     /* beq $0, $10, end */
1070                 MIPS32_NOP,
1071
1072                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
1073                 MIPS32_SB(11,0,9),                                                                      /* sb $11,0($9) */
1074
1075                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
1076                 MIPS32_ADDI(9,9,1),                                                                     /* $9 += 1 */
1077                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
1078
1079                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
1080                 MIPS32_NOP,
1081                                                                                                                         /* end: */
1082                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
1083                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
1084                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
1085                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
1086                 MIPS32_B(NEG16(26)),                                                            /* b start */
1087                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
1088         };
1089
1090         /* TODO remove array */
1091         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
1092         int retval;
1093         int i;
1094         param_in[0] = addr;
1095         param_in[1] = count;
1096
1097         for (i = 0; i < count; i++)
1098         {
1099                 param_in[i + 2] = buf[i];
1100         }
1101
1102         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
1103                 count + 2, param_in, 0, NULL, 1);
1104
1105         free(param_in);
1106
1107         return retval;
1108 }
1109
1110 int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
1111 {
1112         static const uint32_t code[] = {
1113                                                                                                                 /* start: */
1114                 MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_IN)),   /* $2 = MIPS32_PRACC_PARAM_IN */
1115                 MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_IN)),
1116                 MIPS32_LW(1,1*4,2),                                                             /* lw $1,1*4($2) */
1117                 MIPS32_LW(15,15*4,2),                                                   /* lw $15,15*4($2) */
1118                 MIPS32_MTC0(15,31,0),                                                   /* move $15 to COP0 DeSave */
1119                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
1120                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
1121                 MIPS32_SW(1,0,15),                                                              /* sw $1,($15) */
1122                 MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_IN)),   /* $1 = MIPS32_PRACC_PARAM_IN */
1123                 MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_IN)),
1124                 MIPS32_LW(3,3*4,1),                                                             /* lw $3,3*4($1) */
1125                 MIPS32_LW(4,4*4,1),                                                             /* lw $4,4*4($1) */
1126                 MIPS32_LW(5,5*4,1),                                                             /* lw $5,5*4($1) */
1127                 MIPS32_LW(6,6*4,1),                                                             /* lw $6,6*4($1) */
1128                 MIPS32_LW(7,7*4,1),                                                             /* lw $7,7*4($1) */
1129                 MIPS32_LW(8,8*4,1),                                                             /* lw $8,8*4($1) */
1130                 MIPS32_LW(9,9*4,1),                                                             /* lw $9,9*4($1) */
1131                 MIPS32_LW(10,10*4,1),                                                   /* lw $10,10*4($1) */
1132                 MIPS32_LW(11,11*4,1),                                                   /* lw $11,11*4($1) */
1133                 MIPS32_LW(12,12*4,1),                                                   /* lw $12,12*4($1) */
1134                 MIPS32_LW(13,13*4,1),                                                   /* lw $13,13*4($1) */
1135                 MIPS32_LW(14,14*4,1),                                                   /* lw $14,14*4($1) */
1136                 MIPS32_LW(16,16*4,1),                                                   /* lw $16,16*4($1) */
1137                 MIPS32_LW(17,17*4,1),                                                   /* lw $17,17*4($1) */
1138                 MIPS32_LW(18,18*4,1),                                                   /* lw $18,18*4($1) */
1139                 MIPS32_LW(19,19*4,1),                                                   /* lw $19,19*4($1) */
1140                 MIPS32_LW(20,20*4,1),                                                   /* lw $20,20*4($1) */
1141                 MIPS32_LW(21,21*4,1),                                                   /* lw $21,21*4($1) */
1142                 MIPS32_LW(22,22*4,1),                                                   /* lw $22,22*4($1) */
1143                 MIPS32_LW(23,23*4,1),                                                   /* lw $23,23*4($1) */
1144                 MIPS32_LW(24,24*4,1),                                                   /* lw $24,24*4($1) */
1145                 MIPS32_LW(25,25*4,1),                                                   /* lw $25,25*4($1) */
1146                 MIPS32_LW(26,26*4,1),                                                   /* lw $26,26*4($1) */
1147                 MIPS32_LW(27,27*4,1),                                                   /* lw $27,27*4($1) */
1148                 MIPS32_LW(28,28*4,1),                                                   /* lw $28,28*4($1) */
1149                 MIPS32_LW(29,29*4,1),                                                   /* lw $29,29*4($1) */
1150                 MIPS32_LW(30,30*4,1),                                                   /* lw $30,30*4($1) */
1151                 MIPS32_LW(31,31*4,1),                                                   /* lw $31,31*4($1) */
1152
1153                 MIPS32_LW(2,32*4,1),                                                    /* lw $2,32*4($1) */
1154                 MIPS32_MTC0(2,12,0),                                                    /* move $2 to status */
1155                 MIPS32_LW(2,33*4,1),                                                    /* lw $2,33*4($1) */
1156                 MIPS32_MTLO(2),                                                                 /* move $2 to lo */
1157                 MIPS32_LW(2,34*4,1),                                                    /* lw $2,34*4($1) */
1158                 MIPS32_MTHI(2),                                                                 /* move $2 to hi */
1159                 MIPS32_LW(2,35*4,1),                                                    /* lw $2,35*4($1) */
1160                 MIPS32_MTC0(2,8,0),                                                             /* move $2 to badvaddr */
1161                 MIPS32_LW(2,36*4,1),                                                    /* lw $2,36*4($1) */
1162                 MIPS32_MTC0(2,13,0),                                                    /* move $2 to cause*/
1163                 MIPS32_LW(2,37*4,1),                                                    /* lw $2,37*4($1) */
1164                 MIPS32_MTC0(2,24,0),                                                    /* move $2 to depc (pc) */
1165
1166                 MIPS32_LW(2,2*4,1),                                                             /* lw $2,2*4($1) */
1167                 MIPS32_LW(1,0,15),                                                              /* lw $1,($15) */
1168                 MIPS32_B(NEG16(53)),                                                    /* b start */
1169                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
1170         };
1171
1172         int retval;
1173
1174         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
1175                         MIPS32NUMCOREREGS, regs, 0, NULL, 1);
1176
1177         return retval;
1178 }
1179
1180 int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
1181 {
1182         static const uint32_t code[] = {
1183                                                                                                                 /* start: */
1184                 MIPS32_MTC0(2,31,0),                                                    /* move $2 to COP0 DeSave */
1185                 MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_OUT)),  /* $2 = MIPS32_PRACC_PARAM_OUT */
1186                 MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_OUT)),
1187                 MIPS32_SW(0,0*4,2),                                                             /* sw $0,0*4($2) */
1188                 MIPS32_SW(1,1*4,2),                                                             /* sw $1,1*4($2) */
1189                 MIPS32_SW(15,15*4,2),                                                   /* sw $15,15*4($2) */
1190                 MIPS32_MFC0(2,31,0),                                                    /* move COP0 DeSave to $2 */
1191                 MIPS32_MTC0(15,31,0),                                                   /* move $15 to COP0 DeSave */
1192                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
1193                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
1194                 MIPS32_SW(1,0,15),                                                              /* sw $1,($15) */
1195                 MIPS32_SW(2,0,15),                                                              /* sw $2,($15) */
1196                 MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_OUT)),  /* $1 = MIPS32_PRACC_PARAM_OUT */
1197                 MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_OUT)),
1198                 MIPS32_SW(2,2*4,1),                                                             /* sw $2,2*4($1) */
1199                 MIPS32_SW(3,3*4,1),                                                             /* sw $3,3*4($1) */
1200                 MIPS32_SW(4,4*4,1),                                                             /* sw $4,4*4($1) */
1201                 MIPS32_SW(5,5*4,1),                                                             /* sw $5,5*4($1) */
1202                 MIPS32_SW(6,6*4,1),                                                             /* sw $6,6*4($1) */
1203                 MIPS32_SW(7,7*4,1),                                                             /* sw $7,7*4($1) */
1204                 MIPS32_SW(8,8*4,1),                                                             /* sw $8,8*4($1) */
1205                 MIPS32_SW(9,9*4,1),                                                             /* sw $9,9*4($1) */
1206                 MIPS32_SW(10,10*4,1),                                                   /* sw $10,10*4($1) */
1207                 MIPS32_SW(11,11*4,1),                                                   /* sw $11,11*4($1) */
1208                 MIPS32_SW(12,12*4,1),                                                   /* sw $12,12*4($1) */
1209                 MIPS32_SW(13,13*4,1),                                                   /* sw $13,13*4($1) */
1210                 MIPS32_SW(14,14*4,1),                                                   /* sw $14,14*4($1) */
1211                 MIPS32_SW(16,16*4,1),                                                   /* sw $16,16*4($1) */
1212                 MIPS32_SW(17,17*4,1),                                                   /* sw $17,17*4($1) */
1213                 MIPS32_SW(18,18*4,1),                                                   /* sw $18,18*4($1) */
1214                 MIPS32_SW(19,19*4,1),                                                   /* sw $19,19*4($1) */
1215                 MIPS32_SW(20,20*4,1),                                                   /* sw $20,20*4($1) */
1216                 MIPS32_SW(21,21*4,1),                                                   /* sw $21,21*4($1) */
1217                 MIPS32_SW(22,22*4,1),                                                   /* sw $22,22*4($1) */
1218                 MIPS32_SW(23,23*4,1),                                                   /* sw $23,23*4($1) */
1219                 MIPS32_SW(24,24*4,1),                                                   /* sw $24,24*4($1) */
1220                 MIPS32_SW(25,25*4,1),                                                   /* sw $25,25*4($1) */
1221                 MIPS32_SW(26,26*4,1),                                                   /* sw $26,26*4($1) */
1222                 MIPS32_SW(27,27*4,1),                                                   /* sw $27,27*4($1) */
1223                 MIPS32_SW(28,28*4,1),                                                   /* sw $28,28*4($1) */
1224                 MIPS32_SW(29,29*4,1),                                                   /* sw $29,29*4($1) */
1225                 MIPS32_SW(30,30*4,1),                                                   /* sw $30,30*4($1) */
1226                 MIPS32_SW(31,31*4,1),                                                   /* sw $31,31*4($1) */
1227
1228                 MIPS32_MFC0(2,12,0),                                                    /* move status to $2 */
1229                 MIPS32_SW(2,32*4,1),                                                    /* sw $2,32*4($1) */
1230                 MIPS32_MFLO(2),                                                                 /* move lo to $2 */
1231                 MIPS32_SW(2,33*4,1),                                                    /* sw $2,33*4($1) */
1232                 MIPS32_MFHI(2),                                                                 /* move hi to $2 */
1233                 MIPS32_SW(2,34*4,1),                                                    /* sw $2,34*4($1) */
1234                 MIPS32_MFC0(2,8,0),                                                             /* move badvaddr to $2 */
1235                 MIPS32_SW(2,35*4,1),                                                    /* sw $2,35*4($1) */
1236                 MIPS32_MFC0(2,13,0),                                                    /* move cause to $2 */
1237                 MIPS32_SW(2,36*4,1),                                                    /* sw $2,36*4($1) */
1238                 MIPS32_MFC0(2,24,0),                                                    /* move depc (pc) to $2 */
1239                 MIPS32_SW(2,37*4,1),                                                    /* sw $2,37*4($1) */
1240
1241                 MIPS32_LW(2,0,15),                                                              /* lw $2,($15) */
1242                 MIPS32_LW(1,0,15),                                                              /* lw $1,($15) */
1243                 MIPS32_B(NEG16(58)),                                                    /* b start */
1244                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
1245         };
1246
1247         int retval;
1248
1249         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
1250                 0, NULL, MIPS32NUMCOREREGS, regs, 1);
1251
1252         return retval;
1253 }
1254
1255 /* fastdata upload/download requires an initialized working area
1256  * to load the download code; it should not be called otherwise
1257  * fetch order from the fastdata area
1258  * 1. start addr
1259  * 2. end addr
1260  * 3. data ...
1261  */
1262 int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_area *source,
1263                                                                 int write_t, uint32_t addr, int count, uint32_t *buf)
1264 {
1265         uint32_t handler_code[] = {
1266                 /* caution when editing, table is modified below */
1267                 /* r15 points to the start of this code */
1268                 MIPS32_SW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
1269                 MIPS32_SW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
1270                 MIPS32_SW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
1271                 MIPS32_SW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
1272                 /* start of fastdata area in t0 */
1273                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
1274                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
1275                 MIPS32_LW(9,0,8),                                                               /* start addr in t1 */
1276                 MIPS32_LW(10,0,8),                                                              /* end addr to t2 */
1277                                                                                                                 /* loop: */
1278                 /* 8 */ MIPS32_LW(11,0,0),                                              /* lw t3,[t8 | r9] */
1279                 /* 9 */ MIPS32_SW(11,0,0),                                              /* sw t3,[r9 | r8] */
1280                 MIPS32_BNE(10,9,NEG16(3)),                                              /* bne $t2,t1,loop */
1281                 MIPS32_ADDI(9,9,4),                                                             /* addi t1,t1,4 */
1282
1283                 MIPS32_LW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
1284                 MIPS32_LW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
1285                 MIPS32_LW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
1286                 MIPS32_LW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
1287
1288                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_TEXT)),
1289                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_TEXT)),
1290                 MIPS32_JR(15),                                                                  /* jr start */
1291                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
1292         };
1293
1294         uint32_t jmp_code[] = {
1295                 MIPS32_MTC0(15,31,0),                   /* move $15 to COP0 DeSave */
1296                 /* 1 */ MIPS32_LUI(15,0),               /* addr of working area added below */
1297                 /* 2 */ MIPS32_ORI(15,15,0),    /* addr of working area added below */
1298                 MIPS32_JR(15),                                  /* jump to ram program */
1299                 MIPS32_NOP,
1300         };
1301
1302         int retval, i;
1303         uint32_t val, ejtag_ctrl, address;
1304
1305         if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
1306                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1307
1308         if (write_t)
1309         {
1310                 handler_code[8] = MIPS32_LW(11,0,8);    /* load data from probe at fastdata area */
1311                 handler_code[9] = MIPS32_SW(11,0,9);    /* store data to RAM @ r9 */
1312         }
1313         else
1314         {
1315                 handler_code[8] = MIPS32_LW(11,0,9);    /* load data from RAM @ r9 */
1316                 handler_code[9] = MIPS32_SW(11,0,8);    /* store data to probe at fastdata area */
1317         }
1318
1319         /* write program into RAM */
1320         if (write_t != ejtag_info->fast_access_save)
1321         {
1322                 mips32_pracc_write_mem32(ejtag_info, source->address, ARRAY_SIZE(handler_code), handler_code);
1323                 /* save previous operation to speed to any consecutive read/writes */
1324                 ejtag_info->fast_access_save = write_t;
1325         }
1326
1327         LOG_DEBUG("%s using 0x%.8" PRIx32 " for write handler", __func__, source->address);
1328
1329         jmp_code[1] |= UPPER16(source->address);
1330         jmp_code[2] |= LOWER16(source->address);
1331
1332         for (i = 0; i < (int) ARRAY_SIZE(jmp_code); i++)
1333         {
1334                 if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1335                         return retval;
1336
1337                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_DATA);
1338                 mips_ejtag_drscan_32_out(ejtag_info, jmp_code[i]);
1339
1340                 /* Clear the access pending bit (let the processor eat!) */
1341                 ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
1342                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
1343                 mips_ejtag_drscan_32_out(ejtag_info, ejtag_ctrl);
1344         }
1345
1346         if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1347                 return retval;
1348
1349         /* next fetch to dmseg should be in FASTDATA_AREA, check */
1350         address = 0;
1351         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1352         retval = mips_ejtag_drscan_32(ejtag_info, &address);
1353         if (retval != ERROR_OK)
1354                 return retval;
1355
1356         if (address != MIPS32_PRACC_FASTDATA_AREA)
1357                 return ERROR_FAIL;
1358
1359         /* wait PrAcc pending bit for FASTDATA write */
1360         if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1361                 return retval;
1362
1363         /* Send the load start address */
1364         val = addr;
1365         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
1366         mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
1367
1368         /* Send the load end address */
1369         val = addr + (count - 1) * 4;
1370         mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
1371
1372         for (i = 0; i < count; i++)
1373         {
1374                 if ((retval = mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++)) != ERROR_OK)
1375                         return retval;
1376         }
1377
1378         if ((retval = jtag_execute_queue()) != ERROR_OK)
1379         {
1380                 LOG_ERROR("fastdata load failed");
1381                 return retval;
1382         }
1383
1384         if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1385                 return retval;
1386
1387         address = 0;
1388         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1389         retval = mips_ejtag_drscan_32(ejtag_info, &address);
1390         if (retval != ERROR_OK)
1391                 return retval;
1392
1393         if (address != MIPS32_PRACC_TEXT)
1394                 LOG_ERROR("mini program did not return to start");
1395
1396         return retval;
1397 }