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

   1 /***************************************************************************
   2  *   Copyright (C) 2013 by Franck Jullien                                  *
   3  *   elec4fun@gmail.com                                                    *
   4  *                                                                         *
   5  *   Inspired from adv_jtag_bridge which is:                                *
   6  *   Copyright (C) 2008-2010 Nathan Yawn                                   *
   7  *   nyawn@opencores.net                                                   *
   8  *                                                                         *
   9  *   And the Mohor interface version of this file which is:                *
  10  *   Copyright (C) 2011 by Julius Baxter                                   *
  11  *   julius@opencores.org                                                  *
  12  *                                                                         *
  13  *   This program is free software; you can redistribute it and/or modify  *
  14  *   it under the terms of the GNU General Public License as published by  *
  15  *   the Free Software Foundation; either version 2 of the License, or     *
  16  *   (at your option) any later version.                                   *
  17  *                                                                         *
  18  *   This program is distributed in the hope that it will be useful,       *
  19  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  20  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  21  *   GNU General Public License for more details.                          *
  22  *                                                                         *
  23  *   You should have received a copy of the GNU General Public License     *
  24  *   along with this program; if not, write to the                         *
  25  *   Free Software Foundation, Inc.,                                       *
  26  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  27  ***************************************************************************/
  28
  29 #ifdef HAVE_CONFIG_H
  30 #include "config.h"
  31 #endif
  32
  33 #include "or1k_tap.h"
  34 #include "or1k.h"
  35 #include "or1k_du.h"
  36
  37 #include <target/target.h>
  38 #include <jtag/jtag.h>
  39
  40 /* This an option to the adv debug unit.
  41  * If this is defined, status bits will be skipped on burst
  42  * reads and writes to improve download speeds.
  43  * This option must match the RTL configured option.
  44  */
  45 #define ADBG_USE_HISPEED                1
  46
  47 /* Definitions for the top-level debug unit.  This really just consists
  48  * of a single register, used to select the active debug module ("chain").
  49  */
  50 #define DBG_MODULE_SELECT_REG_SIZE      2
  51 #define DBG_MAX_MODULES                 4
  52
  53 #define DC_NONE                         -1
  54 #define DC_WISHBONE                     0
  55 #define DC_CPU0                         1
  56 #define DC_CPU1                         2
  57 #define DC_JSP                          3
  58
  59 /* CPU control register bits mask */
  60 #define DBG_CPU_CR_STALL                0x01
  61 #define DBG_CPU_CR_RESET                0x02
  62
  63 /* Polynomial for the CRC calculation
  64  * Yes, it's backwards.  Yes, this is on purpose.
  65  * The hardware is designed this way to save on logic and routing,
  66  * and it's really all the same to us here.
  67  */
  68 #define ADBG_CRC_POLY                   0xedb88320
  69
  70 /* These are for the internal registers in the Wishbone module
  71  * The first is the length of the index register,
  72  * the indexes of the various registers are defined after that.
  73  */
  74 #define DBG_WB_REG_SEL_LEN              1
  75 #define DBG_WB_REG_ERROR                0
  76
  77 /* Opcode definitions for the Wishbone module. */
  78 #define DBG_WB_OPCODE_LEN               4
  79 #define DBG_WB_CMD_NOP                  0x0
  80 #define DBG_WB_CMD_BWRITE8              0x1
  81 #define DBG_WB_CMD_BWRITE16             0x2
  82 #define DBG_WB_CMD_BWRITE32             0x3
  83 #define DBG_WB_CMD_BREAD8               0x5
  84 #define DBG_WB_CMD_BREAD16              0x6
  85 #define DBG_WB_CMD_BREAD32              0x7
  86 #define DBG_WB_CMD_IREG_WR              0x9
  87 #define DBG_WB_CMD_IREG_SEL             0xd
  88
  89 /* Internal register definitions for the CPU0 module. */
  90 #define DBG_CPU0_REG_SEL_LEN            1
  91 #define DBG_CPU0_REG_STATUS             0
  92
  93 /* Opcode definitions for the first CPU module. */
  94 #define DBG_CPU0_OPCODE_LEN             4
  95 #define DBG_CPU0_CMD_NOP                0x0
  96 #define DBG_CPU0_CMD_BWRITE32           0x3
  97 #define DBG_CPU0_CMD_BREAD32            0x7
  98 #define DBG_CPU0_CMD_IREG_WR            0x9
  99 #define DBG_CPU0_CMD_IREG_SEL           0xd
 100
 101 /* Internal register definitions for the CPU1 module. */
 102 #define DBG_CPU1_REG_SEL_LEN            1
 103 #define DBG_CPU1_REG_STATUS             0
 104
 105 /* Opcode definitions for the second CPU module. */
 106 #define DBG_CPU1_OPCODE_LEN             4
 107 #define DBG_CPU1_CMD_NOP                0x0
 108 #define DBG_CPU1_CMD_BWRITE32           0x3
 109 #define DBG_CPU1_CMD_BREAD32            0x7
 110 #define DBG_CPU1_CMD_IREG_WR            0x9
 111 #define DBG_CPU1_CMD_IREG_SEL           0xd
 112
 113 #define MAX_READ_STATUS_WAIT            10
 114 #define MAX_READ_BUSY_RETRY             2
 115 #define MAX_READ_CRC_RETRY              2
 116 #define MAX_WRITE_CRC_RETRY             2
 117 #define BURST_READ_READY                1
 118 #define MAX_BUS_ERRORS                  2
 119
 120 #define MAX_BURST_SIZE                  (4 * 1024)
 121
 122 #define STATUS_BYTES                    1
 123 #define CRC_LEN                         4
 124
 125 static struct or1k_du or1k_du_adv;
 126
 127 static const char * const chain_name[] = {"WISHBONE", "CPU0", "CPU1", "JSP"};
 128
 129 static uint32_t adbg_compute_crc(uint32_t crc, uint32_t data_in,
 130                                  int length_bits)
 131 {
 132         for (int i = 0; i < length_bits; i++) {
 133                 uint32_t d, c;
 134                 d = ((data_in >> i) & 0x1) ? 0xffffffff : 0;
 135                 c = (crc & 0x1) ? 0xffffffff : 0;
 136                 crc = crc >> 1;
 137                 crc = crc ^ ((d ^ c) & ADBG_CRC_POLY);
 138         }
 139
 140         return crc;
 141 }
 142
 143 static int find_status_bit(void *_buf, int len)
 144 {
 145         int i = 0;
 146         int count = 0;
 147         int ret = -1;
 148         uint8_t *buf = _buf;
 149
 150         while (!(buf[i] & (1 << count++)) && (i < len)) {
 151                 if (count == 8) {
 152                         count = 0;
 153                         i++;
 154                 }
 155         }
 156
 157         if (i < len)
 158                 ret = (i * 8) + count;
 159
 160         return ret;
 161 }
 162
 163 static int or1k_adv_jtag_init(struct or1k_jtag *jtag_info)
 164 {
 165         struct or1k_tap_ip *tap_ip = jtag_info->tap_ip;
 166
 167         int retval = tap_ip->init(jtag_info);
 168         if (retval != ERROR_OK) {
 169                 LOG_ERROR("TAP initialization failed");
 170                 return retval;
 171         }
 172
 173         /* TAP is now configured to communicate with debug interface */
 174         jtag_info->or1k_jtag_inited = 1;
 175
 176         /* TAP reset - not sure what state debug module chain is in now */
 177         jtag_info->or1k_jtag_module_selected = DC_NONE;
 178
 179         jtag_info->current_reg_idx = malloc(DBG_MAX_MODULES * sizeof(uint8_t));
 180         memset(jtag_info->current_reg_idx, 0, DBG_MAX_MODULES * sizeof(uint8_t));
 181
 182         if (or1k_du_adv.options & ADBG_USE_HISPEED)
 183                 LOG_INFO("adv debug unit is configured with option ADBG_USE_HISPEED");
 184
 185         LOG_DEBUG("Init done");
 186
 187         return ERROR_OK;
 188
 189 }
 190
 191 /* Selects one of the modules in the debug unit
 192  * (e.g. wishbone unit, CPU0, etc.)
 193  */
 194 static int adbg_select_module(struct or1k_jtag *jtag_info, int chain)
 195 {
 196         if (jtag_info->or1k_jtag_module_selected == chain)
 197                 return ERROR_OK;
 198
 199         /* MSB of the data out must be set to 1, indicating a module
 200          * select command
 201          */
 202         uint8_t data = chain | (1 << DBG_MODULE_SELECT_REG_SIZE);
 203
 204         LOG_DEBUG("Select module: %s", chain_name[chain]);
 205
 206         struct scan_field field;
 207
 208         field.num_bits = (DBG_MODULE_SELECT_REG_SIZE + 1);
 209         field.out_value = &data;
 210         field.in_value = NULL;
 211         jtag_add_dr_scan(jtag_info->tap, 1, &field, TAP_IDLE);
 212
 213         int retval = jtag_execute_queue();
 214         if (retval != ERROR_OK)
 215                 return retval;
 216
 217         jtag_info->or1k_jtag_module_selected = chain;
 218
 219         return ERROR_OK;
 220 }
 221
 222 /* Set the index of the desired register in the currently selected module
 223  * 1 bit module select command
 224  * 4 bits opcode
 225  * n bits index
 226  */
 227 static int adbg_select_ctrl_reg(struct or1k_jtag *jtag_info, uint8_t regidx)
 228 {
 229         int index_len;
 230         uint32_t opcode;
 231         uint32_t opcode_len;
 232
 233         /* If this reg is already selected, don't do a JTAG transaction */
 234         if (jtag_info->current_reg_idx[jtag_info->or1k_jtag_module_selected] == regidx)
 235                 return ERROR_OK;
 236
 237         switch (jtag_info->or1k_jtag_module_selected) {
 238         case DC_WISHBONE:
 239                 index_len = DBG_WB_REG_SEL_LEN;
 240                 opcode = DBG_WB_CMD_IREG_SEL;
 241                 opcode_len = DBG_WB_OPCODE_LEN;
 242                 break;
 243         case DC_CPU0:
 244                 index_len = DBG_CPU0_REG_SEL_LEN;
 245                 opcode = DBG_CPU0_CMD_IREG_SEL;
 246                 opcode_len = DBG_CPU0_OPCODE_LEN;
 247                 break;
 248         case DC_CPU1:
 249                 index_len = DBG_CPU1_REG_SEL_LEN;
 250                 opcode = DBG_CPU1_CMD_IREG_SEL;
 251                 opcode_len = DBG_CPU1_OPCODE_LEN;
 252                 break;
 253         default:
 254                 LOG_ERROR("Illegal debug chain selected (%i) while selecting control register",
 255                           jtag_info->or1k_jtag_module_selected);
 256                 return ERROR_FAIL;
 257         }
 258
 259         /* MSB must be 0 to access modules */
 260         uint32_t data = (opcode & ~(1 << opcode_len)) << index_len;
 261         data |= regidx;
 262
 263         struct scan_field field;
 264
 265         field.num_bits = (opcode_len + 1) + index_len;
 266         field.out_value = (uint8_t *)&data;
 267         field.in_value = NULL;
 268         jtag_add_dr_scan(jtag_info->tap, 1, &field, TAP_IDLE);
 269
 270         int retval = jtag_execute_queue();
 271         if (retval != ERROR_OK)
 272                 return retval;
 273
 274         jtag_info->current_reg_idx[jtag_info->or1k_jtag_module_selected] = regidx;
 275
 276         return ERROR_OK;
 277 }
 278
 279 /* Write control register (internal to the debug unit) */
 280 static int adbg_ctrl_write(struct or1k_jtag *jtag_info, uint8_t regidx,
 281                            uint32_t *cmd_data, int length_bits)
 282 {
 283         int index_len;
 284         uint32_t opcode;
 285         uint32_t opcode_len;
 286
 287         LOG_DEBUG("Write control register %" PRId8 ": 0x%08" PRIx32, regidx, cmd_data[0]);
 288
 289         int retval = adbg_select_ctrl_reg(jtag_info, regidx);
 290         if (retval != ERROR_OK) {
 291                 LOG_ERROR("Error while calling adbg_select_ctrl_reg");
 292                 return retval;
 293         }
 294
 295         switch (jtag_info->or1k_jtag_module_selected) {
 296         case DC_WISHBONE:
 297                 index_len = DBG_WB_REG_SEL_LEN;
 298                 opcode = DBG_WB_CMD_IREG_WR;
 299                 opcode_len = DBG_WB_OPCODE_LEN;
 300                 break;
 301         case DC_CPU0:
 302                 index_len = DBG_CPU0_REG_SEL_LEN;
 303                 opcode = DBG_CPU0_CMD_IREG_WR;
 304                 opcode_len = DBG_CPU0_OPCODE_LEN;
 305                 break;
 306         case DC_CPU1:
 307                 index_len = DBG_CPU1_REG_SEL_LEN;
 308                 opcode = DBG_CPU1_CMD_IREG_WR;
 309                 opcode_len = DBG_CPU1_OPCODE_LEN;
 310                 break;
 311         default:
 312                 LOG_ERROR("Illegal debug chain selected (%i) while doing control write",
 313                           jtag_info->or1k_jtag_module_selected);
 314                 return ERROR_FAIL;
 315         }
 316
 317         struct scan_field field[2];
 318
 319         /* MSB must be 0 to access modules */
 320         uint32_t data = (opcode & ~(1 << opcode_len)) << index_len;
 321         data |= regidx;
 322
 323         field[0].num_bits = length_bits;
 324         field[0].out_value = (uint8_t *)cmd_data;
 325         field[0].in_value = NULL;
 326
 327         field[1].num_bits = (opcode_len + 1) + index_len;
 328         field[1].out_value = (uint8_t *)&data;
 329         field[1].in_value = NULL;
 330
 331         jtag_add_dr_scan(jtag_info->tap, 2, field, TAP_IDLE);
 332
 333         return jtag_execute_queue();
 334 }
 335
 336 /* Reads control register (internal to the debug unit) */
 337 static int adbg_ctrl_read(struct or1k_jtag *jtag_info, uint32_t regidx,
 338                           uint32_t *data, int length_bits)
 339 {
 340
 341         int retval = adbg_select_ctrl_reg(jtag_info, regidx);
 342         if (retval != ERROR_OK) {
 343                 LOG_ERROR("Error while calling adbg_select_ctrl_reg");
 344                 return retval;
 345         }
 346
 347         int opcode_len;
 348         uint32_t opcode;
 349
 350         /* There is no 'read' command, We write a NOP to read */
 351         switch (jtag_info->or1k_jtag_module_selected) {
 352         case DC_WISHBONE:
 353                 opcode = DBG_WB_CMD_NOP;
 354                 opcode_len = DBG_WB_OPCODE_LEN;
 355                 break;
 356         case DC_CPU0:
 357                 opcode = DBG_CPU0_CMD_NOP;
 358                 opcode_len = DBG_CPU0_OPCODE_LEN;
 359                 break;
 360         case DC_CPU1:
 361                 opcode = DBG_CPU1_CMD_NOP;
 362                 opcode_len = DBG_CPU1_OPCODE_LEN;
 363                 break;
 364         default:
 365                 LOG_ERROR("Illegal debug chain selected (%i) while doing control read",
 366                           jtag_info->or1k_jtag_module_selected);
 367                  return ERROR_FAIL;
 368         }
 369
 370         /* Zero MSB = op for module, not top-level debug unit */
 371         uint32_t outdata = opcode & ~(0x1 << opcode_len);
 372
 373         struct scan_field field[2];
 374
 375         field[0].num_bits = length_bits;
 376         field[0].out_value = NULL;
 377         field[0].in_value = (uint8_t *)data;
 378
 379         field[1].num_bits = opcode_len + 1;
 380         field[1].out_value = (uint8_t *)&outdata;
 381         field[1].in_value = NULL;
 382
 383         jtag_add_dr_scan(jtag_info->tap, 2, field, TAP_IDLE);
 384
 385         return jtag_execute_queue();
 386 }
 387
 388 /* sends out a burst command to the selected module in the debug unit (MSB to LSB):
 389  * 1-bit module command
 390  * 4-bit opcode
 391  * 32-bit address
 392  * 16-bit length (of the burst, in words)
 393  */
 394 static int adbg_burst_command(struct or1k_jtag *jtag_info, uint32_t opcode,
 395                               uint32_t address, uint16_t length_words)
 396 {
 397         uint32_t data[2];
 398
 399         /* Set up the data */
 400         data[0] = length_words | (address << 16);
 401         /* MSB must be 0 to access modules */
 402         data[1] = ((address >> 16) | ((opcode & 0xf) << 16)) & ~(0x1 << 20);
 403
 404         struct scan_field field;
 405
 406         field.num_bits = 53;
 407         field.out_value = (uint8_t *)&data[0];
 408         field.in_value = NULL;
 409
 410         jtag_add_dr_scan(jtag_info->tap, 1, &field, TAP_IDLE);
 411
 412         return jtag_execute_queue();
 413 }
 414
 415 static int adbg_wb_burst_read(struct or1k_jtag *jtag_info, int size,
 416                               int count, uint32_t start_address, uint8_t *data)
 417 {
 418         int retry_full_crc = 0;
 419         int retry_full_busy = 0;
 420         int retval;
 421         uint8_t opcode;
 422
 423         LOG_DEBUG("Doing burst read, word size %d, word count %d, start address 0x%08" PRIx32,
 424                   size, count, start_address);
 425
 426         /* Select the appropriate opcode */
 427         switch (jtag_info->or1k_jtag_module_selected) {
 428         case DC_WISHBONE:
 429                 if (size == 1)
 430                         opcode = DBG_WB_CMD_BREAD8;
 431                 else if (size == 2)
 432                         opcode = DBG_WB_CMD_BREAD16;
 433                 else if (size == 4)
 434                         opcode = DBG_WB_CMD_BREAD32;
 435                 else {
 436                         LOG_WARNING("Tried burst read with invalid word size (%d),"
 437                                   "defaulting to 4-byte words", size);
 438                         opcode = DBG_WB_CMD_BREAD32;
 439                 }
 440                 break;
 441         case DC_CPU0:
 442                 if (size == 4)
 443                         opcode = DBG_CPU0_CMD_BREAD32;
 444                 else {
 445                         LOG_WARNING("Tried burst read with invalid word size (%d),"
 446                                   "defaulting to 4-byte words", size);
 447                         opcode = DBG_CPU0_CMD_BREAD32;
 448                 }
 449                 break;
 450         case DC_CPU1:
 451                 if (size == 4)
 452                         opcode = DBG_CPU1_CMD_BREAD32;
 453                 else {
 454                         LOG_WARNING("Tried burst read with invalid word size (%d),"
 455                                   "defaulting to 4-byte words", size);
 456                         opcode = DBG_CPU0_CMD_BREAD32;
 457                 }
 458                 break;
 459         default:
 460                 LOG_ERROR("Illegal debug chain selected (%i) while doing burst read",
 461                           jtag_info->or1k_jtag_module_selected);
 462                 return ERROR_FAIL;
 463         }
 464
 465         int total_size_bytes = count * size;
 466         struct scan_field field;
 467         uint8_t *in_buffer = malloc(total_size_bytes + CRC_LEN + STATUS_BYTES);
 468
 469 retry_read_full:
 470
 471         /* Send the BURST READ command, returns TAP to idle state */
 472         retval = adbg_burst_command(jtag_info, opcode, start_address, count);
 473         if (retval != ERROR_OK)
 474                 goto out;
 475
 476         field.num_bits = (total_size_bytes + CRC_LEN + STATUS_BYTES) * 8;
 477         field.out_value = NULL;
 478         field.in_value = in_buffer;
 479
 480         jtag_add_dr_scan(jtag_info->tap, 1, &field, TAP_IDLE);
 481
 482         retval = jtag_execute_queue();
 483         if (retval != ERROR_OK)
 484                 goto out;
 485
 486         /* Look for the start bit in the first (STATUS_BYTES * 8) bits */
 487         int shift = find_status_bit(in_buffer, STATUS_BYTES);
 488
 489         /* We expect the status bit to be in the first byte */
 490         if (shift < 0) {
 491                 if (retry_full_busy++ < MAX_READ_BUSY_RETRY) {
 492                         LOG_WARNING("Burst read timed out");
 493                         goto retry_read_full;
 494                 } else {
 495                         LOG_ERROR("Burst read failed");
 496                         retval = ERROR_FAIL;
 497                         goto out;
 498                 }
 499         }
 500
 501         buffer_shr(in_buffer, total_size_bytes + CRC_LEN + STATUS_BYTES, shift);
 502
 503         uint32_t crc_read;
 504         memcpy(data, in_buffer, total_size_bytes);
 505         memcpy(&crc_read, &in_buffer[total_size_bytes], 4);
 506
 507         uint32_t crc_calc = 0xffffffff;
 508         for (int i = 0; i < total_size_bytes; i++)
 509                 crc_calc = adbg_compute_crc(crc_calc, data[i], 8);
 510
 511         if (crc_calc != crc_read) {
 512                 LOG_WARNING("CRC ERROR! Computed 0x%08" PRIx32 ", read CRC 0x%08" PRIx32, crc_calc, crc_read);
 513                 if (retry_full_crc++ < MAX_READ_CRC_RETRY)
 514                         goto retry_read_full;
 515                 else {
 516                         LOG_ERROR("Burst read failed");
 517                         retval = ERROR_FAIL;
 518                         goto out;
 519                 }
 520         } else
 521                 LOG_DEBUG("CRC OK!");
 522
 523         /* Now, read the error register, and retry/recompute as necessary */
 524         if (jtag_info->or1k_jtag_module_selected == DC_WISHBONE &&
 525             !(or1k_du_adv.options & ADBG_USE_HISPEED)) {
 526
 527                 uint32_t err_data[2] = {0, 0};
 528                 uint32_t addr;
 529                 int bus_error_retries = 0;
 530
 531                 /* First, just get 1 bit...read address only if necessary */
 532                 retval = adbg_ctrl_read(jtag_info, DBG_WB_REG_ERROR, err_data, 1);
 533                 if (retval != ERROR_OK)
 534                         goto out;
 535
 536                 /* Then we have a problem */
 537                 if (err_data[0] & 0x1) {
 538
 539                         retval = adbg_ctrl_read(jtag_info, DBG_WB_REG_ERROR, err_data, 33);
 540                         if (retval != ERROR_OK)
 541                                 goto out;
 542
 543                         addr = (err_data[0] >> 1) | (err_data[1] << 31);
 544                         LOG_WARNING("WB bus error during burst read, address 0x%08" PRIx32 ", retrying!", addr);
 545
 546                         bus_error_retries++;
 547                         if (bus_error_retries > MAX_BUS_ERRORS) {
 548                                 LOG_ERROR("Max WB bus errors reached during burst read");
 549                                 retval = ERROR_FAIL;
 550                                 goto out;
 551                         }
 552
 553                         /* Don't call retry_do(), a JTAG reset won't help a WB bus error */
 554                         /* Write 1 bit, to reset the error register */
 555                         err_data[0] = 1;
 556                         retval = adbg_ctrl_write(jtag_info, DBG_WB_REG_ERROR, err_data, 1);
 557                         if (retval != ERROR_OK)
 558                                 goto out;
 559
 560                         goto retry_read_full;
 561                 }
 562         }
 563
 564 out:
 565         free(in_buffer);
 566
 567         return retval;
 568 }
 569
 570 /* Set up and execute a burst write to a contiguous set of addresses */
 571 static int adbg_wb_burst_write(struct or1k_jtag *jtag_info, const uint8_t *data, int size,
 572                         int count, unsigned long start_address)
 573 {
 574         int retry_full_crc = 0;
 575         int retval;
 576         uint8_t opcode;
 577
 578         LOG_DEBUG("Doing burst write, word size %d, word count %d,"
 579                   "start address 0x%08lx", size, count, start_address);
 580
 581         /* Select the appropriate opcode */
 582         switch (jtag_info->or1k_jtag_module_selected) {
 583         case DC_WISHBONE:
 584                 if (size == 1)
 585                         opcode = DBG_WB_CMD_BWRITE8;
 586                 else if (size == 2)
 587                         opcode = DBG_WB_CMD_BWRITE16;
 588                 else if (size == 4)
 589                         opcode = DBG_WB_CMD_BWRITE32;
 590                 else {
 591                         LOG_DEBUG("Tried WB burst write with invalid word size (%d),"
 592                                   "defaulting to 4-byte words", size);
 593                         opcode = DBG_WB_CMD_BWRITE32;
 594                 }
 595                 break;
 596         case DC_CPU0:
 597                 if (size == 4)
 598                         opcode = DBG_CPU0_CMD_BWRITE32;
 599                 else {
 600                         LOG_DEBUG("Tried CPU0 burst write with invalid word size (%d),"
 601                                   "defaulting to 4-byte words", size);
 602                         opcode = DBG_CPU0_CMD_BWRITE32;
 603                 }
 604                 break;
 605         case DC_CPU1:
 606                 if (size == 4)
 607                         opcode = DBG_CPU1_CMD_BWRITE32;
 608                 else {
 609                         LOG_DEBUG("Tried CPU1 burst write with invalid word size (%d),"
 610                                   "defaulting to 4-byte words", size);
 611                         opcode = DBG_CPU0_CMD_BWRITE32;
 612                 }
 613                 break;
 614         default:
 615                 LOG_ERROR("Illegal debug chain selected (%i) while doing burst write",
 616                           jtag_info->or1k_jtag_module_selected);
 617                 return ERROR_FAIL;
 618         }
 619
 620 retry_full_write:
 621
 622         /* Send the BURST WRITE command, returns TAP to idle state */
 623         retval = adbg_burst_command(jtag_info, opcode, start_address, count);
 624         if (retval != ERROR_OK)
 625                 return retval;
 626
 627         struct scan_field field[3];
 628
 629         /* Write a start bit so it knows when to start counting */
 630         uint8_t value = 1;
 631         field[0].num_bits = 1;
 632         field[0].out_value = &value;
 633         field[0].in_value = NULL;
 634
 635         uint32_t crc_calc = 0xffffffff;
 636         for (int i = 0; i < (count * size); i++)
 637                 crc_calc = adbg_compute_crc(crc_calc, data[i], 8);
 638
 639         field[1].num_bits = count * size * 8;
 640         field[1].out_value = data;
 641         field[1].in_value = NULL;
 642
 643         field[2].num_bits = 32;
 644         field[2].out_value = (uint8_t *)&crc_calc;
 645         field[2].in_value = NULL;
 646
 647         jtag_add_dr_scan(jtag_info->tap, 3, field, TAP_DRSHIFT);
 648
 649         /* Read the 'CRC match' bit, and go to idle */
 650         field[0].num_bits = 1;
 651         field[0].out_value = NULL;
 652         field[0].in_value = &value;
 653         jtag_add_dr_scan(jtag_info->tap, 1, field, TAP_IDLE);
 654
 655         retval = jtag_execute_queue();
 656         if (retval != ERROR_OK)
 657                 return retval;
 658
 659         if (!value) {
 660                 LOG_WARNING("CRC ERROR! match bit after write is %" PRIi8 " (computed CRC 0x%08" PRIx32 ")", value, crc_calc);
 661                 if (retry_full_crc++ < MAX_WRITE_CRC_RETRY)
 662                         goto retry_full_write;
 663                 else
 664                         return ERROR_FAIL;
 665         } else
 666                 LOG_DEBUG("CRC OK!\n");
 667
 668         /* Now, read the error register, and retry/recompute as necessary */
 669         if (jtag_info->or1k_jtag_module_selected == DC_WISHBONE &&
 670             !(or1k_du_adv.options & ADBG_USE_HISPEED)) {
 671                 uint32_t addr;
 672                 int bus_error_retries = 0;
 673                 uint32_t err_data[2] = {0, 0};
 674
 675                 /* First, just get 1 bit...read address only if necessary */
 676                 retval = adbg_ctrl_read(jtag_info, DBG_WB_REG_ERROR, err_data, 1);
 677                 if (retval != ERROR_OK)
 678                         return retval;
 679
 680                 /* Then we have a problem */
 681                 if (err_data[0] & 0x1) {
 682
 683                         retval = adbg_ctrl_read(jtag_info, DBG_WB_REG_ERROR, err_data, 33);
 684                         if (retval != ERROR_OK)
 685                                 return retval;
 686
 687                         addr = (err_data[0] >> 1) | (err_data[1] << 31);
 688                         LOG_WARNING("WB bus error during burst write, address 0x%08" PRIx32 ", retrying!", addr);
 689
 690                         bus_error_retries++;
 691                         if (bus_error_retries > MAX_BUS_ERRORS) {
 692                                 LOG_ERROR("Max WB bus errors reached during burst read");
 693                                 retval = ERROR_FAIL;
 694                                 return retval;
 695                         }
 696
 697                         /* Don't call retry_do(), a JTAG reset won't help a WB bus error */
 698                         /* Write 1 bit, to reset the error register */
 699                         err_data[0] = 1;
 700                         retval = adbg_ctrl_write(jtag_info, DBG_WB_REG_ERROR, err_data, 1);
 701                         if (retval != ERROR_OK)
 702                                 return retval;
 703
 704                         goto retry_full_write;
 705                 }
 706         }
 707
 708         return ERROR_OK;
 709 }
 710
 711 /* Currently hard set in functions to 32-bits */
 712 static int or1k_adv_jtag_read_cpu(struct or1k_jtag *jtag_info,
 713                 uint32_t addr, int count, uint32_t *value)
 714 {
 715         int retval;
 716         if (!jtag_info->or1k_jtag_inited) {
 717                 retval = or1k_adv_jtag_init(jtag_info);
 718                 if (retval != ERROR_OK)
 719                         return retval;
 720         }
 721
 722         retval = adbg_select_module(jtag_info, DC_CPU0);
 723         if (retval != ERROR_OK)
 724                 return retval;
 725
 726         return adbg_wb_burst_read(jtag_info, 4, count, addr, (uint8_t *)value);
 727 }
 728
 729 static int or1k_adv_jtag_write_cpu(struct or1k_jtag *jtag_info,
 730                 uint32_t addr, int count, const uint32_t *value)
 731 {
 732         int retval;
 733         if (!jtag_info->or1k_jtag_inited) {
 734                 retval = or1k_adv_jtag_init(jtag_info);
 735                 if (retval != ERROR_OK)
 736                         return retval;
 737         }
 738
 739         retval = adbg_select_module(jtag_info, DC_CPU0);
 740         if (retval != ERROR_OK)
 741                 return retval;
 742
 743         return adbg_wb_burst_write(jtag_info, (uint8_t *)value, 4, count, addr);
 744 }
 745
 746 static int or1k_adv_cpu_stall(struct or1k_jtag *jtag_info, int action)
 747 {
 748         int retval;
 749         if (!jtag_info->or1k_jtag_inited) {
 750                 retval = or1k_adv_jtag_init(jtag_info);
 751                 if (retval != ERROR_OK)
 752                         return retval;
 753         }
 754
 755         retval = adbg_select_module(jtag_info, DC_CPU0);
 756         if (retval != ERROR_OK)
 757                 return retval;
 758
 759         uint32_t cpu_cr;
 760         retval = adbg_ctrl_read(jtag_info, DBG_CPU0_REG_STATUS, &cpu_cr, 2);
 761         if (retval != ERROR_OK)
 762                 return retval;
 763
 764         if (action == CPU_STALL)
 765                 cpu_cr |= DBG_CPU_CR_STALL;
 766         else
 767                 cpu_cr &= ~DBG_CPU_CR_STALL;
 768
 769         retval = adbg_select_module(jtag_info, DC_CPU0);
 770         if (retval != ERROR_OK)
 771                 return retval;
 772
 773         return adbg_ctrl_write(jtag_info, DBG_CPU0_REG_STATUS, &cpu_cr, 2);
 774 }
 775
 776 static int or1k_adv_is_cpu_running(struct or1k_jtag *jtag_info, int *running)
 777 {
 778         int retval;
 779         if (!jtag_info->or1k_jtag_inited) {
 780                 retval = or1k_adv_jtag_init(jtag_info);
 781                 if (retval != ERROR_OK)
 782                         return retval;
 783         }
 784
 785         int current = jtag_info->or1k_jtag_module_selected;
 786
 787         retval = adbg_select_module(jtag_info, DC_CPU0);
 788         if (retval != ERROR_OK)
 789                 return retval;
 790
 791         uint32_t cpu_cr = 0;
 792         retval = adbg_ctrl_read(jtag_info, DBG_CPU0_REG_STATUS, &cpu_cr, 2);
 793         if (retval != ERROR_OK)
 794                 return retval;
 795
 796         if (cpu_cr & DBG_CPU_CR_STALL)
 797                 *running = 0;
 798         else
 799                 *running = 1;
 800
 801         if (current != DC_NONE) {
 802                 retval = adbg_select_module(jtag_info, current);
 803                 if (retval != ERROR_OK)
 804                         return retval;
 805         }
 806
 807         return ERROR_OK;
 808 }
 809
 810 static int or1k_adv_cpu_reset(struct or1k_jtag *jtag_info, int action)
 811 {
 812         int retval;
 813         if (!jtag_info->or1k_jtag_inited) {
 814                 retval = or1k_adv_jtag_init(jtag_info);
 815                 if (retval != ERROR_OK)
 816                         return retval;
 817         }
 818
 819         retval = adbg_select_module(jtag_info, DC_CPU0);
 820         if (retval != ERROR_OK)
 821                 return retval;
 822
 823         uint32_t cpu_cr;
 824         retval = adbg_ctrl_read(jtag_info, DBG_CPU0_REG_STATUS, &cpu_cr, 2);
 825         if (retval != ERROR_OK)
 826                 return retval;
 827
 828         if (action == CPU_RESET)
 829                 cpu_cr |= DBG_CPU_CR_RESET;
 830         else
 831                 cpu_cr &= ~DBG_CPU_CR_RESET;
 832
 833         retval = adbg_select_module(jtag_info, DC_CPU0);
 834         if (retval != ERROR_OK)
 835                 return retval;
 836
 837         return adbg_ctrl_write(jtag_info, DBG_CPU0_REG_STATUS, &cpu_cr, 2);
 838 }
 839
 840 static int or1k_adv_jtag_read_memory(struct or1k_jtag *jtag_info,
 841                             uint32_t addr, uint32_t size, int count, uint8_t *buffer)
 842 {
 843         LOG_DEBUG("Reading WB%" PRId32 " at 0x%08" PRIx32, size * 8, addr);
 844
 845         int retval;
 846         if (!jtag_info->or1k_jtag_inited) {
 847                 retval = or1k_adv_jtag_init(jtag_info);
 848                 if (retval != ERROR_OK)
 849                         return retval;
 850         }
 851
 852         retval = adbg_select_module(jtag_info, DC_WISHBONE);
 853         if (retval != ERROR_OK)
 854                 return retval;
 855
 856         int block_count_left = count;
 857         uint32_t block_count_address = addr;
 858         uint8_t *block_count_buffer = buffer;
 859
 860         while (block_count_left) {
 861
 862                 int blocks_this_round = (block_count_left > MAX_BURST_SIZE) ?
 863                         MAX_BURST_SIZE : block_count_left;
 864
 865                 retval = adbg_wb_burst_read(jtag_info, size, blocks_this_round,
 866                                             block_count_address, block_count_buffer);
 867                 if (retval != ERROR_OK)
 868                         return retval;
 869
 870                 block_count_left -= blocks_this_round;
 871                 block_count_address += size * MAX_BURST_SIZE;
 872                 block_count_buffer += size * MAX_BURST_SIZE;
 873         }
 874
 875         /* The adv_debug_if always return words and half words in
 876          * little-endian order no matter what the target endian is.
 877          * So if the target endian is big, change the order.
 878          */
 879
 880         struct target *target = jtag_info->target;
 881         if ((target->endianness == TARGET_BIG_ENDIAN) && (size != 1)) {
 882                 switch (size) {
 883                 case 4:
 884                         buf_bswap32(buffer, buffer, size * count);
 885                         break;
 886                 case 2:
 887                         buf_bswap16(buffer, buffer, size * count);
 888                         break;
 889                 }
 890         }
 891
 892         return ERROR_OK;
 893 }
 894
 895 static int or1k_adv_jtag_write_memory(struct or1k_jtag *jtag_info,
 896                              uint32_t addr, uint32_t size, int count, const uint8_t *buffer)
 897 {
 898         LOG_DEBUG("Writing WB%" PRId32 " at 0x%08" PRIx32, size * 8, addr);
 899
 900         int retval;
 901         if (!jtag_info->or1k_jtag_inited) {
 902                 retval = or1k_adv_jtag_init(jtag_info);
 903                 if (retval != ERROR_OK)
 904                         return retval;
 905         }
 906
 907         retval = adbg_select_module(jtag_info, DC_WISHBONE);
 908         if (retval != ERROR_OK)
 909                 return retval;
 910
 911         /* The adv_debug_if wants words and half words in little-endian
 912          * order no matter what the target endian is. So if the target
 913          * endian is big, change the order.
 914          */
 915
 916         void *t = NULL;
 917         struct target *target = jtag_info->target;
 918         if ((target->endianness == TARGET_BIG_ENDIAN) && (size != 1)) {
 919                 t = malloc(count * size * sizeof(uint8_t));
 920                 if (t == NULL) {
 921                         LOG_ERROR("Out of memory");
 922                         return ERROR_FAIL;
 923                 }
 924
 925                 switch (size) {
 926                 case 4:
 927                         buf_bswap32(t, buffer, size * count);
 928                         break;
 929                 case 2:
 930                         buf_bswap16(t, buffer, size * count);
 931                         break;
 932                 }
 933                 buffer = t;
 934         }
 935
 936         int block_count_left = count;
 937         uint32_t block_count_address = addr;
 938         uint8_t *block_count_buffer = (uint8_t *)buffer;
 939
 940         while (block_count_left) {
 941
 942                 int blocks_this_round = (block_count_left > MAX_BURST_SIZE) ?
 943                         MAX_BURST_SIZE : block_count_left;
 944
 945                 retval = adbg_wb_burst_write(jtag_info, block_count_buffer,
 946                                              size, blocks_this_round,
 947                                              block_count_address);
 948                 if (retval != ERROR_OK) {
 949                         if (t != NULL)
 950                                 free(t);
 951                         return retval;
 952                 }
 953
 954                 block_count_left -= blocks_this_round;
 955                 block_count_address += size * MAX_BURST_SIZE;
 956                 block_count_buffer += size * MAX_BURST_SIZE;
 957         }
 958
 959         if (t != NULL)
 960                 free(t);
 961
 962         return ERROR_OK;
 963 }
 964
 965 static struct or1k_du or1k_du_adv = {
 966         .name                     = "adv",
 967         .options                  = ADBG_USE_HISPEED,
 968         .or1k_jtag_init           = or1k_adv_jtag_init,
 969
 970         .or1k_is_cpu_running      = or1k_adv_is_cpu_running,
 971         .or1k_cpu_stall           = or1k_adv_cpu_stall,
 972         .or1k_cpu_reset           = or1k_adv_cpu_reset,
 973
 974         .or1k_jtag_read_cpu       = or1k_adv_jtag_read_cpu,
 975         .or1k_jtag_write_cpu      = or1k_adv_jtag_write_cpu,
 976
 977         .or1k_jtag_read_memory    = or1k_adv_jtag_read_memory,
 978         .or1k_jtag_write_memory   = or1k_adv_jtag_write_memory
 979 };
 980
 981 int or1k_du_adv_register(void)
 982 {
 983         list_add_tail(&or1k_du_adv.list, &du_list);
 984         return 0;
 985 }