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