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

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