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