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

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