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

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2
   3 /*
   4  * Support for processors implementing MIPS64 instruction set
   5  *
   6  *   Copyright (C) 2014 by Andrey Sidorov <anysidorov@gmail.com>
   7  *   Copyright (C) 2014 by Aleksey Kuleshov <rndfax@yandex.ru>
   8  *   Copyright (C) 2014-2019 by Peter Mamonov <pmamonov@gmail.com>
   9  *
  10  *   Based on the work of:
  11  *       Copyright (C) 2008 by Spencer Oliver
  12  *       Copyright (C) 2008 by David T.L. Wong
  13  *       Copyright (C) 2010 by Konstantin Kostyukhin, Nikolay Shmyrev
  14  */
  15
  16 #ifdef HAVE_CONFIG_H
  17 #include "config.h"
  18 #endif
  19
  20 #include "mips64.h"
  21 #include "mips64_pracc.h"
  22
  23 #include <helper/time_support.h>
  24 #include <jtag/adapter.h>
  25
  26 #define STACK_DEPTH     32
  27
  28 struct mips64_pracc_context {
  29         uint64_t *local_iparam;
  30         unsigned num_iparam;
  31         uint64_t *local_oparam;
  32         unsigned num_oparam;
  33         const uint32_t *code;
  34         unsigned code_len;
  35         uint64_t stack[STACK_DEPTH];
  36         unsigned stack_offset;
  37         struct mips_ejtag *ejtag_info;
  38 };
  39
  40 static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
  41 {
  42         uint32_t ejtag_ctrl;
  43         int nt = 5;
  44         int rc;
  45
  46         while (1) {
  47                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
  48                 ejtag_ctrl = ejtag_info->ejtag_ctrl;
  49                 rc = mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
  50                 if (rc != ERROR_OK)
  51                         return rc;
  52
  53                 if (ejtag_ctrl & EJTAG_CTRL_PRACC)
  54                         break;
  55                 LOG_DEBUG("DEBUGMODULE: No memory access in progress!\n");
  56                 if (nt == 0)
  57                         return ERROR_JTAG_DEVICE_ERROR;
  58                 nt--;
  59         }
  60
  61         *ctrl = ejtag_ctrl;
  62         return ERROR_OK;
  63 }
  64
  65 static int mips64_pracc_exec_read(struct mips64_pracc_context *ctx, uint64_t address)
  66 {
  67         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
  68         unsigned offset;
  69         uint32_t ejtag_ctrl;
  70         uint64_t data;
  71         int rc;
  72
  73         if ((address >= MIPS64_PRACC_PARAM_IN)
  74             && (address < MIPS64_PRACC_PARAM_IN + ctx->num_iparam * MIPS64_PRACC_DATA_STEP)) {
  75
  76                 offset = (address - MIPS64_PRACC_PARAM_IN) / MIPS64_PRACC_DATA_STEP;
  77
  78                 if (offset >= MIPS64_PRACC_PARAM_IN_SIZE) {
  79                         LOG_ERROR("Error: iparam size exceeds MIPS64_PRACC_PARAM_IN_SIZE");
  80                         return ERROR_JTAG_DEVICE_ERROR;
  81                 }
  82
  83                 if (!ctx->local_iparam) {
  84                         LOG_ERROR("Error: unexpected reading of input parameter");
  85                         return ERROR_JTAG_DEVICE_ERROR;
  86                 }
  87
  88                 data = ctx->local_iparam[offset];
  89                 LOG_DEBUG("Reading %" PRIx64 " at %" PRIx64, data, address);
  90
  91         } else if ((address >= MIPS64_PRACC_PARAM_OUT)
  92                    && (address < MIPS64_PRACC_PARAM_OUT + ctx->num_oparam * MIPS64_PRACC_DATA_STEP)) {
  93
  94                 offset = (address - MIPS64_PRACC_PARAM_OUT) / MIPS64_PRACC_DATA_STEP;
  95                 if (!ctx->local_oparam) {
  96                         LOG_ERROR("Error: unexpected reading of output parameter");
  97                         return ERROR_JTAG_DEVICE_ERROR;
  98                 }
  99
 100                 data = ctx->local_oparam[offset];
 101                 LOG_DEBUG("Reading %" PRIx64 " at %" PRIx64, data, address);
 102
 103         } else if ((address >= MIPS64_PRACC_TEXT)
 104                    && (address < MIPS64_PRACC_TEXT + ctx->code_len * MIPS64_PRACC_ADDR_STEP)) {
 105
 106                 offset = ((address & ~7ull) - MIPS64_PRACC_TEXT) / MIPS64_PRACC_ADDR_STEP;
 107                 data = (uint64_t)ctx->code[offset] << 32;
 108                 if (offset + 1 < ctx->code_len)
 109                         data |= (uint64_t)ctx->code[offset + 1];
 110
 111                 LOG_DEBUG("Running commands %" PRIx64 " at %" PRIx64, data,
 112                           address);
 113
 114         } else if ((address & ~7llu) == MIPS64_PRACC_STACK) {
 115
 116                 /* load from our debug stack */
 117                 if (ctx->stack_offset == 0) {
 118                         LOG_ERROR("Error reading from stack: stack is empty");
 119                         return ERROR_JTAG_DEVICE_ERROR;
 120                 }
 121
 122                 data = ctx->stack[--ctx->stack_offset];
 123                 LOG_DEBUG("Reading %" PRIx64 " at %" PRIx64, data, address);
 124
 125         } else {
 126                 /* TODO: send JMP 0xFF200000 instruction. Hopefully processor jump back
 127                  * to start of debug vector */
 128
 129                 data = 0;
 130                 LOG_ERROR("Error reading unexpected address %" PRIx64, address);
 131                 return ERROR_JTAG_DEVICE_ERROR;
 132         }
 133
 134         /* Send the data out */
 135         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA);
 136         rc = mips_ejtag_drscan_64(ctx->ejtag_info, &data);
 137         if (rc != ERROR_OK)
 138                 return rc;
 139
 140         /* Clear the access pending bit (let the processor eat!) */
 141
 142         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 143         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL);
 144         rc = mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
 145         if (rc != ERROR_OK)
 146                 return rc;
 147
 148         jtag_add_clocks(5);
 149
 150         return jtag_execute_queue();
 151 }
 152
 153 static int mips64_pracc_exec_write(struct mips64_pracc_context *ctx, uint64_t address)
 154 {
 155         uint32_t ejtag_ctrl;
 156         uint64_t data;
 157         unsigned offset;
 158         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
 159         int rc;
 160
 161         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA);
 162         rc = mips_ejtag_drscan_64(ctx->ejtag_info, &data);
 163         if (rc != ERROR_OK)
 164                 return rc;
 165
 166         /* Clear access pending bit */
 167         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 168         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL);
 169         rc = mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
 170         if (rc != ERROR_OK)
 171                 return rc;
 172
 173         jtag_add_clocks(5);
 174         rc = jtag_execute_queue();
 175         if (rc != ERROR_OK)
 176                 return rc;
 177
 178         LOG_DEBUG("Writing %" PRIx64 " at %" PRIx64, data, address);
 179
 180         if ((address >= MIPS64_PRACC_PARAM_IN)
 181                 && (address < MIPS64_PRACC_PARAM_IN + ctx->num_iparam * MIPS64_PRACC_DATA_STEP)) {
 182                 offset = (address - MIPS64_PRACC_PARAM_IN) / MIPS64_PRACC_DATA_STEP;
 183                 if (!ctx->local_iparam) {
 184                         LOG_ERROR("Error: unexpected writing of input parameter");
 185                         return ERROR_JTAG_DEVICE_ERROR;
 186                 }
 187                 ctx->local_iparam[offset] = data;
 188         } else if ((address >= MIPS64_PRACC_PARAM_OUT)
 189                 && (address < MIPS64_PRACC_PARAM_OUT + ctx->num_oparam * MIPS64_PRACC_DATA_STEP)) {
 190                 offset = (address - MIPS64_PRACC_PARAM_OUT) / MIPS64_PRACC_DATA_STEP;
 191                 if (!ctx->local_oparam) {
 192                         LOG_ERROR("Error: unexpected writing of output parameter");
 193                         return ERROR_JTAG_DEVICE_ERROR;
 194                 }
 195                 ctx->local_oparam[offset] = data;
 196         } else if (address == MIPS64_PRACC_STACK) {
 197                 /* save data onto our stack */
 198                 if (ctx->stack_offset >= STACK_DEPTH) {
 199                         LOG_ERROR("Error: PrAcc stack depth exceeded");
 200                         return ERROR_FAIL;
 201                 }
 202                 ctx->stack[ctx->stack_offset++] = data;
 203         } else {
 204                 LOG_ERROR("Error writing unexpected address 0x%" PRIx64, address);
 205                 return ERROR_JTAG_DEVICE_ERROR;
 206         }
 207
 208         return ERROR_OK;
 209 }
 210
 211 int mips64_pracc_exec(struct mips_ejtag *ejtag_info,
 212                       unsigned code_len, const uint32_t *code,
 213                       unsigned num_param_in, uint64_t *param_in,
 214                       unsigned num_param_out, uint64_t *param_out)
 215 {
 216         uint32_t ejtag_ctrl;
 217         uint64_t address = 0, address_prev = 0;
 218         struct mips64_pracc_context ctx;
 219         int retval;
 220         int pass = 0;
 221         bool first_time_call = true;
 222         unsigned i;
 223
 224         for (i = 0; i < code_len; i++)
 225                 LOG_DEBUG("%08" PRIx32, code[i]);
 226
 227         ctx.local_iparam = param_in;
 228         ctx.local_oparam = param_out;
 229         ctx.num_iparam = num_param_in;
 230         ctx.num_oparam = num_param_out;
 231         ctx.code = code;
 232         ctx.code_len = code_len;
 233         ctx.ejtag_info = ejtag_info;
 234         ctx.stack_offset = 0;
 235
 236         while (true) {
 237                 uint32_t address32;
 238                 retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
 239                 if (retval != ERROR_OK) {
 240                         LOG_DEBUG("ERROR wait_for_pracc_rw");
 241                         return retval;
 242                 }
 243                 if (pass)
 244                         address_prev = address;
 245                 else
 246                         address_prev = 0;
 247                 address32 = 0;
 248
 249                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
 250                 mips_ejtag_drscan_32(ejtag_info, &address32);
 251                 LOG_DEBUG("-> %08" PRIx32, address32);
 252                 address = 0xffffffffff200000ull | address32;
 253
 254                 int psz = (ejtag_ctrl >> 29) & 3;
 255                 int address20 = address & 7;
 256                 switch (psz) {
 257                 case 3:
 258                         if (address20 != 7) {
 259                                 LOG_ERROR("PSZ=%d ADDRESS[2:0]=%d: not supported", psz, address20);
 260                                 return ERROR_FAIL;
 261                         }
 262                         address &= ~7ull;
 263                         break;
 264                 case 2:
 265                         if (address20 != 0 && address20 != 4) {
 266                                 LOG_ERROR("PSZ=%d ADDRESS[2:0]=%d: not supported", psz, address20);
 267                                 return ERROR_FAIL;
 268                         }
 269                         break;
 270                 default:
 271                         LOG_ERROR("PSZ=%d ADDRESS[2:0]=%d: not supported", psz, address20);
 272                         return ERROR_FAIL;
 273                 }
 274
 275                 if (first_time_call && address != MIPS64_PRACC_TEXT) {
 276                         LOG_ERROR("Error reading address " TARGET_ADDR_FMT " (0x%08llx expected)",
 277                                 address, MIPS64_PRACC_TEXT);
 278                         return ERROR_JTAG_DEVICE_ERROR;
 279                 }
 280
 281                 first_time_call = false;
 282
 283                 /* Check for read or write */
 284                 if (ejtag_ctrl & EJTAG_CTRL_PRNW) {
 285                         retval = mips64_pracc_exec_write(&ctx, address);
 286                         if (retval != ERROR_OK) {
 287                                 LOG_ERROR("mips64_pracc_exec_write() failed");
 288                                 return retval;
 289                         }
 290                 } else {
 291                         /* Check to see if its reading at the debug vector. The first pass through
 292                          * the module is always read at the vector, so the first one we allow.  When
 293                          * the second read from the vector occurs we are done and just exit. */
 294                         if ((address == MIPS64_PRACC_TEXT) && (pass++)) {
 295                                 LOG_DEBUG("@MIPS64_PRACC_TEXT, address_prev=%" PRIx64, address_prev);
 296                                 break;
 297                         }
 298                         retval = mips64_pracc_exec_read(&ctx, address);
 299                         if (retval != ERROR_OK) {
 300                                 LOG_ERROR("mips64_pracc_exec_read() failed");
 301                                 return retval;
 302                         }
 303
 304                 }
 305         }
 306
 307         /* stack sanity check */
 308         if (ctx.stack_offset != 0)
 309                 LOG_ERROR("Pracc Stack not zero");
 310
 311         return ERROR_OK;
 312 }
 313
 314 static int mips64_pracc_read_u64(struct mips_ejtag *ejtag_info, uint64_t addr,
 315                                  uint64_t *buf)
 316 {
 317         const uint32_t code[] = {
 318                 /* move $15 to COP0 DeSave */
 319                 MIPS64_DMTC0(15, 31, 0),
 320                 /* $15 = MIPS64_PRACC_STACK */
 321                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
 322                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
 323                 /* sd $8, ($15) */
 324                 MIPS64_SD(8, 0, 15),
 325                 /* load R8 @ param_in[0] = address */
 326                 MIPS64_LD(8, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN),  15),
 327                 /* ld $8, 0($8),  Load $8 with the word @mem[$8] */
 328                 MIPS64_LD(8, 0, 8),
 329                 /* sd $8, 0($15) */
 330                 MIPS64_SD(8, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_OUT), 15),
 331                 /* ld $8, ($15) */
 332                 MIPS64_LD(8, 0, 15),
 333                 MIPS64_SYNC,
 334                 /* b start */
 335                 MIPS64_B(NEG16(10)),
 336                 /* move COP0 DeSave to $15 */
 337                 MIPS64_DMFC0(15, 31, 0),
 338                 MIPS64_NOP,
 339                 MIPS64_NOP,
 340                 MIPS64_NOP,
 341                 MIPS64_NOP,
 342                 MIPS64_NOP,
 343                 MIPS64_NOP,
 344                 MIPS64_NOP,
 345                 MIPS64_NOP,
 346         };
 347
 348         uint64_t param_in[1];
 349         param_in[0] = addr;
 350
 351         LOG_DEBUG("enter mips64_pracc_exec");
 352         return mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 353                 ARRAY_SIZE(param_in), param_in, 1, (uint64_t *) buf);
 354 }
 355
 356 static int mips64_pracc_read_mem64(struct mips_ejtag *ejtag_info, uint64_t addr,
 357                             unsigned count, uint64_t *buf)
 358 {
 359         int retval = ERROR_OK;
 360
 361         for (unsigned i = 0; i < count; i++) {
 362                 retval = mips64_pracc_read_u64(ejtag_info, addr + 8*i, &buf[i]);
 363                 if (retval != ERROR_OK)
 364                         return retval;
 365         }
 366         return retval;
 367 }
 368
 369 static int mips64_pracc_read_u32(struct mips_ejtag *ejtag_info, uint64_t addr,
 370                                  uint32_t *buf)
 371 {
 372         const uint32_t code[] = {
 373                 /* move $15 to COP0 DeSave */
 374                 MIPS64_DMTC0(15, 31, 0),
 375                 /* $15 = MIPS64_PRACC_STACK */
 376                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
 377                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
 378                 /* sd $8, ($15) */
 379                 MIPS64_SD(8, 0, 15),
 380                 /* load R8 @ param_in[0] = address */
 381                 MIPS64_LD(8, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN),  15),
 382                 /* lw $8, 0($8),  Load $8 with the word @mem[$8] */
 383                 MIPS64_LW(8, 0, 8),
 384                 /* sd $8, 0($9) */
 385                 MIPS64_SD(8, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_OUT), 15),
 386                 /* ld $8, ($15) */
 387                 MIPS64_LD(8, 0, 15),
 388                 MIPS64_SYNC,
 389                 /* b start */
 390                 MIPS64_B(NEG16(10)),
 391                 /* move COP0 DeSave to $15 */
 392                 MIPS64_DMFC0(15, 31, 0),
 393                 MIPS64_NOP,
 394                 MIPS64_NOP,
 395                 MIPS64_NOP,
 396                 MIPS64_NOP,
 397                 MIPS64_NOP,
 398                 MIPS64_NOP,
 399                 MIPS64_NOP,
 400                 MIPS64_NOP,
 401         };
 402
 403         int retval = ERROR_OK;
 404         uint64_t param_in[1];
 405         uint64_t param_out[1];
 406
 407         param_in[0] = addr;
 408
 409         LOG_DEBUG("enter mips64_pracc_exec");
 410         retval = mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 411                 1, param_in, 1, param_out);
 412         buf[0] = (uint32_t) param_out[0];
 413         return retval;
 414 }
 415
 416 static int mips64_pracc_read_mem32(struct mips_ejtag *ejtag_info, uint64_t addr,
 417                                    unsigned count, uint32_t *buf)
 418 {
 419         int retval = ERROR_OK;
 420
 421         for (unsigned i = 0; i < count; i++) {
 422                 retval = mips64_pracc_read_u32(ejtag_info, addr + 4 * i, &buf[i]);
 423                 if (retval != ERROR_OK)
 424                         return retval;
 425         }
 426         return retval;
 427 }
 428
 429 static int mips64_pracc_read_u16(struct mips_ejtag *ejtag_info, uint64_t addr,
 430                                  uint16_t *buf)
 431 {
 432         const uint32_t code[] = {
 433                 /* move $15 to COP0 DeSave */
 434                 MIPS64_DMTC0(15, 31, 0),
 435                 /* $15 = MIPS64_PRACC_STACK */
 436                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
 437                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
 438                 /* sd $8, ($15) */
 439                 MIPS64_SD(8, 0, 15),
 440                 /* load R8 @ param_in[0] = address */
 441                 MIPS64_LD(8, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN),  15),
 442                 /* lw $8, 0($8),  Load $8 with the word @mem[$8] */
 443                 MIPS64_LHU(8, 0, 8),
 444                 /* sd $8, 0($9) */
 445                 MIPS64_SD(8, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_OUT), 15),
 446                 /* ld $8, ($15) */
 447                 MIPS64_LD(8, 0, 15),
 448                 MIPS64_SYNC,
 449                 /* b start */
 450                 MIPS64_B(NEG16(10)),
 451                 /* move COP0 DeSave to $15 */
 452                 MIPS64_DMFC0(15, 31, 0),
 453                 MIPS64_NOP,
 454                 MIPS64_NOP,
 455                 MIPS64_NOP,
 456                 MIPS64_NOP,
 457                 MIPS64_NOP,
 458                 MIPS64_NOP,
 459                 MIPS64_NOP,
 460                 MIPS64_NOP,
 461         };
 462
 463         int retval;
 464         uint64_t param_in[1];
 465         uint64_t param_out[1];
 466
 467         param_in[0] = addr;
 468
 469         LOG_DEBUG("enter mips64_pracc_exec");
 470         retval = mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 471                 1, param_in, 1, param_out);
 472         buf[0] = (uint16_t)param_out[0];
 473         return retval;
 474 }
 475
 476 static int mips64_pracc_read_mem16(struct mips_ejtag *ejtag_info, uint64_t addr,
 477                             unsigned count, uint16_t *buf)
 478 {
 479         int retval = ERROR_OK;
 480
 481         for (unsigned i = 0; i < count; i++) {
 482                 retval = mips64_pracc_read_u16(ejtag_info, addr + 2*i, &buf[i]);
 483                 if (retval != ERROR_OK)
 484                         return retval;
 485         }
 486         return retval;
 487 }
 488
 489 static int mips64_pracc_read_u8(struct mips_ejtag *ejtag_info, uint64_t addr,
 490                                 uint8_t *buf)
 491 {
 492         const uint32_t code[] = {
 493                 /* move $15 to COP0 DeSave */
 494                 MIPS64_DMTC0(15, 31, 0),
 495                 /* $15 = MIPS64_PRACC_STACK */
 496                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
 497                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
 498                 /* sd $8, ($15) */
 499                 MIPS64_SD(8, 0, 15),
 500                 /* load R8 @ param_in[0] = address */
 501                 MIPS64_LD(8, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN),  15),
 502                 /* lw $8, 0($8),  Load $8 with the word @mem[$8] */
 503                 MIPS64_LBU(8, 0, 8),
 504                 /* sd $8, 0($9) */
 505                 MIPS64_SD(8, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_OUT), 15),
 506                 /* ld $8, ($15) */
 507                 MIPS64_LD(8, 0, 15),
 508                 MIPS64_SYNC,
 509                 /* b start */
 510                 MIPS64_B(NEG16(10)),
 511                 /* move COP0 DeSave to $15 */
 512                 MIPS64_DMFC0(15, 31, 0),
 513                 MIPS64_NOP,
 514                 MIPS64_NOP,
 515                 MIPS64_NOP,
 516                 MIPS64_NOP,
 517                 MIPS64_NOP,
 518                 MIPS64_NOP,
 519                 MIPS64_NOP,
 520                 MIPS64_NOP,
 521         };
 522
 523         int retval;
 524         uint64_t param_in[1];
 525         uint64_t param_out[1];
 526
 527         param_in[0] = addr;
 528
 529         LOG_DEBUG("enter mips64_pracc_exec");
 530         retval = mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 531                 1, param_in, 1, param_out);
 532         buf[0] = (uint8_t)param_out[0];
 533         return retval;
 534 }
 535
 536 static int mips64_pracc_read_mem8(struct mips_ejtag *ejtag_info, uint64_t addr,
 537                           unsigned count, uint8_t *buf)
 538 {
 539         int retval = ERROR_OK;
 540
 541         for (unsigned i = 0; i < count; i++) {
 542                 retval = mips64_pracc_read_u8(ejtag_info, addr + i, &buf[i]);
 543                 if (retval != ERROR_OK)
 544                         return retval;
 545         }
 546         return retval;
 547 }
 548
 549 int mips64_pracc_read_mem(struct mips_ejtag *ejtag_info, uint64_t addr,
 550                           unsigned size, unsigned count, void *buf)
 551 {
 552         switch (size) {
 553         case 1:
 554                 return mips64_pracc_read_mem8(ejtag_info, addr, count, buf);
 555         case 2:
 556                 return mips64_pracc_read_mem16(ejtag_info, addr, count, buf);
 557         case 4:
 558                 return mips64_pracc_read_mem32(ejtag_info, addr, count, buf);
 559         case 8:
 560                 return mips64_pracc_read_mem64(ejtag_info, addr, count, buf);
 561         }
 562         return ERROR_FAIL;
 563 }
 564
 565 static int mips64_pracc_write_u64(struct mips_ejtag *ejtag_info, uint64_t addr,
 566                                   uint64_t *buf)
 567 {
 568         const uint32_t code[] = {
 569                 /* move $15 to COP0 DeSave */
 570                 MIPS64_DMTC0(15, 31, 0),
 571                 /* $15 = MIPS64_PRACC_STACK */
 572                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
 573                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
 574                 /* sd $8, ($15) */
 575                 MIPS64_SD(8, 0, 15),
 576                 /* sd $9, ($15) */
 577                 MIPS64_SD(9, 0, 15),
 578                 /* load R8 @ param_in[1] = data */
 579                 MIPS64_LD(8, NEG16((MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN)-8), 15),
 580                 /* load R9 @ param_in[0] = address */
 581                 MIPS64_LD(9, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN), 15),
 582                 /* sd $8, 0($9) */
 583                 MIPS64_SD(8, 0, 9),
 584                 MIPS64_SYNCI(9, 0),
 585                 /* ld $9, ($15) */
 586                 MIPS64_LD(9, 0, 15),
 587                 /* ld $8, ($15) */
 588                 MIPS64_LD(8, 0, 15),
 589                 MIPS64_SYNC,
 590                 /* b start */
 591                 MIPS64_B(NEG16(13)),
 592                 /* move COP0 DeSave to $15 */
 593                 MIPS64_DMFC0(15, 31, 0),
 594                 MIPS64_NOP,
 595                 MIPS64_NOP,
 596                 MIPS64_NOP,
 597                 MIPS64_NOP,
 598                 MIPS64_NOP,
 599                 MIPS64_NOP,
 600                 MIPS64_NOP,
 601                 MIPS64_NOP,
 602         };
 603
 604         /* TODO remove array */
 605         uint64_t param_in[2];
 606         param_in[0] = addr;
 607         param_in[1] = *buf;
 608
 609         LOG_DEBUG("enter mips64_pracc_exec");
 610         return mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 611                 ARRAY_SIZE(param_in), param_in, 0, NULL);
 612 }
 613
 614 static int mips64_pracc_write_mem64(struct mips_ejtag *ejtag_info,
 615                              uint64_t addr, unsigned count, uint64_t *buf)
 616 {
 617         int retval = ERROR_OK;
 618
 619         for (unsigned i = 0; i < count; i++) {
 620                 retval = mips64_pracc_write_u64(ejtag_info, addr + 8 * i, &buf[i]);
 621                 if (retval != ERROR_OK)
 622                         return retval;
 623         }
 624         return retval;
 625 }
 626
 627 static int mips64_pracc_write_u32(struct mips_ejtag *ejtag_info, uint64_t addr,
 628                                   uint32_t *buf)
 629 {
 630         const uint32_t code[] = {
 631                 MIPS64_DMTC0(15, 31, 0),
 632                 /* move $15 to COP0 DeSave */
 633                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
 634                 /* $15 = MIPS64_PRACC_STACK */
 635                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
 636                 MIPS64_SD(8, 0, 15),
 637                 /* sd $8, ($15) */
 638                 MIPS64_SD(9, 0, 15),
 639                 /* sd $9, ($15) */
 640                 MIPS64_LD(8, NEG16((MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN) - 8), 15),
 641                 /* load R8 @ param_in[1] = data */
 642                 MIPS64_LD(9, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN), 15),
 643                 /* load R9 @ param_in[0] = address */
 644                 MIPS64_SW(8, 0, 9),
 645                 /* sw $8, 0($9) */
 646                 MIPS64_SYNCI(9, 0),
 647                 MIPS64_LD(9, 0, 15),
 648                 /* ld $9, ($15) */
 649                 MIPS64_LD(8, 0, 15),
 650                 /* ld $8, ($15) */
 651                 MIPS64_SYNC,
 652                 MIPS64_B(NEG16(13)),
 653                 /* b start */
 654                 MIPS64_DMFC0(15, 31, 0),
 655                 /* move COP0 DeSave to $15 */
 656                 MIPS64_NOP,
 657                 MIPS64_NOP,
 658                 MIPS64_NOP,
 659                 MIPS64_NOP,
 660                 MIPS64_NOP,
 661                 MIPS64_NOP,
 662                 MIPS64_NOP,
 663                 MIPS64_NOP,
 664         };
 665
 666         /* TODO remove array */
 667         uint64_t param_in[1 + 1];
 668         param_in[0] = addr;
 669         param_in[1] = *buf;
 670
 671         LOG_DEBUG("enter mips64_pracc_exec");
 672         return mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 673                 ARRAY_SIZE(param_in), param_in, 0, NULL);
 674 }
 675
 676 static int mips64_pracc_write_mem32(struct mips_ejtag *ejtag_info, uint64_t addr,
 677                              unsigned count, uint32_t *buf)
 678 {
 679         int retval = ERROR_OK;
 680
 681         for (unsigned i = 0; i < count; i++) {
 682                 retval = mips64_pracc_write_u32(ejtag_info, addr + 4 * i, &buf[i]);
 683                 if (retval != ERROR_OK)
 684                         return retval;
 685         }
 686         return retval;
 687 }
 688
 689 static int mips64_pracc_write_u16(struct mips_ejtag *ejtag_info, uint64_t addr,
 690                                   uint16_t *buf)
 691 {
 692         const uint32_t code[] = {
 693                 /* move $15 to COP0 DeSave */
 694                 MIPS64_DMTC0(15, 31, 0),
 695                 /* $15 = MIPS64_PRACC_STACK */
 696                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
 697                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
 698                 /* sd $8, ($15) */
 699                 MIPS64_SD(8, 0, 15),
 700                 /* sd $9, ($15) */
 701                 MIPS64_SD(9, 0, 15),
 702                 /* load R8 @ param_in[1] = data */
 703                 MIPS64_LD(8, NEG16((MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN) - 8), 15),
 704                 /* load R9 @ param_in[0] = address */
 705                 MIPS64_LD(9, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN), 15),
 706                 /* sh $8, 0($9) */
 707                 MIPS64_SH(8, 0, 9),
 708                 /* ld $9, ($15) */
 709                 MIPS64_LD(9, 0, 15),
 710                 /* ld $8, ($15) */
 711                 MIPS64_LD(8, 0, 15),
 712                 MIPS64_SYNC,
 713                 /* b start */
 714                 MIPS64_B(NEG16(12)),
 715                 /* move COP0 DeSave to $15 */
 716                 MIPS64_DMFC0(15, 31, 0),
 717                 MIPS64_NOP,
 718                 MIPS64_NOP,
 719                 MIPS64_NOP,
 720                 MIPS64_NOP,
 721                 MIPS64_NOP,
 722                 MIPS64_NOP,
 723                 MIPS64_NOP,
 724                 MIPS64_NOP,
 725         };
 726
 727         uint64_t param_in[2];
 728         param_in[0] = addr;
 729         param_in[1] = *buf;
 730
 731         LOG_DEBUG("enter mips64_pracc_exec");
 732         return mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 733                 ARRAY_SIZE(param_in), param_in, 0, NULL);
 734 }
 735
 736 static int mips64_pracc_write_mem16(struct mips_ejtag *ejtag_info,
 737                              uint64_t addr, unsigned count, uint16_t *buf)
 738 {
 739         int retval = ERROR_OK;
 740
 741         for (unsigned i = 0; i < count; i++) {
 742                 retval = mips64_pracc_write_u16(ejtag_info, addr + 2 * i, &buf[i]);
 743                 if (retval != ERROR_OK)
 744                         return retval;
 745         }
 746         return retval;
 747 }
 748
 749 static int mips64_pracc_write_u8(struct mips_ejtag *ejtag_info, uint64_t addr,
 750                                  uint8_t *buf)
 751 {
 752         const uint32_t code[] = {
 753                 /* move $15 to COP0 DeSave */
 754                 MIPS64_DMTC0(15, 31, 0),
 755                 /* $15 = MIPS64_PRACC_STACK */
 756                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
 757                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
 758                 /* sd $8, ($15) */
 759                 MIPS64_SD(8, 0, 15),
 760                 /* sd $9, ($15) */
 761                 MIPS64_SD(9, 0, 15),
 762                 /* load R8 @ param_in[1] = data */
 763                 MIPS64_LD(8, NEG16((MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN) - 8), 15),
 764                 /* load R9 @ param_in[0] = address */
 765                 MIPS64_LD(9, NEG16(MIPS64_PRACC_STACK-MIPS64_PRACC_PARAM_IN), 15),
 766                 /* sh $8, 0($9) */
 767                 MIPS64_SB(8, 0, 9),
 768                 /* ld $9, ($15) */
 769                 MIPS64_LD(9, 0, 15),
 770                 /* ld $8, ($15) */
 771                 MIPS64_LD(8, 0, 15),
 772                 MIPS64_SYNC,
 773                 /* b start */
 774                 MIPS64_B(NEG16(12)),
 775                 /* move COP0 DeSave to $15 */
 776                 MIPS64_DMFC0(15, 31, 0),
 777                 MIPS64_NOP,
 778                 MIPS64_NOP,
 779                 MIPS64_NOP,
 780                 MIPS64_NOP,
 781                 MIPS64_NOP,
 782                 MIPS64_NOP,
 783                 MIPS64_NOP,
 784                 MIPS64_NOP,
 785         };
 786
 787         /* TODO remove array */
 788         uint64_t param_in[2];
 789         param_in[0] = addr;
 790         param_in[1] = *buf;
 791
 792         LOG_DEBUG("enter mips64_pracc_exec");
 793         return mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
 794                 ARRAY_SIZE(param_in), param_in, 0, NULL);
 795 }
 796
 797 static int mips64_pracc_write_mem8(struct mips_ejtag *ejtag_info,
 798                             uint64_t addr, unsigned count, uint8_t *buf)
 799 {
 800         int retval = ERROR_OK;
 801
 802         for (unsigned i = 0; i < count; i++) {
 803                 retval = mips64_pracc_write_u8(ejtag_info, addr + i, &buf[i]);
 804                 if (retval != ERROR_OK)
 805                         return retval;
 806         }
 807         return retval;
 808 }
 809
 810 int mips64_pracc_write_mem(struct mips_ejtag *ejtag_info,
 811                            uint64_t addr, unsigned size,
 812                            unsigned count, void *buf)
 813 {
 814         switch (size) {
 815         case 1:
 816                 return mips64_pracc_write_mem8(ejtag_info, addr, count, buf);
 817         case 2:
 818                 return mips64_pracc_write_mem16(ejtag_info, addr, count, buf);
 819         case 4:
 820                 return mips64_pracc_write_mem32(ejtag_info, addr, count, buf);
 821         case 8:
 822                 return mips64_pracc_write_mem64(ejtag_info, addr, count, buf);
 823         }
 824         return ERROR_FAIL;
 825 }
 826
 827 int mips64_pracc_write_regs(struct mips_ejtag *ejtag_info, uint64_t *regs)
 828 {
 829         const uint32_t code[] = {
 830                 /* move $2 to COP0 DeSave */
 831                 MIPS64_DMTC0(2, 31, 0),
 832                 /* $15 = MIPS64_PRACC_STACK */
 833                 MIPS64_LUI(2, UPPER16(MIPS64_PRACC_PARAM_IN)),
 834                 MIPS64_ORI(2, 2, LOWER16(MIPS64_PRACC_PARAM_IN)),
 835                 /* sd $0, 0*8($2) */
 836                 MIPS64_LD(1, 1*8, 2),
 837                 /* sd $1, 1*8($2) */
 838                 MIPS64_LD(15, 15*8, 2),
 839                 /* sd $11, ($15) */
 840                 MIPS64_DMFC0(2, 31, 0),
 841                 MIPS64_DMTC0(15, 31, 0),
 842                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
 843                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
 844                 MIPS64_SD(1, 0, 15),
 845                 /* $11 = MIPS64_PRACC_PARAM_OUT */
 846                 MIPS64_LUI(1, UPPER16(MIPS64_PRACC_PARAM_IN)),
 847                 MIPS64_ORI(1, 1, LOWER16(MIPS64_PRACC_PARAM_IN)),
 848                 MIPS64_LD(3, 3*8, 1),
 849                 MIPS64_LD(4, 4*8, 1),
 850                 MIPS64_LD(5, 5*8, 1),
 851                 MIPS64_LD(6, 6*8, 1),
 852                 MIPS64_LD(7, 7*8, 1),
 853                 MIPS64_LD(8, 8*8, 1),
 854                 MIPS64_LD(9, 9*8, 1),
 855                 MIPS64_LD(10, 10*8, 1),
 856                 MIPS64_LD(11, 11*8, 1),
 857                 MIPS64_LD(12, 12*8, 1),
 858                 MIPS64_LD(13, 13*8, 1),
 859                 MIPS64_LD(14, 14*8, 1),
 860                 MIPS64_LD(16, 16*8, 1),
 861                 MIPS64_LD(17, 17*8, 1),
 862                 MIPS64_LD(18, 18*8, 1),
 863                 MIPS64_LD(19, 19*8, 1),
 864                 MIPS64_LD(20, 20*8, 1),
 865                 MIPS64_LD(21, 21*8, 1),
 866                 MIPS64_LD(22, 22*8, 1),
 867                 MIPS64_LD(23, 23*8, 1),
 868                 MIPS64_LD(24, 24*8, 1),
 869                 MIPS64_LD(25, 25*8, 1),
 870                 MIPS64_LD(26, 26*8, 1),
 871                 MIPS64_LD(27, 27*8, 1),
 872                 MIPS64_LD(28, 28*8, 1),
 873                 MIPS64_LD(29, 29*8, 1),
 874                 MIPS64_LD(30, 30*8, 1),
 875                 MIPS64_LD(31, 31*8, 1),
 876                 MIPS64_LD(2, 32*8, 1),
 877                 MIPS64_MTLO(2),
 878                 MIPS64_LD(2, 33*8, 1),
 879                 MIPS64_MTHI(2),
 880                 MIPS64_LD(2, MIPS64_NUM_CORE_REGS * 8, 1),
 881                 MIPS64_DMTC0(2, MIPS64_C0_DEPC, 0),
 882                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 2) * 8, 1),
 883                 MIPS64_DMTC0(2, MIPS64_C0_ENTRYLO0, 0),
 884                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 3) * 8, 1),
 885                 MIPS64_DMTC0(2, MIPS64_C0_ENTRYLO1, 0),
 886                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 4) * 8, 1),
 887                 MIPS64_DMTC0(2, MIPS64_C0_CONTEXT, 0),
 888                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 5) * 8, 1),
 889                 MIPS64_MTC0(2, MIPS64_C0_PAGEMASK, 0),
 890                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 6) * 8, 1),
 891                 MIPS64_MTC0(2, MIPS64_C0_WIRED, 0),
 892                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 8) * 8, 1),
 893                 MIPS64_MTC0(2, MIPS64_C0_COUNT, 0),
 894                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 9) * 8, 1),
 895                 MIPS64_DMTC0(2, MIPS64_C0_ENTRYHI, 0),
 896                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 10) * 8, 1),
 897                 MIPS64_MTC0(2, MIPS64_C0_COMPARE, 0),
 898                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 11) * 8, 1),
 899                 MIPS64_MTC0(2, MIPS64_C0_STATUS, 0),
 900                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 12) * 8, 1),
 901                 MIPS64_MTC0(2, MIPS64_C0_CAUSE, 0),
 902                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 13) * 8, 1),
 903                 MIPS64_DMTC0(2, MIPS64_C0_EPC, 0),
 904                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 15) * 8, 1),
 905                 MIPS64_MTC0(2, MIPS64_C0_CONFIG, 0),
 906                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 16) * 8, 1),
 907                 MIPS64_MTC0(2, MIPS64_C0_LLA, 0),
 908                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 21) * 8, 1),
 909                 MIPS64_DMTC0(2, MIPS64_C0_XCONTEXT, 1),
 910                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 22) * 8, 1),
 911                 MIPS64_MTC0(2, MIPS64_C0_MEMCTRL, 0),
 912                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 24) * 8, 1),
 913                 MIPS64_MTC0(2, MIPS64_C0_PERFCOUNT, 0),
 914                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 25) * 8, 1),
 915                 MIPS64_MTC0(2, MIPS64_C0_PERFCOUNT, 1),
 916                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 26) * 8, 1),
 917                 MIPS64_MTC0(2, MIPS64_C0_PERFCOUNT, 2),
 918                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 27) * 8, 1),
 919                 MIPS64_MTC0(2, MIPS64_C0_PERFCOUNT, 3),
 920                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 28) * 8, 1),
 921                 MIPS64_MTC0(2, MIPS64_C0_ECC, 0),
 922                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 29) * 8, 1),
 923                 MIPS64_MTC0(2, MIPS64_C0_CACHERR, 0),
 924                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 30) * 8, 1),
 925                 MIPS64_MTC0(2, MIPS64_C0_TAGLO, 0),
 926                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 31) * 8, 1),
 927                 MIPS64_MTC0(2, MIPS64_C0_TAGHI, 0),
 928                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 32) * 8, 1),
 929                 MIPS64_DMTC0(2, MIPS64_C0_DATAHI, 0),
 930                 MIPS64_LD(2, (MIPS64_NUM_CORE_REGS + 33) * 8, 1),
 931                 MIPS64_DMTC0(2, MIPS64_C0_EEPC, 0),
 932                 /* check if FPU is enabled, */
 933                 MIPS64_MFC0(2, MIPS64_C0_STATUS, 0),
 934                 MIPS64_SRL(2, 2, 29),
 935                 MIPS64_ANDI(2, 2, 1),
 936                 /* skip FPU registers restoration if not */
 937                 MIPS64_BEQ(0, 2, 77),
 938                 MIPS64_NOP,
 939                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 33) * 8, 1),
 940                 MIPS64_CTC1(2, MIPS64_C1_FIR, 0),
 941                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 32) * 8, 1),
 942                 MIPS64_CTC1(2, MIPS64_C1_FCSR, 0),
 943                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 34) * 8, 1),
 944                 MIPS64_CTC1(2, MIPS64_C1_FCONFIG, 0),
 945                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 35) * 8, 1),
 946                 MIPS64_CTC1(2, MIPS64_C1_FCCR, 0),
 947                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 36) * 8, 1),
 948                 MIPS64_CTC1(2, MIPS64_C1_FEXR, 0),
 949                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 37) * 8, 1),
 950                 MIPS64_CTC1(2, MIPS64_C1_FENR, 0),
 951                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 0) * 8, 1),
 952                 MIPS64_DMTC1(2, 0, 0),
 953                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 1) * 8, 1),
 954                 MIPS64_DMTC1(2, 1, 0),
 955                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 2) * 8, 1),
 956                 MIPS64_DMTC1(2, 2, 0),
 957                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 3) * 8, 1),
 958                 MIPS64_DMTC1(2, 3, 0),
 959                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 4) * 8, 1),
 960                 MIPS64_DMTC1(2, 4, 0),
 961                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 5) * 8, 1),
 962                 MIPS64_DMTC1(2, 5, 0),
 963                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 6) * 8, 1),
 964                 MIPS64_DMTC1(2, 6, 0),
 965                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 7) * 8, 1),
 966                 MIPS64_DMTC1(2, 7, 0),
 967                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 8) * 8, 1),
 968                 MIPS64_DMTC1(2, 8, 0),
 969                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 9) * 8, 1),
 970                 MIPS64_DMTC1(2, 9, 0),
 971                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 10) * 8, 1),
 972                 MIPS64_DMTC1(2, 10, 0),
 973                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 11) * 8, 1),
 974                 MIPS64_DMTC1(2, 11, 0),
 975                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 12) * 8, 1),
 976                 MIPS64_DMTC1(2, 12, 0),
 977                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 13) * 8, 1),
 978                 MIPS64_DMTC1(2, 13, 0),
 979                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 14) * 8, 1),
 980                 MIPS64_DMTC1(2, 14, 0),
 981                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 15) * 8, 1),
 982                 MIPS64_DMTC1(2, 15, 0),
 983                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 16) * 8, 1),
 984                 MIPS64_DMTC1(2, 16, 0),
 985                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 17) * 8, 1),
 986                 MIPS64_DMTC1(2, 17, 0),
 987                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 18) * 8, 1),
 988                 MIPS64_DMTC1(2, 18, 0),
 989                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 19) * 8, 1),
 990                 MIPS64_DMTC1(2, 19, 0),
 991                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 20) * 8, 1),
 992                 MIPS64_DMTC1(2, 20, 0),
 993                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 21) * 8, 1),
 994                 MIPS64_DMTC1(2, 21, 0),
 995                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 22) * 8, 1),
 996                 MIPS64_DMTC1(2, 22, 0),
 997                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 23) * 8, 1),
 998                 MIPS64_DMTC1(2, 23, 0),
 999                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 24) * 8, 1),
1000                 MIPS64_DMTC1(2, 24, 0),
1001                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 25) * 8, 1),
1002                 MIPS64_DMTC1(2, 25, 0),
1003                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 26) * 8, 1),
1004                 MIPS64_DMTC1(2, 26, 0),
1005                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 27) * 8, 1),
1006                 MIPS64_DMTC1(2, 27, 0),
1007                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 28) * 8, 1),
1008                 MIPS64_DMTC1(2, 28, 0),
1009                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 29) * 8, 1),
1010                 MIPS64_DMTC1(2, 29, 0),
1011                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 30) * 8, 1),
1012                 MIPS64_DMTC1(2, 30, 0),
1013                 MIPS64_LD(2, (MIPS64_NUM_CORE_C0_REGS + 31) * 8, 1),
1014                 MIPS64_DMTC1(2, 31, 0),
1015                 MIPS64_LD(2, 2 * 8, 1),
1016                 MIPS64_LD(1, 0, 15),
1017                 MIPS64_SYNC,
1018                 /* b start */
1019                 MIPS64_B(NEG16(181)),
1020                 /* move COP0 DeSave to $15 */
1021                 MIPS64_DMFC0(15, 31, 0),
1022                 MIPS64_NOP,
1023                 MIPS64_NOP,
1024                 MIPS64_NOP,
1025                 MIPS64_NOP,
1026                 MIPS64_NOP,
1027                 MIPS64_NOP,
1028                 MIPS64_NOP,
1029                 MIPS64_NOP,
1030         };
1031
1032         LOG_DEBUG("enter mips64_pracc_exec");
1033         return mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
1034                 MIPS64_NUM_REGS, regs, 0, NULL);
1035 }
1036
1037 int mips64_pracc_read_regs(struct mips_ejtag *ejtag_info, uint64_t *regs)
1038 {
1039         const uint32_t code[] = {
1040                 /* move $2 to COP0 DeSave */
1041                 MIPS64_DMTC0(2, 31, 0),
1042                 /* $2 = MIPS64_PRACC_PARAM_OUT */
1043                 MIPS64_LUI(2, UPPER16(MIPS64_PRACC_PARAM_OUT)),
1044                 MIPS64_ORI(2, 2, LOWER16(MIPS64_PRACC_PARAM_OUT)),
1045                 /* sd $0, 0*8($2) */
1046                 MIPS64_SD(0, 0*8, 2),
1047                 /* sd $1, 1*8($2) */
1048                 MIPS64_SD(1, 1*8, 2),
1049                 /* sd $15, 15*8($2) */
1050                 MIPS64_SD(15, 15*8, 2),
1051                 /* move COP0 DeSave to $2 */
1052                 MIPS64_DMFC0(2, 31, 0),
1053                 /* move $15 to COP0 DeSave */
1054                 MIPS64_DMTC0(15, 31, 0),
1055                 /* $15 = MIPS64_PRACC_STACK */
1056                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_STACK)),
1057                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_STACK)),
1058                 /* sd $1, ($15) */
1059                 MIPS64_SD(1, 0, 15),
1060                 /* sd $2, ($15) */
1061                 MIPS64_SD(2, 0, 15),
1062                 /* $1 = MIPS64_PRACC_PARAM_OUT */
1063                 MIPS64_LUI(1, UPPER16(MIPS64_PRACC_PARAM_OUT)),
1064                 MIPS64_ORI(1, 1, LOWER16(MIPS64_PRACC_PARAM_OUT)),
1065                 MIPS64_SD(2, 2 * 8, 1),
1066                 MIPS64_SD(3, 3 * 8, 1),
1067                 MIPS64_SD(4, 4 * 8, 1),
1068                 MIPS64_SD(5, 5 * 8, 1),
1069                 MIPS64_SD(6, 6 * 8, 1),
1070                 MIPS64_SD(7, 7 * 8, 1),
1071                 MIPS64_SD(8, 8 * 8, 1),
1072                 MIPS64_SD(9, 9 * 8, 1),
1073                 MIPS64_SD(10, 10 * 8, 1),
1074                 MIPS64_SD(11, 11 * 8, 1),
1075                 MIPS64_SD(12, 12 * 8, 1),
1076                 MIPS64_SD(13, 13 * 8, 1),
1077                 MIPS64_SD(14, 14 * 8, 1),
1078                 MIPS64_SD(16, 16 * 8, 1),
1079                 MIPS64_SD(17, 17 * 8, 1),
1080                 MIPS64_SD(18, 18 * 8, 1),
1081                 MIPS64_SD(19, 19 * 8, 1),
1082                 MIPS64_SD(20, 20 * 8, 1),
1083                 MIPS64_SD(21, 21 * 8, 1),
1084                 MIPS64_SD(22, 22 * 8, 1),
1085                 MIPS64_SD(23, 23 * 8, 1),
1086                 MIPS64_SD(24, 24 * 8, 1),
1087                 MIPS64_SD(25, 25 * 8, 1),
1088                 MIPS64_SD(26, 26 * 8, 1),
1089                 MIPS64_SD(27, 27 * 8, 1),
1090                 MIPS64_SD(28, 28 * 8, 1),
1091                 MIPS64_SD(29, 29 * 8, 1),
1092                 MIPS64_SD(30, 30 * 8, 1),
1093                 MIPS64_SD(31, 31 * 8, 1),
1094                 MIPS64_MFLO(2),
1095                 MIPS64_SD(2, 32 * 8, 1),
1096                 MIPS64_MFHI(2),
1097                 MIPS64_SD(2, 33 * 8, 1),
1098                 MIPS64_DMFC0(2, MIPS64_C0_DEPC, 0),
1099                 MIPS64_SD(2, MIPS64_NUM_CORE_REGS * 8, 1),
1100                 MIPS64_DMFC0(2, MIPS64_C0_RANDOM, 0),
1101                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 1) * 8, 1),
1102                 MIPS64_DMFC0(2, MIPS64_C0_ENTRYLO0, 0),
1103                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 2) * 8, 1),
1104                 MIPS64_DMFC0(2, MIPS64_C0_ENTRYLO1, 0),
1105                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 3) * 8, 1),
1106                 MIPS64_DMFC0(2, MIPS64_C0_CONTEXT, 0),
1107                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 4) * 8, 1),
1108                 MIPS64_MFC0(2, MIPS64_C0_PAGEMASK, 0),
1109                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 5) * 8, 1),
1110                 MIPS64_MFC0(2, MIPS64_C0_WIRED, 0),
1111                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 6) * 8, 1),
1112                 MIPS64_DMFC0(2, MIPS64_C0_BADVADDR, 0),
1113                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 7) * 8, 1),
1114                 MIPS64_MFC0(2, MIPS64_C0_COUNT, 0),
1115                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 8) * 8, 1),
1116                 MIPS64_DMFC0(2, MIPS64_C0_ENTRYHI, 0),
1117                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 9) * 8, 1),
1118                 MIPS64_MFC0(2, MIPS64_C0_COMPARE, 0),
1119                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 10) * 8, 1),
1120                 MIPS64_MFC0(2, MIPS64_C0_STATUS, 0),
1121                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 11) * 8, 1),
1122                 MIPS64_MFC0(2, MIPS64_C0_CAUSE, 0),
1123                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 12) * 8, 1),
1124                 MIPS64_DMFC0(2, MIPS64_C0_EPC, 0),
1125                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 13) * 8, 1),
1126                 MIPS64_MFC0(2, MIPS64_C0_PRID, 0),
1127                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 14) * 8, 1),
1128                 MIPS64_MFC0(2, MIPS64_C0_CONFIG, 0),
1129                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 15) * 8, 1),
1130                 MIPS64_MFC0(2, MIPS64_C0_LLA, 0),
1131                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 16) * 8, 1),
1132                 MIPS64_DMFC0(2, MIPS64_C0_XCONTEXT, 1),
1133                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 21) * 8, 1),
1134                 MIPS64_MFC0(2, MIPS64_C0_MEMCTRL, 0),
1135                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 22) * 8, 1),
1136                 MIPS64_MFC0(2, MIPS64_C0_DEBUG, 0),
1137                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 23) * 8, 1),
1138                 MIPS64_MFC0(2, MIPS64_C0_PERFCOUNT, 0),
1139                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 24) * 8, 1),
1140                 MIPS64_MFC0(2, MIPS64_C0_PERFCOUNT, 1),
1141                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 25) * 8, 1),
1142                 MIPS64_MFC0(2, MIPS64_C0_PERFCOUNT, 2),
1143                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 26) * 8, 1),
1144                 MIPS64_MFC0(2, MIPS64_C0_PERFCOUNT, 3),
1145                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 27) * 8, 1),
1146                 MIPS64_MFC0(2, MIPS64_C0_ECC, 0),
1147                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 28) * 8, 1),
1148                 MIPS64_MFC0(2, MIPS64_C0_CACHERR, 0),
1149                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 29) * 8, 1),
1150                 MIPS64_MFC0(2, MIPS64_C0_TAGLO, 0),
1151                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 30) * 8, 1),
1152                 MIPS64_MFC0(2, MIPS64_C0_TAGHI, 0),
1153                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 31) * 8, 1),
1154                 MIPS64_DMFC0(2, MIPS64_C0_DATAHI, 0),
1155                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 32) * 8, 1),
1156                 MIPS64_DMFC0(2, MIPS64_C0_EEPC, 0),
1157                 MIPS64_SD(2, (MIPS64_NUM_CORE_REGS + 33) * 8, 1),
1158                 /* check if FPU is enabled, */
1159                 MIPS64_MFC0(2, MIPS64_C0_STATUS, 0),
1160                 MIPS64_SRL(2, 2, 29),
1161                 MIPS64_ANDI(2, 2, 1),
1162                 /* skip FPU registers dump if not */
1163                 MIPS64_BEQ(0, 2, 77),
1164                 MIPS64_NOP,
1165                 MIPS64_CFC1(2, MIPS64_C1_FIR, 0),
1166                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 33) * 8, 1),
1167                 MIPS64_CFC1(2, MIPS64_C1_FCSR, 0),
1168                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 32) * 8, 1),
1169                 MIPS64_CFC1(2, MIPS64_C1_FCONFIG, 0),
1170                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 34) * 8, 1),
1171                 MIPS64_CFC1(2, MIPS64_C1_FCCR, 0),
1172                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 35) * 8, 1),
1173                 MIPS64_CFC1(2, MIPS64_C1_FEXR, 0),
1174                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 36) * 8, 1),
1175                 MIPS64_CFC1(2, MIPS64_C1_FENR, 0),
1176                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 37) * 8, 1),
1177                 MIPS64_DMFC1(2, 0, 0),
1178                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 0) * 8, 1),
1179                 MIPS64_DMFC1(2, 1, 0),
1180                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 1) * 8, 1),
1181                 MIPS64_DMFC1(2, 2, 0),
1182                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 2) * 8, 1),
1183                 MIPS64_DMFC1(2, 3, 0),
1184                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 3) * 8, 1),
1185                 MIPS64_DMFC1(2, 4, 0),
1186                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 4) * 8, 1),
1187                 MIPS64_DMFC1(2, 5, 0),
1188                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 5) * 8, 1),
1189                 MIPS64_DMFC1(2, 6, 0),
1190                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 6) * 8, 1),
1191                 MIPS64_DMFC1(2, 7, 0),
1192                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 7) * 8, 1),
1193                 MIPS64_DMFC1(2, 8, 0),
1194                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 8) * 8, 1),
1195                 MIPS64_DMFC1(2, 9, 0),
1196                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 9) * 8, 1),
1197                 MIPS64_DMFC1(2, 10, 0),
1198                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 10) * 8, 1),
1199                 MIPS64_DMFC1(2, 11, 0),
1200                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 11) * 8, 1),
1201                 MIPS64_DMFC1(2, 12, 0),
1202                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 12) * 8, 1),
1203                 MIPS64_DMFC1(2, 13, 0),
1204                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 13) * 8, 1),
1205                 MIPS64_DMFC1(2, 14, 0),
1206                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 14) * 8, 1),
1207                 MIPS64_DMFC1(2, 15, 0),
1208                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 15) * 8, 1),
1209                 MIPS64_DMFC1(2, 16, 0),
1210                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 16) * 8, 1),
1211                 MIPS64_DMFC1(2, 17, 0),
1212                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 17) * 8, 1),
1213                 MIPS64_DMFC1(2, 18, 0),
1214                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 18) * 8, 1),
1215                 MIPS64_DMFC1(2, 19, 0),
1216                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 19) * 8, 1),
1217                 MIPS64_DMFC1(2, 20, 0),
1218                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 20) * 8, 1),
1219                 MIPS64_DMFC1(2, 21, 0),
1220                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 21) * 8, 1),
1221                 MIPS64_DMFC1(2, 22, 0),
1222                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 22) * 8, 1),
1223                 MIPS64_DMFC1(2, 23, 0),
1224                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 23) * 8, 1),
1225                 MIPS64_DMFC1(2, 24, 0),
1226                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 24) * 8, 1),
1227                 MIPS64_DMFC1(2, 25, 0),
1228                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 25) * 8, 1),
1229                 MIPS64_DMFC1(2, 26, 0),
1230                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 26) * 8, 1),
1231                 MIPS64_DMFC1(2, 27, 0),
1232                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 27) * 8, 1),
1233                 MIPS64_DMFC1(2, 28, 0),
1234                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 28) * 8, 1),
1235                 MIPS64_DMFC1(2, 29, 0),
1236                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 29) * 8, 1),
1237                 MIPS64_DMFC1(2, 30, 0),
1238                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 30) * 8, 1),
1239                 MIPS64_DMFC1(2, 31, 0),
1240                 MIPS64_SD(2, (MIPS64_NUM_CORE_C0_REGS + 31) * 8, 1),
1241                 MIPS64_LD(2, 0, 15),
1242                 MIPS64_LD(1, 0, 15),
1243                 MIPS64_SYNC,
1244                 /* b start */
1245                 MIPS64_B(NEG16(192)),
1246                 /* move COP0 DeSave to $15 */
1247                 MIPS64_DMFC0(15, 31, 0),
1248                 MIPS64_NOP,
1249                 MIPS64_NOP,
1250                 MIPS64_NOP,
1251                 MIPS64_NOP,
1252                 MIPS64_NOP,
1253                 MIPS64_NOP,
1254                 MIPS64_NOP,
1255                 MIPS64_NOP,
1256         };
1257
1258         LOG_DEBUG("enter mips64_pracc_exec");
1259         return mips64_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
1260                 0, NULL, MIPS64_NUM_REGS, regs);
1261 }
1262
1263 /* fastdata upload/download requires an initialized working area
1264  * to load the download code; it should not be called otherwise
1265  * fetch order from the fastdata area
1266  * 1. start addr
1267  * 2. end addr
1268  * 3. data ...
1269  */
1270 int mips64_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info,
1271                                struct working_area *source,
1272                                bool write_t, uint64_t addr,
1273                                unsigned count, uint64_t *buf)
1274 {
1275         uint32_t handler_code[] = {
1276                 /* caution when editing, table is modified below */
1277                 /* r15 points to the start of this code */
1278                 MIPS64_SD(8, MIPS64_FASTDATA_HANDLER_SIZE - 8, 15),
1279                 MIPS64_SD(9, MIPS64_FASTDATA_HANDLER_SIZE - 8 * 2, 15),
1280                 MIPS64_SD(10, MIPS64_FASTDATA_HANDLER_SIZE - 8 * 3, 15),
1281                 MIPS64_SD(11, MIPS64_FASTDATA_HANDLER_SIZE - 8 * 4, 15),
1282                 /* start of fastdata area in t0 */
1283                 MIPS64_LUI(8, UPPER16(MIPS64_PRACC_FASTDATA_AREA)),
1284                 MIPS64_ORI(8, 8, LOWER16(MIPS64_PRACC_FASTDATA_AREA)),
1285                 /* start addr in t1 */
1286                 MIPS64_LD(9, 0, 8),
1287                 /* end addr to t2 */
1288                 MIPS64_LD(10, 0, 8),
1289
1290                 /* loop: */
1291                 /* lw t3,[t8 | r9] */
1292                 /* 8 */ MIPS64_LD(11, 0, 0),
1293                 /* sw t3,[r9 | r8] */
1294                 /* 9 */ MIPS64_SD(11, 0, 0),
1295                 /* bne $t2,t1,loop */
1296                 MIPS64_BNE(10, 9, NEG16(3)),
1297                 /* addi t1,t1,4 */
1298                 MIPS64_DADDIU(9, 9, 8),
1299
1300                 MIPS64_LD(8, MIPS64_FASTDATA_HANDLER_SIZE - 8, 15),
1301                 MIPS64_LD(9, MIPS64_FASTDATA_HANDLER_SIZE - 8 * 2, 15),
1302                 MIPS64_LD(10, MIPS64_FASTDATA_HANDLER_SIZE - 8 * 3, 15),
1303                 MIPS64_LD(11, MIPS64_FASTDATA_HANDLER_SIZE - 8 * 4, 15),
1304
1305                 MIPS64_LUI(15, UPPER16(MIPS64_PRACC_TEXT)),
1306                 MIPS64_ORI(15, 15, LOWER16(MIPS64_PRACC_TEXT)),
1307                 /* jr start */
1308                 MIPS64_JR(15),
1309                 /* move COP0 DeSave to $15 */
1310                 MIPS64_DMFC0(15, 31, 0),
1311         };
1312
1313         uint32_t jmp_code[] = {
1314                 /* addr of working area added below */
1315                 /* 0 */ MIPS64_LUI(15, 0),
1316                 /* addr of working area added below */
1317                 /* 1 */ MIPS64_ORI(15, 15, 0),
1318                 /* jump to ram program */
1319                 MIPS64_JR(15),
1320                 MIPS64_NOP,
1321         };
1322
1323         int retval;
1324         unsigned i;
1325         uint32_t ejtag_ctrl, address32;
1326         uint64_t address, val;
1327
1328         if (source->size < MIPS64_FASTDATA_HANDLER_SIZE)
1329                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1330
1331         if (write_t) {
1332                 /* load data from probe at fastdata area */
1333                 handler_code[8] = MIPS64_LD(11, 0, 8);
1334                 /* store data to RAM @ r9 */
1335                 handler_code[9] = MIPS64_SD(11, 0, 9);
1336         } else {
1337                 /* load data from RAM @ r9 */
1338                 handler_code[8] = MIPS64_LD(11, 0, 9);
1339                 /* store data to probe at fastdata area */
1340                 handler_code[9] = MIPS64_SD(11, 0, 8);
1341         }
1342
1343         /* write program into RAM */
1344         if (write_t != ejtag_info->fast_access_save) {
1345                 mips64_pracc_write_mem(ejtag_info, source->address, 4,
1346                                        ARRAY_SIZE(handler_code), handler_code);
1347                 /* save previous operation to speed to any consecutive read/writes */
1348                 ejtag_info->fast_access_save = write_t;
1349         }
1350
1351         LOG_DEBUG("%s using " TARGET_ADDR_FMT " for write handler", __func__,
1352                   source->address);
1353         LOG_DEBUG("daddiu: %08" PRIx32, handler_code[11]);
1354
1355         jmp_code[0] |= UPPER16(source->address);
1356         jmp_code[1] |= LOWER16(source->address);
1357         mips64_pracc_exec(ejtag_info,
1358                           ARRAY_SIZE(jmp_code), jmp_code,
1359                           0, NULL, 0, NULL);
1360
1361         /* next fetch to dmseg should be in FASTDATA_AREA, check */
1362         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1363         retval = mips_ejtag_drscan_32(ejtag_info, &address32);
1364         if (retval != ERROR_OK)
1365                 return retval;
1366         address = 0xffffffffff200000ull | address32;
1367         if ((address & ~7ull) != MIPS64_PRACC_FASTDATA_AREA) {
1368                 LOG_ERROR("! @MIPS64_PRACC_FASTDATA_AREA (" TARGET_ADDR_FMT ")", address);
1369                 return ERROR_FAIL;
1370         }
1371         /* Send the load start address */
1372         val = addr;
1373         LOG_DEBUG("start: " TARGET_ADDR_FMT, val);
1374         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
1375         mips64_ejtag_fastdata_scan(ejtag_info, 1, &val);
1376
1377         retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
1378         if (retval != ERROR_OK)
1379                 return retval;
1380
1381         /* Send the load end address */
1382         val = addr + (count - 1) * 8;
1383         LOG_DEBUG("stop: " TARGET_ADDR_FMT, val);
1384         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
1385         mips64_ejtag_fastdata_scan(ejtag_info, 1, &val);
1386
1387         /* like in legacy code */
1388         unsigned num_clocks = 0;
1389         if (ejtag_info->mode != 0)
1390                 num_clocks = ((uint64_t)(ejtag_info->scan_delay) * adapter_get_speed_khz() + 500000) / 1000000;
1391         LOG_DEBUG("num_clocks=%d", num_clocks);
1392         for (i = 0; i < count; i++) {
1393                 jtag_add_clocks(num_clocks);
1394                 retval = mips64_ejtag_fastdata_scan(ejtag_info, write_t, buf++);
1395                 if (retval != ERROR_OK) {
1396                         LOG_ERROR("mips64_ejtag_fastdata_scan failed");
1397                         return retval;
1398                 }
1399         }
1400
1401         retval = jtag_execute_queue();
1402         if (retval != ERROR_OK) {
1403                 LOG_ERROR("jtag_execute_queue failed");
1404                 return retval;
1405         }
1406
1407         retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
1408         if (retval != ERROR_OK) {
1409                 LOG_ERROR("wait_for_pracc_rw failed");
1410                 return retval;
1411         }
1412
1413         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1414         retval = mips_ejtag_drscan_32(ejtag_info, &address32);
1415         if (retval != ERROR_OK) {
1416                 LOG_ERROR("mips_ejtag_drscan_32 failed");
1417                 return retval;
1418         }
1419
1420         address = 0xffffffffff200000ull | address32;
1421         if ((address & ~7ull) != MIPS64_PRACC_TEXT)
1422                 LOG_ERROR("mini program did not return to start");
1423
1424         return retval;
1425 }