git.gag.com Git - fw/openocd/blob - src/jtag/aice/aice_usb.c

   1 /***************************************************************************
   2  *   Copyright (C) 2013 by Andes Technology                                *
   3  *   Hsiangkai Wang <hkwang@andestech.com>                                 *
   4  *                                                                         *
   5  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program; if not, write to the                         *
  17  *   Free Software Foundation, Inc.,                                       *
  18  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
  19  ***************************************************************************/
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include <jtag/drivers/libusb_common.h>
  25 #include <helper/log.h>
  26 #include <helper/time_support.h>
  27 #include <target/target.h>
  28 #include <jtag/jtag.h>
  29 #include <target/nds32_insn.h>
  30 #include <target/nds32_reg.h>
  31 #include "aice_usb.h"
  32
  33
  34 /* Global USB buffers */
  35 static uint8_t usb_in_buffer[AICE_IN_BUFFER_SIZE];
  36 static uint8_t usb_out_buffer[AICE_OUT_BUFFER_SIZE];
  37 static uint32_t jtag_clock;
  38 static struct aice_usb_handler_s aice_handler;
  39 /* AICE max retry times. If AICE command timeout, retry it. */
  40 static int aice_max_retry_times = 50;
  41 /* Default endian is little endian. */
  42 static enum aice_target_endian data_endian;
  43
  44 /* Constants for AICE command format length */
  45 static const int32_t AICE_FORMAT_HTDA = 3;
  46 static const int32_t AICE_FORMAT_HTDC   = 7;
  47 static const int32_t AICE_FORMAT_HTDMA = 4;
  48 static const int32_t AICE_FORMAT_HTDMB = 8;
  49 static const int32_t AICE_FORMAT_HTDMC = 8;
  50 static const int32_t AICE_FORMAT_HTDMD = 12;
  51 static const int32_t AICE_FORMAT_DTHA   = 6;
  52 static const int32_t AICE_FORMAT_DTHB   = 2;
  53 static const int32_t AICE_FORMAT_DTHMA = 8;
  54 static const int32_t AICE_FORMAT_DTHMB = 4;
  55
  56 /* Constants for AICE command */
  57 static const uint8_t AICE_CMD_SCAN_CHAIN = 0x00;
  58 static const uint8_t AICE_CMD_T_READ_MISC = 0x20;
  59 static const uint8_t AICE_CMD_T_READ_EDMSR = 0x21;
  60 static const uint8_t AICE_CMD_T_READ_DTR = 0x22;
  61 static const uint8_t AICE_CMD_T_READ_MEM_B = 0x24;
  62 static const uint8_t AICE_CMD_T_READ_MEM_H = 0x25;
  63 static const uint8_t AICE_CMD_T_READ_MEM = 0x26;
  64 static const uint8_t AICE_CMD_T_FASTREAD_MEM = 0x27;
  65 static const uint8_t AICE_CMD_T_WRITE_MISC = 0x28;
  66 static const uint8_t AICE_CMD_T_WRITE_EDMSR     = 0x29;
  67 static const uint8_t AICE_CMD_T_WRITE_DTR = 0x2A;
  68 static const uint8_t AICE_CMD_T_WRITE_DIM = 0x2B;
  69 static const uint8_t AICE_CMD_T_WRITE_MEM_B = 0x2C;
  70 static const uint8_t AICE_CMD_T_WRITE_MEM_H = 0x2D;
  71 static const uint8_t AICE_CMD_T_WRITE_MEM = 0x2E;
  72 static const uint8_t AICE_CMD_T_FASTWRITE_MEM = 0x2F;
  73 static const uint8_t AICE_CMD_T_EXECUTE = 0x3E;
  74 static const uint8_t AICE_CMD_READ_CTRL = 0x50;
  75 static const uint8_t AICE_CMD_WRITE_CTRL = 0x51;
  76 static const uint8_t AICE_CMD_BATCH_BUFFER_READ = 0x60;
  77 static const uint8_t AICE_CMD_READ_DTR_TO_BUFFER = 0x61;
  78 static const uint8_t AICE_CMD_BATCH_BUFFER_WRITE = 0x68;
  79 static const uint8_t AICE_CMD_WRITE_DTR_FROM_BUFFER = 0x69;
  80
  81 /***************************************************************************/
  82 /* AICE commands' pack/unpack functions */
  83 static void aice_pack_htda(uint8_t cmd_code, uint8_t extra_word_length,
  84                 uint32_t address)
  85 {
  86         usb_out_buffer[0] = cmd_code;
  87         usb_out_buffer[1] = extra_word_length;
  88         usb_out_buffer[2] = (uint8_t)(address & 0xFF);
  89 }
  90
  91 static void aice_pack_htdc(uint8_t cmd_code, uint8_t extra_word_length,
  92                 uint32_t address, uint32_t word, enum aice_target_endian access_endian)
  93 {
  94         usb_out_buffer[0] = cmd_code;
  95         usb_out_buffer[1] = extra_word_length;
  96         usb_out_buffer[2] = (uint8_t)(address & 0xFF);
  97         if (access_endian == AICE_BIG_ENDIAN) {
  98                 usb_out_buffer[6] = (uint8_t)((word >> 24) & 0xFF);
  99                 usb_out_buffer[5] = (uint8_t)((word >> 16) & 0xFF);
 100                 usb_out_buffer[4] = (uint8_t)((word >> 8) & 0xFF);
 101                 usb_out_buffer[3] = (uint8_t)(word & 0xFF);
 102         } else {
 103                 usb_out_buffer[3] = (uint8_t)((word >> 24) & 0xFF);
 104                 usb_out_buffer[4] = (uint8_t)((word >> 16) & 0xFF);
 105                 usb_out_buffer[5] = (uint8_t)((word >> 8) & 0xFF);
 106                 usb_out_buffer[6] = (uint8_t)(word & 0xFF);
 107         }
 108 }
 109
 110 static void aice_pack_htdma(uint8_t cmd_code, uint8_t target_id,
 111                 uint8_t extra_word_length, uint32_t address)
 112 {
 113         usb_out_buffer[0] = cmd_code;
 114         usb_out_buffer[1] = target_id;
 115         usb_out_buffer[2] = extra_word_length;
 116         usb_out_buffer[3] = (uint8_t)(address & 0xFF);
 117 }
 118
 119 static void aice_pack_htdmb(uint8_t cmd_code, uint8_t target_id,
 120                 uint8_t extra_word_length, uint32_t address)
 121 {
 122         usb_out_buffer[0] = cmd_code;
 123         usb_out_buffer[1] = target_id;
 124         usb_out_buffer[2] = extra_word_length;
 125         usb_out_buffer[3] = 0;
 126         usb_out_buffer[4] = (uint8_t)((address >> 24) & 0xFF);
 127         usb_out_buffer[5] = (uint8_t)((address >> 16) & 0xFF);
 128         usb_out_buffer[6] = (uint8_t)((address >> 8) & 0xFF);
 129         usb_out_buffer[7] = (uint8_t)(address & 0xFF);
 130 }
 131
 132 static void aice_pack_htdmc(uint8_t cmd_code, uint8_t target_id,
 133                 uint8_t extra_word_length, uint32_t address, uint32_t word,
 134                 enum aice_target_endian access_endian)
 135 {
 136         usb_out_buffer[0] = cmd_code;
 137         usb_out_buffer[1] = target_id;
 138         usb_out_buffer[2] = extra_word_length;
 139         usb_out_buffer[3] = (uint8_t)(address & 0xFF);
 140         if (access_endian == AICE_BIG_ENDIAN) {
 141                 usb_out_buffer[7] = (uint8_t)((word >> 24) & 0xFF);
 142                 usb_out_buffer[6] = (uint8_t)((word >> 16) & 0xFF);
 143                 usb_out_buffer[5] = (uint8_t)((word >> 8) & 0xFF);
 144                 usb_out_buffer[4] = (uint8_t)(word & 0xFF);
 145         } else {
 146                 usb_out_buffer[4] = (uint8_t)((word >> 24) & 0xFF);
 147                 usb_out_buffer[5] = (uint8_t)((word >> 16) & 0xFF);
 148                 usb_out_buffer[6] = (uint8_t)((word >> 8) & 0xFF);
 149                 usb_out_buffer[7] = (uint8_t)(word & 0xFF);
 150         }
 151 }
 152
 153 static void aice_pack_htdmc_multiple_data(uint8_t cmd_code, uint8_t target_id,
 154                 uint8_t extra_word_length, uint32_t address, uint32_t *word,
 155                 uint8_t num_of_words, enum aice_target_endian access_endian)
 156 {
 157         usb_out_buffer[0] = cmd_code;
 158         usb_out_buffer[1] = target_id;
 159         usb_out_buffer[2] = extra_word_length;
 160         usb_out_buffer[3] = (uint8_t)(address & 0xFF);
 161
 162         uint8_t i;
 163         for (i = 0 ; i < num_of_words ; i++, word++) {
 164                 if (access_endian == AICE_BIG_ENDIAN) {
 165                         usb_out_buffer[7 + i * 4] = (uint8_t)((*word >> 24) & 0xFF);
 166                         usb_out_buffer[6 + i * 4] = (uint8_t)((*word >> 16) & 0xFF);
 167                         usb_out_buffer[5 + i * 4] = (uint8_t)((*word >> 8) & 0xFF);
 168                         usb_out_buffer[4 + i * 4] = (uint8_t)(*word & 0xFF);
 169                 } else {
 170                         usb_out_buffer[4 + i * 4] = (uint8_t)((*word >> 24) & 0xFF);
 171                         usb_out_buffer[5 + i * 4] = (uint8_t)((*word >> 16) & 0xFF);
 172                         usb_out_buffer[6 + i * 4] = (uint8_t)((*word >> 8) & 0xFF);
 173                         usb_out_buffer[7 + i * 4] = (uint8_t)(*word & 0xFF);
 174                 }
 175         }
 176 }
 177
 178 static void aice_pack_htdmd(uint8_t cmd_code, uint8_t target_id,
 179                 uint8_t extra_word_length, uint32_t address, uint32_t word,
 180                 enum aice_target_endian access_endian)
 181 {
 182         usb_out_buffer[0] = cmd_code;
 183         usb_out_buffer[1] = target_id;
 184         usb_out_buffer[2] = extra_word_length;
 185         usb_out_buffer[3] = 0;
 186         usb_out_buffer[4] = (uint8_t)((address >> 24) & 0xFF);
 187         usb_out_buffer[5] = (uint8_t)((address >> 16) & 0xFF);
 188         usb_out_buffer[6] = (uint8_t)((address >> 8) & 0xFF);
 189         usb_out_buffer[7] = (uint8_t)(address & 0xFF);
 190         if (access_endian == AICE_BIG_ENDIAN) {
 191                 usb_out_buffer[11] = (uint8_t)((word >> 24) & 0xFF);
 192                 usb_out_buffer[10] = (uint8_t)((word >> 16) & 0xFF);
 193                 usb_out_buffer[9] = (uint8_t)((word >> 8) & 0xFF);
 194                 usb_out_buffer[8] = (uint8_t)(word & 0xFF);
 195         } else {
 196                 usb_out_buffer[8] = (uint8_t)((word >> 24) & 0xFF);
 197                 usb_out_buffer[9] = (uint8_t)((word >> 16) & 0xFF);
 198                 usb_out_buffer[10] = (uint8_t)((word >> 8) & 0xFF);
 199                 usb_out_buffer[11] = (uint8_t)(word & 0xFF);
 200         }
 201 }
 202
 203 static void aice_pack_htdmd_multiple_data(uint8_t cmd_code, uint8_t target_id,
 204                 uint8_t extra_word_length, uint32_t address, const uint8_t *word,
 205                 enum aice_target_endian access_endian)
 206 {
 207         usb_out_buffer[0] = cmd_code;
 208         usb_out_buffer[1] = target_id;
 209         usb_out_buffer[2] = extra_word_length;
 210         usb_out_buffer[3] = 0;
 211         usb_out_buffer[4] = (uint8_t)((address >> 24) & 0xFF);
 212         usb_out_buffer[5] = (uint8_t)((address >> 16) & 0xFF);
 213         usb_out_buffer[6] = (uint8_t)((address >> 8) & 0xFF);
 214         usb_out_buffer[7] = (uint8_t)(address & 0xFF);
 215
 216         uint32_t i;
 217         /* num_of_words may be over 0xFF, so use uint32_t */
 218         uint32_t num_of_words = extra_word_length + 1;
 219
 220         for (i = 0 ; i < num_of_words ; i++, word += 4) {
 221                 if (access_endian == AICE_BIG_ENDIAN) {
 222                         usb_out_buffer[11 + i * 4] = word[3];
 223                         usb_out_buffer[10 + i * 4] = word[2];
 224                         usb_out_buffer[9 + i * 4] = word[1];
 225                         usb_out_buffer[8 + i * 4] = word[0];
 226                 } else {
 227                         usb_out_buffer[8 + i * 4] = word[3];
 228                         usb_out_buffer[9 + i * 4] = word[2];
 229                         usb_out_buffer[10 + i * 4] = word[1];
 230                         usb_out_buffer[11 + i * 4] = word[0];
 231                 }
 232         }
 233 }
 234
 235 static void aice_unpack_dtha(uint8_t *cmd_ack_code, uint8_t *extra_word_length,
 236                 uint32_t *word, enum aice_target_endian access_endian)
 237 {
 238         *cmd_ack_code = usb_in_buffer[0];
 239         *extra_word_length = usb_in_buffer[1];
 240
 241         if (access_endian == AICE_BIG_ENDIAN) {
 242                 *word = (usb_in_buffer[5] << 24) |
 243                         (usb_in_buffer[4] << 16) |
 244                         (usb_in_buffer[3] << 8) |
 245                         (usb_in_buffer[2]);
 246         } else {
 247                 *word = (usb_in_buffer[2] << 24) |
 248                         (usb_in_buffer[3] << 16) |
 249                         (usb_in_buffer[4] << 8) |
 250                         (usb_in_buffer[5]);
 251         }
 252 }
 253
 254 static void aice_unpack_dtha_multiple_data(uint8_t *cmd_ack_code,
 255                 uint8_t *extra_word_length, uint32_t *word, uint8_t num_of_words,
 256                 enum aice_target_endian access_endian)
 257 {
 258         *cmd_ack_code = usb_in_buffer[0];
 259         *extra_word_length = usb_in_buffer[1];
 260
 261         uint8_t i;
 262         for (i = 0 ; i < num_of_words ; i++, word++) {
 263                 if (access_endian == AICE_BIG_ENDIAN) {
 264                         *word = (usb_in_buffer[5 + i * 4] << 24) |
 265                                 (usb_in_buffer[4 + i * 4] << 16) |
 266                                 (usb_in_buffer[3 + i * 4] << 8) |
 267                                 (usb_in_buffer[2 + i * 4]);
 268                 } else {
 269                         *word = (usb_in_buffer[2 + i * 4] << 24) |
 270                                 (usb_in_buffer[3 + i * 4] << 16) |
 271                                 (usb_in_buffer[4 + i * 4] << 8) |
 272                                 (usb_in_buffer[5 + i * 4]);
 273                 }
 274         }
 275 }
 276
 277 static void aice_unpack_dthb(uint8_t *cmd_ack_code, uint8_t *extra_word_length)
 278 {
 279         *cmd_ack_code = usb_in_buffer[0];
 280         *extra_word_length = usb_in_buffer[1];
 281 }
 282
 283 static void aice_unpack_dthma(uint8_t *cmd_ack_code, uint8_t *target_id,
 284                 uint8_t *extra_word_length, uint32_t *word,
 285                 enum aice_target_endian access_endian)
 286 {
 287         *cmd_ack_code = usb_in_buffer[0];
 288         *target_id = usb_in_buffer[1];
 289         *extra_word_length = usb_in_buffer[2];
 290         if (access_endian == AICE_BIG_ENDIAN) {
 291                 *word = (usb_in_buffer[7] << 24) |
 292                         (usb_in_buffer[6] << 16) |
 293                         (usb_in_buffer[5] << 8) |
 294                         (usb_in_buffer[4]);
 295         } else {
 296                 *word = (usb_in_buffer[4] << 24) |
 297                         (usb_in_buffer[5] << 16) |
 298                         (usb_in_buffer[6] << 8) |
 299                         (usb_in_buffer[7]);
 300         }
 301 }
 302
 303 static void aice_unpack_dthma_multiple_data(uint8_t *cmd_ack_code,
 304                 uint8_t *target_id, uint8_t *extra_word_length, uint8_t *word,
 305                 enum aice_target_endian access_endian)
 306 {
 307         *cmd_ack_code = usb_in_buffer[0];
 308         *target_id = usb_in_buffer[1];
 309         *extra_word_length = usb_in_buffer[2];
 310         if (access_endian == AICE_BIG_ENDIAN) {
 311                 word[0] = usb_in_buffer[4];
 312                 word[1] = usb_in_buffer[5];
 313                 word[2] = usb_in_buffer[6];
 314                 word[3] = usb_in_buffer[7];
 315         } else {
 316                 word[0] = usb_in_buffer[7];
 317                 word[1] = usb_in_buffer[6];
 318                 word[2] = usb_in_buffer[5];
 319                 word[3] = usb_in_buffer[4];
 320         }
 321         word += 4;
 322
 323         uint8_t i;
 324         for (i = 0; i < *extra_word_length; i++) {
 325                 if (access_endian == AICE_BIG_ENDIAN) {
 326                         word[0] = usb_in_buffer[8 + i * 4];
 327                         word[1] = usb_in_buffer[9 + i * 4];
 328                         word[2] = usb_in_buffer[10 + i * 4];
 329                         word[3] = usb_in_buffer[11 + i * 4];
 330                 } else {
 331                         word[0] = usb_in_buffer[11 + i * 4];
 332                         word[1] = usb_in_buffer[10 + i * 4];
 333                         word[2] = usb_in_buffer[9 + i * 4];
 334                         word[3] = usb_in_buffer[8 + i * 4];
 335                 }
 336                 word += 4;
 337         }
 338 }
 339
 340 static void aice_unpack_dthmb(uint8_t *cmd_ack_code, uint8_t *target_id,
 341                 uint8_t *extra_word_length)
 342 {
 343         *cmd_ack_code = usb_in_buffer[0];
 344         *target_id = usb_in_buffer[1];
 345         *extra_word_length = usb_in_buffer[2];
 346 }
 347
 348 /***************************************************************************/
 349 /* End of AICE commands' pack/unpack functions */
 350
 351 /* calls the given usb_bulk_* function, allowing for the data to
 352  * trickle in with some timeouts  */
 353 static int usb_bulk_with_retries(
 354                         int (*f)(jtag_libusb_device_handle *, int, char *, int, int),
 355                         jtag_libusb_device_handle *dev, int ep,
 356                         char *bytes, int size, int timeout)
 357 {
 358         int tries = 3, count = 0;
 359
 360         while (tries && (count < size)) {
 361                 int result = f(dev, ep, bytes + count, size - count, timeout);
 362                 if (result > 0)
 363                         count += result;
 364                 else if ((-ETIMEDOUT != result) || !--tries)
 365                         return result;
 366         }
 367         return count;
 368 }
 369
 370 static int wrap_usb_bulk_write(jtag_libusb_device_handle *dev, int ep,
 371                 char *buff, int size, int timeout)
 372 {
 373         /* usb_bulk_write() takes const char *buff */
 374         return jtag_libusb_bulk_write(dev, ep, buff, size, timeout);
 375 }
 376
 377 static inline int usb_bulk_write_ex(jtag_libusb_device_handle *dev, int ep,
 378                 char *bytes, int size, int timeout)
 379 {
 380         return usb_bulk_with_retries(&wrap_usb_bulk_write,
 381                         dev, ep, bytes, size, timeout);
 382 }
 383
 384 static inline int usb_bulk_read_ex(jtag_libusb_device_handle *dev, int ep,
 385                 char *bytes, int size, int timeout)
 386 {
 387         return usb_bulk_with_retries(&jtag_libusb_bulk_read,
 388                         dev, ep, bytes, size, timeout);
 389 }
 390
 391 /* Write data from out_buffer to USB. */
 392 static int aice_usb_write(uint8_t *out_buffer, int out_length)
 393 {
 394         int result;
 395
 396         if (out_length > AICE_OUT_BUFFER_SIZE) {
 397                 LOG_ERROR("aice_write illegal out_length=%i (max=%i)",
 398                                 out_length, AICE_OUT_BUFFER_SIZE);
 399                 return -1;
 400         }
 401
 402         result = usb_bulk_write_ex(aice_handler.usb_handle, aice_handler.usb_write_ep,
 403                         (char *)out_buffer, out_length, AICE_USB_TIMEOUT);
 404
 405         DEBUG_JTAG_IO("aice_usb_write, out_length = %i, result = %i",
 406                         out_length, result);
 407
 408         return result;
 409 }
 410
 411 /* Read data from USB into in_buffer. */
 412 static int aice_usb_read(uint8_t *in_buffer, int expected_size)
 413 {
 414         int32_t result = usb_bulk_read_ex(aice_handler.usb_handle, aice_handler.usb_read_ep,
 415                         (char *)in_buffer, expected_size, AICE_USB_TIMEOUT);
 416
 417         DEBUG_JTAG_IO("aice_usb_read, result = %" PRId32, result);
 418
 419         return result;
 420 }
 421
 422 static uint8_t usb_out_packets_buffer[AICE_OUT_PACKETS_BUFFER_SIZE];
 423 static uint8_t usb_in_packets_buffer[AICE_IN_PACKETS_BUFFER_SIZE];
 424 static uint32_t usb_out_packets_buffer_length;
 425 static uint32_t usb_in_packets_buffer_length;
 426 static enum aice_command_mode aice_command_mode;
 427
 428 static int aice_batch_buffer_write(uint8_t buf_index, const uint8_t *word,
 429                 uint32_t num_of_words);
 430
 431 static int aice_usb_packet_flush(void)
 432 {
 433         if (usb_out_packets_buffer_length == 0)
 434                 return 0;
 435
 436         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
 437                 LOG_DEBUG("Flush usb packets (AICE_COMMAND_MODE_PACK)");
 438
 439                 if (aice_usb_write(usb_out_packets_buffer,
 440                                         usb_out_packets_buffer_length) < 0)
 441                         return ERROR_FAIL;
 442
 443                 if (aice_usb_read(usb_in_packets_buffer,
 444                                         usb_in_packets_buffer_length) < 0)
 445                         return ERROR_FAIL;
 446
 447                 usb_out_packets_buffer_length = 0;
 448                 usb_in_packets_buffer_length = 0;
 449
 450         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
 451                 LOG_DEBUG("Flush usb packets (AICE_COMMAND_MODE_BATCH)");
 452
 453                 /* use BATCH_BUFFER_WRITE to fill command-batch-buffer */
 454                 if (aice_batch_buffer_write(AICE_BATCH_COMMAND_BUFFER_0,
 455                                 usb_out_packets_buffer,
 456                                 (usb_out_packets_buffer_length + 3) / 4) != ERROR_OK)
 457                         return ERROR_FAIL;
 458
 459                 usb_out_packets_buffer_length = 0;
 460                 usb_in_packets_buffer_length = 0;
 461
 462                 /* enable BATCH command */
 463                 aice_command_mode = AICE_COMMAND_MODE_NORMAL;
 464                 if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_CTRL, 0x80000000) != ERROR_OK)
 465                         return ERROR_FAIL;
 466                 aice_command_mode = AICE_COMMAND_MODE_BATCH;
 467
 468                 /* wait 1 second (AICE bug, workaround) */
 469                 alive_sleep(1000);
 470
 471                 /* check status */
 472                 uint32_t i;
 473                 uint32_t batch_status;
 474
 475                 i = 0;
 476                 while (1) {
 477                         aice_read_ctrl(AICE_READ_CTRL_BATCH_STATUS, &batch_status);
 478
 479                         if (batch_status & 0x1)
 480                                 return ERROR_OK;
 481                         else if (batch_status & 0xE)
 482                                 return ERROR_FAIL;
 483
 484                         if ((i % 30) == 0)
 485                                 keep_alive();
 486
 487                         i++;
 488                 }
 489         }
 490
 491         return ERROR_OK;
 492 }
 493
 494 static int aice_usb_packet_append(uint8_t *out_buffer, int out_length, int in_length)
 495 {
 496         uint32_t max_packet_size = AICE_OUT_PACKETS_BUFFER_SIZE;
 497
 498         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
 499                 max_packet_size = AICE_OUT_PACK_COMMAND_SIZE;
 500         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
 501                 max_packet_size = AICE_OUT_BATCH_COMMAND_SIZE;
 502         } else {
 503                 /* AICE_COMMAND_MODE_NORMAL */
 504                 if (aice_usb_packet_flush() != ERROR_OK)
 505                         return ERROR_FAIL;
 506         }
 507
 508         if (usb_out_packets_buffer_length + out_length > max_packet_size)
 509                 if (aice_usb_packet_flush() != ERROR_OK) {
 510                         LOG_DEBUG("Flush usb packets failed");
 511                         return ERROR_FAIL;
 512                 }
 513
 514         LOG_DEBUG("Append usb packets 0x%02x", out_buffer[0]);
 515
 516         memcpy(usb_out_packets_buffer + usb_out_packets_buffer_length, out_buffer, out_length);
 517         usb_out_packets_buffer_length += out_length;
 518         usb_in_packets_buffer_length += in_length;
 519
 520         return ERROR_OK;
 521 }
 522
 523 /***************************************************************************/
 524 /* AICE commands */
 525 static int aice_reset_box(void)
 526 {
 527         if (aice_write_ctrl(AICE_WRITE_CTRL_CLEAR_TIMEOUT_STATUS, 0x1) != ERROR_OK)
 528                 return ERROR_FAIL;
 529
 530         /* turn off FASTMODE */
 531         uint32_t pin_status;
 532         if (aice_read_ctrl(AICE_READ_CTRL_GET_JTAG_PIN_STATUS, &pin_status)
 533                         != ERROR_OK)
 534                 return ERROR_FAIL;
 535
 536         if (aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_STATUS, pin_status & (~0x2))
 537                         != ERROR_OK)
 538                 return ERROR_FAIL;
 539
 540         return ERROR_OK;
 541 }
 542
 543 static int aice_scan_chain(uint32_t *id_codes, uint8_t *num_of_ids)
 544 {
 545         int32_t result;
 546         int retry_times = 0;
 547
 548         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
 549                 (AICE_COMMAND_MODE_BATCH == aice_command_mode))
 550                 aice_usb_packet_flush();
 551
 552         do {
 553                 aice_pack_htda(AICE_CMD_SCAN_CHAIN, 0x0F, 0x0);
 554
 555                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDA);
 556
 557                 LOG_DEBUG("SCAN_CHAIN, length: 0x0F");
 558
 559                 /** TODO: modify receive length */
 560                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHA);
 561                 if (AICE_FORMAT_DTHA != result) {
 562                         LOG_ERROR("aice_usb_read failed (requested=%" PRIu32 ", result=%" PRId32 ")",
 563                                         AICE_FORMAT_DTHA, result);
 564                         return ERROR_FAIL;
 565                 }
 566
 567                 uint8_t cmd_ack_code;
 568                 aice_unpack_dtha_multiple_data(&cmd_ack_code, num_of_ids, id_codes,
 569                                 0x10, AICE_LITTLE_ENDIAN);
 570
 571                 if (cmd_ack_code != AICE_CMD_SCAN_CHAIN) {
 572
 573                         if (retry_times > aice_max_retry_times) {
 574                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
 575                                                 AICE_CMD_SCAN_CHAIN, cmd_ack_code);
 576                                 return ERROR_FAIL;
 577                         }
 578
 579                         /* clear timeout and retry */
 580                         if (aice_reset_box() != ERROR_OK)
 581                                 return ERROR_FAIL;
 582
 583                         retry_times++;
 584                         continue;
 585                 }
 586
 587                 LOG_DEBUG("SCAN_CHAIN response, # of IDs: %" PRIu8, *num_of_ids);
 588
 589                 if (*num_of_ids == 0xFF) {
 590                         LOG_ERROR("No target connected");
 591                         return ERROR_FAIL;
 592                 } else if (*num_of_ids == AICE_MAX_NUM_CORE) {
 593                         LOG_INFO("The ice chain over 16 targets");
 594                 } else {
 595                         (*num_of_ids)++;
 596                 }
 597                 break;
 598         } while (1);
 599
 600         return ERROR_OK;
 601 }
 602
 603 int aice_read_ctrl(uint32_t address, uint32_t *data)
 604 {
 605         int32_t result;
 606
 607         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
 608                 (AICE_COMMAND_MODE_BATCH == aice_command_mode))
 609                 aice_usb_packet_flush();
 610
 611         aice_pack_htda(AICE_CMD_READ_CTRL, 0, address);
 612
 613         aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDA);
 614
 615         LOG_DEBUG("READ_CTRL, address: 0x%" PRIx32, address);
 616
 617         result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHA);
 618         if (AICE_FORMAT_DTHA != result) {
 619                 LOG_ERROR("aice_usb_read failed (requested=%" PRIu32 ", result=%" PRId32 ")",
 620                                 AICE_FORMAT_DTHA, result);
 621                 return ERROR_FAIL;
 622         }
 623
 624         uint8_t cmd_ack_code;
 625         uint8_t extra_length;
 626         aice_unpack_dtha(&cmd_ack_code, &extra_length, data, AICE_LITTLE_ENDIAN);
 627
 628         LOG_DEBUG("READ_CTRL response, data: 0x%" PRIx32, *data);
 629
 630         if (cmd_ack_code != AICE_CMD_READ_CTRL) {
 631                 LOG_ERROR("aice command error (command=0x%" PRIx32 ", response=0x%" PRIx8 ")",
 632                                 (uint32_t)AICE_CMD_READ_CTRL, cmd_ack_code);
 633                 return ERROR_FAIL;
 634         }
 635
 636         return ERROR_OK;
 637 }
 638
 639 int aice_write_ctrl(uint32_t address, uint32_t data)
 640 {
 641         int32_t result;
 642
 643         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
 644                 aice_usb_packet_flush();
 645         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
 646                 aice_pack_htdc(AICE_CMD_WRITE_CTRL, 0, address, data, AICE_LITTLE_ENDIAN);
 647                 return aice_usb_packet_append(usb_out_buffer, AICE_FORMAT_HTDC,
 648                                 AICE_FORMAT_DTHB);
 649         }
 650
 651         aice_pack_htdc(AICE_CMD_WRITE_CTRL, 0, address, data, AICE_LITTLE_ENDIAN);
 652
 653         aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDC);
 654
 655         LOG_DEBUG("WRITE_CTRL, address: 0x%" PRIx32 ", data: 0x%" PRIx32, address, data);
 656
 657         result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHB);
 658         if (AICE_FORMAT_DTHB != result) {
 659                 LOG_ERROR("aice_usb_read failed (requested=%" PRIu32 ", result=%" PRId32 ")",
 660                                 AICE_FORMAT_DTHB, result);
 661                 return ERROR_FAIL;
 662         }
 663
 664         uint8_t cmd_ack_code;
 665         uint8_t extra_length;
 666         aice_unpack_dthb(&cmd_ack_code, &extra_length);
 667
 668         LOG_DEBUG("WRITE_CTRL response");
 669
 670         if (cmd_ack_code != AICE_CMD_WRITE_CTRL) {
 671                 LOG_ERROR("aice command error (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
 672                                 AICE_CMD_WRITE_CTRL, cmd_ack_code);
 673                 return ERROR_FAIL;
 674         }
 675
 676         return ERROR_OK;
 677 }
 678
 679 int aice_read_dtr(uint8_t target_id, uint32_t *data)
 680 {
 681         int32_t result;
 682         int retry_times = 0;
 683
 684         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
 685                 (AICE_COMMAND_MODE_BATCH == aice_command_mode))
 686                 aice_usb_packet_flush();
 687
 688         do {
 689                 aice_pack_htdma(AICE_CMD_T_READ_DTR, target_id, 0, 0);
 690
 691                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMA);
 692
 693                 LOG_DEBUG("READ_DTR, COREID: %" PRIu8, target_id);
 694
 695                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMA);
 696                 if (AICE_FORMAT_DTHMA != result) {
 697                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
 698                                         AICE_FORMAT_DTHMA, result);
 699                         return ERROR_FAIL;
 700                 }
 701
 702                 uint8_t cmd_ack_code;
 703                 uint8_t extra_length;
 704                 uint8_t res_target_id;
 705                 aice_unpack_dthma(&cmd_ack_code, &res_target_id, &extra_length,
 706                                 data, AICE_LITTLE_ENDIAN);
 707
 708                 if (cmd_ack_code == AICE_CMD_T_READ_DTR) {
 709                         LOG_DEBUG("READ_DTR response, data: 0x%" PRIx32, *data);
 710                         break;
 711                 } else {
 712
 713                         if (retry_times > aice_max_retry_times) {
 714                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
 715                                                 AICE_CMD_T_READ_DTR, cmd_ack_code);
 716                                 return ERROR_FAIL;
 717                         }
 718
 719                         /* clear timeout and retry */
 720                         if (aice_reset_box() != ERROR_OK)
 721                                 return ERROR_FAIL;
 722
 723                         retry_times++;
 724                 }
 725         } while (1);
 726
 727         return ERROR_OK;
 728 }
 729
 730 int aice_read_dtr_to_buffer(uint8_t target_id, uint32_t buffer_idx)
 731 {
 732         int32_t result;
 733         int retry_times = 0;
 734
 735         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
 736                 aice_usb_packet_flush();
 737         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
 738                 aice_pack_htdma(AICE_CMD_READ_DTR_TO_BUFFER, target_id, 0, buffer_idx);
 739                 return aice_usb_packet_append(usb_out_buffer, AICE_FORMAT_HTDMA,
 740                                 AICE_FORMAT_DTHMB);
 741         }
 742
 743         do {
 744                 aice_pack_htdma(AICE_CMD_READ_DTR_TO_BUFFER, target_id, 0, buffer_idx);
 745
 746                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMA);
 747
 748                 LOG_DEBUG("READ_DTR_TO_BUFFER, COREID: %" PRIu8, target_id);
 749
 750                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
 751                 if (AICE_FORMAT_DTHMB != result) {
 752                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")", AICE_FORMAT_DTHMB, result);
 753                         return ERROR_FAIL;
 754                 }
 755
 756                 uint8_t cmd_ack_code;
 757                 uint8_t extra_length;
 758                 uint8_t res_target_id;
 759                 aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
 760
 761                 if (cmd_ack_code == AICE_CMD_READ_DTR_TO_BUFFER) {
 762                         break;
 763                 } else {
 764                         if (retry_times > aice_max_retry_times) {
 765                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
 766                                                 AICE_CMD_READ_DTR_TO_BUFFER, cmd_ack_code);
 767
 768                                 return ERROR_FAIL;
 769                         }
 770
 771                         /* clear timeout and retry */
 772                         if (aice_reset_box() != ERROR_OK)
 773                                 return ERROR_FAIL;
 774
 775                         retry_times++;
 776                 }
 777         } while (1);
 778
 779         return ERROR_OK;
 780 }
 781
 782 int aice_write_dtr(uint8_t target_id, uint32_t data)
 783 {
 784         int32_t result;
 785         int retry_times = 0;
 786
 787         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
 788                 aice_usb_packet_flush();
 789         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
 790                 aice_pack_htdmc(AICE_CMD_T_WRITE_DTR, target_id, 0, 0, data, AICE_LITTLE_ENDIAN);
 791                 return aice_usb_packet_append(usb_out_buffer, AICE_FORMAT_HTDMC,
 792                                 AICE_FORMAT_DTHMB);
 793         }
 794
 795         do {
 796                 aice_pack_htdmc(AICE_CMD_T_WRITE_DTR, target_id, 0, 0, data, AICE_LITTLE_ENDIAN);
 797
 798                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMC);
 799
 800                 LOG_DEBUG("WRITE_DTR, COREID: %" PRIu8 ", data: 0x%" PRIx32, target_id, data);
 801
 802                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
 803                 if (AICE_FORMAT_DTHMB != result) {
 804                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")", AICE_FORMAT_DTHMB, result);
 805                         return ERROR_FAIL;
 806                 }
 807
 808                 uint8_t cmd_ack_code;
 809                 uint8_t extra_length;
 810                 uint8_t res_target_id;
 811                 aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
 812
 813                 if (cmd_ack_code == AICE_CMD_T_WRITE_DTR) {
 814                         LOG_DEBUG("WRITE_DTR response");
 815                         break;
 816                 } else {
 817                         if (retry_times > aice_max_retry_times) {
 818                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
 819                                                 AICE_CMD_T_WRITE_DTR, cmd_ack_code);
 820
 821                                 return ERROR_FAIL;
 822                         }
 823
 824                         /* clear timeout and retry */
 825                         if (aice_reset_box() != ERROR_OK)
 826                                 return ERROR_FAIL;
 827
 828                         retry_times++;
 829                 }
 830         } while (1);
 831
 832         return ERROR_OK;
 833 }
 834
 835 int aice_write_dtr_from_buffer(uint8_t target_id, uint32_t buffer_idx)
 836 {
 837         int32_t result;
 838         int retry_times = 0;
 839
 840         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
 841                 aice_usb_packet_flush();
 842         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
 843                 aice_pack_htdma(AICE_CMD_WRITE_DTR_FROM_BUFFER, target_id, 0, buffer_idx);
 844                 return aice_usb_packet_append(usb_out_buffer, AICE_FORMAT_HTDMA,
 845                                 AICE_FORMAT_DTHMB);
 846         }
 847
 848         do {
 849                 aice_pack_htdma(AICE_CMD_WRITE_DTR_FROM_BUFFER, target_id, 0, buffer_idx);
 850
 851                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMA);
 852
 853                 LOG_DEBUG("WRITE_DTR_FROM_BUFFER, COREID: %" PRIu8 "", target_id);
 854
 855                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
 856                 if (AICE_FORMAT_DTHMB != result) {
 857                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")", AICE_FORMAT_DTHMB, result);
 858                         return ERROR_FAIL;
 859                 }
 860
 861                 uint8_t cmd_ack_code;
 862                 uint8_t extra_length;
 863                 uint8_t res_target_id;
 864                 aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
 865
 866                 if (cmd_ack_code == AICE_CMD_WRITE_DTR_FROM_BUFFER) {
 867                         break;
 868                 } else {
 869                         if (retry_times > aice_max_retry_times) {
 870                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
 871                                                 AICE_CMD_WRITE_DTR_FROM_BUFFER, cmd_ack_code);
 872
 873                                 return ERROR_FAIL;
 874                         }
 875
 876                         /* clear timeout and retry */
 877                         if (aice_reset_box() != ERROR_OK)
 878                                 return ERROR_FAIL;
 879
 880                         retry_times++;
 881                 }
 882         } while (1);
 883
 884         return ERROR_OK;
 885 }
 886
 887 int aice_read_misc(uint8_t target_id, uint32_t address, uint32_t *data)
 888 {
 889         int32_t result;
 890         int retry_times = 0;
 891
 892         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
 893                 (AICE_COMMAND_MODE_BATCH == aice_command_mode))
 894                 aice_usb_packet_flush();
 895
 896         do {
 897                 aice_pack_htdma(AICE_CMD_T_READ_MISC, target_id, 0, address);
 898
 899                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMA);
 900
 901                 LOG_DEBUG("READ_MISC, COREID: %" PRIu8 ", address: 0x%" PRIx32, target_id, address);
 902
 903                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMA);
 904                 if (AICE_FORMAT_DTHMA != result) {
 905                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
 906                                         AICE_FORMAT_DTHMA, result);
 907                         return ERROR_AICE_DISCONNECT;
 908                 }
 909
 910                 uint8_t cmd_ack_code;
 911                 uint8_t extra_length;
 912                 uint8_t res_target_id;
 913                 aice_unpack_dthma(&cmd_ack_code, &res_target_id, &extra_length,
 914                                 data, AICE_LITTLE_ENDIAN);
 915
 916                 if (cmd_ack_code == AICE_CMD_T_READ_MISC) {
 917                         LOG_DEBUG("READ_MISC response, data: 0x%" PRIx32, *data);
 918                         break;
 919                 } else {
 920                         if (retry_times > aice_max_retry_times) {
 921                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
 922                                                 AICE_CMD_T_READ_MISC, cmd_ack_code);
 923                                 return ERROR_FAIL;
 924                         }
 925
 926                         /* clear timeout and retry */
 927                         if (aice_reset_box() != ERROR_OK)
 928                                 return ERROR_FAIL;
 929
 930                         retry_times++;
 931                 }
 932         } while (1);
 933
 934         return ERROR_OK;
 935 }
 936
 937 int aice_write_misc(uint8_t target_id, uint32_t address, uint32_t data)
 938 {
 939         int32_t result;
 940         int retry_times = 0;
 941
 942         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
 943                 aice_usb_packet_flush();
 944         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
 945                 aice_pack_htdmc(AICE_CMD_T_WRITE_MISC, target_id, 0, address, data,
 946                                 AICE_LITTLE_ENDIAN);
 947                 return aice_usb_packet_append(usb_out_buffer, AICE_FORMAT_HTDMC,
 948                                 AICE_FORMAT_DTHMB);
 949         }
 950
 951         do {
 952                 aice_pack_htdmc(AICE_CMD_T_WRITE_MISC, target_id, 0, address,
 953                                 data, AICE_LITTLE_ENDIAN);
 954
 955                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMC);
 956
 957                 LOG_DEBUG("WRITE_MISC, COREID: %" PRIu8 ", address: 0x%" PRIx32 ", data: 0x%" PRIx32,
 958                                 target_id, address, data);
 959
 960                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
 961                 if (AICE_FORMAT_DTHMB != result) {
 962                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
 963                                         AICE_FORMAT_DTHMB, result);
 964                         return ERROR_FAIL;
 965                 }
 966
 967                 uint8_t cmd_ack_code;
 968                 uint8_t extra_length;
 969                 uint8_t res_target_id;
 970                 aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
 971
 972                 if (cmd_ack_code == AICE_CMD_T_WRITE_MISC) {
 973                         LOG_DEBUG("WRITE_MISC response");
 974                         break;
 975                 } else {
 976                         if (retry_times > aice_max_retry_times) {
 977                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
 978                                                 AICE_CMD_T_WRITE_MISC, cmd_ack_code);
 979
 980                                 return ERROR_FAIL;
 981                         }
 982
 983                         /* clear timeout and retry */
 984                         if (aice_reset_box() != ERROR_OK)
 985                                 return ERROR_FAIL;
 986
 987                         retry_times++;
 988                 }
 989         } while (1);
 990
 991         return ERROR_OK;
 992 }
 993
 994 int aice_read_edmsr(uint8_t target_id, uint32_t address, uint32_t *data)
 995 {
 996         int32_t result;
 997         int retry_times = 0;
 998
 999         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
1000                 (AICE_COMMAND_MODE_BATCH == aice_command_mode))
1001                 aice_usb_packet_flush();
1002
1003         do {
1004                 aice_pack_htdma(AICE_CMD_T_READ_EDMSR, target_id, 0, address);
1005
1006                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMA);
1007
1008                 LOG_DEBUG("READ_EDMSR, COREID: %" PRIu8 ", address: 0x%" PRIx32, target_id, address);
1009
1010                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMA);
1011                 if (AICE_FORMAT_DTHMA != result) {
1012                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1013                                         AICE_FORMAT_DTHMA, result);
1014                         return ERROR_FAIL;
1015                 }
1016
1017                 uint8_t cmd_ack_code;
1018                 uint8_t extra_length;
1019                 uint8_t res_target_id;
1020                 aice_unpack_dthma(&cmd_ack_code, &res_target_id, &extra_length,
1021                                 data, AICE_LITTLE_ENDIAN);
1022
1023                 if (cmd_ack_code == AICE_CMD_T_READ_EDMSR) {
1024                         LOG_DEBUG("READ_EDMSR response, data: 0x%" PRIx32, *data);
1025                         break;
1026                 } else {
1027                         if (retry_times > aice_max_retry_times) {
1028                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1029                                                 AICE_CMD_T_READ_EDMSR, cmd_ack_code);
1030
1031                                 return ERROR_FAIL;
1032                         }
1033
1034                         /* clear timeout and retry */
1035                         if (aice_reset_box() != ERROR_OK)
1036                                 return ERROR_FAIL;
1037
1038                         retry_times++;
1039                 }
1040         } while (1);
1041
1042         return ERROR_OK;
1043 }
1044
1045 int aice_write_edmsr(uint8_t target_id, uint32_t address, uint32_t data)
1046 {
1047         int32_t result;
1048         int retry_times = 0;
1049
1050         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
1051                 aice_usb_packet_flush();
1052         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
1053                 aice_pack_htdmc(AICE_CMD_T_WRITE_EDMSR, target_id, 0, address, data,
1054                                 AICE_LITTLE_ENDIAN);
1055                 return aice_usb_packet_append(usb_out_buffer, AICE_FORMAT_HTDMC,
1056                                 AICE_FORMAT_DTHMB);
1057         }
1058
1059         do {
1060                 aice_pack_htdmc(AICE_CMD_T_WRITE_EDMSR, target_id, 0, address,
1061                                 data, AICE_LITTLE_ENDIAN);
1062
1063                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMC);
1064
1065                 LOG_DEBUG("WRITE_EDMSR, COREID: %" PRIu8 ", address: 0x%" PRIx32 ", data: 0x%" PRIx32,
1066                                 target_id, address, data);
1067
1068                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
1069                 if (AICE_FORMAT_DTHMB != result) {
1070                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1071                                         AICE_FORMAT_DTHMB, result);
1072                         return ERROR_FAIL;
1073                 }
1074
1075                 uint8_t cmd_ack_code;
1076                 uint8_t extra_length;
1077                 uint8_t res_target_id;
1078                 aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
1079
1080                 if (cmd_ack_code == AICE_CMD_T_WRITE_EDMSR) {
1081                         LOG_DEBUG("WRITE_EDMSR response");
1082                         break;
1083                 } else {
1084                         if (retry_times > aice_max_retry_times) {
1085                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1086                                                 AICE_CMD_T_WRITE_EDMSR, cmd_ack_code);
1087
1088                                 return ERROR_FAIL;
1089                         }
1090
1091                         /* clear timeout and retry */
1092                         if (aice_reset_box() != ERROR_OK)
1093                                 return ERROR_FAIL;
1094
1095                         retry_times++;
1096                 }
1097         } while (1);
1098
1099         return ERROR_OK;
1100 }
1101
1102 static int aice_switch_to_big_endian(uint32_t *word, uint8_t num_of_words)
1103 {
1104         uint32_t tmp;
1105
1106         for (uint8_t i = 0 ; i < num_of_words ; i++) {
1107                 tmp = ((word[i] >> 24) & 0x000000FF) |
1108                         ((word[i] >>  8) & 0x0000FF00) |
1109                         ((word[i] <<  8) & 0x00FF0000) |
1110                         ((word[i] << 24) & 0xFF000000);
1111                 word[i] = tmp;
1112         }
1113
1114         return ERROR_OK;
1115 }
1116
1117 static int aice_write_dim(uint8_t target_id, uint32_t *word, uint8_t num_of_words)
1118 {
1119         int32_t result;
1120         uint32_t big_endian_word[4];
1121         int retry_times = 0;
1122
1123         /** instruction is big-endian */
1124         memcpy(big_endian_word, word, sizeof(big_endian_word));
1125         aice_switch_to_big_endian(big_endian_word, num_of_words);
1126
1127         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
1128                 aice_usb_packet_flush();
1129         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
1130                 aice_pack_htdmc_multiple_data(AICE_CMD_T_WRITE_DIM, target_id,
1131                                 num_of_words - 1, 0, big_endian_word, num_of_words,
1132                                 AICE_LITTLE_ENDIAN);
1133                 return aice_usb_packet_append(usb_out_buffer,
1134                                 AICE_FORMAT_HTDMC + (num_of_words - 1) * 4,
1135                                 AICE_FORMAT_DTHMB);
1136         }
1137
1138         do {
1139                 aice_pack_htdmc_multiple_data(AICE_CMD_T_WRITE_DIM, target_id, num_of_words - 1, 0,
1140                                 big_endian_word, num_of_words, AICE_LITTLE_ENDIAN);
1141
1142                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMC + (num_of_words - 1) * 4);
1143
1144                 LOG_DEBUG("WRITE_DIM, COREID: %" PRIu8
1145                                 ", data: 0x%08" PRIx32 ", 0x%08" PRIx32 ", 0x%08" PRIx32 ", 0x%08" PRIx32,
1146                                 target_id,
1147                                 big_endian_word[0],
1148                                 big_endian_word[1],
1149                                 big_endian_word[2],
1150                                 big_endian_word[3]);
1151
1152                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
1153                 if (AICE_FORMAT_DTHMB != result) {
1154                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")", AICE_FORMAT_DTHMB, result);
1155                         return ERROR_FAIL;
1156                 }
1157
1158                 uint8_t cmd_ack_code;
1159                 uint8_t extra_length;
1160                 uint8_t res_target_id;
1161                 aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
1162
1163
1164                 if (cmd_ack_code == AICE_CMD_T_WRITE_DIM) {
1165                         LOG_DEBUG("WRITE_DIM response");
1166                         break;
1167                 } else {
1168                         if (retry_times > aice_max_retry_times) {
1169                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8
1170                                                 ", response=0x%" PRIx8 ")",
1171                                                 AICE_CMD_T_WRITE_DIM, cmd_ack_code);
1172
1173                                 return ERROR_FAIL;
1174                         }
1175
1176                         /* clear timeout and retry */
1177                         if (aice_reset_box() != ERROR_OK)
1178                                 return ERROR_FAIL;
1179
1180                         retry_times++;
1181                 }
1182         } while (1);
1183
1184         return ERROR_OK;
1185 }
1186
1187 static int aice_do_execute(uint8_t target_id)
1188 {
1189         int32_t result;
1190         int retry_times = 0;
1191
1192         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
1193                 aice_usb_packet_flush();
1194         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
1195                 aice_pack_htdmc(AICE_CMD_T_EXECUTE, target_id, 0, 0, 0, AICE_LITTLE_ENDIAN);
1196                 return aice_usb_packet_append(usb_out_buffer,
1197                                 AICE_FORMAT_HTDMC,
1198                                 AICE_FORMAT_DTHMB);
1199         }
1200
1201         do {
1202                 aice_pack_htdmc(AICE_CMD_T_EXECUTE, target_id, 0, 0, 0, AICE_LITTLE_ENDIAN);
1203
1204                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMC);
1205
1206                 LOG_DEBUG("EXECUTE, COREID: %" PRIu8 "", target_id);
1207
1208                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
1209                 if (AICE_FORMAT_DTHMB != result) {
1210                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1211                                         AICE_FORMAT_DTHMB, result);
1212                         return ERROR_FAIL;
1213                 }
1214
1215                 uint8_t cmd_ack_code;
1216                 uint8_t extra_length;
1217                 uint8_t res_target_id;
1218                 aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
1219
1220                 if (cmd_ack_code == AICE_CMD_T_EXECUTE) {
1221                         LOG_DEBUG("EXECUTE response");
1222                         break;
1223                 } else {
1224                         if (retry_times > aice_max_retry_times) {
1225                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1226                                                 AICE_CMD_T_EXECUTE, cmd_ack_code);
1227
1228                                 return ERROR_FAIL;
1229                         }
1230
1231                         /* clear timeout and retry */
1232                         if (aice_reset_box() != ERROR_OK)
1233                                 return ERROR_FAIL;
1234
1235                         retry_times++;
1236                 }
1237         } while (1);
1238
1239         return ERROR_OK;
1240 }
1241
1242 int aice_write_mem_b(uint8_t target_id, uint32_t address, uint32_t data)
1243 {
1244         int32_t result;
1245         int retry_times = 0;
1246
1247         LOG_DEBUG("WRITE_MEM_B, COREID: %" PRIu8 ", ADDRESS %08" PRIx32 "  VALUE %08" PRIx32,
1248                         target_id,
1249                         address,
1250                         data);
1251
1252         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
1253                 (AICE_COMMAND_MODE_BATCH == aice_command_mode)) {
1254                 aice_pack_htdmd(AICE_CMD_T_WRITE_MEM_B, target_id, 0, address,
1255                                 data & 0x000000FF, data_endian);
1256                 return aice_usb_packet_append(usb_out_buffer, AICE_FORMAT_HTDMD,
1257                                 AICE_FORMAT_DTHMB);
1258         } else {
1259                 do {
1260                         aice_pack_htdmd(AICE_CMD_T_WRITE_MEM_B, target_id, 0,
1261                                         address, data & 0x000000FF, data_endian);
1262                         aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMD);
1263
1264                         result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
1265                         if (AICE_FORMAT_DTHMB != result) {
1266                                 LOG_ERROR("aice_usb_read failed (requested=%" PRId32
1267                                                 ", result=%" PRId32 ")", AICE_FORMAT_DTHMB, result);
1268                                 return ERROR_FAIL;
1269                         }
1270
1271                         uint8_t cmd_ack_code;
1272                         uint8_t extra_length;
1273                         uint8_t res_target_id;
1274                         aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
1275
1276                         if (cmd_ack_code == AICE_CMD_T_WRITE_MEM_B) {
1277                                 break;
1278                         } else {
1279                                 if (retry_times > aice_max_retry_times) {
1280                                         LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1281                                                         AICE_CMD_T_WRITE_MEM_B, cmd_ack_code);
1282
1283                                         return ERROR_FAIL;
1284                                 }
1285
1286                                 /* clear timeout and retry */
1287                                 if (aice_reset_box() != ERROR_OK)
1288                                         return ERROR_FAIL;
1289
1290                                 retry_times++;
1291                         }
1292                 } while (1);
1293         }
1294
1295         return ERROR_OK;
1296 }
1297
1298 int aice_write_mem_h(uint8_t target_id, uint32_t address, uint32_t data)
1299 {
1300         int32_t result;
1301         int retry_times = 0;
1302
1303         LOG_DEBUG("WRITE_MEM_H, COREID: %" PRIu8 ", ADDRESS %08" PRIx32 "  VALUE %08" PRIx32,
1304                         target_id,
1305                         address,
1306                         data);
1307
1308         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
1309                 (AICE_COMMAND_MODE_BATCH == aice_command_mode)) {
1310                 aice_pack_htdmd(AICE_CMD_T_WRITE_MEM_H, target_id, 0,
1311                                 (address >> 1) & 0x7FFFFFFF, data & 0x0000FFFF, data_endian);
1312                 return aice_usb_packet_append(usb_out_buffer, AICE_FORMAT_HTDMD,
1313                                 AICE_FORMAT_DTHMB);
1314         } else {
1315                 do {
1316                         aice_pack_htdmd(AICE_CMD_T_WRITE_MEM_H, target_id, 0,
1317                                         (address >> 1) & 0x7FFFFFFF, data & 0x0000FFFF, data_endian);
1318                         aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMD);
1319
1320                         result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
1321                         if (AICE_FORMAT_DTHMB != result) {
1322                                 LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1323                                                 AICE_FORMAT_DTHMB, result);
1324                                 return ERROR_FAIL;
1325                         }
1326
1327                         uint8_t cmd_ack_code;
1328                         uint8_t extra_length;
1329                         uint8_t res_target_id;
1330                         aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
1331
1332                         if (cmd_ack_code == AICE_CMD_T_WRITE_MEM_H) {
1333                                 break;
1334                         } else {
1335                                 if (retry_times > aice_max_retry_times) {
1336                                         LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1337                                                         AICE_CMD_T_WRITE_MEM_H, cmd_ack_code);
1338
1339                                         return ERROR_FAIL;
1340                                 }
1341
1342                                 /* clear timeout and retry */
1343                                 if (aice_reset_box() != ERROR_OK)
1344                                         return ERROR_FAIL;
1345
1346                                 retry_times++;
1347                         }
1348                 } while (1);
1349         }
1350
1351         return ERROR_OK;
1352 }
1353
1354 int aice_write_mem(uint8_t target_id, uint32_t address, uint32_t data)
1355 {
1356         int32_t result;
1357         int retry_times = 0;
1358
1359         LOG_DEBUG("WRITE_MEM, COREID: %" PRIu8 ", ADDRESS %08" PRIx32 "  VALUE %08" PRIx32,
1360                         target_id,
1361                         address,
1362                         data);
1363
1364         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
1365                 (AICE_COMMAND_MODE_BATCH == aice_command_mode)) {
1366                 aice_pack_htdmd(AICE_CMD_T_WRITE_MEM, target_id, 0,
1367                                 (address >> 2) & 0x3FFFFFFF, data, data_endian);
1368                 return aice_usb_packet_append(usb_out_buffer, AICE_FORMAT_HTDMD,
1369                                 AICE_FORMAT_DTHMB);
1370         } else {
1371                 do {
1372                         aice_pack_htdmd(AICE_CMD_T_WRITE_MEM, target_id, 0,
1373                                         (address >> 2) & 0x3FFFFFFF, data, data_endian);
1374                         aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMD);
1375
1376                         result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
1377                         if (AICE_FORMAT_DTHMB != result) {
1378                                 LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1379                                                 AICE_FORMAT_DTHMB, result);
1380                                 return ERROR_FAIL;
1381                         }
1382
1383                         uint8_t cmd_ack_code;
1384                         uint8_t extra_length;
1385                         uint8_t res_target_id;
1386                         aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
1387
1388                         if (cmd_ack_code == AICE_CMD_T_WRITE_MEM) {
1389                                 break;
1390                         } else {
1391                                 if (retry_times > aice_max_retry_times) {
1392                                         LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1393                                                         AICE_CMD_T_WRITE_MEM, cmd_ack_code);
1394
1395                                         return ERROR_FAIL;
1396                                 }
1397
1398                                 /* clear timeout and retry */
1399                                 if (aice_reset_box() != ERROR_OK)
1400                                         return ERROR_FAIL;
1401
1402                                 retry_times++;
1403                         }
1404                 } while (1);
1405         }
1406
1407         return ERROR_OK;
1408 }
1409
1410 int aice_fastread_mem(uint8_t target_id, uint8_t *word, uint32_t num_of_words)
1411 {
1412         int32_t result;
1413         int retry_times = 0;
1414
1415         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
1416                 (AICE_COMMAND_MODE_BATCH == aice_command_mode))
1417                 aice_usb_packet_flush();
1418
1419         do {
1420                 aice_pack_htdmb(AICE_CMD_T_FASTREAD_MEM, target_id, num_of_words - 1, 0);
1421
1422                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMB);
1423
1424                 LOG_DEBUG("FASTREAD_MEM, COREID: %" PRIu8 ", # of DATA %08" PRIx32,
1425                                 target_id, num_of_words);
1426
1427                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMA + (num_of_words - 1) * 4);
1428                 if (result < 0) {
1429                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1430                                         AICE_FORMAT_DTHMA + (num_of_words - 1) * 4, result);
1431                         return ERROR_FAIL;
1432                 }
1433
1434                 uint8_t cmd_ack_code;
1435                 uint8_t extra_length;
1436                 uint8_t res_target_id;
1437                 aice_unpack_dthma_multiple_data(&cmd_ack_code, &res_target_id,
1438                                 &extra_length, word, data_endian);
1439
1440                 if (cmd_ack_code == AICE_CMD_T_FASTREAD_MEM) {
1441                         break;
1442                 } else {
1443                         if (retry_times > aice_max_retry_times) {
1444                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1445                                                 AICE_CMD_T_FASTREAD_MEM, cmd_ack_code);
1446
1447                                 return ERROR_FAIL;
1448                         }
1449
1450                         /* clear timeout and retry */
1451                         if (aice_reset_box() != ERROR_OK)
1452                                 return ERROR_FAIL;
1453
1454                         retry_times++;
1455                 }
1456         } while (1);
1457
1458         return ERROR_OK;
1459 }
1460
1461 int aice_fastwrite_mem(uint8_t target_id, const uint8_t *word, uint32_t num_of_words)
1462 {
1463         int32_t result;
1464         int retry_times = 0;
1465
1466         if (AICE_COMMAND_MODE_PACK == aice_command_mode) {
1467                 aice_usb_packet_flush();
1468         } else if (AICE_COMMAND_MODE_BATCH == aice_command_mode) {
1469                 aice_pack_htdmd_multiple_data(AICE_CMD_T_FASTWRITE_MEM, target_id,
1470                                 num_of_words - 1, 0, word, data_endian);
1471                 return aice_usb_packet_append(usb_out_buffer,
1472                                 AICE_FORMAT_HTDMD + (num_of_words - 1) * 4,
1473                                 AICE_FORMAT_DTHMB);
1474         }
1475
1476         do {
1477                 aice_pack_htdmd_multiple_data(AICE_CMD_T_FASTWRITE_MEM, target_id,
1478                                 num_of_words - 1, 0, word, data_endian);
1479
1480                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMD + (num_of_words - 1) * 4);
1481
1482                 LOG_DEBUG("FASTWRITE_MEM, COREID: %" PRIu8 ", # of DATA %08" PRIx32,
1483                                 target_id, num_of_words);
1484
1485                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
1486                 if (AICE_FORMAT_DTHMB != result) {
1487                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1488                                         AICE_FORMAT_DTHMB, result);
1489                         return ERROR_FAIL;
1490                 }
1491
1492                 uint8_t cmd_ack_code;
1493                 uint8_t extra_length;
1494                 uint8_t res_target_id;
1495                 aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
1496
1497                 if (cmd_ack_code == AICE_CMD_T_FASTWRITE_MEM) {
1498                         break;
1499                 } else {
1500                         if (retry_times > aice_max_retry_times) {
1501                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1502                                                 AICE_CMD_T_FASTWRITE_MEM, cmd_ack_code);
1503
1504                                 return ERROR_FAIL;
1505                         }
1506
1507                         /* clear timeout and retry */
1508                         if (aice_reset_box() != ERROR_OK)
1509                                 return ERROR_FAIL;
1510
1511                         retry_times++;
1512                 }
1513         } while (1);
1514
1515         return ERROR_OK;
1516 }
1517
1518 int aice_read_mem_b(uint8_t target_id, uint32_t address, uint32_t *data)
1519 {
1520         int32_t result;
1521         int retry_times = 0;
1522
1523         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
1524                 (AICE_COMMAND_MODE_BATCH == aice_command_mode))
1525                 aice_usb_packet_flush();
1526
1527         do {
1528                 aice_pack_htdmb(AICE_CMD_T_READ_MEM_B, target_id, 0, address);
1529
1530                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMB);
1531
1532                 LOG_DEBUG("READ_MEM_B, COREID: %" PRIu8 "", target_id);
1533
1534                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMA);
1535                 if (AICE_FORMAT_DTHMA != result) {
1536                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1537                                         AICE_FORMAT_DTHMA, result);
1538                         return ERROR_FAIL;
1539                 }
1540
1541                 uint8_t cmd_ack_code;
1542                 uint8_t extra_length;
1543                 uint8_t res_target_id;
1544                 aice_unpack_dthma(&cmd_ack_code, &res_target_id, &extra_length,
1545                                 data, data_endian);
1546
1547                 if (cmd_ack_code == AICE_CMD_T_READ_MEM_B) {
1548                         LOG_DEBUG("READ_MEM_B response, data: 0x%02" PRIx32, *data);
1549                         break;
1550                 } else {
1551                         if (retry_times > aice_max_retry_times) {
1552                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1553                                                 AICE_CMD_T_READ_MEM_B, cmd_ack_code);
1554
1555                                 return ERROR_FAIL;
1556                         }
1557
1558                         /* clear timeout and retry */
1559                         if (aice_reset_box() != ERROR_OK)
1560                                 return ERROR_FAIL;
1561
1562                         retry_times++;
1563                 }
1564         } while (1);
1565
1566         return ERROR_OK;
1567 }
1568
1569 int aice_read_mem_h(uint8_t target_id, uint32_t address, uint32_t *data)
1570 {
1571         int32_t result;
1572         int retry_times = 0;
1573
1574         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
1575                 (AICE_COMMAND_MODE_BATCH == aice_command_mode))
1576                 aice_usb_packet_flush();
1577
1578         do {
1579                 aice_pack_htdmb(AICE_CMD_T_READ_MEM_H, target_id, 0, (address >> 1) & 0x7FFFFFFF);
1580
1581                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMB);
1582
1583                 LOG_DEBUG("READ_MEM_H, CORE_ID: %" PRIu8 "", target_id);
1584
1585                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMA);
1586                 if (AICE_FORMAT_DTHMA != result) {
1587                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1588                                         AICE_FORMAT_DTHMA, result);
1589                         return ERROR_FAIL;
1590                 }
1591
1592                 uint8_t cmd_ack_code;
1593                 uint8_t extra_length;
1594                 uint8_t res_target_id;
1595                 aice_unpack_dthma(&cmd_ack_code, &res_target_id, &extra_length,
1596                                 data, data_endian);
1597
1598                 if (cmd_ack_code == AICE_CMD_T_READ_MEM_H) {
1599                         LOG_DEBUG("READ_MEM_H response, data: 0x%" PRIx32, *data);
1600                         break;
1601                 } else {
1602                         if (retry_times > aice_max_retry_times) {
1603                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1604                                                 AICE_CMD_T_READ_MEM_H, cmd_ack_code);
1605
1606                                 return ERROR_FAIL;
1607                         }
1608
1609                         /* clear timeout and retry */
1610                         if (aice_reset_box() != ERROR_OK)
1611                                 return ERROR_FAIL;
1612
1613                         retry_times++;
1614                 }
1615         } while (1);
1616
1617         return ERROR_OK;
1618 }
1619
1620 int aice_read_mem(uint8_t target_id, uint32_t address, uint32_t *data)
1621 {
1622         int32_t result;
1623         int retry_times = 0;
1624
1625         if ((AICE_COMMAND_MODE_PACK == aice_command_mode) ||
1626                 (AICE_COMMAND_MODE_BATCH == aice_command_mode))
1627                 aice_usb_packet_flush();
1628
1629         do {
1630                 aice_pack_htdmb(AICE_CMD_T_READ_MEM, target_id, 0,
1631                                 (address >> 2) & 0x3FFFFFFF);
1632
1633                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMB);
1634
1635                 LOG_DEBUG("READ_MEM, COREID: %" PRIu8 "", target_id);
1636
1637                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMA);
1638                 if (AICE_FORMAT_DTHMA != result) {
1639                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1640                                         AICE_FORMAT_DTHMA, result);
1641                         return ERROR_FAIL;
1642                 }
1643
1644                 uint8_t cmd_ack_code;
1645                 uint8_t extra_length;
1646                 uint8_t res_target_id;
1647                 aice_unpack_dthma(&cmd_ack_code, &res_target_id, &extra_length,
1648                                 data, data_endian);
1649
1650                 if (cmd_ack_code == AICE_CMD_T_READ_MEM) {
1651                         LOG_DEBUG("READ_MEM response, data: 0x%" PRIx32, *data);
1652                         break;
1653                 } else {
1654                         if (retry_times > aice_max_retry_times) {
1655                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1656                                                 AICE_CMD_T_READ_MEM, cmd_ack_code);
1657
1658                                 return ERROR_FAIL;
1659                         }
1660
1661                         /* clear timeout and retry */
1662                         if (aice_reset_box() != ERROR_OK)
1663                                 return ERROR_FAIL;
1664
1665                         retry_times++;
1666                 }
1667         } while (1);
1668
1669         return ERROR_OK;
1670 }
1671
1672 int aice_batch_buffer_read(uint8_t buf_index, uint32_t *word, uint32_t num_of_words)
1673 {
1674         int32_t result;
1675         int retry_times = 0;
1676
1677         do {
1678                 aice_pack_htdma(AICE_CMD_BATCH_BUFFER_READ, 0, num_of_words - 1, buf_index);
1679
1680                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMA);
1681
1682                 LOG_DEBUG("BATCH_BUFFER_READ, # of DATA %08" PRIx32, num_of_words);
1683
1684                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMA + (num_of_words - 1) * 4);
1685                 if (result < 0) {
1686                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1687                                         AICE_FORMAT_DTHMA + (num_of_words - 1) * 4, result);
1688                         return ERROR_FAIL;
1689                 }
1690
1691                 uint8_t cmd_ack_code;
1692                 uint8_t extra_length;
1693                 uint8_t res_target_id;
1694                 aice_unpack_dthma_multiple_data(&cmd_ack_code, &res_target_id,
1695                                 &extra_length, (uint8_t *)word, data_endian);
1696
1697                 if (cmd_ack_code == AICE_CMD_BATCH_BUFFER_READ) {
1698                         break;
1699                 } else {
1700                         if (retry_times > aice_max_retry_times) {
1701                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1702                                                 AICE_CMD_BATCH_BUFFER_READ, cmd_ack_code);
1703
1704                                 return ERROR_FAIL;
1705                         }
1706
1707                         /* clear timeout and retry */
1708                         if (aice_reset_box() != ERROR_OK)
1709                                 return ERROR_FAIL;
1710
1711                         retry_times++;
1712                 }
1713         } while (1);
1714
1715         return ERROR_OK;
1716 }
1717
1718 int aice_batch_buffer_write(uint8_t buf_index, const uint8_t *word, uint32_t num_of_words)
1719 {
1720         int32_t result;
1721         int retry_times = 0;
1722
1723         if (num_of_words == 0)
1724                 return ERROR_OK;
1725
1726         do {
1727                 /* only pack AICE_CMD_BATCH_BUFFER_WRITE command header */
1728                 aice_pack_htdmc(AICE_CMD_BATCH_BUFFER_WRITE, 0, num_of_words - 1, buf_index,
1729                                 0, data_endian);
1730
1731                 /* use append instead of pack */
1732                 memcpy(usb_out_buffer + 4, word, num_of_words * 4);
1733
1734                 aice_usb_write(usb_out_buffer, AICE_FORMAT_HTDMC + (num_of_words - 1) * 4);
1735
1736                 LOG_DEBUG("BATCH_BUFFER_WRITE, # of DATA %08" PRIx32, num_of_words);
1737
1738                 result = aice_usb_read(usb_in_buffer, AICE_FORMAT_DTHMB);
1739                 if (AICE_FORMAT_DTHMB != result) {
1740                         LOG_ERROR("aice_usb_read failed (requested=%" PRId32 ", result=%" PRId32 ")",
1741                                         AICE_FORMAT_DTHMB, result);
1742                         return ERROR_FAIL;
1743                 }
1744
1745                 uint8_t cmd_ack_code;
1746                 uint8_t extra_length;
1747                 uint8_t res_target_id;
1748                 aice_unpack_dthmb(&cmd_ack_code, &res_target_id, &extra_length);
1749
1750                 if (cmd_ack_code == AICE_CMD_BATCH_BUFFER_WRITE) {
1751                         break;
1752                 } else {
1753                         if (retry_times > aice_max_retry_times) {
1754                                 LOG_ERROR("aice command timeout (command=0x%" PRIx8 ", response=0x%" PRIx8 ")",
1755                                                 AICE_CMD_BATCH_BUFFER_WRITE, cmd_ack_code);
1756
1757                                 return ERROR_FAIL;
1758                         }
1759
1760                         /* clear timeout and retry */
1761                         if (aice_reset_box() != ERROR_OK)
1762                                 return ERROR_FAIL;
1763
1764                         retry_times++;
1765                 }
1766         } while (1);
1767
1768         return ERROR_OK;
1769 }
1770
1771 /***************************************************************************/
1772 /* End of AICE commands */
1773
1774 typedef int (*read_mem_func_t)(uint32_t coreid, uint32_t address, uint32_t *data);
1775 typedef int (*write_mem_func_t)(uint32_t coreid, uint32_t address, uint32_t data);
1776
1777 struct aice_nds32_info core_info[AICE_MAX_NUM_CORE];
1778 static uint8_t total_num_of_core;
1779
1780 static char *custom_srst_script;
1781 static char *custom_trst_script;
1782 static char *custom_restart_script;
1783 static uint32_t aice_count_to_check_dbger = 30;
1784
1785 static int aice_read_reg(uint32_t coreid, uint32_t num, uint32_t *val);
1786 static int aice_write_reg(uint32_t coreid, uint32_t num, uint32_t val);
1787
1788 static int check_suppressed_exception(uint32_t coreid, uint32_t dbger_value)
1789 {
1790         uint32_t ir4_value;
1791         uint32_t ir6_value;
1792         /* the default value of handling_suppressed_exception is false */
1793         static bool handling_suppressed_exception;
1794
1795         if (handling_suppressed_exception)
1796                 return ERROR_OK;
1797
1798         if ((dbger_value & NDS_DBGER_ALL_SUPRS_EX) == NDS_DBGER_ALL_SUPRS_EX) {
1799                 LOG_ERROR("<-- TARGET WARNING! Exception is detected and suppressed. -->");
1800                 handling_suppressed_exception = true;
1801
1802                 aice_read_reg(coreid, IR4, &ir4_value);
1803                 /* Clear IR6.SUPRS_EXC, IR6.IMP_EXC */
1804                 aice_read_reg(coreid, IR6, &ir6_value);
1805                 /*
1806                  * For MCU version(MSC_CFG.MCU == 1) like V3m
1807                  *  | SWID[30:16] | Reserved[15:10] | SUPRS_EXC[9]  | IMP_EXC[8]
1808                  *  |VECTOR[7:5] | INST[4] | Exc Type[3:0] |
1809                  *
1810                  * For non-MCU version(MSC_CFG.MCU == 0) like V3
1811                  *  | SWID[30:16] | Reserved[15:14] | SUPRS_EXC[13] | IMP_EXC[12]
1812                  *  | VECTOR[11:5] | INST[4] | Exc Type[3:0] |
1813                  */
1814                 LOG_INFO("EVA: 0x%08" PRIx32, ir4_value);
1815                 LOG_INFO("ITYPE: 0x%08" PRIx32, ir6_value);
1816
1817                 ir6_value = ir6_value & (~0x300); /* for MCU */
1818                 ir6_value = ir6_value & (~0x3000); /* for non-MCU */
1819                 aice_write_reg(coreid, IR6, ir6_value);
1820
1821                 handling_suppressed_exception = false;
1822         }
1823
1824         return ERROR_OK;
1825 }
1826
1827 static int check_privilege(uint32_t coreid, uint32_t dbger_value)
1828 {
1829         if ((dbger_value & NDS_DBGER_ILL_SEC_ACC) == NDS_DBGER_ILL_SEC_ACC) {
1830                 LOG_ERROR("<-- TARGET ERROR! Insufficient security privilege "
1831                                 "to execute the debug operations. -->");
1832
1833                 /* Clear DBGER.ILL_SEC_ACC */
1834                 if (aice_write_misc(coreid, NDS_EDM_MISC_DBGER,
1835                                         NDS_DBGER_ILL_SEC_ACC) != ERROR_OK)
1836                         return ERROR_FAIL;
1837         }
1838
1839         return ERROR_OK;
1840 }
1841
1842 static int aice_check_dbger(uint32_t coreid, uint32_t expect_status)
1843 {
1844         uint32_t i = 0;
1845         uint32_t value_dbger;
1846
1847         while (1) {
1848                 aice_read_misc(coreid, NDS_EDM_MISC_DBGER, &value_dbger);
1849
1850                 if ((value_dbger & expect_status) == expect_status) {
1851                         if (ERROR_OK != check_suppressed_exception(coreid, value_dbger))
1852                                 return ERROR_FAIL;
1853                         if (ERROR_OK != check_privilege(coreid, value_dbger))
1854                                 return ERROR_FAIL;
1855                         return ERROR_OK;
1856                 }
1857
1858                 if ((i % 30) == 0)
1859                         keep_alive();
1860
1861                 long long then = 0;
1862                 if (i == aice_count_to_check_dbger)
1863                         then = timeval_ms();
1864                 if (i >= aice_count_to_check_dbger) {
1865                         if ((timeval_ms() - then) > 1000) {
1866                                 LOG_ERROR("Timeout (1000ms) waiting for $DBGER status "
1867                                                 "being 0x%08" PRIx32, expect_status);
1868                                 return ERROR_FAIL;
1869                         }
1870                 }
1871                 i++;
1872         }
1873
1874         return ERROR_FAIL;
1875 }
1876
1877 static int aice_execute_dim(uint32_t coreid, uint32_t *insts, uint8_t n_inst)
1878 {
1879         /** fill DIM */
1880         if (aice_write_dim(coreid, insts, n_inst) != ERROR_OK)
1881                 return ERROR_FAIL;
1882
1883         /** clear DBGER.DPED */
1884         if (aice_write_misc(coreid, NDS_EDM_MISC_DBGER, NDS_DBGER_DPED) != ERROR_OK)
1885                 return ERROR_FAIL;
1886
1887         /** execute DIM */
1888         if (aice_do_execute(coreid) != ERROR_OK)
1889                 return ERROR_FAIL;
1890
1891         /** read DBGER.DPED */
1892         if (aice_check_dbger(coreid, NDS_DBGER_DPED) != ERROR_OK) {
1893                 LOG_ERROR("<-- TARGET ERROR! Debug operations do not finish properly: "
1894                                 "0x%08" PRIx32 "0x%08" PRIx32 "0x%08" PRIx32 "0x%08" PRIx32 ". -->",
1895                                 insts[0],
1896                                 insts[1],
1897                                 insts[2],
1898                                 insts[3]);
1899                 return ERROR_FAIL;
1900         }
1901
1902         return ERROR_OK;
1903 }
1904
1905 static int aice_read_reg(uint32_t coreid, uint32_t num, uint32_t *val)
1906 {
1907         LOG_DEBUG("aice_read_reg, reg_no: 0x%08" PRIx32, num);
1908
1909         uint32_t instructions[4]; /** execute instructions in DIM */
1910
1911         if (NDS32_REG_TYPE_GPR == nds32_reg_type(num)) { /* general registers */
1912                 instructions[0] = MTSR_DTR(num);
1913                 instructions[1] = DSB;
1914                 instructions[2] = NOP;
1915                 instructions[3] = BEQ_MINUS_12;
1916         } else if (NDS32_REG_TYPE_SPR == nds32_reg_type(num)) { /* user special registers */
1917                 instructions[0] = MFUSR_G0(0, nds32_reg_sr_index(num));
1918                 instructions[1] = MTSR_DTR(0);
1919                 instructions[2] = DSB;
1920                 instructions[3] = BEQ_MINUS_12;
1921         } else if (NDS32_REG_TYPE_AUMR == nds32_reg_type(num)) { /* audio registers */
1922                 if ((CB_CTL <= num) && (num <= CBE3)) {
1923                         instructions[0] = AMFAR2(0, nds32_reg_sr_index(num));
1924                         instructions[1] = MTSR_DTR(0);
1925                         instructions[2] = DSB;
1926                         instructions[3] = BEQ_MINUS_12;
1927                 } else {
1928                         instructions[0] = AMFAR(0, nds32_reg_sr_index(num));
1929                         instructions[1] = MTSR_DTR(0);
1930                         instructions[2] = DSB;
1931                         instructions[3] = BEQ_MINUS_12;
1932                 }
1933         } else if (NDS32_REG_TYPE_FPU == nds32_reg_type(num)) { /* fpu registers */
1934                 if (FPCSR == num) {
1935                         instructions[0] = FMFCSR;
1936                         instructions[1] = MTSR_DTR(0);
1937                         instructions[2] = DSB;
1938                         instructions[3] = BEQ_MINUS_12;
1939                 } else if (FPCFG == num) {
1940                         instructions[0] = FMFCFG;
1941                         instructions[1] = MTSR_DTR(0);
1942                         instructions[2] = DSB;
1943                         instructions[3] = BEQ_MINUS_12;
1944                 } else {
1945                         if (FS0 <= num && num <= FS31) { /* single precision */
1946                                 instructions[0] = FMFSR(0, nds32_reg_sr_index(num));
1947                                 instructions[1] = MTSR_DTR(0);
1948                                 instructions[2] = DSB;
1949                                 instructions[3] = BEQ_MINUS_12;
1950                         } else if (FD0 <= num && num <= FD31) { /* double precision */
1951                                 instructions[0] = FMFDR(0, nds32_reg_sr_index(num));
1952                                 instructions[1] = MTSR_DTR(0);
1953                                 instructions[2] = DSB;
1954                                 instructions[3] = BEQ_MINUS_12;
1955                         }
1956                 }
1957         } else { /* system registers */
1958                 instructions[0] = MFSR(0, nds32_reg_sr_index(num));
1959                 instructions[1] = MTSR_DTR(0);
1960                 instructions[2] = DSB;
1961                 instructions[3] = BEQ_MINUS_12;
1962         }
1963
1964         aice_execute_dim(coreid, instructions, 4);
1965
1966         uint32_t value_edmsw;
1967         aice_read_edmsr(coreid, NDS_EDM_SR_EDMSW, &value_edmsw);
1968         if (value_edmsw & NDS_EDMSW_WDV)
1969                 aice_read_dtr(coreid, val);
1970         else {
1971                 LOG_ERROR("<-- TARGET ERROR! The debug target failed to update "
1972                                 "the DTR register. -->");
1973                 return ERROR_FAIL;
1974         }
1975
1976         return ERROR_OK;
1977 }
1978
1979 static int aice_usb_read_reg(uint32_t coreid, uint32_t num, uint32_t *val)
1980 {
1981         LOG_DEBUG("aice_usb_read_reg");
1982
1983         if (num == R0) {
1984                 *val = core_info[coreid].r0_backup;
1985         } else if (num == R1) {
1986                 *val = core_info[coreid].r1_backup;
1987         } else if (num == DR41) {
1988                 /* As target is halted, OpenOCD will backup DR41/DR42/DR43.
1989                  * As user wants to read these registers, OpenOCD should return
1990                  * the backup values, instead of reading the real values.
1991                  * As user wants to write these registers, OpenOCD should write
1992                  * to the backup values, instead of writing to real registers. */
1993                 *val = core_info[coreid].edmsw_backup;
1994         } else if (num == DR42) {
1995                 *val = core_info[coreid].edm_ctl_backup;
1996         } else if ((core_info[coreid].target_dtr_valid == true) && (num == DR43)) {
1997                 *val = core_info[coreid].target_dtr_backup;
1998         } else {
1999                 if (ERROR_OK != aice_read_reg(coreid, num, val))
2000                         *val = 0xBBADBEEF;
2001         }
2002
2003         return ERROR_OK;
2004 }
2005
2006 static int aice_write_reg(uint32_t coreid, uint32_t num, uint32_t val)
2007 {
2008         LOG_DEBUG("aice_write_reg, reg_no: 0x%08" PRIx32 ", value: 0x%08" PRIx32, num, val);
2009
2010         uint32_t instructions[4]; /** execute instructions in DIM */
2011         uint32_t value_edmsw;
2012
2013         aice_write_dtr(coreid, val);
2014         aice_read_edmsr(coreid, NDS_EDM_SR_EDMSW, &value_edmsw);
2015         if (0 == (value_edmsw & NDS_EDMSW_RDV)) {
2016                 LOG_ERROR("<-- TARGET ERROR! AICE failed to write to the DTR register. -->");
2017                 return ERROR_FAIL;
2018         }
2019
2020         if (NDS32_REG_TYPE_GPR == nds32_reg_type(num)) { /* general registers */
2021                 instructions[0] = MFSR_DTR(num);
2022                 instructions[1] = DSB;
2023                 instructions[2] = NOP;
2024                 instructions[3] = BEQ_MINUS_12;
2025         } else if (NDS32_REG_TYPE_SPR == nds32_reg_type(num)) { /* user special registers */
2026                 instructions[0] = MFSR_DTR(0);
2027                 instructions[1] = MTUSR_G0(0, nds32_reg_sr_index(num));
2028                 instructions[2] = DSB;
2029                 instructions[3] = BEQ_MINUS_12;
2030         } else if (NDS32_REG_TYPE_AUMR == nds32_reg_type(num)) { /* audio registers */
2031                 if ((CB_CTL <= num) && (num <= CBE3)) {
2032                         instructions[0] = MFSR_DTR(0);
2033                         instructions[1] = AMTAR2(0, nds32_reg_sr_index(num));
2034                         instructions[2] = DSB;
2035                         instructions[3] = BEQ_MINUS_12;
2036                 } else {
2037                         instructions[0] = MFSR_DTR(0);
2038                         instructions[1] = AMTAR(0, nds32_reg_sr_index(num));
2039                         instructions[2] = DSB;
2040                         instructions[3] = BEQ_MINUS_12;
2041                 }
2042         } else if (NDS32_REG_TYPE_FPU == nds32_reg_type(num)) { /* fpu registers */
2043                 if (FPCSR == num) {
2044                         instructions[0] = MFSR_DTR(0);
2045                         instructions[1] = FMTCSR;
2046                         instructions[2] = DSB;
2047                         instructions[3] = BEQ_MINUS_12;
2048                 } else if (FPCFG == num) {
2049                         /* FPCFG is readonly */
2050                 } else {
2051                         if (FS0 <= num && num <= FS31) { /* single precision */
2052                                 instructions[0] = MFSR_DTR(0);
2053                                 instructions[1] = FMTSR(0, nds32_reg_sr_index(num));
2054                                 instructions[2] = DSB;
2055                                 instructions[3] = BEQ_MINUS_12;
2056                         } else if (FD0 <= num && num <= FD31) { /* double precision */
2057                                 instructions[0] = MFSR_DTR(0);
2058                                 instructions[1] = FMTDR(0, nds32_reg_sr_index(num));
2059                                 instructions[2] = DSB;
2060                                 instructions[3] = BEQ_MINUS_12;
2061                         }
2062                 }
2063         } else {
2064                 instructions[0] = MFSR_DTR(0);
2065                 instructions[1] = MTSR(0, nds32_reg_sr_index(num));
2066                 instructions[2] = DSB;
2067                 instructions[3] = BEQ_MINUS_12;
2068         }
2069
2070         return aice_execute_dim(coreid, instructions, 4);
2071 }
2072
2073 static int aice_usb_write_reg(uint32_t coreid, uint32_t num, uint32_t val)
2074 {
2075         LOG_DEBUG("aice_usb_write_reg");
2076
2077         if (num == R0)
2078                 core_info[coreid].r0_backup = val;
2079         else if (num == R1)
2080                 core_info[coreid].r1_backup = val;
2081         else if (num == DR42)
2082                 /* As target is halted, OpenOCD will backup DR41/DR42/DR43.
2083                  * As user wants to read these registers, OpenOCD should return
2084                  * the backup values, instead of reading the real values.
2085                  * As user wants to write these registers, OpenOCD should write
2086                  * to the backup values, instead of writing to real registers. */
2087                 core_info[coreid].edm_ctl_backup = val;
2088         else if ((core_info[coreid].target_dtr_valid == true) && (num == DR43))
2089                 core_info[coreid].target_dtr_backup = val;
2090         else
2091                 return aice_write_reg(coreid, num, val);
2092
2093         return ERROR_OK;
2094 }
2095
2096 static int aice_usb_open(struct aice_port_param_s *param)
2097 {
2098         const uint16_t vids[] = { param->vid, 0 };
2099         const uint16_t pids[] = { param->pid, 0 };
2100         struct jtag_libusb_device_handle *devh;
2101
2102         if (jtag_libusb_open(vids, pids, &devh) != ERROR_OK)
2103                 return ERROR_FAIL;
2104
2105         /* BE ***VERY CAREFUL*** ABOUT MAKING CHANGES IN THIS
2106          * AREA!!!!!!!!!!!  The behavior of libusb is not completely
2107          * consistent across Windows, Linux, and Mac OS X platforms.
2108          * The actions taken in the following compiler conditionals may
2109          * not agree with published documentation for libusb, but were
2110          * found to be necessary through trials and tribulations.  Even
2111          * little tweaks can break one or more platforms, so if you do
2112          * make changes test them carefully on all platforms before
2113          * committing them!
2114          */
2115
2116 #if IS_WIN32 == 0
2117
2118         jtag_libusb_reset_device(devh);
2119
2120 #if IS_DARWIN == 0
2121
2122         int timeout = 5;
2123         /* reopen jlink after usb_reset
2124          * on win32 this may take a second or two to re-enumerate */
2125         int retval;
2126         while ((retval = jtag_libusb_open(vids, pids, &devh)) != ERROR_OK) {
2127                 usleep(1000);
2128                 timeout--;
2129                 if (!timeout)
2130                         break;
2131         }
2132         if (ERROR_OK != retval)
2133                 return ERROR_FAIL;
2134 #endif
2135
2136 #endif
2137
2138         /* usb_set_configuration required under win32 */
2139         struct jtag_libusb_device *udev = jtag_libusb_get_device(devh);
2140         jtag_libusb_set_configuration(devh, 0);
2141         jtag_libusb_claim_interface(devh, 0);
2142
2143         unsigned int aice_read_ep;
2144         unsigned int aice_write_ep;
2145         jtag_libusb_get_endpoints(udev, &aice_read_ep, &aice_write_ep);
2146
2147         aice_handler.usb_read_ep = aice_read_ep;
2148         aice_handler.usb_write_ep = aice_write_ep;
2149         aice_handler.usb_handle = devh;
2150
2151         return ERROR_OK;
2152 }
2153
2154 static int aice_usb_read_reg_64(uint32_t coreid, uint32_t num, uint64_t *val)
2155 {
2156         LOG_DEBUG("aice_usb_read_reg_64, %s", nds32_reg_simple_name(num));
2157
2158         uint32_t value;
2159         uint32_t high_value;
2160
2161         if (ERROR_OK != aice_read_reg(coreid, num, &value))
2162                 value = 0xBBADBEEF;
2163
2164         aice_read_reg(coreid, R1, &high_value);
2165
2166         LOG_DEBUG("low: 0x%08" PRIx32 ", high: 0x%08" PRIx32 "\n", value, high_value);
2167
2168         if (data_endian == AICE_BIG_ENDIAN)
2169                 *val = (((uint64_t)high_value) << 32) | value;
2170         else
2171                 *val = (((uint64_t)value) << 32) | high_value;
2172
2173         return ERROR_OK;
2174 }
2175
2176 static int aice_usb_write_reg_64(uint32_t coreid, uint32_t num, uint64_t val)
2177 {
2178         uint32_t value;
2179         uint32_t high_value;
2180
2181         if (data_endian == AICE_BIG_ENDIAN) {
2182                 value = val & 0xFFFFFFFF;
2183                 high_value = (val >> 32) & 0xFFFFFFFF;
2184         } else {
2185                 high_value = val & 0xFFFFFFFF;
2186                 value = (val >> 32) & 0xFFFFFFFF;
2187         }
2188
2189         LOG_DEBUG("aice_usb_write_reg_64, %s, low: 0x%08" PRIx32 ", high: 0x%08" PRIx32 "\n",
2190                         nds32_reg_simple_name(num), value, high_value);
2191
2192         aice_write_reg(coreid, R1, high_value);
2193         return aice_write_reg(coreid, num, value);
2194 }
2195
2196 static int aice_get_version_info(void)
2197 {
2198         uint32_t hardware_version;
2199         uint32_t firmware_version;
2200         uint32_t fpga_version;
2201
2202         if (aice_read_ctrl(AICE_READ_CTRL_GET_HARDWARE_VERSION, &hardware_version) != ERROR_OK)
2203                 return ERROR_FAIL;
2204
2205         if (aice_read_ctrl(AICE_READ_CTRL_GET_FIRMWARE_VERSION, &firmware_version) != ERROR_OK)
2206                 return ERROR_FAIL;
2207
2208         if (aice_read_ctrl(AICE_READ_CTRL_GET_FPGA_VERSION, &fpga_version) != ERROR_OK)
2209                 return ERROR_FAIL;
2210
2211         LOG_INFO("AICE version: hw_ver = 0x%" PRIx32 ", fw_ver = 0x%" PRIx32 ", fpga_ver = 0x%" PRIx32,
2212                         hardware_version, firmware_version, fpga_version);
2213
2214         return ERROR_OK;
2215 }
2216
2217 #define LINE_BUFFER_SIZE 1024
2218
2219 static int aice_execute_custom_script(const char *script)
2220 {
2221         FILE *script_fd;
2222         char line_buffer[LINE_BUFFER_SIZE];
2223         char *op_str;
2224         char *reset_str;
2225         uint32_t delay;
2226         uint32_t write_ctrl_value;
2227         bool set_op;
2228
2229         script_fd = fopen(script, "r");
2230         if (script_fd == NULL) {
2231                 return ERROR_FAIL;
2232         } else {
2233                 while (fgets(line_buffer, LINE_BUFFER_SIZE, script_fd) != NULL) {
2234                         /* execute operations */
2235                         set_op = false;
2236                         op_str = strstr(line_buffer, "set");
2237                         if (op_str != NULL) {
2238                                 set_op = true;
2239                                 goto get_reset_type;
2240                         }
2241
2242                         op_str = strstr(line_buffer, "clear");
2243                         if (op_str == NULL)
2244                                 continue;
2245 get_reset_type:
2246                         reset_str = strstr(op_str, "srst");
2247                         if (reset_str != NULL) {
2248                                 if (set_op)
2249                                         write_ctrl_value = AICE_CUSTOM_DELAY_SET_SRST;
2250                                 else
2251                                         write_ctrl_value = AICE_CUSTOM_DELAY_CLEAN_SRST;
2252                                 goto get_delay;
2253                         }
2254                         reset_str = strstr(op_str, "dbgi");
2255                         if (reset_str != NULL) {
2256                                 if (set_op)
2257                                         write_ctrl_value = AICE_CUSTOM_DELAY_SET_DBGI;
2258                                 else
2259                                         write_ctrl_value = AICE_CUSTOM_DELAY_CLEAN_DBGI;
2260                                 goto get_delay;
2261                         }
2262                         reset_str = strstr(op_str, "trst");
2263                         if (reset_str != NULL) {
2264                                 if (set_op)
2265                                         write_ctrl_value = AICE_CUSTOM_DELAY_SET_TRST;
2266                                 else
2267                                         write_ctrl_value = AICE_CUSTOM_DELAY_CLEAN_TRST;
2268                                 goto get_delay;
2269                         }
2270                         continue;
2271 get_delay:
2272                         /* get delay */
2273                         delay = strtoul(reset_str + 4, NULL, 0);
2274                         write_ctrl_value |= (delay << 16);
2275
2276                         if (aice_write_ctrl(AICE_WRITE_CTRL_CUSTOM_DELAY,
2277                                                 write_ctrl_value) != ERROR_OK) {
2278                                 fclose(script_fd);
2279                                 return ERROR_FAIL;
2280                         }
2281                 }
2282                 fclose(script_fd);
2283         }
2284
2285         return ERROR_OK;
2286 }
2287
2288 static int aice_usb_set_clock(int set_clock)
2289 {
2290         if (aice_write_ctrl(AICE_WRITE_CTRL_TCK_CONTROL,
2291                                 AICE_TCK_CONTROL_TCK_SCAN) != ERROR_OK)
2292                 return ERROR_FAIL;
2293
2294         /* Read out TCK_SCAN clock value */
2295         uint32_t scan_clock;
2296         if (aice_read_ctrl(AICE_READ_CTRL_GET_ICE_STATE, &scan_clock) != ERROR_OK)
2297                 return ERROR_FAIL;
2298
2299         scan_clock &= 0x0F;
2300
2301         uint32_t scan_base_freq;
2302         if (scan_clock & 0x8)
2303                 scan_base_freq = 48000; /* 48 MHz */
2304         else
2305                 scan_base_freq = 30000; /* 30 MHz */
2306
2307         uint32_t set_base_freq;
2308         if (set_clock & 0x8)
2309                 set_base_freq = 48000;
2310         else
2311                 set_base_freq = 30000;
2312
2313         uint32_t set_freq;
2314         uint32_t scan_freq;
2315         set_freq = set_base_freq >> (set_clock & 0x7);
2316         scan_freq = scan_base_freq >> (scan_clock & 0x7);
2317
2318         if (scan_freq < set_freq) {
2319                 LOG_ERROR("User specifies higher jtag clock than TCK_SCAN clock");
2320                 return ERROR_FAIL;
2321         }
2322
2323         if (aice_write_ctrl(AICE_WRITE_CTRL_TCK_CONTROL, set_clock) != ERROR_OK)
2324                 return ERROR_FAIL;
2325
2326         uint32_t check_speed;
2327         if (aice_read_ctrl(AICE_READ_CTRL_GET_ICE_STATE, &check_speed) != ERROR_OK)
2328                 return ERROR_FAIL;
2329
2330         if (((int)check_speed & 0x0F) != set_clock) {
2331                 LOG_ERROR("Set jtag clock failed");
2332                 return ERROR_FAIL;
2333         }
2334
2335         return ERROR_OK;
2336 }
2337
2338 static int aice_edm_init(uint32_t coreid)
2339 {
2340         aice_write_edmsr(coreid, NDS_EDM_SR_DIMBR, 0xFFFF0000);
2341         aice_write_misc(coreid, NDS_EDM_MISC_DIMIR, 0);
2342
2343         /* unconditionally try to turn on V3_EDM_MODE */
2344         uint32_t edm_ctl_value;
2345         aice_read_edmsr(coreid, NDS_EDM_SR_EDM_CTL, &edm_ctl_value);
2346         aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL, edm_ctl_value | 0x00000040);
2347
2348         /* clear DBGER */
2349         aice_write_misc(coreid, NDS_EDM_MISC_DBGER,
2350                         NDS_DBGER_DPED | NDS_DBGER_CRST | NDS_DBGER_AT_MAX);
2351
2352         /* get EDM version */
2353         uint32_t value_edmcfg;
2354         aice_read_edmsr(coreid, NDS_EDM_SR_EDM_CFG, &value_edmcfg);
2355         core_info[coreid].edm_version = (value_edmcfg >> 16) & 0xFFFF;
2356
2357         return ERROR_OK;
2358 }
2359
2360 static bool is_v2_edm(uint32_t coreid)
2361 {
2362         if ((core_info[coreid].edm_version & 0x1000) == 0)
2363                 return true;
2364         else
2365                 return false;
2366 }
2367
2368 static int aice_init_edm_registers(uint32_t coreid, bool clear_dex_use_psw)
2369 {
2370         /* enable DEH_SEL & MAX_STOP & V3_EDM_MODE & DBGI_MASK */
2371         uint32_t host_edm_ctl = core_info[coreid].edm_ctl_backup | 0xA000004F;
2372         if (clear_dex_use_psw)
2373                 /* After entering debug mode, OpenOCD may set
2374                  * DEX_USE_PSW accidentally through backup value
2375                  * of target EDM_CTL.
2376                  * So, clear DEX_USE_PSW by force. */
2377                 host_edm_ctl &= ~(0x40000000);
2378
2379         LOG_DEBUG("aice_init_edm_registers - EDM_CTL: 0x%08" PRIx32, host_edm_ctl);
2380
2381         int result = aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL, host_edm_ctl);
2382
2383         return result;
2384 }
2385
2386 /**
2387  * EDM_CTL will be modified by OpenOCD as debugging. OpenOCD has the
2388  * responsibility to keep EDM_CTL untouched after debugging.
2389  *
2390  * There are two scenarios to consider:
2391  * 1. single step/running as debugging (running under debug session)
2392  * 2. detached from gdb (exit debug session)
2393  *
2394  * So, we need to bakcup EDM_CTL before halted and restore it after
2395  * running. The difference of these two scenarios is EDM_CTL.DEH_SEL
2396  * is on for scenario 1, and off for scenario 2.
2397  */
2398 static int aice_backup_edm_registers(uint32_t coreid)
2399 {
2400         int result = aice_read_edmsr(coreid, NDS_EDM_SR_EDM_CTL,
2401                         &core_info[coreid].edm_ctl_backup);
2402
2403         /* To call aice_backup_edm_registers() after DEX on, DEX_USE_PSW
2404          * may be not correct.  (For example, hit breakpoint, then backup
2405          * EDM_CTL. EDM_CTL.DEX_USE_PSW will be cleared.)  Because debug
2406          * interrupt will clear DEX_USE_PSW, DEX_USE_PSW is always off after
2407          * DEX is on.  It only backups correct value before OpenOCD issues DBGI.
2408          * (Backup EDM_CTL, then issue DBGI actively (refer aice_usb_halt())) */
2409         if (core_info[coreid].edm_ctl_backup & 0x40000000)
2410                 core_info[coreid].dex_use_psw_on = true;
2411         else
2412                 core_info[coreid].dex_use_psw_on = false;
2413
2414         LOG_DEBUG("aice_backup_edm_registers - EDM_CTL: 0x%08" PRIx32 ", DEX_USE_PSW: %s",
2415                         core_info[coreid].edm_ctl_backup,
2416                         core_info[coreid].dex_use_psw_on ? "on" : "off");
2417
2418         return result;
2419 }
2420
2421 static int aice_restore_edm_registers(uint32_t coreid)
2422 {
2423         LOG_DEBUG("aice_restore_edm_registers -");
2424
2425         /* set DEH_SEL, because target still under EDM control */
2426         int result = aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL,
2427                         core_info[coreid].edm_ctl_backup | 0x80000000);
2428
2429         return result;
2430 }
2431
2432 static int aice_backup_tmp_registers(uint32_t coreid)
2433 {
2434         LOG_DEBUG("backup_tmp_registers -");
2435
2436         /* backup target DTR first(if the target DTR is valid) */
2437         uint32_t value_edmsw;
2438         aice_read_edmsr(coreid, NDS_EDM_SR_EDMSW, &value_edmsw);
2439         core_info[coreid].edmsw_backup = value_edmsw;
2440         if (value_edmsw & 0x1) { /* EDMSW.WDV == 1 */
2441                 aice_read_dtr(coreid, &core_info[coreid].target_dtr_backup);
2442                 core_info[coreid].target_dtr_valid = true;
2443
2444                 LOG_DEBUG("Backup target DTR: 0x%08" PRIx32, core_info[coreid].target_dtr_backup);
2445         } else {
2446                 core_info[coreid].target_dtr_valid = false;
2447         }
2448
2449         /* Target DTR has been backup, then backup $R0 and $R1 */
2450         aice_read_reg(coreid, R0, &core_info[coreid].r0_backup);
2451         aice_read_reg(coreid, R1, &core_info[coreid].r1_backup);
2452
2453         /* backup host DTR(if the host DTR is valid) */
2454         if (value_edmsw & 0x2) { /* EDMSW.RDV == 1*/
2455                 /* read out host DTR and write into target DTR, then use aice_read_edmsr to
2456                  * read out */
2457                 uint32_t instructions[4] = {
2458                         MFSR_DTR(R0), /* R0 has already been backup */
2459                         DSB,
2460                         MTSR_DTR(R0),
2461                         BEQ_MINUS_12
2462                 };
2463                 aice_execute_dim(coreid, instructions, 4);
2464
2465                 aice_read_dtr(coreid, &core_info[coreid].host_dtr_backup);
2466                 core_info[coreid].host_dtr_valid = true;
2467
2468                 LOG_DEBUG("Backup host DTR: 0x%08" PRIx32, core_info[coreid].host_dtr_backup);
2469         } else {
2470                 core_info[coreid].host_dtr_valid = false;
2471         }
2472
2473         LOG_DEBUG("r0: 0x%08" PRIx32 ", r1: 0x%08" PRIx32,
2474                         core_info[coreid].r0_backup, core_info[coreid].r1_backup);
2475
2476         return ERROR_OK;
2477 }
2478
2479 static int aice_restore_tmp_registers(uint32_t coreid)
2480 {
2481         LOG_DEBUG("restore_tmp_registers - r0: 0x%08" PRIx32 ", r1: 0x%08" PRIx32,
2482                         core_info[coreid].r0_backup, core_info[coreid].r1_backup);
2483
2484         if (core_info[coreid].target_dtr_valid) {
2485                 uint32_t instructions[4] = {
2486                         SETHI(R0, core_info[coreid].target_dtr_backup >> 12),
2487                         ORI(R0, R0, core_info[coreid].target_dtr_backup & 0x00000FFF),
2488                         NOP,
2489                         BEQ_MINUS_12
2490                 };
2491                 aice_execute_dim(coreid, instructions, 4);
2492
2493                 instructions[0] = MTSR_DTR(R0);
2494                 instructions[1] = DSB;
2495                 instructions[2] = NOP;
2496                 instructions[3] = BEQ_MINUS_12;
2497                 aice_execute_dim(coreid, instructions, 4);
2498
2499                 LOG_DEBUG("Restore target DTR: 0x%08" PRIx32, core_info[coreid].target_dtr_backup);
2500         }
2501
2502         aice_write_reg(coreid, R0, core_info[coreid].r0_backup);
2503         aice_write_reg(coreid, R1, core_info[coreid].r1_backup);
2504
2505         if (core_info[coreid].host_dtr_valid) {
2506                 aice_write_dtr(coreid, core_info[coreid].host_dtr_backup);
2507
2508                 LOG_DEBUG("Restore host DTR: 0x%08" PRIx32, core_info[coreid].host_dtr_backup);
2509         }
2510
2511         return ERROR_OK;
2512 }
2513
2514 static int aice_open_device(struct aice_port_param_s *param)
2515 {
2516         if (ERROR_OK != aice_usb_open(param))
2517                 return ERROR_FAIL;
2518
2519         if (ERROR_FAIL == aice_get_version_info()) {
2520                 LOG_ERROR("Cannot get AICE version!");
2521                 return ERROR_FAIL;
2522         }
2523
2524         LOG_INFO("AICE initialization started");
2525
2526         /* attempt to reset Andes EDM */
2527         if (ERROR_FAIL == aice_reset_box()) {
2528                 LOG_ERROR("Cannot initial AICE box!");
2529                 return ERROR_FAIL;
2530         }
2531
2532         return ERROR_OK;
2533 }
2534
2535 static int aice_usb_set_jtag_clock(uint32_t a_clock)
2536 {
2537         jtag_clock = a_clock;
2538
2539         if (ERROR_OK != aice_usb_set_clock(a_clock)) {
2540                 LOG_ERROR("Cannot set AICE JTAG clock!");
2541                 return ERROR_FAIL;
2542         }
2543
2544         return ERROR_OK;
2545 }
2546
2547 static int aice_usb_close(void)
2548 {
2549         jtag_libusb_close(aice_handler.usb_handle);
2550
2551         if (custom_srst_script)
2552                 free(custom_srst_script);
2553
2554         if (custom_trst_script)
2555                 free(custom_trst_script);
2556
2557         if (custom_restart_script)
2558                 free(custom_restart_script);
2559
2560         return ERROR_OK;
2561 }
2562
2563 static int aice_core_init(uint32_t coreid)
2564 {
2565         core_info[coreid].access_channel = NDS_MEMORY_ACC_CPU;
2566         core_info[coreid].memory_select = NDS_MEMORY_SELECT_AUTO;
2567         core_info[coreid].core_state = AICE_TARGET_UNKNOWN;
2568
2569         return ERROR_OK;
2570 }
2571
2572 static int aice_usb_idcode(uint32_t *idcode, uint8_t *num_of_idcode)
2573 {
2574         int retval;
2575
2576         retval = aice_scan_chain(idcode, num_of_idcode);
2577         if (ERROR_OK == retval) {
2578                 for (int i = 0; i < *num_of_idcode; i++) {
2579                         aice_core_init(i);
2580                         aice_edm_init(i);
2581                 }
2582                 total_num_of_core = *num_of_idcode;
2583         }
2584
2585         return retval;
2586 }
2587
2588 static int aice_usb_halt(uint32_t coreid)
2589 {
2590         if (core_info[coreid].core_state == AICE_TARGET_HALTED) {
2591                 LOG_DEBUG("aice_usb_halt check halted");
2592                 return ERROR_OK;
2593         }
2594
2595         LOG_DEBUG("aice_usb_halt");
2596
2597         /** backup EDM registers */
2598         aice_backup_edm_registers(coreid);
2599         /** init EDM for host debugging */
2600         /** no need to clear dex_use_psw, because dbgi will clear it */
2601         aice_init_edm_registers(coreid, false);
2602
2603         /** Clear EDM_CTL.DBGIM & EDM_CTL.DBGACKM */
2604         uint32_t edm_ctl_value;
2605         aice_read_edmsr(coreid, NDS_EDM_SR_EDM_CTL, &edm_ctl_value);
2606         if (edm_ctl_value & 0x3)
2607                 aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL, edm_ctl_value & ~(0x3));
2608
2609         uint32_t dbger;
2610         uint32_t acc_ctl_value;
2611
2612         core_info[coreid].debug_under_dex_on = false;
2613         aice_read_misc(coreid, NDS_EDM_MISC_DBGER, &dbger);
2614
2615         if (dbger & NDS_DBGER_AT_MAX)
2616                 LOG_ERROR("<-- TARGET ERROR! Reaching the max interrupt stack level. -->");
2617
2618         if (dbger & NDS_DBGER_DEX) {
2619                 if (is_v2_edm(coreid) == false) {
2620                         /** debug 'debug mode'. use force_debug to issue dbgi */
2621                         aice_read_misc(coreid, NDS_EDM_MISC_ACC_CTL, &acc_ctl_value);
2622                         acc_ctl_value |= 0x8;
2623                         aice_write_misc(coreid, NDS_EDM_MISC_ACC_CTL, acc_ctl_value);
2624                         core_info[coreid].debug_under_dex_on = true;
2625
2626                         aice_write_misc(coreid, NDS_EDM_MISC_EDM_CMDR, 0);
2627                         /* If CPU stalled due to AT_MAX, clear AT_MAX status. */
2628                         if (dbger & NDS_DBGER_AT_MAX)
2629                                 aice_write_misc(coreid, NDS_EDM_MISC_DBGER, NDS_DBGER_AT_MAX);
2630                 }
2631         } else {
2632                 /** Issue DBGI normally */
2633                 aice_write_misc(coreid, NDS_EDM_MISC_EDM_CMDR, 0);
2634                 /* If CPU stalled due to AT_MAX, clear AT_MAX status. */
2635                 if (dbger & NDS_DBGER_AT_MAX)
2636                         aice_write_misc(coreid, NDS_EDM_MISC_DBGER, NDS_DBGER_AT_MAX);
2637         }
2638
2639         if (aice_check_dbger(coreid, NDS_DBGER_DEX) != ERROR_OK) {
2640                 LOG_ERROR("<-- TARGET ERROR! Unable to stop the debug target through DBGI. -->");
2641                 return ERROR_FAIL;
2642         }
2643
2644         if (core_info[coreid].debug_under_dex_on) {
2645                 if (core_info[coreid].dex_use_psw_on == false) {
2646                         /* under debug 'debug mode', force $psw to 'debug mode' bahavior */
2647                         /* !!!NOTICE!!! this is workaround for debug 'debug mode'.
2648                          * it is only for debugging 'debug exception handler' purpose.
2649                          * after openocd detaches from target, target behavior is
2650                          * undefined. */
2651                         uint32_t ir0_value;
2652                         uint32_t debug_mode_ir0_value;
2653                         aice_read_reg(coreid, IR0, &ir0_value);
2654                         debug_mode_ir0_value = ir0_value | 0x408; /* turn on DEX, set POM = 1 */
2655                         debug_mode_ir0_value &= ~(0x000000C1); /* turn off DT/IT/GIE */
2656                         aice_write_reg(coreid, IR0, debug_mode_ir0_value);
2657                 }
2658         }
2659
2660         /** set EDM_CTL.DBGIM & EDM_CTL.DBGACKM after halt */
2661         if (edm_ctl_value & 0x3)
2662                 aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL, edm_ctl_value);
2663
2664         /* backup r0 & r1 */
2665         aice_backup_tmp_registers(coreid);
2666         core_info[coreid].core_state = AICE_TARGET_HALTED;
2667
2668         return ERROR_OK;
2669 }
2670
2671 static int aice_usb_state(uint32_t coreid, enum aice_target_state_s *state)
2672 {
2673         uint32_t dbger_value;
2674         uint32_t ice_state;
2675
2676         int result = aice_read_misc(coreid, NDS_EDM_MISC_DBGER, &dbger_value);
2677
2678         if (ERROR_AICE_TIMEOUT == result) {
2679                 if (aice_read_ctrl(AICE_READ_CTRL_GET_ICE_STATE, &ice_state) != ERROR_OK) {
2680                         LOG_ERROR("<-- AICE ERROR! AICE is unplugged. -->");
2681                         return ERROR_FAIL;
2682                 }
2683
2684                 if ((ice_state & 0x20) == 0) {
2685                         LOG_ERROR("<-- TARGET ERROR! Target is disconnected with AICE. -->");
2686                         return ERROR_FAIL;
2687                 } else {
2688                         return ERROR_FAIL;
2689                 }
2690         } else if (ERROR_AICE_DISCONNECT == result) {
2691                 LOG_ERROR("<-- AICE ERROR! AICE is unplugged. -->");
2692                 return ERROR_FAIL;
2693         }
2694
2695         if ((dbger_value & NDS_DBGER_ILL_SEC_ACC) == NDS_DBGER_ILL_SEC_ACC) {
2696                 LOG_ERROR("<-- TARGET ERROR! Insufficient security privilege. -->");
2697
2698                 /* Clear ILL_SEC_ACC */
2699                 aice_write_misc(coreid, NDS_EDM_MISC_DBGER, NDS_DBGER_ILL_SEC_ACC);
2700
2701                 *state = AICE_TARGET_RUNNING;
2702                 core_info[coreid].core_state = AICE_TARGET_RUNNING;
2703         } else if ((dbger_value & NDS_DBGER_AT_MAX) == NDS_DBGER_AT_MAX) {
2704                 /* Issue DBGI to exit cpu stall */
2705                 aice_usb_halt(coreid);
2706
2707                 /* Read OIPC to find out the trigger point */
2708                 uint32_t ir11_value;
2709                 aice_read_reg(coreid, IR11, &ir11_value);
2710
2711                 LOG_ERROR("<-- TARGET ERROR! Reaching the max interrupt stack level; "
2712                                 "CPU is stalled at 0x%08" PRIx32 " for debugging. -->", ir11_value);
2713
2714                 *state = AICE_TARGET_HALTED;
2715         } else if ((dbger_value & NDS_DBGER_CRST) == NDS_DBGER_CRST) {
2716                 LOG_DEBUG("DBGER.CRST is on.");
2717
2718                 *state = AICE_TARGET_RESET;
2719                 core_info[coreid].core_state = AICE_TARGET_RUNNING;
2720
2721                 /* Clear CRST */
2722                 aice_write_misc(coreid, NDS_EDM_MISC_DBGER, NDS_DBGER_CRST);
2723         } else if ((dbger_value & NDS_DBGER_DEX) == NDS_DBGER_DEX) {
2724                 if (AICE_TARGET_RUNNING == core_info[coreid].core_state) {
2725                         /* enter debug mode, init EDM registers */
2726                         /* backup EDM registers */
2727                         aice_backup_edm_registers(coreid);
2728                         /* init EDM for host debugging */
2729                         aice_init_edm_registers(coreid, true);
2730                         aice_backup_tmp_registers(coreid);
2731                         core_info[coreid].core_state = AICE_TARGET_HALTED;
2732                 } else if (AICE_TARGET_UNKNOWN == core_info[coreid].core_state) {
2733                         /* debug 'debug mode', use force debug to halt core */
2734                         aice_usb_halt(coreid);
2735                 }
2736                 *state = AICE_TARGET_HALTED;
2737         } else {
2738                 *state = AICE_TARGET_RUNNING;
2739                 core_info[coreid].core_state = AICE_TARGET_RUNNING;
2740         }
2741
2742         return ERROR_OK;
2743 }
2744
2745 static int aice_usb_reset(void)
2746 {
2747         if (aice_reset_box() != ERROR_OK)
2748                 return ERROR_FAIL;
2749
2750         /* issue TRST */
2751         if (custom_trst_script == NULL) {
2752                 if (aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_CONTROL,
2753                                         AICE_JTAG_PIN_CONTROL_TRST) != ERROR_OK)
2754                         return ERROR_FAIL;
2755         } else {
2756                 /* custom trst operations */
2757                 if (aice_execute_custom_script(custom_trst_script) != ERROR_OK)
2758                         return ERROR_FAIL;
2759         }
2760
2761         if (aice_usb_set_clock(jtag_clock) != ERROR_OK)
2762                 return ERROR_FAIL;
2763
2764         return ERROR_OK;
2765 }
2766
2767 static int aice_issue_srst(uint32_t coreid)
2768 {
2769         LOG_DEBUG("aice_issue_srst");
2770
2771         /* After issuing srst, target will be running. So we need to restore EDM_CTL. */
2772         aice_restore_edm_registers(coreid);
2773
2774         if (custom_srst_script == NULL) {
2775                 if (aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_CONTROL,
2776                                         AICE_JTAG_PIN_CONTROL_SRST) != ERROR_OK)
2777                         return ERROR_FAIL;
2778         } else {
2779                 /* custom srst operations */
2780                 if (aice_execute_custom_script(custom_srst_script) != ERROR_OK)
2781                         return ERROR_FAIL;
2782         }
2783
2784         /* wait CRST infinitely */
2785         uint32_t dbger_value;
2786         int i = 0;
2787         while (1) {
2788                 if (aice_read_misc(coreid,
2789                                         NDS_EDM_MISC_DBGER, &dbger_value) != ERROR_OK)
2790                         return ERROR_FAIL;
2791
2792                 if (dbger_value & NDS_DBGER_CRST)
2793                         break;
2794
2795                 if ((i % 30) == 0)
2796                         keep_alive();
2797                 i++;
2798         }
2799
2800         core_info[coreid].host_dtr_valid = false;
2801         core_info[coreid].target_dtr_valid = false;
2802
2803         core_info[coreid].core_state = AICE_TARGET_RUNNING;
2804         return ERROR_OK;
2805 }
2806
2807 static int aice_issue_reset_hold(uint32_t coreid)
2808 {
2809         LOG_DEBUG("aice_issue_reset_hold");
2810
2811         /* set no_dbgi_pin to 0 */
2812         uint32_t pin_status;
2813         aice_read_ctrl(AICE_READ_CTRL_GET_JTAG_PIN_STATUS, &pin_status);
2814         if (pin_status | 0x4)
2815                 aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_STATUS, pin_status & (~0x4));
2816
2817         /* issue restart */
2818         if (custom_restart_script == NULL) {
2819                 if (aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_CONTROL,
2820                                         AICE_JTAG_PIN_CONTROL_RESTART) != ERROR_OK)
2821                         return ERROR_FAIL;
2822         } else {
2823                 /* custom restart operations */
2824                 if (aice_execute_custom_script(custom_restart_script) != ERROR_OK)
2825                         return ERROR_FAIL;
2826         }
2827
2828         if (aice_check_dbger(coreid, NDS_DBGER_CRST | NDS_DBGER_DEX) == ERROR_OK) {
2829                 aice_backup_tmp_registers(coreid);
2830                 core_info[coreid].core_state = AICE_TARGET_HALTED;
2831
2832                 return ERROR_OK;
2833         } else {
2834                 /* set no_dbgi_pin to 1 */
2835                 aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_STATUS, pin_status | 0x4);
2836
2837                 /* issue restart again */
2838                 if (custom_restart_script == NULL) {
2839                         if (aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_CONTROL,
2840                                                 AICE_JTAG_PIN_CONTROL_RESTART) != ERROR_OK)
2841                                 return ERROR_FAIL;
2842                 } else {
2843                         /* custom restart operations */
2844                         if (aice_execute_custom_script(custom_restart_script) != ERROR_OK)
2845                                 return ERROR_FAIL;
2846                 }
2847
2848                 if (aice_check_dbger(coreid, NDS_DBGER_CRST | NDS_DBGER_DEX) == ERROR_OK) {
2849                         aice_backup_tmp_registers(coreid);
2850                         core_info[coreid].core_state = AICE_TARGET_HALTED;
2851
2852                         return ERROR_OK;
2853                 }
2854
2855                 /* do software reset-and-hold */
2856                 aice_issue_srst(coreid);
2857                 aice_usb_halt(coreid);
2858
2859                 uint32_t value_ir3;
2860                 aice_read_reg(coreid, IR3, &value_ir3);
2861                 aice_write_reg(coreid, PC, value_ir3 & 0xFFFF0000);
2862         }
2863
2864         return ERROR_FAIL;
2865 }
2866
2867 static int aice_issue_reset_hold_multi(void)
2868 {
2869         uint32_t write_ctrl_value = 0;
2870
2871         /* set SRST */
2872         write_ctrl_value = AICE_CUSTOM_DELAY_SET_SRST;
2873         write_ctrl_value |= (0x200 << 16);
2874         if (aice_write_ctrl(AICE_WRITE_CTRL_CUSTOM_DELAY,
2875                                 write_ctrl_value) != ERROR_OK)
2876                 return ERROR_FAIL;
2877
2878         for (uint8_t i = 0 ; i < total_num_of_core ; i++)
2879                 aice_write_misc(i, NDS_EDM_MISC_EDM_CMDR, 0);
2880
2881         /* clear SRST */
2882         write_ctrl_value = AICE_CUSTOM_DELAY_CLEAN_SRST;
2883         write_ctrl_value |= (0x200 << 16);
2884         if (aice_write_ctrl(AICE_WRITE_CTRL_CUSTOM_DELAY,
2885                                 write_ctrl_value) != ERROR_OK)
2886                 return ERROR_FAIL;
2887
2888         for (uint8_t i = 0; i < total_num_of_core; i++)
2889                 aice_edm_init(i);
2890
2891         return ERROR_FAIL;
2892 }
2893
2894 static int aice_usb_assert_srst(uint32_t coreid, enum aice_srst_type_s srst)
2895 {
2896         if ((AICE_SRST != srst) && (AICE_RESET_HOLD != srst))
2897                 return ERROR_FAIL;
2898
2899         /* clear DBGER */
2900         if (aice_write_misc(coreid, NDS_EDM_MISC_DBGER,
2901                                 NDS_DBGER_CLEAR_ALL) != ERROR_OK)
2902                 return ERROR_FAIL;
2903
2904         int result = ERROR_OK;
2905         if (AICE_SRST == srst)
2906                 result = aice_issue_srst(coreid);
2907         else {
2908                 if (1 == total_num_of_core)
2909                         result = aice_issue_reset_hold(coreid);
2910                 else
2911                         result = aice_issue_reset_hold_multi();
2912         }
2913
2914         /* Clear DBGER.CRST after reset to avoid 'core-reset checking' errors.
2915          * assert_srst is user-intentional reset behavior, so we could
2916          * clear DBGER.CRST safely.
2917          */
2918         if (aice_write_misc(coreid,
2919                                 NDS_EDM_MISC_DBGER, NDS_DBGER_CRST) != ERROR_OK)
2920                 return ERROR_FAIL;
2921
2922         return result;
2923 }
2924
2925 static int aice_usb_run(uint32_t coreid)
2926 {
2927         LOG_DEBUG("aice_usb_run");
2928
2929         uint32_t dbger_value;
2930         if (aice_read_misc(coreid,
2931                                 NDS_EDM_MISC_DBGER, &dbger_value) != ERROR_OK)
2932                 return ERROR_FAIL;
2933
2934         if ((dbger_value & NDS_DBGER_DEX) != NDS_DBGER_DEX) {
2935                 LOG_WARNING("<-- TARGET WARNING! The debug target exited "
2936                                 "the debug mode unexpectedly. -->");
2937                 return ERROR_FAIL;
2938         }
2939
2940         /* restore r0 & r1 before free run */
2941         aice_restore_tmp_registers(coreid);
2942         core_info[coreid].core_state = AICE_TARGET_RUNNING;
2943
2944         /* clear DBGER */
2945         aice_write_misc(coreid, NDS_EDM_MISC_DBGER,
2946                         NDS_DBGER_CLEAR_ALL);
2947
2948         /** restore EDM registers */
2949         /** OpenOCD should restore EDM_CTL **before** to exit debug state.
2950          *  Otherwise, following instruction will read wrong EDM_CTL value.
2951          *
2952          *  pc -> mfsr $p0, EDM_CTL (single step)
2953          *        slli $p0, $p0, 1
2954          *        slri $p0, $p0, 31
2955          */
2956         aice_restore_edm_registers(coreid);
2957
2958         /** execute instructions in DIM */
2959         uint32_t instructions[4] = {
2960                 NOP,
2961                 NOP,
2962                 NOP,
2963                 IRET
2964         };
2965         int result = aice_execute_dim(coreid, instructions, 4);
2966
2967         return result;
2968 }
2969
2970 static int aice_usb_step(uint32_t coreid)
2971 {
2972         LOG_DEBUG("aice_usb_step");
2973
2974         uint32_t ir0_value;
2975         uint32_t ir0_reg_num;
2976
2977         if (is_v2_edm(coreid) == true)
2978                 /* V2 EDM will push interrupt stack as debug exception */
2979                 ir0_reg_num = IR1;
2980         else
2981                 ir0_reg_num = IR0;
2982
2983         /** enable HSS */
2984         aice_read_reg(coreid, ir0_reg_num, &ir0_value);
2985         if ((ir0_value & 0x800) == 0) {
2986                 /** set PSW.HSS */
2987                 ir0_value |= (0x01 << 11);
2988                 aice_write_reg(coreid, ir0_reg_num, ir0_value);
2989         }
2990
2991         if (ERROR_FAIL == aice_usb_run(coreid))
2992                 return ERROR_FAIL;
2993
2994         int i = 0;
2995         enum aice_target_state_s state;
2996         while (1) {
2997                 /* read DBGER */
2998                 if (aice_usb_state(coreid, &state) != ERROR_OK)
2999                         return ERROR_FAIL;
3000
3001                 if (AICE_TARGET_HALTED == state)
3002                         break;
3003
3004                 long long then = 0;
3005                 if (i == 30)
3006                         then = timeval_ms();
3007
3008                 if (i >= 30) {
3009                         if ((timeval_ms() - then) > 1000)
3010                                 LOG_WARNING("Timeout (1000ms) waiting for halt to complete");
3011
3012                         return ERROR_FAIL;
3013                 }
3014                 i++;
3015         }
3016
3017         /** disable HSS */
3018         aice_read_reg(coreid, ir0_reg_num, &ir0_value);
3019         ir0_value &= ~(0x01 << 11);
3020         aice_write_reg(coreid, ir0_reg_num, ir0_value);
3021
3022         return ERROR_OK;
3023 }
3024
3025 static int aice_usb_read_mem_b_bus(uint32_t coreid, uint32_t address, uint32_t *data)
3026 {
3027         return aice_read_mem_b(coreid, address, data);
3028 }
3029
3030 static int aice_usb_read_mem_h_bus(uint32_t coreid, uint32_t address, uint32_t *data)
3031 {
3032         return aice_read_mem_h(coreid, address, data);
3033 }
3034
3035 static int aice_usb_read_mem_w_bus(uint32_t coreid, uint32_t address, uint32_t *data)
3036 {
3037         return aice_read_mem(coreid, address, data);
3038 }
3039
3040 static int aice_usb_read_mem_b_dim(uint32_t coreid, uint32_t address, uint32_t *data)
3041 {
3042         uint32_t value;
3043         uint32_t instructions[4] = {
3044                 LBI_BI(R1, R0),
3045                 MTSR_DTR(R1),
3046                 DSB,
3047                 BEQ_MINUS_12
3048         };
3049
3050         aice_execute_dim(coreid, instructions, 4);
3051
3052         aice_read_dtr(coreid, &value);
3053         *data = value & 0xFF;
3054
3055         return ERROR_OK;
3056 }
3057
3058 static int aice_usb_read_mem_h_dim(uint32_t coreid, uint32_t address, uint32_t *data)
3059 {
3060         uint32_t value;
3061         uint32_t instructions[4] = {
3062                 LHI_BI(R1, R0),
3063                 MTSR_DTR(R1),
3064                 DSB,
3065                 BEQ_MINUS_12
3066         };
3067
3068         aice_execute_dim(coreid, instructions, 4);
3069
3070         aice_read_dtr(coreid, &value);
3071         *data = value & 0xFFFF;
3072
3073         return ERROR_OK;
3074 }
3075
3076 static int aice_usb_read_mem_w_dim(uint32_t coreid, uint32_t address, uint32_t *data)
3077 {
3078         uint32_t instructions[4] = {
3079                 LWI_BI(R1, R0),
3080                 MTSR_DTR(R1),
3081                 DSB,
3082                 BEQ_MINUS_12
3083         };
3084
3085         aice_execute_dim(coreid, instructions, 4);
3086
3087         aice_read_dtr(coreid, data);
3088
3089         return ERROR_OK;
3090 }
3091
3092 static int aice_usb_set_address_dim(uint32_t coreid, uint32_t address)
3093 {
3094         uint32_t instructions[4] = {
3095                 SETHI(R0, address >> 12),
3096                 ORI(R0, R0, address & 0x00000FFF),
3097                 NOP,
3098                 BEQ_MINUS_12
3099         };
3100
3101         return aice_execute_dim(coreid, instructions, 4);
3102 }
3103
3104 static int aice_usb_read_memory_unit(uint32_t coreid, uint32_t addr, uint32_t size,
3105                 uint32_t count, uint8_t *buffer)
3106 {
3107         LOG_DEBUG("aice_usb_read_memory_unit, addr: 0x%08" PRIx32
3108                         ", size: %" PRIu32 ", count: %" PRIu32 "",
3109                         addr, size, count);
3110
3111         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3112                 aice_usb_set_address_dim(coreid, addr);
3113
3114         uint32_t value;
3115         size_t i;
3116         read_mem_func_t read_mem_func;
3117
3118         switch (size) {
3119                 case 1:
3120                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3121                                 read_mem_func = aice_usb_read_mem_b_bus;
3122                         else
3123                                 read_mem_func = aice_usb_read_mem_b_dim;
3124
3125                         for (i = 0; i < count; i++) {
3126                                 read_mem_func(coreid, addr, &value);
3127                                 *buffer++ = (uint8_t)value;
3128                                 addr++;
3129                         }
3130                         break;
3131                 case 2:
3132                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3133                                 read_mem_func = aice_usb_read_mem_h_bus;
3134                         else
3135                                 read_mem_func = aice_usb_read_mem_h_dim;
3136
3137                         for (i = 0; i < count; i++) {
3138                                 read_mem_func(coreid, addr, &value);
3139                                 uint16_t svalue = value;
3140                                 memcpy(buffer, &svalue, sizeof(uint16_t));
3141                                 buffer += 2;
3142                                 addr += 2;
3143                         }
3144                         break;
3145                 case 4:
3146                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3147                                 read_mem_func = aice_usb_read_mem_w_bus;
3148                         else
3149                                 read_mem_func = aice_usb_read_mem_w_dim;
3150
3151                         for (i = 0; i < count; i++) {
3152                                 read_mem_func(coreid, addr, &value);
3153                                 memcpy(buffer, &value, sizeof(uint32_t));
3154                                 buffer += 4;
3155                                 addr += 4;
3156                         }
3157                         break;
3158         }
3159
3160         return ERROR_OK;
3161 }
3162
3163 static int aice_usb_write_mem_b_bus(uint32_t coreid, uint32_t address, uint32_t data)
3164 {
3165         return aice_write_mem_b(coreid, address, data);
3166 }
3167
3168 static int aice_usb_write_mem_h_bus(uint32_t coreid, uint32_t address, uint32_t data)
3169 {
3170         return aice_write_mem_h(coreid, address, data);
3171 }
3172
3173 static int aice_usb_write_mem_w_bus(uint32_t coreid, uint32_t address, uint32_t data)
3174 {
3175         return aice_write_mem(coreid, address, data);
3176 }
3177
3178 static int aice_usb_write_mem_b_dim(uint32_t coreid, uint32_t address, uint32_t data)
3179 {
3180         uint32_t instructions[4] = {
3181                 MFSR_DTR(R1),
3182                 SBI_BI(R1, R0),
3183                 DSB,
3184                 BEQ_MINUS_12
3185         };
3186
3187         aice_write_dtr(coreid, data & 0xFF);
3188         aice_execute_dim(coreid, instructions, 4);
3189
3190         return ERROR_OK;
3191 }
3192
3193 static int aice_usb_write_mem_h_dim(uint32_t coreid, uint32_t address, uint32_t data)
3194 {
3195         uint32_t instructions[4] = {
3196                 MFSR_DTR(R1),
3197                 SHI_BI(R1, R0),
3198                 DSB,
3199                 BEQ_MINUS_12
3200         };
3201
3202         aice_write_dtr(coreid, data & 0xFFFF);
3203         aice_execute_dim(coreid, instructions, 4);
3204
3205         return ERROR_OK;
3206 }
3207
3208 static int aice_usb_write_mem_w_dim(uint32_t coreid, uint32_t address, uint32_t data)
3209 {
3210         uint32_t instructions[4] = {
3211                 MFSR_DTR(R1),
3212                 SWI_BI(R1, R0),
3213                 DSB,
3214                 BEQ_MINUS_12
3215         };
3216
3217         aice_write_dtr(coreid, data);
3218         aice_execute_dim(coreid, instructions, 4);
3219
3220         return ERROR_OK;
3221 }
3222
3223 static int aice_usb_write_memory_unit(uint32_t coreid, uint32_t addr, uint32_t size,
3224                 uint32_t count, const uint8_t *buffer)
3225 {
3226         LOG_DEBUG("aice_usb_write_memory_unit, addr: 0x%08" PRIx32
3227                         ", size: %" PRIu32 ", count: %" PRIu32 "",
3228                         addr, size, count);
3229
3230         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3231                 aice_usb_set_address_dim(coreid, addr);
3232
3233         size_t i;
3234         write_mem_func_t write_mem_func;
3235
3236         switch (size) {
3237                 case 1:
3238                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3239                                 write_mem_func = aice_usb_write_mem_b_bus;
3240                         else
3241                                 write_mem_func = aice_usb_write_mem_b_dim;
3242
3243                         for (i = 0; i < count; i++) {
3244                                 write_mem_func(coreid, addr, *buffer);
3245                                 buffer++;
3246                                 addr++;
3247                         }
3248                         break;
3249                 case 2:
3250                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3251                                 write_mem_func = aice_usb_write_mem_h_bus;
3252                         else
3253                                 write_mem_func = aice_usb_write_mem_h_dim;
3254
3255                         for (i = 0; i < count; i++) {
3256                                 uint16_t value;
3257                                 memcpy(&value, buffer, sizeof(uint16_t));
3258
3259                                 write_mem_func(coreid, addr, value);
3260                                 buffer += 2;
3261                                 addr += 2;
3262                         }
3263                         break;
3264                 case 4:
3265                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3266                                 write_mem_func = aice_usb_write_mem_w_bus;
3267                         else
3268                                 write_mem_func = aice_usb_write_mem_w_dim;
3269
3270                         for (i = 0; i < count; i++) {
3271                                 uint32_t value;
3272                                 memcpy(&value, buffer, sizeof(uint32_t));
3273
3274                                 write_mem_func(coreid, addr, value);
3275                                 buffer += 4;
3276                                 addr += 4;
3277                         }
3278                         break;
3279         }
3280
3281         return ERROR_OK;
3282 }
3283
3284 static int aice_bulk_read_mem(uint32_t coreid, uint32_t addr, uint32_t count,
3285                 uint8_t *buffer)
3286 {
3287         uint32_t packet_size;
3288
3289         while (count > 0) {
3290                 packet_size = (count >= 0x100) ? 0x100 : count;
3291
3292                 /** set address */
3293                 addr &= 0xFFFFFFFC;
3294                 if (aice_write_misc(coreid, NDS_EDM_MISC_SBAR, addr) != ERROR_OK)
3295                         return ERROR_FAIL;
3296
3297                 if (aice_fastread_mem(coreid, buffer,
3298                                         packet_size) != ERROR_OK)
3299                         return ERROR_FAIL;
3300
3301                 buffer += (packet_size * 4);
3302                 addr += (packet_size * 4);
3303                 count -= packet_size;
3304         }
3305
3306         return ERROR_OK;
3307 }
3308
3309 static int aice_bulk_write_mem(uint32_t coreid, uint32_t addr, uint32_t count,
3310                 const uint8_t *buffer)
3311 {
3312         uint32_t packet_size;
3313
3314         while (count > 0) {
3315                 packet_size = (count >= 0x100) ? 0x100 : count;
3316
3317                 /** set address */
3318                 addr &= 0xFFFFFFFC;
3319                 if (aice_write_misc(coreid, NDS_EDM_MISC_SBAR, addr | 1) != ERROR_OK)
3320                         return ERROR_FAIL;
3321
3322                 if (aice_fastwrite_mem(coreid, buffer,
3323                                         packet_size) != ERROR_OK)
3324                         return ERROR_FAIL;
3325
3326                 buffer += (packet_size * 4);
3327                 addr += (packet_size * 4);
3328                 count -= packet_size;
3329         }
3330
3331         return ERROR_OK;
3332 }
3333
3334 static int aice_usb_bulk_read_mem(uint32_t coreid, uint32_t addr,
3335                 uint32_t length, uint8_t *buffer)
3336 {
3337         LOG_DEBUG("aice_usb_bulk_read_mem, addr: 0x%08" PRIx32 ", length: 0x%08" PRIx32, addr, length);
3338
3339         int retval;
3340
3341         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3342                 aice_usb_set_address_dim(coreid, addr);
3343
3344         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3345                 retval = aice_usb_read_memory_unit(coreid, addr, 4, length / 4, buffer);
3346         else
3347                 retval = aice_bulk_read_mem(coreid, addr, length / 4, buffer);
3348
3349         return retval;
3350 }
3351
3352 static int aice_usb_bulk_write_mem(uint32_t coreid, uint32_t addr,
3353                 uint32_t length, const uint8_t *buffer)
3354 {
3355         LOG_DEBUG("aice_usb_bulk_write_mem, addr: 0x%08" PRIx32 ", length: 0x%08" PRIx32, addr, length);
3356
3357         int retval;
3358
3359         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3360                 aice_usb_set_address_dim(coreid, addr);
3361
3362         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3363                 retval = aice_usb_write_memory_unit(coreid, addr, 4, length / 4, buffer);
3364         else
3365                 retval = aice_bulk_write_mem(coreid, addr, length / 4, buffer);
3366
3367         return retval;
3368 }
3369
3370 static int aice_usb_read_debug_reg(uint32_t coreid, uint32_t addr, uint32_t *val)
3371 {
3372         if (AICE_TARGET_HALTED == core_info[coreid].core_state) {
3373                 if (NDS_EDM_SR_EDMSW == addr) {
3374                         *val = core_info[coreid].edmsw_backup;
3375                 } else if (NDS_EDM_SR_EDM_DTR == addr) {
3376                         if (core_info[coreid].target_dtr_valid) {
3377                                 /* if EDM_DTR has read out, clear it. */
3378                                 *val = core_info[coreid].target_dtr_backup;
3379                                 core_info[coreid].edmsw_backup &= (~0x1);
3380                                 core_info[coreid].target_dtr_valid = false;
3381                         } else {
3382                                 *val = 0;
3383                         }
3384                 }
3385         }
3386
3387         return aice_read_edmsr(coreid, addr, val);
3388 }
3389
3390 static int aice_usb_write_debug_reg(uint32_t coreid, uint32_t addr, const uint32_t val)
3391 {
3392         if (AICE_TARGET_HALTED == core_info[coreid].core_state) {
3393                 if (NDS_EDM_SR_EDM_DTR == addr) {
3394                         core_info[coreid].host_dtr_backup = val;
3395                         core_info[coreid].edmsw_backup |= 0x2;
3396                         core_info[coreid].host_dtr_valid = true;
3397                 }
3398         }
3399
3400         return aice_write_edmsr(coreid, addr, val);
3401 }
3402
3403 static int aice_usb_memory_access(uint32_t coreid, enum nds_memory_access channel)
3404 {
3405         LOG_DEBUG("aice_usb_memory_access, access channel: %u", channel);
3406
3407         core_info[coreid].access_channel = channel;
3408
3409         return ERROR_OK;
3410 }
3411
3412 static int aice_usb_memory_mode(uint32_t coreid, enum nds_memory_select mem_select)
3413 {
3414         if (core_info[coreid].memory_select == mem_select)
3415                 return ERROR_OK;
3416
3417         LOG_DEBUG("aice_usb_memory_mode, memory select: %u", mem_select);
3418
3419         core_info[coreid].memory_select = mem_select;
3420
3421         if (NDS_MEMORY_SELECT_AUTO != core_info[coreid].memory_select)
3422                 aice_write_misc(coreid, NDS_EDM_MISC_ACC_CTL,
3423                                 core_info[coreid].memory_select - 1);
3424         else
3425                 aice_write_misc(coreid, NDS_EDM_MISC_ACC_CTL,
3426                                 NDS_MEMORY_SELECT_MEM - 1);
3427
3428         return ERROR_OK;
3429 }
3430
3431 static int aice_usb_read_tlb(uint32_t coreid, uint32_t virtual_address,
3432                 uint32_t *physical_address)
3433 {
3434         LOG_DEBUG("aice_usb_read_tlb, virtual address: 0x%08" PRIx32, virtual_address);
3435
3436         uint32_t instructions[4];
3437         uint32_t probe_result;
3438         uint32_t value_mr3;
3439         uint32_t value_mr4;
3440         uint32_t access_page_size;
3441         uint32_t virtual_offset;
3442         uint32_t physical_page_number;
3443
3444         aice_write_dtr(coreid, virtual_address);
3445
3446         /* probe TLB first */
3447         instructions[0] = MFSR_DTR(R0);
3448         instructions[1] = TLBOP_TARGET_PROBE(R1, R0);
3449         instructions[2] = DSB;
3450         instructions[3] = BEQ_MINUS_12;
3451         aice_execute_dim(coreid, instructions, 4);
3452
3453         aice_read_reg(coreid, R1, &probe_result);
3454
3455         if (probe_result & 0x80000000)
3456                 return ERROR_FAIL;
3457
3458         /* read TLB entry */
3459         aice_write_dtr(coreid, probe_result & 0x7FF);
3460
3461         /* probe TLB first */
3462         instructions[0] = MFSR_DTR(R0);
3463         instructions[1] = TLBOP_TARGET_READ(R0);
3464         instructions[2] = DSB;
3465         instructions[3] = BEQ_MINUS_12;
3466         aice_execute_dim(coreid, instructions, 4);
3467
3468         /* TODO: it should backup mr3, mr4 */
3469         aice_read_reg(coreid, MR3, &value_mr3);
3470         aice_read_reg(coreid, MR4, &value_mr4);
3471
3472         access_page_size = value_mr4 & 0xF;
3473         if (0 == access_page_size) { /* 4K page */
3474                 virtual_offset = virtual_address & 0x00000FFF;
3475                 physical_page_number = value_mr3 & 0xFFFFF000;
3476         } else if (1 == access_page_size) { /* 8K page */
3477                 virtual_offset = virtual_address & 0x00001FFF;
3478                 physical_page_number = value_mr3 & 0xFFFFE000;
3479         } else if (5 == access_page_size) { /* 1M page */
3480                 virtual_offset = virtual_address & 0x000FFFFF;
3481                 physical_page_number = value_mr3 & 0xFFF00000;
3482         } else {
3483                 return ERROR_FAIL;
3484         }
3485
3486         *physical_address = physical_page_number | virtual_offset;
3487
3488         return ERROR_OK;
3489 }
3490
3491 static int aice_usb_init_cache(uint32_t coreid)
3492 {
3493         LOG_DEBUG("aice_usb_init_cache");
3494
3495         uint32_t value_cr1;
3496         uint32_t value_cr2;
3497
3498         aice_read_reg(coreid, CR1, &value_cr1);
3499         aice_read_reg(coreid, CR2, &value_cr2);
3500
3501         struct cache_info *icache = &core_info[coreid].icache;
3502
3503         icache->set = value_cr1 & 0x7;
3504         icache->log2_set = icache->set + 6;
3505         icache->set = 64 << icache->set;
3506         icache->way = ((value_cr1 >> 3) & 0x7) + 1;
3507         icache->line_size = (value_cr1 >> 6) & 0x7;
3508         if (icache->line_size != 0) {
3509                 icache->log2_line_size = icache->line_size + 2;
3510                 icache->line_size = 8 << (icache->line_size - 1);
3511         } else {
3512                 icache->log2_line_size = 0;
3513         }
3514
3515         LOG_DEBUG("\ticache set: %" PRIu32 ", way: %" PRIu32 ", line size: %" PRIu32 ", "
3516                         "log2(set): %" PRIu32 ", log2(line_size): %" PRIu32 "",
3517                         icache->set, icache->way, icache->line_size,
3518                         icache->log2_set, icache->log2_line_size);
3519
3520         struct cache_info *dcache = &core_info[coreid].dcache;
3521
3522         dcache->set = value_cr2 & 0x7;
3523         dcache->log2_set = dcache->set + 6;
3524         dcache->set = 64 << dcache->set;
3525         dcache->way = ((value_cr2 >> 3) & 0x7) + 1;
3526         dcache->line_size = (value_cr2 >> 6) & 0x7;
3527         if (dcache->line_size != 0) {
3528                 dcache->log2_line_size = dcache->line_size + 2;
3529                 dcache->line_size = 8 << (dcache->line_size - 1);
3530         } else {
3531                 dcache->log2_line_size = 0;
3532         }
3533
3534         LOG_DEBUG("\tdcache set: %" PRIu32 ", way: %" PRIu32 ", line size: %" PRIu32 ", "
3535                         "log2(set): %" PRIu32 ", log2(line_size): %" PRIu32 "",
3536                         dcache->set, dcache->way, dcache->line_size,
3537                         dcache->log2_set, dcache->log2_line_size);
3538
3539         core_info[coreid].cache_init = true;
3540
3541         return ERROR_OK;
3542 }
3543
3544 static int aice_usb_dcache_inval_all(uint32_t coreid)
3545 {
3546         LOG_DEBUG("aice_usb_dcache_inval_all");
3547
3548         uint32_t set_index;
3549         uint32_t way_index;
3550         uint32_t cache_index;
3551         uint32_t instructions[4];
3552
3553         instructions[0] = MFSR_DTR(R0);
3554         instructions[1] = L1D_IX_INVAL(R0);
3555         instructions[2] = DSB;
3556         instructions[3] = BEQ_MINUS_12;
3557
3558         struct cache_info *dcache = &core_info[coreid].dcache;
3559
3560         for (set_index = 0; set_index < dcache->set; set_index++) {
3561                 for (way_index = 0; way_index < dcache->way; way_index++) {
3562                         cache_index = (way_index << (dcache->log2_set + dcache->log2_line_size)) |
3563                                 (set_index << dcache->log2_line_size);
3564
3565                         if (ERROR_OK != aice_write_dtr(coreid, cache_index))
3566                                 return ERROR_FAIL;
3567
3568                         if (ERROR_OK != aice_execute_dim(coreid, instructions, 4))
3569                                 return ERROR_FAIL;
3570                 }
3571         }
3572
3573         return ERROR_OK;
3574 }
3575
3576 static int aice_usb_dcache_va_inval(uint32_t coreid, uint32_t address)
3577 {
3578         LOG_DEBUG("aice_usb_dcache_va_inval");
3579
3580         uint32_t instructions[4];
3581
3582         aice_write_dtr(coreid, address);
3583
3584         instructions[0] = MFSR_DTR(R0);
3585         instructions[1] = L1D_VA_INVAL(R0);
3586         instructions[2] = DSB;
3587         instructions[3] = BEQ_MINUS_12;
3588
3589         return aice_execute_dim(coreid, instructions, 4);
3590 }
3591
3592 static int aice_usb_dcache_wb_all(uint32_t coreid)
3593 {
3594         LOG_DEBUG("aice_usb_dcache_wb_all");
3595
3596         uint32_t set_index;
3597         uint32_t way_index;
3598         uint32_t cache_index;
3599         uint32_t instructions[4];
3600
3601         instructions[0] = MFSR_DTR(R0);
3602         instructions[1] = L1D_IX_WB(R0);
3603         instructions[2] = DSB;
3604         instructions[3] = BEQ_MINUS_12;
3605
3606         struct cache_info *dcache = &core_info[coreid].dcache;
3607
3608         for (set_index = 0; set_index < dcache->set; set_index++) {
3609                 for (way_index = 0; way_index < dcache->way; way_index++) {
3610                         cache_index = (way_index << (dcache->log2_set + dcache->log2_line_size)) |
3611                                 (set_index << dcache->log2_line_size);
3612
3613                         if (ERROR_OK != aice_write_dtr(coreid, cache_index))
3614                                 return ERROR_FAIL;
3615
3616                         if (ERROR_OK != aice_execute_dim(coreid, instructions, 4))
3617                                 return ERROR_FAIL;
3618                 }
3619         }
3620
3621         return ERROR_OK;
3622 }
3623
3624 static int aice_usb_dcache_va_wb(uint32_t coreid, uint32_t address)
3625 {
3626         LOG_DEBUG("aice_usb_dcache_va_wb");
3627
3628         uint32_t instructions[4];
3629
3630         aice_write_dtr(coreid, address);
3631
3632         instructions[0] = MFSR_DTR(R0);
3633         instructions[1] = L1D_VA_WB(R0);
3634         instructions[2] = DSB;
3635         instructions[3] = BEQ_MINUS_12;
3636
3637         return aice_execute_dim(coreid, instructions, 4);
3638 }
3639
3640 static int aice_usb_icache_inval_all(uint32_t coreid)
3641 {
3642         LOG_DEBUG("aice_usb_icache_inval_all");
3643
3644         uint32_t set_index;
3645         uint32_t way_index;
3646         uint32_t cache_index;
3647         uint32_t instructions[4];
3648
3649         instructions[0] = MFSR_DTR(R0);
3650         instructions[1] = L1I_IX_INVAL(R0);
3651         instructions[2] = ISB;
3652         instructions[3] = BEQ_MINUS_12;
3653
3654         struct cache_info *icache = &core_info[coreid].icache;
3655
3656         for (set_index = 0; set_index < icache->set; set_index++) {
3657                 for (way_index = 0; way_index < icache->way; way_index++) {
3658                         cache_index = (way_index << (icache->log2_set + icache->log2_line_size)) |
3659                                 (set_index << icache->log2_line_size);
3660
3661                         if (ERROR_OK != aice_write_dtr(coreid, cache_index))
3662                                 return ERROR_FAIL;
3663
3664                         if (ERROR_OK != aice_execute_dim(coreid, instructions, 4))
3665                                 return ERROR_FAIL;
3666                 }
3667         }
3668
3669         return ERROR_OK;
3670 }
3671
3672 static int aice_usb_icache_va_inval(uint32_t coreid, uint32_t address)
3673 {
3674         LOG_DEBUG("aice_usb_icache_va_inval");
3675
3676         uint32_t instructions[4];
3677
3678         aice_write_dtr(coreid, address);
3679
3680         instructions[0] = MFSR_DTR(R0);
3681         instructions[1] = L1I_VA_INVAL(R0);
3682         instructions[2] = ISB;
3683         instructions[3] = BEQ_MINUS_12;
3684
3685         return aice_execute_dim(coreid, instructions, 4);
3686 }
3687
3688 static int aice_usb_cache_ctl(uint32_t coreid, uint32_t subtype, uint32_t address)
3689 {
3690         LOG_DEBUG("aice_usb_cache_ctl");
3691
3692         int result;
3693
3694         if (core_info[coreid].cache_init == false)
3695                 aice_usb_init_cache(coreid);
3696
3697         switch (subtype) {
3698                 case AICE_CACHE_CTL_L1D_INVALALL:
3699                         result = aice_usb_dcache_inval_all(coreid);
3700                         break;
3701                 case AICE_CACHE_CTL_L1D_VA_INVAL:
3702                         result = aice_usb_dcache_va_inval(coreid, address);
3703                         break;
3704                 case AICE_CACHE_CTL_L1D_WBALL:
3705                         result = aice_usb_dcache_wb_all(coreid);
3706                         break;
3707                 case AICE_CACHE_CTL_L1D_VA_WB:
3708                         result = aice_usb_dcache_va_wb(coreid, address);
3709                         break;
3710                 case AICE_CACHE_CTL_L1I_INVALALL:
3711                         result = aice_usb_icache_inval_all(coreid);
3712                         break;
3713                 case AICE_CACHE_CTL_L1I_VA_INVAL:
3714                         result = aice_usb_icache_va_inval(coreid, address);
3715                         break;
3716                 default:
3717                         result = ERROR_FAIL;
3718                         break;
3719         }
3720
3721         return result;
3722 }
3723
3724 static int aice_usb_set_retry_times(uint32_t a_retry_times)
3725 {
3726         aice_max_retry_times = a_retry_times;
3727         return ERROR_OK;
3728 }
3729
3730 static int aice_usb_program_edm(uint32_t coreid, char *command_sequence)
3731 {
3732         char *command_str;
3733         char *reg_name_0;
3734         char *reg_name_1;
3735         uint32_t data_value;
3736         int i;
3737
3738         /* init strtok() */
3739         command_str = strtok(command_sequence, ";");
3740         if (command_str == NULL)
3741                 return ERROR_OK;
3742
3743         do {
3744                 i = 0;
3745                 /* process one command */
3746                 while (command_str[i] == ' ' ||
3747                                 command_str[i] == '\n' ||
3748                                 command_str[i] == '\r' ||
3749                                 command_str[i] == '\t')
3750                         i++;
3751
3752                 /* skip ' ', '\r', '\n', '\t' */
3753                 command_str = command_str + i;
3754
3755                 if (strncmp(command_str, "write_misc", 10) == 0) {
3756                         reg_name_0 = strstr(command_str, "gen_port0");
3757                         reg_name_1 = strstr(command_str, "gen_port1");
3758
3759                         if (reg_name_0 != NULL) {
3760                                 data_value = strtoul(reg_name_0 + 9, NULL, 0);
3761
3762                                 if (aice_write_misc(coreid,
3763                                                         NDS_EDM_MISC_GEN_PORT0, data_value) != ERROR_OK)
3764                                         return ERROR_FAIL;
3765
3766                         } else if (reg_name_1 != NULL) {
3767                                 data_value = strtoul(reg_name_1 + 9, NULL, 0);
3768
3769                                 if (aice_write_misc(coreid,
3770                                                         NDS_EDM_MISC_GEN_PORT1, data_value) != ERROR_OK)
3771                                         return ERROR_FAIL;
3772                         } else {
3773                                 LOG_ERROR("program EDM, unsupported misc register: %s", command_str);
3774                         }
3775                 } else {
3776                         LOG_ERROR("program EDM, unsupported command: %s", command_str);
3777                 }
3778
3779                 /* update command_str */
3780                 command_str = strtok(NULL, ";");
3781
3782         } while (command_str != NULL);
3783
3784         return ERROR_OK;
3785 }
3786
3787 static int aice_usb_set_command_mode(enum aice_command_mode command_mode)
3788 {
3789         int retval = ERROR_OK;
3790
3791         /* flush usb_packets_buffer as users change mode */
3792         retval = aice_usb_packet_flush();
3793
3794         if (AICE_COMMAND_MODE_BATCH == command_mode) {
3795                 /* reset batch buffer */
3796                 aice_command_mode = AICE_COMMAND_MODE_NORMAL;
3797                 retval = aice_write_ctrl(AICE_WRITE_CTRL_BATCH_CMD_BUF0_CTRL, 0x40000);
3798         }
3799
3800         aice_command_mode = command_mode;
3801
3802         return retval;
3803 }
3804
3805 static int aice_usb_execute(uint32_t coreid, uint32_t *instructions,
3806                 uint32_t instruction_num)
3807 {
3808         uint32_t i, j;
3809         uint8_t current_instruction_num;
3810         uint32_t dim_instructions[4] = {NOP, NOP, NOP, BEQ_MINUS_12};
3811
3812         /* To execute 4 instructions as a special case */
3813         if (instruction_num == 4)
3814                 return aice_execute_dim(coreid, instructions, 4);
3815
3816         for (i = 0 ; i < instruction_num ; i += 3) {
3817                 if (instruction_num - i < 3) {
3818                         current_instruction_num = instruction_num - i;
3819                         for (j = current_instruction_num ; j < 3 ; j++)
3820                                 dim_instructions[j] = NOP;
3821                 } else {
3822                         current_instruction_num = 3;
3823                 }
3824
3825                 memcpy(dim_instructions, instructions + i,
3826                                 current_instruction_num * sizeof(uint32_t));
3827
3828                 /** fill DIM */
3829                 if (aice_write_dim(coreid,
3830                                         dim_instructions,
3831                                         4) != ERROR_OK)
3832                         return ERROR_FAIL;
3833
3834                 /** clear DBGER.DPED */
3835                 if (aice_write_misc(coreid,
3836                                         NDS_EDM_MISC_DBGER, NDS_DBGER_DPED) != ERROR_OK)
3837                         return ERROR_FAIL;
3838
3839                 /** execute DIM */
3840                 if (aice_do_execute(coreid) != ERROR_OK)
3841                         return ERROR_FAIL;
3842
3843                 /** check DBGER.DPED */
3844                 if (aice_check_dbger(coreid, NDS_DBGER_DPED) != ERROR_OK) {
3845
3846                         LOG_ERROR("<-- TARGET ERROR! Debug operations do not finish properly:"
3847                                         "0x%08" PRIx32 " 0x%08" PRIx32 " 0x%08" PRIx32 " 0x%08" PRIx32 ". -->",
3848                                         dim_instructions[0],
3849                                         dim_instructions[1],
3850                                         dim_instructions[2],
3851                                         dim_instructions[3]);
3852                         return ERROR_FAIL;
3853                 }
3854         }
3855
3856         return ERROR_OK;
3857 }
3858
3859 static int aice_usb_set_custom_srst_script(const char *script)
3860 {
3861         custom_srst_script = strdup(script);
3862
3863         return ERROR_OK;
3864 }
3865
3866 static int aice_usb_set_custom_trst_script(const char *script)
3867 {
3868         custom_trst_script = strdup(script);
3869
3870         return ERROR_OK;
3871 }
3872
3873 static int aice_usb_set_custom_restart_script(const char *script)
3874 {
3875         custom_restart_script = strdup(script);
3876
3877         return ERROR_OK;
3878 }
3879
3880 static int aice_usb_set_count_to_check_dbger(uint32_t count_to_check)
3881 {
3882         aice_count_to_check_dbger = count_to_check;
3883
3884         return ERROR_OK;
3885 }
3886
3887 static int aice_usb_set_data_endian(uint32_t coreid,
3888                 enum aice_target_endian target_data_endian)
3889 {
3890         data_endian = target_data_endian;
3891
3892         return ERROR_OK;
3893 }
3894
3895 static int fill_profiling_batch_commands(uint32_t coreid, uint32_t reg_no)
3896 {
3897         uint32_t dim_instructions[4];
3898
3899         aice_usb_set_command_mode(AICE_COMMAND_MODE_BATCH);
3900
3901         /* halt */
3902         if (aice_write_misc(coreid, NDS_EDM_MISC_EDM_CMDR, 0) != ERROR_OK)
3903                 return ERROR_FAIL;
3904
3905         /* backup $r0 */
3906         dim_instructions[0] = MTSR_DTR(0);
3907         dim_instructions[1] = DSB;
3908         dim_instructions[2] = NOP;
3909         dim_instructions[3] = BEQ_MINUS_12;
3910         if (aice_write_dim(coreid, dim_instructions, 4) != ERROR_OK)
3911                 return ERROR_FAIL;
3912         aice_read_dtr_to_buffer(coreid, AICE_BATCH_DATA_BUFFER_0);
3913
3914         /* get samples */
3915         if (NDS32_REG_TYPE_GPR == nds32_reg_type(reg_no)) {
3916                 /* general registers */
3917                 dim_instructions[0] = MTSR_DTR(reg_no);
3918                 dim_instructions[1] = DSB;
3919                 dim_instructions[2] = NOP;
3920                 dim_instructions[3] = BEQ_MINUS_12;
3921         } else if (NDS32_REG_TYPE_SPR == nds32_reg_type(reg_no)) {
3922                 /* user special registers */
3923                 dim_instructions[0] = MFUSR_G0(0, nds32_reg_sr_index(reg_no));
3924                 dim_instructions[1] = MTSR_DTR(0);
3925                 dim_instructions[2] = DSB;
3926                 dim_instructions[3] = BEQ_MINUS_12;
3927         } else { /* system registers */
3928                 dim_instructions[0] = MFSR(0, nds32_reg_sr_index(reg_no));
3929                 dim_instructions[1] = MTSR_DTR(0);
3930                 dim_instructions[2] = DSB;
3931                 dim_instructions[3] = BEQ_MINUS_12;
3932         }
3933         if (aice_write_dim(coreid, dim_instructions, 4) != ERROR_OK)
3934                 return ERROR_FAIL;
3935         aice_read_dtr_to_buffer(coreid, AICE_BATCH_DATA_BUFFER_1);
3936
3937         /* restore $r0 */
3938         aice_write_dtr_from_buffer(coreid, AICE_BATCH_DATA_BUFFER_0);
3939         dim_instructions[0] = MFSR_DTR(0);
3940         dim_instructions[1] = DSB;
3941         dim_instructions[2] = NOP;
3942         dim_instructions[3] = IRET;  /* free run */
3943         if (aice_write_dim(coreid, dim_instructions, 4) != ERROR_OK)
3944                 return ERROR_FAIL;
3945
3946         aice_command_mode = AICE_COMMAND_MODE_NORMAL;
3947
3948         /* use BATCH_BUFFER_WRITE to fill command-batch-buffer */
3949         if (aice_batch_buffer_write(AICE_BATCH_COMMAND_BUFFER_0,
3950                                 usb_out_packets_buffer,
3951                                 (usb_out_packets_buffer_length + 3) / 4) != ERROR_OK)
3952                 return ERROR_FAIL;
3953
3954         usb_out_packets_buffer_length = 0;
3955         usb_in_packets_buffer_length = 0;
3956
3957         return ERROR_OK;
3958 }
3959
3960 static int aice_usb_profiling(uint32_t coreid, uint32_t interval, uint32_t iteration,
3961                 uint32_t reg_no, uint32_t *samples, uint32_t *num_samples)
3962 {
3963         uint32_t iteration_count;
3964         uint32_t this_iteration;
3965         int retval = ERROR_OK;
3966         const uint32_t MAX_ITERATION = 250;
3967
3968         *num_samples = 0;
3969
3970         /* init DIM size */
3971         if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_DIM_SIZE, 4) != ERROR_OK)
3972                 return ERROR_FAIL;
3973
3974         /* Use AICE_BATCH_DATA_BUFFER_0 to read/write $DTR.
3975          * Set it to circular buffer */
3976         if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_DATA_BUF0_CTRL, 0xC0000) != ERROR_OK)
3977                 return ERROR_FAIL;
3978
3979         fill_profiling_batch_commands(coreid, reg_no);
3980
3981         iteration_count = 0;
3982         while (iteration_count < iteration) {
3983                 if (iteration - iteration_count < MAX_ITERATION)
3984                         this_iteration = iteration - iteration_count;
3985                 else
3986                         this_iteration = MAX_ITERATION;
3987
3988                 /* set number of iterations */
3989                 uint32_t val_iteration;
3990                 val_iteration = interval << 16 | this_iteration;
3991                 if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_ITERATION,
3992                                         val_iteration) != ERROR_OK) {
3993                         retval = ERROR_FAIL;
3994                         goto end_profiling;
3995                 }
3996
3997                 /* init AICE_WRITE_CTRL_BATCH_DATA_BUF1_CTRL to store $PC */
3998                 if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_DATA_BUF1_CTRL,
3999                                         0x40000) != ERROR_OK) {
4000                         retval = ERROR_FAIL;
4001                         goto end_profiling;
4002                 }
4003
4004                 aice_usb_run(coreid);
4005
4006                 /* enable BATCH command */
4007                 if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_CTRL,
4008                                         0x80000000) != ERROR_OK) {
4009                         aice_usb_halt(coreid);
4010                         retval = ERROR_FAIL;
4011                         goto end_profiling;
4012                 }
4013
4014                 /* wait a while (AICE bug, workaround) */
4015                 alive_sleep(this_iteration);
4016
4017                 /* check status */
4018                 uint32_t i;
4019                 uint32_t batch_status;
4020
4021                 i = 0;
4022                 while (1) {
4023                         aice_read_ctrl(AICE_READ_CTRL_BATCH_STATUS, &batch_status);
4024
4025                         if (batch_status & 0x1) {
4026                                 break;
4027                         } else if (batch_status & 0xE) {
4028                                 aice_usb_halt(coreid);
4029                                 retval = ERROR_FAIL;
4030                                 goto end_profiling;
4031                         }
4032
4033                         if ((i % 30) == 0)
4034                                 keep_alive();
4035
4036                         i++;
4037                 }
4038
4039                 aice_usb_halt(coreid);
4040
4041                 /* get samples from batch data buffer */
4042                 if (aice_batch_buffer_read(AICE_BATCH_DATA_BUFFER_1,
4043                                         samples + iteration_count, this_iteration) != ERROR_OK) {
4044                         retval = ERROR_FAIL;
4045                         goto end_profiling;
4046                 }
4047
4048                 iteration_count += this_iteration;
4049         }
4050
4051 end_profiling:
4052         *num_samples = iteration_count;
4053
4054         return retval;
4055 }
4056
4057 /** */
4058 struct aice_port_api_s aice_usb_api = {
4059         /** */
4060         .open = aice_open_device,
4061         /** */
4062         .close = aice_usb_close,
4063         /** */
4064         .idcode = aice_usb_idcode,
4065         /** */
4066         .state = aice_usb_state,
4067         /** */
4068         .reset = aice_usb_reset,
4069         /** */
4070         .assert_srst = aice_usb_assert_srst,
4071         /** */
4072         .run = aice_usb_run,
4073         /** */
4074         .halt = aice_usb_halt,
4075         /** */
4076         .step = aice_usb_step,
4077         /** */
4078         .read_reg = aice_usb_read_reg,
4079         /** */
4080         .write_reg = aice_usb_write_reg,
4081         /** */
4082         .read_reg_64 = aice_usb_read_reg_64,
4083         /** */
4084         .write_reg_64 = aice_usb_write_reg_64,
4085         /** */
4086         .read_mem_unit = aice_usb_read_memory_unit,
4087         /** */
4088         .write_mem_unit = aice_usb_write_memory_unit,
4089         /** */
4090         .read_mem_bulk = aice_usb_bulk_read_mem,
4091         /** */
4092         .write_mem_bulk = aice_usb_bulk_write_mem,
4093         /** */
4094         .read_debug_reg = aice_usb_read_debug_reg,
4095         /** */
4096         .write_debug_reg = aice_usb_write_debug_reg,
4097         /** */
4098         .set_jtag_clock = aice_usb_set_jtag_clock,
4099         /** */
4100         .memory_access = aice_usb_memory_access,
4101         /** */
4102         .memory_mode = aice_usb_memory_mode,
4103         /** */
4104         .read_tlb = aice_usb_read_tlb,
4105         /** */
4106         .cache_ctl = aice_usb_cache_ctl,
4107         /** */
4108         .set_retry_times = aice_usb_set_retry_times,
4109         /** */
4110         .program_edm = aice_usb_program_edm,
4111         /** */
4112         .set_command_mode = aice_usb_set_command_mode,
4113         /** */
4114         .execute = aice_usb_execute,
4115         /** */
4116         .set_custom_srst_script = aice_usb_set_custom_srst_script,
4117         /** */
4118         .set_custom_trst_script = aice_usb_set_custom_trst_script,
4119         /** */
4120         .set_custom_restart_script = aice_usb_set_custom_restart_script,
4121         /** */
4122         .set_count_to_check_dbger = aice_usb_set_count_to_check_dbger,
4123         /** */
4124         .set_data_endian = aice_usb_set_data_endian,
4125         /** */
4126         .profiling = aice_usb_profiling,
4127 };