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

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