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