git.gag.com Git - fw/openocd/blob - src/target/aarch64.c

   1 /***************************************************************************
   2  *   Copyright (C) 2015 by David Ung                                       *
   3  *                                                                         *
   4  *   This program is free software; you can redistribute it and/or modify  *
   5  *   it under the terms of the GNU General Public License as published by  *
   6  *   the Free Software Foundation; either version 2 of the License, or     *
   7  *   (at your option) any later version.                                   *
   8  *                                                                         *
   9  *   This program is distributed in the hope that it will be useful,       *
  10  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  11  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  12  *   GNU General Public License for more details.                          *
  13  *                                                                         *
  14  *   You should have received a copy of the GNU General Public License     *
  15  *   along with this program; if not, write to the                         *
  16  *   Free Software Foundation, Inc.,                                       *
  17  *                                                                         *
  18  ***************************************************************************/
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include "breakpoints.h"
  25 #include "aarch64.h"
  26 #include "a64_disassembler.h"
  27 #include "register.h"
  28 #include "target_request.h"
  29 #include "target_type.h"
  30 #include "armv8_opcodes.h"
  31 #include "armv8_cache.h"
  32 #include "arm_semihosting.h"
  33 #include "jtag/interface.h"
  34 #include "smp.h"
  35 #include <helper/time_support.h>
  36
  37 enum restart_mode {
  38         RESTART_LAZY,
  39         RESTART_SYNC,
  40 };
  41
  42 enum halt_mode {
  43         HALT_LAZY,
  44         HALT_SYNC,
  45 };
  46
  47 struct aarch64_private_config {
  48         struct adiv5_private_config adiv5_config;
  49         struct arm_cti *cti;
  50 };
  51
  52 static int aarch64_poll(struct target *target);
  53 static int aarch64_debug_entry(struct target *target);
  54 static int aarch64_restore_context(struct target *target, bool bpwp);
  55 static int aarch64_set_breakpoint(struct target *target,
  56         struct breakpoint *breakpoint, uint8_t matchmode);
  57 static int aarch64_set_context_breakpoint(struct target *target,
  58         struct breakpoint *breakpoint, uint8_t matchmode);
  59 static int aarch64_set_hybrid_breakpoint(struct target *target,
  60         struct breakpoint *breakpoint);
  61 static int aarch64_unset_breakpoint(struct target *target,
  62         struct breakpoint *breakpoint);
  63 static int aarch64_mmu(struct target *target, int *enabled);
  64 static int aarch64_virt2phys(struct target *target,
  65         target_addr_t virt, target_addr_t *phys);
  66 static int aarch64_read_cpu_memory(struct target *target,
  67         uint64_t address, uint32_t size, uint32_t count, uint8_t *buffer);
  68
  69 static int aarch64_restore_system_control_reg(struct target *target)
  70 {
  71         enum arm_mode target_mode = ARM_MODE_ANY;
  72         int retval = ERROR_OK;
  73         uint32_t instr;
  74
  75         struct aarch64_common *aarch64 = target_to_aarch64(target);
  76         struct armv8_common *armv8 = target_to_armv8(target);
  77
  78         if (aarch64->system_control_reg != aarch64->system_control_reg_curr) {
  79                 aarch64->system_control_reg_curr = aarch64->system_control_reg;
  80                 /* LOG_INFO("cp15_control_reg: %8.8" PRIx32, cortex_v8->cp15_control_reg); */
  81
  82                 switch (armv8->arm.core_mode) {
  83                 case ARMV8_64_EL0T:
  84                         target_mode = ARMV8_64_EL1H;
  85                         /* fall through */
  86                 case ARMV8_64_EL1T:
  87                 case ARMV8_64_EL1H:
  88                         instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL1, 0);
  89                         break;
  90                 case ARMV8_64_EL2T:
  91                 case ARMV8_64_EL2H:
  92                         instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL2, 0);
  93                         break;
  94                 case ARMV8_64_EL3H:
  95                 case ARMV8_64_EL3T:
  96                         instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL3, 0);
  97                         break;
  98
  99                 case ARM_MODE_SVC:
 100                 case ARM_MODE_ABT:
 101                 case ARM_MODE_FIQ:
 102                 case ARM_MODE_IRQ:
 103                 case ARM_MODE_HYP:
 104                 case ARM_MODE_SYS:
 105                         instr = ARMV4_5_MCR(15, 0, 0, 1, 0, 0);
 106                         break;
 107
 108                 default:
 109                         LOG_ERROR("cannot read system control register in this mode: (%s : 0x%x)",
 110                                         armv8_mode_name(armv8->arm.core_mode), armv8->arm.core_mode);
 111                         return ERROR_FAIL;
 112                 }
 113
 114                 if (target_mode != ARM_MODE_ANY)
 115                         armv8_dpm_modeswitch(&armv8->dpm, target_mode);
 116
 117                 retval = armv8->dpm.instr_write_data_r0(&armv8->dpm, instr, aarch64->system_control_reg);
 118                 if (retval != ERROR_OK)
 119                         return retval;
 120
 121                 if (target_mode != ARM_MODE_ANY)
 122                         armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
 123         }
 124
 125         return retval;
 126 }
 127
 128 /*  modify system_control_reg in order to enable or disable mmu for :
 129  *  - virt2phys address conversion
 130  *  - read or write memory in phys or virt address */
 131 static int aarch64_mmu_modify(struct target *target, int enable)
 132 {
 133         struct aarch64_common *aarch64 = target_to_aarch64(target);
 134         struct armv8_common *armv8 = &aarch64->armv8_common;
 135         int retval = ERROR_OK;
 136         uint32_t instr = 0;
 137
 138         if (enable) {
 139                 /*      if mmu enabled at target stop and mmu not enable */
 140                 if (!(aarch64->system_control_reg & 0x1U)) {
 141                         LOG_ERROR("trying to enable mmu on target stopped with mmu disable");
 142                         return ERROR_FAIL;
 143                 }
 144                 if (!(aarch64->system_control_reg_curr & 0x1U))
 145                         aarch64->system_control_reg_curr |= 0x1U;
 146         } else {
 147                 if (aarch64->system_control_reg_curr & 0x4U) {
 148                         /*  data cache is active */
 149                         aarch64->system_control_reg_curr &= ~0x4U;
 150                         /* flush data cache armv8 function to be called */
 151                         if (armv8->armv8_mmu.armv8_cache.flush_all_data_cache)
 152                                 armv8->armv8_mmu.armv8_cache.flush_all_data_cache(target);
 153                 }
 154                 if ((aarch64->system_control_reg_curr & 0x1U)) {
 155                         aarch64->system_control_reg_curr &= ~0x1U;
 156                 }
 157         }
 158
 159         switch (armv8->arm.core_mode) {
 160         case ARMV8_64_EL0T:
 161         case ARMV8_64_EL1T:
 162         case ARMV8_64_EL1H:
 163                 instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL1, 0);
 164                 break;
 165         case ARMV8_64_EL2T:
 166         case ARMV8_64_EL2H:
 167                 instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL2, 0);
 168                 break;
 169         case ARMV8_64_EL3H:
 170         case ARMV8_64_EL3T:
 171                 instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL3, 0);
 172                 break;
 173
 174         case ARM_MODE_SVC:
 175         case ARM_MODE_ABT:
 176         case ARM_MODE_FIQ:
 177         case ARM_MODE_IRQ:
 178         case ARM_MODE_HYP:
 179         case ARM_MODE_SYS:
 180                 instr = ARMV4_5_MCR(15, 0, 0, 1, 0, 0);
 181                 break;
 182
 183         default:
 184                 LOG_DEBUG("unknown cpu state 0x%x", armv8->arm.core_mode);
 185                 break;
 186         }
 187
 188         retval = armv8->dpm.instr_write_data_r0(&armv8->dpm, instr,
 189                                 aarch64->system_control_reg_curr);
 190         return retval;
 191 }
 192
 193 /*
 194  * Basic debug access, very low level assumes state is saved
 195  */
 196 static int aarch64_init_debug_access(struct target *target)
 197 {
 198         struct armv8_common *armv8 = target_to_armv8(target);
 199         int retval;
 200         uint32_t dummy;
 201
 202         LOG_DEBUG("%s", target_name(target));
 203
 204         retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 205                         armv8->debug_base + CPUV8_DBG_OSLAR, 0);
 206         if (retval != ERROR_OK) {
 207                 LOG_DEBUG("Examine %s failed", "oslock");
 208                 return retval;
 209         }
 210
 211         /* Clear Sticky Power Down status Bit in PRSR to enable access to
 212            the registers in the Core Power Domain */
 213         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 214                         armv8->debug_base + CPUV8_DBG_PRSR, &dummy);
 215         if (retval != ERROR_OK)
 216                 return retval;
 217
 218         /*
 219          * Static CTI configuration:
 220          * Channel 0 -> trigger outputs HALT request to PE
 221          * Channel 1 -> trigger outputs Resume request to PE
 222          * Gate all channel trigger events from entering the CTM
 223          */
 224
 225         /* Enable CTI */
 226         retval = arm_cti_enable(armv8->cti, true);
 227         /* By default, gate all channel events to and from the CTM */
 228         if (retval == ERROR_OK)
 229                 retval = arm_cti_write_reg(armv8->cti, CTI_GATE, 0);
 230         /* output halt requests to PE on channel 0 event */
 231         if (retval == ERROR_OK)
 232                 retval = arm_cti_write_reg(armv8->cti, CTI_OUTEN0, CTI_CHNL(0));
 233         /* output restart requests to PE on channel 1 event */
 234         if (retval == ERROR_OK)
 235                 retval = arm_cti_write_reg(armv8->cti, CTI_OUTEN1, CTI_CHNL(1));
 236         if (retval != ERROR_OK)
 237                 return retval;
 238
 239         /* Resync breakpoint registers */
 240
 241         return ERROR_OK;
 242 }
 243
 244 /* Write to memory mapped registers directly with no cache or mmu handling */
 245 static int aarch64_dap_write_memap_register_u32(struct target *target,
 246         uint32_t address,
 247         uint32_t value)
 248 {
 249         int retval;
 250         struct armv8_common *armv8 = target_to_armv8(target);
 251
 252         retval = mem_ap_write_atomic_u32(armv8->debug_ap, address, value);
 253
 254         return retval;
 255 }
 256
 257 static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
 258 {
 259         struct arm_dpm *dpm = &a8->armv8_common.dpm;
 260         int retval;
 261
 262         dpm->arm = &a8->armv8_common.arm;
 263         dpm->didr = debug;
 264
 265         retval = armv8_dpm_setup(dpm);
 266         if (retval == ERROR_OK)
 267                 retval = armv8_dpm_initialize(dpm);
 268
 269         return retval;
 270 }
 271
 272 static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
 273 {
 274         struct armv8_common *armv8 = target_to_armv8(target);
 275         return armv8_set_dbgreg_bits(armv8, CPUV8_DBG_DSCR, bit_mask, value);
 276 }
 277
 278 static int aarch64_check_state_one(struct target *target,
 279                 uint32_t mask, uint32_t val, int *p_result, uint32_t *p_prsr)
 280 {
 281         struct armv8_common *armv8 = target_to_armv8(target);
 282         uint32_t prsr;
 283         int retval;
 284
 285         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 286                         armv8->debug_base + CPUV8_DBG_PRSR, &prsr);
 287         if (retval != ERROR_OK)
 288                 return retval;
 289
 290         if (p_prsr)
 291                 *p_prsr = prsr;
 292
 293         if (p_result)
 294                 *p_result = (prsr & mask) == (val & mask);
 295
 296         return ERROR_OK;
 297 }
 298
 299 static int aarch64_wait_halt_one(struct target *target)
 300 {
 301         int retval = ERROR_OK;
 302         uint32_t prsr;
 303
 304         int64_t then = timeval_ms();
 305         for (;;) {
 306                 int halted;
 307
 308                 retval = aarch64_check_state_one(target, PRSR_HALT, PRSR_HALT, &halted, &prsr);
 309                 if (retval != ERROR_OK || halted)
 310                         break;
 311
 312                 if (timeval_ms() > then + 1000) {
 313                         retval = ERROR_TARGET_TIMEOUT;
 314                         LOG_DEBUG("target %s timeout, prsr=0x%08"PRIx32, target_name(target), prsr);
 315                         break;
 316                 }
 317         }
 318         return retval;
 319 }
 320
 321 static int aarch64_prepare_halt_smp(struct target *target, bool exc_target, struct target **p_first)
 322 {
 323         int retval = ERROR_OK;
 324         struct target_list *head = target->head;
 325         struct target *first = NULL;
 326
 327         LOG_DEBUG("target %s exc %i", target_name(target), exc_target);
 328
 329         while (head != NULL) {
 330                 struct target *curr = head->target;
 331                 struct armv8_common *armv8 = target_to_armv8(curr);
 332                 head = head->next;
 333
 334                 if (exc_target && curr == target)
 335                         continue;
 336                 if (!target_was_examined(curr))
 337                         continue;
 338                 if (curr->state != TARGET_RUNNING)
 339                         continue;
 340
 341                 /* HACK: mark this target as prepared for halting */
 342                 curr->debug_reason = DBG_REASON_DBGRQ;
 343
 344                 /* open the gate for channel 0 to let HALT requests pass to the CTM */
 345                 retval = arm_cti_ungate_channel(armv8->cti, 0);
 346                 if (retval == ERROR_OK)
 347                         retval = aarch64_set_dscr_bits(curr, DSCR_HDE, DSCR_HDE);
 348                 if (retval != ERROR_OK)
 349                         break;
 350
 351                 LOG_DEBUG("target %s prepared", target_name(curr));
 352
 353                 if (first == NULL)
 354                         first = curr;
 355         }
 356
 357         if (p_first) {
 358                 if (exc_target && first)
 359                         *p_first = first;
 360                 else
 361                         *p_first = target;
 362         }
 363
 364         return retval;
 365 }
 366
 367 static int aarch64_halt_one(struct target *target, enum halt_mode mode)
 368 {
 369         int retval = ERROR_OK;
 370         struct armv8_common *armv8 = target_to_armv8(target);
 371
 372         LOG_DEBUG("%s", target_name(target));
 373
 374         /* allow Halting Debug Mode */
 375         retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
 376         if (retval != ERROR_OK)
 377                 return retval;
 378
 379         /* trigger an event on channel 0, this outputs a halt request to the PE */
 380         retval = arm_cti_pulse_channel(armv8->cti, 0);
 381         if (retval != ERROR_OK)
 382                 return retval;
 383
 384         if (mode == HALT_SYNC) {
 385                 retval = aarch64_wait_halt_one(target);
 386                 if (retval != ERROR_OK) {
 387                         if (retval == ERROR_TARGET_TIMEOUT)
 388                                 LOG_ERROR("Timeout waiting for target %s halt", target_name(target));
 389                         return retval;
 390                 }
 391         }
 392
 393         return ERROR_OK;
 394 }
 395
 396 static int aarch64_halt_smp(struct target *target, bool exc_target)
 397 {
 398         struct target *next = target;
 399         int retval;
 400
 401         /* prepare halt on all PEs of the group */
 402         retval = aarch64_prepare_halt_smp(target, exc_target, &next);
 403
 404         if (exc_target && next == target)
 405                 return retval;
 406
 407         /* halt the target PE */
 408         if (retval == ERROR_OK)
 409                 retval = aarch64_halt_one(next, HALT_LAZY);
 410
 411         if (retval != ERROR_OK)
 412                 return retval;
 413
 414         /* wait for all PEs to halt */
 415         int64_t then = timeval_ms();
 416         for (;;) {
 417                 bool all_halted = true;
 418                 struct target_list *head;
 419                 struct target *curr;
 420
 421                 foreach_smp_target(head, target->head) {
 422                         int halted;
 423
 424                         curr = head->target;
 425
 426                         if (!target_was_examined(curr))
 427                                 continue;
 428
 429                         retval = aarch64_check_state_one(curr, PRSR_HALT, PRSR_HALT, &halted, NULL);
 430                         if (retval != ERROR_OK || !halted) {
 431                                 all_halted = false;
 432                                 break;
 433                         }
 434                 }
 435
 436                 if (all_halted)
 437                         break;
 438
 439                 if (timeval_ms() > then + 1000) {
 440                         retval = ERROR_TARGET_TIMEOUT;
 441                         break;
 442                 }
 443
 444                 /*
 445                  * HACK: on Hi6220 there are 8 cores organized in 2 clusters
 446                  * and it looks like the CTI's are not connected by a common
 447                  * trigger matrix. It seems that we need to halt one core in each
 448                  * cluster explicitly. So if we find that a core has not halted
 449                  * yet, we trigger an explicit halt for the second cluster.
 450                  */
 451                 retval = aarch64_halt_one(curr, HALT_LAZY);
 452                 if (retval != ERROR_OK)
 453                         break;
 454         }
 455
 456         return retval;
 457 }
 458
 459 static int update_halt_gdb(struct target *target, enum target_debug_reason debug_reason)
 460 {
 461         struct target *gdb_target = NULL;
 462         struct target_list *head;
 463         struct target *curr;
 464
 465         if (debug_reason == DBG_REASON_NOTHALTED) {
 466                 LOG_DEBUG("Halting remaining targets in SMP group");
 467                 aarch64_halt_smp(target, true);
 468         }
 469
 470         /* poll all targets in the group, but skip the target that serves GDB */
 471         foreach_smp_target(head, target->head) {
 472                 curr = head->target;
 473                 /* skip calling context */
 474                 if (curr == target)
 475                         continue;
 476                 if (!target_was_examined(curr))
 477                         continue;
 478                 /* skip targets that were already halted */
 479                 if (curr->state == TARGET_HALTED)
 480                         continue;
 481                 /* remember the gdb_service->target */
 482                 if (curr->gdb_service != NULL)
 483                         gdb_target = curr->gdb_service->target;
 484                 /* skip it */
 485                 if (curr == gdb_target)
 486                         continue;
 487
 488                 /* avoid recursion in aarch64_poll() */
 489                 curr->smp = 0;
 490                 aarch64_poll(curr);
 491                 curr->smp = 1;
 492         }
 493
 494         /* after all targets were updated, poll the gdb serving target */
 495         if (gdb_target != NULL && gdb_target != target)
 496                 aarch64_poll(gdb_target);
 497
 498         return ERROR_OK;
 499 }
 500
 501 /*
 502  * Aarch64 Run control
 503  */
 504
 505 static int aarch64_poll(struct target *target)
 506 {
 507         enum target_state prev_target_state;
 508         int retval = ERROR_OK;
 509         int halted;
 510
 511         retval = aarch64_check_state_one(target,
 512                                 PRSR_HALT, PRSR_HALT, &halted, NULL);
 513         if (retval != ERROR_OK)
 514                 return retval;
 515
 516         if (halted) {
 517                 prev_target_state = target->state;
 518                 if (prev_target_state != TARGET_HALTED) {
 519                         enum target_debug_reason debug_reason = target->debug_reason;
 520
 521                         /* We have a halting debug event */
 522                         target->state = TARGET_HALTED;
 523                         LOG_DEBUG("Target %s halted", target_name(target));
 524                         retval = aarch64_debug_entry(target);
 525                         if (retval != ERROR_OK)
 526                                 return retval;
 527
 528                         if (target->smp)
 529                                 update_halt_gdb(target, debug_reason);
 530
 531                         if (arm_semihosting(target, &retval) != 0)
 532                                 return retval;
 533
 534                         switch (prev_target_state) {
 535                         case TARGET_RUNNING:
 536                         case TARGET_UNKNOWN:
 537                         case TARGET_RESET:
 538                                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 539                                 break;
 540                         case TARGET_DEBUG_RUNNING:
 541                                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 542                                 break;
 543                         default:
 544                                 break;
 545                         }
 546                 }
 547         } else
 548                 target->state = TARGET_RUNNING;
 549
 550         return retval;
 551 }
 552
 553 static int aarch64_halt(struct target *target)
 554 {
 555         struct armv8_common *armv8 = target_to_armv8(target);
 556         armv8->last_run_control_op = ARMV8_RUNCONTROL_HALT;
 557
 558         if (target->smp)
 559                 return aarch64_halt_smp(target, false);
 560
 561         return aarch64_halt_one(target, HALT_SYNC);
 562 }
 563
 564 static int aarch64_restore_one(struct target *target, int current,
 565         uint64_t *address, int handle_breakpoints, int debug_execution)
 566 {
 567         struct armv8_common *armv8 = target_to_armv8(target);
 568         struct arm *arm = &armv8->arm;
 569         int retval;
 570         uint64_t resume_pc;
 571
 572         LOG_DEBUG("%s", target_name(target));
 573
 574         if (!debug_execution)
 575                 target_free_all_working_areas(target);
 576
 577         /* current = 1: continue on current pc, otherwise continue at <address> */
 578         resume_pc = buf_get_u64(arm->pc->value, 0, 64);
 579         if (!current)
 580                 resume_pc = *address;
 581         else
 582                 *address = resume_pc;
 583
 584         /* Make sure that the Armv7 gdb thumb fixups does not
 585          * kill the return address
 586          */
 587         switch (arm->core_state) {
 588                 case ARM_STATE_ARM:
 589                         resume_pc &= 0xFFFFFFFC;
 590                         break;
 591                 case ARM_STATE_AARCH64:
 592                         resume_pc &= 0xFFFFFFFFFFFFFFFC;
 593                         break;
 594                 case ARM_STATE_THUMB:
 595                 case ARM_STATE_THUMB_EE:
 596                         /* When the return address is loaded into PC
 597                          * bit 0 must be 1 to stay in Thumb state
 598                          */
 599                         resume_pc |= 0x1;
 600                         break;
 601                 case ARM_STATE_JAZELLE:
 602                         LOG_ERROR("How do I resume into Jazelle state??");
 603                         return ERROR_FAIL;
 604         }
 605         LOG_DEBUG("resume pc = 0x%016" PRIx64, resume_pc);
 606         buf_set_u64(arm->pc->value, 0, 64, resume_pc);
 607         arm->pc->dirty = true;
 608         arm->pc->valid = true;
 609
 610         /* called it now before restoring context because it uses cpu
 611          * register r0 for restoring system control register */
 612         retval = aarch64_restore_system_control_reg(target);
 613         if (retval == ERROR_OK)
 614                 retval = aarch64_restore_context(target, handle_breakpoints);
 615
 616         return retval;
 617 }
 618
 619 /**
 620  * prepare single target for restart
 621  *
 622  *
 623  */
 624 static int aarch64_prepare_restart_one(struct target *target)
 625 {
 626         struct armv8_common *armv8 = target_to_armv8(target);
 627         int retval;
 628         uint32_t dscr;
 629         uint32_t tmp;
 630
 631         LOG_DEBUG("%s", target_name(target));
 632
 633         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 634                         armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 635         if (retval != ERROR_OK)
 636                 return retval;
 637
 638         if ((dscr & DSCR_ITE) == 0)
 639                 LOG_ERROR("DSCR.ITE must be set before leaving debug!");
 640         if ((dscr & DSCR_ERR) != 0)
 641                 LOG_ERROR("DSCR.ERR must be cleared before leaving debug!");
 642
 643         /* acknowledge a pending CTI halt event */
 644         retval = arm_cti_ack_events(armv8->cti, CTI_TRIG(HALT));
 645         /*
 646          * open the CTI gate for channel 1 so that the restart events
 647          * get passed along to all PEs. Also close gate for channel 0
 648          * to isolate the PE from halt events.
 649          */
 650         if (retval == ERROR_OK)
 651                 retval = arm_cti_ungate_channel(armv8->cti, 1);
 652         if (retval == ERROR_OK)
 653                 retval = arm_cti_gate_channel(armv8->cti, 0);
 654
 655         /* make sure that DSCR.HDE is set */
 656         if (retval == ERROR_OK) {
 657                 dscr |= DSCR_HDE;
 658                 retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 659                                 armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 660         }
 661
 662         if (retval == ERROR_OK) {
 663                 /* clear sticky bits in PRSR, SDR is now 0 */
 664                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 665                                 armv8->debug_base + CPUV8_DBG_PRSR, &tmp);
 666         }
 667
 668         return retval;
 669 }
 670
 671 static int aarch64_do_restart_one(struct target *target, enum restart_mode mode)
 672 {
 673         struct armv8_common *armv8 = target_to_armv8(target);
 674         int retval;
 675
 676         LOG_DEBUG("%s", target_name(target));
 677
 678         /* trigger an event on channel 1, generates a restart request to the PE */
 679         retval = arm_cti_pulse_channel(armv8->cti, 1);
 680         if (retval != ERROR_OK)
 681                 return retval;
 682
 683         if (mode == RESTART_SYNC) {
 684                 int64_t then = timeval_ms();
 685                 for (;;) {
 686                         int resumed;
 687                         /*
 688                          * if PRSR.SDR is set now, the target did restart, even
 689                          * if it's now already halted again (e.g. due to breakpoint)
 690                          */
 691                         retval = aarch64_check_state_one(target,
 692                                                 PRSR_SDR, PRSR_SDR, &resumed, NULL);
 693                         if (retval != ERROR_OK || resumed)
 694                                 break;
 695
 696                         if (timeval_ms() > then + 1000) {
 697                                 LOG_ERROR("%s: Timeout waiting for resume"PRIx32, target_name(target));
 698                                 retval = ERROR_TARGET_TIMEOUT;
 699                                 break;
 700                         }
 701                 }
 702         }
 703
 704         if (retval != ERROR_OK)
 705                 return retval;
 706
 707         target->debug_reason = DBG_REASON_NOTHALTED;
 708         target->state = TARGET_RUNNING;
 709
 710         return ERROR_OK;
 711 }
 712
 713 static int aarch64_restart_one(struct target *target, enum restart_mode mode)
 714 {
 715         int retval;
 716
 717         LOG_DEBUG("%s", target_name(target));
 718
 719         retval = aarch64_prepare_restart_one(target);
 720         if (retval == ERROR_OK)
 721                 retval = aarch64_do_restart_one(target, mode);
 722
 723         return retval;
 724 }
 725
 726 /*
 727  * prepare all but the current target for restart
 728  */
 729 static int aarch64_prep_restart_smp(struct target *target, int handle_breakpoints, struct target **p_first)
 730 {
 731         int retval = ERROR_OK;
 732         struct target_list *head;
 733         struct target *first = NULL;
 734         uint64_t address;
 735
 736         foreach_smp_target(head, target->head) {
 737                 struct target *curr = head->target;
 738
 739                 /* skip calling target */
 740                 if (curr == target)
 741                         continue;
 742                 if (!target_was_examined(curr))
 743                         continue;
 744                 if (curr->state != TARGET_HALTED)
 745                         continue;
 746
 747                 /*  resume at current address, not in step mode */
 748                 retval = aarch64_restore_one(curr, 1, &address, handle_breakpoints, 0);
 749                 if (retval == ERROR_OK)
 750                         retval = aarch64_prepare_restart_one(curr);
 751                 if (retval != ERROR_OK) {
 752                         LOG_ERROR("failed to restore target %s", target_name(curr));
 753                         break;
 754                 }
 755                 /* remember the first valid target in the group */
 756                 if (first == NULL)
 757                         first = curr;
 758         }
 759
 760         if (p_first)
 761                 *p_first = first;
 762
 763         return retval;
 764 }
 765
 766
 767 static int aarch64_step_restart_smp(struct target *target)
 768 {
 769         int retval = ERROR_OK;
 770         struct target_list *head;
 771         struct target *first = NULL;
 772
 773         LOG_DEBUG("%s", target_name(target));
 774
 775         retval = aarch64_prep_restart_smp(target, 0, &first);
 776         if (retval != ERROR_OK)
 777                 return retval;
 778
 779         if (first != NULL)
 780                 retval = aarch64_do_restart_one(first, RESTART_LAZY);
 781         if (retval != ERROR_OK) {
 782                 LOG_DEBUG("error restarting target %s", target_name(first));
 783                 return retval;
 784         }
 785
 786         int64_t then = timeval_ms();
 787         for (;;) {
 788                 struct target *curr = target;
 789                 bool all_resumed = true;
 790
 791                 foreach_smp_target(head, target->head) {
 792                         uint32_t prsr;
 793                         int resumed;
 794
 795                         curr = head->target;
 796
 797                         if (curr == target)
 798                                 continue;
 799
 800                         if (!target_was_examined(curr))
 801                                 continue;
 802
 803                         retval = aarch64_check_state_one(curr,
 804                                         PRSR_SDR, PRSR_SDR, &resumed, &prsr);
 805                         if (retval != ERROR_OK || (!resumed && (prsr & PRSR_HALT))) {
 806                                 all_resumed = false;
 807                                 break;
 808                         }
 809
 810                         if (curr->state != TARGET_RUNNING) {
 811                                 curr->state = TARGET_RUNNING;
 812                                 curr->debug_reason = DBG_REASON_NOTHALTED;
 813                                 target_call_event_callbacks(curr, TARGET_EVENT_RESUMED);
 814                         }
 815                 }
 816
 817                 if (all_resumed)
 818                         break;
 819
 820                 if (timeval_ms() > then + 1000) {
 821                         LOG_ERROR("%s: timeout waiting for target resume", __func__);
 822                         retval = ERROR_TARGET_TIMEOUT;
 823                         break;
 824                 }
 825                 /*
 826                  * HACK: on Hi6220 there are 8 cores organized in 2 clusters
 827                  * and it looks like the CTI's are not connected by a common
 828                  * trigger matrix. It seems that we need to halt one core in each
 829                  * cluster explicitly. So if we find that a core has not halted
 830                  * yet, we trigger an explicit resume for the second cluster.
 831                  */
 832                 retval = aarch64_do_restart_one(curr, RESTART_LAZY);
 833                 if (retval != ERROR_OK)
 834                         break;
 835 }
 836
 837         return retval;
 838 }
 839
 840 static int aarch64_resume(struct target *target, int current,
 841         target_addr_t address, int handle_breakpoints, int debug_execution)
 842 {
 843         int retval = 0;
 844         uint64_t addr = address;
 845
 846         struct armv8_common *armv8 = target_to_armv8(target);
 847         armv8->last_run_control_op = ARMV8_RUNCONTROL_RESUME;
 848
 849         if (target->state != TARGET_HALTED)
 850                 return ERROR_TARGET_NOT_HALTED;
 851
 852         /*
 853          * If this target is part of a SMP group, prepare the others
 854          * targets for resuming. This involves restoring the complete
 855          * target register context and setting up CTI gates to accept
 856          * resume events from the trigger matrix.
 857          */
 858         if (target->smp) {
 859                 retval = aarch64_prep_restart_smp(target, handle_breakpoints, NULL);
 860                 if (retval != ERROR_OK)
 861                         return retval;
 862         }
 863
 864         /* all targets prepared, restore and restart the current target */
 865         retval = aarch64_restore_one(target, current, &addr, handle_breakpoints,
 866                                  debug_execution);
 867         if (retval == ERROR_OK)
 868                 retval = aarch64_restart_one(target, RESTART_SYNC);
 869         if (retval != ERROR_OK)
 870                 return retval;
 871
 872         if (target->smp) {
 873                 int64_t then = timeval_ms();
 874                 for (;;) {
 875                         struct target *curr = target;
 876                         struct target_list *head;
 877                         bool all_resumed = true;
 878
 879                         foreach_smp_target(head, target->head) {
 880                                 uint32_t prsr;
 881                                 int resumed;
 882
 883                                 curr = head->target;
 884                                 if (curr == target)
 885                                         continue;
 886                                 if (!target_was_examined(curr))
 887                                         continue;
 888
 889                                 retval = aarch64_check_state_one(curr,
 890                                                 PRSR_SDR, PRSR_SDR, &resumed, &prsr);
 891                                 if (retval != ERROR_OK || (!resumed && (prsr & PRSR_HALT))) {
 892                                         all_resumed = false;
 893                                         break;
 894                                 }
 895
 896                                 if (curr->state != TARGET_RUNNING) {
 897                                         curr->state = TARGET_RUNNING;
 898                                         curr->debug_reason = DBG_REASON_NOTHALTED;
 899                                         target_call_event_callbacks(curr, TARGET_EVENT_RESUMED);
 900                                 }
 901                         }
 902
 903                         if (all_resumed)
 904                                 break;
 905
 906                         if (timeval_ms() > then + 1000) {
 907                                 LOG_ERROR("%s: timeout waiting for target %s to resume", __func__, target_name(curr));
 908                                 retval = ERROR_TARGET_TIMEOUT;
 909                                 break;
 910                         }
 911
 912                         /*
 913                          * HACK: on Hi6220 there are 8 cores organized in 2 clusters
 914                          * and it looks like the CTI's are not connected by a common
 915                          * trigger matrix. It seems that we need to halt one core in each
 916                          * cluster explicitly. So if we find that a core has not halted
 917                          * yet, we trigger an explicit resume for the second cluster.
 918                          */
 919                         retval = aarch64_do_restart_one(curr, RESTART_LAZY);
 920                         if (retval != ERROR_OK)
 921                                 break;
 922                 }
 923         }
 924
 925         if (retval != ERROR_OK)
 926                 return retval;
 927
 928         target->debug_reason = DBG_REASON_NOTHALTED;
 929
 930         if (!debug_execution) {
 931                 target->state = TARGET_RUNNING;
 932                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 933                 LOG_DEBUG("target resumed at 0x%" PRIx64, addr);
 934         } else {
 935                 target->state = TARGET_DEBUG_RUNNING;
 936                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
 937                 LOG_DEBUG("target debug resumed at 0x%" PRIx64, addr);
 938         }
 939
 940         return ERROR_OK;
 941 }
 942
 943 static int aarch64_debug_entry(struct target *target)
 944 {
 945         int retval = ERROR_OK;
 946         struct armv8_common *armv8 = target_to_armv8(target);
 947         struct arm_dpm *dpm = &armv8->dpm;
 948         enum arm_state core_state;
 949         uint32_t dscr;
 950
 951         /* make sure to clear all sticky errors */
 952         retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 953                         armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 954         if (retval == ERROR_OK)
 955                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 956                                 armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 957         if (retval == ERROR_OK)
 958                 retval = arm_cti_ack_events(armv8->cti, CTI_TRIG(HALT));
 959
 960         if (retval != ERROR_OK)
 961                 return retval;
 962
 963         LOG_DEBUG("%s dscr = 0x%08" PRIx32, target_name(target), dscr);
 964
 965         dpm->dscr = dscr;
 966         core_state = armv8_dpm_get_core_state(dpm);
 967         armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
 968         armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
 969
 970         /* close the CTI gate for all events */
 971         if (retval == ERROR_OK)
 972                 retval = arm_cti_write_reg(armv8->cti, CTI_GATE, 0);
 973         /* discard async exceptions */
 974         if (retval == ERROR_OK)
 975                 retval = dpm->instr_cpsr_sync(dpm);
 976         if (retval != ERROR_OK)
 977                 return retval;
 978
 979         /* Examine debug reason */
 980         armv8_dpm_report_dscr(dpm, dscr);
 981
 982         /* save address of instruction that triggered the watchpoint? */
 983         if (target->debug_reason == DBG_REASON_WATCHPOINT) {
 984                 uint32_t tmp;
 985                 uint64_t wfar = 0;
 986
 987                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 988                                 armv8->debug_base + CPUV8_DBG_WFAR1,
 989                                 &tmp);
 990                 if (retval != ERROR_OK)
 991                         return retval;
 992                 wfar = tmp;
 993                 wfar = (wfar << 32);
 994                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 995                                 armv8->debug_base + CPUV8_DBG_WFAR0,
 996                                 &tmp);
 997                 if (retval != ERROR_OK)
 998                         return retval;
 999                 wfar |= tmp;
1000                 armv8_dpm_report_wfar(&armv8->dpm, wfar);
1001         }
1002
1003         retval = armv8_dpm_read_current_registers(&armv8->dpm);
1004
1005         if (retval == ERROR_OK && armv8->post_debug_entry)
1006                 retval = armv8->post_debug_entry(target);
1007
1008         return retval;
1009 }
1010
1011 static int aarch64_post_debug_entry(struct target *target)
1012 {
1013         struct aarch64_common *aarch64 = target_to_aarch64(target);
1014         struct armv8_common *armv8 = &aarch64->armv8_common;
1015         int retval;
1016         enum arm_mode target_mode = ARM_MODE_ANY;
1017         uint32_t instr;
1018
1019         switch (armv8->arm.core_mode) {
1020         case ARMV8_64_EL0T:
1021                 target_mode = ARMV8_64_EL1H;
1022                 /* fall through */
1023         case ARMV8_64_EL1T:
1024         case ARMV8_64_EL1H:
1025                 instr = ARMV8_MRS(SYSTEM_SCTLR_EL1, 0);
1026                 break;
1027         case ARMV8_64_EL2T:
1028         case ARMV8_64_EL2H:
1029                 instr = ARMV8_MRS(SYSTEM_SCTLR_EL2, 0);
1030                 break;
1031         case ARMV8_64_EL3H:
1032         case ARMV8_64_EL3T:
1033                 instr = ARMV8_MRS(SYSTEM_SCTLR_EL3, 0);
1034                 break;
1035
1036         case ARM_MODE_SVC:
1037         case ARM_MODE_ABT:
1038         case ARM_MODE_FIQ:
1039         case ARM_MODE_IRQ:
1040         case ARM_MODE_HYP:
1041         case ARM_MODE_SYS:
1042                 instr = ARMV4_5_MRC(15, 0, 0, 1, 0, 0);
1043                 break;
1044
1045         default:
1046                 LOG_ERROR("cannot read system control register in this mode: (%s : 0x%x)",
1047                                 armv8_mode_name(armv8->arm.core_mode), armv8->arm.core_mode);
1048                 return ERROR_FAIL;
1049         }
1050
1051         if (target_mode != ARM_MODE_ANY)
1052                 armv8_dpm_modeswitch(&armv8->dpm, target_mode);
1053
1054         retval = armv8->dpm.instr_read_data_r0(&armv8->dpm, instr, &aarch64->system_control_reg);
1055         if (retval != ERROR_OK)
1056                 return retval;
1057
1058         if (target_mode != ARM_MODE_ANY)
1059                 armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
1060
1061         LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
1062         aarch64->system_control_reg_curr = aarch64->system_control_reg;
1063
1064         if (armv8->armv8_mmu.armv8_cache.info == -1) {
1065                 armv8_identify_cache(armv8);
1066                 armv8_read_mpidr(armv8);
1067         }
1068
1069         armv8->armv8_mmu.mmu_enabled =
1070                         (aarch64->system_control_reg & 0x1U) ? 1 : 0;
1071         armv8->armv8_mmu.armv8_cache.d_u_cache_enabled =
1072                 (aarch64->system_control_reg & 0x4U) ? 1 : 0;
1073         armv8->armv8_mmu.armv8_cache.i_cache_enabled =
1074                 (aarch64->system_control_reg & 0x1000U) ? 1 : 0;
1075         return ERROR_OK;
1076 }
1077
1078 /*
1079  * single-step a target
1080  */
1081 static int aarch64_step(struct target *target, int current, target_addr_t address,
1082         int handle_breakpoints)
1083 {
1084         struct armv8_common *armv8 = target_to_armv8(target);
1085         struct aarch64_common *aarch64 = target_to_aarch64(target);
1086         int saved_retval = ERROR_OK;
1087         int retval;
1088         uint32_t edecr;
1089
1090         armv8->last_run_control_op = ARMV8_RUNCONTROL_STEP;
1091
1092         if (target->state != TARGET_HALTED) {
1093                 LOG_WARNING("target not halted");
1094                 return ERROR_TARGET_NOT_HALTED;
1095         }
1096
1097         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
1098                         armv8->debug_base + CPUV8_DBG_EDECR, &edecr);
1099         /* make sure EDECR.SS is not set when restoring the register */
1100
1101         if (retval == ERROR_OK) {
1102                 edecr &= ~0x4;
1103                 /* set EDECR.SS to enter hardware step mode */
1104                 retval = mem_ap_write_atomic_u32(armv8->debug_ap,
1105                                 armv8->debug_base + CPUV8_DBG_EDECR, (edecr|0x4));
1106         }
1107         /* disable interrupts while stepping */
1108         if (retval == ERROR_OK && aarch64->isrmasking_mode == AARCH64_ISRMASK_ON)
1109                 retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0x3 << 22);
1110         /* bail out if stepping setup has failed */
1111         if (retval != ERROR_OK)
1112                 return retval;
1113
1114         if (target->smp && (current == 1)) {
1115                 /*
1116                  * isolate current target so that it doesn't get resumed
1117                  * together with the others
1118                  */
1119                 retval = arm_cti_gate_channel(armv8->cti, 1);
1120                 /* resume all other targets in the group */
1121                 if (retval == ERROR_OK)
1122                         retval = aarch64_step_restart_smp(target);
1123                 if (retval != ERROR_OK) {
1124                         LOG_ERROR("Failed to restart non-stepping targets in SMP group");
1125                         return retval;
1126                 }
1127                 LOG_DEBUG("Restarted all non-stepping targets in SMP group");
1128         }
1129
1130         /* all other targets running, restore and restart the current target */
1131         retval = aarch64_restore_one(target, current, &address, 0, 0);
1132         if (retval == ERROR_OK)
1133                 retval = aarch64_restart_one(target, RESTART_LAZY);
1134
1135         if (retval != ERROR_OK)
1136                 return retval;
1137
1138         LOG_DEBUG("target step-resumed at 0x%" PRIx64, address);
1139         if (!handle_breakpoints)
1140                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
1141
1142         int64_t then = timeval_ms();
1143         for (;;) {
1144                 int stepped;
1145                 uint32_t prsr;
1146
1147                 retval = aarch64_check_state_one(target,
1148                                         PRSR_SDR|PRSR_HALT, PRSR_SDR|PRSR_HALT, &stepped, &prsr);
1149                 if (retval != ERROR_OK || stepped)
1150                         break;
1151
1152                 if (timeval_ms() > then + 100) {
1153                         LOG_ERROR("timeout waiting for target %s halt after step",
1154                                         target_name(target));
1155                         retval = ERROR_TARGET_TIMEOUT;
1156                         break;
1157                 }
1158         }
1159
1160         /*
1161          * At least on one SoC (Renesas R8A7795) stepping over a WFI instruction
1162          * causes a timeout. The core takes the step but doesn't complete it and so
1163          * debug state is never entered. However, you can manually halt the core
1164          * as an external debug even is also a WFI wakeup event.
1165          */
1166         if (retval == ERROR_TARGET_TIMEOUT)
1167                 saved_retval = aarch64_halt_one(target, HALT_SYNC);
1168
1169         /* restore EDECR */
1170         retval = mem_ap_write_atomic_u32(armv8->debug_ap,
1171                         armv8->debug_base + CPUV8_DBG_EDECR, edecr);
1172         if (retval != ERROR_OK)
1173                 return retval;
1174
1175         /* restore interrupts */
1176         if (aarch64->isrmasking_mode == AARCH64_ISRMASK_ON) {
1177                 retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0);
1178                 if (retval != ERROR_OK)
1179                         return ERROR_OK;
1180         }
1181
1182         if (saved_retval != ERROR_OK)
1183                 return saved_retval;
1184
1185         return ERROR_OK;
1186 }
1187
1188 static int aarch64_restore_context(struct target *target, bool bpwp)
1189 {
1190         struct armv8_common *armv8 = target_to_armv8(target);
1191         struct arm *arm = &armv8->arm;
1192
1193         int retval;
1194
1195         LOG_DEBUG("%s", target_name(target));
1196
1197         if (armv8->pre_restore_context)
1198                 armv8->pre_restore_context(target);
1199
1200         retval = armv8_dpm_write_dirty_registers(&armv8->dpm, bpwp);
1201         if (retval == ERROR_OK) {
1202                 /* registers are now invalid */
1203                 register_cache_invalidate(arm->core_cache);
1204                 register_cache_invalidate(arm->core_cache->next);
1205         }
1206
1207         return retval;
1208 }
1209
1210 /*
1211  * Cortex-A8 Breakpoint and watchpoint functions
1212  */
1213
1214 /* Setup hardware Breakpoint Register Pair */
1215 static int aarch64_set_breakpoint(struct target *target,
1216         struct breakpoint *breakpoint, uint8_t matchmode)
1217 {
1218         int retval;
1219         int brp_i = 0;
1220         uint32_t control;
1221         uint8_t byte_addr_select = 0x0F;
1222         struct aarch64_common *aarch64 = target_to_aarch64(target);
1223         struct armv8_common *armv8 = &aarch64->armv8_common;
1224         struct aarch64_brp *brp_list = aarch64->brp_list;
1225
1226         if (breakpoint->set) {
1227                 LOG_WARNING("breakpoint already set");
1228                 return ERROR_OK;
1229         }
1230
1231         if (breakpoint->type == BKPT_HARD) {
1232                 int64_t bpt_value;
1233                 while (brp_list[brp_i].used && (brp_i < aarch64->brp_num))
1234                         brp_i++;
1235                 if (brp_i >= aarch64->brp_num) {
1236                         LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
1237                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1238                 }
1239                 breakpoint->set = brp_i + 1;
1240                 if (breakpoint->length == 2)
1241                         byte_addr_select = (3 << (breakpoint->address & 0x02));
1242                 control = ((matchmode & 0x7) << 20)
1243                         | (1 << 13)
1244                         | (byte_addr_select << 5)
1245                         | (3 << 1) | 1;
1246                 brp_list[brp_i].used = 1;
1247                 brp_list[brp_i].value = breakpoint->address & 0xFFFFFFFFFFFFFFFC;
1248                 brp_list[brp_i].control = control;
1249                 bpt_value = brp_list[brp_i].value;
1250
1251                 retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1252                                 + CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
1253                                 (uint32_t)(bpt_value & 0xFFFFFFFF));
1254                 if (retval != ERROR_OK)
1255                         return retval;
1256                 retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1257                                 + CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
1258                                 (uint32_t)(bpt_value >> 32));
1259                 if (retval != ERROR_OK)
1260                         return retval;
1261
1262                 retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1263                                 + CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
1264                                 brp_list[brp_i].control);
1265                 if (retval != ERROR_OK)
1266                         return retval;
1267                 LOG_DEBUG("brp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
1268                         brp_list[brp_i].control,
1269                         brp_list[brp_i].value);
1270
1271         } else if (breakpoint->type == BKPT_SOFT) {
1272                 uint32_t opcode;
1273                 uint8_t code[4];
1274
1275                 if (armv8_dpm_get_core_state(&armv8->dpm) == ARM_STATE_AARCH64) {
1276                         opcode = ARMV8_HLT(11);
1277
1278                         if (breakpoint->length != 4)
1279                                 LOG_ERROR("bug: breakpoint length should be 4 in AArch64 mode");
1280                 } else {
1281                         /**
1282                          * core_state is ARM_STATE_ARM
1283                          * in that case the opcode depends on breakpoint length:
1284                          *  - if length == 4 => A32 opcode
1285                          *  - if length == 2 => T32 opcode
1286                          *  - if length == 3 => T32 opcode (refer to gdb doc : ARM-Breakpoint-Kinds)
1287                          *    in that case the length should be changed from 3 to 4 bytes
1288                          **/
1289                         opcode = (breakpoint->length == 4) ? ARMV8_HLT_A1(11) :
1290                                         (uint32_t) (ARMV8_HLT_T1(11) | ARMV8_HLT_T1(11) << 16);
1291
1292                         if (breakpoint->length == 3)
1293                                 breakpoint->length = 4;
1294                 }
1295
1296                 buf_set_u32(code, 0, 32, opcode);
1297
1298                 retval = target_read_memory(target,
1299                                 breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1300                                 breakpoint->length, 1,
1301                                 breakpoint->orig_instr);
1302                 if (retval != ERROR_OK)
1303                         return retval;
1304
1305                 armv8_cache_d_inner_flush_virt(armv8,
1306                                 breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1307                                 breakpoint->length);
1308
1309                 retval = target_write_memory(target,
1310                                 breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1311                                 breakpoint->length, 1, code);
1312                 if (retval != ERROR_OK)
1313                         return retval;
1314
1315                 armv8_cache_d_inner_flush_virt(armv8,
1316                                 breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1317                                 breakpoint->length);
1318
1319                 armv8_cache_i_inner_inval_virt(armv8,
1320                                 breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1321                                 breakpoint->length);
1322
1323                 breakpoint->set = 0x11; /* Any nice value but 0 */
1324         }
1325
1326         /* Ensure that halting debug mode is enable */
1327         retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
1328         if (retval != ERROR_OK) {
1329                 LOG_DEBUG("Failed to set DSCR.HDE");
1330                 return retval;
1331         }
1332
1333         return ERROR_OK;
1334 }
1335
1336 static int aarch64_set_context_breakpoint(struct target *target,
1337         struct breakpoint *breakpoint, uint8_t matchmode)
1338 {
1339         int retval = ERROR_FAIL;
1340         int brp_i = 0;
1341         uint32_t control;
1342         uint8_t byte_addr_select = 0x0F;
1343         struct aarch64_common *aarch64 = target_to_aarch64(target);
1344         struct armv8_common *armv8 = &aarch64->armv8_common;
1345         struct aarch64_brp *brp_list = aarch64->brp_list;
1346
1347         if (breakpoint->set) {
1348                 LOG_WARNING("breakpoint already set");
1349                 return retval;
1350         }
1351         /*check available context BRPs*/
1352         while ((brp_list[brp_i].used ||
1353                 (brp_list[brp_i].type != BRP_CONTEXT)) && (brp_i < aarch64->brp_num))
1354                 brp_i++;
1355
1356         if (brp_i >= aarch64->brp_num) {
1357                 LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
1358                 return ERROR_FAIL;
1359         }
1360
1361         breakpoint->set = brp_i + 1;
1362         control = ((matchmode & 0x7) << 20)
1363                 | (1 << 13)
1364                 | (byte_addr_select << 5)
1365                 | (3 << 1) | 1;
1366         brp_list[brp_i].used = 1;
1367         brp_list[brp_i].value = (breakpoint->asid);
1368         brp_list[brp_i].control = control;
1369         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1370                         + CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
1371                         brp_list[brp_i].value);
1372         if (retval != ERROR_OK)
1373                 return retval;
1374         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1375                         + CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
1376                         brp_list[brp_i].control);
1377         if (retval != ERROR_OK)
1378                 return retval;
1379         LOG_DEBUG("brp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
1380                 brp_list[brp_i].control,
1381                 brp_list[brp_i].value);
1382         return ERROR_OK;
1383
1384 }
1385
1386 static int aarch64_set_hybrid_breakpoint(struct target *target, struct breakpoint *breakpoint)
1387 {
1388         int retval = ERROR_FAIL;
1389         int brp_1 = 0;  /* holds the contextID pair */
1390         int brp_2 = 0;  /* holds the IVA pair */
1391         uint32_t control_CTX, control_IVA;
1392         uint8_t CTX_byte_addr_select = 0x0F;
1393         uint8_t IVA_byte_addr_select = 0x0F;
1394         uint8_t CTX_machmode = 0x03;
1395         uint8_t IVA_machmode = 0x01;
1396         struct aarch64_common *aarch64 = target_to_aarch64(target);
1397         struct armv8_common *armv8 = &aarch64->armv8_common;
1398         struct aarch64_brp *brp_list = aarch64->brp_list;
1399
1400         if (breakpoint->set) {
1401                 LOG_WARNING("breakpoint already set");
1402                 return retval;
1403         }
1404         /*check available context BRPs*/
1405         while ((brp_list[brp_1].used ||
1406                 (brp_list[brp_1].type != BRP_CONTEXT)) && (brp_1 < aarch64->brp_num))
1407                 brp_1++;
1408
1409         printf("brp(CTX) found num: %d\n", brp_1);
1410         if (brp_1 >= aarch64->brp_num) {
1411                 LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
1412                 return ERROR_FAIL;
1413         }
1414
1415         while ((brp_list[brp_2].used ||
1416                 (brp_list[brp_2].type != BRP_NORMAL)) && (brp_2 < aarch64->brp_num))
1417                 brp_2++;
1418
1419         printf("brp(IVA) found num: %d\n", brp_2);
1420         if (brp_2 >= aarch64->brp_num) {
1421                 LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
1422                 return ERROR_FAIL;
1423         }
1424
1425         breakpoint->set = brp_1 + 1;
1426         breakpoint->linked_BRP = brp_2;
1427         control_CTX = ((CTX_machmode & 0x7) << 20)
1428                 | (brp_2 << 16)
1429                 | (0 << 14)
1430                 | (CTX_byte_addr_select << 5)
1431                 | (3 << 1) | 1;
1432         brp_list[brp_1].used = 1;
1433         brp_list[brp_1].value = (breakpoint->asid);
1434         brp_list[brp_1].control = control_CTX;
1435         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1436                         + CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_1].BRPn,
1437                         brp_list[brp_1].value);
1438         if (retval != ERROR_OK)
1439                 return retval;
1440         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1441                         + CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_1].BRPn,
1442                         brp_list[brp_1].control);
1443         if (retval != ERROR_OK)
1444                 return retval;
1445
1446         control_IVA = ((IVA_machmode & 0x7) << 20)
1447                 | (brp_1 << 16)
1448                 | (1 << 13)
1449                 | (IVA_byte_addr_select << 5)
1450                 | (3 << 1) | 1;
1451         brp_list[brp_2].used = 1;
1452         brp_list[brp_2].value = breakpoint->address & 0xFFFFFFFFFFFFFFFC;
1453         brp_list[brp_2].control = control_IVA;
1454         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1455                         + CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_2].BRPn,
1456                         brp_list[brp_2].value & 0xFFFFFFFF);
1457         if (retval != ERROR_OK)
1458                 return retval;
1459         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1460                         + CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_2].BRPn,
1461                         brp_list[brp_2].value >> 32);
1462         if (retval != ERROR_OK)
1463                 return retval;
1464         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1465                         + CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_2].BRPn,
1466                         brp_list[brp_2].control);
1467         if (retval != ERROR_OK)
1468                 return retval;
1469
1470         return ERROR_OK;
1471 }
1472
1473 static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
1474 {
1475         int retval;
1476         struct aarch64_common *aarch64 = target_to_aarch64(target);
1477         struct armv8_common *armv8 = &aarch64->armv8_common;
1478         struct aarch64_brp *brp_list = aarch64->brp_list;
1479
1480         if (!breakpoint->set) {
1481                 LOG_WARNING("breakpoint not set");
1482                 return ERROR_OK;
1483         }
1484
1485         if (breakpoint->type == BKPT_HARD) {
1486                 if ((breakpoint->address != 0) && (breakpoint->asid != 0)) {
1487                         int brp_i = breakpoint->set - 1;
1488                         int brp_j = breakpoint->linked_BRP;
1489                         if ((brp_i < 0) || (brp_i >= aarch64->brp_num)) {
1490                                 LOG_DEBUG("Invalid BRP number in breakpoint");
1491                                 return ERROR_OK;
1492                         }
1493                         LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
1494                                 brp_list[brp_i].control, brp_list[brp_i].value);
1495                         brp_list[brp_i].used = 0;
1496                         brp_list[brp_i].value = 0;
1497                         brp_list[brp_i].control = 0;
1498                         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1499                                         + CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
1500                                         brp_list[brp_i].control);
1501                         if (retval != ERROR_OK)
1502                                 return retval;
1503                         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1504                                         + CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
1505                                         (uint32_t)brp_list[brp_i].value);
1506                         if (retval != ERROR_OK)
1507                                 return retval;
1508                         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1509                                         + CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
1510                                         (uint32_t)brp_list[brp_i].value);
1511                         if (retval != ERROR_OK)
1512                                 return retval;
1513                         if ((brp_j < 0) || (brp_j >= aarch64->brp_num)) {
1514                                 LOG_DEBUG("Invalid BRP number in breakpoint");
1515                                 return ERROR_OK;
1516                         }
1517                         LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%0" PRIx64, brp_j,
1518                                 brp_list[brp_j].control, brp_list[brp_j].value);
1519                         brp_list[brp_j].used = 0;
1520                         brp_list[brp_j].value = 0;
1521                         brp_list[brp_j].control = 0;
1522                         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1523                                         + CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_j].BRPn,
1524                                         brp_list[brp_j].control);
1525                         if (retval != ERROR_OK)
1526                                 return retval;
1527                         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1528                                         + CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_j].BRPn,
1529                                         (uint32_t)brp_list[brp_j].value);
1530                         if (retval != ERROR_OK)
1531                                 return retval;
1532                         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1533                                         + CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_j].BRPn,
1534                                         (uint32_t)brp_list[brp_j].value);
1535                         if (retval != ERROR_OK)
1536                                 return retval;
1537
1538                         breakpoint->linked_BRP = 0;
1539                         breakpoint->set = 0;
1540                         return ERROR_OK;
1541
1542                 } else {
1543                         int brp_i = breakpoint->set - 1;
1544                         if ((brp_i < 0) || (brp_i >= aarch64->brp_num)) {
1545                                 LOG_DEBUG("Invalid BRP number in breakpoint");
1546                                 return ERROR_OK;
1547                         }
1548                         LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%0" PRIx64, brp_i,
1549                                 brp_list[brp_i].control, brp_list[brp_i].value);
1550                         brp_list[brp_i].used = 0;
1551                         brp_list[brp_i].value = 0;
1552                         brp_list[brp_i].control = 0;
1553                         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1554                                         + CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
1555                                         brp_list[brp_i].control);
1556                         if (retval != ERROR_OK)
1557                                 return retval;
1558                         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1559                                         + CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
1560                                         brp_list[brp_i].value);
1561                         if (retval != ERROR_OK)
1562                                 return retval;
1563
1564                         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1565                                         + CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
1566                                         (uint32_t)brp_list[brp_i].value);
1567                         if (retval != ERROR_OK)
1568                                 return retval;
1569                         breakpoint->set = 0;
1570                         return ERROR_OK;
1571                 }
1572         } else {
1573                 /* restore original instruction (kept in target endianness) */
1574
1575                 armv8_cache_d_inner_flush_virt(armv8,
1576                                 breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1577                                 breakpoint->length);
1578
1579                 if (breakpoint->length == 4) {
1580                         retval = target_write_memory(target,
1581                                         breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1582                                         4, 1, breakpoint->orig_instr);
1583                         if (retval != ERROR_OK)
1584                                 return retval;
1585                 } else {
1586                         retval = target_write_memory(target,
1587                                         breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1588                                         2, 1, breakpoint->orig_instr);
1589                         if (retval != ERROR_OK)
1590                                 return retval;
1591                 }
1592
1593                 armv8_cache_d_inner_flush_virt(armv8,
1594                                 breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1595                                 breakpoint->length);
1596
1597                 armv8_cache_i_inner_inval_virt(armv8,
1598                                 breakpoint->address & 0xFFFFFFFFFFFFFFFE,
1599                                 breakpoint->length);
1600         }
1601         breakpoint->set = 0;
1602
1603         return ERROR_OK;
1604 }
1605
1606 static int aarch64_add_breakpoint(struct target *target,
1607         struct breakpoint *breakpoint)
1608 {
1609         struct aarch64_common *aarch64 = target_to_aarch64(target);
1610
1611         if ((breakpoint->type == BKPT_HARD) && (aarch64->brp_num_available < 1)) {
1612                 LOG_INFO("no hardware breakpoint available");
1613                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1614         }
1615
1616         if (breakpoint->type == BKPT_HARD)
1617                 aarch64->brp_num_available--;
1618
1619         return aarch64_set_breakpoint(target, breakpoint, 0x00);        /* Exact match */
1620 }
1621
1622 static int aarch64_add_context_breakpoint(struct target *target,
1623         struct breakpoint *breakpoint)
1624 {
1625         struct aarch64_common *aarch64 = target_to_aarch64(target);
1626
1627         if ((breakpoint->type == BKPT_HARD) && (aarch64->brp_num_available < 1)) {
1628                 LOG_INFO("no hardware breakpoint available");
1629                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1630         }
1631
1632         if (breakpoint->type == BKPT_HARD)
1633                 aarch64->brp_num_available--;
1634
1635         return aarch64_set_context_breakpoint(target, breakpoint, 0x02);        /* asid match */
1636 }
1637
1638 static int aarch64_add_hybrid_breakpoint(struct target *target,
1639         struct breakpoint *breakpoint)
1640 {
1641         struct aarch64_common *aarch64 = target_to_aarch64(target);
1642
1643         if ((breakpoint->type == BKPT_HARD) && (aarch64->brp_num_available < 1)) {
1644                 LOG_INFO("no hardware breakpoint available");
1645                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1646         }
1647
1648         if (breakpoint->type == BKPT_HARD)
1649                 aarch64->brp_num_available--;
1650
1651         return aarch64_set_hybrid_breakpoint(target, breakpoint);       /* ??? */
1652 }
1653
1654 static int aarch64_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
1655 {
1656         struct aarch64_common *aarch64 = target_to_aarch64(target);
1657
1658 #if 0
1659 /* It is perfectly possible to remove breakpoints while the target is running */
1660         if (target->state != TARGET_HALTED) {
1661                 LOG_WARNING("target not halted");
1662                 return ERROR_TARGET_NOT_HALTED;
1663         }
1664 #endif
1665
1666         if (breakpoint->set) {
1667                 aarch64_unset_breakpoint(target, breakpoint);
1668                 if (breakpoint->type == BKPT_HARD)
1669                         aarch64->brp_num_available++;
1670         }
1671
1672         return ERROR_OK;
1673 }
1674
1675 /* Setup hardware Watchpoint Register Pair */
1676 static int aarch64_set_watchpoint(struct target *target,
1677         struct watchpoint *watchpoint)
1678 {
1679         int retval;
1680         int wp_i = 0;
1681         uint32_t control, offset, length;
1682         struct aarch64_common *aarch64 = target_to_aarch64(target);
1683         struct armv8_common *armv8 = &aarch64->armv8_common;
1684         struct aarch64_brp *wp_list = aarch64->wp_list;
1685
1686         if (watchpoint->set) {
1687                 LOG_WARNING("watchpoint already set");
1688                 return ERROR_OK;
1689         }
1690
1691         while (wp_list[wp_i].used && (wp_i < aarch64->wp_num))
1692                 wp_i++;
1693         if (wp_i >= aarch64->wp_num) {
1694                 LOG_ERROR("ERROR Can not find free Watchpoint Register Pair");
1695                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1696         }
1697
1698         control = (1 << 0)      /* enable */
1699                 | (3 << 1)      /* both user and privileged access */
1700                 | (1 << 13);    /* higher mode control */
1701
1702         switch (watchpoint->rw) {
1703         case WPT_READ:
1704                 control |= 1 << 3;
1705                 break;
1706         case WPT_WRITE:
1707                 control |= 2 << 3;
1708                 break;
1709         case WPT_ACCESS:
1710                 control |= 3 << 3;
1711                 break;
1712         }
1713
1714         /* Match up to 8 bytes. */
1715         offset = watchpoint->address & 7;
1716         length = watchpoint->length;
1717         if (offset + length > sizeof(uint64_t)) {
1718                 length = sizeof(uint64_t) - offset;
1719                 LOG_WARNING("Adjust watchpoint match inside 8-byte boundary");
1720         }
1721         for (; length > 0; offset++, length--)
1722                 control |= (1 << offset) << 5;
1723
1724         wp_list[wp_i].value = watchpoint->address & 0xFFFFFFFFFFFFFFF8ULL;
1725         wp_list[wp_i].control = control;
1726
1727         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1728                         + CPUV8_DBG_WVR_BASE + 16 * wp_list[wp_i].BRPn,
1729                         (uint32_t)(wp_list[wp_i].value & 0xFFFFFFFF));
1730         if (retval != ERROR_OK)
1731                 return retval;
1732         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1733                         + CPUV8_DBG_WVR_BASE + 4 + 16 * wp_list[wp_i].BRPn,
1734                         (uint32_t)(wp_list[wp_i].value >> 32));
1735         if (retval != ERROR_OK)
1736                 return retval;
1737
1738         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1739                         + CPUV8_DBG_WCR_BASE + 16 * wp_list[wp_i].BRPn,
1740                         control);
1741         if (retval != ERROR_OK)
1742                 return retval;
1743         LOG_DEBUG("wp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, wp_i,
1744                 wp_list[wp_i].control, wp_list[wp_i].value);
1745
1746         /* Ensure that halting debug mode is enable */
1747         retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
1748         if (retval != ERROR_OK) {
1749                 LOG_DEBUG("Failed to set DSCR.HDE");
1750                 return retval;
1751         }
1752
1753         wp_list[wp_i].used = 1;
1754         watchpoint->set = wp_i + 1;
1755
1756         return ERROR_OK;
1757 }
1758
1759 /* Clear hardware Watchpoint Register Pair */
1760 static int aarch64_unset_watchpoint(struct target *target,
1761         struct watchpoint *watchpoint)
1762 {
1763         int retval, wp_i;
1764         struct aarch64_common *aarch64 = target_to_aarch64(target);
1765         struct armv8_common *armv8 = &aarch64->armv8_common;
1766         struct aarch64_brp *wp_list = aarch64->wp_list;
1767
1768         if (!watchpoint->set) {
1769                 LOG_WARNING("watchpoint not set");
1770                 return ERROR_OK;
1771         }
1772
1773         wp_i = watchpoint->set - 1;
1774         if ((wp_i < 0) || (wp_i >= aarch64->wp_num)) {
1775                 LOG_DEBUG("Invalid WP number in watchpoint");
1776                 return ERROR_OK;
1777         }
1778         LOG_DEBUG("rwp %i control 0x%0" PRIx32 " value 0x%0" PRIx64, wp_i,
1779                 wp_list[wp_i].control, wp_list[wp_i].value);
1780         wp_list[wp_i].used = 0;
1781         wp_list[wp_i].value = 0;
1782         wp_list[wp_i].control = 0;
1783         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1784                         + CPUV8_DBG_WCR_BASE + 16 * wp_list[wp_i].BRPn,
1785                         wp_list[wp_i].control);
1786         if (retval != ERROR_OK)
1787                 return retval;
1788         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1789                         + CPUV8_DBG_WVR_BASE + 16 * wp_list[wp_i].BRPn,
1790                         wp_list[wp_i].value);
1791         if (retval != ERROR_OK)
1792                 return retval;
1793
1794         retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
1795                         + CPUV8_DBG_WVR_BASE + 4 + 16 * wp_list[wp_i].BRPn,
1796                         (uint32_t)wp_list[wp_i].value);
1797         if (retval != ERROR_OK)
1798                 return retval;
1799         watchpoint->set = 0;
1800
1801         return ERROR_OK;
1802 }
1803
1804 static int aarch64_add_watchpoint(struct target *target,
1805         struct watchpoint *watchpoint)
1806 {
1807         int retval;
1808         struct aarch64_common *aarch64 = target_to_aarch64(target);
1809
1810         if (aarch64->wp_num_available < 1) {
1811                 LOG_INFO("no hardware watchpoint available");
1812                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1813         }
1814
1815         retval = aarch64_set_watchpoint(target, watchpoint);
1816         if (retval == ERROR_OK)
1817                 aarch64->wp_num_available--;
1818
1819         return retval;
1820 }
1821
1822 static int aarch64_remove_watchpoint(struct target *target,
1823         struct watchpoint *watchpoint)
1824 {
1825         struct aarch64_common *aarch64 = target_to_aarch64(target);
1826
1827         if (watchpoint->set) {
1828                 aarch64_unset_watchpoint(target, watchpoint);
1829                 aarch64->wp_num_available++;
1830         }
1831
1832         return ERROR_OK;
1833 }
1834
1835 /**
1836  * find out which watchpoint hits
1837  * get exception address and compare the address to watchpoints
1838  */
1839 int aarch64_hit_watchpoint(struct target *target,
1840         struct watchpoint **hit_watchpoint)
1841 {
1842         if (target->debug_reason != DBG_REASON_WATCHPOINT)
1843                 return ERROR_FAIL;
1844
1845         struct armv8_common *armv8 = target_to_armv8(target);
1846
1847         uint64_t exception_address;
1848         struct watchpoint *wp;
1849
1850         exception_address = armv8->dpm.wp_pc;
1851
1852         if (exception_address == 0xFFFFFFFF)
1853                 return ERROR_FAIL;
1854
1855         /**********************************************************/
1856         /* see if a watchpoint address matches a value read from  */
1857         /* the EDWAR register. Testing shows that on some ARM CPUs*/
1858         /* the EDWAR value needs to have 8 added to it so we add  */
1859         /* that check as well not sure if that is a core bug)     */
1860         /**********************************************************/
1861         for (exception_address = armv8->dpm.wp_pc; exception_address <= (armv8->dpm.wp_pc + 8);
1862                 exception_address += 8) {
1863                 for (wp = target->watchpoints; wp; wp = wp->next) {
1864                         if ((exception_address >= wp->address) && (exception_address < (wp->address + wp->length))) {
1865                                 *hit_watchpoint = wp;
1866                                 if (exception_address != armv8->dpm.wp_pc)
1867                                         LOG_DEBUG("watchpoint hit required EDWAR to be increased by 8");
1868                                 return ERROR_OK;
1869                         }
1870                 }
1871         }
1872
1873         return ERROR_FAIL;
1874 }
1875
1876 /*
1877  * Cortex-A8 Reset functions
1878  */
1879
1880 static int aarch64_enable_reset_catch(struct target *target, bool enable)
1881 {
1882         struct armv8_common *armv8 = target_to_armv8(target);
1883         uint32_t edecr;
1884         int retval;
1885
1886         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
1887                         armv8->debug_base + CPUV8_DBG_EDECR, &edecr);
1888         LOG_DEBUG("EDECR = 0x%08" PRIx32 ", enable=%d", edecr, enable);
1889         if (retval != ERROR_OK)
1890                 return retval;
1891
1892         if (enable)
1893                 edecr |= ECR_RCE;
1894         else
1895                 edecr &= ~ECR_RCE;
1896
1897         return mem_ap_write_atomic_u32(armv8->debug_ap,
1898                         armv8->debug_base + CPUV8_DBG_EDECR, edecr);
1899 }
1900
1901 static int aarch64_clear_reset_catch(struct target *target)
1902 {
1903         struct armv8_common *armv8 = target_to_armv8(target);
1904         uint32_t edesr;
1905         int retval;
1906         bool was_triggered;
1907
1908         /* check if Reset Catch debug event triggered as expected */
1909         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
1910                 armv8->debug_base + CPUV8_DBG_EDESR, &edesr);
1911         if (retval != ERROR_OK)
1912                 return retval;
1913
1914         was_triggered = !!(edesr & ESR_RC);
1915         LOG_DEBUG("Reset Catch debug event %s",
1916                         was_triggered ? "triggered" : "NOT triggered!");
1917
1918         if (was_triggered) {
1919                 /* clear pending Reset Catch debug event */
1920                 edesr &= ~ESR_RC;
1921                 retval = mem_ap_write_atomic_u32(armv8->debug_ap,
1922                         armv8->debug_base + CPUV8_DBG_EDESR, edesr);
1923                 if (retval != ERROR_OK)
1924                         return retval;
1925         }
1926
1927         return ERROR_OK;
1928 }
1929
1930 static int aarch64_assert_reset(struct target *target)
1931 {
1932         struct armv8_common *armv8 = target_to_armv8(target);
1933         enum reset_types reset_config = jtag_get_reset_config();
1934         int retval;
1935
1936         LOG_DEBUG(" ");
1937
1938         /* Issue some kind of warm reset. */
1939         if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT))
1940                 target_handle_event(target, TARGET_EVENT_RESET_ASSERT);
1941         else if (reset_config & RESET_HAS_SRST) {
1942                 bool srst_asserted = false;
1943
1944                 if (target->reset_halt) {
1945                         if (target_was_examined(target)) {
1946
1947                                 if (reset_config & RESET_SRST_NO_GATING) {
1948                                         /*
1949                                          * SRST needs to be asserted *before* Reset Catch
1950                                          * debug event can be set up.
1951                                          */
1952                                         adapter_assert_reset();
1953                                         srst_asserted = true;
1954
1955                                         /* make sure to clear all sticky errors */
1956                                         mem_ap_write_atomic_u32(armv8->debug_ap,
1957                                                         armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
1958                                 }
1959
1960                                 /* set up Reset Catch debug event to halt the CPU after reset */
1961                                 retval = aarch64_enable_reset_catch(target, true);
1962                                 if (retval != ERROR_OK)
1963                                         LOG_WARNING("%s: Error enabling Reset Catch debug event; the CPU will not halt immediately after reset!",
1964                                                         target_name(target));
1965                         } else {
1966                                 LOG_WARNING("%s: Target not examined, will not halt immediately after reset!",
1967                                                 target_name(target));
1968                         }
1969                 }
1970
1971                 /* REVISIT handle "pulls" cases, if there's
1972                  * hardware that needs them to work.
1973                  */
1974                 if (!srst_asserted)
1975                         adapter_assert_reset();
1976         } else {
1977                 LOG_ERROR("%s: how to reset?", target_name(target));
1978                 return ERROR_FAIL;
1979         }
1980
1981         /* registers are now invalid */
1982         if (target_was_examined(target)) {
1983                 register_cache_invalidate(armv8->arm.core_cache);
1984                 register_cache_invalidate(armv8->arm.core_cache->next);
1985         }
1986
1987         target->state = TARGET_RESET;
1988
1989         return ERROR_OK;
1990 }
1991
1992 static int aarch64_deassert_reset(struct target *target)
1993 {
1994         int retval;
1995
1996         LOG_DEBUG(" ");
1997
1998         /* be certain SRST is off */
1999         adapter_deassert_reset();
2000
2001         if (!target_was_examined(target))
2002                 return ERROR_OK;
2003
2004         retval = aarch64_init_debug_access(target);
2005         if (retval != ERROR_OK)
2006                 return retval;
2007
2008         retval = aarch64_poll(target);
2009         if (retval != ERROR_OK)
2010                 return retval;
2011
2012         if (target->reset_halt) {
2013                 /* clear pending Reset Catch debug event */
2014                 retval = aarch64_clear_reset_catch(target);
2015                 if (retval != ERROR_OK)
2016                         LOG_WARNING("%s: Clearing Reset Catch debug event failed",
2017                                         target_name(target));
2018
2019                 /* disable Reset Catch debug event */
2020                 retval = aarch64_enable_reset_catch(target, false);
2021                 if (retval != ERROR_OK)
2022                         LOG_WARNING("%s: Disabling Reset Catch debug event failed",
2023                                         target_name(target));
2024
2025                 if (target->state != TARGET_HALTED) {
2026                         LOG_WARNING("%s: ran after reset and before halt ...",
2027                                 target_name(target));
2028                         retval = target_halt(target);
2029                         if (retval != ERROR_OK)
2030                                 return retval;
2031                 }
2032         }
2033
2034         return ERROR_OK;
2035 }
2036
2037 static int aarch64_write_cpu_memory_slow(struct target *target,
2038         uint32_t size, uint32_t count, const uint8_t *buffer, uint32_t *dscr)
2039 {
2040         struct armv8_common *armv8 = target_to_armv8(target);
2041         struct arm_dpm *dpm = &armv8->dpm;
2042         struct arm *arm = &armv8->arm;
2043         int retval;
2044
2045         armv8_reg_current(arm, 1)->dirty = true;
2046
2047         /* change DCC to normal mode if necessary */
2048         if (*dscr & DSCR_MA) {
2049                 *dscr &= ~DSCR_MA;
2050                 retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
2051                                 armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
2052                 if (retval != ERROR_OK)
2053                         return retval;
2054         }
2055
2056         while (count) {
2057                 uint32_t data, opcode;
2058
2059                 /* write the data to store into DTRRX */
2060                 if (size == 1)
2061                         data = *buffer;
2062                 else if (size == 2)
2063                         data = target_buffer_get_u16(target, buffer);
2064                 else
2065                         data = target_buffer_get_u32(target, buffer);
2066                 retval = mem_ap_write_atomic_u32(armv8->debug_ap,
2067                                 armv8->debug_base + CPUV8_DBG_DTRRX, data);
2068                 if (retval != ERROR_OK)
2069                         return retval;
2070
2071                 if (arm->core_state == ARM_STATE_AARCH64)
2072                         retval = dpm->instr_execute(dpm, ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 1));
2073                 else
2074                         retval = dpm->instr_execute(dpm, ARMV4_5_MRC(14, 0, 1, 0, 5, 0));
2075                 if (retval != ERROR_OK)
2076                         return retval;
2077
2078                 if (size == 1)
2079                         opcode = armv8_opcode(armv8, ARMV8_OPC_STRB_IP);
2080                 else if (size == 2)
2081                         opcode = armv8_opcode(armv8, ARMV8_OPC_STRH_IP);
2082                 else
2083                         opcode = armv8_opcode(armv8, ARMV8_OPC_STRW_IP);
2084                 retval = dpm->instr_execute(dpm, opcode);
2085                 if (retval != ERROR_OK)
2086                         return retval;
2087
2088                 /* Advance */
2089                 buffer += size;
2090                 --count;
2091         }
2092
2093         return ERROR_OK;
2094 }
2095
2096 static int aarch64_write_cpu_memory_fast(struct target *target,
2097         uint32_t count, const uint8_t *buffer, uint32_t *dscr)
2098 {
2099         struct armv8_common *armv8 = target_to_armv8(target);
2100         struct arm *arm = &armv8->arm;
2101         int retval;
2102
2103         armv8_reg_current(arm, 1)->dirty = true;
2104
2105         /* Step 1.d   - Change DCC to memory mode */
2106         *dscr |= DSCR_MA;
2107         retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
2108                         armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
2109         if (retval != ERROR_OK)
2110                 return retval;
2111
2112
2113         /* Step 2.a   - Do the write */
2114         retval = mem_ap_write_buf_noincr(armv8->debug_ap,
2115                                         buffer, 4, count, armv8->debug_base + CPUV8_DBG_DTRRX);
2116         if (retval != ERROR_OK)
2117                 return retval;
2118
2119         /* Step 3.a   - Switch DTR mode back to Normal mode */
2120         *dscr &= ~DSCR_MA;
2121         retval = mem_ap_write_atomic_u32(armv8->debug_ap,
2122                                 armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
2123         if (retval != ERROR_OK)
2124                 return retval;
2125
2126         return ERROR_OK;
2127 }
2128
2129 static int aarch64_write_cpu_memory(struct target *target,
2130         uint64_t address, uint32_t size,
2131         uint32_t count, const uint8_t *buffer)
2132 {
2133         /* write memory through APB-AP */
2134         int retval = ERROR_COMMAND_SYNTAX_ERROR;
2135         struct armv8_common *armv8 = target_to_armv8(target);
2136         struct arm_dpm *dpm = &armv8->dpm;
2137         struct arm *arm = &armv8->arm;
2138         uint32_t dscr;
2139
2140         if (target->state != TARGET_HALTED) {
2141                 LOG_WARNING("target not halted");
2142                 return ERROR_TARGET_NOT_HALTED;
2143         }
2144
2145         /* Mark register X0 as dirty, as it will be used
2146          * for transferring the data.
2147          * It will be restored automatically when exiting
2148          * debug mode
2149          */
2150         armv8_reg_current(arm, 0)->dirty = true;
2151
2152         /* This algorithm comes from DDI0487A.g, chapter J9.1 */
2153
2154         /* Read DSCR */
2155         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2156                         armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
2157         if (retval != ERROR_OK)
2158                 return retval;
2159
2160         /* Set Normal access mode  */
2161         dscr = (dscr & ~DSCR_MA);
2162         retval = mem_ap_write_atomic_u32(armv8->debug_ap,
2163                         armv8->debug_base + CPUV8_DBG_DSCR, dscr);
2164         if (retval != ERROR_OK)
2165                 return retval;
2166
2167         if (arm->core_state == ARM_STATE_AARCH64) {
2168                 /* Write X0 with value 'address' using write procedure */
2169                 /* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
2170                 /* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
2171                 retval = dpm->instr_write_data_dcc_64(dpm,
2172                                 ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address);
2173         } else {
2174                 /* Write R0 with value 'address' using write procedure */
2175                 /* Step 1.a+b - Write the address for read access into DBGDTRRX */
2176                 /* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
2177                 retval = dpm->instr_write_data_dcc(dpm,
2178                                 ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address);
2179         }
2180
2181         if (retval != ERROR_OK)
2182                 return retval;
2183
2184         if (size == 4 && (address % 4) == 0)
2185                 retval = aarch64_write_cpu_memory_fast(target, count, buffer, &dscr);
2186         else
2187                 retval = aarch64_write_cpu_memory_slow(target, size, count, buffer, &dscr);
2188
2189         if (retval != ERROR_OK) {
2190                 /* Unset DTR mode */
2191                 mem_ap_read_atomic_u32(armv8->debug_ap,
2192                                         armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
2193                 dscr &= ~DSCR_MA;
2194                 mem_ap_write_atomic_u32(armv8->debug_ap,
2195                                         armv8->debug_base + CPUV8_DBG_DSCR, dscr);
2196         }
2197
2198         /* Check for sticky abort flags in the DSCR */
2199         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2200                                 armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
2201         if (retval != ERROR_OK)
2202                 return retval;
2203
2204         dpm->dscr = dscr;
2205         if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
2206                 /* Abort occurred - clear it and exit */
2207                 LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
2208                 armv8_dpm_handle_exception(dpm, true);
2209                 return ERROR_FAIL;
2210         }
2211
2212         /* Done */
2213         return ERROR_OK;
2214 }
2215
2216 static int aarch64_read_cpu_memory_slow(struct target *target,
2217         uint32_t size, uint32_t count, uint8_t *buffer, uint32_t *dscr)
2218 {
2219         struct armv8_common *armv8 = target_to_armv8(target);
2220         struct arm_dpm *dpm = &armv8->dpm;
2221         struct arm *arm = &armv8->arm;
2222         int retval;
2223
2224         armv8_reg_current(arm, 1)->dirty = true;
2225
2226         /* change DCC to normal mode (if necessary) */
2227         if (*dscr & DSCR_MA) {
2228                 *dscr &= DSCR_MA;
2229                 retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
2230                                 armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
2231                 if (retval != ERROR_OK)
2232                         return retval;
2233         }
2234
2235         while (count) {
2236                 uint32_t opcode, data;
2237
2238                 if (size == 1)
2239                         opcode = armv8_opcode(armv8, ARMV8_OPC_LDRB_IP);
2240                 else if (size == 2)
2241                         opcode = armv8_opcode(armv8, ARMV8_OPC_LDRH_IP);
2242                 else
2243                         opcode = armv8_opcode(armv8, ARMV8_OPC_LDRW_IP);
2244                 retval = dpm->instr_execute(dpm, opcode);
2245                 if (retval != ERROR_OK)
2246                         return retval;
2247
2248                 if (arm->core_state == ARM_STATE_AARCH64)
2249                         retval = dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 1));
2250                 else
2251                         retval = dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 1, 0, 5, 0));
2252                 if (retval != ERROR_OK)
2253                         return retval;
2254
2255                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2256                                 armv8->debug_base + CPUV8_DBG_DTRTX, &data);
2257                 if (retval != ERROR_OK)
2258                         return retval;
2259
2260                 if (size == 1)
2261                         *buffer = (uint8_t)data;
2262                 else if (size == 2)
2263                         target_buffer_set_u16(target, buffer, (uint16_t)data);
2264                 else
2265                         target_buffer_set_u32(target, buffer, data);
2266
2267                 /* Advance */
2268                 buffer += size;
2269                 --count;
2270         }
2271
2272         return ERROR_OK;
2273 }
2274
2275 static int aarch64_read_cpu_memory_fast(struct target *target,
2276         uint32_t count, uint8_t *buffer, uint32_t *dscr)
2277 {
2278         struct armv8_common *armv8 = target_to_armv8(target);
2279         struct arm_dpm *dpm = &armv8->dpm;
2280         struct arm *arm = &armv8->arm;
2281         int retval;
2282         uint32_t value;
2283
2284         /* Mark X1 as dirty */
2285         armv8_reg_current(arm, 1)->dirty = true;
2286
2287         if (arm->core_state == ARM_STATE_AARCH64) {
2288                 /* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
2289                 retval = dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0));
2290         } else {
2291                 /* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
2292                 retval = dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 0, 0, 5, 0));
2293         }
2294
2295         if (retval != ERROR_OK)
2296                 return retval;
2297
2298         /* Step 1.e - Change DCC to memory mode */
2299         *dscr |= DSCR_MA;
2300         retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
2301                         armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
2302         if (retval != ERROR_OK)
2303                 return retval;
2304
2305         /* Step 1.f - read DBGDTRTX and discard the value */
2306         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2307                         armv8->debug_base + CPUV8_DBG_DTRTX, &value);
2308         if (retval != ERROR_OK)
2309                 return retval;
2310
2311         count--;
2312         /* Read the data - Each read of the DTRTX register causes the instruction to be reissued
2313          * Abort flags are sticky, so can be read at end of transactions
2314          *
2315          * This data is read in aligned to 32 bit boundary.
2316          */
2317
2318         if (count) {
2319                 /* Step 2.a - Loop n-1 times, each read of DBGDTRTX reads the data from [X0] and
2320                  * increments X0 by 4. */
2321                 retval = mem_ap_read_buf_noincr(armv8->debug_ap, buffer, 4, count,
2322                                                                         armv8->debug_base + CPUV8_DBG_DTRTX);
2323                 if (retval != ERROR_OK)
2324                         return retval;
2325         }
2326
2327         /* Step 3.a - set DTR access mode back to Normal mode   */
2328         *dscr &= ~DSCR_MA;
2329         retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
2330                                         armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
2331         if (retval != ERROR_OK)
2332                 return retval;
2333
2334         /* Step 3.b - read DBGDTRTX for the final value */
2335         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2336                         armv8->debug_base + CPUV8_DBG_DTRTX, &value);
2337         if (retval != ERROR_OK)
2338                 return retval;
2339
2340         target_buffer_set_u32(target, buffer + count * 4, value);
2341         return retval;
2342 }
2343
2344 static int aarch64_read_cpu_memory(struct target *target,
2345         target_addr_t address, uint32_t size,
2346         uint32_t count, uint8_t *buffer)
2347 {
2348         /* read memory through APB-AP */
2349         int retval = ERROR_COMMAND_SYNTAX_ERROR;
2350         struct armv8_common *armv8 = target_to_armv8(target);
2351         struct arm_dpm *dpm = &armv8->dpm;
2352         struct arm *arm = &armv8->arm;
2353         uint32_t dscr;
2354
2355         LOG_DEBUG("Reading CPU memory address 0x%016" PRIx64 " size %" PRIu32 " count %" PRIu32,
2356                         address, size, count);
2357
2358         if (target->state != TARGET_HALTED) {
2359                 LOG_WARNING("target not halted");
2360                 return ERROR_TARGET_NOT_HALTED;
2361         }
2362
2363         /* Mark register X0 as dirty, as it will be used
2364          * for transferring the data.
2365          * It will be restored automatically when exiting
2366          * debug mode
2367          */
2368         armv8_reg_current(arm, 0)->dirty = true;
2369
2370         /* Read DSCR */
2371         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2372                                 armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
2373         if (retval != ERROR_OK)
2374                 return retval;
2375
2376         /* This algorithm comes from DDI0487A.g, chapter J9.1 */
2377
2378         /* Set Normal access mode  */
2379         dscr &= ~DSCR_MA;
2380         retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
2381                         armv8->debug_base + CPUV8_DBG_DSCR, dscr);
2382         if (retval != ERROR_OK)
2383                 return retval;
2384
2385         if (arm->core_state == ARM_STATE_AARCH64) {
2386                 /* Write X0 with value 'address' using write procedure */
2387                 /* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
2388                 /* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
2389                 retval = dpm->instr_write_data_dcc_64(dpm,
2390                                 ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address);
2391         } else {
2392                 /* Write R0 with value 'address' using write procedure */
2393                 /* Step 1.a+b - Write the address for read access into DBGDTRRXint */
2394                 /* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
2395                 retval = dpm->instr_write_data_dcc(dpm,
2396                                 ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address);
2397         }
2398
2399         if (retval != ERROR_OK)
2400                 return retval;
2401
2402         if (size == 4 && (address % 4) == 0)
2403                 retval = aarch64_read_cpu_memory_fast(target, count, buffer, &dscr);
2404         else
2405                 retval = aarch64_read_cpu_memory_slow(target, size, count, buffer, &dscr);
2406
2407         if (dscr & DSCR_MA) {
2408                 dscr &= ~DSCR_MA;
2409                 mem_ap_write_atomic_u32(armv8->debug_ap,
2410                                         armv8->debug_base + CPUV8_DBG_DSCR, dscr);
2411         }
2412
2413         if (retval != ERROR_OK)
2414                 return retval;
2415
2416         /* Check for sticky abort flags in the DSCR */
2417         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2418                                 armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
2419         if (retval != ERROR_OK)
2420                 return retval;
2421
2422         dpm->dscr = dscr;
2423
2424         if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
2425                 /* Abort occurred - clear it and exit */
2426                 LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
2427                 armv8_dpm_handle_exception(dpm, true);
2428                 return ERROR_FAIL;
2429         }
2430
2431         /* Done */
2432         return ERROR_OK;
2433 }
2434
2435 static int aarch64_read_phys_memory(struct target *target,
2436         target_addr_t address, uint32_t size,
2437         uint32_t count, uint8_t *buffer)
2438 {
2439         int retval = ERROR_COMMAND_SYNTAX_ERROR;
2440
2441         if (count && buffer) {
2442                 /* read memory through APB-AP */
2443                 retval = aarch64_mmu_modify(target, 0);
2444                 if (retval != ERROR_OK)
2445                         return retval;
2446                 retval = aarch64_read_cpu_memory(target, address, size, count, buffer);
2447         }
2448         return retval;
2449 }
2450
2451 static int aarch64_read_memory(struct target *target, target_addr_t address,
2452         uint32_t size, uint32_t count, uint8_t *buffer)
2453 {
2454         int mmu_enabled = 0;
2455         int retval;
2456
2457         /* determine if MMU was enabled on target stop */
2458         retval = aarch64_mmu(target, &mmu_enabled);
2459         if (retval != ERROR_OK)
2460                 return retval;
2461
2462         if (mmu_enabled) {
2463                 /* enable MMU as we could have disabled it for phys access */
2464                 retval = aarch64_mmu_modify(target, 1);
2465                 if (retval != ERROR_OK)
2466                         return retval;
2467         }
2468         return aarch64_read_cpu_memory(target, address, size, count, buffer);
2469 }
2470
2471 static int aarch64_write_phys_memory(struct target *target,
2472         target_addr_t address, uint32_t size,
2473         uint32_t count, const uint8_t *buffer)
2474 {
2475         int retval = ERROR_COMMAND_SYNTAX_ERROR;
2476
2477         if (count && buffer) {
2478                 /* write memory through APB-AP */
2479                 retval = aarch64_mmu_modify(target, 0);
2480                 if (retval != ERROR_OK)
2481                         return retval;
2482                 return aarch64_write_cpu_memory(target, address, size, count, buffer);
2483         }
2484
2485         return retval;
2486 }
2487
2488 static int aarch64_write_memory(struct target *target, target_addr_t address,
2489         uint32_t size, uint32_t count, const uint8_t *buffer)
2490 {
2491         int mmu_enabled = 0;
2492         int retval;
2493
2494         /* determine if MMU was enabled on target stop */
2495         retval = aarch64_mmu(target, &mmu_enabled);
2496         if (retval != ERROR_OK)
2497                 return retval;
2498
2499         if (mmu_enabled) {
2500                 /* enable MMU as we could have disabled it for phys access */
2501                 retval = aarch64_mmu_modify(target, 1);
2502                 if (retval != ERROR_OK)
2503                         return retval;
2504         }
2505         return aarch64_write_cpu_memory(target, address, size, count, buffer);
2506 }
2507
2508 static int aarch64_handle_target_request(void *priv)
2509 {
2510         struct target *target = priv;
2511         struct armv8_common *armv8 = target_to_armv8(target);
2512         int retval;
2513
2514         if (!target_was_examined(target))
2515                 return ERROR_OK;
2516         if (!target->dbg_msg_enabled)
2517                 return ERROR_OK;
2518
2519         if (target->state == TARGET_RUNNING) {
2520                 uint32_t request;
2521                 uint32_t dscr;
2522                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2523                                 armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
2524
2525                 /* check if we have data */
2526                 while ((dscr & DSCR_DTR_TX_FULL) && (retval == ERROR_OK)) {
2527                         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2528                                         armv8->debug_base + CPUV8_DBG_DTRTX, &request);
2529                         if (retval == ERROR_OK) {
2530                                 target_request(target, request);
2531                                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
2532                                                 armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
2533                         }
2534                 }
2535         }
2536
2537         return ERROR_OK;
2538 }
2539
2540 static int aarch64_examine_first(struct target *target)
2541 {
2542         struct aarch64_common *aarch64 = target_to_aarch64(target);
2543         struct armv8_common *armv8 = &aarch64->armv8_common;
2544         struct adiv5_dap *swjdp = armv8->arm.dap;
2545         struct aarch64_private_config *pc = target->private_config;
2546         int i;
2547         int retval = ERROR_OK;
2548         uint64_t debug, ttypr;
2549         uint32_t cpuid;
2550         uint32_t tmp0, tmp1, tmp2, tmp3;
2551         debug = ttypr = cpuid = 0;
2552
2553         if (pc == NULL)
2554                 return ERROR_FAIL;
2555
2556         if (pc->adiv5_config.ap_num == DP_APSEL_INVALID) {
2557                 /* Search for the APB-AB */
2558                 retval = dap_find_ap(swjdp, AP_TYPE_APB_AP, &armv8->debug_ap);
2559                 if (retval != ERROR_OK) {
2560                         LOG_ERROR("Could not find APB-AP for debug access");
2561                         return retval;
2562                 }
2563         } else {
2564                 armv8->debug_ap = dap_ap(swjdp, pc->adiv5_config.ap_num);
2565         }
2566
2567         retval = mem_ap_init(armv8->debug_ap);
2568         if (retval != ERROR_OK) {
2569                 LOG_ERROR("Could not initialize the APB-AP");
2570                 return retval;
2571         }
2572
2573         armv8->debug_ap->memaccess_tck = 10;
2574
2575         if (!target->dbgbase_set) {
2576                 uint32_t dbgbase;
2577                 /* Get ROM Table base */
2578                 uint32_t apid;
2579                 int32_t coreidx = target->coreid;
2580                 retval = dap_get_debugbase(armv8->debug_ap, &dbgbase, &apid);
2581                 if (retval != ERROR_OK)
2582                         return retval;
2583                 /* Lookup 0x15 -- Processor DAP */
2584                 retval = dap_lookup_cs_component(armv8->debug_ap, dbgbase, 0x15,
2585                                 &armv8->debug_base, &coreidx);
2586                 if (retval != ERROR_OK)
2587                         return retval;
2588                 LOG_DEBUG("Detected core %" PRId32 " dbgbase: %08" PRIx32
2589                                 " apid: %08" PRIx32, coreidx, armv8->debug_base, apid);
2590         } else
2591                 armv8->debug_base = target->dbgbase;
2592
2593         retval = mem_ap_write_atomic_u32(armv8->debug_ap,
2594                         armv8->debug_base + CPUV8_DBG_OSLAR, 0);
2595         if (retval != ERROR_OK) {
2596                 LOG_DEBUG("Examine %s failed", "oslock");
2597                 return retval;
2598         }
2599
2600         retval = mem_ap_read_u32(armv8->debug_ap,
2601                         armv8->debug_base + CPUV8_DBG_MAINID0, &cpuid);
2602         if (retval != ERROR_OK) {
2603                 LOG_DEBUG("Examine %s failed", "CPUID");
2604                 return retval;
2605         }
2606
2607         retval = mem_ap_read_u32(armv8->debug_ap,
2608                         armv8->debug_base + CPUV8_DBG_MEMFEATURE0, &tmp0);
2609         retval += mem_ap_read_u32(armv8->debug_ap,
2610                         armv8->debug_base + CPUV8_DBG_MEMFEATURE0 + 4, &tmp1);
2611         if (retval != ERROR_OK) {
2612                 LOG_DEBUG("Examine %s failed", "Memory Model Type");
2613                 return retval;
2614         }
2615         retval = mem_ap_read_u32(armv8->debug_ap,
2616                         armv8->debug_base + CPUV8_DBG_DBGFEATURE0, &tmp2);
2617         retval += mem_ap_read_u32(armv8->debug_ap,
2618                         armv8->debug_base + CPUV8_DBG_DBGFEATURE0 + 4, &tmp3);
2619         if (retval != ERROR_OK) {
2620                 LOG_DEBUG("Examine %s failed", "ID_AA64DFR0_EL1");
2621                 return retval;
2622         }
2623
2624         retval = dap_run(armv8->debug_ap->dap);
2625         if (retval != ERROR_OK) {
2626                 LOG_ERROR("%s: examination failed\n", target_name(target));
2627                 return retval;
2628         }
2629
2630         ttypr |= tmp1;
2631         ttypr = (ttypr << 32) | tmp0;
2632         debug |= tmp3;
2633         debug = (debug << 32) | tmp2;
2634
2635         LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
2636         LOG_DEBUG("ttypr = 0x%08" PRIx64, ttypr);
2637         LOG_DEBUG("debug = 0x%08" PRIx64, debug);
2638
2639         if (pc->cti == NULL)
2640                 return ERROR_FAIL;
2641
2642         armv8->cti = pc->cti;
2643
2644         retval = aarch64_dpm_setup(aarch64, debug);
2645         if (retval != ERROR_OK)
2646                 return retval;
2647
2648         /* Setup Breakpoint Register Pairs */
2649         aarch64->brp_num = (uint32_t)((debug >> 12) & 0x0F) + 1;
2650         aarch64->brp_num_context = (uint32_t)((debug >> 28) & 0x0F) + 1;
2651         aarch64->brp_num_available = aarch64->brp_num;
2652         aarch64->brp_list = calloc(aarch64->brp_num, sizeof(struct aarch64_brp));
2653         for (i = 0; i < aarch64->brp_num; i++) {
2654                 aarch64->brp_list[i].used = 0;
2655                 if (i < (aarch64->brp_num-aarch64->brp_num_context))
2656                         aarch64->brp_list[i].type = BRP_NORMAL;
2657                 else
2658                         aarch64->brp_list[i].type = BRP_CONTEXT;
2659                 aarch64->brp_list[i].value = 0;
2660                 aarch64->brp_list[i].control = 0;
2661                 aarch64->brp_list[i].BRPn = i;
2662         }
2663
2664         /* Setup Watchpoint Register Pairs */
2665         aarch64->wp_num = (uint32_t)((debug >> 20) & 0x0F) + 1;
2666         aarch64->wp_num_available = aarch64->wp_num;
2667         aarch64->wp_list = calloc(aarch64->wp_num, sizeof(struct aarch64_brp));
2668         for (i = 0; i < aarch64->wp_num; i++) {
2669                 aarch64->wp_list[i].used = 0;
2670                 aarch64->wp_list[i].type = BRP_NORMAL;
2671                 aarch64->wp_list[i].value = 0;
2672                 aarch64->wp_list[i].control = 0;
2673                 aarch64->wp_list[i].BRPn = i;
2674         }
2675
2676         LOG_DEBUG("Configured %i hw breakpoints, %i watchpoints",
2677                 aarch64->brp_num, aarch64->wp_num);
2678
2679         target->state = TARGET_UNKNOWN;
2680         target->debug_reason = DBG_REASON_NOTHALTED;
2681         aarch64->isrmasking_mode = AARCH64_ISRMASK_ON;
2682         target_set_examined(target);
2683         return ERROR_OK;
2684 }
2685
2686 static int aarch64_examine(struct target *target)
2687 {
2688         int retval = ERROR_OK;
2689
2690         /* don't re-probe hardware after each reset */
2691         if (!target_was_examined(target))
2692                 retval = aarch64_examine_first(target);
2693
2694         /* Configure core debug access */
2695         if (retval == ERROR_OK)
2696                 retval = aarch64_init_debug_access(target);
2697
2698         return retval;
2699 }
2700
2701 /*
2702  *      Cortex-A8 target creation and initialization
2703  */
2704
2705 static int aarch64_init_target(struct command_context *cmd_ctx,
2706         struct target *target)
2707 {
2708         /* examine_first() does a bunch of this */
2709         arm_semihosting_init(target);
2710         return ERROR_OK;
2711 }
2712
2713 static int aarch64_init_arch_info(struct target *target,
2714         struct aarch64_common *aarch64, struct adiv5_dap *dap)
2715 {
2716         struct armv8_common *armv8 = &aarch64->armv8_common;
2717
2718         /* Setup struct aarch64_common */
2719         aarch64->common_magic = AARCH64_COMMON_MAGIC;
2720         armv8->arm.dap = dap;
2721
2722         /* register arch-specific functions */
2723         armv8->examine_debug_reason = NULL;
2724         armv8->post_debug_entry = aarch64_post_debug_entry;
2725         armv8->pre_restore_context = NULL;
2726         armv8->armv8_mmu.read_physical_memory = aarch64_read_phys_memory;
2727
2728         armv8_init_arch_info(target, armv8);
2729         target_register_timer_callback(aarch64_handle_target_request, 1,
2730                 TARGET_TIMER_TYPE_PERIODIC, target);
2731
2732         return ERROR_OK;
2733 }
2734
2735 static int aarch64_target_create(struct target *target, Jim_Interp *interp)
2736 {
2737         struct aarch64_private_config *pc = target->private_config;
2738         struct aarch64_common *aarch64;
2739
2740         if (adiv5_verify_config(&pc->adiv5_config) != ERROR_OK)
2741                 return ERROR_FAIL;
2742
2743         aarch64 = calloc(1, sizeof(struct aarch64_common));
2744         if (aarch64 == NULL) {
2745                 LOG_ERROR("Out of memory");
2746                 return ERROR_FAIL;
2747         }
2748
2749         return aarch64_init_arch_info(target, aarch64, pc->adiv5_config.dap);
2750 }
2751
2752 static void aarch64_deinit_target(struct target *target)
2753 {
2754         struct aarch64_common *aarch64 = target_to_aarch64(target);
2755         struct armv8_common *armv8 = &aarch64->armv8_common;
2756         struct arm_dpm *dpm = &armv8->dpm;
2757
2758         armv8_free_reg_cache(target);
2759         free(aarch64->brp_list);
2760         free(dpm->dbp);
2761         free(dpm->dwp);
2762         free(target->private_config);
2763         free(aarch64);
2764 }
2765
2766 static int aarch64_mmu(struct target *target, int *enabled)
2767 {
2768         if (target->state != TARGET_HALTED) {
2769                 LOG_ERROR("%s: target %s not halted", __func__, target_name(target));
2770                 return ERROR_TARGET_INVALID;
2771         }
2772
2773         *enabled = target_to_aarch64(target)->armv8_common.armv8_mmu.mmu_enabled;
2774         return ERROR_OK;
2775 }
2776
2777 static int aarch64_virt2phys(struct target *target, target_addr_t virt,
2778                              target_addr_t *phys)
2779 {
2780         return armv8_mmu_translate_va_pa(target, virt, phys, 1);
2781 }
2782
2783 /*
2784  * private target configuration items
2785  */
2786 enum aarch64_cfg_param {
2787         CFG_CTI,
2788 };
2789
2790 static const Jim_Nvp nvp_config_opts[] = {
2791         { .name = "-cti", .value = CFG_CTI },
2792         { .name = NULL, .value = -1 }
2793 };
2794
2795 static int aarch64_jim_configure(struct target *target, Jim_GetOptInfo *goi)
2796 {
2797         struct aarch64_private_config *pc;
2798         Jim_Nvp *n;
2799         int e;
2800
2801         pc = (struct aarch64_private_config *)target->private_config;
2802         if (pc == NULL) {
2803                         pc = calloc(1, sizeof(struct aarch64_private_config));
2804                         pc->adiv5_config.ap_num = DP_APSEL_INVALID;
2805                         target->private_config = pc;
2806         }
2807
2808         /*
2809          * Call adiv5_jim_configure() to parse the common DAP options
2810          * It will return JIM_CONTINUE if it didn't find any known
2811          * options, JIM_OK if it correctly parsed the topmost option
2812          * and JIM_ERR if an error occurred during parameter evaluation.
2813          * For JIM_CONTINUE, we check our own params.
2814          *
2815          * adiv5_jim_configure() assumes 'private_config' to point to
2816          * 'struct adiv5_private_config'. Override 'private_config'!
2817          */
2818         target->private_config = &pc->adiv5_config;
2819         e = adiv5_jim_configure(target, goi);
2820         target->private_config = pc;
2821         if (e != JIM_CONTINUE)
2822                 return e;
2823
2824         /* parse config or cget options ... */
2825         if (goi->argc > 0) {
2826                 Jim_SetEmptyResult(goi->interp);
2827
2828                 /* check first if topmost item is for us */
2829                 e = Jim_Nvp_name2value_obj(goi->interp, nvp_config_opts,
2830                                 goi->argv[0], &n);
2831                 if (e != JIM_OK)
2832                         return JIM_CONTINUE;
2833
2834                 e = Jim_GetOpt_Obj(goi, NULL);
2835                 if (e != JIM_OK)
2836                         return e;
2837
2838                 switch (n->value) {
2839                 case CFG_CTI: {
2840                         if (goi->isconfigure) {
2841                                 Jim_Obj *o_cti;
2842                                 struct arm_cti *cti;
2843                                 e = Jim_GetOpt_Obj(goi, &o_cti);
2844                                 if (e != JIM_OK)
2845                                         return e;
2846                                 cti = cti_instance_by_jim_obj(goi->interp, o_cti);
2847                                 if (cti == NULL) {
2848                                         Jim_SetResultString(goi->interp, "CTI name invalid!", -1);
2849                                         return JIM_ERR;
2850                                 }
2851                                 pc->cti = cti;
2852                         } else {
2853                                 if (goi->argc != 0) {
2854                                         Jim_WrongNumArgs(goi->interp,
2855                                                         goi->argc, goi->argv,
2856                                                         "NO PARAMS");
2857                                         return JIM_ERR;
2858                                 }
2859
2860                                 if (pc == NULL || pc->cti == NULL) {
2861                                         Jim_SetResultString(goi->interp, "CTI not configured", -1);
2862                                         return JIM_ERR;
2863                                 }
2864                                 Jim_SetResultString(goi->interp, arm_cti_name(pc->cti), -1);
2865                         }
2866                         break;
2867                 }
2868
2869                 default:
2870                         return JIM_CONTINUE;
2871                 }
2872         }
2873
2874         return JIM_OK;
2875 }
2876
2877 COMMAND_HANDLER(aarch64_handle_cache_info_command)
2878 {
2879         struct target *target = get_current_target(CMD_CTX);
2880         struct armv8_common *armv8 = target_to_armv8(target);
2881
2882         return armv8_handle_cache_info_command(CMD,
2883                         &armv8->armv8_mmu.armv8_cache);
2884 }
2885
2886 COMMAND_HANDLER(aarch64_handle_dbginit_command)
2887 {
2888         struct target *target = get_current_target(CMD_CTX);
2889         if (!target_was_examined(target)) {
2890                 LOG_ERROR("target not examined yet");
2891                 return ERROR_FAIL;
2892         }
2893
2894         return aarch64_init_debug_access(target);
2895 }
2896
2897 COMMAND_HANDLER(aarch64_handle_disassemble_command)
2898 {
2899         struct target *target = get_current_target(CMD_CTX);
2900
2901         if (target == NULL) {
2902                 LOG_ERROR("No target selected");
2903                 return ERROR_FAIL;
2904         }
2905
2906         struct aarch64_common *aarch64 = target_to_aarch64(target);
2907
2908         if (aarch64->common_magic != AARCH64_COMMON_MAGIC) {
2909                 command_print(CMD, "current target isn't an AArch64");
2910                 return ERROR_FAIL;
2911         }
2912
2913         int count = 1;
2914         target_addr_t address;
2915
2916         switch (CMD_ARGC) {
2917                 case 2:
2918                         COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], count);
2919                 /* FALL THROUGH */
2920                 case 1:
2921                         COMMAND_PARSE_ADDRESS(CMD_ARGV[0], address);
2922                         break;
2923                 default:
2924                         return ERROR_COMMAND_SYNTAX_ERROR;
2925         }
2926
2927         return a64_disassemble(CMD, target, address, count);
2928 }
2929
2930 COMMAND_HANDLER(aarch64_mask_interrupts_command)
2931 {
2932         struct target *target = get_current_target(CMD_CTX);
2933         struct aarch64_common *aarch64 = target_to_aarch64(target);
2934
2935         static const Jim_Nvp nvp_maskisr_modes[] = {
2936                 { .name = "off", .value = AARCH64_ISRMASK_OFF },
2937                 { .name = "on", .value = AARCH64_ISRMASK_ON },
2938                 { .name = NULL, .value = -1 },
2939         };
2940         const Jim_Nvp *n;
2941
2942         if (CMD_ARGC > 0) {
2943                 n = Jim_Nvp_name2value_simple(nvp_maskisr_modes, CMD_ARGV[0]);
2944                 if (n->name == NULL) {
2945                         LOG_ERROR("Unknown parameter: %s - should be off or on", CMD_ARGV[0]);
2946                         return ERROR_COMMAND_SYNTAX_ERROR;
2947                 }
2948
2949                 aarch64->isrmasking_mode = n->value;
2950         }
2951
2952         n = Jim_Nvp_value2name_simple(nvp_maskisr_modes, aarch64->isrmasking_mode);
2953         command_print(CMD, "aarch64 interrupt mask %s", n->name);
2954
2955         return ERROR_OK;
2956 }
2957
2958 static int jim_mcrmrc(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
2959 {
2960         struct command_context *context;
2961         struct target *target;
2962         struct arm *arm;
2963         int retval;
2964         bool is_mcr = false;
2965         int arg_cnt = 0;
2966
2967         if (Jim_CompareStringImmediate(interp, argv[0], "mcr")) {
2968                 is_mcr = true;
2969                 arg_cnt = 7;
2970         } else {
2971                 arg_cnt = 6;
2972         }
2973
2974         context = current_command_context(interp);
2975         assert(context != NULL);
2976
2977         target = get_current_target(context);
2978         if (target == NULL) {
2979                 LOG_ERROR("%s: no current target", __func__);
2980                 return JIM_ERR;
2981         }
2982         if (!target_was_examined(target)) {
2983                 LOG_ERROR("%s: not yet examined", target_name(target));
2984                 return JIM_ERR;
2985         }
2986
2987         arm = target_to_arm(target);
2988         if (!is_arm(arm)) {
2989                 LOG_ERROR("%s: not an ARM", target_name(target));
2990                 return JIM_ERR;
2991         }
2992
2993         if (target->state != TARGET_HALTED)
2994                 return ERROR_TARGET_NOT_HALTED;
2995
2996         if (arm->core_state == ARM_STATE_AARCH64) {
2997                 LOG_ERROR("%s: not 32-bit arm target", target_name(target));
2998                 return JIM_ERR;
2999         }
3000
3001         if (argc != arg_cnt) {
3002                 LOG_ERROR("%s: wrong number of arguments", __func__);
3003                 return JIM_ERR;
3004         }
3005
3006         int cpnum;
3007         uint32_t op1;
3008         uint32_t op2;
3009         uint32_t CRn;
3010         uint32_t CRm;
3011         uint32_t value;
3012         long l;
3013
3014         /* NOTE:  parameter sequence matches ARM instruction set usage:
3015          *      MCR     pNUM, op1, rX, CRn, CRm, op2    ; write CP from rX
3016          *      MRC     pNUM, op1, rX, CRn, CRm, op2    ; read CP into rX
3017          * The "rX" is necessarily omitted; it uses Tcl mechanisms.
3018          */
3019         retval = Jim_GetLong(interp, argv[1], &l);
3020         if (retval != JIM_OK)
3021                 return retval;
3022         if (l & ~0xf) {
3023                 LOG_ERROR("%s: %s %d out of range", __func__,
3024                         "coprocessor", (int) l);
3025                 return JIM_ERR;
3026         }
3027         cpnum = l;
3028
3029         retval = Jim_GetLong(interp, argv[2], &l);
3030         if (retval != JIM_OK)
3031                 return retval;
3032         if (l & ~0x7) {
3033                 LOG_ERROR("%s: %s %d out of range", __func__,
3034                         "op1", (int) l);
3035                 return JIM_ERR;
3036         }
3037         op1 = l;
3038
3039         retval = Jim_GetLong(interp, argv[3], &l);
3040         if (retval != JIM_OK)
3041                 return retval;
3042         if (l & ~0xf) {
3043                 LOG_ERROR("%s: %s %d out of range", __func__,
3044                         "CRn", (int) l);
3045                 return JIM_ERR;
3046         }
3047         CRn = l;
3048
3049         retval = Jim_GetLong(interp, argv[4], &l);
3050         if (retval != JIM_OK)
3051                 return retval;
3052         if (l & ~0xf) {
3053                 LOG_ERROR("%s: %s %d out of range", __func__,
3054                         "CRm", (int) l);
3055                 return JIM_ERR;
3056         }
3057         CRm = l;
3058
3059         retval = Jim_GetLong(interp, argv[5], &l);
3060         if (retval != JIM_OK)
3061                 return retval;
3062         if (l & ~0x7) {
3063                 LOG_ERROR("%s: %s %d out of range", __func__,
3064                         "op2", (int) l);
3065                 return JIM_ERR;
3066         }
3067         op2 = l;
3068
3069         value = 0;
3070
3071         if (is_mcr == true) {
3072                 retval = Jim_GetLong(interp, argv[6], &l);
3073                 if (retval != JIM_OK)
3074                         return retval;
3075                 value = l;
3076
3077                 /* NOTE: parameters reordered! */
3078                 /* ARMV4_5_MCR(cpnum, op1, 0, CRn, CRm, op2) */
3079                 retval = arm->mcr(target, cpnum, op1, op2, CRn, CRm, value);
3080                 if (retval != ERROR_OK)
3081                         return JIM_ERR;
3082         } else {
3083                 /* NOTE: parameters reordered! */
3084                 /* ARMV4_5_MRC(cpnum, op1, 0, CRn, CRm, op2) */
3085                 retval = arm->mrc(target, cpnum, op1, op2, CRn, CRm, &value);
3086                 if (retval != ERROR_OK)
3087                         return JIM_ERR;
3088
3089                 Jim_SetResult(interp, Jim_NewIntObj(interp, value));
3090         }
3091
3092         return JIM_OK;
3093 }
3094
3095 static const struct command_registration aarch64_exec_command_handlers[] = {
3096         {
3097                 .name = "cache_info",
3098                 .handler = aarch64_handle_cache_info_command,
3099                 .mode = COMMAND_EXEC,
3100                 .help = "display information about target caches",
3101                 .usage = "",
3102         },
3103         {
3104                 .name = "dbginit",
3105                 .handler = aarch64_handle_dbginit_command,
3106                 .mode = COMMAND_EXEC,
3107                 .help = "Initialize core debug",
3108                 .usage = "",
3109         },
3110         {
3111                 .name = "disassemble",
3112                 .handler = aarch64_handle_disassemble_command,
3113                 .mode = COMMAND_EXEC,
3114                 .help = "Disassemble instructions",
3115                 .usage = "address [count]",
3116         },
3117         {
3118                 .name = "maskisr",
3119                 .handler = aarch64_mask_interrupts_command,
3120                 .mode = COMMAND_ANY,
3121                 .help = "mask aarch64 interrupts during single-step",
3122                 .usage = "['on'|'off']",
3123         },
3124         {
3125                 .name = "mcr",
3126                 .mode = COMMAND_EXEC,
3127                 .jim_handler = jim_mcrmrc,
3128                 .help = "write coprocessor register",
3129                 .usage = "cpnum op1 CRn CRm op2 value",
3130         },
3131         {
3132                 .name = "mrc",
3133                 .mode = COMMAND_EXEC,
3134                 .jim_handler = jim_mcrmrc,
3135                 .help = "read coprocessor register",
3136                 .usage = "cpnum op1 CRn CRm op2",
3137         },
3138         {
3139                 .chain = smp_command_handlers,
3140         },
3141
3142
3143         COMMAND_REGISTRATION_DONE
3144 };
3145
3146 extern const struct command_registration semihosting_common_handlers[];
3147
3148 static const struct command_registration aarch64_command_handlers[] = {
3149         {
3150                 .name = "arm",
3151                 .mode = COMMAND_ANY,
3152                 .help = "ARM Command Group",
3153                 .usage = "",
3154                 .chain = semihosting_common_handlers
3155         },
3156         {
3157                 .chain = armv8_command_handlers,
3158         },
3159         {
3160                 .name = "aarch64",
3161                 .mode = COMMAND_ANY,
3162                 .help = "Aarch64 command group",
3163                 .usage = "",
3164                 .chain = aarch64_exec_command_handlers,
3165         },
3166         COMMAND_REGISTRATION_DONE
3167 };
3168
3169 struct target_type aarch64_target = {
3170         .name = "aarch64",
3171
3172         .poll = aarch64_poll,
3173         .arch_state = armv8_arch_state,
3174
3175         .halt = aarch64_halt,
3176         .resume = aarch64_resume,
3177         .step = aarch64_step,
3178
3179         .assert_reset = aarch64_assert_reset,
3180         .deassert_reset = aarch64_deassert_reset,
3181
3182         /* REVISIT allow exporting VFP3 registers ... */
3183         .get_gdb_arch = armv8_get_gdb_arch,
3184         .get_gdb_reg_list = armv8_get_gdb_reg_list,
3185
3186         .read_memory = aarch64_read_memory,
3187         .write_memory = aarch64_write_memory,
3188
3189         .add_breakpoint = aarch64_add_breakpoint,
3190         .add_context_breakpoint = aarch64_add_context_breakpoint,
3191         .add_hybrid_breakpoint = aarch64_add_hybrid_breakpoint,
3192         .remove_breakpoint = aarch64_remove_breakpoint,
3193         .add_watchpoint = aarch64_add_watchpoint,
3194         .remove_watchpoint = aarch64_remove_watchpoint,
3195         .hit_watchpoint = aarch64_hit_watchpoint,
3196
3197         .commands = aarch64_command_handlers,
3198         .target_create = aarch64_target_create,
3199         .target_jim_configure = aarch64_jim_configure,
3200         .init_target = aarch64_init_target,
3201         .deinit_target = aarch64_deinit_target,
3202         .examine = aarch64_examine,
3203
3204         .read_phys_memory = aarch64_read_phys_memory,
3205         .write_phys_memory = aarch64_write_phys_memory,
3206         .mmu = aarch64_mmu,
3207         .virt2phys = aarch64_virt2phys,
3208 };