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