git.sur5r.net Git - openocd/blob - src/jtag/aice/aice_usb.c

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