git.sur5r.net Git - openocd/blob - src/target/openrisc/or1k_du_adv.c

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