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