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