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