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

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