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

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