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

   1 /***************************************************************************
   2  *   Copyright (C) 2008 by Spencer Oliver                                  *
   3  *   spen@spen-soft.co.uk                                                  *
   4  *                                                                         *
   5  *   Copyright (C) 2008 by David T.L. Wong                                 *
   6  *                                                                         *
   7  *   Copyright (C) 2009 by David N. Claffey <dnclaffey@gmail.com>          *
   8  *                                                                         *
   9  *   This program is free software; you can redistribute it and/or modify  *
  10  *   it under the terms of the GNU General Public License as published by  *
  11  *   the Free Software Foundation; either version 2 of the License, or     *
  12  *   (at your option) any later version.                                   *
  13  *                                                                         *
  14  *   This program is distributed in the hope that it will be useful,       *
  15  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  16  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  17  *   GNU General Public License for more details.                          *
  18  *                                                                         *
  19  *   You should have received a copy of the GNU General Public License     *
  20  *   along with this program; if not, write to the                         *
  21  *   Free Software Foundation, Inc.,                                       *
  22  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  23  ***************************************************************************/
  24
  25 /*
  26 This version has optimized assembly routines for 32 bit operations:
  27 - read word
  28 - write word
  29 - write array of words
  30
  31 One thing to be aware of is that the MIPS32 cpu will execute the
  32 instruction after a branch instruction (one delay slot).
  33
  34 For example:
  35
  36     LW $2, ($5 +10)
  37     B foo
  38     LW $1, ($2 +100)
  39
  40 The LW $1, ($2 +100) instruction is also executed. If this is
  41 not wanted a NOP can be inserted:
  42
  43     LW $2, ($5 +10)
  44     B foo
  45     NOP
  46     LW $1, ($2 +100)
  47
  48 or the code can be changed to:
  49
  50     B foo
  51     LW $2, ($5 +10)
  52     LW $1, ($2 +100)
  53
  54 The original code contained NOPs. I have removed these and moved
  55 the branches.
  56
  57 I also moved the PRACC_STACK to 0xFF204000. This allows
  58 the use of 16 bits offsets to get pointers to the input
  59 and output area relative to the stack. Note that the stack
  60 isn't really a stack (the stack pointer is not 'moving')
  61 but a FIFO simulated in software.
  62
  63 These changes result in a 35% speed increase when programming an
  64 external flash.
  65
  66 More improvement could be gained if the registers do no need
  67 to be preserved but in that case the routines should be aware
  68 OpenOCD is used as a flash programmer or as a debug tool.
  69
  70 Nico Coesel
  71 */
  72
  73 #ifdef HAVE_CONFIG_H
  74 #include "config.h"
  75 #endif
  76
  77 #include "mips32.h"
  78 #include "mips32_pracc.h"
  79
  80 struct mips32_pracc_context
  81 {
  82         uint32_t *local_iparam;
  83         int num_iparam;
  84         uint32_t *local_oparam;
  85         int num_oparam;
  86         const uint32_t *code;
  87         int code_len;
  88         uint32_t stack[32];
  89         int stack_offset;
  90         struct mips_ejtag *ejtag_info;
  91 };
  92
  93 static int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info,
  94                 uint32_t addr, int count, uint8_t *buf);
  95 static int mips32_pracc_read_mem16(struct mips_ejtag *ejtag_info,
  96                 uint32_t addr, int count, uint16_t *buf);
  97 static int mips32_pracc_read_mem32(struct mips_ejtag *ejtag_info,
  98                 uint32_t addr, int count, uint32_t *buf);
  99 static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info,
 100                 uint32_t addr, uint32_t *buf);
 101
 102 static int mips32_pracc_write_mem8(struct mips_ejtag *ejtag_info,
 103                 uint32_t addr, int count, uint8_t *buf);
 104 static int mips32_pracc_write_mem16(struct mips_ejtag *ejtag_info,
 105                 uint32_t addr, int count, uint16_t *buf);
 106 static int mips32_pracc_write_mem32(struct mips_ejtag *ejtag_info,
 107                 uint32_t addr, int count, uint32_t *buf);
 108 static int mips32_pracc_write_u32(struct mips_ejtag *ejtag_info,
 109                 uint32_t addr, uint32_t *buf);
 110
 111 static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
 112 {
 113         uint32_t ejtag_ctrl;
 114
 115         while (1)
 116         {
 117                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
 118                 ejtag_ctrl = ejtag_info->ejtag_ctrl;
 119                 mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
 120                 if (ejtag_ctrl & EJTAG_CTRL_PRACC)
 121                         break;
 122                 LOG_DEBUG("DEBUGMODULE: No memory access in progress!");
 123                 return ERROR_JTAG_DEVICE_ERROR;
 124         }
 125
 126         *ctrl = ejtag_ctrl;
 127         return ERROR_OK;
 128 }
 129
 130 static int mips32_pracc_exec_read(struct mips32_pracc_context *ctx, uint32_t address)
 131 {
 132         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
 133         int offset;
 134         uint32_t ejtag_ctrl, data;
 135
 136         if ((address >= MIPS32_PRACC_PARAM_IN)
 137                 && (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
 138         {
 139                 offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
 140                 data = ctx->local_iparam[offset];
 141         }
 142         else if ((address >= MIPS32_PRACC_PARAM_OUT)
 143                 && (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
 144         {
 145                 offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
 146                 data = ctx->local_oparam[offset];
 147         }
 148         else if ((address >= MIPS32_PRACC_TEXT)
 149                 && (address <= MIPS32_PRACC_TEXT + ctx->code_len * 4))
 150         {
 151                 offset = (address - MIPS32_PRACC_TEXT) / 4;
 152                 data = ctx->code[offset];
 153         }
 154         else if (address == MIPS32_PRACC_STACK)
 155         {
 156                 /* save to our debug stack */
 157                 data = ctx->stack[--ctx->stack_offset];
 158         }
 159         else
 160         {
 161                 /* TODO: send JMP 0xFF200000 instruction. Hopefully processor jump back
 162                  * to start of debug vector */
 163
 164                 data = 0;
 165                 LOG_ERROR("Error reading unexpected address 0x%8.8" PRIx32 "", address);
 166                 return ERROR_JTAG_DEVICE_ERROR;
 167         }
 168
 169         /* Send the data out */
 170         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA);
 171         mips_ejtag_drscan_32(ctx->ejtag_info, &data);
 172
 173         /* Clear the access pending bit (let the processor eat!) */
 174         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 175         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL);
 176         mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
 177
 178         jtag_add_clocks(5);
 179         return jtag_execute_queue();
 180 }
 181
 182 static int mips32_pracc_exec_write(struct mips32_pracc_context *ctx, uint32_t address)
 183 {
 184         uint32_t ejtag_ctrl,data;
 185         int offset;
 186         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
 187
 188         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA);
 189         mips_ejtag_drscan_32(ctx->ejtag_info, &data);
 190
 191         /* Clear access pending bit */
 192         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 193         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL);
 194         mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
 195
 196         jtag_add_clocks(5);
 197         int retval;
 198         retval = jtag_execute_queue();
 199         if (retval != ERROR_OK)
 200                 return retval;
 201
 202         if ((address >= MIPS32_PRACC_PARAM_IN)
 203                 && (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
 204         {
 205                 offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
 206                 ctx->local_iparam[offset] = data;
 207         }
 208         else if ((address >= MIPS32_PRACC_PARAM_OUT)
 209                 && (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
 210         {
 211                 offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
 212                 ctx->local_oparam[offset] = data;
 213         }
 214         else if (address == MIPS32_PRACC_STACK)
 215         {
 216                 /* save data onto our stack */
 217                 ctx->stack[ctx->stack_offset++] = data;
 218         }
 219         else
 220         {
 221                 LOG_ERROR("Error writing unexpected address 0x%8.8" PRIx32 "", address);
 222                 return ERROR_JTAG_DEVICE_ERROR;
 223         }
 224
 225         return ERROR_OK;
 226 }
 227
 228 int mips32_pracc_exec(struct mips_ejtag *ejtag_info, int code_len, const uint32_t *code,
 229                 int num_param_in, uint32_t *param_in, int num_param_out, uint32_t *param_out, int cycle)
 230 {
 231         uint32_t ejtag_ctrl;
 232         uint32_t address, data;
 233         struct mips32_pracc_context ctx;
 234         int retval;
 235         int pass = 0;
 236
 237         ctx.local_iparam = param_in;
 238         ctx.local_oparam = param_out;
 239         ctx.num_iparam = num_param_in;
 240         ctx.num_oparam = num_param_out;
 241         ctx.code = code;
 242         ctx.code_len = code_len;
 243         ctx.ejtag_info = ejtag_info;
 244         ctx.stack_offset = 0;
 245
 246         while (1)
 247         {
 248                 if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
 249                         return retval;
 250
 251                 address = data = 0;
 252                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
 253                 mips_ejtag_drscan_32(ejtag_info, &address);
 254
 255                 /* Check for read or write */
 256                 if (ejtag_ctrl & EJTAG_CTRL_PRNW)
 257                 {
 258                         if ((retval = mips32_pracc_exec_write(&ctx, address)) != ERROR_OK)
 259                                 return retval;
 260                 }
 261                 else
 262                 {
 263                         /* Check to see if its reading at the debug vector. The first pass through
 264                          * the module is always read at the vector, so the first one we allow.  When
 265                          * the second read from the vector occurs we are done and just exit. */
 266                         if ((address == MIPS32_PRACC_TEXT) && (pass++))
 267                         {
 268                                 break;
 269                         }
 270
 271                         if ((retval = mips32_pracc_exec_read(&ctx, address)) != ERROR_OK)
 272                                 return retval;
 273                 }
 274
 275                 if (cycle == 0)
 276                         break;
 277         }
 278
 279         /* stack sanity check */
 280         if (ctx.stack_offset != 0)
 281         {
 282                 LOG_DEBUG("Pracc Stack not zero");
 283         }
 284
 285         return ERROR_OK;
 286 }
 287
 288 int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
 289 {
 290         switch (size)
 291         {
 292                 case 1:
 293                         return mips32_pracc_read_mem8(ejtag_info, addr, count, (uint8_t*)buf);
 294                 case 2:
 295                         return mips32_pracc_read_mem16(ejtag_info, addr, count, (uint16_t*)buf);
 296                 case 4:
 297                         if (count == 1)
 298                                 return mips32_pracc_read_u32(ejtag_info, addr, (uint32_t*)buf);
 299                         else
 300                                 return mips32_pracc_read_mem32(ejtag_info, addr, count, (uint32_t*)buf);
 301         }
 302
 303         return ERROR_OK;
 304 }
 305
 306 static int mips32_pracc_read_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
 307 {
 308         static const uint32_t code[] = {
 309                                                                                                                         /* start: */
 310                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 311                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 312                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 313                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 314                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 315                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 316                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 317
 318                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 319                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 320                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 321                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 322                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 323                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 324                                                                                                                         /* loop: */
 325                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
 326                 MIPS32_NOP,
 327
 328                 MIPS32_LW(8,0,9),                                                                       /* lw $8,0($9), Load $8 with the word @mem[$9] */
 329                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 330
 331                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 332                 MIPS32_ADDI(9,9,4),                                                                     /* $1 += 4 */
 333                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 334
 335                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 336                 MIPS32_NOP,
 337                                                                                                                         /* end: */
 338                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 339                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 340                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 341                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 342                 MIPS32_B(NEG16(27)),                                                            /* b start */
 343                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 344         };
 345
 346         int retval = ERROR_OK;
 347         int blocksize;
 348         int bytesread;
 349         uint32_t param_in[2];
 350
 351         bytesread = 0;
 352
 353         while (count > 0)
 354         {
 355                 blocksize = count;
 356                 if (count > 0x400)
 357                         blocksize = 0x400;
 358
 359                 param_in[0] = addr;
 360                 param_in[1] = blocksize;
 361
 362                 if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 363                         ARRAY_SIZE(param_in), param_in, blocksize, &buf[bytesread], 1)) != ERROR_OK)
 364                 {
 365                         return retval;
 366                 }
 367
 368                 count -= blocksize;
 369                 addr += blocksize;
 370                 bytesread += blocksize;
 371         }
 372
 373         return retval;
 374 }
 375
 376 static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 377 {
 378         static const uint32_t code[] = {
 379                                                                                                                         /* start: */
 380                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 381                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 382                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 383                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 384
 385                 MIPS32_LW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN),15), /* load R8 @ param_in[0] = address */
 386
 387                 MIPS32_LW(8,0,8),                                                                       /* lw $8,0($8), Load $8 with the word @mem[$8] */
 388                 MIPS32_SW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_OUT),15), /* store R8 @ param_out[0] */
 389
 390                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 391                 MIPS32_B(NEG16(9)),                                                                     /* b start */
 392                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 393         };
 394
 395         int retval = ERROR_OK;
 396         uint32_t param_in[1];
 397
 398         param_in[0] = addr;
 399
 400         if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 401                 ARRAY_SIZE(param_in), param_in, 1, buf, 1)) != ERROR_OK)
 402         {
 403                 return retval;
 404         }
 405
 406         return retval;
 407 }
 408
 409 static int mips32_pracc_read_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
 410 {
 411         static const uint32_t code[] = {
 412                                                                                                                         /* start: */
 413                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 414                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 415                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 416                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 417                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 418                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 419                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 420
 421                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 422                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 423                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 424                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 425                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 426                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 427                                                                                                                         /* loop: */
 428                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
 429                 MIPS32_NOP,
 430
 431                 MIPS32_LHU(8,0,9),                                                                      /* lw $8,0($9), Load $8 with the halfword @mem[$9] */
 432                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 433
 434                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 435                 MIPS32_ADDI(9,9,2),                                                                     /* $9 += 2 */
 436                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 437                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 438                 MIPS32_NOP,
 439                                                                                                                         /* end: */
 440                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 441                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 442                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 443                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 444                 MIPS32_B(NEG16(27)),                                                            /* b start */
 445                 MIPS32_MFC0(15,30,0),                                                           /* move COP0 DeSave to $15 */
 446         };
 447
 448         /* TODO remove array */
 449         uint32_t *param_out = malloc(count * sizeof(uint32_t));
 450         int i;
 451
 452 //      int retval;
 453         int blocksize;
 454         int bytesread;
 455         uint32_t param_in[2];
 456
 457         bytesread = 0;
 458
 459         //while (count > 0)
 460         {
 461                 blocksize = count;
 462                 if (count > 0x400)
 463                         blocksize = 0x400;
 464
 465                 param_in[0] = addr;
 466                 param_in[1] = blocksize;
 467
 468                 mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
 469                         ARRAY_SIZE(param_in), param_in, count, param_out, 1);
 470
 471 //              count -= blocksize;
 472 //              addr += blocksize;
 473 //              bytesread += blocksize;
 474         }
 475
 476         for (i = 0; i < count; i++)
 477         {
 478                 buf[i] = param_out[i];
 479         }
 480
 481         free(param_out);
 482
 483         return ERROR_OK;
 484 }
 485
 486 static int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
 487 {
 488         static const uint32_t code[] = {
 489                                                                                                                         /* start: */
 490                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 491                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 492                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 493                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 494                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 495                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 496                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 497
 498                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 499                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 500                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 501                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 502                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 503                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 504                                                                                                                         /* loop: */
 505                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
 506                 MIPS32_NOP,
 507
 508                 MIPS32_LBU(8,0,9),                                                                      /* lw $8,0($9), Load t4 with the byte @mem[t1] */
 509                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 510
 511                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 512                 MIPS32_ADDI(9,9,1),                                                                     /* $9 += 1 */
 513                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 514                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 515                 MIPS32_NOP,
 516                                                                                                                         /* end: */
 517                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 518                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 519                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 520                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 521                 MIPS32_B(NEG16(27)),                                                            /* b start */
 522                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 523         };
 524
 525         /* TODO remove array */
 526         uint32_t *param_out = malloc(count * sizeof(uint32_t));
 527         int i;
 528
 529 //      int retval;
 530         int blocksize;
 531         int bytesread;
 532         uint32_t param_in[2];
 533
 534         bytesread = 0;
 535
 536 //      while (count > 0)
 537         {
 538                 blocksize = count;
 539                 if (count > 0x400)
 540                         blocksize = 0x400;
 541
 542                 param_in[0] = addr;
 543                 param_in[1] = blocksize;
 544
 545                 mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
 546                         ARRAY_SIZE(param_in), param_in, count, param_out, 1);
 547
 548 //              count -= blocksize;
 549 //              addr += blocksize;
 550 //              bytesread += blocksize;
 551         }
 552
 553         for (i = 0; i < count; i++)
 554         {
 555                 buf[i] = param_out[i];
 556         }
 557
 558         free(param_out);
 559
 560         return ERROR_OK;
 561 }
 562
 563 int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
 564 {
 565         switch (size)
 566         {
 567                 case 1:
 568                         return mips32_pracc_write_mem8(ejtag_info, addr, count, (uint8_t*)buf);
 569                 case 2:
 570                         return mips32_pracc_write_mem16(ejtag_info, addr, count,(uint16_t*)buf);
 571                 case 4:
 572                         if (count == 1)
 573                                 return mips32_pracc_write_u32(ejtag_info, addr, (uint32_t*)buf);
 574                         else
 575                                 return mips32_pracc_write_mem32(ejtag_info, addr, count, (uint32_t*)buf);
 576         }
 577
 578         return ERROR_OK;
 579 }
 580
 581 static int mips32_pracc_write_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
 582 {
 583         static const uint32_t code[] = {
 584                                                                                                                         /* start: */
 585                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 586                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 587                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 588                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 589                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 590                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 591                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 592
 593                 MIPS32_ADDI(8,15,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)),  /* $8= MIPS32_PRACC_PARAM_IN */
 594                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
 595                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
 596                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 beginning of data */
 597
 598                                                                                                                         /* loop: */
 599                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
 600                 MIPS32_SW(11,0,9),                                                                      /* sw $11,0($9) */
 601
 602                 MIPS32_ADDI(9,9,4),                                                                     /* $9 += 4 */
 603                 MIPS32_BNE(10,9,NEG16(4)),                                                      /* bne $10, $9, loop */
 604                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
 605
 606                                                                                                                         /* end: */
 607                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 608                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 609                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 610                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 611                 MIPS32_B(NEG16(21)),                                                            /* b start */
 612                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 613         };
 614
 615         /* TODO remove array */
 616         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
 617         param_in[0] = addr;
 618         param_in[1] = addr + (count * sizeof(uint32_t));        /* last address */
 619
 620         memcpy(&param_in[2], buf, count * sizeof(uint32_t));
 621
 622         mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
 623                 count + 2, param_in, 0, NULL, 1);
 624
 625         free(param_in);
 626
 627         return ERROR_OK;
 628 }
 629
 630 static int mips32_pracc_write_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 631 {
 632         static const uint32_t code[] = {
 633                                                                                                                         /* start: */
 634                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 635                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 636                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 637                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 638                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 639
 640                 MIPS32_LW(8,NEG16((MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)-4), 15),   /* load R8 @ param_in[1] = data */
 641                 MIPS32_LW(9,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN), 15),               /* load R9 @ param_in[0] = address */
 642
 643                 MIPS32_SW(8,0,9),                                                                       /* sw $8,0($9) */
 644
 645                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 646                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 647                 MIPS32_B(NEG16(11)),                                                            /* b start */
 648                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 649         };
 650
 651         /* TODO remove array */
 652         uint32_t param_in[1 + 1];
 653         param_in[0] = addr;
 654         param_in[1] = *buf;
 655
 656         mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
 657                 ARRAY_SIZE(param_in), param_in, 0, NULL, 1);
 658
 659         return ERROR_OK;
 660 }
 661
 662 static int mips32_pracc_write_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
 663 {
 664         static const uint32_t code[] = {
 665                                                                                                                         /* start: */
 666                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 667                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 668                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 669                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 670                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 671                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 672                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 673
 674                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 675                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 676                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
 677                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
 678                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 */
 679                                                                                                                         /* loop: */
 680                 MIPS32_BEQ(0,10,8),                                                                     /* beq $0, $10, end */
 681                 MIPS32_NOP,
 682
 683                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
 684                 MIPS32_SH(11,0,9),                                                                      /* sh $11,0($9) */
 685
 686                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 687                 MIPS32_ADDI(9,9,2),                                                                     /* $9 += 2 */
 688                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
 689
 690                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 691                 MIPS32_NOP,
 692                                                                                                                         /* end: */
 693                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 694                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 695                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 696                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 697                 MIPS32_B(NEG16(26)),                                                            /* b start */
 698                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 699         };
 700
 701         /* TODO remove array */
 702         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
 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, ARRAY_SIZE(code), code, \
 713                 count + 2, param_in, 0, NULL, 1);
 714
 715         free(param_in);
 716
 717         return ERROR_OK;
 718 }
 719
 720 static int mips32_pracc_write_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
 721 {
 722         static const uint32_t code[] = {
 723                                                                                                                         /* start: */
 724                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 725                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 726                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 727                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 728                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 729                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 730                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 731
 732                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 733                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 734                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
 735                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
 736                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 */
 737                                                                                                                         /* loop: */
 738                 MIPS32_BEQ(0,10,8),                                                                     /* beq $0, $10, end */
 739                 MIPS32_NOP,
 740
 741                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
 742                 MIPS32_SB(11,0,9),                                                                      /* sb $11,0($9) */
 743
 744                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 745                 MIPS32_ADDI(9,9,1),                                                                     /* $9 += 1 */
 746                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
 747
 748                 MIPS32_B(NEG16(8)),                                                                     /* 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_B(NEG16(26)),                                                            /* b start */
 756                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 757         };
 758
 759         /* TODO remove array */
 760         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
 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, ARRAY_SIZE(code), code, \
 772                 count + 2, param_in, 0, NULL, 1);
 773
 774         free(param_in);
 775
 776         return retval;
 777 }
 778
 779 int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 780 {
 781         static const uint32_t 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 depc (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_B(NEG16(53)),                                                    /* b start */
 838                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
 839         };
 840
 841         int retval;
 842
 843         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
 844                         MIPS32NUMCOREREGS, regs, 0, NULL, 1);
 845
 846         return retval;
 847 }
 848
 849 int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 850 {
 851         static const 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 depc (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_B(NEG16(58)),                                                    /* b start */
 913                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
 914         };
 915
 916         int retval;
 917
 918         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
 919                 0, NULL, MIPS32NUMCOREREGS, regs, 1);
 920
 921         return retval;
 922 }
 923
 924 /* fastdata upload/download requires an initialized working area
 925  * to load the download code; it should not be called otherwise
 926  * fetch order from the fastdata area
 927  * 1. start addr
 928  * 2. end addr
 929  * 3. data ...
 930  */
 931 int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_area *source,
 932                                                                 int write_t, uint32_t addr, int count, uint32_t *buf)
 933 {
 934         uint32_t handler_code[] = {
 935                 /* caution when editing, table is modified below */
 936                 /* r15 points to the start of this code */
 937                 MIPS32_SW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
 938                 MIPS32_SW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
 939                 MIPS32_SW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
 940                 MIPS32_SW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
 941                 /* start of fastdata area in t0 */
 942                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
 943                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
 944                 MIPS32_LW(9,0,8),                                                               /* start addr in t1 */
 945                 MIPS32_LW(10,0,8),                                                              /* end addr to t2 */
 946                                                                                                                 /* loop: */
 947                 /* 8 */ MIPS32_LW(11,0,0),                                              /* lw t3,[t8 | r9] */
 948                 /* 9 */ MIPS32_SW(11,0,0),                                              /* sw t3,[r9 | r8] */
 949                 MIPS32_BNE(10,9,NEG16(3)),                                              /* bne $t2,t1,loop */
 950                 MIPS32_ADDI(9,9,4),                                                             /* addi t1,t1,4 */
 951
 952                 MIPS32_LW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
 953                 MIPS32_LW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
 954                 MIPS32_LW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
 955                 MIPS32_LW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
 956
 957                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_TEXT)),
 958                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_TEXT)),
 959                 MIPS32_JR(15),                                                                  /* jr start */
 960                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
 961         };
 962
 963         uint32_t jmp_code[] = {
 964                 MIPS32_MTC0(15,31,0),                   /* move $15 to COP0 DeSave */
 965                 /* 1 */ MIPS32_LUI(15,0),               /* addr of working area added below */
 966                 /* 2 */ MIPS32_ORI(15,15,0),    /* addr of working area added below */
 967                 MIPS32_JR(15),                                  /* jump to ram program */
 968                 MIPS32_NOP,
 969         };
 970
 971         int retval, i;
 972         uint32_t val, ejtag_ctrl, address;
 973
 974         if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
 975                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 976
 977         if (write_t)
 978         {
 979                 handler_code[8] = MIPS32_LW(11,0,8);    /* load data from probe at fastdata area */
 980                 handler_code[9] = MIPS32_SW(11,0,9);    /* store data to RAM @ r9 */
 981         }
 982         else
 983         {
 984                 handler_code[8] = MIPS32_LW(11,0,9);    /* load data from RAM @ r9 */
 985                 handler_code[9] = MIPS32_SW(11,0,8);    /* store data to probe at fastdata area */
 986         }
 987
 988         /* write program into RAM */
 989         if (write_t != ejtag_info->fast_access_save)
 990         {
 991                 mips32_pracc_write_mem32(ejtag_info, source->address, ARRAY_SIZE(handler_code), handler_code);
 992                 /* save previous operation to speed to any consecutive read/writes */
 993                 ejtag_info->fast_access_save = write_t;
 994         }
 995
 996         LOG_DEBUG("%s using 0x%.8" PRIx32 " for write handler", __func__, source->address);
 997
 998         jmp_code[1] |= UPPER16(source->address);
 999         jmp_code[2] |= LOWER16(source->address);
1000
1001         for (i = 0; i < (int) ARRAY_SIZE(jmp_code); i++)
1002         {
1003                 if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1004                         return retval;
1005
1006                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_DATA);
1007                 mips_ejtag_drscan_32(ejtag_info, &jmp_code[i]);
1008
1009                 /* Clear the access pending bit (let the processor eat!) */
1010                 ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
1011                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
1012                 mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
1013         }
1014
1015         if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1016                 return retval;
1017
1018         /* next fetch to dmseg should be in FASTDATA_AREA, check */
1019         address = 0;
1020         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1021         mips_ejtag_drscan_32(ejtag_info, &address);
1022
1023         if (address != MIPS32_PRACC_FASTDATA_AREA)
1024                 return ERROR_FAIL;
1025
1026         /* Send the load start address */
1027         val = addr;
1028         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
1029         mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
1030
1031         /* Send the load end address */
1032         val = addr + (count - 1) * 4;
1033         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
1034         mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
1035
1036         for (i = 0; i < count; i++)
1037         {
1038                 /* Send the data out using fastdata (clears the access pending bit) */
1039                 if ((retval = mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++)) != ERROR_OK)
1040                         return retval;
1041         }
1042
1043         if ((retval = jtag_execute_queue()) != ERROR_OK)
1044         {
1045                 LOG_ERROR("fastdata load failed");
1046                 return retval;
1047         }
1048
1049         if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1050                 return retval;
1051
1052         address = 0;
1053         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1054         mips_ejtag_drscan_32(ejtag_info, &address);
1055
1056         if (address != MIPS32_PRACC_TEXT)
1057                 LOG_ERROR("mini program did not return to start");
1058
1059         return retval;
1060 }