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

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