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

   1 /***************************************************************************
   2  *   Copyright (C) 2015 by David Ung                                       *
   3  *                                                                         *
   4  *   Copyright (C) 2018 by Liviu Ionescu                                   *
   5  *   <ilg@livius.net>                                                      *
   6  *                                                                         *
   7  *   This program is free software; you can redistribute it and/or modify  *
   8  *   it under the terms of the GNU General Public License as published by  *
   9  *   the Free Software Foundation; either version 2 of the License, or     *
  10  *   (at your option) any later version.                                   *
  11  *                                                                         *
  12  *   This program is distributed in the hope that it will be useful,       *
  13  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  14  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  15  *   GNU General Public License for more details.                          *
  16  *                                                                         *
  17  *   You should have received a copy of the GNU General Public License     *
  18  *   along with this program; if not, write to the                         *
  19  *   Free Software Foundation, Inc.,                                       *
  20  ***************************************************************************/
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include "config.h"
  24 #endif
  25
  26 #include <helper/replacements.h>
  27
  28 #include "armv8.h"
  29 #include "arm_disassembler.h"
  30
  31 #include "register.h"
  32 #include <helper/binarybuffer.h>
  33 #include <helper/command.h>
  34
  35 #include <stdlib.h>
  36 #include <string.h>
  37 #include <unistd.h>
  38
  39 #include "armv8_opcodes.h"
  40 #include "target.h"
  41 #include "target_type.h"
  42 #include "semihosting_common.h"
  43
  44 static const char * const armv8_state_strings[] = {
  45         "AArch32", "Thumb", "Jazelle", "ThumbEE", "AArch64",
  46 };
  47
  48 static const struct {
  49         const char *name;
  50         unsigned psr;
  51 } armv8_mode_data[] = {
  52         {
  53                 .name = "USR",
  54                 .psr = ARM_MODE_USR,
  55         },
  56         {
  57                 .name = "FIQ",
  58                 .psr = ARM_MODE_FIQ,
  59         },
  60         {
  61                 .name = "IRQ",
  62                 .psr = ARM_MODE_IRQ,
  63         },
  64         {
  65                 .name = "SVC",
  66                 .psr = ARM_MODE_SVC,
  67         },
  68         {
  69                 .name = "MON",
  70                 .psr = ARM_MODE_MON,
  71         },
  72         {
  73                 .name = "ABT",
  74                 .psr = ARM_MODE_ABT,
  75         },
  76         {
  77                 .name = "HYP",
  78                 .psr = ARM_MODE_HYP,
  79         },
  80         {
  81                 .name = "SYS",
  82                 .psr = ARM_MODE_SYS,
  83         },
  84         {
  85                 .name = "EL0T",
  86                 .psr = ARMV8_64_EL0T,
  87         },
  88         {
  89                 .name = "EL1T",
  90                 .psr = ARMV8_64_EL1T,
  91         },
  92         {
  93                 .name = "EL1H",
  94                 .psr = ARMV8_64_EL1H,
  95         },
  96         {
  97                 .name = "EL2T",
  98                 .psr = ARMV8_64_EL2T,
  99         },
 100         {
 101                 .name = "EL2H",
 102                 .psr = ARMV8_64_EL2H,
 103         },
 104         {
 105                 .name = "EL3T",
 106                 .psr = ARMV8_64_EL3T,
 107         },
 108         {
 109                 .name = "EL3H",
 110                 .psr = ARMV8_64_EL3H,
 111         },
 112 };
 113
 114 /** Map PSR mode bits to the name of an ARM processor operating mode. */
 115 const char *armv8_mode_name(unsigned psr_mode)
 116 {
 117         for (unsigned i = 0; i < ARRAY_SIZE(armv8_mode_data); i++) {
 118                 if (armv8_mode_data[i].psr == psr_mode)
 119                         return armv8_mode_data[i].name;
 120         }
 121         LOG_ERROR("unrecognized psr mode: %#02x", psr_mode);
 122         return "UNRECOGNIZED";
 123 }
 124
 125 static int armv8_read_reg(struct armv8_common *armv8, int regnum, uint64_t *regval)
 126 {
 127         struct arm_dpm *dpm = &armv8->dpm;
 128         int retval;
 129         uint32_t value;
 130         uint64_t value_64;
 131
 132         switch (regnum) {
 133         case 0 ... 30:
 134                 retval = dpm->instr_read_data_dcc_64(dpm,
 135                                 ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum), &value_64);
 136                 break;
 137         case ARMV8_SP:
 138                 retval = dpm->instr_read_data_r0_64(dpm,
 139                                 ARMV8_MOVFSP_64(0), &value_64);
 140                 break;
 141         case ARMV8_PC:
 142                 retval = dpm->instr_read_data_r0_64(dpm,
 143                                 ARMV8_MRS_DLR(0), &value_64);
 144                 break;
 145         case ARMV8_xPSR:
 146                 retval = dpm->instr_read_data_r0(dpm,
 147                                 ARMV8_MRS_DSPSR(0), &value);
 148                 value_64 = value;
 149                 break;
 150         case ARMV8_FPSR:
 151                 retval = dpm->instr_read_data_r0(dpm,
 152                                 ARMV8_MRS_FPSR(0), &value);
 153                 value_64 = value;
 154                 break;
 155         case ARMV8_FPCR:
 156                 retval = dpm->instr_read_data_r0(dpm,
 157                                 ARMV8_MRS_FPCR(0), &value);
 158                 value_64 = value;
 159                 break;
 160         case ARMV8_ELR_EL1:
 161                 retval = dpm->instr_read_data_r0_64(dpm,
 162                                 ARMV8_MRS(SYSTEM_ELR_EL1, 0), &value_64);
 163                 break;
 164         case ARMV8_ELR_EL2:
 165                 retval = dpm->instr_read_data_r0_64(dpm,
 166                                 ARMV8_MRS(SYSTEM_ELR_EL2, 0), &value_64);
 167                 break;
 168         case ARMV8_ELR_EL3:
 169                 retval = dpm->instr_read_data_r0_64(dpm,
 170                                 ARMV8_MRS(SYSTEM_ELR_EL3, 0), &value_64);
 171                 break;
 172         case ARMV8_ESR_EL1:
 173                 retval = dpm->instr_read_data_r0(dpm,
 174                                 ARMV8_MRS(SYSTEM_ESR_EL1, 0), &value);
 175                 value_64 = value;
 176                 break;
 177         case ARMV8_ESR_EL2:
 178                 retval = dpm->instr_read_data_r0(dpm,
 179                                 ARMV8_MRS(SYSTEM_ESR_EL2, 0), &value);
 180                 value_64 = value;
 181                 break;
 182         case ARMV8_ESR_EL3:
 183                 retval = dpm->instr_read_data_r0(dpm,
 184                                 ARMV8_MRS(SYSTEM_ESR_EL3, 0), &value);
 185                 value_64 = value;
 186                 break;
 187         case ARMV8_SPSR_EL1:
 188                 retval = dpm->instr_read_data_r0(dpm,
 189                                 ARMV8_MRS(SYSTEM_SPSR_EL1, 0), &value);
 190                 value_64 = value;
 191                 break;
 192         case ARMV8_SPSR_EL2:
 193                 retval = dpm->instr_read_data_r0(dpm,
 194                                 ARMV8_MRS(SYSTEM_SPSR_EL2, 0), &value);
 195                 value_64 = value;
 196                 break;
 197         case ARMV8_SPSR_EL3:
 198                 retval = dpm->instr_read_data_r0(dpm,
 199                                 ARMV8_MRS(SYSTEM_SPSR_EL3, 0), &value);
 200                 value_64 = value;
 201                 break;
 202         default:
 203                 retval = ERROR_FAIL;
 204                 break;
 205         }
 206
 207         if (retval == ERROR_OK && regval != NULL)
 208                 *regval = value_64;
 209         else
 210                 retval = ERROR_FAIL;
 211
 212         return retval;
 213 }
 214
 215 static int armv8_read_reg_simdfp_aarch64(struct armv8_common *armv8, int regnum, uint64_t *lvalue, uint64_t *hvalue)
 216 {
 217         int retval = ERROR_FAIL;
 218         struct arm_dpm *dpm = &armv8->dpm;
 219
 220         switch (regnum) {
 221         case ARMV8_V0 ... ARMV8_V31:
 222                 retval = dpm->instr_read_data_r0_64(dpm,
 223                                 ARMV8_MOV_GPR_VFP(0, (regnum - ARMV8_V0), 1), hvalue);
 224                 if (retval != ERROR_OK)
 225                         return retval;
 226                 retval = dpm->instr_read_data_r0_64(dpm,
 227                                 ARMV8_MOV_GPR_VFP(0, (regnum - ARMV8_V0), 0), lvalue);
 228                 break;
 229
 230         default:
 231                 retval = ERROR_FAIL;
 232                 break;
 233         }
 234
 235         return retval;
 236 }
 237
 238 static int armv8_write_reg(struct armv8_common *armv8, int regnum, uint64_t value_64)
 239 {
 240         struct arm_dpm *dpm = &armv8->dpm;
 241         int retval;
 242         uint32_t value;
 243
 244         switch (regnum) {
 245         case 0 ... 30:
 246                 retval = dpm->instr_write_data_dcc_64(dpm,
 247                         ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
 248                         value_64);
 249                 break;
 250         case ARMV8_SP:
 251                 retval = dpm->instr_write_data_r0_64(dpm,
 252                         ARMV8_MOVTSP_64(0),
 253                         value_64);
 254                 break;
 255         case ARMV8_PC:
 256                 retval = dpm->instr_write_data_r0_64(dpm,
 257                         ARMV8_MSR_DLR(0),
 258                         value_64);
 259                 break;
 260         case ARMV8_xPSR:
 261                 value = value_64;
 262                 retval = dpm->instr_write_data_r0(dpm,
 263                         ARMV8_MSR_DSPSR(0),
 264                         value);
 265                 break;
 266         case ARMV8_FPSR:
 267                 value = value_64;
 268                 retval = dpm->instr_write_data_r0(dpm,
 269                         ARMV8_MSR_FPSR(0),
 270                         value);
 271                 break;
 272         case ARMV8_FPCR:
 273                 value = value_64;
 274                 retval = dpm->instr_write_data_r0(dpm,
 275                         ARMV8_MSR_FPCR(0),
 276                         value);
 277                 break;
 278         /* registers clobbered by taking exception in debug state */
 279         case ARMV8_ELR_EL1:
 280                 retval = dpm->instr_write_data_r0_64(dpm,
 281                                 ARMV8_MSR_GP(SYSTEM_ELR_EL1, 0), value_64);
 282                 break;
 283         case ARMV8_ELR_EL2:
 284                 retval = dpm->instr_write_data_r0_64(dpm,
 285                                 ARMV8_MSR_GP(SYSTEM_ELR_EL2, 0), value_64);
 286                 break;
 287         case ARMV8_ELR_EL3:
 288                 retval = dpm->instr_write_data_r0_64(dpm,
 289                                 ARMV8_MSR_GP(SYSTEM_ELR_EL3, 0), value_64);
 290                 break;
 291         case ARMV8_ESR_EL1:
 292                 value = value_64;
 293                 retval = dpm->instr_write_data_r0(dpm,
 294                                 ARMV8_MSR_GP(SYSTEM_ESR_EL1, 0), value);
 295                 break;
 296         case ARMV8_ESR_EL2:
 297                 value = value_64;
 298                 retval = dpm->instr_write_data_r0(dpm,
 299                                 ARMV8_MSR_GP(SYSTEM_ESR_EL2, 0), value);
 300                 break;
 301         case ARMV8_ESR_EL3:
 302                 value = value_64;
 303                 retval = dpm->instr_write_data_r0(dpm,
 304                                 ARMV8_MSR_GP(SYSTEM_ESR_EL3, 0), value);
 305                 break;
 306         case ARMV8_SPSR_EL1:
 307                 value = value_64;
 308                 retval = dpm->instr_write_data_r0(dpm,
 309                                 ARMV8_MSR_GP(SYSTEM_SPSR_EL1, 0), value);
 310                 break;
 311         case ARMV8_SPSR_EL2:
 312                 value = value_64;
 313                 retval = dpm->instr_write_data_r0(dpm,
 314                                 ARMV8_MSR_GP(SYSTEM_SPSR_EL2, 0), value);
 315                 break;
 316         case ARMV8_SPSR_EL3:
 317                 value = value_64;
 318                 retval = dpm->instr_write_data_r0(dpm,
 319                                 ARMV8_MSR_GP(SYSTEM_SPSR_EL3, 0), value);
 320                 break;
 321         default:
 322                 retval = ERROR_FAIL;
 323                 break;
 324         }
 325
 326         return retval;
 327 }
 328
 329 static int armv8_write_reg_simdfp_aarch64(struct armv8_common *armv8, int regnum, uint64_t lvalue, uint64_t hvalue)
 330 {
 331         int retval = ERROR_FAIL;
 332         struct arm_dpm *dpm = &armv8->dpm;
 333
 334         switch (regnum) {
 335         case ARMV8_V0 ... ARMV8_V31:
 336                 retval = dpm->instr_write_data_r0_64(dpm,
 337                                 ARMV8_MOV_VFP_GPR((regnum - ARMV8_V0), 0, 1), hvalue);
 338                 if (retval != ERROR_OK)
 339                         return retval;
 340                 retval = dpm->instr_write_data_r0_64(dpm,
 341                                 ARMV8_MOV_VFP_GPR((regnum - ARMV8_V0), 0, 0), lvalue);
 342                 break;
 343
 344         default:
 345                 retval = ERROR_FAIL;
 346                 break;
 347         }
 348
 349         return retval;
 350 }
 351
 352 static int armv8_read_reg32(struct armv8_common *armv8, int regnum, uint64_t *regval)
 353 {
 354         struct arm_dpm *dpm = &armv8->dpm;
 355         uint32_t value = 0;
 356         int retval;
 357
 358         switch (regnum) {
 359         case ARMV8_R0 ... ARMV8_R14:
 360                 /* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
 361                 retval = dpm->instr_read_data_dcc(dpm,
 362                         ARMV4_5_MCR(14, 0, regnum, 0, 5, 0),
 363                         &value);
 364                 break;
 365         case ARMV8_SP:
 366                 retval = dpm->instr_read_data_dcc(dpm,
 367                         ARMV4_5_MCR(14, 0, 13, 0, 5, 0),
 368                         &value);
 369                 break;
 370         case ARMV8_PC:
 371                 retval = dpm->instr_read_data_r0(dpm,
 372                         ARMV8_MRC_DLR(0),
 373                         &value);
 374                 break;
 375         case ARMV8_xPSR:
 376                 retval = dpm->instr_read_data_r0(dpm,
 377                         ARMV8_MRC_DSPSR(0),
 378                         &value);
 379                 break;
 380         case ARMV8_ELR_EL1: /* mapped to LR_svc */
 381                 retval = dpm->instr_read_data_dcc(dpm,
 382                                 ARMV4_5_MCR(14, 0, 14, 0, 5, 0),
 383                                 &value);
 384                 break;
 385         case ARMV8_ELR_EL2: /* mapped to ELR_hyp */
 386                 retval = dpm->instr_read_data_r0(dpm,
 387                                 ARMV8_MRS_T1(0, 14, 0, 1),
 388                                 &value);
 389                 break;
 390         case ARMV8_ELR_EL3: /* mapped to LR_mon */
 391                 retval = dpm->instr_read_data_dcc(dpm,
 392                                 ARMV4_5_MCR(14, 0, 14, 0, 5, 0),
 393                                 &value);
 394                 break;
 395         case ARMV8_ESR_EL1: /* mapped to DFSR */
 396                 retval = dpm->instr_read_data_r0(dpm,
 397                                 ARMV4_5_MRC(15, 0, 0, 5, 0, 0),
 398                                 &value);
 399                 break;
 400         case ARMV8_ESR_EL2: /* mapped to HSR */
 401                 retval = dpm->instr_read_data_r0(dpm,
 402                                 ARMV4_5_MRC(15, 4, 0, 5, 2, 0),
 403                                 &value);
 404                 break;
 405         case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
 406                 retval = ERROR_FAIL;
 407                 break;
 408         case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
 409                 retval = dpm->instr_read_data_r0(dpm,
 410                                 ARMV8_MRS_xPSR_T1(1, 0),
 411                                 &value);
 412                 break;
 413         case ARMV8_SPSR_EL2: /* mapped to SPSR_hyp */
 414                 retval = dpm->instr_read_data_r0(dpm,
 415                                 ARMV8_MRS_xPSR_T1(1, 0),
 416                                 &value);
 417                 break;
 418         case ARMV8_SPSR_EL3: /* mapped to SPSR_mon */
 419                 retval = dpm->instr_read_data_r0(dpm,
 420                                 ARMV8_MRS_xPSR_T1(1, 0),
 421                                 &value);
 422                 break;
 423         case ARMV8_FPSR:
 424                 /* "VMRS r0, FPSCR"; then return via DCC */
 425                 retval = dpm->instr_read_data_r0(dpm,
 426                         ARMV4_5_VMRS(0), &value);
 427                 break;
 428         default:
 429                 retval = ERROR_FAIL;
 430                 break;
 431         }
 432
 433         if (retval == ERROR_OK && regval != NULL)
 434                 *regval = value;
 435
 436         return retval;
 437 }
 438
 439 static int armv8_read_reg_simdfp_aarch32(struct armv8_common *armv8, int regnum, uint64_t *lvalue, uint64_t *hvalue)
 440 {
 441         int retval = ERROR_FAIL;
 442         struct arm_dpm *dpm = &armv8->dpm;
 443         struct reg *reg_r1 = dpm->arm->core_cache->reg_list + ARMV8_R1;
 444         uint32_t value_r0 = 0, value_r1 = 0;
 445         unsigned num = (regnum - ARMV8_V0) << 1;
 446
 447         switch (regnum) {
 448         case ARMV8_V0 ... ARMV8_V15:
 449                 /* we are going to write R1, mark it dirty */
 450                 reg_r1->dirty = true;
 451                 /* move from double word register to r0:r1: "vmov r0, r1, vm"
 452                  * then read r0 via dcc
 453                  */
 454                 retval = dpm->instr_read_data_r0(dpm,
 455                                 ARMV4_5_VMOV(1, 1, 0, (num >> 4), (num & 0xf)),
 456                                 &value_r0);
 457                 /* read r1 via dcc */
 458                 retval = dpm->instr_read_data_dcc(dpm,
 459                                 ARMV4_5_MCR(14, 0, 1, 0, 5, 0),
 460                                 &value_r1);
 461                 if (retval == ERROR_OK) {
 462                         *lvalue = value_r1;
 463                         *lvalue = ((*lvalue) << 32) | value_r0;
 464                 } else
 465                         return retval;
 466
 467                 num++;
 468                 /* repeat above steps for high 64 bits of V register */
 469                 retval = dpm->instr_read_data_r0(dpm,
 470                                 ARMV4_5_VMOV(1, 1, 0, (num >> 4), (num & 0xf)),
 471                                 &value_r0);
 472                 retval = dpm->instr_read_data_dcc(dpm,
 473                                 ARMV4_5_MCR(14, 0, 1, 0, 5, 0),
 474                                 &value_r1);
 475                 if (retval == ERROR_OK) {
 476                         *hvalue = value_r1;
 477                         *hvalue = ((*hvalue) << 32) | value_r0;
 478                 } else
 479                         return retval;
 480                 break;
 481         default:
 482                 retval = ERROR_FAIL;
 483                 break;
 484         }
 485
 486         return retval;
 487 }
 488
 489 static int armv8_write_reg32(struct armv8_common *armv8, int regnum, uint64_t value)
 490 {
 491         struct arm_dpm *dpm = &armv8->dpm;
 492         int retval;
 493
 494         switch (regnum) {
 495         case ARMV8_R0 ... ARMV8_R14:
 496                 /* load register from DCC:  "MRC p14, 0, Rnum, c0, c5, 0" */
 497                 retval = dpm->instr_write_data_dcc(dpm,
 498                                 ARMV4_5_MRC(14, 0, regnum, 0, 5, 0), value);
 499                 break;
 500         case ARMV8_SP:
 501                 retval = dpm->instr_write_data_dcc(dpm,
 502                                 ARMV4_5_MRC(14, 0, 13, 0, 5, 0), value);
 503                         break;
 504         case ARMV8_PC:/* PC
 505                  * read r0 from DCC; then "MOV pc, r0" */
 506                 retval = dpm->instr_write_data_r0(dpm,
 507                                 ARMV8_MCR_DLR(0), value);
 508                 break;
 509         case ARMV8_xPSR: /* CPSR */
 510                 /* read r0 from DCC, then "MCR r0, DSPSR" */
 511                 retval = dpm->instr_write_data_r0(dpm,
 512                                 ARMV8_MCR_DSPSR(0), value);
 513                 break;
 514         case ARMV8_ELR_EL1: /* mapped to LR_svc */
 515                 retval = dpm->instr_write_data_dcc(dpm,
 516                                 ARMV4_5_MRC(14, 0, 14, 0, 5, 0),
 517                                 value);
 518                 break;
 519         case ARMV8_ELR_EL2: /* mapped to ELR_hyp */
 520                 retval = dpm->instr_write_data_r0(dpm,
 521                                 ARMV8_MSR_GP_T1(0, 14, 0, 1),
 522                                 value);
 523                 break;
 524         case ARMV8_ELR_EL3: /* mapped to LR_mon */
 525                 retval = dpm->instr_write_data_dcc(dpm,
 526                                 ARMV4_5_MRC(14, 0, 14, 0, 5, 0),
 527                                 value);
 528                 break;
 529         case ARMV8_ESR_EL1: /* mapped to DFSR */
 530                 retval = dpm->instr_write_data_r0(dpm,
 531                                 ARMV4_5_MCR(15, 0, 0, 5, 0, 0),
 532                                 value);
 533                 break;
 534         case ARMV8_ESR_EL2: /* mapped to HSR */
 535                 retval = dpm->instr_write_data_r0(dpm,
 536                                 ARMV4_5_MCR(15, 4, 0, 5, 2, 0),
 537                                 value);
 538                 break;
 539         case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
 540                 retval = ERROR_FAIL;
 541                 break;
 542         case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
 543                 retval = dpm->instr_write_data_r0(dpm,
 544                                 ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
 545                                 value);
 546                 break;
 547         case ARMV8_SPSR_EL2: /* mapped to SPSR_hyp */
 548                 retval = dpm->instr_write_data_r0(dpm,
 549                                 ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
 550                                 value);
 551                 break;
 552         case ARMV8_SPSR_EL3: /* mapped to SPSR_mon */
 553                 retval = dpm->instr_write_data_r0(dpm,
 554                                 ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
 555                                 value);
 556                 break;
 557         case ARMV8_FPSR:
 558                 /* move to r0 from DCC, then "VMSR FPSCR, r0" */
 559                 retval = dpm->instr_write_data_r0(dpm,
 560                         ARMV4_5_VMSR(0), value);
 561                 break;
 562         default:
 563                 retval = ERROR_FAIL;
 564                 break;
 565         }
 566
 567         return retval;
 568
 569 }
 570
 571 static int armv8_write_reg_simdfp_aarch32(struct armv8_common *armv8, int regnum, uint64_t lvalue, uint64_t hvalue)
 572 {
 573         int retval = ERROR_FAIL;
 574         struct arm_dpm *dpm = &armv8->dpm;
 575         struct reg *reg_r1 = dpm->arm->core_cache->reg_list + ARMV8_R1;
 576         uint32_t value_r0 = 0, value_r1 = 0;
 577         unsigned num = (regnum - ARMV8_V0) << 1;
 578
 579         switch (regnum) {
 580         case ARMV8_V0 ... ARMV8_V15:
 581                 /* we are going to write R1, mark it dirty */
 582                 reg_r1->dirty = true;
 583                 value_r1 = lvalue >> 32;
 584                 value_r0 = lvalue & 0xFFFFFFFF;
 585                 /* write value_r1 to r1 via dcc */
 586                 retval = dpm->instr_write_data_dcc(dpm,
 587                         ARMV4_5_MRC(14, 0, 1, 0, 5, 0),
 588                         value_r1);
 589                 /* write value_r0 to r0 via dcc then,
 590                  * move to double word register from r0:r1: "vmov vm, r0, r1"
 591                  */
 592                 retval = dpm->instr_write_data_r0(dpm,
 593                         ARMV4_5_VMOV(0, 1, 0, (num >> 4), (num & 0xf)),
 594                         value_r0);
 595
 596                 num++;
 597                 /* repeat above steps for high 64 bits of V register */
 598                 value_r1 = hvalue >> 32;
 599                 value_r0 = hvalue & 0xFFFFFFFF;
 600                 retval = dpm->instr_write_data_dcc(dpm,
 601                         ARMV4_5_MRC(14, 0, 1, 0, 5, 0),
 602                         value_r1);
 603                 retval = dpm->instr_write_data_r0(dpm,
 604                         ARMV4_5_VMOV(0, 1, 0, (num >> 4), (num & 0xf)),
 605                         value_r0);
 606                 break;
 607         default:
 608                 retval = ERROR_FAIL;
 609                 break;
 610         }
 611
 612         return retval;
 613 }
 614
 615 void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64)
 616 {
 617         if (is_aarch64) {
 618                 armv8->read_reg_u64 = armv8_read_reg;
 619                 armv8->write_reg_u64 = armv8_write_reg;
 620                 armv8->read_reg_u128 = armv8_read_reg_simdfp_aarch64;
 621                 armv8->write_reg_u128 = armv8_write_reg_simdfp_aarch64;
 622
 623         } else {
 624                 armv8->read_reg_u64 = armv8_read_reg32;
 625                 armv8->write_reg_u64 = armv8_write_reg32;
 626                 armv8->read_reg_u128 = armv8_read_reg_simdfp_aarch32;
 627                 armv8->write_reg_u128 = armv8_write_reg_simdfp_aarch32;
 628         }
 629 }
 630
 631 /*  retrieve core id cluster id  */
 632 int armv8_read_mpidr(struct armv8_common *armv8)
 633 {
 634         int retval = ERROR_FAIL;
 635         struct arm *arm = &armv8->arm;
 636         struct arm_dpm *dpm = armv8->arm.dpm;
 637         uint32_t mpidr;
 638
 639         retval = dpm->prepare(dpm);
 640         if (retval != ERROR_OK)
 641                 goto done;
 642
 643         /* check if we're in an unprivileged mode */
 644         if (armv8_curel_from_core_mode(arm->core_mode) < SYSTEM_CUREL_EL1) {
 645                 retval = armv8_dpm_modeswitch(dpm, ARMV8_64_EL1H);
 646                 if (retval != ERROR_OK)
 647                         return retval;
 648         }
 649
 650         retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_MPIDR), &mpidr);
 651         if (retval != ERROR_OK)
 652                 goto done;
 653         if (mpidr & 1U<<31) {
 654                 armv8->multi_processor_system = (mpidr >> 30) & 1;
 655                 armv8->cluster_id = (mpidr >> 8) & 0xf;
 656                 armv8->cpu_id = mpidr & 0x3;
 657                 LOG_INFO("%s cluster %x core %x %s", target_name(armv8->arm.target),
 658                         armv8->cluster_id,
 659                         armv8->cpu_id,
 660                         armv8->multi_processor_system == 0 ? "multi core" : "single core");
 661         } else
 662                 LOG_ERROR("mpidr not in multiprocessor format");
 663
 664 done:
 665         armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 666         dpm->finish(dpm);
 667         return retval;
 668 }
 669
 670 /**
 671  * Configures host-side ARM records to reflect the specified CPSR.
 672  * Later, code can use arm_reg_current() to map register numbers
 673  * according to how they are exposed by this mode.
 674  */
 675 void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
 676 {
 677         uint32_t mode = cpsr & 0x1F;
 678
 679         /* NOTE:  this may be called very early, before the register
 680          * cache is set up.  We can't defend against many errors, in
 681          * particular against CPSRs that aren't valid *here* ...
 682          */
 683         if (arm->cpsr) {
 684                 buf_set_u32(arm->cpsr->value, 0, 32, cpsr);
 685                 arm->cpsr->valid = true;
 686                 arm->cpsr->dirty = false;
 687         }
 688
 689         /* Older ARMs won't have the J bit */
 690         enum arm_state state = 0xFF;
 691
 692         if ((cpsr & 0x10) != 0) {
 693                 /* Aarch32 state */
 694                 if (cpsr & (1 << 5)) {  /* T */
 695                         if (cpsr & (1 << 24)) { /* J */
 696                                 LOG_WARNING("ThumbEE -- incomplete support");
 697                                 state = ARM_STATE_THUMB_EE;
 698                         } else
 699                                 state = ARM_STATE_THUMB;
 700                 } else {
 701                         if (cpsr & (1 << 24)) { /* J */
 702                                 LOG_ERROR("Jazelle state handling is BROKEN!");
 703                                 state = ARM_STATE_JAZELLE;
 704                         } else
 705                                 state = ARM_STATE_ARM;
 706                 }
 707         } else {
 708                 /* Aarch64 state */
 709                 state = ARM_STATE_AARCH64;
 710         }
 711
 712         arm->core_state = state;
 713         arm->core_mode = mode;
 714
 715         LOG_DEBUG("set CPSR %#8.8x: %s mode, %s state", (unsigned) cpsr,
 716                 armv8_mode_name(arm->core_mode),
 717                 armv8_state_strings[arm->core_state]);
 718 }
 719
 720 static void armv8_show_fault_registers32(struct armv8_common *armv8)
 721 {
 722         uint32_t dfsr, ifsr, dfar, ifar;
 723         struct arm_dpm *dpm = armv8->arm.dpm;
 724         int retval;
 725
 726         retval = dpm->prepare(dpm);
 727         if (retval != ERROR_OK)
 728                 return;
 729
 730         /* ARMV4_5_MRC(cpnum, op1, r0, CRn, CRm, op2) */
 731
 732         /* c5/c0 - {data, instruction} fault status registers */
 733         retval = dpm->instr_read_data_r0(dpm,
 734                         ARMV4_5_MRC(15, 0, 0, 5, 0, 0),
 735                         &dfsr);
 736         if (retval != ERROR_OK)
 737                 goto done;
 738
 739         retval = dpm->instr_read_data_r0(dpm,
 740                         ARMV4_5_MRC(15, 0, 0, 5, 0, 1),
 741                         &ifsr);
 742         if (retval != ERROR_OK)
 743                 goto done;
 744
 745         /* c6/c0 - {data, instruction} fault address registers */
 746         retval = dpm->instr_read_data_r0(dpm,
 747                         ARMV4_5_MRC(15, 0, 0, 6, 0, 0),
 748                         &dfar);
 749         if (retval != ERROR_OK)
 750                 goto done;
 751
 752         retval = dpm->instr_read_data_r0(dpm,
 753                         ARMV4_5_MRC(15, 0, 0, 6, 0, 2),
 754                         &ifar);
 755         if (retval != ERROR_OK)
 756                 goto done;
 757
 758         LOG_USER("Data fault registers        DFSR: %8.8" PRIx32
 759                 ", DFAR: %8.8" PRIx32, dfsr, dfar);
 760         LOG_USER("Instruction fault registers IFSR: %8.8" PRIx32
 761                 ", IFAR: %8.8" PRIx32, ifsr, ifar);
 762
 763 done:
 764         /* (void) */ dpm->finish(dpm);
 765 }
 766
 767 static __attribute__((unused)) void armv8_show_fault_registers(struct target *target)
 768 {
 769         struct armv8_common *armv8 = target_to_armv8(target);
 770
 771         if (armv8->arm.core_state != ARM_STATE_AARCH64)
 772                 armv8_show_fault_registers32(armv8);
 773 }
 774
 775 static uint8_t armv8_pa_size(uint32_t ps)
 776 {
 777         uint8_t ret = 0;
 778         switch (ps) {
 779                 case 0:
 780                         ret = 32;
 781                         break;
 782                 case 1:
 783                         ret = 36;
 784                         break;
 785                 case 2:
 786                         ret = 40;
 787                         break;
 788                 case 3:
 789                         ret = 42;
 790                         break;
 791                 case 4:
 792                         ret = 44;
 793                         break;
 794                 case 5:
 795                         ret = 48;
 796                         break;
 797                 default:
 798                         LOG_INFO("Unknown physical address size");
 799                         break;
 800         }
 801         return ret;
 802 }
 803
 804 static __attribute__((unused)) int armv8_read_ttbcr32(struct target *target)
 805 {
 806         struct armv8_common *armv8 = target_to_armv8(target);
 807         struct arm_dpm *dpm = armv8->arm.dpm;
 808         uint32_t ttbcr, ttbcr_n;
 809         int retval = dpm->prepare(dpm);
 810         if (retval != ERROR_OK)
 811                 goto done;
 812         /*  MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/
 813         retval = dpm->instr_read_data_r0(dpm,
 814                         ARMV4_5_MRC(15, 0, 0, 2, 0, 2),
 815                         &ttbcr);
 816         if (retval != ERROR_OK)
 817                 goto done;
 818
 819         LOG_DEBUG("ttbcr %" PRIx32, ttbcr);
 820
 821         ttbcr_n = ttbcr & 0x7;
 822         armv8->armv8_mmu.ttbcr = ttbcr;
 823
 824         /*
 825          * ARM Architecture Reference Manual (ARMv7-A and ARMv7-R edition),
 826          * document # ARM DDI 0406C
 827          */
 828         armv8->armv8_mmu.ttbr_range[0]  = 0xffffffff >> ttbcr_n;
 829         armv8->armv8_mmu.ttbr_range[1] = 0xffffffff;
 830         armv8->armv8_mmu.ttbr_mask[0] = 0xffffffff << (14 - ttbcr_n);
 831         armv8->armv8_mmu.ttbr_mask[1] = 0xffffffff << 14;
 832
 833         LOG_DEBUG("ttbr1 %s, ttbr0_mask %" PRIx32 " ttbr1_mask %" PRIx32,
 834                   (ttbcr_n != 0) ? "used" : "not used",
 835                   armv8->armv8_mmu.ttbr_mask[0],
 836                   armv8->armv8_mmu.ttbr_mask[1]);
 837
 838 done:
 839         dpm->finish(dpm);
 840         return retval;
 841 }
 842
 843 static __attribute__((unused)) int armv8_read_ttbcr(struct target *target)
 844 {
 845         struct armv8_common *armv8 = target_to_armv8(target);
 846         struct arm_dpm *dpm = armv8->arm.dpm;
 847         struct arm *arm = &armv8->arm;
 848         uint32_t ttbcr;
 849         uint64_t ttbcr_64;
 850
 851         int retval = dpm->prepare(dpm);
 852         if (retval != ERROR_OK)
 853                 goto done;
 854
 855         /* clear ttrr1_used and ttbr0_mask */
 856         memset(&armv8->armv8_mmu.ttbr1_used, 0, sizeof(armv8->armv8_mmu.ttbr1_used));
 857         memset(&armv8->armv8_mmu.ttbr0_mask, 0, sizeof(armv8->armv8_mmu.ttbr0_mask));
 858
 859         switch (armv8_curel_from_core_mode(arm->core_mode)) {
 860         case SYSTEM_CUREL_EL3:
 861                 retval = dpm->instr_read_data_r0(dpm,
 862                                 ARMV8_MRS(SYSTEM_TCR_EL3, 0),
 863                                 &ttbcr);
 864                 retval += dpm->instr_read_data_r0_64(dpm,
 865                                 ARMV8_MRS(SYSTEM_TTBR0_EL3, 0),
 866                                 &armv8->ttbr_base);
 867                 if (retval != ERROR_OK)
 868                         goto done;
 869                 armv8->va_size = 64 - (ttbcr & 0x3F);
 870                 armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
 871                 armv8->page_size = (ttbcr >> 14) & 3;
 872                 break;
 873         case SYSTEM_CUREL_EL2:
 874                 retval = dpm->instr_read_data_r0(dpm,
 875                                 ARMV8_MRS(SYSTEM_TCR_EL2, 0),
 876                                 &ttbcr);
 877                 retval += dpm->instr_read_data_r0_64(dpm,
 878                                 ARMV8_MRS(SYSTEM_TTBR0_EL2, 0),
 879                                 &armv8->ttbr_base);
 880                 if (retval != ERROR_OK)
 881                         goto done;
 882                 armv8->va_size = 64 - (ttbcr & 0x3F);
 883                 armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
 884                 armv8->page_size = (ttbcr >> 14) & 3;
 885                 break;
 886         case SYSTEM_CUREL_EL0:
 887                 armv8_dpm_modeswitch(dpm, ARMV8_64_EL1H);
 888                 /* fall through */
 889         case SYSTEM_CUREL_EL1:
 890                 retval = dpm->instr_read_data_r0_64(dpm,
 891                                 ARMV8_MRS(SYSTEM_TCR_EL1, 0),
 892                                 &ttbcr_64);
 893                 armv8->va_size = 64 - (ttbcr_64 & 0x3F);
 894                 armv8->pa_size = armv8_pa_size((ttbcr_64 >> 32) & 7);
 895                 armv8->page_size = (ttbcr_64 >> 14) & 3;
 896                 armv8->armv8_mmu.ttbr1_used = (((ttbcr_64 >> 16) & 0x3F) != 0) ? 1 : 0;
 897                 armv8->armv8_mmu.ttbr0_mask  = 0x0000FFFFFFFFFFFF;
 898                 retval += dpm->instr_read_data_r0_64(dpm,
 899                                 ARMV8_MRS(SYSTEM_TTBR0_EL1 | (armv8->armv8_mmu.ttbr1_used), 0),
 900                                 &armv8->ttbr_base);
 901                 if (retval != ERROR_OK)
 902                         goto done;
 903                 break;
 904         default:
 905                 LOG_ERROR("unknown core state");
 906                 retval = ERROR_FAIL;
 907                 break;
 908         }
 909         if (retval != ERROR_OK)
 910                 goto done;
 911
 912         if (armv8->armv8_mmu.ttbr1_used == 1)
 913                 LOG_INFO("TTBR0 access above %" PRIx64, (uint64_t)(armv8->armv8_mmu.ttbr0_mask));
 914
 915 done:
 916         armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 917         dpm->finish(dpm);
 918         return retval;
 919 }
 920
 921 /*  method adapted to cortex A : reused arm v4 v5 method*/
 922 int armv8_mmu_translate_va(struct target *target,  target_addr_t va, target_addr_t *val)
 923 {
 924         return ERROR_OK;
 925 }
 926
 927 /*  V8 method VA TO PA  */
 928 int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 929         target_addr_t *val, int meminfo)
 930 {
 931         struct armv8_common *armv8 = target_to_armv8(target);
 932         struct arm *arm = target_to_arm(target);
 933         struct arm_dpm *dpm = &armv8->dpm;
 934         enum arm_mode target_mode = ARM_MODE_ANY;
 935         uint32_t retval;
 936         uint32_t instr = 0;
 937         uint64_t par;
 938
 939         static const char * const shared_name[] = {
 940                         "Non-", "UNDEFINED ", "Outer ", "Inner "
 941         };
 942
 943         static const char * const secure_name[] = {
 944                         "Secure", "Not Secure"
 945         };
 946
 947         if (target->state != TARGET_HALTED) {
 948                 LOG_WARNING("target %s not halted", target_name(target));
 949                 return ERROR_TARGET_NOT_HALTED;
 950         }
 951
 952         retval = dpm->prepare(dpm);
 953         if (retval != ERROR_OK)
 954                 return retval;
 955
 956         switch (armv8_curel_from_core_mode(arm->core_mode)) {
 957         case SYSTEM_CUREL_EL0:
 958                 instr = ARMV8_SYS(SYSTEM_ATS12E0R, 0);
 959                 /* can only execute instruction at EL2 */
 960                 target_mode = ARMV8_64_EL2H;
 961                 break;
 962         case SYSTEM_CUREL_EL1:
 963                 instr = ARMV8_SYS(SYSTEM_ATS12E1R, 0);
 964                 /* can only execute instruction at EL2 */
 965                 target_mode = ARMV8_64_EL2H;
 966                 break;
 967         case SYSTEM_CUREL_EL2:
 968                 instr = ARMV8_SYS(SYSTEM_ATS1E2R, 0);
 969                 break;
 970         case SYSTEM_CUREL_EL3:
 971                 instr = ARMV8_SYS(SYSTEM_ATS1E3R, 0);
 972                 break;
 973
 974         default:
 975                 break;
 976         };
 977
 978         if (target_mode != ARM_MODE_ANY)
 979                 armv8_dpm_modeswitch(dpm, target_mode);
 980
 981         /* write VA to R0 and execute translation instruction */
 982         retval = dpm->instr_write_data_r0_64(dpm, instr, (uint64_t)va);
 983         /* read result from PAR_EL1 */
 984         if (retval == ERROR_OK)
 985                 retval = dpm->instr_read_data_r0_64(dpm, ARMV8_MRS(SYSTEM_PAR_EL1, 0), &par);
 986
 987         /* switch back to saved PE mode */
 988         if (target_mode != ARM_MODE_ANY)
 989                 armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 990
 991         dpm->finish(dpm);
 992
 993         if (retval != ERROR_OK)
 994                 return retval;
 995
 996         if (par & 1) {
 997                 LOG_ERROR("Address translation failed at stage %i, FST=%x, PTW=%i",
 998                                 ((int)(par >> 9) & 1)+1, (int)(par >> 1) & 0x3f, (int)(par >> 8) & 1);
 999
1000                 *val = 0;
1001                 retval = ERROR_FAIL;
1002         } else {
1003                 *val = (par & 0xFFFFFFFFF000UL) | (va & 0xFFF);
1004                 if (meminfo) {
1005                         int SH = (par >> 7) & 3;
1006                         int NS = (par >> 9) & 1;
1007                         int ATTR = (par >> 56) & 0xFF;
1008
1009                         char *memtype = (ATTR & 0xF0) == 0 ? "Device Memory" : "Normal Memory";
1010
1011                         LOG_USER("%sshareable, %s",
1012                                         shared_name[SH], secure_name[NS]);
1013                         LOG_USER("%s", memtype);
1014                 }
1015         }
1016
1017         return retval;
1018 }
1019
1020 COMMAND_HANDLER(armv8_handle_exception_catch_command)
1021 {
1022         struct target *target = get_current_target(CMD_CTX);
1023         struct armv8_common *armv8 = target_to_armv8(target);
1024         uint32_t edeccr = 0;
1025         unsigned int argp = 0;
1026         int retval;
1027
1028         static const Jim_Nvp nvp_ecatch_modes[] = {
1029                 { .name = "off",       .value = 0 },
1030                 { .name = "nsec_el1",  .value = (1 << 5) },
1031                 { .name = "nsec_el2",  .value = (2 << 5) },
1032                 { .name = "nsec_el12", .value = (3 << 5) },
1033                 { .name = "sec_el1",   .value = (1 << 1) },
1034                 { .name = "sec_el3",   .value = (4 << 1) },
1035                 { .name = "sec_el13",  .value = (5 << 1) },
1036                 { .name = NULL, .value = -1 },
1037         };
1038         const Jim_Nvp *n;
1039
1040         if (CMD_ARGC == 0) {
1041                 const char *sec = NULL, *nsec = NULL;
1042
1043                 retval = mem_ap_read_atomic_u32(armv8->debug_ap,
1044                                         armv8->debug_base + CPUV8_DBG_ECCR, &edeccr);
1045                 if (retval != ERROR_OK)
1046                         return retval;
1047
1048                 n = Jim_Nvp_value2name_simple(nvp_ecatch_modes, edeccr & 0x0f);
1049                 if (n->name != NULL)
1050                         sec = n->name;
1051
1052                 n = Jim_Nvp_value2name_simple(nvp_ecatch_modes, edeccr & 0xf0);
1053                 if (n->name != NULL)
1054                         nsec = n->name;
1055
1056                 if (sec == NULL || nsec == NULL) {
1057                         LOG_WARNING("Exception Catch: unknown exception catch configuration: EDECCR = %02" PRIx32, edeccr & 0xff);
1058                         return ERROR_FAIL;
1059                 }
1060
1061                 command_print(CMD, "Exception Catch: Secure: %s, Non-Secure: %s", sec, nsec);
1062                 return ERROR_OK;
1063         }
1064
1065         while (CMD_ARGC > argp) {
1066                 n = Jim_Nvp_name2value_simple(nvp_ecatch_modes, CMD_ARGV[argp]);
1067                 if (n->name == NULL) {
1068                         LOG_ERROR("Unknown option: %s", CMD_ARGV[argp]);
1069                         return ERROR_FAIL;
1070                 }
1071
1072                 LOG_DEBUG("found: %s", n->name);
1073
1074                 edeccr |= n->value;
1075                 argp++;
1076         }
1077
1078         retval = mem_ap_write_atomic_u32(armv8->debug_ap,
1079                                 armv8->debug_base + CPUV8_DBG_ECCR, edeccr);
1080         if (retval != ERROR_OK)
1081                 return retval;
1082
1083         return ERROR_OK;
1084 }
1085
1086 int armv8_handle_cache_info_command(struct command_invocation *cmd,
1087         struct armv8_cache_common *armv8_cache)
1088 {
1089         if (armv8_cache->info == -1) {
1090                 command_print(cmd, "cache not yet identified");
1091                 return ERROR_OK;
1092         }
1093
1094         if (armv8_cache->display_cache_info)
1095                 armv8_cache->display_cache_info(cmd, armv8_cache);
1096         return ERROR_OK;
1097 }
1098
1099 static int armv8_setup_semihosting(struct target *target, int enable)
1100 {
1101         struct arm *arm = target_to_arm(target);
1102
1103         if (arm->core_state != ARM_STATE_AARCH64) {
1104                 LOG_ERROR("semihosting only supported in AArch64 state\n");
1105                 return ERROR_FAIL;
1106         }
1107
1108         return ERROR_OK;
1109 }
1110
1111 int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
1112 {
1113         struct arm *arm = &armv8->arm;
1114         arm->arch_info = armv8;
1115         target->arch_info = &armv8->arm;
1116         arm->setup_semihosting = armv8_setup_semihosting;
1117         /*  target is useful in all function arm v4 5 compatible */
1118         armv8->arm.target = target;
1119         armv8->arm.common_magic = ARM_COMMON_MAGIC;
1120         armv8->common_magic = ARMV8_COMMON_MAGIC;
1121
1122         armv8->armv8_mmu.armv8_cache.l2_cache = NULL;
1123         armv8->armv8_mmu.armv8_cache.info = -1;
1124         armv8->armv8_mmu.armv8_cache.flush_all_data_cache = NULL;
1125         armv8->armv8_mmu.armv8_cache.display_cache_info = NULL;
1126         return ERROR_OK;
1127 }
1128
1129 static int armv8_aarch64_state(struct target *target)
1130 {
1131         struct arm *arm = target_to_arm(target);
1132
1133         if (arm->common_magic != ARM_COMMON_MAGIC) {
1134                 LOG_ERROR("BUG: called for a non-ARM target");
1135                 return ERROR_FAIL;
1136         }
1137
1138         LOG_USER("%s halted in %s state due to %s, current mode: %s\n"
1139                 "cpsr: 0x%8.8" PRIx32 " pc: 0x%" PRIx64 "%s",
1140                 target_name(target),
1141                 armv8_state_strings[arm->core_state],
1142                 debug_reason_name(target),
1143                 armv8_mode_name(arm->core_mode),
1144                 buf_get_u32(arm->cpsr->value, 0, 32),
1145                 buf_get_u64(arm->pc->value, 0, 64),
1146                 (target->semihosting && target->semihosting->is_active) ? ", semihosting" : "");
1147
1148         return ERROR_OK;
1149 }
1150
1151 int armv8_arch_state(struct target *target)
1152 {
1153         static const char * const state[] = {
1154                 "disabled", "enabled"
1155         };
1156
1157         struct armv8_common *armv8 = target_to_armv8(target);
1158         struct arm *arm = &armv8->arm;
1159
1160         if (armv8->common_magic != ARMV8_COMMON_MAGIC) {
1161                 LOG_ERROR("BUG: called for a non-Armv8 target");
1162                 return ERROR_COMMAND_SYNTAX_ERROR;
1163         }
1164
1165         if (arm->core_state == ARM_STATE_AARCH64)
1166                 armv8_aarch64_state(target);
1167         else
1168                 arm_arch_state(target);
1169
1170         LOG_USER("MMU: %s, D-Cache: %s, I-Cache: %s",
1171                 state[armv8->armv8_mmu.mmu_enabled],
1172                 state[armv8->armv8_mmu.armv8_cache.d_u_cache_enabled],
1173                 state[armv8->armv8_mmu.armv8_cache.i_cache_enabled]);
1174
1175         if (arm->core_mode == ARM_MODE_ABT)
1176                 armv8_show_fault_registers(target);
1177
1178         if (target->debug_reason == DBG_REASON_WATCHPOINT)
1179                 LOG_USER("Watchpoint triggered at PC %#08x",
1180                         (unsigned) armv8->dpm.wp_pc);
1181
1182         return ERROR_OK;
1183 }
1184
1185 static struct reg_data_type aarch64_vector_base_types[] = {
1186         {REG_TYPE_IEEE_DOUBLE, "ieee_double", 0, {NULL} },
1187         {REG_TYPE_UINT64, "uint64", 0, {NULL} },
1188         {REG_TYPE_INT64, "int64", 0, {NULL} },
1189         {REG_TYPE_IEEE_SINGLE, "ieee_single", 0, {NULL} },
1190         {REG_TYPE_UINT32, "uint32", 0, {NULL} },
1191         {REG_TYPE_INT32, "int32", 0, {NULL} },
1192         {REG_TYPE_UINT16, "uint16", 0, {NULL} },
1193         {REG_TYPE_INT16, "int16", 0, {NULL} },
1194         {REG_TYPE_UINT8, "uint8", 0, {NULL} },
1195         {REG_TYPE_INT8, "int8", 0, {NULL} },
1196         {REG_TYPE_UINT128, "uint128", 0, {NULL} },
1197         {REG_TYPE_INT128, "int128", 0, {NULL} }
1198 };
1199
1200 static struct reg_data_type_vector aarch64_vector_types[] = {
1201         {aarch64_vector_base_types + 0, 2},
1202         {aarch64_vector_base_types + 1, 2},
1203         {aarch64_vector_base_types + 2, 2},
1204         {aarch64_vector_base_types + 3, 4},
1205         {aarch64_vector_base_types + 4, 4},
1206         {aarch64_vector_base_types + 5, 4},
1207         {aarch64_vector_base_types + 6, 8},
1208         {aarch64_vector_base_types + 7, 8},
1209         {aarch64_vector_base_types + 8, 16},
1210         {aarch64_vector_base_types + 9, 16},
1211         {aarch64_vector_base_types + 10, 01},
1212         {aarch64_vector_base_types + 11, 01},
1213 };
1214
1215 static struct reg_data_type aarch64_fpu_vector[] = {
1216         {REG_TYPE_ARCH_DEFINED, "v2d",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 0} },
1217         {REG_TYPE_ARCH_DEFINED, "v2u",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 1} },
1218         {REG_TYPE_ARCH_DEFINED, "v2i",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 2} },
1219         {REG_TYPE_ARCH_DEFINED, "v4f",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 3} },
1220         {REG_TYPE_ARCH_DEFINED, "v4u",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 4} },
1221         {REG_TYPE_ARCH_DEFINED, "v4i",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 5} },
1222         {REG_TYPE_ARCH_DEFINED, "v8u",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 6} },
1223         {REG_TYPE_ARCH_DEFINED, "v8i",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 7} },
1224         {REG_TYPE_ARCH_DEFINED, "v16u", REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 8} },
1225         {REG_TYPE_ARCH_DEFINED, "v16i", REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 9} },
1226         {REG_TYPE_ARCH_DEFINED, "v1u",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 10} },
1227         {REG_TYPE_ARCH_DEFINED, "v1i",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 11} },
1228 };
1229
1230 static struct reg_data_type_union_field aarch64_union_fields_vnd[] = {
1231         {"f", aarch64_fpu_vector + 0, aarch64_union_fields_vnd + 1},
1232         {"u", aarch64_fpu_vector + 1, aarch64_union_fields_vnd + 2},
1233         {"s", aarch64_fpu_vector + 2, NULL},
1234 };
1235
1236 static struct reg_data_type_union_field aarch64_union_fields_vns[] = {
1237         {"f", aarch64_fpu_vector + 3, aarch64_union_fields_vns + 1},
1238         {"u", aarch64_fpu_vector + 4, aarch64_union_fields_vns + 2},
1239         {"s", aarch64_fpu_vector + 5, NULL},
1240 };
1241
1242 static struct reg_data_type_union_field aarch64_union_fields_vnh[] = {
1243         {"u", aarch64_fpu_vector + 6, aarch64_union_fields_vnh + 1},
1244         {"s", aarch64_fpu_vector + 7, NULL},
1245 };
1246
1247 static struct reg_data_type_union_field aarch64_union_fields_vnb[] = {
1248         {"u", aarch64_fpu_vector + 8, aarch64_union_fields_vnb + 1},
1249         {"s", aarch64_fpu_vector + 9, NULL},
1250 };
1251
1252 static struct reg_data_type_union_field aarch64_union_fields_vnq[] = {
1253         {"u", aarch64_fpu_vector + 10, aarch64_union_fields_vnq + 1},
1254         {"s", aarch64_fpu_vector + 11, NULL},
1255 };
1256
1257 static struct reg_data_type_union aarch64_union_types[] = {
1258         {aarch64_union_fields_vnd},
1259         {aarch64_union_fields_vns},
1260         {aarch64_union_fields_vnh},
1261         {aarch64_union_fields_vnb},
1262         {aarch64_union_fields_vnq},
1263 };
1264
1265 static struct reg_data_type aarch64_fpu_union[] = {
1266         {REG_TYPE_ARCH_DEFINED, "vnd", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 0} },
1267         {REG_TYPE_ARCH_DEFINED, "vns", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 1} },
1268         {REG_TYPE_ARCH_DEFINED, "vnh", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 2} },
1269         {REG_TYPE_ARCH_DEFINED, "vnb", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 3} },
1270         {REG_TYPE_ARCH_DEFINED, "vnq", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 4} },
1271 };
1272
1273 static struct reg_data_type_union_field aarch64v_union_fields[] = {
1274         {"d", aarch64_fpu_union + 0, aarch64v_union_fields + 1},
1275         {"s", aarch64_fpu_union + 1, aarch64v_union_fields + 2},
1276         {"h", aarch64_fpu_union + 2, aarch64v_union_fields + 3},
1277         {"b", aarch64_fpu_union + 3, aarch64v_union_fields + 4},
1278         {"q", aarch64_fpu_union + 4, NULL},
1279 };
1280
1281 static struct reg_data_type_union aarch64v_union[] = {
1282         {aarch64v_union_fields}
1283 };
1284
1285 static struct reg_data_type aarch64v[] = {
1286         {REG_TYPE_ARCH_DEFINED, "aarch64v", REG_TYPE_CLASS_UNION,
1287                 {.reg_type_union = aarch64v_union} },
1288 };
1289
1290 static struct reg_data_type_bitfield aarch64_cpsr_bits[] = {
1291         {  0,  0, REG_TYPE_UINT8 },
1292         {  2,  3, REG_TYPE_UINT8 },
1293         {  4,  4, REG_TYPE_UINT8 },
1294         {  6,  6, REG_TYPE_BOOL },
1295         {  7,  7, REG_TYPE_BOOL },
1296         {  8,  8, REG_TYPE_BOOL },
1297         {  9,  9, REG_TYPE_BOOL },
1298         { 20, 20, REG_TYPE_BOOL },
1299         { 21, 21, REG_TYPE_BOOL },
1300         { 28, 28, REG_TYPE_BOOL },
1301         { 29, 29, REG_TYPE_BOOL },
1302         { 30, 30, REG_TYPE_BOOL },
1303         { 31, 31, REG_TYPE_BOOL },
1304 };
1305
1306 static struct reg_data_type_flags_field aarch64_cpsr_fields[] = {
1307         { "SP",  aarch64_cpsr_bits + 0,  aarch64_cpsr_fields + 1 },
1308         { "EL",  aarch64_cpsr_bits + 1,  aarch64_cpsr_fields + 2 },
1309         { "nRW", aarch64_cpsr_bits + 2,  aarch64_cpsr_fields + 3 },
1310         { "F",   aarch64_cpsr_bits + 3,  aarch64_cpsr_fields + 4 },
1311         { "I",   aarch64_cpsr_bits + 4,  aarch64_cpsr_fields + 5 },
1312         { "A",   aarch64_cpsr_bits + 5,  aarch64_cpsr_fields + 6 },
1313         { "D",   aarch64_cpsr_bits + 6,  aarch64_cpsr_fields + 7 },
1314         { "IL",  aarch64_cpsr_bits + 7,  aarch64_cpsr_fields + 8 },
1315         { "SS",  aarch64_cpsr_bits + 8,  aarch64_cpsr_fields + 9 },
1316         { "V",   aarch64_cpsr_bits + 9,  aarch64_cpsr_fields + 10 },
1317         { "C",   aarch64_cpsr_bits + 10, aarch64_cpsr_fields + 11 },
1318         { "Z",   aarch64_cpsr_bits + 11, aarch64_cpsr_fields + 12 },
1319         { "N",   aarch64_cpsr_bits + 12, NULL }
1320 };
1321
1322 static struct reg_data_type_flags aarch64_cpsr_flags[] = {
1323         { 4, aarch64_cpsr_fields }
1324 };
1325
1326 static struct reg_data_type aarch64_flags_cpsr[] = {
1327         {REG_TYPE_ARCH_DEFINED, "cpsr_flags", REG_TYPE_CLASS_FLAGS,
1328                 {.reg_type_flags = aarch64_cpsr_flags} },
1329 };
1330
1331 static const struct {
1332         unsigned id;
1333         const char *name;
1334         unsigned bits;
1335         enum arm_mode mode;
1336         enum reg_type type;
1337         const char *group;
1338         const char *feature;
1339         struct reg_data_type *data_type;
1340 } armv8_regs[] = {
1341         { ARMV8_R0,  "x0",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1342         { ARMV8_R1,  "x1",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1343         { ARMV8_R2,  "x2",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1344         { ARMV8_R3,  "x3",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1345         { ARMV8_R4,  "x4",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1346         { ARMV8_R5,  "x5",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1347         { ARMV8_R6,  "x6",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1348         { ARMV8_R7,  "x7",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1349         { ARMV8_R8,  "x8",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1350         { ARMV8_R9,  "x9",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1351         { ARMV8_R10, "x10", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1352         { ARMV8_R11, "x11", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1353         { ARMV8_R12, "x12", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1354         { ARMV8_R13, "x13", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1355         { ARMV8_R14, "x14", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1356         { ARMV8_R15, "x15", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1357         { ARMV8_R16, "x16", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1358         { ARMV8_R17, "x17", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1359         { ARMV8_R18, "x18", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1360         { ARMV8_R19, "x19", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1361         { ARMV8_R20, "x20", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1362         { ARMV8_R21, "x21", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1363         { ARMV8_R22, "x22", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1364         { ARMV8_R23, "x23", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1365         { ARMV8_R24, "x24", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1366         { ARMV8_R25, "x25", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1367         { ARMV8_R26, "x26", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1368         { ARMV8_R27, "x27", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1369         { ARMV8_R28, "x28", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1370         { ARMV8_R29, "x29", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1371         { ARMV8_R30, "x30", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
1372
1373         { ARMV8_SP, "sp", 64, ARM_MODE_ANY, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core", NULL},
1374         { ARMV8_PC, "pc", 64, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core", NULL},
1375         { ARMV8_xPSR, "cpsr", 32, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED,
1376                 "general", "org.gnu.gdb.aarch64.core", aarch64_flags_cpsr},
1377         { ARMV8_V0,  "v0",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1378         { ARMV8_V1,  "v1",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1379         { ARMV8_V2,  "v2",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1380         { ARMV8_V3,  "v3",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1381         { ARMV8_V4,  "v4",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1382         { ARMV8_V5,  "v5",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1383         { ARMV8_V6,  "v6",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1384         { ARMV8_V7,  "v7",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1385         { ARMV8_V8,  "v8",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1386         { ARMV8_V9,  "v9",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1387         { ARMV8_V10, "v10", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1388         { ARMV8_V11, "v11", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1389         { ARMV8_V12, "v12", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1390         { ARMV8_V13, "v13", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1391         { ARMV8_V14, "v14", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1392         { ARMV8_V15, "v15", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1393         { ARMV8_V16, "v16", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1394         { ARMV8_V17, "v17", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1395         { ARMV8_V18, "v18", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1396         { ARMV8_V19, "v19", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1397         { ARMV8_V20, "v20", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1398         { ARMV8_V21, "v21", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1399         { ARMV8_V22, "v22", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1400         { ARMV8_V23, "v23", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1401         { ARMV8_V24, "v24", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1402         { ARMV8_V25, "v25", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1403         { ARMV8_V26, "v26", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1404         { ARMV8_V27, "v27", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1405         { ARMV8_V28, "v28", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1406         { ARMV8_V29, "v29", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1407         { ARMV8_V30, "v30", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1408         { ARMV8_V31, "v31", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
1409         { ARMV8_FPSR, "fpsr", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "simdfp", "org.gnu.gdb.aarch64.fpu", NULL},
1410         { ARMV8_FPCR, "fpcr", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "simdfp", "org.gnu.gdb.aarch64.fpu", NULL},
1411
1412         { ARMV8_ELR_EL1, "ELR_EL1", 64, ARMV8_64_EL1H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked",
1413                                                                                                                 NULL},
1414         { ARMV8_ESR_EL1, "ESR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
1415                                                                                                                 NULL},
1416         { ARMV8_SPSR_EL1, "SPSR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
1417                                                                                                                 NULL},
1418
1419         { ARMV8_ELR_EL2, "ELR_EL2", 64, ARMV8_64_EL2H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked",
1420                                                                                                                 NULL},
1421         { ARMV8_ESR_EL2, "ESR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
1422                                                                                                                 NULL},
1423         { ARMV8_SPSR_EL2, "SPSR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
1424                                                                                                                 NULL},
1425
1426         { ARMV8_ELR_EL3, "ELR_EL3", 64, ARMV8_64_EL3H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked",
1427                                                                                                                 NULL},
1428         { ARMV8_ESR_EL3, "ESR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
1429                                                                                                                 NULL},
1430         { ARMV8_SPSR_EL3, "SPSR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
1431                                                                                                                 NULL},
1432 };
1433
1434 static const struct {
1435         unsigned id;
1436         unsigned mapping;
1437         const char *name;
1438         unsigned bits;
1439         enum arm_mode mode;
1440         enum reg_type type;
1441         const char *group;
1442         const char *feature;
1443 } armv8_regs32[] = {
1444         { ARMV8_R0, 0,  "r0",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1445         { ARMV8_R1, 0,  "r1",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1446         { ARMV8_R2, 0,  "r2",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1447         { ARMV8_R3, 0,  "r3",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1448         { ARMV8_R4, 0,  "r4",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1449         { ARMV8_R5, 0,  "r5",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1450         { ARMV8_R6, 0,  "r6",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1451         { ARMV8_R7, 0,  "r7",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1452         { ARMV8_R8, 0,  "r8",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1453         { ARMV8_R9, 0,  "r9",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1454         { ARMV8_R10, 0, "r10", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1455         { ARMV8_R11, 0, "r11", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1456         { ARMV8_R12, 0, "r12", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1457         { ARMV8_R13, 0, "sp", 32, ARM_MODE_ANY, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.arm.core" },
1458         { ARMV8_R14, 0, "lr",  32, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.arm.core" },
1459         { ARMV8_PC, 0, "pc",   32, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.arm.core" },
1460         { ARMV8_xPSR, 0, "cpsr", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
1461         { ARMV8_V0, 0, "d0",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1462         { ARMV8_V0, 8, "d1",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1463         { ARMV8_V1, 0, "d2",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1464         { ARMV8_V1, 8, "d3",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1465         { ARMV8_V2, 0, "d4",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1466         { ARMV8_V2, 8, "d5",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1467         { ARMV8_V3, 0, "d6",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1468         { ARMV8_V3, 8, "d7",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1469         { ARMV8_V4, 0, "d8",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1470         { ARMV8_V4, 8, "d9",  64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1471         { ARMV8_V5, 0, "d10", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1472         { ARMV8_V5, 8, "d11", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1473         { ARMV8_V6, 0, "d12", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1474         { ARMV8_V6, 8, "d13", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1475         { ARMV8_V7, 0, "d14", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1476         { ARMV8_V7, 8, "d15", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1477         { ARMV8_V8, 0, "d16", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1478         { ARMV8_V8, 8, "d17", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1479         { ARMV8_V9, 0, "d18", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1480         { ARMV8_V9, 8, "d19", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1481         { ARMV8_V10, 0, "d20", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1482         { ARMV8_V10, 8, "d21", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1483         { ARMV8_V11, 0, "d22", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1484         { ARMV8_V11, 8, "d23", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1485         { ARMV8_V12, 0, "d24", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1486         { ARMV8_V12, 8, "d25", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1487         { ARMV8_V13, 0, "d26", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1488         { ARMV8_V13, 8, "d27", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1489         { ARMV8_V14, 0, "d28", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1490         { ARMV8_V14, 8, "d29", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1491         { ARMV8_V15, 0, "d30", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1492         { ARMV8_V15, 8, "d31", 64, ARM_MODE_ANY, REG_TYPE_IEEE_DOUBLE, NULL, "org.gnu.gdb.arm.vfp"},
1493         { ARMV8_FPSR, 0, "fpscr", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "float", "org.gnu.gdb.arm.vfp"},
1494 };
1495
1496 #define ARMV8_NUM_REGS ARRAY_SIZE(armv8_regs)
1497 #define ARMV8_NUM_REGS32 ARRAY_SIZE(armv8_regs32)
1498
1499 static int armv8_get_core_reg(struct reg *reg)
1500 {
1501         struct arm_reg *armv8_reg = reg->arch_info;
1502         struct target *target = armv8_reg->target;
1503         struct arm *arm = target_to_arm(target);
1504
1505         if (target->state != TARGET_HALTED)
1506                 return ERROR_TARGET_NOT_HALTED;
1507
1508         return arm->read_core_reg(target, reg, armv8_reg->num, arm->core_mode);
1509 }
1510
1511 static int armv8_set_core_reg(struct reg *reg, uint8_t *buf)
1512 {
1513         struct arm_reg *armv8_reg = reg->arch_info;
1514         struct target *target = armv8_reg->target;
1515         struct arm *arm = target_to_arm(target);
1516         uint64_t value = buf_get_u64(buf, 0, reg->size);
1517
1518         if (target->state != TARGET_HALTED)
1519                 return ERROR_TARGET_NOT_HALTED;
1520
1521         if (reg->size <= 64) {
1522                 if (reg == arm->cpsr)
1523                         armv8_set_cpsr(arm, (uint32_t)value);
1524                 else {
1525                         buf_set_u64(reg->value, 0, reg->size, value);
1526                         reg->valid = true;
1527                 }
1528         } else if (reg->size <= 128) {
1529                 uint64_t hvalue = buf_get_u64(buf + 8, 0, reg->size - 64);
1530
1531                 buf_set_u64(reg->value, 0, 64, value);
1532                 buf_set_u64(reg->value + 8, 0, reg->size - 64, hvalue);
1533                 reg->valid = true;
1534         }
1535
1536         reg->dirty = true;
1537
1538         return ERROR_OK;
1539 }
1540
1541 static const struct reg_arch_type armv8_reg_type = {
1542         .get = armv8_get_core_reg,
1543         .set = armv8_set_core_reg,
1544 };
1545
1546 static int armv8_get_core_reg32(struct reg *reg)
1547 {
1548         struct arm_reg *armv8_reg = reg->arch_info;
1549         struct target *target = armv8_reg->target;
1550         struct arm *arm = target_to_arm(target);
1551         struct reg_cache *cache = arm->core_cache;
1552         struct reg *reg64;
1553         int retval;
1554
1555         if (target->state != TARGET_HALTED)
1556                 return ERROR_TARGET_NOT_HALTED;
1557
1558         /* get the corresponding Aarch64 register */
1559         reg64 = cache->reg_list + armv8_reg->num;
1560         if (reg64->valid) {
1561                 reg->valid = true;
1562                 return ERROR_OK;
1563         }
1564
1565         retval = arm->read_core_reg(target, reg64, armv8_reg->num, arm->core_mode);
1566         if (retval == ERROR_OK)
1567                 reg->valid = reg64->valid;
1568
1569         return retval;
1570 }
1571
1572 static int armv8_set_core_reg32(struct reg *reg, uint8_t *buf)
1573 {
1574         struct arm_reg *armv8_reg = reg->arch_info;
1575         struct target *target = armv8_reg->target;
1576         struct arm *arm = target_to_arm(target);
1577         struct reg_cache *cache = arm->core_cache;
1578         struct reg *reg64 = cache->reg_list + armv8_reg->num;
1579         uint32_t value = buf_get_u32(buf, 0, 32);
1580
1581         if (target->state != TARGET_HALTED)
1582                 return ERROR_TARGET_NOT_HALTED;
1583
1584         if (reg64 == arm->cpsr) {
1585                 armv8_set_cpsr(arm, value);
1586         } else {
1587                 if (reg->size <= 32)
1588                         buf_set_u32(reg->value, 0, 32, value);
1589                 else if (reg->size <= 64) {
1590                         uint64_t value64 = buf_get_u64(buf, 0, 64);
1591                         buf_set_u64(reg->value, 0, 64, value64);
1592                 }
1593                 reg->valid = true;
1594                 reg64->valid = true;
1595         }
1596
1597         reg64->dirty = true;
1598
1599         return ERROR_OK;
1600 }
1601
1602 static const struct reg_arch_type armv8_reg32_type = {
1603         .get = armv8_get_core_reg32,
1604         .set = armv8_set_core_reg32,
1605 };
1606
1607 /** Builds cache of architecturally defined registers.  */
1608 struct reg_cache *armv8_build_reg_cache(struct target *target)
1609 {
1610         struct armv8_common *armv8 = target_to_armv8(target);
1611         struct arm *arm = &armv8->arm;
1612         int num_regs = ARMV8_NUM_REGS;
1613         int num_regs32 = ARMV8_NUM_REGS32;
1614         struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
1615         struct reg_cache *cache = malloc(sizeof(struct reg_cache));
1616         struct reg_cache *cache32 = malloc(sizeof(struct reg_cache));
1617         struct reg *reg_list = calloc(num_regs, sizeof(struct reg));
1618         struct reg *reg_list32 = calloc(num_regs32, sizeof(struct reg));
1619         struct arm_reg *arch_info = calloc(num_regs, sizeof(struct arm_reg));
1620         struct reg_feature *feature;
1621         int i;
1622
1623         /* Build the process context cache */
1624         cache->name = "Aarch64 registers";
1625         cache->next = cache32;
1626         cache->reg_list = reg_list;
1627         cache->num_regs = num_regs;
1628
1629         for (i = 0; i < num_regs; i++) {
1630                 arch_info[i].num = armv8_regs[i].id;
1631                 arch_info[i].mode = armv8_regs[i].mode;
1632                 arch_info[i].target = target;
1633                 arch_info[i].arm = arm;
1634
1635                 reg_list[i].name = armv8_regs[i].name;
1636                 reg_list[i].size = armv8_regs[i].bits;
1637                 reg_list[i].value = &arch_info[i].value[0];
1638                 reg_list[i].type = &armv8_reg_type;
1639                 reg_list[i].arch_info = &arch_info[i];
1640
1641                 reg_list[i].group = armv8_regs[i].group;
1642                 reg_list[i].number = i;
1643                 reg_list[i].exist = true;
1644                 reg_list[i].caller_save = true; /* gdb defaults to true */
1645
1646                 feature = calloc(1, sizeof(struct reg_feature));
1647                 if (feature) {
1648                         feature->name = armv8_regs[i].feature;
1649                         reg_list[i].feature = feature;
1650                 } else
1651                         LOG_ERROR("unable to allocate feature list");
1652
1653                 reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
1654                 if (reg_list[i].reg_data_type) {
1655                         if (armv8_regs[i].data_type == NULL)
1656                                 reg_list[i].reg_data_type->type = armv8_regs[i].type;
1657                         else
1658                                 *reg_list[i].reg_data_type = *armv8_regs[i].data_type;
1659                 } else
1660                         LOG_ERROR("unable to allocate reg type list");
1661         }
1662
1663         arm->cpsr = reg_list + ARMV8_xPSR;
1664         arm->pc = reg_list + ARMV8_PC;
1665         arm->core_cache = cache;
1666
1667         /* shadow cache for ARM mode registers */
1668         cache32->name = "Aarch32 registers";
1669         cache32->next = NULL;
1670         cache32->reg_list = reg_list32;
1671         cache32->num_regs = num_regs32;
1672
1673         for (i = 0; i < num_regs32; i++) {
1674                 reg_list32[i].name = armv8_regs32[i].name;
1675                 reg_list32[i].size = armv8_regs32[i].bits;
1676                 reg_list32[i].value = &arch_info[armv8_regs32[i].id].value[armv8_regs32[i].mapping];
1677                 reg_list32[i].type = &armv8_reg32_type;
1678                 reg_list32[i].arch_info = &arch_info[armv8_regs32[i].id];
1679                 reg_list32[i].group = armv8_regs32[i].group;
1680                 reg_list32[i].number = i;
1681                 reg_list32[i].exist = true;
1682                 reg_list32[i].caller_save = true;
1683
1684                 feature = calloc(1, sizeof(struct reg_feature));
1685                 if (feature) {
1686                         feature->name = armv8_regs32[i].feature;
1687                         reg_list32[i].feature = feature;
1688                 } else
1689                         LOG_ERROR("unable to allocate feature list");
1690
1691                 reg_list32[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
1692                 if (reg_list32[i].reg_data_type)
1693                         reg_list32[i].reg_data_type->type = armv8_regs32[i].type;
1694                 else
1695                         LOG_ERROR("unable to allocate reg type list");
1696         }
1697
1698         (*cache_p) = cache;
1699         return cache;
1700 }
1701
1702 struct reg *armv8_reg_current(struct arm *arm, unsigned regnum)
1703 {
1704         struct reg *r;
1705
1706         if (regnum > (ARMV8_LAST_REG - 1))
1707                 return NULL;
1708
1709         r = arm->core_cache->reg_list + regnum;
1710         return r;
1711 }
1712
1713 static void armv8_free_cache(struct reg_cache *cache, bool regs32)
1714 {
1715         struct reg *reg;
1716         unsigned int i;
1717
1718         if (!cache)
1719                 return;
1720
1721         for (i = 0; i < cache->num_regs; i++) {
1722                 reg = &cache->reg_list[i];
1723
1724                 free(reg->feature);
1725                 free(reg->reg_data_type);
1726         }
1727
1728         if (!regs32)
1729                 free(cache->reg_list[0].arch_info);
1730         free(cache->reg_list);
1731         free(cache);
1732 }
1733
1734 void armv8_free_reg_cache(struct target *target)
1735 {
1736         struct armv8_common *armv8 = target_to_armv8(target);
1737         struct arm *arm = &armv8->arm;
1738         struct reg_cache *cache = NULL, *cache32 = NULL;
1739
1740         cache = arm->core_cache;
1741         if (cache != NULL)
1742                 cache32 = cache->next;
1743         armv8_free_cache(cache32, true);
1744         armv8_free_cache(cache, false);
1745         arm->core_cache = NULL;
1746 }
1747
1748 const struct command_registration armv8_command_handlers[] = {
1749         {
1750                 .name = "catch_exc",
1751                 .handler = armv8_handle_exception_catch_command,
1752                 .mode = COMMAND_EXEC,
1753                 .help = "configure exception catch",
1754                 .usage = "[(nsec_el1,nsec_el2,sec_el1,sec_el3)+,off]",
1755         },
1756         COMMAND_REGISTRATION_DONE
1757 };
1758
1759 const char *armv8_get_gdb_arch(struct target *target)
1760 {
1761         struct arm *arm = target_to_arm(target);
1762         return arm->core_state == ARM_STATE_AARCH64 ? "aarch64" : "arm";
1763 }
1764
1765 int armv8_get_gdb_reg_list(struct target *target,
1766         struct reg **reg_list[], int *reg_list_size,
1767         enum target_register_class reg_class)
1768 {
1769         struct arm *arm = target_to_arm(target);
1770         int i;
1771
1772         if (arm->core_state == ARM_STATE_AARCH64) {
1773
1774                 LOG_DEBUG("Creating Aarch64 register list for target %s", target_name(target));
1775
1776                 switch (reg_class) {
1777                 case REG_CLASS_GENERAL:
1778                         *reg_list_size = ARMV8_V0;
1779                         *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
1780
1781                         for (i = 0; i < *reg_list_size; i++)
1782                                         (*reg_list)[i] = armv8_reg_current(arm, i);
1783                         return ERROR_OK;
1784
1785                 case REG_CLASS_ALL:
1786                         *reg_list_size = ARMV8_LAST_REG;
1787                         *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
1788
1789                         for (i = 0; i < *reg_list_size; i++)
1790                                         (*reg_list)[i] = armv8_reg_current(arm, i);
1791
1792                         return ERROR_OK;
1793
1794                 default:
1795                         LOG_ERROR("not a valid register class type in query.");
1796                         return ERROR_FAIL;
1797                 }
1798         } else {
1799                 struct reg_cache *cache32 = arm->core_cache->next;
1800
1801                 LOG_DEBUG("Creating Aarch32 register list for target %s", target_name(target));
1802
1803                 switch (reg_class) {
1804                 case REG_CLASS_GENERAL:
1805                         *reg_list_size = ARMV8_R14 + 3;
1806                         *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
1807
1808                         for (i = 0; i < *reg_list_size; i++)
1809                                 (*reg_list)[i] = cache32->reg_list + i;
1810
1811                         return ERROR_OK;
1812                 case REG_CLASS_ALL:
1813                         *reg_list_size = cache32->num_regs;
1814                         *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
1815
1816                         for (i = 0; i < *reg_list_size; i++)
1817                                 (*reg_list)[i] = cache32->reg_list + i;
1818
1819                         return ERROR_OK;
1820                 default:
1821                         LOG_ERROR("not a valid register class type in query.");
1822                         return ERROR_FAIL;
1823                 }
1824         }
1825 }
1826
1827 int armv8_set_dbgreg_bits(struct armv8_common *armv8, unsigned int reg, unsigned long mask, unsigned long value)
1828 {
1829         uint32_t tmp;
1830
1831         /* Read register */
1832         int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
1833                         armv8->debug_base + reg, &tmp);
1834         if (ERROR_OK != retval)
1835                 return retval;
1836
1837         /* clear bitfield */
1838         tmp &= ~mask;
1839         /* put new value */
1840         tmp |= value & mask;
1841
1842         /* write new value */
1843         retval = mem_ap_write_atomic_u32(armv8->debug_ap,
1844                         armv8->debug_base + reg, tmp);
1845         return retval;
1846 }