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

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