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

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