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