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