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