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

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