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, NULL, &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, NULL, &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         jtag_libusb_set_configuration(devh, 0);
2140
2141         unsigned int aice_read_ep;
2142         unsigned int aice_write_ep;
2143         jtag_libusb_choose_interface(devh, &aice_read_ep, &aice_write_ep, -1, -1, -1);
2144
2145         aice_handler.usb_read_ep = aice_read_ep;
2146         aice_handler.usb_write_ep = aice_write_ep;
2147         aice_handler.usb_handle = devh;
2148
2149         return ERROR_OK;
2150 }
2151
2152 static int aice_usb_read_reg_64(uint32_t coreid, uint32_t num, uint64_t *val)
2153 {
2154         LOG_DEBUG("aice_usb_read_reg_64, %s", nds32_reg_simple_name(num));
2155
2156         uint32_t value;
2157         uint32_t high_value;
2158
2159         if (ERROR_OK != aice_read_reg(coreid, num, &value))
2160                 value = 0xBBADBEEF;
2161
2162         aice_read_reg(coreid, R1, &high_value);
2163
2164         LOG_DEBUG("low: 0x%08" PRIx32 ", high: 0x%08" PRIx32 "\n", value, high_value);
2165
2166         if (data_endian == AICE_BIG_ENDIAN)
2167                 *val = (((uint64_t)high_value) << 32) | value;
2168         else
2169                 *val = (((uint64_t)value) << 32) | high_value;
2170
2171         return ERROR_OK;
2172 }
2173
2174 static int aice_usb_write_reg_64(uint32_t coreid, uint32_t num, uint64_t val)
2175 {
2176         uint32_t value;
2177         uint32_t high_value;
2178
2179         if (data_endian == AICE_BIG_ENDIAN) {
2180                 value = val & 0xFFFFFFFF;
2181                 high_value = (val >> 32) & 0xFFFFFFFF;
2182         } else {
2183                 high_value = val & 0xFFFFFFFF;
2184                 value = (val >> 32) & 0xFFFFFFFF;
2185         }
2186
2187         LOG_DEBUG("aice_usb_write_reg_64, %s, low: 0x%08" PRIx32 ", high: 0x%08" PRIx32 "\n",
2188                         nds32_reg_simple_name(num), value, high_value);
2189
2190         aice_write_reg(coreid, R1, high_value);
2191         return aice_write_reg(coreid, num, value);
2192 }
2193
2194 static int aice_get_version_info(void)
2195 {
2196         uint32_t hardware_version;
2197         uint32_t firmware_version;
2198         uint32_t fpga_version;
2199
2200         if (aice_read_ctrl(AICE_READ_CTRL_GET_HARDWARE_VERSION, &hardware_version) != ERROR_OK)
2201                 return ERROR_FAIL;
2202
2203         if (aice_read_ctrl(AICE_READ_CTRL_GET_FIRMWARE_VERSION, &firmware_version) != ERROR_OK)
2204                 return ERROR_FAIL;
2205
2206         if (aice_read_ctrl(AICE_READ_CTRL_GET_FPGA_VERSION, &fpga_version) != ERROR_OK)
2207                 return ERROR_FAIL;
2208
2209         LOG_INFO("AICE version: hw_ver = 0x%" PRIx32 ", fw_ver = 0x%" PRIx32 ", fpga_ver = 0x%" PRIx32,
2210                         hardware_version, firmware_version, fpga_version);
2211
2212         return ERROR_OK;
2213 }
2214
2215 #define LINE_BUFFER_SIZE 1024
2216
2217 static int aice_execute_custom_script(const char *script)
2218 {
2219         FILE *script_fd;
2220         char line_buffer[LINE_BUFFER_SIZE];
2221         char *op_str;
2222         char *reset_str;
2223         uint32_t delay;
2224         uint32_t write_ctrl_value;
2225         bool set_op;
2226
2227         script_fd = fopen(script, "r");
2228         if (script_fd == NULL) {
2229                 return ERROR_FAIL;
2230         } else {
2231                 while (fgets(line_buffer, LINE_BUFFER_SIZE, script_fd) != NULL) {
2232                         /* execute operations */
2233                         set_op = false;
2234                         op_str = strstr(line_buffer, "set");
2235                         if (op_str != NULL) {
2236                                 set_op = true;
2237                                 goto get_reset_type;
2238                         }
2239
2240                         op_str = strstr(line_buffer, "clear");
2241                         if (op_str == NULL)
2242                                 continue;
2243 get_reset_type:
2244                         reset_str = strstr(op_str, "srst");
2245                         if (reset_str != NULL) {
2246                                 if (set_op)
2247                                         write_ctrl_value = AICE_CUSTOM_DELAY_SET_SRST;
2248                                 else
2249                                         write_ctrl_value = AICE_CUSTOM_DELAY_CLEAN_SRST;
2250                                 goto get_delay;
2251                         }
2252                         reset_str = strstr(op_str, "dbgi");
2253                         if (reset_str != NULL) {
2254                                 if (set_op)
2255                                         write_ctrl_value = AICE_CUSTOM_DELAY_SET_DBGI;
2256                                 else
2257                                         write_ctrl_value = AICE_CUSTOM_DELAY_CLEAN_DBGI;
2258                                 goto get_delay;
2259                         }
2260                         reset_str = strstr(op_str, "trst");
2261                         if (reset_str != NULL) {
2262                                 if (set_op)
2263                                         write_ctrl_value = AICE_CUSTOM_DELAY_SET_TRST;
2264                                 else
2265                                         write_ctrl_value = AICE_CUSTOM_DELAY_CLEAN_TRST;
2266                                 goto get_delay;
2267                         }
2268                         continue;
2269 get_delay:
2270                         /* get delay */
2271                         delay = strtoul(reset_str + 4, NULL, 0);
2272                         write_ctrl_value |= (delay << 16);
2273
2274                         if (aice_write_ctrl(AICE_WRITE_CTRL_CUSTOM_DELAY,
2275                                                 write_ctrl_value) != ERROR_OK) {
2276                                 fclose(script_fd);
2277                                 return ERROR_FAIL;
2278                         }
2279                 }
2280                 fclose(script_fd);
2281         }
2282
2283         return ERROR_OK;
2284 }
2285
2286 static int aice_usb_set_clock(int set_clock)
2287 {
2288         if (aice_write_ctrl(AICE_WRITE_CTRL_TCK_CONTROL,
2289                                 AICE_TCK_CONTROL_TCK_SCAN) != ERROR_OK)
2290                 return ERROR_FAIL;
2291
2292         /* Read out TCK_SCAN clock value */
2293         uint32_t scan_clock;
2294         if (aice_read_ctrl(AICE_READ_CTRL_GET_ICE_STATE, &scan_clock) != ERROR_OK)
2295                 return ERROR_FAIL;
2296
2297         scan_clock &= 0x0F;
2298
2299         uint32_t scan_base_freq;
2300         if (scan_clock & 0x8)
2301                 scan_base_freq = 48000; /* 48 MHz */
2302         else
2303                 scan_base_freq = 30000; /* 30 MHz */
2304
2305         uint32_t set_base_freq;
2306         if (set_clock & 0x8)
2307                 set_base_freq = 48000;
2308         else
2309                 set_base_freq = 30000;
2310
2311         uint32_t set_freq;
2312         uint32_t scan_freq;
2313         set_freq = set_base_freq >> (set_clock & 0x7);
2314         scan_freq = scan_base_freq >> (scan_clock & 0x7);
2315
2316         if (scan_freq < set_freq) {
2317                 LOG_ERROR("User specifies higher jtag clock than TCK_SCAN clock");
2318                 return ERROR_FAIL;
2319         }
2320
2321         if (aice_write_ctrl(AICE_WRITE_CTRL_TCK_CONTROL, set_clock) != ERROR_OK)
2322                 return ERROR_FAIL;
2323
2324         uint32_t check_speed;
2325         if (aice_read_ctrl(AICE_READ_CTRL_GET_ICE_STATE, &check_speed) != ERROR_OK)
2326                 return ERROR_FAIL;
2327
2328         if (((int)check_speed & 0x0F) != set_clock) {
2329                 LOG_ERROR("Set jtag clock failed");
2330                 return ERROR_FAIL;
2331         }
2332
2333         return ERROR_OK;
2334 }
2335
2336 static int aice_edm_init(uint32_t coreid)
2337 {
2338         aice_write_edmsr(coreid, NDS_EDM_SR_DIMBR, 0xFFFF0000);
2339         aice_write_misc(coreid, NDS_EDM_MISC_DIMIR, 0);
2340
2341         /* unconditionally try to turn on V3_EDM_MODE */
2342         uint32_t edm_ctl_value;
2343         aice_read_edmsr(coreid, NDS_EDM_SR_EDM_CTL, &edm_ctl_value);
2344         aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL, edm_ctl_value | 0x00000040);
2345
2346         /* clear DBGER */
2347         aice_write_misc(coreid, NDS_EDM_MISC_DBGER,
2348                         NDS_DBGER_DPED | NDS_DBGER_CRST | NDS_DBGER_AT_MAX);
2349
2350         /* get EDM version */
2351         uint32_t value_edmcfg;
2352         aice_read_edmsr(coreid, NDS_EDM_SR_EDM_CFG, &value_edmcfg);
2353         core_info[coreid].edm_version = (value_edmcfg >> 16) & 0xFFFF;
2354
2355         return ERROR_OK;
2356 }
2357
2358 static bool is_v2_edm(uint32_t coreid)
2359 {
2360         if ((core_info[coreid].edm_version & 0x1000) == 0)
2361                 return true;
2362         else
2363                 return false;
2364 }
2365
2366 static int aice_init_edm_registers(uint32_t coreid, bool clear_dex_use_psw)
2367 {
2368         /* enable DEH_SEL & MAX_STOP & V3_EDM_MODE & DBGI_MASK */
2369         uint32_t host_edm_ctl = core_info[coreid].edm_ctl_backup | 0xA000004F;
2370         if (clear_dex_use_psw)
2371                 /* After entering debug mode, OpenOCD may set
2372                  * DEX_USE_PSW accidentally through backup value
2373                  * of target EDM_CTL.
2374                  * So, clear DEX_USE_PSW by force. */
2375                 host_edm_ctl &= ~(0x40000000);
2376
2377         LOG_DEBUG("aice_init_edm_registers - EDM_CTL: 0x%08" PRIx32, host_edm_ctl);
2378
2379         int result = aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL, host_edm_ctl);
2380
2381         return result;
2382 }
2383
2384 /**
2385  * EDM_CTL will be modified by OpenOCD as debugging. OpenOCD has the
2386  * responsibility to keep EDM_CTL untouched after debugging.
2387  *
2388  * There are two scenarios to consider:
2389  * 1. single step/running as debugging (running under debug session)
2390  * 2. detached from gdb (exit debug session)
2391  *
2392  * So, we need to bakcup EDM_CTL before halted and restore it after
2393  * running. The difference of these two scenarios is EDM_CTL.DEH_SEL
2394  * is on for scenario 1, and off for scenario 2.
2395  */
2396 static int aice_backup_edm_registers(uint32_t coreid)
2397 {
2398         int result = aice_read_edmsr(coreid, NDS_EDM_SR_EDM_CTL,
2399                         &core_info[coreid].edm_ctl_backup);
2400
2401         /* To call aice_backup_edm_registers() after DEX on, DEX_USE_PSW
2402          * may be not correct.  (For example, hit breakpoint, then backup
2403          * EDM_CTL. EDM_CTL.DEX_USE_PSW will be cleared.)  Because debug
2404          * interrupt will clear DEX_USE_PSW, DEX_USE_PSW is always off after
2405          * DEX is on.  It only backups correct value before OpenOCD issues DBGI.
2406          * (Backup EDM_CTL, then issue DBGI actively (refer aice_usb_halt())) */
2407         if (core_info[coreid].edm_ctl_backup & 0x40000000)
2408                 core_info[coreid].dex_use_psw_on = true;
2409         else
2410                 core_info[coreid].dex_use_psw_on = false;
2411
2412         LOG_DEBUG("aice_backup_edm_registers - EDM_CTL: 0x%08" PRIx32 ", DEX_USE_PSW: %s",
2413                         core_info[coreid].edm_ctl_backup,
2414                         core_info[coreid].dex_use_psw_on ? "on" : "off");
2415
2416         return result;
2417 }
2418
2419 static int aice_restore_edm_registers(uint32_t coreid)
2420 {
2421         LOG_DEBUG("aice_restore_edm_registers -");
2422
2423         /* set DEH_SEL, because target still under EDM control */
2424         int result = aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL,
2425                         core_info[coreid].edm_ctl_backup | 0x80000000);
2426
2427         return result;
2428 }
2429
2430 static int aice_backup_tmp_registers(uint32_t coreid)
2431 {
2432         LOG_DEBUG("backup_tmp_registers -");
2433
2434         /* backup target DTR first(if the target DTR is valid) */
2435         uint32_t value_edmsw;
2436         aice_read_edmsr(coreid, NDS_EDM_SR_EDMSW, &value_edmsw);
2437         core_info[coreid].edmsw_backup = value_edmsw;
2438         if (value_edmsw & 0x1) { /* EDMSW.WDV == 1 */
2439                 aice_read_dtr(coreid, &core_info[coreid].target_dtr_backup);
2440                 core_info[coreid].target_dtr_valid = true;
2441
2442                 LOG_DEBUG("Backup target DTR: 0x%08" PRIx32, core_info[coreid].target_dtr_backup);
2443         } else {
2444                 core_info[coreid].target_dtr_valid = false;
2445         }
2446
2447         /* Target DTR has been backup, then backup $R0 and $R1 */
2448         aice_read_reg(coreid, R0, &core_info[coreid].r0_backup);
2449         aice_read_reg(coreid, R1, &core_info[coreid].r1_backup);
2450
2451         /* backup host DTR(if the host DTR is valid) */
2452         if (value_edmsw & 0x2) { /* EDMSW.RDV == 1*/
2453                 /* read out host DTR and write into target DTR, then use aice_read_edmsr to
2454                  * read out */
2455                 uint32_t instructions[4] = {
2456                         MFSR_DTR(R0), /* R0 has already been backup */
2457                         DSB,
2458                         MTSR_DTR(R0),
2459                         BEQ_MINUS_12
2460                 };
2461                 aice_execute_dim(coreid, instructions, 4);
2462
2463                 aice_read_dtr(coreid, &core_info[coreid].host_dtr_backup);
2464                 core_info[coreid].host_dtr_valid = true;
2465
2466                 LOG_DEBUG("Backup host DTR: 0x%08" PRIx32, core_info[coreid].host_dtr_backup);
2467         } else {
2468                 core_info[coreid].host_dtr_valid = false;
2469         }
2470
2471         LOG_DEBUG("r0: 0x%08" PRIx32 ", r1: 0x%08" PRIx32,
2472                         core_info[coreid].r0_backup, core_info[coreid].r1_backup);
2473
2474         return ERROR_OK;
2475 }
2476
2477 static int aice_restore_tmp_registers(uint32_t coreid)
2478 {
2479         LOG_DEBUG("restore_tmp_registers - r0: 0x%08" PRIx32 ", r1: 0x%08" PRIx32,
2480                         core_info[coreid].r0_backup, core_info[coreid].r1_backup);
2481
2482         if (core_info[coreid].target_dtr_valid) {
2483                 uint32_t instructions[4] = {
2484                         SETHI(R0, core_info[coreid].target_dtr_backup >> 12),
2485                         ORI(R0, R0, core_info[coreid].target_dtr_backup & 0x00000FFF),
2486                         NOP,
2487                         BEQ_MINUS_12
2488                 };
2489                 aice_execute_dim(coreid, instructions, 4);
2490
2491                 instructions[0] = MTSR_DTR(R0);
2492                 instructions[1] = DSB;
2493                 instructions[2] = NOP;
2494                 instructions[3] = BEQ_MINUS_12;
2495                 aice_execute_dim(coreid, instructions, 4);
2496
2497                 LOG_DEBUG("Restore target DTR: 0x%08" PRIx32, core_info[coreid].target_dtr_backup);
2498         }
2499
2500         aice_write_reg(coreid, R0, core_info[coreid].r0_backup);
2501         aice_write_reg(coreid, R1, core_info[coreid].r1_backup);
2502
2503         if (core_info[coreid].host_dtr_valid) {
2504                 aice_write_dtr(coreid, core_info[coreid].host_dtr_backup);
2505
2506                 LOG_DEBUG("Restore host DTR: 0x%08" PRIx32, core_info[coreid].host_dtr_backup);
2507         }
2508
2509         return ERROR_OK;
2510 }
2511
2512 static int aice_open_device(struct aice_port_param_s *param)
2513 {
2514         if (ERROR_OK != aice_usb_open(param))
2515                 return ERROR_FAIL;
2516
2517         if (ERROR_FAIL == aice_get_version_info()) {
2518                 LOG_ERROR("Cannot get AICE version!");
2519                 return ERROR_FAIL;
2520         }
2521
2522         LOG_INFO("AICE initialization started");
2523
2524         /* attempt to reset Andes EDM */
2525         if (ERROR_FAIL == aice_reset_box()) {
2526                 LOG_ERROR("Cannot initial AICE box!");
2527                 return ERROR_FAIL;
2528         }
2529
2530         return ERROR_OK;
2531 }
2532
2533 static int aice_usb_set_jtag_clock(uint32_t a_clock)
2534 {
2535         jtag_clock = a_clock;
2536
2537         if (ERROR_OK != aice_usb_set_clock(a_clock)) {
2538                 LOG_ERROR("Cannot set AICE JTAG clock!");
2539                 return ERROR_FAIL;
2540         }
2541
2542         return ERROR_OK;
2543 }
2544
2545 static int aice_usb_close(void)
2546 {
2547         jtag_libusb_close(aice_handler.usb_handle);
2548
2549         if (custom_srst_script)
2550                 free(custom_srst_script);
2551
2552         if (custom_trst_script)
2553                 free(custom_trst_script);
2554
2555         if (custom_restart_script)
2556                 free(custom_restart_script);
2557
2558         return ERROR_OK;
2559 }
2560
2561 static int aice_core_init(uint32_t coreid)
2562 {
2563         core_info[coreid].access_channel = NDS_MEMORY_ACC_CPU;
2564         core_info[coreid].memory_select = NDS_MEMORY_SELECT_AUTO;
2565         core_info[coreid].core_state = AICE_TARGET_UNKNOWN;
2566
2567         return ERROR_OK;
2568 }
2569
2570 static int aice_usb_idcode(uint32_t *idcode, uint8_t *num_of_idcode)
2571 {
2572         int retval;
2573
2574         retval = aice_scan_chain(idcode, num_of_idcode);
2575         if (ERROR_OK == retval) {
2576                 for (int i = 0; i < *num_of_idcode; i++) {
2577                         aice_core_init(i);
2578                         aice_edm_init(i);
2579                 }
2580                 total_num_of_core = *num_of_idcode;
2581         }
2582
2583         return retval;
2584 }
2585
2586 static int aice_usb_halt(uint32_t coreid)
2587 {
2588         if (core_info[coreid].core_state == AICE_TARGET_HALTED) {
2589                 LOG_DEBUG("aice_usb_halt check halted");
2590                 return ERROR_OK;
2591         }
2592
2593         LOG_DEBUG("aice_usb_halt");
2594
2595         /** backup EDM registers */
2596         aice_backup_edm_registers(coreid);
2597         /** init EDM for host debugging */
2598         /** no need to clear dex_use_psw, because dbgi will clear it */
2599         aice_init_edm_registers(coreid, false);
2600
2601         /** Clear EDM_CTL.DBGIM & EDM_CTL.DBGACKM */
2602         uint32_t edm_ctl_value;
2603         aice_read_edmsr(coreid, NDS_EDM_SR_EDM_CTL, &edm_ctl_value);
2604         if (edm_ctl_value & 0x3)
2605                 aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL, edm_ctl_value & ~(0x3));
2606
2607         uint32_t dbger;
2608         uint32_t acc_ctl_value;
2609
2610         core_info[coreid].debug_under_dex_on = false;
2611         aice_read_misc(coreid, NDS_EDM_MISC_DBGER, &dbger);
2612
2613         if (dbger & NDS_DBGER_AT_MAX)
2614                 LOG_ERROR("<-- TARGET ERROR! Reaching the max interrupt stack level. -->");
2615
2616         if (dbger & NDS_DBGER_DEX) {
2617                 if (is_v2_edm(coreid) == false) {
2618                         /** debug 'debug mode'. use force_debug to issue dbgi */
2619                         aice_read_misc(coreid, NDS_EDM_MISC_ACC_CTL, &acc_ctl_value);
2620                         acc_ctl_value |= 0x8;
2621                         aice_write_misc(coreid, NDS_EDM_MISC_ACC_CTL, acc_ctl_value);
2622                         core_info[coreid].debug_under_dex_on = true;
2623
2624                         aice_write_misc(coreid, NDS_EDM_MISC_EDM_CMDR, 0);
2625                         /* If CPU stalled due to AT_MAX, clear AT_MAX status. */
2626                         if (dbger & NDS_DBGER_AT_MAX)
2627                                 aice_write_misc(coreid, NDS_EDM_MISC_DBGER, NDS_DBGER_AT_MAX);
2628                 }
2629         } else {
2630                 /** Issue DBGI normally */
2631                 aice_write_misc(coreid, NDS_EDM_MISC_EDM_CMDR, 0);
2632                 /* If CPU stalled due to AT_MAX, clear AT_MAX status. */
2633                 if (dbger & NDS_DBGER_AT_MAX)
2634                         aice_write_misc(coreid, NDS_EDM_MISC_DBGER, NDS_DBGER_AT_MAX);
2635         }
2636
2637         if (aice_check_dbger(coreid, NDS_DBGER_DEX) != ERROR_OK) {
2638                 LOG_ERROR("<-- TARGET ERROR! Unable to stop the debug target through DBGI. -->");
2639                 return ERROR_FAIL;
2640         }
2641
2642         if (core_info[coreid].debug_under_dex_on) {
2643                 if (core_info[coreid].dex_use_psw_on == false) {
2644                         /* under debug 'debug mode', force $psw to 'debug mode' bahavior */
2645                         /* !!!NOTICE!!! this is workaround for debug 'debug mode'.
2646                          * it is only for debugging 'debug exception handler' purpose.
2647                          * after openocd detaches from target, target behavior is
2648                          * undefined. */
2649                         uint32_t ir0_value;
2650                         uint32_t debug_mode_ir0_value;
2651                         aice_read_reg(coreid, IR0, &ir0_value);
2652                         debug_mode_ir0_value = ir0_value | 0x408; /* turn on DEX, set POM = 1 */
2653                         debug_mode_ir0_value &= ~(0x000000C1); /* turn off DT/IT/GIE */
2654                         aice_write_reg(coreid, IR0, debug_mode_ir0_value);
2655                 }
2656         }
2657
2658         /** set EDM_CTL.DBGIM & EDM_CTL.DBGACKM after halt */
2659         if (edm_ctl_value & 0x3)
2660                 aice_write_edmsr(coreid, NDS_EDM_SR_EDM_CTL, edm_ctl_value);
2661
2662         /* backup r0 & r1 */
2663         aice_backup_tmp_registers(coreid);
2664         core_info[coreid].core_state = AICE_TARGET_HALTED;
2665
2666         return ERROR_OK;
2667 }
2668
2669 static int aice_usb_state(uint32_t coreid, enum aice_target_state_s *state)
2670 {
2671         uint32_t dbger_value;
2672         uint32_t ice_state;
2673
2674         int result = aice_read_misc(coreid, NDS_EDM_MISC_DBGER, &dbger_value);
2675
2676         if (ERROR_AICE_TIMEOUT == result) {
2677                 if (aice_read_ctrl(AICE_READ_CTRL_GET_ICE_STATE, &ice_state) != ERROR_OK) {
2678                         LOG_ERROR("<-- AICE ERROR! AICE is unplugged. -->");
2679                         return ERROR_FAIL;
2680                 }
2681
2682                 if ((ice_state & 0x20) == 0) {
2683                         LOG_ERROR("<-- TARGET ERROR! Target is disconnected with AICE. -->");
2684                         return ERROR_FAIL;
2685                 } else {
2686                         return ERROR_FAIL;
2687                 }
2688         } else if (ERROR_AICE_DISCONNECT == result) {
2689                 LOG_ERROR("<-- AICE ERROR! AICE is unplugged. -->");
2690                 return ERROR_FAIL;
2691         }
2692
2693         if ((dbger_value & NDS_DBGER_ILL_SEC_ACC) == NDS_DBGER_ILL_SEC_ACC) {
2694                 LOG_ERROR("<-- TARGET ERROR! Insufficient security privilege. -->");
2695
2696                 /* Clear ILL_SEC_ACC */
2697                 aice_write_misc(coreid, NDS_EDM_MISC_DBGER, NDS_DBGER_ILL_SEC_ACC);
2698
2699                 *state = AICE_TARGET_RUNNING;
2700                 core_info[coreid].core_state = AICE_TARGET_RUNNING;
2701         } else if ((dbger_value & NDS_DBGER_AT_MAX) == NDS_DBGER_AT_MAX) {
2702                 /* Issue DBGI to exit cpu stall */
2703                 aice_usb_halt(coreid);
2704
2705                 /* Read OIPC to find out the trigger point */
2706                 uint32_t ir11_value;
2707                 aice_read_reg(coreid, IR11, &ir11_value);
2708
2709                 LOG_ERROR("<-- TARGET ERROR! Reaching the max interrupt stack level; "
2710                                 "CPU is stalled at 0x%08" PRIx32 " for debugging. -->", ir11_value);
2711
2712                 *state = AICE_TARGET_HALTED;
2713         } else if ((dbger_value & NDS_DBGER_CRST) == NDS_DBGER_CRST) {
2714                 LOG_DEBUG("DBGER.CRST is on.");
2715
2716                 *state = AICE_TARGET_RESET;
2717                 core_info[coreid].core_state = AICE_TARGET_RUNNING;
2718
2719                 /* Clear CRST */
2720                 aice_write_misc(coreid, NDS_EDM_MISC_DBGER, NDS_DBGER_CRST);
2721         } else if ((dbger_value & NDS_DBGER_DEX) == NDS_DBGER_DEX) {
2722                 if (AICE_TARGET_RUNNING == core_info[coreid].core_state) {
2723                         /* enter debug mode, init EDM registers */
2724                         /* backup EDM registers */
2725                         aice_backup_edm_registers(coreid);
2726                         /* init EDM for host debugging */
2727                         aice_init_edm_registers(coreid, true);
2728                         aice_backup_tmp_registers(coreid);
2729                         core_info[coreid].core_state = AICE_TARGET_HALTED;
2730                 } else if (AICE_TARGET_UNKNOWN == core_info[coreid].core_state) {
2731                         /* debug 'debug mode', use force debug to halt core */
2732                         aice_usb_halt(coreid);
2733                 }
2734                 *state = AICE_TARGET_HALTED;
2735         } else {
2736                 *state = AICE_TARGET_RUNNING;
2737                 core_info[coreid].core_state = AICE_TARGET_RUNNING;
2738         }
2739
2740         return ERROR_OK;
2741 }
2742
2743 static int aice_usb_reset(void)
2744 {
2745         if (aice_reset_box() != ERROR_OK)
2746                 return ERROR_FAIL;
2747
2748         /* issue TRST */
2749         if (custom_trst_script == NULL) {
2750                 if (aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_CONTROL,
2751                                         AICE_JTAG_PIN_CONTROL_TRST) != ERROR_OK)
2752                         return ERROR_FAIL;
2753         } else {
2754                 /* custom trst operations */
2755                 if (aice_execute_custom_script(custom_trst_script) != ERROR_OK)
2756                         return ERROR_FAIL;
2757         }
2758
2759         if (aice_usb_set_clock(jtag_clock) != ERROR_OK)
2760                 return ERROR_FAIL;
2761
2762         return ERROR_OK;
2763 }
2764
2765 static int aice_issue_srst(uint32_t coreid)
2766 {
2767         LOG_DEBUG("aice_issue_srst");
2768
2769         /* After issuing srst, target will be running. So we need to restore EDM_CTL. */
2770         aice_restore_edm_registers(coreid);
2771
2772         if (custom_srst_script == NULL) {
2773                 if (aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_CONTROL,
2774                                         AICE_JTAG_PIN_CONTROL_SRST) != ERROR_OK)
2775                         return ERROR_FAIL;
2776         } else {
2777                 /* custom srst operations */
2778                 if (aice_execute_custom_script(custom_srst_script) != ERROR_OK)
2779                         return ERROR_FAIL;
2780         }
2781
2782         /* wait CRST infinitely */
2783         uint32_t dbger_value;
2784         int i = 0;
2785         while (1) {
2786                 if (aice_read_misc(coreid,
2787                                         NDS_EDM_MISC_DBGER, &dbger_value) != ERROR_OK)
2788                         return ERROR_FAIL;
2789
2790                 if (dbger_value & NDS_DBGER_CRST)
2791                         break;
2792
2793                 if ((i % 30) == 0)
2794                         keep_alive();
2795                 i++;
2796         }
2797
2798         core_info[coreid].host_dtr_valid = false;
2799         core_info[coreid].target_dtr_valid = false;
2800
2801         core_info[coreid].core_state = AICE_TARGET_RUNNING;
2802         return ERROR_OK;
2803 }
2804
2805 static int aice_issue_reset_hold(uint32_t coreid)
2806 {
2807         LOG_DEBUG("aice_issue_reset_hold");
2808
2809         /* set no_dbgi_pin to 0 */
2810         uint32_t pin_status;
2811         aice_read_ctrl(AICE_READ_CTRL_GET_JTAG_PIN_STATUS, &pin_status);
2812         if (pin_status | 0x4)
2813                 aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_STATUS, pin_status & (~0x4));
2814
2815         /* issue restart */
2816         if (custom_restart_script == NULL) {
2817                 if (aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_CONTROL,
2818                                         AICE_JTAG_PIN_CONTROL_RESTART) != ERROR_OK)
2819                         return ERROR_FAIL;
2820         } else {
2821                 /* custom restart operations */
2822                 if (aice_execute_custom_script(custom_restart_script) != ERROR_OK)
2823                         return ERROR_FAIL;
2824         }
2825
2826         if (aice_check_dbger(coreid, NDS_DBGER_CRST | NDS_DBGER_DEX) == ERROR_OK) {
2827                 aice_backup_tmp_registers(coreid);
2828                 core_info[coreid].core_state = AICE_TARGET_HALTED;
2829
2830                 return ERROR_OK;
2831         } else {
2832                 /* set no_dbgi_pin to 1 */
2833                 aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_STATUS, pin_status | 0x4);
2834
2835                 /* issue restart again */
2836                 if (custom_restart_script == NULL) {
2837                         if (aice_write_ctrl(AICE_WRITE_CTRL_JTAG_PIN_CONTROL,
2838                                                 AICE_JTAG_PIN_CONTROL_RESTART) != ERROR_OK)
2839                                 return ERROR_FAIL;
2840                 } else {
2841                         /* custom restart operations */
2842                         if (aice_execute_custom_script(custom_restart_script) != ERROR_OK)
2843                                 return ERROR_FAIL;
2844                 }
2845
2846                 if (aice_check_dbger(coreid, NDS_DBGER_CRST | NDS_DBGER_DEX) == ERROR_OK) {
2847                         aice_backup_tmp_registers(coreid);
2848                         core_info[coreid].core_state = AICE_TARGET_HALTED;
2849
2850                         return ERROR_OK;
2851                 }
2852
2853                 /* do software reset-and-hold */
2854                 aice_issue_srst(coreid);
2855                 aice_usb_halt(coreid);
2856
2857                 uint32_t value_ir3;
2858                 aice_read_reg(coreid, IR3, &value_ir3);
2859                 aice_write_reg(coreid, PC, value_ir3 & 0xFFFF0000);
2860         }
2861
2862         return ERROR_FAIL;
2863 }
2864
2865 static int aice_issue_reset_hold_multi(void)
2866 {
2867         uint32_t write_ctrl_value = 0;
2868
2869         /* set SRST */
2870         write_ctrl_value = AICE_CUSTOM_DELAY_SET_SRST;
2871         write_ctrl_value |= (0x200 << 16);
2872         if (aice_write_ctrl(AICE_WRITE_CTRL_CUSTOM_DELAY,
2873                                 write_ctrl_value) != ERROR_OK)
2874                 return ERROR_FAIL;
2875
2876         for (uint8_t i = 0 ; i < total_num_of_core ; i++)
2877                 aice_write_misc(i, NDS_EDM_MISC_EDM_CMDR, 0);
2878
2879         /* clear SRST */
2880         write_ctrl_value = AICE_CUSTOM_DELAY_CLEAN_SRST;
2881         write_ctrl_value |= (0x200 << 16);
2882         if (aice_write_ctrl(AICE_WRITE_CTRL_CUSTOM_DELAY,
2883                                 write_ctrl_value) != ERROR_OK)
2884                 return ERROR_FAIL;
2885
2886         for (uint8_t i = 0; i < total_num_of_core; i++)
2887                 aice_edm_init(i);
2888
2889         return ERROR_FAIL;
2890 }
2891
2892 static int aice_usb_assert_srst(uint32_t coreid, enum aice_srst_type_s srst)
2893 {
2894         if ((AICE_SRST != srst) && (AICE_RESET_HOLD != srst))
2895                 return ERROR_FAIL;
2896
2897         /* clear DBGER */
2898         if (aice_write_misc(coreid, NDS_EDM_MISC_DBGER,
2899                                 NDS_DBGER_CLEAR_ALL) != ERROR_OK)
2900                 return ERROR_FAIL;
2901
2902         int result = ERROR_OK;
2903         if (AICE_SRST == srst)
2904                 result = aice_issue_srst(coreid);
2905         else {
2906                 if (1 == total_num_of_core)
2907                         result = aice_issue_reset_hold(coreid);
2908                 else
2909                         result = aice_issue_reset_hold_multi();
2910         }
2911
2912         /* Clear DBGER.CRST after reset to avoid 'core-reset checking' errors.
2913          * assert_srst is user-intentional reset behavior, so we could
2914          * clear DBGER.CRST safely.
2915          */
2916         if (aice_write_misc(coreid,
2917                                 NDS_EDM_MISC_DBGER, NDS_DBGER_CRST) != ERROR_OK)
2918                 return ERROR_FAIL;
2919
2920         return result;
2921 }
2922
2923 static int aice_usb_run(uint32_t coreid)
2924 {
2925         LOG_DEBUG("aice_usb_run");
2926
2927         uint32_t dbger_value;
2928         if (aice_read_misc(coreid,
2929                                 NDS_EDM_MISC_DBGER, &dbger_value) != ERROR_OK)
2930                 return ERROR_FAIL;
2931
2932         if ((dbger_value & NDS_DBGER_DEX) != NDS_DBGER_DEX) {
2933                 LOG_WARNING("<-- TARGET WARNING! The debug target exited "
2934                                 "the debug mode unexpectedly. -->");
2935                 return ERROR_FAIL;
2936         }
2937
2938         /* restore r0 & r1 before free run */
2939         aice_restore_tmp_registers(coreid);
2940         core_info[coreid].core_state = AICE_TARGET_RUNNING;
2941
2942         /* clear DBGER */
2943         aice_write_misc(coreid, NDS_EDM_MISC_DBGER,
2944                         NDS_DBGER_CLEAR_ALL);
2945
2946         /** restore EDM registers */
2947         /** OpenOCD should restore EDM_CTL **before** to exit debug state.
2948          *  Otherwise, following instruction will read wrong EDM_CTL value.
2949          *
2950          *  pc -> mfsr $p0, EDM_CTL (single step)
2951          *        slli $p0, $p0, 1
2952          *        slri $p0, $p0, 31
2953          */
2954         aice_restore_edm_registers(coreid);
2955
2956         /** execute instructions in DIM */
2957         uint32_t instructions[4] = {
2958                 NOP,
2959                 NOP,
2960                 NOP,
2961                 IRET
2962         };
2963         int result = aice_execute_dim(coreid, instructions, 4);
2964
2965         return result;
2966 }
2967
2968 static int aice_usb_step(uint32_t coreid)
2969 {
2970         LOG_DEBUG("aice_usb_step");
2971
2972         uint32_t ir0_value;
2973         uint32_t ir0_reg_num;
2974
2975         if (is_v2_edm(coreid) == true)
2976                 /* V2 EDM will push interrupt stack as debug exception */
2977                 ir0_reg_num = IR1;
2978         else
2979                 ir0_reg_num = IR0;
2980
2981         /** enable HSS */
2982         aice_read_reg(coreid, ir0_reg_num, &ir0_value);
2983         if ((ir0_value & 0x800) == 0) {
2984                 /** set PSW.HSS */
2985                 ir0_value |= (0x01 << 11);
2986                 aice_write_reg(coreid, ir0_reg_num, ir0_value);
2987         }
2988
2989         if (ERROR_FAIL == aice_usb_run(coreid))
2990                 return ERROR_FAIL;
2991
2992         int i = 0;
2993         enum aice_target_state_s state;
2994         while (1) {
2995                 /* read DBGER */
2996                 if (aice_usb_state(coreid, &state) != ERROR_OK)
2997                         return ERROR_FAIL;
2998
2999                 if (AICE_TARGET_HALTED == state)
3000                         break;
3001
3002                 long long then = 0;
3003                 if (i == 30)
3004                         then = timeval_ms();
3005
3006                 if (i >= 30) {
3007                         if ((timeval_ms() - then) > 1000)
3008                                 LOG_WARNING("Timeout (1000ms) waiting for halt to complete");
3009
3010                         return ERROR_FAIL;
3011                 }
3012                 i++;
3013         }
3014
3015         /** disable HSS */
3016         aice_read_reg(coreid, ir0_reg_num, &ir0_value);
3017         ir0_value &= ~(0x01 << 11);
3018         aice_write_reg(coreid, ir0_reg_num, ir0_value);
3019
3020         return ERROR_OK;
3021 }
3022
3023 static int aice_usb_read_mem_b_bus(uint32_t coreid, uint32_t address, uint32_t *data)
3024 {
3025         return aice_read_mem_b(coreid, address, data);
3026 }
3027
3028 static int aice_usb_read_mem_h_bus(uint32_t coreid, uint32_t address, uint32_t *data)
3029 {
3030         return aice_read_mem_h(coreid, address, data);
3031 }
3032
3033 static int aice_usb_read_mem_w_bus(uint32_t coreid, uint32_t address, uint32_t *data)
3034 {
3035         return aice_read_mem(coreid, address, data);
3036 }
3037
3038 static int aice_usb_read_mem_b_dim(uint32_t coreid, uint32_t address, uint32_t *data)
3039 {
3040         uint32_t value;
3041         uint32_t instructions[4] = {
3042                 LBI_BI(R1, R0),
3043                 MTSR_DTR(R1),
3044                 DSB,
3045                 BEQ_MINUS_12
3046         };
3047
3048         aice_execute_dim(coreid, instructions, 4);
3049
3050         aice_read_dtr(coreid, &value);
3051         *data = value & 0xFF;
3052
3053         return ERROR_OK;
3054 }
3055
3056 static int aice_usb_read_mem_h_dim(uint32_t coreid, uint32_t address, uint32_t *data)
3057 {
3058         uint32_t value;
3059         uint32_t instructions[4] = {
3060                 LHI_BI(R1, R0),
3061                 MTSR_DTR(R1),
3062                 DSB,
3063                 BEQ_MINUS_12
3064         };
3065
3066         aice_execute_dim(coreid, instructions, 4);
3067
3068         aice_read_dtr(coreid, &value);
3069         *data = value & 0xFFFF;
3070
3071         return ERROR_OK;
3072 }
3073
3074 static int aice_usb_read_mem_w_dim(uint32_t coreid, uint32_t address, uint32_t *data)
3075 {
3076         uint32_t instructions[4] = {
3077                 LWI_BI(R1, R0),
3078                 MTSR_DTR(R1),
3079                 DSB,
3080                 BEQ_MINUS_12
3081         };
3082
3083         aice_execute_dim(coreid, instructions, 4);
3084
3085         aice_read_dtr(coreid, data);
3086
3087         return ERROR_OK;
3088 }
3089
3090 static int aice_usb_set_address_dim(uint32_t coreid, uint32_t address)
3091 {
3092         uint32_t instructions[4] = {
3093                 SETHI(R0, address >> 12),
3094                 ORI(R0, R0, address & 0x00000FFF),
3095                 NOP,
3096                 BEQ_MINUS_12
3097         };
3098
3099         return aice_execute_dim(coreid, instructions, 4);
3100 }
3101
3102 static int aice_usb_read_memory_unit(uint32_t coreid, uint32_t addr, uint32_t size,
3103                 uint32_t count, uint8_t *buffer)
3104 {
3105         LOG_DEBUG("aice_usb_read_memory_unit, addr: 0x%08" PRIx32
3106                         ", size: %" PRIu32 ", count: %" PRIu32 "",
3107                         addr, size, count);
3108
3109         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3110                 aice_usb_set_address_dim(coreid, addr);
3111
3112         uint32_t value;
3113         size_t i;
3114         read_mem_func_t read_mem_func;
3115
3116         switch (size) {
3117                 case 1:
3118                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3119                                 read_mem_func = aice_usb_read_mem_b_bus;
3120                         else
3121                                 read_mem_func = aice_usb_read_mem_b_dim;
3122
3123                         for (i = 0; i < count; i++) {
3124                                 read_mem_func(coreid, addr, &value);
3125                                 *buffer++ = (uint8_t)value;
3126                                 addr++;
3127                         }
3128                         break;
3129                 case 2:
3130                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3131                                 read_mem_func = aice_usb_read_mem_h_bus;
3132                         else
3133                                 read_mem_func = aice_usb_read_mem_h_dim;
3134
3135                         for (i = 0; i < count; i++) {
3136                                 read_mem_func(coreid, addr, &value);
3137                                 uint16_t svalue = value;
3138                                 memcpy(buffer, &svalue, sizeof(uint16_t));
3139                                 buffer += 2;
3140                                 addr += 2;
3141                         }
3142                         break;
3143                 case 4:
3144                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3145                                 read_mem_func = aice_usb_read_mem_w_bus;
3146                         else
3147                                 read_mem_func = aice_usb_read_mem_w_dim;
3148
3149                         for (i = 0; i < count; i++) {
3150                                 read_mem_func(coreid, addr, &value);
3151                                 memcpy(buffer, &value, sizeof(uint32_t));
3152                                 buffer += 4;
3153                                 addr += 4;
3154                         }
3155                         break;
3156         }
3157
3158         return ERROR_OK;
3159 }
3160
3161 static int aice_usb_write_mem_b_bus(uint32_t coreid, uint32_t address, uint32_t data)
3162 {
3163         return aice_write_mem_b(coreid, address, data);
3164 }
3165
3166 static int aice_usb_write_mem_h_bus(uint32_t coreid, uint32_t address, uint32_t data)
3167 {
3168         return aice_write_mem_h(coreid, address, data);
3169 }
3170
3171 static int aice_usb_write_mem_w_bus(uint32_t coreid, uint32_t address, uint32_t data)
3172 {
3173         return aice_write_mem(coreid, address, data);
3174 }
3175
3176 static int aice_usb_write_mem_b_dim(uint32_t coreid, uint32_t address, uint32_t data)
3177 {
3178         uint32_t instructions[4] = {
3179                 MFSR_DTR(R1),
3180                 SBI_BI(R1, R0),
3181                 DSB,
3182                 BEQ_MINUS_12
3183         };
3184
3185         aice_write_dtr(coreid, data & 0xFF);
3186         aice_execute_dim(coreid, instructions, 4);
3187
3188         return ERROR_OK;
3189 }
3190
3191 static int aice_usb_write_mem_h_dim(uint32_t coreid, uint32_t address, uint32_t data)
3192 {
3193         uint32_t instructions[4] = {
3194                 MFSR_DTR(R1),
3195                 SHI_BI(R1, R0),
3196                 DSB,
3197                 BEQ_MINUS_12
3198         };
3199
3200         aice_write_dtr(coreid, data & 0xFFFF);
3201         aice_execute_dim(coreid, instructions, 4);
3202
3203         return ERROR_OK;
3204 }
3205
3206 static int aice_usb_write_mem_w_dim(uint32_t coreid, uint32_t address, uint32_t data)
3207 {
3208         uint32_t instructions[4] = {
3209                 MFSR_DTR(R1),
3210                 SWI_BI(R1, R0),
3211                 DSB,
3212                 BEQ_MINUS_12
3213         };
3214
3215         aice_write_dtr(coreid, data);
3216         aice_execute_dim(coreid, instructions, 4);
3217
3218         return ERROR_OK;
3219 }
3220
3221 static int aice_usb_write_memory_unit(uint32_t coreid, uint32_t addr, uint32_t size,
3222                 uint32_t count, const uint8_t *buffer)
3223 {
3224         LOG_DEBUG("aice_usb_write_memory_unit, addr: 0x%08" PRIx32
3225                         ", size: %" PRIu32 ", count: %" PRIu32 "",
3226                         addr, size, count);
3227
3228         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3229                 aice_usb_set_address_dim(coreid, addr);
3230
3231         size_t i;
3232         write_mem_func_t write_mem_func;
3233
3234         switch (size) {
3235                 case 1:
3236                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3237                                 write_mem_func = aice_usb_write_mem_b_bus;
3238                         else
3239                                 write_mem_func = aice_usb_write_mem_b_dim;
3240
3241                         for (i = 0; i < count; i++) {
3242                                 write_mem_func(coreid, addr, *buffer);
3243                                 buffer++;
3244                                 addr++;
3245                         }
3246                         break;
3247                 case 2:
3248                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3249                                 write_mem_func = aice_usb_write_mem_h_bus;
3250                         else
3251                                 write_mem_func = aice_usb_write_mem_h_dim;
3252
3253                         for (i = 0; i < count; i++) {
3254                                 uint16_t value;
3255                                 memcpy(&value, buffer, sizeof(uint16_t));
3256
3257                                 write_mem_func(coreid, addr, value);
3258                                 buffer += 2;
3259                                 addr += 2;
3260                         }
3261                         break;
3262                 case 4:
3263                         if (NDS_MEMORY_ACC_BUS == core_info[coreid].access_channel)
3264                                 write_mem_func = aice_usb_write_mem_w_bus;
3265                         else
3266                                 write_mem_func = aice_usb_write_mem_w_dim;
3267
3268                         for (i = 0; i < count; i++) {
3269                                 uint32_t value;
3270                                 memcpy(&value, buffer, sizeof(uint32_t));
3271
3272                                 write_mem_func(coreid, addr, value);
3273                                 buffer += 4;
3274                                 addr += 4;
3275                         }
3276                         break;
3277         }
3278
3279         return ERROR_OK;
3280 }
3281
3282 static int aice_bulk_read_mem(uint32_t coreid, uint32_t addr, uint32_t count,
3283                 uint8_t *buffer)
3284 {
3285         uint32_t packet_size;
3286
3287         while (count > 0) {
3288                 packet_size = (count >= 0x100) ? 0x100 : count;
3289
3290                 /** set address */
3291                 addr &= 0xFFFFFFFC;
3292                 if (aice_write_misc(coreid, NDS_EDM_MISC_SBAR, addr) != ERROR_OK)
3293                         return ERROR_FAIL;
3294
3295                 if (aice_fastread_mem(coreid, buffer,
3296                                         packet_size) != ERROR_OK)
3297                         return ERROR_FAIL;
3298
3299                 buffer += (packet_size * 4);
3300                 addr += (packet_size * 4);
3301                 count -= packet_size;
3302         }
3303
3304         return ERROR_OK;
3305 }
3306
3307 static int aice_bulk_write_mem(uint32_t coreid, uint32_t addr, uint32_t count,
3308                 const uint8_t *buffer)
3309 {
3310         uint32_t packet_size;
3311
3312         while (count > 0) {
3313                 packet_size = (count >= 0x100) ? 0x100 : count;
3314
3315                 /** set address */
3316                 addr &= 0xFFFFFFFC;
3317                 if (aice_write_misc(coreid, NDS_EDM_MISC_SBAR, addr | 1) != ERROR_OK)
3318                         return ERROR_FAIL;
3319
3320                 if (aice_fastwrite_mem(coreid, buffer,
3321                                         packet_size) != ERROR_OK)
3322                         return ERROR_FAIL;
3323
3324                 buffer += (packet_size * 4);
3325                 addr += (packet_size * 4);
3326                 count -= packet_size;
3327         }
3328
3329         return ERROR_OK;
3330 }
3331
3332 static int aice_usb_bulk_read_mem(uint32_t coreid, uint32_t addr,
3333                 uint32_t length, uint8_t *buffer)
3334 {
3335         LOG_DEBUG("aice_usb_bulk_read_mem, addr: 0x%08" PRIx32 ", length: 0x%08" PRIx32, addr, length);
3336
3337         int retval;
3338
3339         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3340                 aice_usb_set_address_dim(coreid, addr);
3341
3342         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3343                 retval = aice_usb_read_memory_unit(coreid, addr, 4, length / 4, buffer);
3344         else
3345                 retval = aice_bulk_read_mem(coreid, addr, length / 4, buffer);
3346
3347         return retval;
3348 }
3349
3350 static int aice_usb_bulk_write_mem(uint32_t coreid, uint32_t addr,
3351                 uint32_t length, const uint8_t *buffer)
3352 {
3353         LOG_DEBUG("aice_usb_bulk_write_mem, addr: 0x%08" PRIx32 ", length: 0x%08" PRIx32, addr, length);
3354
3355         int retval;
3356
3357         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3358                 aice_usb_set_address_dim(coreid, addr);
3359
3360         if (NDS_MEMORY_ACC_CPU == core_info[coreid].access_channel)
3361                 retval = aice_usb_write_memory_unit(coreid, addr, 4, length / 4, buffer);
3362         else
3363                 retval = aice_bulk_write_mem(coreid, addr, length / 4, buffer);
3364
3365         return retval;
3366 }
3367
3368 static int aice_usb_read_debug_reg(uint32_t coreid, uint32_t addr, uint32_t *val)
3369 {
3370         if (AICE_TARGET_HALTED == core_info[coreid].core_state) {
3371                 if (NDS_EDM_SR_EDMSW == addr) {
3372                         *val = core_info[coreid].edmsw_backup;
3373                 } else if (NDS_EDM_SR_EDM_DTR == addr) {
3374                         if (core_info[coreid].target_dtr_valid) {
3375                                 /* if EDM_DTR has read out, clear it. */
3376                                 *val = core_info[coreid].target_dtr_backup;
3377                                 core_info[coreid].edmsw_backup &= (~0x1);
3378                                 core_info[coreid].target_dtr_valid = false;
3379                         } else {
3380                                 *val = 0;
3381                         }
3382                 }
3383         }
3384
3385         return aice_read_edmsr(coreid, addr, val);
3386 }
3387
3388 static int aice_usb_write_debug_reg(uint32_t coreid, uint32_t addr, const uint32_t val)
3389 {
3390         if (AICE_TARGET_HALTED == core_info[coreid].core_state) {
3391                 if (NDS_EDM_SR_EDM_DTR == addr) {
3392                         core_info[coreid].host_dtr_backup = val;
3393                         core_info[coreid].edmsw_backup |= 0x2;
3394                         core_info[coreid].host_dtr_valid = true;
3395                 }
3396         }
3397
3398         return aice_write_edmsr(coreid, addr, val);
3399 }
3400
3401 static int aice_usb_memory_access(uint32_t coreid, enum nds_memory_access channel)
3402 {
3403         LOG_DEBUG("aice_usb_memory_access, access channel: %u", channel);
3404
3405         core_info[coreid].access_channel = channel;
3406
3407         return ERROR_OK;
3408 }
3409
3410 static int aice_usb_memory_mode(uint32_t coreid, enum nds_memory_select mem_select)
3411 {
3412         if (core_info[coreid].memory_select == mem_select)
3413                 return ERROR_OK;
3414
3415         LOG_DEBUG("aice_usb_memory_mode, memory select: %u", mem_select);
3416
3417         core_info[coreid].memory_select = mem_select;
3418
3419         if (NDS_MEMORY_SELECT_AUTO != core_info[coreid].memory_select)
3420                 aice_write_misc(coreid, NDS_EDM_MISC_ACC_CTL,
3421                                 core_info[coreid].memory_select - 1);
3422         else
3423                 aice_write_misc(coreid, NDS_EDM_MISC_ACC_CTL,
3424                                 NDS_MEMORY_SELECT_MEM - 1);
3425
3426         return ERROR_OK;
3427 }
3428
3429 static int aice_usb_read_tlb(uint32_t coreid, uint32_t virtual_address,
3430                 uint32_t *physical_address)
3431 {
3432         LOG_DEBUG("aice_usb_read_tlb, virtual address: 0x%08" PRIx32, virtual_address);
3433
3434         uint32_t instructions[4];
3435         uint32_t probe_result;
3436         uint32_t value_mr3;
3437         uint32_t value_mr4;
3438         uint32_t access_page_size;
3439         uint32_t virtual_offset;
3440         uint32_t physical_page_number;
3441
3442         aice_write_dtr(coreid, virtual_address);
3443
3444         /* probe TLB first */
3445         instructions[0] = MFSR_DTR(R0);
3446         instructions[1] = TLBOP_TARGET_PROBE(R1, R0);
3447         instructions[2] = DSB;
3448         instructions[3] = BEQ_MINUS_12;
3449         aice_execute_dim(coreid, instructions, 4);
3450
3451         aice_read_reg(coreid, R1, &probe_result);
3452
3453         if (probe_result & 0x80000000)
3454                 return ERROR_FAIL;
3455
3456         /* read TLB entry */
3457         aice_write_dtr(coreid, probe_result & 0x7FF);
3458
3459         /* probe TLB first */
3460         instructions[0] = MFSR_DTR(R0);
3461         instructions[1] = TLBOP_TARGET_READ(R0);
3462         instructions[2] = DSB;
3463         instructions[3] = BEQ_MINUS_12;
3464         aice_execute_dim(coreid, instructions, 4);
3465
3466         /* TODO: it should backup mr3, mr4 */
3467         aice_read_reg(coreid, MR3, &value_mr3);
3468         aice_read_reg(coreid, MR4, &value_mr4);
3469
3470         access_page_size = value_mr4 & 0xF;
3471         if (0 == access_page_size) { /* 4K page */
3472                 virtual_offset = virtual_address & 0x00000FFF;
3473                 physical_page_number = value_mr3 & 0xFFFFF000;
3474         } else if (1 == access_page_size) { /* 8K page */
3475                 virtual_offset = virtual_address & 0x00001FFF;
3476                 physical_page_number = value_mr3 & 0xFFFFE000;
3477         } else if (5 == access_page_size) { /* 1M page */
3478                 virtual_offset = virtual_address & 0x000FFFFF;
3479                 physical_page_number = value_mr3 & 0xFFF00000;
3480         } else {
3481                 return ERROR_FAIL;
3482         }
3483
3484         *physical_address = physical_page_number | virtual_offset;
3485
3486         return ERROR_OK;
3487 }
3488
3489 static int aice_usb_init_cache(uint32_t coreid)
3490 {
3491         LOG_DEBUG("aice_usb_init_cache");
3492
3493         uint32_t value_cr1;
3494         uint32_t value_cr2;
3495
3496         aice_read_reg(coreid, CR1, &value_cr1);
3497         aice_read_reg(coreid, CR2, &value_cr2);
3498
3499         struct cache_info *icache = &core_info[coreid].icache;
3500
3501         icache->set = value_cr1 & 0x7;
3502         icache->log2_set = icache->set + 6;
3503         icache->set = 64 << icache->set;
3504         icache->way = ((value_cr1 >> 3) & 0x7) + 1;
3505         icache->line_size = (value_cr1 >> 6) & 0x7;
3506         if (icache->line_size != 0) {
3507                 icache->log2_line_size = icache->line_size + 2;
3508                 icache->line_size = 8 << (icache->line_size - 1);
3509         } else {
3510                 icache->log2_line_size = 0;
3511         }
3512
3513         LOG_DEBUG("\ticache set: %" PRIu32 ", way: %" PRIu32 ", line size: %" PRIu32 ", "
3514                         "log2(set): %" PRIu32 ", log2(line_size): %" PRIu32 "",
3515                         icache->set, icache->way, icache->line_size,
3516                         icache->log2_set, icache->log2_line_size);
3517
3518         struct cache_info *dcache = &core_info[coreid].dcache;
3519
3520         dcache->set = value_cr2 & 0x7;
3521         dcache->log2_set = dcache->set + 6;
3522         dcache->set = 64 << dcache->set;
3523         dcache->way = ((value_cr2 >> 3) & 0x7) + 1;
3524         dcache->line_size = (value_cr2 >> 6) & 0x7;
3525         if (dcache->line_size != 0) {
3526                 dcache->log2_line_size = dcache->line_size + 2;
3527                 dcache->line_size = 8 << (dcache->line_size - 1);
3528         } else {
3529                 dcache->log2_line_size = 0;
3530         }
3531
3532         LOG_DEBUG("\tdcache set: %" PRIu32 ", way: %" PRIu32 ", line size: %" PRIu32 ", "
3533                         "log2(set): %" PRIu32 ", log2(line_size): %" PRIu32 "",
3534                         dcache->set, dcache->way, dcache->line_size,
3535                         dcache->log2_set, dcache->log2_line_size);
3536
3537         core_info[coreid].cache_init = true;
3538
3539         return ERROR_OK;
3540 }
3541
3542 static int aice_usb_dcache_inval_all(uint32_t coreid)
3543 {
3544         LOG_DEBUG("aice_usb_dcache_inval_all");
3545
3546         uint32_t set_index;
3547         uint32_t way_index;
3548         uint32_t cache_index;
3549         uint32_t instructions[4];
3550
3551         instructions[0] = MFSR_DTR(R0);
3552         instructions[1] = L1D_IX_INVAL(R0);
3553         instructions[2] = DSB;
3554         instructions[3] = BEQ_MINUS_12;
3555
3556         struct cache_info *dcache = &core_info[coreid].dcache;
3557
3558         for (set_index = 0; set_index < dcache->set; set_index++) {
3559                 for (way_index = 0; way_index < dcache->way; way_index++) {
3560                         cache_index = (way_index << (dcache->log2_set + dcache->log2_line_size)) |
3561                                 (set_index << dcache->log2_line_size);
3562
3563                         if (ERROR_OK != aice_write_dtr(coreid, cache_index))
3564                                 return ERROR_FAIL;
3565
3566                         if (ERROR_OK != aice_execute_dim(coreid, instructions, 4))
3567                                 return ERROR_FAIL;
3568                 }
3569         }
3570
3571         return ERROR_OK;
3572 }
3573
3574 static int aice_usb_dcache_va_inval(uint32_t coreid, uint32_t address)
3575 {
3576         LOG_DEBUG("aice_usb_dcache_va_inval");
3577
3578         uint32_t instructions[4];
3579
3580         aice_write_dtr(coreid, address);
3581
3582         instructions[0] = MFSR_DTR(R0);
3583         instructions[1] = L1D_VA_INVAL(R0);
3584         instructions[2] = DSB;
3585         instructions[3] = BEQ_MINUS_12;
3586
3587         return aice_execute_dim(coreid, instructions, 4);
3588 }
3589
3590 static int aice_usb_dcache_wb_all(uint32_t coreid)
3591 {
3592         LOG_DEBUG("aice_usb_dcache_wb_all");
3593
3594         uint32_t set_index;
3595         uint32_t way_index;
3596         uint32_t cache_index;
3597         uint32_t instructions[4];
3598
3599         instructions[0] = MFSR_DTR(R0);
3600         instructions[1] = L1D_IX_WB(R0);
3601         instructions[2] = DSB;
3602         instructions[3] = BEQ_MINUS_12;
3603
3604         struct cache_info *dcache = &core_info[coreid].dcache;
3605
3606         for (set_index = 0; set_index < dcache->set; set_index++) {
3607                 for (way_index = 0; way_index < dcache->way; way_index++) {
3608                         cache_index = (way_index << (dcache->log2_set + dcache->log2_line_size)) |
3609                                 (set_index << dcache->log2_line_size);
3610
3611                         if (ERROR_OK != aice_write_dtr(coreid, cache_index))
3612                                 return ERROR_FAIL;
3613
3614                         if (ERROR_OK != aice_execute_dim(coreid, instructions, 4))
3615                                 return ERROR_FAIL;
3616                 }
3617         }
3618
3619         return ERROR_OK;
3620 }
3621
3622 static int aice_usb_dcache_va_wb(uint32_t coreid, uint32_t address)
3623 {
3624         LOG_DEBUG("aice_usb_dcache_va_wb");
3625
3626         uint32_t instructions[4];
3627
3628         aice_write_dtr(coreid, address);
3629
3630         instructions[0] = MFSR_DTR(R0);
3631         instructions[1] = L1D_VA_WB(R0);
3632         instructions[2] = DSB;
3633         instructions[3] = BEQ_MINUS_12;
3634
3635         return aice_execute_dim(coreid, instructions, 4);
3636 }
3637
3638 static int aice_usb_icache_inval_all(uint32_t coreid)
3639 {
3640         LOG_DEBUG("aice_usb_icache_inval_all");
3641
3642         uint32_t set_index;
3643         uint32_t way_index;
3644         uint32_t cache_index;
3645         uint32_t instructions[4];
3646
3647         instructions[0] = MFSR_DTR(R0);
3648         instructions[1] = L1I_IX_INVAL(R0);
3649         instructions[2] = ISB;
3650         instructions[3] = BEQ_MINUS_12;
3651
3652         struct cache_info *icache = &core_info[coreid].icache;
3653
3654         for (set_index = 0; set_index < icache->set; set_index++) {
3655                 for (way_index = 0; way_index < icache->way; way_index++) {
3656                         cache_index = (way_index << (icache->log2_set + icache->log2_line_size)) |
3657                                 (set_index << icache->log2_line_size);
3658
3659                         if (ERROR_OK != aice_write_dtr(coreid, cache_index))
3660                                 return ERROR_FAIL;
3661
3662                         if (ERROR_OK != aice_execute_dim(coreid, instructions, 4))
3663                                 return ERROR_FAIL;
3664                 }
3665         }
3666
3667         return ERROR_OK;
3668 }
3669
3670 static int aice_usb_icache_va_inval(uint32_t coreid, uint32_t address)
3671 {
3672         LOG_DEBUG("aice_usb_icache_va_inval");
3673
3674         uint32_t instructions[4];
3675
3676         aice_write_dtr(coreid, address);
3677
3678         instructions[0] = MFSR_DTR(R0);
3679         instructions[1] = L1I_VA_INVAL(R0);
3680         instructions[2] = ISB;
3681         instructions[3] = BEQ_MINUS_12;
3682
3683         return aice_execute_dim(coreid, instructions, 4);
3684 }
3685
3686 static int aice_usb_cache_ctl(uint32_t coreid, uint32_t subtype, uint32_t address)
3687 {
3688         LOG_DEBUG("aice_usb_cache_ctl");
3689
3690         int result;
3691
3692         if (core_info[coreid].cache_init == false)
3693                 aice_usb_init_cache(coreid);
3694
3695         switch (subtype) {
3696                 case AICE_CACHE_CTL_L1D_INVALALL:
3697                         result = aice_usb_dcache_inval_all(coreid);
3698                         break;
3699                 case AICE_CACHE_CTL_L1D_VA_INVAL:
3700                         result = aice_usb_dcache_va_inval(coreid, address);
3701                         break;
3702                 case AICE_CACHE_CTL_L1D_WBALL:
3703                         result = aice_usb_dcache_wb_all(coreid);
3704                         break;
3705                 case AICE_CACHE_CTL_L1D_VA_WB:
3706                         result = aice_usb_dcache_va_wb(coreid, address);
3707                         break;
3708                 case AICE_CACHE_CTL_L1I_INVALALL:
3709                         result = aice_usb_icache_inval_all(coreid);
3710                         break;
3711                 case AICE_CACHE_CTL_L1I_VA_INVAL:
3712                         result = aice_usb_icache_va_inval(coreid, address);
3713                         break;
3714                 default:
3715                         result = ERROR_FAIL;
3716                         break;
3717         }
3718
3719         return result;
3720 }
3721
3722 static int aice_usb_set_retry_times(uint32_t a_retry_times)
3723 {
3724         aice_max_retry_times = a_retry_times;
3725         return ERROR_OK;
3726 }
3727
3728 static int aice_usb_program_edm(uint32_t coreid, char *command_sequence)
3729 {
3730         char *command_str;
3731         char *reg_name_0;
3732         char *reg_name_1;
3733         uint32_t data_value;
3734         int i;
3735
3736         /* init strtok() */
3737         command_str = strtok(command_sequence, ";");
3738         if (command_str == NULL)
3739                 return ERROR_OK;
3740
3741         do {
3742                 i = 0;
3743                 /* process one command */
3744                 while (command_str[i] == ' ' ||
3745                                 command_str[i] == '\n' ||
3746                                 command_str[i] == '\r' ||
3747                                 command_str[i] == '\t')
3748                         i++;
3749
3750                 /* skip ' ', '\r', '\n', '\t' */
3751                 command_str = command_str + i;
3752
3753                 if (strncmp(command_str, "write_misc", 10) == 0) {
3754                         reg_name_0 = strstr(command_str, "gen_port0");
3755                         reg_name_1 = strstr(command_str, "gen_port1");
3756
3757                         if (reg_name_0 != NULL) {
3758                                 data_value = strtoul(reg_name_0 + 9, NULL, 0);
3759
3760                                 if (aice_write_misc(coreid,
3761                                                         NDS_EDM_MISC_GEN_PORT0, data_value) != ERROR_OK)
3762                                         return ERROR_FAIL;
3763
3764                         } else if (reg_name_1 != NULL) {
3765                                 data_value = strtoul(reg_name_1 + 9, NULL, 0);
3766
3767                                 if (aice_write_misc(coreid,
3768                                                         NDS_EDM_MISC_GEN_PORT1, data_value) != ERROR_OK)
3769                                         return ERROR_FAIL;
3770                         } else {
3771                                 LOG_ERROR("program EDM, unsupported misc register: %s", command_str);
3772                         }
3773                 } else {
3774                         LOG_ERROR("program EDM, unsupported command: %s", command_str);
3775                 }
3776
3777                 /* update command_str */
3778                 command_str = strtok(NULL, ";");
3779
3780         } while (command_str != NULL);
3781
3782         return ERROR_OK;
3783 }
3784
3785 static int aice_usb_set_command_mode(enum aice_command_mode command_mode)
3786 {
3787         int retval = ERROR_OK;
3788
3789         /* flush usb_packets_buffer as users change mode */
3790         retval = aice_usb_packet_flush();
3791
3792         if (AICE_COMMAND_MODE_BATCH == command_mode) {
3793                 /* reset batch buffer */
3794                 aice_command_mode = AICE_COMMAND_MODE_NORMAL;
3795                 retval = aice_write_ctrl(AICE_WRITE_CTRL_BATCH_CMD_BUF0_CTRL, 0x40000);
3796         }
3797
3798         aice_command_mode = command_mode;
3799
3800         return retval;
3801 }
3802
3803 static int aice_usb_execute(uint32_t coreid, uint32_t *instructions,
3804                 uint32_t instruction_num)
3805 {
3806         uint32_t i, j;
3807         uint8_t current_instruction_num;
3808         uint32_t dim_instructions[4] = {NOP, NOP, NOP, BEQ_MINUS_12};
3809
3810         /* To execute 4 instructions as a special case */
3811         if (instruction_num == 4)
3812                 return aice_execute_dim(coreid, instructions, 4);
3813
3814         for (i = 0 ; i < instruction_num ; i += 3) {
3815                 if (instruction_num - i < 3) {
3816                         current_instruction_num = instruction_num - i;
3817                         for (j = current_instruction_num ; j < 3 ; j++)
3818                                 dim_instructions[j] = NOP;
3819                 } else {
3820                         current_instruction_num = 3;
3821                 }
3822
3823                 memcpy(dim_instructions, instructions + i,
3824                                 current_instruction_num * sizeof(uint32_t));
3825
3826                 /** fill DIM */
3827                 if (aice_write_dim(coreid,
3828                                         dim_instructions,
3829                                         4) != ERROR_OK)
3830                         return ERROR_FAIL;
3831
3832                 /** clear DBGER.DPED */
3833                 if (aice_write_misc(coreid,
3834                                         NDS_EDM_MISC_DBGER, NDS_DBGER_DPED) != ERROR_OK)
3835                         return ERROR_FAIL;
3836
3837                 /** execute DIM */
3838                 if (aice_do_execute(coreid) != ERROR_OK)
3839                         return ERROR_FAIL;
3840
3841                 /** check DBGER.DPED */
3842                 if (aice_check_dbger(coreid, NDS_DBGER_DPED) != ERROR_OK) {
3843
3844                         LOG_ERROR("<-- TARGET ERROR! Debug operations do not finish properly:"
3845                                         "0x%08" PRIx32 " 0x%08" PRIx32 " 0x%08" PRIx32 " 0x%08" PRIx32 ". -->",
3846                                         dim_instructions[0],
3847                                         dim_instructions[1],
3848                                         dim_instructions[2],
3849                                         dim_instructions[3]);
3850                         return ERROR_FAIL;
3851                 }
3852         }
3853
3854         return ERROR_OK;
3855 }
3856
3857 static int aice_usb_set_custom_srst_script(const char *script)
3858 {
3859         custom_srst_script = strdup(script);
3860
3861         return ERROR_OK;
3862 }
3863
3864 static int aice_usb_set_custom_trst_script(const char *script)
3865 {
3866         custom_trst_script = strdup(script);
3867
3868         return ERROR_OK;
3869 }
3870
3871 static int aice_usb_set_custom_restart_script(const char *script)
3872 {
3873         custom_restart_script = strdup(script);
3874
3875         return ERROR_OK;
3876 }
3877
3878 static int aice_usb_set_count_to_check_dbger(uint32_t count_to_check)
3879 {
3880         aice_count_to_check_dbger = count_to_check;
3881
3882         return ERROR_OK;
3883 }
3884
3885 static int aice_usb_set_data_endian(uint32_t coreid,
3886                 enum aice_target_endian target_data_endian)
3887 {
3888         data_endian = target_data_endian;
3889
3890         return ERROR_OK;
3891 }
3892
3893 static int fill_profiling_batch_commands(uint32_t coreid, uint32_t reg_no)
3894 {
3895         uint32_t dim_instructions[4];
3896
3897         aice_usb_set_command_mode(AICE_COMMAND_MODE_BATCH);
3898
3899         /* halt */
3900         if (aice_write_misc(coreid, NDS_EDM_MISC_EDM_CMDR, 0) != ERROR_OK)
3901                 return ERROR_FAIL;
3902
3903         /* backup $r0 */
3904         dim_instructions[0] = MTSR_DTR(0);
3905         dim_instructions[1] = DSB;
3906         dim_instructions[2] = NOP;
3907         dim_instructions[3] = BEQ_MINUS_12;
3908         if (aice_write_dim(coreid, dim_instructions, 4) != ERROR_OK)
3909                 return ERROR_FAIL;
3910         aice_read_dtr_to_buffer(coreid, AICE_BATCH_DATA_BUFFER_0);
3911
3912         /* get samples */
3913         if (NDS32_REG_TYPE_GPR == nds32_reg_type(reg_no)) {
3914                 /* general registers */
3915                 dim_instructions[0] = MTSR_DTR(reg_no);
3916                 dim_instructions[1] = DSB;
3917                 dim_instructions[2] = NOP;
3918                 dim_instructions[3] = BEQ_MINUS_12;
3919         } else if (NDS32_REG_TYPE_SPR == nds32_reg_type(reg_no)) {
3920                 /* user special registers */
3921                 dim_instructions[0] = MFUSR_G0(0, nds32_reg_sr_index(reg_no));
3922                 dim_instructions[1] = MTSR_DTR(0);
3923                 dim_instructions[2] = DSB;
3924                 dim_instructions[3] = BEQ_MINUS_12;
3925         } else { /* system registers */
3926                 dim_instructions[0] = MFSR(0, nds32_reg_sr_index(reg_no));
3927                 dim_instructions[1] = MTSR_DTR(0);
3928                 dim_instructions[2] = DSB;
3929                 dim_instructions[3] = BEQ_MINUS_12;
3930         }
3931         if (aice_write_dim(coreid, dim_instructions, 4) != ERROR_OK)
3932                 return ERROR_FAIL;
3933         aice_read_dtr_to_buffer(coreid, AICE_BATCH_DATA_BUFFER_1);
3934
3935         /* restore $r0 */
3936         aice_write_dtr_from_buffer(coreid, AICE_BATCH_DATA_BUFFER_0);
3937         dim_instructions[0] = MFSR_DTR(0);
3938         dim_instructions[1] = DSB;
3939         dim_instructions[2] = NOP;
3940         dim_instructions[3] = IRET;  /* free run */
3941         if (aice_write_dim(coreid, dim_instructions, 4) != ERROR_OK)
3942                 return ERROR_FAIL;
3943
3944         aice_command_mode = AICE_COMMAND_MODE_NORMAL;
3945
3946         /* use BATCH_BUFFER_WRITE to fill command-batch-buffer */
3947         if (aice_batch_buffer_write(AICE_BATCH_COMMAND_BUFFER_0,
3948                                 usb_out_packets_buffer,
3949                                 (usb_out_packets_buffer_length + 3) / 4) != ERROR_OK)
3950                 return ERROR_FAIL;
3951
3952         usb_out_packets_buffer_length = 0;
3953         usb_in_packets_buffer_length = 0;
3954
3955         return ERROR_OK;
3956 }
3957
3958 static int aice_usb_profiling(uint32_t coreid, uint32_t interval, uint32_t iteration,
3959                 uint32_t reg_no, uint32_t *samples, uint32_t *num_samples)
3960 {
3961         uint32_t iteration_count;
3962         uint32_t this_iteration;
3963         int retval = ERROR_OK;
3964         const uint32_t MAX_ITERATION = 250;
3965
3966         *num_samples = 0;
3967
3968         /* init DIM size */
3969         if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_DIM_SIZE, 4) != ERROR_OK)
3970                 return ERROR_FAIL;
3971
3972         /* Use AICE_BATCH_DATA_BUFFER_0 to read/write $DTR.
3973          * Set it to circular buffer */
3974         if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_DATA_BUF0_CTRL, 0xC0000) != ERROR_OK)
3975                 return ERROR_FAIL;
3976
3977         fill_profiling_batch_commands(coreid, reg_no);
3978
3979         iteration_count = 0;
3980         while (iteration_count < iteration) {
3981                 if (iteration - iteration_count < MAX_ITERATION)
3982                         this_iteration = iteration - iteration_count;
3983                 else
3984                         this_iteration = MAX_ITERATION;
3985
3986                 /* set number of iterations */
3987                 uint32_t val_iteration;
3988                 val_iteration = interval << 16 | this_iteration;
3989                 if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_ITERATION,
3990                                         val_iteration) != ERROR_OK) {
3991                         retval = ERROR_FAIL;
3992                         goto end_profiling;
3993                 }
3994
3995                 /* init AICE_WRITE_CTRL_BATCH_DATA_BUF1_CTRL to store $PC */
3996                 if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_DATA_BUF1_CTRL,
3997                                         0x40000) != ERROR_OK) {
3998                         retval = ERROR_FAIL;
3999                         goto end_profiling;
4000                 }
4001
4002                 aice_usb_run(coreid);
4003
4004                 /* enable BATCH command */
4005                 if (aice_write_ctrl(AICE_WRITE_CTRL_BATCH_CTRL,
4006                                         0x80000000) != ERROR_OK) {
4007                         aice_usb_halt(coreid);
4008                         retval = ERROR_FAIL;
4009                         goto end_profiling;
4010                 }
4011
4012                 /* wait a while (AICE bug, workaround) */
4013                 alive_sleep(this_iteration);
4014
4015                 /* check status */
4016                 uint32_t i;
4017                 uint32_t batch_status;
4018
4019                 i = 0;
4020                 while (1) {
4021                         aice_read_ctrl(AICE_READ_CTRL_BATCH_STATUS, &batch_status);
4022
4023                         if (batch_status & 0x1) {
4024                                 break;
4025                         } else if (batch_status & 0xE) {
4026                                 aice_usb_halt(coreid);
4027                                 retval = ERROR_FAIL;
4028                                 goto end_profiling;
4029                         }
4030
4031                         if ((i % 30) == 0)
4032                                 keep_alive();
4033
4034                         i++;
4035                 }
4036
4037                 aice_usb_halt(coreid);
4038
4039                 /* get samples from batch data buffer */
4040                 if (aice_batch_buffer_read(AICE_BATCH_DATA_BUFFER_1,
4041                                         samples + iteration_count, this_iteration) != ERROR_OK) {
4042                         retval = ERROR_FAIL;
4043                         goto end_profiling;
4044                 }
4045
4046                 iteration_count += this_iteration;
4047         }
4048
4049 end_profiling:
4050         *num_samples = iteration_count;
4051
4052         return retval;
4053 }
4054
4055 /** */
4056 struct aice_port_api_s aice_usb_api = {
4057         /** */
4058         .open = aice_open_device,
4059         /** */
4060         .close = aice_usb_close,
4061         /** */
4062         .idcode = aice_usb_idcode,
4063         /** */
4064         .state = aice_usb_state,
4065         /** */
4066         .reset = aice_usb_reset,
4067         /** */
4068         .assert_srst = aice_usb_assert_srst,
4069         /** */
4070         .run = aice_usb_run,
4071         /** */
4072         .halt = aice_usb_halt,
4073         /** */
4074         .step = aice_usb_step,
4075         /** */
4076         .read_reg = aice_usb_read_reg,
4077         /** */
4078         .write_reg = aice_usb_write_reg,
4079         /** */
4080         .read_reg_64 = aice_usb_read_reg_64,
4081         /** */
4082         .write_reg_64 = aice_usb_write_reg_64,
4083         /** */
4084         .read_mem_unit = aice_usb_read_memory_unit,
4085         /** */
4086         .write_mem_unit = aice_usb_write_memory_unit,
4087         /** */
4088         .read_mem_bulk = aice_usb_bulk_read_mem,
4089         /** */
4090         .write_mem_bulk = aice_usb_bulk_write_mem,
4091         /** */
4092         .read_debug_reg = aice_usb_read_debug_reg,
4093         /** */
4094         .write_debug_reg = aice_usb_write_debug_reg,
4095         /** */
4096         .set_jtag_clock = aice_usb_set_jtag_clock,
4097         /** */
4098         .memory_access = aice_usb_memory_access,
4099         /** */
4100         .memory_mode = aice_usb_memory_mode,
4101         /** */
4102         .read_tlb = aice_usb_read_tlb,
4103         /** */
4104         .cache_ctl = aice_usb_cache_ctl,
4105         /** */
4106         .set_retry_times = aice_usb_set_retry_times,
4107         /** */
4108         .program_edm = aice_usb_program_edm,
4109         /** */
4110         .set_command_mode = aice_usb_set_command_mode,
4111         /** */
4112         .execute = aice_usb_execute,
4113         /** */
4114         .set_custom_srst_script = aice_usb_set_custom_srst_script,
4115         /** */
4116         .set_custom_trst_script = aice_usb_set_custom_trst_script,
4117         /** */
4118         .set_custom_restart_script = aice_usb_set_custom_restart_script,
4119         /** */
4120         .set_count_to_check_dbger = aice_usb_set_count_to_check_dbger,
4121         /** */
4122         .set_data_endian = aice_usb_set_data_endian,
4123         /** */
4124         .profiling = aice_usb_profiling,
4125 };