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