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         return jtag_execute_queue();
 198
 199         if ((address >= MIPS32_PRACC_PARAM_IN)
 200                 && (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
 201         {
 202                 offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
 203                 ctx->local_iparam[offset] = data;
 204         }
 205         else if ((address >= MIPS32_PRACC_PARAM_OUT)
 206                 && (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
 207         {
 208                 offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
 209                 ctx->local_oparam[offset] = data;
 210         }
 211         else if (address == MIPS32_PRACC_STACK)
 212         {
 213                 /* save data onto our stack */
 214                 ctx->stack[ctx->stack_offset++] = data;
 215         }
 216         else
 217         {
 218                 LOG_ERROR("Error writing unexpected address 0x%8.8" PRIx32 "", address);
 219                 return ERROR_JTAG_DEVICE_ERROR;
 220         }
 221
 222         return ERROR_OK;
 223 }
 224
 225 int mips32_pracc_exec(struct mips_ejtag *ejtag_info, int code_len, const uint32_t *code,
 226                 int num_param_in, uint32_t *param_in, int num_param_out, uint32_t *param_out, int cycle)
 227 {
 228         uint32_t ejtag_ctrl;
 229         uint32_t address, data;
 230         struct mips32_pracc_context ctx;
 231         int retval;
 232         int pass = 0;
 233
 234         ctx.local_iparam = param_in;
 235         ctx.local_oparam = param_out;
 236         ctx.num_iparam = num_param_in;
 237         ctx.num_oparam = num_param_out;
 238         ctx.code = code;
 239         ctx.code_len = code_len;
 240         ctx.ejtag_info = ejtag_info;
 241         ctx.stack_offset = 0;
 242
 243         while (1)
 244         {
 245                 if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
 246                         return retval;
 247
 248                 address = data = 0;
 249                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
 250                 mips_ejtag_drscan_32(ejtag_info, &address);
 251
 252                 /* Check for read or write */
 253                 if (ejtag_ctrl & EJTAG_CTRL_PRNW)
 254                 {
 255                         if ((retval = mips32_pracc_exec_write(&ctx, address)) != ERROR_OK)
 256                                 return retval;
 257                 }
 258                 else
 259                 {
 260                         /* Check to see if its reading at the debug vector. The first pass through
 261                          * the module is always read at the vector, so the first one we allow.  When
 262                          * the second read from the vector occurs we are done and just exit. */
 263                         if ((address == MIPS32_PRACC_TEXT) && (pass++))
 264                         {
 265                                 break;
 266                         }
 267
 268                         if ((retval = mips32_pracc_exec_read(&ctx, address)) != ERROR_OK)
 269                                 return retval;
 270                 }
 271
 272                 if (cycle == 0)
 273                         break;
 274         }
 275
 276         /* stack sanity check */
 277         if (ctx.stack_offset != 0)
 278         {
 279                 LOG_DEBUG("Pracc Stack not zero");
 280         }
 281
 282         return ERROR_OK;
 283 }
 284
 285 int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
 286 {
 287         switch (size)
 288         {
 289                 case 1:
 290                         return mips32_pracc_read_mem8(ejtag_info, addr, count, (uint8_t*)buf);
 291                 case 2:
 292                         return mips32_pracc_read_mem16(ejtag_info, addr, count, (uint16_t*)buf);
 293                 case 4:
 294                         if (count == 1)
 295                                 return mips32_pracc_read_u32(ejtag_info, addr, (uint32_t*)buf);
 296                         else
 297                                 return mips32_pracc_read_mem32(ejtag_info, addr, count, (uint32_t*)buf);
 298         }
 299
 300         return ERROR_OK;
 301 }
 302
 303 static int mips32_pracc_read_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
 304 {
 305         static const uint32_t code[] = {
 306                                                                                                                         /* start: */
 307                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 308                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 309                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 310                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 311                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 312                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 313                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 314
 315                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 316                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 317                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 318                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 319                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 320                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 321                                                                                                                         /* loop: */
 322                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
 323                 MIPS32_NOP,
 324
 325                 MIPS32_LW(8,0,9),                                                                       /* lw $8,0($9), Load $8 with the word @mem[$9] */
 326                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 327
 328                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 329                 MIPS32_ADDI(9,9,4),                                                                     /* $1 += 4 */
 330                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 331
 332                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 333                 MIPS32_NOP,
 334                                                                                                                         /* end: */
 335                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 336                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 337                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 338                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 339                 MIPS32_B(NEG16(27)),                                                            /* b start */
 340                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 341         };
 342
 343         int retval = ERROR_OK;
 344         int blocksize;
 345         int bytesread;
 346         uint32_t param_in[2];
 347
 348         bytesread = 0;
 349
 350         while (count > 0)
 351         {
 352                 blocksize = count;
 353                 if (count > 0x400)
 354                         blocksize = 0x400;
 355
 356                 param_in[0] = addr;
 357                 param_in[1] = blocksize;
 358
 359                 if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 360                         ARRAY_SIZE(param_in), param_in, blocksize, &buf[bytesread], 1)) != ERROR_OK)
 361                 {
 362                         return retval;
 363                 }
 364
 365                 count -= blocksize;
 366                 addr += blocksize;
 367                 bytesread += blocksize;
 368         }
 369
 370         return retval;
 371 }
 372
 373 static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 374 {
 375         static const uint32_t code[] = {
 376                                                                                                                         /* start: */
 377                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 378                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 379                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 380                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 381
 382                 MIPS32_LW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN),15), /* load R8 @ param_in[0] = address */
 383
 384                 MIPS32_LW(8,0,8),                                                                       /* lw $8,0($8), Load $8 with the word @mem[$8] */
 385                 MIPS32_SW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_OUT),15), /* store R8 @ param_out[0] */
 386
 387                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 388                 MIPS32_B(NEG16(9)),                                                                     /* b start */
 389                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 390         };
 391
 392         int retval = ERROR_OK;
 393         uint32_t param_in[1];
 394
 395         param_in[0] = addr;
 396
 397         if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 398                 ARRAY_SIZE(param_in), param_in, 1, buf, 1)) != ERROR_OK)
 399         {
 400                 return retval;
 401         }
 402
 403         return retval;
 404 }
 405
 406 static int mips32_pracc_read_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
 407 {
 408         static const uint32_t code[] = {
 409                                                                                                                         /* start: */
 410                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 411                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 412                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 413                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 414                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 415                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 416                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 417
 418                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 419                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 420                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 421                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 422                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 423                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 424                                                                                                                         /* loop: */
 425                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
 426                 MIPS32_NOP,
 427
 428                 MIPS32_LHU(8,0,9),                                                                      /* lw $8,0($9), Load $8 with the halfword @mem[$9] */
 429                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 430
 431                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 432                 MIPS32_ADDI(9,9,2),                                                                     /* $9 += 2 */
 433                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 434                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 435                 MIPS32_NOP,
 436                                                                                                                         /* end: */
 437                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 438                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 439                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 440                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 441                 MIPS32_B(NEG16(27)),                                                            /* b start */
 442                 MIPS32_MFC0(15,30,0),                                                           /* move COP0 DeSave to $15 */
 443         };
 444
 445         /* TODO remove array */
 446         uint32_t *param_out = malloc(count * sizeof(uint32_t));
 447         int i;
 448
 449         int retval = ERROR_OK;
 450         int blocksize;
 451         int bytesread;
 452         uint32_t param_in[2];
 453
 454         bytesread = 0;
 455
 456         //while (count > 0)
 457         {
 458                 blocksize = count;
 459                 if (count > 0x400)
 460                         blocksize = 0x400;
 461
 462                 param_in[0] = addr;
 463                 param_in[1] = blocksize;
 464
 465                 retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 466                         ARRAY_SIZE(param_in), param_in, count, param_out, 1);
 467
 468 //              count -= blocksize;
 469 //              addr += blocksize;
 470 //              bytesread += blocksize;
 471         }
 472
 473         for (i = 0; i < count; i++)
 474         {
 475                 buf[i] = param_out[i];
 476         }
 477
 478         free(param_out);
 479
 480         return retval;
 481 }
 482
 483 static int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
 484 {
 485         static const uint32_t code[] = {
 486                                                                                                                         /* start: */
 487                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 488                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 489                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 490                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 491                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 492                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 493                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 494
 495                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 496                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 497                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
 498                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
 499                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
 500                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 501                                                                                                                         /* loop: */
 502                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
 503                 MIPS32_NOP,
 504
 505                 MIPS32_LBU(8,0,9),                                                                      /* lw $8,0($9), Load t4 with the byte @mem[t1] */
 506                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
 507
 508                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 509                 MIPS32_ADDI(9,9,1),                                                                     /* $9 += 1 */
 510                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
 511                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 512                 MIPS32_NOP,
 513                                                                                                                         /* end: */
 514                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 515                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 516                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 517                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 518                 MIPS32_B(NEG16(27)),                                                            /* b start */
 519                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 520         };
 521
 522         /* TODO remove array */
 523         uint32_t *param_out = malloc(count * sizeof(uint32_t));
 524         int i;
 525
 526         int retval = ERROR_OK;
 527         int blocksize;
 528         int bytesread;
 529         uint32_t 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                 retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 543                         ARRAY_SIZE(param_in), 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         free(param_out);
 556
 557         return retval;
 558 }
 559
 560 int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
 561 {
 562         switch (size)
 563         {
 564                 case 1:
 565                         return mips32_pracc_write_mem8(ejtag_info, addr, count, (uint8_t*)buf);
 566                 case 2:
 567                         return mips32_pracc_write_mem16(ejtag_info, addr, count,(uint16_t*)buf);
 568                 case 4:
 569                         if (count == 1)
 570                                 return mips32_pracc_write_u32(ejtag_info, addr, (uint32_t*)buf);
 571                         else
 572                                 return mips32_pracc_write_mem32(ejtag_info, addr, count, (uint32_t*)buf);
 573         }
 574
 575         return ERROR_OK;
 576 }
 577
 578 static int mips32_pracc_write_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
 579 {
 580         static const uint32_t 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),                                                                      /* 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)),                                                      /* bne $10, $9, loop */
 601                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
 602
 603                                                                                                                         /* end: */
 604                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 605                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 606                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 607                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 608                 MIPS32_B(NEG16(21)),                                                            /* b start */
 609                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 610         };
 611
 612         /* TODO remove array */
 613         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
 614         param_in[0] = addr;
 615         param_in[1] = addr + (count * sizeof(uint32_t));        /* last address */
 616
 617         memcpy(&param_in[2], buf, count * sizeof(uint32_t));
 618
 619         int retval;
 620         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 621                 count + 2, param_in, 0, NULL, 1);
 622
 623         free(param_in);
 624
 625         return retval;
 626 }
 627
 628 static int mips32_pracc_write_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 629 {
 630         static const uint32_t code[] = {
 631                                                                                                                         /* start: */
 632                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 633                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 634                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 635                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 636                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 637
 638                 MIPS32_LW(8,NEG16((MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)-4), 15),   /* load R8 @ param_in[1] = data */
 639                 MIPS32_LW(9,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN), 15),               /* load R9 @ param_in[0] = address */
 640
 641                 MIPS32_SW(8,0,9),                                                                       /* sw $8,0($9) */
 642
 643                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 644                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 645                 MIPS32_B(NEG16(11)),                                                            /* b start */
 646                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 647         };
 648
 649         /* TODO remove array */
 650         uint32_t param_in[1 + 1];
 651         param_in[0] = addr;
 652         param_in[1] = *buf;
 653
 654         return mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 655                 ARRAY_SIZE(param_in), param_in, 0, NULL, 1);
 656 }
 657
 658 static int mips32_pracc_write_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
 659 {
 660         static const uint32_t 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                                                                                                                         /* loop: */
 676                 MIPS32_BEQ(0,10,8),                                                                     /* beq $0, $10, end */
 677                 MIPS32_NOP,
 678
 679                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
 680                 MIPS32_SH(11,0,9),                                                                      /* sh $11,0($9) */
 681
 682                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 683                 MIPS32_ADDI(9,9,2),                                                                     /* $9 += 2 */
 684                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
 685
 686                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 687                 MIPS32_NOP,
 688                                                                                                                         /* end: */
 689                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 690                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 691                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 692                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 693                 MIPS32_B(NEG16(26)),                                                            /* b start */
 694                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 695         };
 696
 697         /* TODO remove array */
 698         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
 699         int i;
 700         param_in[0] = addr;
 701         param_in[1] = count;
 702
 703         for (i = 0; i < count; i++)
 704         {
 705                 param_in[i + 2] = buf[i];
 706         }
 707
 708         int retval;
 709         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 710                 count + 2, param_in, 0, NULL, 1);
 711
 712         free(param_in);
 713
 714         return retval;
 715 }
 716
 717 static int mips32_pracc_write_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
 718 {
 719         static const uint32_t code[] = {
 720                                                                                                                         /* start: */
 721                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
 722                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
 723                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 724                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
 725                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
 726                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
 727                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
 728
 729                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
 730                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
 731                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
 732                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
 733                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 */
 734                                                                                                                         /* loop: */
 735                 MIPS32_BEQ(0,10,8),                                                                     /* beq $0, $10, end */
 736                 MIPS32_NOP,
 737
 738                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
 739                 MIPS32_SB(11,0,9),                                                                      /* sb $11,0($9) */
 740
 741                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
 742                 MIPS32_ADDI(9,9,1),                                                                     /* $9 += 1 */
 743                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
 744
 745                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
 746                 MIPS32_NOP,
 747                                                                                                                         /* end: */
 748                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
 749                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
 750                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
 751                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
 752                 MIPS32_B(NEG16(26)),                                                            /* b start */
 753                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
 754         };
 755
 756         /* TODO remove array */
 757         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
 758         int retval;
 759         int i;
 760         param_in[0] = addr;
 761         param_in[1] = count;
 762
 763         for (i = 0; i < count; i++)
 764         {
 765                 param_in[i + 2] = buf[i];
 766         }
 767
 768         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 769                 count + 2, param_in, 0, NULL, 1);
 770
 771         free(param_in);
 772
 773         return retval;
 774 }
 775
 776 int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 777 {
 778         static const uint32_t code[] = {
 779                                                                                                                 /* start: */
 780                 MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_IN)),   /* $2 = MIPS32_PRACC_PARAM_IN */
 781                 MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_IN)),
 782                 MIPS32_LW(1,1*4,2),                                                             /* lw $1,1*4($2) */
 783                 MIPS32_LW(15,15*4,2),                                                   /* lw $15,15*4($2) */
 784                 MIPS32_MTC0(15,31,0),                                                   /* move $15 to COP0 DeSave */
 785                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
 786                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 787                 MIPS32_SW(1,0,15),                                                              /* sw $1,($15) */
 788                 MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_IN)),   /* $1 = MIPS32_PRACC_PARAM_IN */
 789                 MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_IN)),
 790                 MIPS32_LW(3,3*4,1),                                                             /* lw $3,3*4($1) */
 791                 MIPS32_LW(4,4*4,1),                                                             /* lw $4,4*4($1) */
 792                 MIPS32_LW(5,5*4,1),                                                             /* lw $5,5*4($1) */
 793                 MIPS32_LW(6,6*4,1),                                                             /* lw $6,6*4($1) */
 794                 MIPS32_LW(7,7*4,1),                                                             /* lw $7,7*4($1) */
 795                 MIPS32_LW(8,8*4,1),                                                             /* lw $8,8*4($1) */
 796                 MIPS32_LW(9,9*4,1),                                                             /* lw $9,9*4($1) */
 797                 MIPS32_LW(10,10*4,1),                                                   /* lw $10,10*4($1) */
 798                 MIPS32_LW(11,11*4,1),                                                   /* lw $11,11*4($1) */
 799                 MIPS32_LW(12,12*4,1),                                                   /* lw $12,12*4($1) */
 800                 MIPS32_LW(13,13*4,1),                                                   /* lw $13,13*4($1) */
 801                 MIPS32_LW(14,14*4,1),                                                   /* lw $14,14*4($1) */
 802                 MIPS32_LW(16,16*4,1),                                                   /* lw $16,16*4($1) */
 803                 MIPS32_LW(17,17*4,1),                                                   /* lw $17,17*4($1) */
 804                 MIPS32_LW(18,18*4,1),                                                   /* lw $18,18*4($1) */
 805                 MIPS32_LW(19,19*4,1),                                                   /* lw $19,19*4($1) */
 806                 MIPS32_LW(20,20*4,1),                                                   /* lw $20,20*4($1) */
 807                 MIPS32_LW(21,21*4,1),                                                   /* lw $21,21*4($1) */
 808                 MIPS32_LW(22,22*4,1),                                                   /* lw $22,22*4($1) */
 809                 MIPS32_LW(23,23*4,1),                                                   /* lw $23,23*4($1) */
 810                 MIPS32_LW(24,24*4,1),                                                   /* lw $24,24*4($1) */
 811                 MIPS32_LW(25,25*4,1),                                                   /* lw $25,25*4($1) */
 812                 MIPS32_LW(26,26*4,1),                                                   /* lw $26,26*4($1) */
 813                 MIPS32_LW(27,27*4,1),                                                   /* lw $27,27*4($1) */
 814                 MIPS32_LW(28,28*4,1),                                                   /* lw $28,28*4($1) */
 815                 MIPS32_LW(29,29*4,1),                                                   /* lw $29,29*4($1) */
 816                 MIPS32_LW(30,30*4,1),                                                   /* lw $30,30*4($1) */
 817                 MIPS32_LW(31,31*4,1),                                                   /* lw $31,31*4($1) */
 818
 819                 MIPS32_LW(2,32*4,1),                                                    /* lw $2,32*4($1) */
 820                 MIPS32_MTC0(2,12,0),                                                    /* move $2 to status */
 821                 MIPS32_LW(2,33*4,1),                                                    /* lw $2,33*4($1) */
 822                 MIPS32_MTLO(2),                                                                 /* move $2 to lo */
 823                 MIPS32_LW(2,34*4,1),                                                    /* lw $2,34*4($1) */
 824                 MIPS32_MTHI(2),                                                                 /* move $2 to hi */
 825                 MIPS32_LW(2,35*4,1),                                                    /* lw $2,35*4($1) */
 826                 MIPS32_MTC0(2,8,0),                                                             /* move $2 to badvaddr */
 827                 MIPS32_LW(2,36*4,1),                                                    /* lw $2,36*4($1) */
 828                 MIPS32_MTC0(2,13,0),                                                    /* move $2 to cause*/
 829                 MIPS32_LW(2,37*4,1),                                                    /* lw $2,37*4($1) */
 830                 MIPS32_MTC0(2,24,0),                                                    /* move $2 to depc (pc) */
 831
 832                 MIPS32_LW(2,2*4,1),                                                             /* lw $2,2*4($1) */
 833                 MIPS32_LW(1,0,15),                                                              /* lw $1,($15) */
 834                 MIPS32_B(NEG16(53)),                                                    /* b start */
 835                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
 836         };
 837
 838         int retval;
 839
 840         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 841                         MIPS32NUMCOREREGS, regs, 0, NULL, 1);
 842
 843         return retval;
 844 }
 845
 846 int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 847 {
 848         static const uint32_t code[] = {
 849                                                                                                                 /* start: */
 850                 MIPS32_MTC0(2,31,0),                                                    /* move $2 to COP0 DeSave */
 851                 MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_OUT)),  /* $2 = MIPS32_PRACC_PARAM_OUT */
 852                 MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 853                 MIPS32_SW(0,0*4,2),                                                             /* sw $0,0*4($2) */
 854                 MIPS32_SW(1,1*4,2),                                                             /* sw $1,1*4($2) */
 855                 MIPS32_SW(15,15*4,2),                                                   /* sw $15,15*4($2) */
 856                 MIPS32_MFC0(2,31,0),                                                    /* move COP0 DeSave to $2 */
 857                 MIPS32_MTC0(15,31,0),                                                   /* move $15 to COP0 DeSave */
 858                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
 859                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
 860                 MIPS32_SW(1,0,15),                                                              /* sw $1,($15) */
 861                 MIPS32_SW(2,0,15),                                                              /* sw $2,($15) */
 862                 MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_OUT)),  /* $1 = MIPS32_PRACC_PARAM_OUT */
 863                 MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_OUT)),
 864                 MIPS32_SW(2,2*4,1),                                                             /* sw $2,2*4($1) */
 865                 MIPS32_SW(3,3*4,1),                                                             /* sw $3,3*4($1) */
 866                 MIPS32_SW(4,4*4,1),                                                             /* sw $4,4*4($1) */
 867                 MIPS32_SW(5,5*4,1),                                                             /* sw $5,5*4($1) */
 868                 MIPS32_SW(6,6*4,1),                                                             /* sw $6,6*4($1) */
 869                 MIPS32_SW(7,7*4,1),                                                             /* sw $7,7*4($1) */
 870                 MIPS32_SW(8,8*4,1),                                                             /* sw $8,8*4($1) */
 871                 MIPS32_SW(9,9*4,1),                                                             /* sw $9,9*4($1) */
 872                 MIPS32_SW(10,10*4,1),                                                   /* sw $10,10*4($1) */
 873                 MIPS32_SW(11,11*4,1),                                                   /* sw $11,11*4($1) */
 874                 MIPS32_SW(12,12*4,1),                                                   /* sw $12,12*4($1) */
 875                 MIPS32_SW(13,13*4,1),                                                   /* sw $13,13*4($1) */
 876                 MIPS32_SW(14,14*4,1),                                                   /* sw $14,14*4($1) */
 877                 MIPS32_SW(16,16*4,1),                                                   /* sw $16,16*4($1) */
 878                 MIPS32_SW(17,17*4,1),                                                   /* sw $17,17*4($1) */
 879                 MIPS32_SW(18,18*4,1),                                                   /* sw $18,18*4($1) */
 880                 MIPS32_SW(19,19*4,1),                                                   /* sw $19,19*4($1) */
 881                 MIPS32_SW(20,20*4,1),                                                   /* sw $20,20*4($1) */
 882                 MIPS32_SW(21,21*4,1),                                                   /* sw $21,21*4($1) */
 883                 MIPS32_SW(22,22*4,1),                                                   /* sw $22,22*4($1) */
 884                 MIPS32_SW(23,23*4,1),                                                   /* sw $23,23*4($1) */
 885                 MIPS32_SW(24,24*4,1),                                                   /* sw $24,24*4($1) */
 886                 MIPS32_SW(25,25*4,1),                                                   /* sw $25,25*4($1) */
 887                 MIPS32_SW(26,26*4,1),                                                   /* sw $26,26*4($1) */
 888                 MIPS32_SW(27,27*4,1),                                                   /* sw $27,27*4($1) */
 889                 MIPS32_SW(28,28*4,1),                                                   /* sw $28,28*4($1) */
 890                 MIPS32_SW(29,29*4,1),                                                   /* sw $29,29*4($1) */
 891                 MIPS32_SW(30,30*4,1),                                                   /* sw $30,30*4($1) */
 892                 MIPS32_SW(31,31*4,1),                                                   /* sw $31,31*4($1) */
 893
 894                 MIPS32_MFC0(2,12,0),                                                    /* move status to $2 */
 895                 MIPS32_SW(2,32*4,1),                                                    /* sw $2,32*4($1) */
 896                 MIPS32_MFLO(2),                                                                 /* move lo to $2 */
 897                 MIPS32_SW(2,33*4,1),                                                    /* sw $2,33*4($1) */
 898                 MIPS32_MFHI(2),                                                                 /* move hi to $2 */
 899                 MIPS32_SW(2,34*4,1),                                                    /* sw $2,34*4($1) */
 900                 MIPS32_MFC0(2,8,0),                                                             /* move badvaddr to $2 */
 901                 MIPS32_SW(2,35*4,1),                                                    /* sw $2,35*4($1) */
 902                 MIPS32_MFC0(2,13,0),                                                    /* move cause to $2 */
 903                 MIPS32_SW(2,36*4,1),                                                    /* sw $2,36*4($1) */
 904                 MIPS32_MFC0(2,24,0),                                                    /* move depc (pc) to $2 */
 905                 MIPS32_SW(2,37*4,1),                                                    /* sw $2,37*4($1) */
 906
 907                 MIPS32_LW(2,0,15),                                                              /* lw $2,($15) */
 908                 MIPS32_LW(1,0,15),                                                              /* lw $1,($15) */
 909                 MIPS32_B(NEG16(58)),                                                    /* b start */
 910                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
 911         };
 912
 913         int retval;
 914
 915         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 916                 0, NULL, MIPS32NUMCOREREGS, regs, 1);
 917
 918         return retval;
 919 }
 920
 921 /* fastdata upload/download requires an initialized working area
 922  * to load the download code; it should not be called otherwise
 923  * fetch order from the fastdata area
 924  * 1. start addr
 925  * 2. end addr
 926  * 3. data ...
 927  */
 928 int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_area *source,
 929                                                                 int write_t, uint32_t addr, int count, uint32_t *buf)
 930 {
 931         uint32_t handler_code[] = {
 932                 /* caution when editing, table is modified below */
 933                 /* r15 points to the start of this code */
 934                 MIPS32_SW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
 935                 MIPS32_SW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
 936                 MIPS32_SW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
 937                 MIPS32_SW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
 938                 /* start of fastdata area in t0 */
 939                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
 940                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
 941                 MIPS32_LW(9,0,8),                                                               /* start addr in t1 */
 942                 MIPS32_LW(10,0,8),                                                              /* end addr to t2 */
 943                                                                                                                 /* loop: */
 944                 /* 8 */ MIPS32_LW(11,0,0),                                              /* lw t3,[t8 | r9] */
 945                 /* 9 */ MIPS32_SW(11,0,0),                                              /* sw t3,[r9 | r8] */
 946                 MIPS32_BNE(10,9,NEG16(3)),                                              /* bne $t2,t1,loop */
 947                 MIPS32_ADDI(9,9,4),                                                             /* addi t1,t1,4 */
 948
 949                 MIPS32_LW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
 950                 MIPS32_LW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
 951                 MIPS32_LW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
 952                 MIPS32_LW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
 953
 954                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_TEXT)),
 955                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_TEXT)),
 956                 MIPS32_JR(15),                                                                  /* jr start */
 957                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
 958         };
 959
 960         uint32_t jmp_code[] = {
 961                 MIPS32_MTC0(15,31,0),                   /* move $15 to COP0 DeSave */
 962                 /* 1 */ MIPS32_LUI(15,0),               /* addr of working area added below */
 963                 /* 2 */ MIPS32_ORI(15,15,0),    /* addr of working area added below */
 964                 MIPS32_JR(15),                                  /* jump to ram program */
 965                 MIPS32_NOP,
 966         };
 967
 968         int retval, i;
 969         uint32_t val, ejtag_ctrl, address;
 970
 971         if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
 972                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 973
 974         if (write_t)
 975         {
 976                 handler_code[8] = MIPS32_LW(11,0,8);    /* load data from probe at fastdata area */
 977                 handler_code[9] = MIPS32_SW(11,0,9);    /* store data to RAM @ r9 */
 978         }
 979         else
 980         {
 981                 handler_code[8] = MIPS32_LW(11,0,9);    /* load data from RAM @ r9 */
 982                 handler_code[9] = MIPS32_SW(11,0,8);    /* store data to probe at fastdata area */
 983         }
 984
 985         /* write program into RAM */
 986         if (write_t != ejtag_info->fast_access_save)
 987         {
 988                 mips32_pracc_write_mem32(ejtag_info, source->address, ARRAY_SIZE(handler_code), handler_code);
 989                 /* save previous operation to speed to any consecutive read/writes */
 990                 ejtag_info->fast_access_save = write_t;
 991         }
 992
 993         LOG_DEBUG("%s using 0x%.8" PRIx32 " for write handler", __func__, source->address);
 994
 995         jmp_code[1] |= UPPER16(source->address);
 996         jmp_code[2] |= LOWER16(source->address);
 997
 998         for (i = 0; i < (int) ARRAY_SIZE(jmp_code); i++)
 999         {
1000                 if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1001                         return retval;
1002
1003                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_DATA);
1004                 mips_ejtag_drscan_32(ejtag_info, &jmp_code[i]);
1005
1006                 /* Clear the access pending bit (let the processor eat!) */
1007                 ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
1008                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
1009                 mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
1010         }
1011
1012         if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1013                 return retval;
1014
1015         /* next fetch to dmseg should be in FASTDATA_AREA, check */
1016         address = 0;
1017         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1018         mips_ejtag_drscan_32(ejtag_info, &address);
1019
1020         if (address != MIPS32_PRACC_FASTDATA_AREA)
1021                 return ERROR_FAIL;
1022
1023         /* Send the load start address */
1024         val = addr;
1025         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
1026         mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
1027
1028         /* Send the load end address */
1029         val = addr + (count - 1) * 4;
1030         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
1031         mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
1032
1033         for (i = 0; i < count; i++)
1034         {
1035                 /* Send the data out using fastdata (clears the access pending bit) */
1036                 if ((retval = mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++)) != ERROR_OK)
1037                         return retval;
1038         }
1039
1040         if ((retval = jtag_execute_queue()) != ERROR_OK)
1041         {
1042                 LOG_ERROR("fastdata load failed");
1043                 return retval;
1044         }
1045
1046         if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1047                 return retval;
1048
1049         address = 0;
1050         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1051         mips_ejtag_drscan_32(ejtag_info, &address);
1052
1053         if (address != MIPS32_PRACC_TEXT)
1054                 LOG_ERROR("mini program did not return to start");
1055
1056         return retval;
1057 }