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