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

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