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