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  *   This program is free software; you can redistribute it and/or modify  *
   8  *   it under the terms of the GNU General Public License as published by  *
   9  *   the Free Software Foundation; either version 2 of the License, or     *
  10  *   (at your option) any later version.                                   *
  11  *                                                                         *
  12  *   This program is distributed in the hope that it will be useful,       *
  13  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  14  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  15  *   GNU General Public License for more details.                          *
  16  *                                                                         *
  17  *   You should have received a copy of the GNU General Public License     *
  18  *   along with this program; if not, write to the                         *
  19  *   Free Software Foundation, Inc.,                                       *
  20  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  21  ***************************************************************************/
  22
  23 /*
  24 This version has optimized assembly routines for 32 bit operations:
  25 - read word
  26 - write word
  27 - write array of words
  28
  29 One thing to be aware of is that the MIPS32 cpu will execute the
  30 instruction after a branch instruction (one delay slot).
  31
  32 For example:
  33
  34
  35     LW $2, ($5 +10)
  36     B foo
  37     LW $1, ($2 +100)
  38
  39 The LW $1, ($2 +100) instruction is also executed. If this is
  40 not wanted a NOP can be inserted:
  41
  42     LW $2, ($5 +10)
  43     B foo
  44     NOP
  45     LW $1, ($2 +100)
  46
  47 or the code can be changed to:
  48
  49     B foo
  50     LW $2, ($5 +10)
  51     LW $1, ($2 +100)
  52
  53 The original code contained NOPs. I have removed these and moved
  54 the branches.
  55
  56 I also moved the PRACC_STACK to 0xFF204000. This allows
  57 the use of 16 bits offsets to get pointers to the input
  58 and output area relative to the stack. Note that the stack
  59 isn't really a stack (the stack pointer is not 'moving')
  60 but a FIFO simulated in software.
  61
  62 These changes result in a 35% speed increase when programming an
  63 external flash.
  64
  65 More improvement could be gained if the registers do no need
  66 to be preserved but in that case the routines should be aware
  67 OpenOCD is used as a flash programmer or as a debug tool.
  68
  69 Nico Coesel
  70 */
  71
  72
  73 #ifdef HAVE_CONFIG_H
  74 #include "config.h"
  75 #endif
  76
  77 #include "mips32.h"
  78 #include "mips32_pracc.h"
  79
  80 struct mips32_pracc_context
  81 {
  82         uint32_t *local_iparam;
  83         int num_iparam;
  84         uint32_t *local_oparam;
  85         int num_oparam;
  86         uint32_t *code;
  87         int code_len;
  88         uint32_t stack[32];
  89         int stack_offset;
  90         struct mips_ejtag *ejtag_info;
  91 };
  92
  93 static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
  94 {
  95         uint32_t ejtag_ctrl;
  96
  97         while (1)
  98         {
  99                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL, NULL);
 100                 ejtag_ctrl = ejtag_info->ejtag_ctrl;
 101                 mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
 102                 if (ejtag_ctrl & EJTAG_CTRL_PRACC)
 103                         break;
 104                 LOG_DEBUG("DEBUGMODULE: No memory access in progress!\n");
 105                 return ERROR_JTAG_DEVICE_ERROR;
 106         }
 107
 108         *ctrl = ejtag_ctrl;
 109         return ERROR_OK;
 110 }
 111
 112 static int mips32_pracc_exec_read(struct mips32_pracc_context *ctx, uint32_t address)
 113 {
 114         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
 115         int offset;
 116         uint32_t ejtag_ctrl, data;
 117
 118         if ((address >= MIPS32_PRACC_PARAM_IN)
 119                 && (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
 120         {
 121                 offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
 122                 data = ctx->local_iparam[offset];
 123         }
 124         else if ((address >= MIPS32_PRACC_PARAM_OUT)
 125                 && (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
 126         {
 127                 offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
 128                 data = ctx->local_oparam[offset];
 129         }
 130         else if ((address >= MIPS32_PRACC_TEXT)
 131                 && (address <= MIPS32_PRACC_TEXT + ctx->code_len*4))
 132         {
 133                 offset = (address - MIPS32_PRACC_TEXT) / 4;
 134                 data = ctx->code[offset];
 135         }
 136         else if (address == MIPS32_PRACC_STACK)
 137         {
 138                 /* save to our debug stack */
 139                 data = ctx->stack[--ctx->stack_offset];
 140         }
 141         else
 142         {
 143                 /* TODO: send JMP 0xFF200000 instruction. Hopefully processor jump back
 144                  * to start of debug vector */
 145
 146                 data = 0;
 147                 LOG_ERROR("Error reading unexpected address %8.8" PRIx32 "", address);
 148                 return ERROR_JTAG_DEVICE_ERROR;
 149         }
 150
 151         /* Send the data out */
 152         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA, NULL);
 153         mips_ejtag_drscan_32(ctx->ejtag_info, &data);
 154
 155         /* Clear the access pending bit (let the processor eat!) */
 156
 157         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 158         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL, NULL);
 159         mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
 160
 161         jtag_add_clocks(5);
 162         jtag_execute_queue();
 163
 164
 165         return ERROR_OK;
 166 }
 167
 168 static int mips32_pracc_exec_write(struct mips32_pracc_context *ctx, uint32_t address)
 169 {
 170         uint32_t ejtag_ctrl,data;
 171         int offset;
 172         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
 173
 174         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA, NULL);
 175         mips_ejtag_drscan_32(ctx->ejtag_info, &data);
 176
 177         /* Clear access pending bit */
 178         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 179         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL, NULL);
 180         mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
 181
 182         jtag_add_clocks(5);
 183         jtag_execute_queue();
 184
 185         if ((address >= MIPS32_PRACC_PARAM_IN)
 186                 && (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
 187         {
 188                 offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
 189                 ctx->local_iparam[offset] = data;
 190         }
 191         else if ((address >= MIPS32_PRACC_PARAM_OUT)
 192                 && (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
 193         {
 194                 offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
 195                 ctx->local_oparam[offset] = data;
 196         }
 197         else if (address == MIPS32_PRACC_STACK)
 198         {
 199                 /* save data onto our stack */
 200                 ctx->stack[ctx->stack_offset++] = data;
 201         }
 202         else
 203         {
 204                 LOG_ERROR("Error writing unexpected address %8.8" PRIx32 "", address);
 205                 return ERROR_JTAG_DEVICE_ERROR;
 206         }
 207
 208         return ERROR_OK;
 209 }
 210
 211 int mips32_pracc_exec(struct mips_ejtag *ejtag_info, int code_len, uint32_t *code, int num_param_in, uint32_t *param_in, int num_param_out, uint32_t *param_out, int cycle)
 212 {
 213         uint32_t ejtag_ctrl;
 214         uint32_t address, data;
 215         struct mips32_pracc_context ctx;
 216         int retval;
 217         int pass = 0;
 218
 219         ctx.local_iparam = param_in;
 220         ctx.local_oparam = param_out;
 221         ctx.num_iparam = num_param_in;
 222         ctx.num_oparam = num_param_out;
 223         ctx.code = code;
 224         ctx.code_len = code_len;
 225         ctx.ejtag_info = ejtag_info;
 226         ctx.stack_offset = 0;
 227
 228         while (1)
 229         {
 230                 if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
 231                         return retval;
 232
 233                 address = data = 0;
 234                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS, NULL);
 235                 mips_ejtag_drscan_32(ejtag_info, &address);
 236
 237 //              printf("Adres: %.8x\n", address);
 238
 239                 /* Check for read or write */
 240                 if (ejtag_ctrl & EJTAG_CTRL_PRNW)
 241                 {
 242                         if ((retval = mips32_pracc_exec_write(&ctx, address)) != ERROR_OK)
 243                                 return retval;
 244                 }
 245                 else
 246                 {
 247                         /* Check to see if its reading at the debug vector. The first pass through
 248                          * the module is always read at the vector, so the first one we allow.  When
 249                          * the second read from the vector occurs we are done and just exit. */
 250                         if ((address == MIPS32_PRACC_TEXT) && (pass++))
 251                         {
 252                                 break;
 253                         }
 254
 255                         if ((retval = mips32_pracc_exec_read(&ctx, address)) != ERROR_OK)
 256                                 return retval;
 257
 258                 }
 259
 260                 if (cycle == 0)
 261                         break;
 262         }
 263
 264         /* stack sanity check */
 265         if (ctx.stack_offset != 0)
 266         {
 267                 LOG_DEBUG("Pracc Stack not zero");
 268         }
 269
 270         return ERROR_OK;
 271 }
 272
 273 int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
 274 {
 275         switch (size)
 276         {
 277                 case 1:
 278                         return mips32_pracc_read_mem8(ejtag_info, addr, count, (uint8_t*)buf);
 279                 case 2:
 280                         return mips32_pracc_read_mem16(ejtag_info, addr, count, (uint16_t*)buf);
 281                 case 4:
 282                         if (count == 1)
 283                                 return mips32_pracc_read_u32(ejtag_info, addr, (uint32_t*)buf);
 284                         else
 285                                 return mips32_pracc_read_mem32(ejtag_info, addr, count, (uint32_t*)buf);
 286         }
 287
 288         return ERROR_OK;
 289 }
 290
 291 int mips32_pracc_read_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
 292 {
 293         uint32_t code[] = {
 294                                                                                                                         /* start: */
 295                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 296                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
 297                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 298                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 299                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 300                 MIPS32_SW(10,0,15),                                                             /* sw $10,($15) */
 301                 MIPS32_SW(11,0,15),                                                             /* sw $11,($15) */
 302
 303                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 304                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 305                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 306                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 307                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 308                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 309                 MIPS32_NOP,
 310                                                                                                                         /* loop: */
 311                 MIPS32_BEQ(0,10,9),                                                                     /* beq 0, $10, end */
 312                 MIPS32_NOP,
 313
 314                 MIPS32_LW(8,0,9),                                                                       /* lw $8,0($9), Load $8 with the word @mem[$9] */
 315                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 316
 317                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 318                 MIPS32_ADDI(9,9,4),                                                             /* $1 += 4 */
 319                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 320
 321                 MIPS32_NOP,
 322                 MIPS32_B(NEG16(9)),                                                                     /* b loop */
 323                 MIPS32_NOP,
 324                                                                                                                         /* end: */
 325                 MIPS32_LW(11,0,15),                                                             /* lw $11,($15) */
 326                 MIPS32_LW(10,0,15),                                                             /* lw $10,($15) */
 327                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 328                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 329                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 330                 MIPS32_NOP,
 331                 MIPS32_B(NEG16(31)),                                                            /* b start */
 332                 MIPS32_NOP,
 333         };
 334
 335         int retval = ERROR_OK;
 336         int blocksize;
 337         int bytesread;
 338         uint32_t param_in[2];
 339
 340         bytesread = 0;
 341
 342         while (count > 0)
 343         {
 344                 blocksize = count;
 345                 if (count > 0x400)
 346                         blocksize = 0x400;
 347
 348                 param_in[0] = addr;
 349                 param_in[1] = blocksize;
 350
 351                 if ((retval = mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code,
 352                         sizeof(param_in)/sizeof(param_in[0]), param_in, blocksize, &buf[bytesread], 1)) != ERROR_OK)
 353                 {
 354                         return retval;
 355                 }
 356
 357                 count -= blocksize;
 358                 addr += blocksize;
 359                 bytesread += blocksize;
 360         }
 361
 362         return retval;
 363 }
 364
 365 int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 366 {
 367         uint32_t code[] = {
 368                                                                                                                         /* start: */
 369                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 370                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
 371                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 372                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 373
 374                 MIPS32_LW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN), 15),  //load R8 @ param_in[0] = address
 375
 376                 MIPS32_LW(8,0,8),                                                                       /* lw $8,0($8), Load $8 with the word @mem[$8] */
 377                 MIPS32_SW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_OUT),15),                                                                       /* sw $8,0($9) */
 378
 379                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 380                 MIPS32_B(NEG16(9)),     //was 17                                                        /* b start */
 381                 MIPS32_MFC0(15,31,0),   //this instruction will be executed (MIPS executes instruction after jump)                                                      /* move COP0 DeSave to $15 */
 382                 MIPS32_NOP,
 383         };
 384
 385         int retval = ERROR_OK;
 386         uint32_t param_in[1];
 387
 388         param_in[0] = addr;
 389
 390         if ((retval = mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code,
 391                 sizeof(param_in)/sizeof(param_in[0]), param_in, sizeof(uint32_t), buf, 1)) != ERROR_OK)
 392         {
 393                 return retval;
 394         }
 395
 396         return retval;
 397 }
 398
 399 int mips32_pracc_read_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
 400 {
 401         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                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 408                 MIPS32_SW(10,0,15),                                                             /* sw $10,($15) */
 409                 MIPS32_SW(11,0,15),                                                             /* sw $11,($15) */
 410
 411                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 412                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 413                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 414                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 415                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 416                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 417                 MIPS32_NOP,
 418                                                                                                                         /* loop: */
 419                 MIPS32_BEQ(0,10,9),                                                             /* beq 0, $10, end */
 420                 MIPS32_NOP,
 421
 422                 MIPS32_LHU(8,0,9),                                                                      /* lw $8,0($9), Load $8 with the halfword @mem[$9] */
 423                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 424
 425                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 426                 MIPS32_ADDI(9,9,2),                                                             /* $9 += 2 */
 427                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 428                 MIPS32_NOP,
 429                 MIPS32_B(NEG16(9)),                                                                     /* b loop */
 430                 MIPS32_NOP,
 431
 432                 MIPS32_LW(11,0,15),                                                             /* lw $11,($15) */
 433                 MIPS32_LW(10,0,15),                                                             /* lw $10,($15) */
 434                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 435                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 436                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 437                 MIPS32_NOP,
 438                 MIPS32_B(NEG16(31)),                                                            /* b start */
 439                 MIPS32_NOP,
 440         };
 441
 442 //      /* TODO remove array */
 443         uint32_t param_out[count];
 444         int i;
 445
 446 //      int retval;
 447         int blocksize;
 448         int bytesread;
 449         uint32_t param_in[2];
 450
 451         bytesread = 0;
 452
 453         //while (count > 0)
 454         {
 455                 blocksize = count;
 456                 if (count > 0x400)
 457                         blocksize = 0x400;
 458
 459                 param_in[0] = addr;
 460                 param_in[1] = blocksize;
 461
 462                 mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code, \
 463                         sizeof(param_in)/sizeof(param_in[0]), param_in, count, param_out, 1);
 464
 465 //              count -= blocksize;
 466 //              addr += blocksize;
 467 //              bytesread += blocksize;
 468         }
 469
 470         for (i = 0; i < count; i++)
 471         {
 472                 buf[i] = param_out[i];
 473         }
 474
 475         return ERROR_OK;
 476 }
 477
 478 int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
 479 {
 480         uint32_t code[] = {
 481                                                                                                                         /* start: */
 482                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 483                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
 484                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 485                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 486                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 487                 MIPS32_SW(10,0,15),                                                             /* sw $10,($15) */
 488                 MIPS32_SW(11,0,15),                                                             /* sw $11,($15) */
 489
 490                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 491                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 492                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 493                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 494                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 495                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 496                 MIPS32_NOP,
 497                                                                                                                         /* loop: */
 498                 MIPS32_BEQ(0,10,9),                                                             /* beq 0, $10, end */
 499                 MIPS32_NOP,
 500
 501                 MIPS32_LBU(8,0,9),                                                                      /* lw $8,0($9), Load t4 with the byte @mem[t1] */
 502                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 503
 504                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 505                 MIPS32_ADDI(9,9,1),                                                             /* $9 += 1 */
 506                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 507                 MIPS32_NOP,
 508                 MIPS32_B(NEG16(9)),                                                                     /* b loop */
 509                 MIPS32_NOP,
 510                                                                                                                         /* end: */
 511                 MIPS32_LW(11,0,15),                                                             /* lw $11,($15) */
 512                 MIPS32_LW(10,0,15),                                                             /* lw $10,($15) */
 513                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 514                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 515                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 516                 MIPS32_NOP,
 517                 MIPS32_B(NEG16(31)),                                                            /* b start */
 518                 MIPS32_NOP,
 519         };
 520
 521 //      /* TODO remove array */
 522         uint32_t param_out[count];
 523         int i;
 524
 525 //      int retval;
 526         int blocksize;
 527         int bytesread;
 528         uint32_t param_in[2];
 529
 530         bytesread = 0;
 531
 532 //      while (count > 0)
 533         {
 534                 blocksize = count;
 535                 if (count > 0x400)
 536                         blocksize = 0x400;
 537
 538                 param_in[0] = addr;
 539                 param_in[1] = blocksize;
 540
 541                 mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code, \
 542                         sizeof(param_in)/sizeof(param_in[0]), param_in, count, param_out, 1);
 543
 544 //              count -= blocksize;
 545 //              addr += blocksize;
 546 //              bytesread += blocksize;
 547         }
 548
 549         for (i = 0; i < count; i++)
 550         {
 551                 buf[i] = param_out[i];
 552         }
 553
 554         return ERROR_OK;
 555 }
 556
 557 int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
 558 {
 559         switch (size)
 560         {
 561                 case 1:
 562                         return mips32_pracc_write_mem8(ejtag_info, addr, count, (uint8_t*)buf);
 563                 case 2:
 564                         return mips32_pracc_write_mem16(ejtag_info, addr, count,(uint16_t*)buf);
 565                 case 4:
 566                         if (count == 1)
 567                                 return mips32_pracc_write_u32(ejtag_info, addr, (uint32_t*)buf);
 568                         else
 569                                 return mips32_pracc_write_mem32(ejtag_info, addr, count, (uint32_t*)buf);
 570         }
 571
 572         return ERROR_OK;
 573 }
 574
 575 int mips32_pracc_write_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
 576 {
 577
 578 //NC: use destination pointer as loop counter (last address is in $10)
 579         uint32_t code[] = {
 580                                                                                                                         /* start: */
 581                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 582                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
 583                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 584                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 585                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 586                 MIPS32_SW(10,0,15),                                                             /* sw $10,($15) */
 587                 MIPS32_SW(11,0,15),                                                             /* sw $11,($15) */
 588
 589                 MIPS32_ADDI(8,15,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)),  //$8= MIPS32_PRACC_PARAM_IN
 590                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
 591                 MIPS32_LW(10,4,8),      //last address                                                                  /* Load write count to $10 */
 592                 MIPS32_ADDI(8,8,8),     // $8 += 8 beginning of data
 593
 594 //loop:
 595                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
 596                 MIPS32_SW(11,0,9),                                                                      /* sw $11,0($9) */
 597
 598                 MIPS32_ADDI(9,9,4),                                                             /* $9 += 4 */
 599                 MIPS32_BNE(10,9,NEG16(4)),  //was 9 BNE $10, 9, loop                                                                    /* b loop */
 600                 MIPS32_ADDI(8,8,4),  //this instruction is part of the loop (one delay slot)!   /* $8 += 4 */
 601                                                                                                                         /* end: */
 602                 MIPS32_LW(11,0,15),                                                             /* lw $11,($15) */
 603                 MIPS32_LW(10,0,15),                                                             /* lw $10,($15) */
 604                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 605                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 606                 MIPS32_B(NEG16(21)),     //was 30                                                       /* b start */
 607                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 608                 MIPS32_NOP, //this one will not be executed
 609         };
 610
 611         /* TODO remove array */
 612         uint32_t param_in[count + 2];
 613         param_in[0] = addr;
 614         param_in[1] = addr + count * sizeof(uint32_t);  //last address
 615
 616         memcpy(&param_in[2], buf, count * sizeof(uint32_t));
 617
 618         mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code, \
 619                 sizeof(param_in)/sizeof(param_in[0]),param_in, 0, NULL, 1);
 620
 621         return ERROR_OK;
 622 }
 623
 624 int mips32_pracc_write_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 625 {
 626         uint32_t code[] = {
 627                                                                                                                         /* start: */
 628                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 629                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
 630                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 631                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 632                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 633
 634                 MIPS32_LW(8,NEG16((MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)-4), 15),  //load R8 @ param_in[1] = data
 635                 MIPS32_LW(9,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN), 15),  //load R9 @ param_in[0] = address
 636
 637                 MIPS32_SW(8,0,9),                                                                       /* sw $8,0($9) */
 638
 639                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 640                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 641                 MIPS32_B(NEG16(11)),                                                            /* b start */
 642                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
 643                 MIPS32_NOP,
 644         };
 645
 646         /* TODO remove array */
 647         uint32_t param_in[1 + 1];
 648         param_in[0] = addr;
 649         param_in[1] = *buf;
 650
 651         mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code, \
 652                 sizeof(param_in)/sizeof(param_in[0]),param_in, 0, NULL, 1);
 653
 654         return ERROR_OK;
 655 }
 656
 657 int mips32_pracc_write_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
 658 {
 659         uint32_t code[] = {
 660                                                                                                                         /* start: */
 661                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 662                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
 663                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 664                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 665                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 666                 MIPS32_SW(10,0,15),                                                             /* sw $10,($15) */
 667                 MIPS32_SW(11,0,15),                                                             /* sw $11,($15) */
 668
 669                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 670                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 671                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
 672                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
 673                 MIPS32_ADDI(8,8,8),                                                             /* $8 += 8 */
 674                 MIPS32_NOP,
 675                                                                                                                         /* loop: */
 676                 MIPS32_BEQ(0,10,9),                                                                     /* beq $0, $10, end */
 677                 MIPS32_NOP,
 678
 679                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
 680                 MIPS32_SH(11,0,9),                                                                      /* sh $11,0($9) */
 681
 682                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 683                 MIPS32_ADDI(9,9,2),                                                             /* $9 += 2 */
 684                 MIPS32_ADDI(8,8,4),                                                             /* $8 += 4 */
 685
 686                 MIPS32_NOP,
 687                 MIPS32_B(NEG16(9)),                                                                     /* b loop */
 688                 MIPS32_NOP,
 689                                                                                                                         /* end: */
 690                 MIPS32_LW(11,0,15),                                                             /* lw $11,($15) */
 691                 MIPS32_LW(10,0,15),                                                             /* lw $10,($15) */
 692                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 693                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 694                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 695                 MIPS32_NOP,
 696                 MIPS32_B(NEG16(30)),                                                            /* b start */
 697                 MIPS32_NOP,
 698         };
 699
 700         /* TODO remove array */
 701         uint32_t param_in[count + 2];
 702         int i;
 703         param_in[0] = addr;
 704         param_in[1] = count;
 705
 706         for (i = 0; i < count; i++)
 707         {
 708                 param_in[i + 2] = buf[i];
 709         }
 710
 711         mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code, \
 712                 sizeof(param_in)/sizeof(param_in[0]), param_in, 0, NULL, 1);
 713
 714         return ERROR_OK;
 715 }
 716
 717 int mips32_pracc_write_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
 718 {
 719         uint32_t code[] = {
 720                                                                                                                         /* start: */
 721                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 722                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
 723                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 724                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 725                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 726                 MIPS32_SW(10,0,15),                                                             /* sw $10,($15) */
 727                 MIPS32_SW(11,0,15),                                                             /* sw $11,($15) */
 728
 729                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 730                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 731                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
 732                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
 733                 MIPS32_ADDI(8,8,8),                                                             /* $8 += 8 */
 734                 MIPS32_NOP,
 735                                                                                                                         /* loop: */
 736                 MIPS32_BEQ(0,10,9),                                                                     /* beq $0, $10, end */
 737                 MIPS32_NOP,
 738
 739                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
 740                 MIPS32_SB(11,0,9),                                                                      /* sb $11,0($9) */
 741
 742                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 743                 MIPS32_ADDI(9,9,1),                                                             /* $9 += 1 */
 744                 MIPS32_ADDI(8,8,4),                                                             /* $8 += 4 */
 745
 746                 MIPS32_NOP,
 747                 MIPS32_B(NEG16(9)),                                                                     /* b loop */
 748                 MIPS32_NOP,
 749                                                                                                                         /* end: */
 750                 MIPS32_LW(11,0,15),                                                             /* lw $11,($15) */
 751                 MIPS32_LW(10,0,15),                                                             /* lw $10,($15) */
 752                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 753                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 754                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 755                 MIPS32_NOP,
 756                 MIPS32_B(NEG16(30)),                                                            /* b start */
 757                 MIPS32_NOP,
 758         };
 759
 760         /* TODO remove array */
 761         uint32_t param_in[count + 2];
 762         int retval;
 763         int i;
 764         param_in[0] = addr;
 765         param_in[1] = count;
 766
 767         for (i = 0; i < count; i++)
 768         {
 769                 param_in[i + 2] = buf[i];
 770         }
 771
 772         retval = mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code, \
 773                 sizeof(param_in)/sizeof(param_in[0]), param_in, 0, NULL, 1);
 774
 775         return retval;
 776 }
 777
 778 int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 779 {
 780         uint32_t code[] = {
 781                                                                                                                 /* start: */
 782                 MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_IN)),   /* $2 = MIPS32_PRACC_PARAM_IN */
 783                 MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_IN)),
 784                 MIPS32_LW(1,1*4,2),                                                     /* lw $1,1*4($2) */
 785                 MIPS32_LW(15,15*4,2),                                                   /* lw $15,15*4($2) */
 786                 MIPS32_MTC0(15,31,0),                                                   /* move $15 to COP0 DeSave */
 787                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),     /* $15 = MIPS32_PRACC_STACK */
 788                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 789                 MIPS32_SW(1,0,15),                                                              /* sw $1,($15) */
 790                 MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_IN)),   /* $1 = MIPS32_PRACC_PARAM_IN */
 791                 MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_IN)),
 792                 MIPS32_LW(3,3*4,1),                                                     /* lw $3,3*4($1) */
 793                 MIPS32_LW(4,4*4,1),                                                     /* lw $4,4*4($1) */
 794                 MIPS32_LW(5,5*4,1),                                                     /* lw $5,5*4($1) */
 795                 MIPS32_LW(6,6*4,1),                                                     /* lw $6,6*4($1) */
 796                 MIPS32_LW(7,7*4,1),                                                     /* lw $7,7*4($1) */
 797                 MIPS32_LW(8,8*4,1),                                                     /* lw $8,8*4($1) */
 798                 MIPS32_LW(9,9*4,1),                                                     /* lw $9,9*4($1) */
 799                 MIPS32_LW(10,10*4,1),                                                   /* lw $10,10*4($1) */
 800                 MIPS32_LW(11,11*4,1),                                                   /* lw $11,11*4($1) */
 801                 MIPS32_LW(12,12*4,1),                                                   /* lw $12,12*4($1) */
 802                 MIPS32_LW(13,13*4,1),                                                   /* lw $13,13*4($1) */
 803                 MIPS32_LW(14,14*4,1),                                                   /* lw $14,14*4($1) */
 804                 MIPS32_LW(16,16*4,1),                                                   /* lw $16,16*4($1) */
 805                 MIPS32_LW(17,17*4,1),                                                   /* lw $17,17*4($1) */
 806                 MIPS32_LW(18,18*4,1),                                                   /* lw $18,18*4($1) */
 807                 MIPS32_LW(19,19*4,1),                                                   /* lw $19,19*4($1) */
 808                 MIPS32_LW(20,20*4,1),                                                   /* lw $20,20*4($1) */
 809                 MIPS32_LW(21,21*4,1),                                                   /* lw $21,21*4($1) */
 810                 MIPS32_LW(22,22*4,1),                                                   /* lw $22,22*4($1) */
 811                 MIPS32_LW(23,23*4,1),                                                   /* lw $23,23*4($1) */
 812                 MIPS32_LW(24,24*4,1),                                                   /* lw $24,24*4($1) */
 813                 MIPS32_LW(25,25*4,1),                                                   /* lw $25,25*4($1) */
 814                 MIPS32_LW(26,26*4,1),                                                   /* lw $26,26*4($1) */
 815                 MIPS32_LW(27,27*4,1),                                                   /* lw $27,27*4($1) */
 816                 MIPS32_LW(28,28*4,1),                                                   /* lw $28,28*4($1) */
 817                 MIPS32_LW(29,29*4,1),                                                   /* lw $29,29*4($1) */
 818                 MIPS32_LW(30,30*4,1),                                                   /* lw $30,30*4($1) */
 819                 MIPS32_LW(31,31*4,1),                                                   /* lw $31,31*4($1) */
 820
 821                 MIPS32_LW(2,32*4,1),                                                    /* lw $2,32*4($1) */
 822                 MIPS32_MTC0(2,12,0),                                                    /* move $2 to status */
 823                 MIPS32_LW(2,33*4,1),                                                    /* lw $2,33*4($1) */
 824                 MIPS32_MTLO(2),                                                                 /* move $2 to lo */
 825                 MIPS32_LW(2,34*4,1),                                                    /* lw $2,34*4($1) */
 826                 MIPS32_MTHI(2),                                                                 /* move $2 to hi */
 827                 MIPS32_LW(2,35*4,1),                                                    /* lw $2,35*4($1) */
 828                 MIPS32_MTC0(2,8,0),                                                             /* move $2 to badvaddr */
 829                 MIPS32_LW(2,36*4,1),                                                    /* lw $2,36*4($1) */
 830                 MIPS32_MTC0(2,13,0),                                                    /* move $2 to cause*/
 831                 MIPS32_LW(2,37*4,1),                                                    /* lw $2,37*4($1) */
 832                 MIPS32_MTC0(2,24,0),                                                    /* move $2 to pc */
 833
 834                 MIPS32_LW(2,2*4,1),                                                     /* lw $2,2*4($1) */
 835                 MIPS32_LW(1,0,15),                                                              /* lw $1,($15) */
 836                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
 837                 MIPS32_NOP,
 838                 MIPS32_B(NEG16(55)),                                                    /* b start */
 839                 MIPS32_NOP,
 840         };
 841
 842         int retval;
 843
 844         retval = mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code, \
 845                 38, regs, 0, NULL, 1);
 846
 847         return retval;
 848 }
 849
 850 int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 851 {
 852         uint32_t code[] = {
 853                                                                                                                 /* start: */
 854                 MIPS32_MTC0(2,31,0),                                                    /* move $2 to COP0 DeSave */
 855                 MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_OUT)),  /* $2 = MIPS32_PRACC_PARAM_OUT */
 856                 MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 857                 MIPS32_SW(0,0*4,2),                                                             /* sw $0,0*4($2) */
 858                 MIPS32_SW(1,1*4,2),                                                     /* sw $1,1*4($2) */
 859                 MIPS32_SW(15,15*4,2),                                                   /* sw $15,15*4($2) */
 860                 MIPS32_MFC0(2,31,0),                                                    /* move COP0 DeSave to $2 */
 861                 MIPS32_MTC0(15,31,0),                                                   /* move $15 to COP0 DeSave */
 862                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),     /* $15 = MIPS32_PRACC_STACK */
 863                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 864                 MIPS32_SW(1,0,15),                                                              /* sw $1,($15) */
 865                 MIPS32_SW(2,0,15),                                                              /* sw $2,($15) */
 866                 MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_OUT)),  /* $1 = MIPS32_PRACC_PARAM_OUT */
 867                 MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 868                 MIPS32_SW(2,2*4,1),                                                     /* sw $2,2*4($1) */
 869                 MIPS32_SW(3,3*4,1),                                                     /* sw $3,3*4($1) */
 870                 MIPS32_SW(4,4*4,1),                                                     /* sw $4,4*4($1) */
 871                 MIPS32_SW(5,5*4,1),                                                     /* sw $5,5*4($1) */
 872                 MIPS32_SW(6,6*4,1),                                                     /* sw $6,6*4($1) */
 873                 MIPS32_SW(7,7*4,1),                                                     /* sw $7,7*4($1) */
 874                 MIPS32_SW(8,8*4,1),                                                     /* sw $8,8*4($1) */
 875                 MIPS32_SW(9,9*4,1),                                                     /* sw $9,9*4($1) */
 876                 MIPS32_SW(10,10*4,1),                                                   /* sw $10,10*4($1) */
 877                 MIPS32_SW(11,11*4,1),                                                   /* sw $11,11*4($1) */
 878                 MIPS32_SW(12,12*4,1),                                                   /* sw $12,12*4($1) */
 879                 MIPS32_SW(13,13*4,1),                                                   /* sw $13,13*4($1) */
 880                 MIPS32_SW(14,14*4,1),                                                   /* sw $14,14*4($1) */
 881                 MIPS32_SW(16,16*4,1),                                                   /* sw $16,16*4($1) */
 882                 MIPS32_SW(17,17*4,1),                                                   /* sw $17,17*4($1) */
 883                 MIPS32_SW(18,18*4,1),                                                   /* sw $18,18*4($1) */
 884                 MIPS32_SW(19,19*4,1),                                                   /* sw $19,19*4($1) */
 885                 MIPS32_SW(20,20*4,1),                                                   /* sw $20,20*4($1) */
 886                 MIPS32_SW(21,21*4,1),                                                   /* sw $21,21*4($1) */
 887                 MIPS32_SW(22,22*4,1),                                                   /* sw $22,22*4($1) */
 888                 MIPS32_SW(23,23*4,1),                                                   /* sw $23,23*4($1) */
 889                 MIPS32_SW(24,24*4,1),                                                   /* sw $24,24*4($1) */
 890                 MIPS32_SW(25,25*4,1),                                                   /* sw $25,25*4($1) */
 891                 MIPS32_SW(26,26*4,1),                                                   /* sw $26,26*4($1) */
 892                 MIPS32_SW(27,27*4,1),                                                   /* sw $27,27*4($1) */
 893                 MIPS32_SW(28,28*4,1),                                                   /* sw $28,28*4($1) */
 894                 MIPS32_SW(29,29*4,1),                                                   /* sw $29,29*4($1) */
 895                 MIPS32_SW(30,30*4,1),                                                   /* sw $30,30*4($1) */
 896                 MIPS32_SW(31,31*4,1),                                                   /* sw $31,31*4($1) */
 897
 898                 MIPS32_MFC0(2,12,0),                                                    /* move status to $2 */
 899                 MIPS32_SW(2,32*4,1),                                                    /* sw $2,32*4($1) */
 900                 MIPS32_MFLO(2),                                                                 /* move lo to $2 */
 901                 MIPS32_SW(2,33*4,1),                                                    /* sw $2,33*4($1) */
 902                 MIPS32_MFHI(2),                                                                 /* move hi to $2 */
 903                 MIPS32_SW(2,34*4,1),                                                    /* sw $2,34*4($1) */
 904                 MIPS32_MFC0(2,8,0),                                                             /* move badvaddr to $2 */
 905                 MIPS32_SW(2,35*4,1),                                                    /* sw $2,35*4($1) */
 906                 MIPS32_MFC0(2,13,0),                                                    /* move cause to $2 */
 907                 MIPS32_SW(2,36*4,1),                                                    /* sw $2,36*4($1) */
 908                 MIPS32_MFC0(2,24,0),                                                    /* move pc to $2 */
 909                 MIPS32_SW(2,37*4,1),                                                    /* sw $2,37*4($1) */
 910
 911                 MIPS32_LW(2,0,15),                                                              /* lw $2,($15) */
 912                 MIPS32_LW(1,0,15),                                                              /* lw $1,($15) */
 913                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
 914                 MIPS32_NOP,
 915                 MIPS32_B(NEG16(60)),                                                    /* b start */
 916                 MIPS32_NOP,
 917         };
 918
 919         int retval;
 920
 921         retval = mips32_pracc_exec(ejtag_info, sizeof(code)/sizeof(code[0]), code, \
 922                 0, NULL, 38, regs, 1);
 923
 924         return retval;
 925 }