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

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