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