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