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

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