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