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