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