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

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2
   3 /*
   4  * Copyright (C) 2009 by David Brownell
   5  */
   6
   7 #ifdef HAVE_CONFIG_H
   8 #include "config.h"
   9 #endif
  10
  11 #include "arm.h"
  12 #include "armv8.h"
  13 #include "armv8_dpm.h"
  14 #include <jtag/jtag.h>
  15 #include "register.h"
  16 #include "breakpoints.h"
  17 #include "target_type.h"
  18 #include "armv8_opcodes.h"
  19
  20 #include "helper/time_support.h"
  21
  22 /* T32 ITR format */
  23 #define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
  24
  25 /**
  26  * @file
  27  * Implements various ARM DPM operations using architectural debug registers.
  28  * These routines layer over core-specific communication methods to cope with
  29  * implementation differences between cores like ARM1136 and Cortex-A8.
  30  *
  31  * The "Debug Programmers' Model" (DPM) for ARMv6 and ARMv7 is defined by
  32  * Part C (Debug Architecture) of the ARM Architecture Reference Manual,
  33  * ARMv7-A and ARMv7-R edition (ARM DDI 0406B).  In OpenOCD, DPM operations
  34  * are abstracted through internal programming interfaces to share code and
  35  * to minimize needless differences in debug behavior between cores.
  36  */
  37
  38 /**
  39  * Get core state from EDSCR, without necessity to retrieve CPSR
  40  */
  41 enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm)
  42 {
  43         int el = (dpm->dscr >> 8) & 0x3;
  44         int rw = (dpm->dscr >> 10) & 0xF;
  45
  46         dpm->last_el = el;
  47
  48         /* In Debug state, each bit gives the current Execution state of each EL */
  49         if ((rw >> el) & 0b1)
  50                 return ARM_STATE_AARCH64;
  51
  52         return ARM_STATE_ARM;
  53 }
  54
  55 /*----------------------------------------------------------------------*/
  56
  57 static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
  58 {
  59         return mem_ap_write_u32(armv8->debug_ap,
  60                                 armv8->debug_base + CPUV8_DBG_DTRRX, data);
  61 }
  62
  63 static int dpmv8_write_dcc_64(struct armv8_common *armv8, uint64_t data)
  64 {
  65         int ret;
  66         ret = mem_ap_write_u32(armv8->debug_ap,
  67                                armv8->debug_base + CPUV8_DBG_DTRRX, data);
  68         if (ret == ERROR_OK)
  69                 ret = mem_ap_write_u32(armv8->debug_ap,
  70                                 armv8->debug_base + CPUV8_DBG_DTRTX, data >> 32);
  71         return ret;
  72 }
  73
  74 static int dpmv8_read_dcc(struct armv8_common *armv8, uint32_t *data,
  75         uint32_t *dscr_p)
  76 {
  77         uint32_t dscr = DSCR_ITE;
  78         int retval;
  79
  80         if (dscr_p)
  81                 dscr = *dscr_p;
  82
  83         /* Wait for DTRRXfull */
  84         long long then = timeval_ms();
  85         while ((dscr & DSCR_DTR_TX_FULL) == 0) {
  86                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
  87                                 armv8->debug_base + CPUV8_DBG_DSCR,
  88                                 &dscr);
  89                 if (retval != ERROR_OK)
  90                         return retval;
  91                 if (timeval_ms() > then + 1000) {
  92                         LOG_ERROR("Timeout waiting for read dcc");
  93                         return ERROR_FAIL;
  94                 }
  95         }
  96
  97         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
  98                                             armv8->debug_base + CPUV8_DBG_DTRTX,
  99                                             data);
 100         if (retval != ERROR_OK)
 101                 return retval;
 102
 103         if (dscr_p)
 104                 *dscr_p = dscr;
 105
 106         return retval;
 107 }
 108
 109 static int dpmv8_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
 110         uint32_t *dscr_p)
 111 {
 112         uint32_t dscr = DSCR_ITE;
 113         uint32_t higher;
 114         int retval;
 115
 116         if (dscr_p)
 117                 dscr = *dscr_p;
 118
 119         /* Wait for DTRRXfull */
 120         long long then = timeval_ms();
 121         while ((dscr & DSCR_DTR_TX_FULL) == 0) {
 122                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 123                                 armv8->debug_base + CPUV8_DBG_DSCR,
 124                                 &dscr);
 125                 if (retval != ERROR_OK)
 126                         return retval;
 127                 if (timeval_ms() > then + 1000) {
 128                         LOG_ERROR("Timeout waiting for DTR_TX_FULL, dscr = 0x%08" PRIx32, dscr);
 129                         return ERROR_FAIL;
 130                 }
 131         }
 132
 133         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 134                                             armv8->debug_base + CPUV8_DBG_DTRTX,
 135                                             (uint32_t *)data);
 136         if (retval != ERROR_OK)
 137                 return retval;
 138
 139         retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 140                                             armv8->debug_base + CPUV8_DBG_DTRRX,
 141                                             &higher);
 142         if (retval != ERROR_OK)
 143                 return retval;
 144
 145         *data = *(uint32_t *)data | (uint64_t)higher << 32;
 146
 147         if (dscr_p)
 148                 *dscr_p = dscr;
 149
 150         return retval;
 151 }
 152
 153 static int dpmv8_dpm_prepare(struct arm_dpm *dpm)
 154 {
 155         struct armv8_common *armv8 = dpm->arm->arch_info;
 156         uint32_t dscr;
 157         int retval;
 158
 159         /* set up invariant:  ITE is set after ever DPM operation */
 160         long long then = timeval_ms();
 161         for (;; ) {
 162                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 163                                 armv8->debug_base + CPUV8_DBG_DSCR,
 164                                 &dscr);
 165                 if (retval != ERROR_OK)
 166                         return retval;
 167                 if ((dscr & DSCR_ITE) != 0)
 168                         break;
 169                 if (timeval_ms() > then + 1000) {
 170                         LOG_ERROR("Timeout waiting for dpm prepare");
 171                         return ERROR_FAIL;
 172                 }
 173         }
 174
 175         /* update the stored copy of dscr */
 176         dpm->dscr = dscr;
 177
 178         /* this "should never happen" ... */
 179         if (dscr & DSCR_DTR_RX_FULL) {
 180                 LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
 181                 /* Clear DCCRX */
 182                 retval = mem_ap_read_u32(armv8->debug_ap,
 183                         armv8->debug_base + CPUV8_DBG_DTRRX, &dscr);
 184                 if (retval != ERROR_OK)
 185                         return retval;
 186         }
 187
 188         return retval;
 189 }
 190
 191 static int dpmv8_dpm_finish(struct arm_dpm *dpm)
 192 {
 193         /* REVISIT what could be done here? */
 194         return ERROR_OK;
 195 }
 196
 197 static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 198         uint32_t opcode, uint32_t *p_dscr)
 199 {
 200         struct armv8_common *armv8 = dpm->arm->arch_info;
 201         uint32_t dscr = dpm->dscr;
 202         int retval;
 203
 204         if (p_dscr)
 205                 dscr = *p_dscr;
 206
 207         /* Wait for InstrCompl bit to be set */
 208         long long then = timeval_ms();
 209         while ((dscr & DSCR_ITE) == 0) {
 210                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 211                                 armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 212                 if (retval != ERROR_OK) {
 213                         LOG_ERROR("Could not read DSCR register, opcode = 0x%08" PRIx32, opcode);
 214                         return retval;
 215                 }
 216                 if (timeval_ms() > then + 1000) {
 217                         LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
 218                         return ERROR_FAIL;
 219                 }
 220         }
 221
 222         if (armv8_dpm_get_core_state(dpm) != ARM_STATE_AARCH64)
 223                 opcode = T32_FMTITR(opcode);
 224
 225         retval = mem_ap_write_u32(armv8->debug_ap,
 226                         armv8->debug_base + CPUV8_DBG_ITR, opcode);
 227         if (retval != ERROR_OK)
 228                 return retval;
 229
 230         then = timeval_ms();
 231         do {
 232                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 233                                 armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 234                 if (retval != ERROR_OK) {
 235                         LOG_ERROR("Could not read DSCR register");
 236                         return retval;
 237                 }
 238                 if (timeval_ms() > then + 1000) {
 239                         LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
 240                         return ERROR_FAIL;
 241                 }
 242         } while ((dscr & DSCR_ITE) == 0);       /* Wait for InstrCompl bit to be set */
 243
 244         /* update dscr and el after each command execution */
 245         dpm->dscr = dscr;
 246         if (dpm->last_el != ((dscr >> 8) & 3))
 247                 LOG_DEBUG("EL %i -> %" PRIu32, dpm->last_el, (dscr >> 8) & 3);
 248         dpm->last_el = (dscr >> 8) & 3;
 249
 250         if (dscr & DSCR_ERR) {
 251                 LOG_ERROR("Opcode 0x%08" PRIx32 ", DSCR.ERR=1, DSCR.EL=%i", opcode, dpm->last_el);
 252                 armv8_dpm_handle_exception(dpm, true);
 253                 retval = ERROR_FAIL;
 254         }
 255
 256         if (p_dscr)
 257                 *p_dscr = dscr;
 258
 259         return retval;
 260 }
 261
 262 static int dpmv8_instr_execute(struct arm_dpm *dpm, uint32_t opcode)
 263 {
 264         return dpmv8_exec_opcode(dpm, opcode, NULL);
 265 }
 266
 267 static int dpmv8_instr_write_data_dcc(struct arm_dpm *dpm,
 268         uint32_t opcode, uint32_t data)
 269 {
 270         struct armv8_common *armv8 = dpm->arm->arch_info;
 271         int retval;
 272
 273         retval = dpmv8_write_dcc(armv8, data);
 274         if (retval != ERROR_OK)
 275                 return retval;
 276
 277         return dpmv8_exec_opcode(dpm, opcode, 0);
 278 }
 279
 280 static int dpmv8_instr_write_data_dcc_64(struct arm_dpm *dpm,
 281         uint32_t opcode, uint64_t data)
 282 {
 283         struct armv8_common *armv8 = dpm->arm->arch_info;
 284         int retval;
 285
 286         retval = dpmv8_write_dcc_64(armv8, data);
 287         if (retval != ERROR_OK)
 288                 return retval;
 289
 290         return dpmv8_exec_opcode(dpm, opcode, 0);
 291 }
 292
 293 static int dpmv8_instr_write_data_r0(struct arm_dpm *dpm,
 294         uint32_t opcode, uint32_t data)
 295 {
 296         struct armv8_common *armv8 = dpm->arm->arch_info;
 297         uint32_t dscr = DSCR_ITE;
 298         int retval;
 299
 300         retval = dpmv8_write_dcc(armv8, data);
 301         if (retval != ERROR_OK)
 302                 return retval;
 303
 304         retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, READ_REG_DTRRX), &dscr);
 305         if (retval != ERROR_OK)
 306                 return retval;
 307
 308         /* then the opcode, taking data from R0 */
 309         return dpmv8_exec_opcode(dpm, opcode, &dscr);
 310 }
 311
 312 static int dpmv8_instr_write_data_r0_64(struct arm_dpm *dpm,
 313         uint32_t opcode, uint64_t data)
 314 {
 315         struct armv8_common *armv8 = dpm->arm->arch_info;
 316         int retval;
 317
 318         if (dpm->arm->core_state != ARM_STATE_AARCH64)
 319                 return dpmv8_instr_write_data_r0(dpm, opcode, data);
 320
 321         /* transfer data from DCC to R0 */
 322         retval = dpmv8_write_dcc_64(armv8, data);
 323         if (retval == ERROR_OK)
 324                 retval = dpmv8_exec_opcode(dpm, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dpm->dscr);
 325
 326         /* then the opcode, taking data from R0 */
 327         if (retval == ERROR_OK)
 328                 retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 329
 330         return retval;
 331 }
 332
 333 static int dpmv8_instr_cpsr_sync(struct arm_dpm *dpm)
 334 {
 335         int retval;
 336         struct armv8_common *armv8 = dpm->arm->arch_info;
 337
 338         /* "Prefetch flush" after modifying execution status in CPSR */
 339         retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, ARMV8_OPC_DSB_SY), &dpm->dscr);
 340         if (retval == ERROR_OK)
 341                 dpmv8_exec_opcode(dpm, armv8_opcode(armv8, ARMV8_OPC_ISB_SY), &dpm->dscr);
 342         return retval;
 343 }
 344
 345 static int dpmv8_instr_read_data_dcc(struct arm_dpm *dpm,
 346         uint32_t opcode, uint32_t *data)
 347 {
 348         struct armv8_common *armv8 = dpm->arm->arch_info;
 349         int retval;
 350
 351         /* the opcode, writing data to DCC */
 352         retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 353         if (retval != ERROR_OK)
 354                 return retval;
 355
 356         return dpmv8_read_dcc(armv8, data, &dpm->dscr);
 357 }
 358
 359 static int dpmv8_instr_read_data_dcc_64(struct arm_dpm *dpm,
 360         uint32_t opcode, uint64_t *data)
 361 {
 362         struct armv8_common *armv8 = dpm->arm->arch_info;
 363         int retval;
 364
 365         /* the opcode, writing data to DCC */
 366         retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 367         if (retval != ERROR_OK)
 368                 return retval;
 369
 370         return dpmv8_read_dcc_64(armv8, data, &dpm->dscr);
 371 }
 372
 373 static int dpmv8_instr_read_data_r0(struct arm_dpm *dpm,
 374         uint32_t opcode, uint32_t *data)
 375 {
 376         struct armv8_common *armv8 = dpm->arm->arch_info;
 377         int retval;
 378
 379         /* the opcode, writing data to R0 */
 380         retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 381         if (retval != ERROR_OK)
 382                 return retval;
 383
 384         /* write R0 to DCC */
 385         retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, WRITE_REG_DTRTX), &dpm->dscr);
 386         if (retval != ERROR_OK)
 387                 return retval;
 388
 389         return dpmv8_read_dcc(armv8, data, &dpm->dscr);
 390 }
 391
 392 static int dpmv8_instr_read_data_r0_64(struct arm_dpm *dpm,
 393         uint32_t opcode, uint64_t *data)
 394 {
 395         struct armv8_common *armv8 = dpm->arm->arch_info;
 396         int retval;
 397
 398         if (dpm->arm->core_state != ARM_STATE_AARCH64) {
 399                 uint32_t tmp;
 400                 retval = dpmv8_instr_read_data_r0(dpm, opcode, &tmp);
 401                 if (retval == ERROR_OK)
 402                         *data = tmp;
 403                 return retval;
 404         }
 405
 406         /* the opcode, writing data to R0 */
 407         retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 408         if (retval != ERROR_OK)
 409                 return retval;
 410
 411         /* write R0 to DCC */
 412         retval = dpmv8_exec_opcode(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dpm->dscr);
 413         if (retval != ERROR_OK)
 414                 return retval;
 415
 416         return dpmv8_read_dcc_64(armv8, data, &dpm->dscr);
 417 }
 418
 419 #if 0
 420 static int dpmv8_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
 421         target_addr_t addr, uint32_t control)
 422 {
 423         struct armv8_common *armv8 = dpm->arm->arch_info;
 424         uint32_t vr = armv8->debug_base;
 425         uint32_t cr = armv8->debug_base;
 426         int retval;
 427
 428         switch (index_t) {
 429                 case 0 ... 15:  /* breakpoints */
 430                         vr += CPUV8_DBG_BVR_BASE;
 431                         cr += CPUV8_DBG_BCR_BASE;
 432                         break;
 433                 case 16 ... 31: /* watchpoints */
 434                         vr += CPUV8_DBG_WVR_BASE;
 435                         cr += CPUV8_DBG_WCR_BASE;
 436                         index_t -= 16;
 437                         break;
 438                 default:
 439                         return ERROR_FAIL;
 440         }
 441         vr += 16 * index_t;
 442         cr += 16 * index_t;
 443
 444         LOG_DEBUG("A8: bpwp enable, vr %08x cr %08x",
 445                 (unsigned) vr, (unsigned) cr);
 446
 447         retval = mem_ap_write_atomic_u32(armv8->debug_ap, vr, addr);
 448         if (retval != ERROR_OK)
 449                 return retval;
 450         return mem_ap_write_atomic_u32(armv8->debug_ap, cr, control);
 451 }
 452 #endif
 453
 454 static int dpmv8_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
 455 {
 456         struct armv8_common *armv8 = dpm->arm->arch_info;
 457         uint32_t cr;
 458
 459         switch (index_t) {
 460                 case 0 ... 15:
 461                         cr = armv8->debug_base + CPUV8_DBG_BCR_BASE;
 462                         break;
 463                 case 16 ... 31:
 464                         cr = armv8->debug_base + CPUV8_DBG_WCR_BASE;
 465                         index_t -= 16;
 466                         break;
 467                 default:
 468                         return ERROR_FAIL;
 469         }
 470         cr += 16 * index_t;
 471
 472         LOG_DEBUG("A: bpwp disable, cr %08x", (unsigned) cr);
 473
 474         /* clear control register */
 475         return mem_ap_write_atomic_u32(armv8->debug_ap, cr, 0);
 476 }
 477
 478 /*
 479  * Coprocessor support
 480  */
 481
 482 /* Read coprocessor */
 483 static int dpmv8_mrc(struct target *target, int cpnum,
 484         uint32_t op1, uint32_t op2, uint32_t crn, uint32_t crm,
 485         uint32_t *value)
 486 {
 487         struct arm *arm = target_to_arm(target);
 488         struct arm_dpm *dpm = arm->dpm;
 489         int retval;
 490
 491         retval = dpm->prepare(dpm);
 492         if (retval != ERROR_OK)
 493                 return retval;
 494
 495         LOG_DEBUG("MRC p%d, %d, r0, c%d, c%d, %d", cpnum,
 496                 (int) op1, (int) crn,
 497                 (int) crm, (int) op2);
 498
 499         /* read coprocessor register into R0; return via DCC */
 500         retval = dpm->instr_read_data_r0(dpm,
 501                         ARMV4_5_MRC(cpnum, op1, 0, crn, crm, op2),
 502                         value);
 503
 504         /* (void) */ dpm->finish(dpm);
 505         return retval;
 506 }
 507
 508 static int dpmv8_mcr(struct target *target, int cpnum,
 509         uint32_t op1, uint32_t op2, uint32_t crn, uint32_t crm,
 510         uint32_t value)
 511 {
 512         struct arm *arm = target_to_arm(target);
 513         struct arm_dpm *dpm = arm->dpm;
 514         int retval;
 515
 516         retval = dpm->prepare(dpm);
 517         if (retval != ERROR_OK)
 518                 return retval;
 519
 520         LOG_DEBUG("MCR p%d, %d, r0, c%d, c%d, %d", cpnum,
 521                 (int) op1, (int) crn,
 522                 (int) crm, (int) op2);
 523
 524         /* read DCC into r0; then write coprocessor register from R0 */
 525         retval = dpm->instr_write_data_r0(dpm,
 526                         ARMV4_5_MCR(cpnum, op1, 0, crn, crm, op2),
 527                         value);
 528
 529         /* (void) */ dpm->finish(dpm);
 530         return retval;
 531 }
 532
 533 /*----------------------------------------------------------------------*/
 534
 535 /*
 536  * Register access utilities
 537  */
 538
 539 int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 540 {
 541         struct armv8_common *armv8 = (struct armv8_common *)dpm->arm->arch_info;
 542         int retval = ERROR_OK;
 543         unsigned int target_el;
 544         enum arm_state core_state;
 545         uint32_t cpsr;
 546
 547         /* restore previous mode */
 548         if (mode == ARM_MODE_ANY) {
 549                 cpsr = buf_get_u32(dpm->arm->cpsr->value, 0, 32);
 550
 551                 LOG_DEBUG("restoring mode, cpsr = 0x%08"PRIx32, cpsr);
 552
 553         } else {
 554                 LOG_DEBUG("setting mode 0x%x", mode);
 555                 cpsr = mode;
 556         }
 557
 558         switch (cpsr & 0x1f) {
 559         /* aarch32 modes */
 560         case ARM_MODE_USR:
 561                 target_el = 0;
 562                 break;
 563         case ARM_MODE_SVC:
 564         case ARM_MODE_ABT:
 565         case ARM_MODE_IRQ:
 566         case ARM_MODE_FIQ:
 567         case ARM_MODE_SYS:
 568                 target_el = 1;
 569                 break;
 570         /*
 571          * TODO: handle ARM_MODE_HYP
 572          * case ARM_MODE_HYP:
 573          *      target_el = 2;
 574          *      break;
 575          */
 576         case ARM_MODE_MON:
 577                 target_el = 3;
 578                 break;
 579         /* aarch64 modes */
 580         default:
 581                 target_el = (cpsr >> 2) & 3;
 582         }
 583
 584         if (target_el > SYSTEM_CUREL_EL3) {
 585                 LOG_ERROR("%s: Invalid target exception level %i", __func__, target_el);
 586                 return ERROR_FAIL;
 587         }
 588
 589         LOG_DEBUG("target_el = %i, last_el = %i", target_el, dpm->last_el);
 590         if (target_el > dpm->last_el) {
 591                 retval = dpm->instr_execute(dpm,
 592                                 armv8_opcode(armv8, ARMV8_OPC_DCPS) | target_el);
 593
 594                 /* DCPS clobbers registers just like an exception taken */
 595                 armv8_dpm_handle_exception(dpm, false);
 596         } else {
 597                 core_state = armv8_dpm_get_core_state(dpm);
 598                 if (core_state != ARM_STATE_AARCH64) {
 599                         /* cannot do DRPS/ERET when already in EL0 */
 600                         if (dpm->last_el != 0) {
 601                                 /* load SPSR with the desired mode and execute DRPS */
 602                                 LOG_DEBUG("SPSR = 0x%08"PRIx32, cpsr);
 603                                 retval = dpm->instr_write_data_r0(dpm,
 604                                                 ARMV8_MSR_GP_XPSR_T1(1, 0, 15), cpsr);
 605                                 if (retval == ERROR_OK)
 606                                         retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_DRPS));
 607                         }
 608                 } else {
 609                         /*
 610                          * need to execute multiple DRPS instructions until target_el
 611                          * is reached
 612                          */
 613                         while (retval == ERROR_OK && dpm->last_el != target_el) {
 614                                 unsigned int cur_el = dpm->last_el;
 615                                 retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_DRPS));
 616                                 if (cur_el == dpm->last_el) {
 617                                         LOG_INFO("Cannot reach EL %i, SPSR corrupted?", target_el);
 618                                         break;
 619                                 }
 620                         }
 621                 }
 622
 623                 /* On executing DRPS, DSPSR and DLR become UNKNOWN, mark them as dirty */
 624                 dpm->arm->cpsr->dirty = true;
 625                 dpm->arm->pc->dirty = true;
 626
 627                 /*
 628                  * re-evaluate the core state, we might be in Aarch32 state now
 629                  * we rely on dpm->dscr being up-to-date
 630                  */
 631                 core_state = armv8_dpm_get_core_state(dpm);
 632                 armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
 633                 armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
 634         }
 635
 636         return retval;
 637 }
 638
 639 /*
 640  * Common register read, relies on armv8_select_reg_access() having been called.
 641  */
 642 static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 643 {
 644         struct armv8_common *armv8 = dpm->arm->arch_info;
 645         int retval = ERROR_FAIL;
 646
 647         if (r->size <= 64) {
 648                 uint64_t value_64;
 649                 retval = armv8->read_reg_u64(armv8, regnum, &value_64);
 650
 651                 if (retval == ERROR_OK) {
 652                         r->valid = true;
 653                         r->dirty = false;
 654                         buf_set_u64(r->value, 0, r->size, value_64);
 655                         if (r->size == 64)
 656                                 LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
 657                         else
 658                                 LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned int) value_64);
 659                 }
 660         } else if (r->size <= 128) {
 661                 uint64_t lvalue = 0, hvalue = 0;
 662                 retval = armv8->read_reg_u128(armv8, regnum, &lvalue, &hvalue);
 663
 664                 if (retval == ERROR_OK) {
 665                         r->valid = true;
 666                         r->dirty = false;
 667
 668                         buf_set_u64(r->value, 0, 64, lvalue);
 669                         buf_set_u64(r->value + 8, 0, r->size - 64, hvalue);
 670
 671                         LOG_DEBUG("READ: %s, lvalue=%16.8llx", r->name, (unsigned long long) lvalue);
 672                         LOG_DEBUG("READ: %s, hvalue=%16.8llx", r->name, (unsigned long long) hvalue);
 673                 }
 674         }
 675
 676         if (retval != ERROR_OK)
 677                 LOG_ERROR("Failed to read %s register", r->name);
 678
 679         return retval;
 680 }
 681
 682 /*
 683  * Common register write, relies on armv8_select_reg_access() having been called.
 684  */
 685 static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 686 {
 687         struct armv8_common *armv8 = dpm->arm->arch_info;
 688         int retval = ERROR_FAIL;
 689
 690         if (r->size <= 64) {
 691                 uint64_t value_64;
 692
 693                 value_64 = buf_get_u64(r->value, 0, r->size);
 694                 retval = armv8->write_reg_u64(armv8, regnum, value_64);
 695
 696                 if (retval == ERROR_OK) {
 697                         r->dirty = false;
 698                         if (r->size == 64)
 699                                 LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long)value_64);
 700                         else
 701                                 LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned int)value_64);
 702                 }
 703         } else if (r->size <= 128) {
 704                 uint64_t lvalue, hvalue;
 705
 706                 lvalue = buf_get_u64(r->value, 0, 64);
 707                 hvalue = buf_get_u64(r->value + 8, 0, r->size - 64);
 708                 retval = armv8->write_reg_u128(armv8, regnum, lvalue, hvalue);
 709
 710                 if (retval == ERROR_OK) {
 711                         r->dirty = false;
 712
 713                         LOG_DEBUG("WRITE: %s, lvalue=%16.8llx", r->name, (unsigned long long) lvalue);
 714                         LOG_DEBUG("WRITE: %s, hvalue=%16.8llx", r->name, (unsigned long long) hvalue);
 715                 }
 716         }
 717
 718         if (retval != ERROR_OK)
 719                 LOG_ERROR("Failed to write %s register", r->name);
 720
 721         return retval;
 722 }
 723
 724 /**
 725  * Read basic registers of the current context:  R0 to R15, and CPSR;
 726  * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
 727  * In normal operation this is called on entry to halting debug state,
 728  * possibly after some other operations supporting restore of debug state
 729  * or making sure the CPU is fully idle (drain write buffer, etc).
 730  */
 731 int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 732 {
 733         struct arm *arm = dpm->arm;
 734         struct armv8_common *armv8 = (struct armv8_common *)arm->arch_info;
 735         struct reg_cache *cache;
 736         struct reg *r;
 737         uint32_t cpsr;
 738         int retval;
 739
 740         retval = dpm->prepare(dpm);
 741         if (retval != ERROR_OK)
 742                 return retval;
 743
 744         cache = arm->core_cache;
 745
 746         /* read R0 first (it's used for scratch), then CPSR */
 747         r = cache->reg_list + ARMV8_R0;
 748         if (!r->valid) {
 749                 retval = dpmv8_read_reg(dpm, r, ARMV8_R0);
 750                 if (retval != ERROR_OK)
 751                         goto fail;
 752         }
 753         r->dirty = true;
 754
 755         /* read R1, too, it will be clobbered during memory access */
 756         r = cache->reg_list + ARMV8_R1;
 757         if (!r->valid) {
 758                 retval = dpmv8_read_reg(dpm, r, ARMV8_R1);
 759                 if (retval != ERROR_OK)
 760                         goto fail;
 761         }
 762
 763         /* read cpsr to r0 and get it back */
 764         retval = dpm->instr_read_data_r0(dpm,
 765                         armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
 766         if (retval != ERROR_OK)
 767                 goto fail;
 768
 769         /* update core mode and state */
 770         armv8_set_cpsr(arm, cpsr);
 771
 772         for (unsigned int i = ARMV8_PC; i < cache->num_regs ; i++) {
 773                 struct arm_reg *arm_reg;
 774
 775                 r = armv8_reg_current(arm, i);
 776                 if (!r->exist || r->valid)
 777                         continue;
 778
 779                 /* Skip reading FP-SIMD registers */
 780                 if (r->number >= ARMV8_V0 && r->number <= ARMV8_FPCR)
 781                         continue;
 782
 783                 /*
 784                  * Only read registers that are available from the
 785                  * current EL (or core mode).
 786                  */
 787                 arm_reg = r->arch_info;
 788                 if (arm_reg->mode != ARM_MODE_ANY &&
 789                                 dpm->last_el != armv8_curel_from_core_mode(arm_reg->mode))
 790                         continue;
 791
 792                 /* Special case: ARM_MODE_SYS has no SPSR at EL1 */
 793                 if (r->number == ARMV8_SPSR_EL1 && arm->core_mode == ARM_MODE_SYS)
 794                         continue;
 795
 796                 retval = dpmv8_read_reg(dpm, r, i);
 797                 if (retval != ERROR_OK)
 798                         goto fail;
 799
 800         }
 801
 802 fail:
 803         dpm->finish(dpm);
 804         return retval;
 805 }
 806
 807 /* Avoid needless I/O ... leave breakpoints and watchpoints alone
 808  * unless they're removed, or need updating because of single-stepping
 809  * or running debugger code.
 810  */
 811 static int dpmv8_maybe_update_bpwp(struct arm_dpm *dpm, bool bpwp,
 812         struct dpm_bpwp *xp, bool *set_p)
 813 {
 814         int retval = ERROR_OK;
 815         bool disable;
 816
 817         if (!set_p) {
 818                 if (!xp->dirty)
 819                         goto done;
 820                 xp->dirty = false;
 821                 /* removed or startup; we must disable it */
 822                 disable = true;
 823         } else if (bpwp) {
 824                 if (!xp->dirty)
 825                         goto done;
 826                 /* disabled, but we must set it */
 827                 xp->dirty = disable = false;
 828                 *set_p = true;
 829         } else {
 830                 if (!*set_p)
 831                         goto done;
 832                 /* set, but we must temporarily disable it */
 833                 xp->dirty = disable = true;
 834                 *set_p = false;
 835         }
 836
 837         if (disable)
 838                 retval = dpm->bpwp_disable(dpm, xp->number);
 839         else
 840                 retval = dpm->bpwp_enable(dpm, xp->number,
 841                                 xp->address, xp->control);
 842
 843         if (retval != ERROR_OK)
 844                 LOG_ERROR("%s: can't %s HW %spoint %d",
 845                         disable ? "disable" : "enable",
 846                         target_name(dpm->arm->target),
 847                         (xp->number < 16) ? "break" : "watch",
 848                         xp->number & 0xf);
 849 done:
 850         return retval;
 851 }
 852
 853 static int dpmv8_add_breakpoint(struct target *target, struct breakpoint *bp);
 854
 855 /**
 856  * Writes all modified core registers for all processor modes.  In normal
 857  * operation this is called on exit from halting debug state.
 858  *
 859  * @param dpm: represents the processor
 860  * @param bpwp: true ensures breakpoints and watchpoints are set,
 861  *      false ensures they are cleared
 862  */
 863 int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 864 {
 865         struct arm *arm = dpm->arm;
 866         struct reg_cache *cache = arm->core_cache;
 867         int retval;
 868
 869         retval = dpm->prepare(dpm);
 870         if (retval != ERROR_OK)
 871                 goto done;
 872
 873         /* If we're managing hardware breakpoints for this core, enable
 874          * or disable them as requested.
 875          *
 876          * REVISIT We don't yet manage them for ANY cores.  Eventually
 877          * we should be able to assume we handle them; but until then,
 878          * cope with the hand-crafted breakpoint code.
 879          */
 880         if (arm->target->type->add_breakpoint == dpmv8_add_breakpoint) {
 881                 for (unsigned i = 0; i < dpm->nbp; i++) {
 882                         struct dpm_bp *dbp = dpm->dbp + i;
 883                         struct breakpoint *bp = dbp->bp;
 884
 885                         retval = dpmv8_maybe_update_bpwp(dpm, bpwp, &dbp->bpwp,
 886                                         bp ? &bp->is_set : NULL);
 887                         if (retval != ERROR_OK)
 888                                 goto done;
 889                 }
 890         }
 891
 892         /* enable/disable watchpoints */
 893         for (unsigned i = 0; i < dpm->nwp; i++) {
 894                 struct dpm_wp *dwp = dpm->dwp + i;
 895                 struct watchpoint *wp = dwp->wp;
 896
 897                 retval = dpmv8_maybe_update_bpwp(dpm, bpwp, &dwp->bpwp,
 898                                 wp ? &wp->is_set : NULL);
 899                 if (retval != ERROR_OK)
 900                         goto done;
 901         }
 902
 903         /* NOTE:  writes to breakpoint and watchpoint registers might
 904          * be queued, and need (efficient/batched) flushing later.
 905          */
 906
 907         /* Restore original core mode and state */
 908         retval = armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 909         if (retval != ERROR_OK)
 910                 goto done;
 911
 912         /* check everything except our scratch register R0 */
 913         for (unsigned i = 1; i < cache->num_regs; i++) {
 914                 struct arm_reg *r;
 915
 916                 /* skip non-existent */
 917                 if (!cache->reg_list[i].exist)
 918                         continue;
 919                 /* skip PC and CPSR */
 920                 if (i == ARMV8_PC || i == ARMV8_XPSR)
 921                         continue;
 922                 /* skip invalid */
 923                 if (!cache->reg_list[i].valid)
 924                         continue;
 925                 /* skip non-dirty */
 926                 if (!cache->reg_list[i].dirty)
 927                         continue;
 928
 929                 /* skip all registers not on the current EL */
 930                 r = cache->reg_list[i].arch_info;
 931                 if (r->mode != ARM_MODE_ANY &&
 932                                 dpm->last_el != armv8_curel_from_core_mode(r->mode))
 933                         continue;
 934
 935                 retval = dpmv8_write_reg(dpm, &cache->reg_list[i], i);
 936                 if (retval != ERROR_OK)
 937                         break;
 938         }
 939
 940         /* flush CPSR and PC */
 941         if (retval == ERROR_OK)
 942                 retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_XPSR], ARMV8_XPSR);
 943         if (retval == ERROR_OK)
 944                 retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_PC], ARMV8_PC);
 945         /* flush R0 -- it's *very* dirty by now */
 946         if (retval == ERROR_OK)
 947                 retval = dpmv8_write_reg(dpm, &cache->reg_list[0], 0);
 948         if (retval == ERROR_OK)
 949                 dpm->instr_cpsr_sync(dpm);
 950 done:
 951         dpm->finish(dpm);
 952         return retval;
 953 }
 954
 955 /*
 956  * Standard ARM register accessors ... there are three methods
 957  * in "struct arm", to support individual read/write and bulk read
 958  * of registers.
 959  */
 960
 961 static int armv8_dpm_read_core_reg(struct target *target, struct reg *r,
 962         int regnum, enum arm_mode mode)
 963 {
 964         struct arm *arm = target_to_arm(target);
 965         struct arm_dpm *dpm = target_to_arm(target)->dpm;
 966         int retval;
 967         int max = arm->core_cache->num_regs;
 968
 969         if (regnum < 0 || regnum >= max)
 970                 return ERROR_COMMAND_SYNTAX_ERROR;
 971
 972         /*
 973          * REVISIT what happens if we try to read SPSR in a core mode
 974          * which has no such register?
 975          */
 976         retval = dpm->prepare(dpm);
 977         if (retval != ERROR_OK)
 978                 return retval;
 979
 980         retval = dpmv8_read_reg(dpm, r, regnum);
 981         if (retval != ERROR_OK)
 982                 goto fail;
 983
 984 fail:
 985         /* (void) */ dpm->finish(dpm);
 986         return retval;
 987 }
 988
 989 static int armv8_dpm_write_core_reg(struct target *target, struct reg *r,
 990         int regnum, enum arm_mode mode, uint8_t *value)
 991 {
 992         struct arm *arm = target_to_arm(target);
 993         struct arm_dpm *dpm = target_to_arm(target)->dpm;
 994         int retval;
 995         int max = arm->core_cache->num_regs;
 996
 997         if (regnum < 0 || regnum > max)
 998                 return ERROR_COMMAND_SYNTAX_ERROR;
 999
1000         /* REVISIT what happens if we try to write SPSR in a core mode
1001          * which has no such register?
1002          */
1003
1004         retval = dpm->prepare(dpm);
1005         if (retval != ERROR_OK)
1006                 return retval;
1007
1008         retval = dpmv8_write_reg(dpm, r, regnum);
1009
1010         /* always clean up, regardless of error */
1011         dpm->finish(dpm);
1012
1013         return retval;
1014 }
1015
1016 static int armv8_dpm_full_context(struct target *target)
1017 {
1018         struct arm *arm = target_to_arm(target);
1019         struct arm_dpm *dpm = arm->dpm;
1020         struct reg_cache *cache = arm->core_cache;
1021         int retval;
1022         bool did_read;
1023
1024         retval = dpm->prepare(dpm);
1025         if (retval != ERROR_OK)
1026                 goto done;
1027
1028         do {
1029                 enum arm_mode mode = ARM_MODE_ANY;
1030
1031                 did_read = false;
1032
1033                 /* We "know" arm_dpm_read_current_registers() was called so
1034                  * the unmapped registers (R0..R7, PC, AND CPSR) and some
1035                  * view of R8..R14 are current.  We also "know" oddities of
1036                  * register mapping: special cases for R8..R12 and SPSR.
1037                  *
1038                  * Pick some mode with unread registers and read them all.
1039                  * Repeat until done.
1040                  */
1041                 for (unsigned i = 0; i < cache->num_regs; i++) {
1042                         struct arm_reg *r;
1043
1044                         if (!cache->reg_list[i].exist || cache->reg_list[i].valid)
1045                                 continue;
1046                         r = cache->reg_list[i].arch_info;
1047
1048                         /* may need to pick a mode and set CPSR */
1049                         if (!did_read) {
1050                                 did_read = true;
1051                                 mode = r->mode;
1052
1053                                 /* For regular (ARM_MODE_ANY) R8..R12
1054                                  * in case we've entered debug state
1055                                  * in FIQ mode we need to patch mode.
1056                                  */
1057                                 if (mode != ARM_MODE_ANY)
1058                                         retval = armv8_dpm_modeswitch(dpm, mode);
1059                                 else
1060                                         retval = armv8_dpm_modeswitch(dpm, ARM_MODE_USR);
1061
1062                                 if (retval != ERROR_OK)
1063                                         goto done;
1064                         }
1065                         if (r->mode != mode)
1066                                 continue;
1067
1068                         /* CPSR was read, so "R16" must mean SPSR */
1069                         retval = dpmv8_read_reg(dpm,
1070                                         &cache->reg_list[i],
1071                                         (r->num == 16) ? 17 : r->num);
1072                         if (retval != ERROR_OK)
1073                                 goto done;
1074                 }
1075
1076         } while (did_read);
1077
1078         retval = armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
1079         /* (void) */ dpm->finish(dpm);
1080 done:
1081         return retval;
1082 }
1083
1084
1085 /*----------------------------------------------------------------------*/
1086
1087 /*
1088  * Breakpoint and Watchpoint support.
1089  *
1090  * Hardware {break,watch}points are usually left active, to minimize
1091  * debug entry/exit costs.  When they are set or cleared, it's done in
1092  * batches.  Also, DPM-conformant hardware can update debug registers
1093  * regardless of whether the CPU is running or halted ... though that
1094  * fact isn't currently leveraged.
1095  */
1096
1097 static int dpmv8_bpwp_setup(struct arm_dpm *dpm, struct dpm_bpwp *xp,
1098         uint32_t addr, uint32_t length)
1099 {
1100         uint32_t control;
1101
1102         control = (1 << 0)      /* enable */
1103                 | (3 << 1);     /* both user and privileged access */
1104
1105         /* Match 1, 2, or all 4 byte addresses in this word.
1106          *
1107          * FIXME:  v7 hardware allows lengths up to 2 GB for BP and WP.
1108          * Support larger length, when addr is suitably aligned.  In
1109          * particular, allow watchpoints on 8 byte "double" values.
1110          *
1111          * REVISIT allow watchpoints on unaligned 2-bit values; and on
1112          * v7 hardware, unaligned 4-byte ones too.
1113          */
1114         switch (length) {
1115                 case 1:
1116                         control |= (1 << (addr & 3)) << 5;
1117                         break;
1118                 case 2:
1119                         /* require 2-byte alignment */
1120                         if (!(addr & 1)) {
1121                                 control |= (3 << (addr & 2)) << 5;
1122                                 break;
1123                         }
1124                 /* FALL THROUGH */
1125                 case 4:
1126                         /* require 4-byte alignment */
1127                         if (!(addr & 3)) {
1128                                 control |= 0xf << 5;
1129                                 break;
1130                         }
1131                 /* FALL THROUGH */
1132                 default:
1133                         LOG_ERROR("unsupported {break,watch}point length/alignment");
1134                         return ERROR_COMMAND_SYNTAX_ERROR;
1135         }
1136
1137         /* other shared control bits:
1138          * bits 15:14 == 0 ... both secure and nonsecure states (v6.1+ only)
1139          * bit 20 == 0 ... not linked to a context ID
1140          * bit 28:24 == 0 ... not ignoring N LSBs (v7 only)
1141          */
1142
1143         xp->address = addr & ~3;
1144         xp->control = control;
1145         xp->dirty = true;
1146
1147         LOG_DEBUG("BPWP: addr %8.8" PRIx32 ", control %" PRIx32 ", number %d",
1148                 xp->address, control, xp->number);
1149
1150         /* hardware is updated in write_dirty_registers() */
1151         return ERROR_OK;
1152 }
1153
1154 static int dpmv8_add_breakpoint(struct target *target, struct breakpoint *bp)
1155 {
1156         struct arm *arm = target_to_arm(target);
1157         struct arm_dpm *dpm = arm->dpm;
1158         int retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1159
1160         if (bp->length < 2)
1161                 return ERROR_COMMAND_SYNTAX_ERROR;
1162         if (!dpm->bpwp_enable)
1163                 return retval;
1164
1165         /* FIXME we need a generic solution for software breakpoints. */
1166         if (bp->type == BKPT_SOFT)
1167                 LOG_DEBUG("using HW bkpt, not SW...");
1168
1169         for (unsigned i = 0; i < dpm->nbp; i++) {
1170                 if (!dpm->dbp[i].bp) {
1171                         retval = dpmv8_bpwp_setup(dpm, &dpm->dbp[i].bpwp,
1172                                         bp->address, bp->length);
1173                         if (retval == ERROR_OK)
1174                                 dpm->dbp[i].bp = bp;
1175                         break;
1176                 }
1177         }
1178
1179         return retval;
1180 }
1181
1182 static int dpmv8_remove_breakpoint(struct target *target, struct breakpoint *bp)
1183 {
1184         struct arm *arm = target_to_arm(target);
1185         struct arm_dpm *dpm = arm->dpm;
1186         int retval = ERROR_COMMAND_SYNTAX_ERROR;
1187
1188         for (unsigned i = 0; i < dpm->nbp; i++) {
1189                 if (dpm->dbp[i].bp == bp) {
1190                         dpm->dbp[i].bp = NULL;
1191                         dpm->dbp[i].bpwp.dirty = true;
1192
1193                         /* hardware is updated in write_dirty_registers() */
1194                         retval = ERROR_OK;
1195                         break;
1196                 }
1197         }
1198
1199         return retval;
1200 }
1201
1202 static int dpmv8_watchpoint_setup(struct arm_dpm *dpm, unsigned index_t,
1203         struct watchpoint *wp)
1204 {
1205         int retval;
1206         struct dpm_wp *dwp = dpm->dwp + index_t;
1207         uint32_t control;
1208
1209         /* this hardware doesn't support data value matching or masking */
1210         if (wp->value || wp->mask != ~(uint32_t)0) {
1211                 LOG_DEBUG("watchpoint values and masking not supported");
1212                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1213         }
1214
1215         retval = dpmv8_bpwp_setup(dpm, &dwp->bpwp, wp->address, wp->length);
1216         if (retval != ERROR_OK)
1217                 return retval;
1218
1219         control = dwp->bpwp.control;
1220         switch (wp->rw) {
1221                 case WPT_READ:
1222                         control |= 1 << 3;
1223                         break;
1224                 case WPT_WRITE:
1225                         control |= 2 << 3;
1226                         break;
1227                 case WPT_ACCESS:
1228                         control |= 3 << 3;
1229                         break;
1230         }
1231         dwp->bpwp.control = control;
1232
1233         dpm->dwp[index_t].wp = wp;
1234
1235         return retval;
1236 }
1237
1238 static int dpmv8_add_watchpoint(struct target *target, struct watchpoint *wp)
1239 {
1240         struct arm *arm = target_to_arm(target);
1241         struct arm_dpm *dpm = arm->dpm;
1242         int retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1243
1244         if (dpm->bpwp_enable) {
1245                 for (unsigned i = 0; i < dpm->nwp; i++) {
1246                         if (!dpm->dwp[i].wp) {
1247                                 retval = dpmv8_watchpoint_setup(dpm, i, wp);
1248                                 break;
1249                         }
1250                 }
1251         }
1252
1253         return retval;
1254 }
1255
1256 static int dpmv8_remove_watchpoint(struct target *target, struct watchpoint *wp)
1257 {
1258         struct arm *arm = target_to_arm(target);
1259         struct arm_dpm *dpm = arm->dpm;
1260         int retval = ERROR_COMMAND_SYNTAX_ERROR;
1261
1262         for (unsigned i = 0; i < dpm->nwp; i++) {
1263                 if (dpm->dwp[i].wp == wp) {
1264                         dpm->dwp[i].wp = NULL;
1265                         dpm->dwp[i].bpwp.dirty = true;
1266
1267                         /* hardware is updated in write_dirty_registers() */
1268                         retval = ERROR_OK;
1269                         break;
1270                 }
1271         }
1272
1273         return retval;
1274 }
1275
1276 /*
1277  * Handle exceptions taken in debug state. This happens mostly for memory
1278  * accesses that violated a MMU policy. Taking an exception while in debug
1279  * state clobbers certain state registers on the target exception level.
1280  * Just mark those registers dirty so that they get restored on resume.
1281  * This works both for Aarch32 and Aarch64 states.
1282  *
1283  * This function must not perform any actions that trigger another exception
1284  * or a recursion will happen.
1285  */
1286 void armv8_dpm_handle_exception(struct arm_dpm *dpm, bool do_restore)
1287 {
1288         struct armv8_common *armv8 = dpm->arm->arch_info;
1289         struct reg_cache *cache = dpm->arm->core_cache;
1290         enum arm_state core_state;
1291         uint64_t dlr;
1292         uint32_t dspsr;
1293         unsigned int el;
1294
1295         static const int clobbered_regs_by_el[3][5] = {
1296                 { ARMV8_PC, ARMV8_XPSR, ARMV8_ELR_EL1, ARMV8_ESR_EL1, ARMV8_SPSR_EL1 },
1297                 { ARMV8_PC, ARMV8_XPSR, ARMV8_ELR_EL2, ARMV8_ESR_EL2, ARMV8_SPSR_EL2 },
1298                 { ARMV8_PC, ARMV8_XPSR, ARMV8_ELR_EL3, ARMV8_ESR_EL3, ARMV8_SPSR_EL3 },
1299         };
1300
1301         el = (dpm->dscr >> 8) & 3;
1302
1303         /* safety check, must not happen since EL0 cannot be a target for an exception */
1304         if (el < SYSTEM_CUREL_EL1 || el > SYSTEM_CUREL_EL3) {
1305                 LOG_ERROR("%s: EL %i is invalid, DSCR corrupted?", __func__, el);
1306                 return;
1307         }
1308
1309         /* Clear sticky error */
1310         mem_ap_write_u32(armv8->debug_ap,
1311                 armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
1312
1313         armv8->read_reg_u64(armv8, ARMV8_XPSR, &dlr);
1314         dspsr = dlr;
1315         armv8->read_reg_u64(armv8, ARMV8_PC, &dlr);
1316
1317         LOG_DEBUG("Exception taken to EL %i, DLR=0x%016"PRIx64" DSPSR=0x%08"PRIx32,
1318                         el, dlr, dspsr);
1319
1320         /* mark all clobbered registers as dirty */
1321         for (int i = 0; i < 5; i++)
1322                 cache->reg_list[clobbered_regs_by_el[el-1][i]].dirty = true;
1323
1324         /*
1325          * re-evaluate the core state, we might be in Aarch64 state now
1326          * we rely on dpm->dscr being up-to-date
1327          */
1328         core_state = armv8_dpm_get_core_state(dpm);
1329         armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
1330         armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
1331
1332         if (do_restore)
1333                 armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
1334 }
1335
1336 /*----------------------------------------------------------------------*/
1337
1338 /*
1339  * Other debug and support utilities
1340  */
1341
1342 void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
1343 {
1344         struct target *target = dpm->arm->target;
1345
1346         dpm->dscr = dscr;
1347         dpm->last_el = (dscr >> 8) & 3;
1348
1349         /* Examine debug reason */
1350         switch (DSCR_ENTRY(dscr)) {
1351                 /* FALL THROUGH -- assume a v6 core in abort mode */
1352                 case DSCRV8_ENTRY_EXT_DEBUG:    /* EDBGRQ */
1353                         target->debug_reason = DBG_REASON_DBGRQ;
1354                         break;
1355                 case DSCRV8_ENTRY_HALT_STEP_EXECLU:     /* HALT step */
1356                 case DSCRV8_ENTRY_HALT_STEP_NORMAL: /* Halt step*/
1357                 case DSCRV8_ENTRY_HALT_STEP:
1358                         target->debug_reason = DBG_REASON_SINGLESTEP;
1359                         break;
1360                 case DSCRV8_ENTRY_HLT:  /* HLT instruction (software breakpoint) */
1361                 case DSCRV8_ENTRY_BKPT: /* SW BKPT (?) */
1362                 case DSCRV8_ENTRY_RESET_CATCH:  /* Reset catch */
1363                 case DSCRV8_ENTRY_OS_UNLOCK:  /*OS unlock catch*/
1364                 case DSCRV8_ENTRY_SW_ACCESS_DBG: /*SW access dbg register*/
1365                         target->debug_reason = DBG_REASON_BREAKPOINT;
1366                         break;
1367                 case DSCRV8_ENTRY_WATCHPOINT:   /* asynch watchpoint */
1368                         target->debug_reason = DBG_REASON_WATCHPOINT;
1369                         break;
1370                 case DSCRV8_ENTRY_EXCEPTION_CATCH:  /*exception catch*/
1371                         target->debug_reason = DBG_REASON_EXC_CATCH;
1372                         break;
1373                 default:
1374                         target->debug_reason = DBG_REASON_UNDEFINED;
1375                         break;
1376         }
1377
1378 }
1379
1380 /*----------------------------------------------------------------------*/
1381
1382 /*
1383  * Setup and management support.
1384  */
1385
1386 /**
1387  * Hooks up this DPM to its associated target; call only once.
1388  * Initially this only covers the register cache.
1389  *
1390  * Oh, and watchpoints.  Yeah.
1391  */
1392 int armv8_dpm_setup(struct arm_dpm *dpm)
1393 {
1394         struct arm *arm = dpm->arm;
1395         struct target *target = arm->target;
1396         struct reg_cache *cache;
1397         arm->dpm = dpm;
1398
1399         /* register access setup */
1400         arm->full_context = armv8_dpm_full_context;
1401         arm->read_core_reg = armv8_dpm_read_core_reg;
1402         arm->write_core_reg = armv8_dpm_write_core_reg;
1403
1404         if (!arm->core_cache) {
1405                 cache = armv8_build_reg_cache(target);
1406                 if (!cache)
1407                         return ERROR_FAIL;
1408         }
1409
1410         /* coprocessor access setup */
1411         arm->mrc = dpmv8_mrc;
1412         arm->mcr = dpmv8_mcr;
1413
1414         dpm->prepare = dpmv8_dpm_prepare;
1415         dpm->finish = dpmv8_dpm_finish;
1416
1417         dpm->instr_execute = dpmv8_instr_execute;
1418         dpm->instr_write_data_dcc = dpmv8_instr_write_data_dcc;
1419         dpm->instr_write_data_dcc_64 = dpmv8_instr_write_data_dcc_64;
1420         dpm->instr_write_data_r0 = dpmv8_instr_write_data_r0;
1421         dpm->instr_write_data_r0_64 = dpmv8_instr_write_data_r0_64;
1422         dpm->instr_cpsr_sync = dpmv8_instr_cpsr_sync;
1423
1424         dpm->instr_read_data_dcc = dpmv8_instr_read_data_dcc;
1425         dpm->instr_read_data_dcc_64 = dpmv8_instr_read_data_dcc_64;
1426         dpm->instr_read_data_r0 = dpmv8_instr_read_data_r0;
1427         dpm->instr_read_data_r0_64 = dpmv8_instr_read_data_r0_64;
1428
1429         dpm->arm_reg_current = armv8_reg_current;
1430
1431 /*      dpm->bpwp_enable = dpmv8_bpwp_enable; */
1432         dpm->bpwp_disable = dpmv8_bpwp_disable;
1433
1434         /* breakpoint setup -- optional until it works everywhere */
1435         if (!target->type->add_breakpoint) {
1436                 target->type->add_breakpoint = dpmv8_add_breakpoint;
1437                 target->type->remove_breakpoint = dpmv8_remove_breakpoint;
1438         }
1439
1440         /* watchpoint setup */
1441         if (!target->type->add_watchpoint) {
1442                 target->type->add_watchpoint = dpmv8_add_watchpoint;
1443                 target->type->remove_watchpoint = dpmv8_remove_watchpoint;
1444         }
1445
1446         /* FIXME add vector catch support */
1447
1448         dpm->nbp = 1 + ((dpm->didr >> 12) & 0xf);
1449         dpm->dbp = calloc(dpm->nbp, sizeof(*dpm->dbp));
1450
1451         dpm->nwp = 1 + ((dpm->didr >> 20) & 0xf);
1452         dpm->dwp = calloc(dpm->nwp, sizeof(*dpm->dwp));
1453
1454         if (!dpm->dbp || !dpm->dwp) {
1455                 free(dpm->dbp);
1456                 free(dpm->dwp);
1457                 return ERROR_FAIL;
1458         }
1459
1460         LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints",
1461                 target_name(target), dpm->nbp, dpm->nwp);
1462
1463         /* REVISIT ... and some of those breakpoints could match
1464          * execution context IDs...
1465          */
1466
1467         return ERROR_OK;
1468 }
1469
1470 /**
1471  * Reinitializes DPM state at the beginning of a new debug session
1472  * or after a reset which may have affected the debug module.
1473  */
1474 int armv8_dpm_initialize(struct arm_dpm *dpm)
1475 {
1476         /* Disable all breakpoints and watchpoints at startup. */
1477         if (dpm->bpwp_disable) {
1478                 unsigned i;
1479
1480                 for (i = 0; i < dpm->nbp; i++) {
1481                         dpm->dbp[i].bpwp.number = i;
1482                         (void) dpm->bpwp_disable(dpm, i);
1483                 }
1484                 for (i = 0; i < dpm->nwp; i++) {
1485                         dpm->dwp[i].bpwp.number = 16 + i;
1486                         (void) dpm->bpwp_disable(dpm, 16 + i);
1487                 }
1488         } else
1489                 LOG_WARNING("%s: can't disable breakpoints and watchpoints",
1490                         target_name(dpm->arm->target));
1491
1492         return ERROR_OK;
1493 }