qemu/target-arm/op_helper.c

   1 /*
   2  *  ARM helper routines
   3  *
   4  *  Copyright (c) 2005-2007 CodeSourcery, LLC
   5  *
   6  * This library is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU Lesser General Public
   8  * License as published by the Free Software Foundation; either
   9  * version 2 of the License, or (at your option) any later version.
  10  *
  11  * This library is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14  * Lesser General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU Lesser General Public
  17  * License along with this library; if not, see <http://www.gnu.org/licenses/>.
  18  */
  19 #include "cpu.h"
  20 #include "exec/helper-proto.h"
  21 #include "internals.h"
  22 #include "exec/cpu_ldst.h"
  23
  24 #define SIGNBIT (uint32_t)0x80000000
  25 #define SIGNBIT64 ((uint64_t)1 << 63)
  26
  27 static void raise_exception(CPUARMState *env, uint32_t excp,
  28                             uint32_t syndrome, uint32_t target_el)
  29 {
  30     CPUState *cs = CPU(arm_env_get_cpu(env));
  31
  32     assert(!excp_is_internal(excp));
  33     cs->exception_index = excp;
  34     env->exception.syndrome = syndrome;
  35     env->exception.target_el = target_el;
  36     cpu_loop_exit(cs);
  37 }
  38
  39 static int exception_target_el(CPUARMState *env)
  40 {
  41     int target_el = MAX(1, arm_current_el(env));
  42
  43     /* No such thing as secure EL1 if EL3 is aarch32, so update the target EL
  44      * to EL3 in this case.
  45      */
  46     if (arm_is_secure(env) && !arm_el_is_aa64(env, 3) && target_el == 1) {
  47         target_el = 3;
  48     }
  49
  50     return target_el;
  51 }
  52
  53 uint32_t HELPER(neon_tbl)(CPUARMState *env, uint32_t ireg, uint32_t def,
  54                           uint32_t rn, uint32_t maxindex)
  55 {
  56     uint32_t val;
  57     uint32_t tmp;
  58     int index;
  59     int shift;
  60     uint64_t *table;
  61     table = (uint64_t *)&env->vfp.regs[rn];
  62     val = 0;
  63     for (shift = 0; shift < 32; shift += 8) {
  64         index = (ireg >> shift) & 0xff;
  65         if (index < maxindex) {
  66             tmp = (table[index >> 3] >> ((index & 7) << 3)) & 0xff;
  67             val |= tmp << shift;
  68         } else {
  69             val |= def & (0xff << shift);
  70         }
  71     }
  72     return val;
  73 }
  74
  75 #if !defined(CONFIG_USER_ONLY)
  76
  77 /* try to fill the TLB and return an exception if error. If retaddr is
  78  * NULL, it means that the function was called in C code (i.e. not
  79  * from generated code or from helper.c)
  80  */
  81 void tlb_fill(CPUState *cs, target_ulong addr, int is_write, int mmu_idx,
  82               uintptr_t retaddr)
  83 {
  84     bool ret;
  85     uint32_t fsr = 0;
  86
  87     ret = arm_tlb_fill(cs, addr, is_write, mmu_idx, &fsr);
  88     if (unlikely(ret)) {
  89         ARMCPU *cpu = ARM_CPU(cs);
  90         CPUARMState *env = &cpu->env;
  91         uint32_t syn, exc;
  92         bool same_el = (arm_current_el(env) != 0);
  93
  94         if (retaddr) {
  95             /* now we have a real cpu fault */
  96             cpu_restore_state(cs, retaddr);
  97         }
  98
  99         /* AArch64 syndrome does not have an LPAE bit */
 100         syn = fsr & ~(1 << 9);
 101
 102         /* For insn and data aborts we assume there is no instruction syndrome
 103          * information; this is always true for exceptions reported to EL1.
 104          */
 105         if (is_write == 2) {
 106             syn = syn_insn_abort(same_el, 0, 0, syn);
 107             exc = EXCP_PREFETCH_ABORT;
 108         } else {
 109             syn = syn_data_abort(same_el, 0, 0, 0, is_write == 1, syn);
 110             if (is_write == 1 && arm_feature(env, ARM_FEATURE_V6)) {
 111                 fsr |= (1 << 11);
 112             }
 113             exc = EXCP_DATA_ABORT;
 114         }
 115
 116         env->exception.vaddress = addr;
 117         env->exception.fsr = fsr;
 118         raise_exception(env, exc, syn, exception_target_el(env));
 119     }
 120 }
 121 #endif
 122
 123 uint32_t HELPER(add_setq)(CPUARMState *env, uint32_t a, uint32_t b)
 124 {
 125     uint32_t res = a + b;
 126     if (((res ^ a) & SIGNBIT) && !((a ^ b) & SIGNBIT))
 127         env->QF = 1;
 128     return res;
 129 }
 130
 131 uint32_t HELPER(add_saturate)(CPUARMState *env, uint32_t a, uint32_t b)
 132 {
 133     uint32_t res = a + b;
 134     if (((res ^ a) & SIGNBIT) && !((a ^ b) & SIGNBIT)) {
 135         env->QF = 1;
 136         res = ~(((int32_t)a >> 31) ^ SIGNBIT);
 137     }
 138     return res;
 139 }
 140
 141 uint32_t HELPER(sub_saturate)(CPUARMState *env, uint32_t a, uint32_t b)
 142 {
 143     uint32_t res = a - b;
 144     if (((res ^ a) & SIGNBIT) && ((a ^ b) & SIGNBIT)) {
 145         env->QF = 1;
 146         res = ~(((int32_t)a >> 31) ^ SIGNBIT);
 147     }
 148     return res;
 149 }
 150
 151 uint32_t HELPER(double_saturate)(CPUARMState *env, int32_t val)
 152 {
 153     uint32_t res;
 154     if (val >= 0x40000000) {
 155         res = ~SIGNBIT;
 156         env->QF = 1;
 157     } else if (val <= (int32_t)0xc0000000) {
 158         res = SIGNBIT;
 159         env->QF = 1;
 160     } else {
 161         res = val << 1;
 162     }
 163     return res;
 164 }
 165
 166 uint32_t HELPER(add_usaturate)(CPUARMState *env, uint32_t a, uint32_t b)
 167 {
 168     uint32_t res = a + b;
 169     if (res < a) {
 170         env->QF = 1;
 171         res = ~0;
 172     }
 173     return res;
 174 }
 175
 176 uint32_t HELPER(sub_usaturate)(CPUARMState *env, uint32_t a, uint32_t b)
 177 {
 178     uint32_t res = a - b;
 179     if (res > a) {
 180         env->QF = 1;
 181         res = 0;
 182     }
 183     return res;
 184 }
 185
 186 /* Signed saturation.  */
 187 static inline uint32_t do_ssat(CPUARMState *env, int32_t val, int shift)
 188 {
 189     int32_t top;
 190     uint32_t mask;
 191
 192     top = val >> shift;
 193     mask = (1u << shift) - 1;
 194     if (top > 0) {
 195         env->QF = 1;
 196         return mask;
 197     } else if (top < -1) {
 198         env->QF = 1;
 199         return ~mask;
 200     }
 201     return val;
 202 }
 203
 204 /* Unsigned saturation.  */
 205 static inline uint32_t do_usat(CPUARMState *env, int32_t val, int shift)
 206 {
 207     uint32_t max;
 208
 209     max = (1u << shift) - 1;
 210     if (val < 0) {
 211         env->QF = 1;
 212         return 0;
 213     } else if (val > max) {
 214         env->QF = 1;
 215         return max;
 216     }
 217     return val;
 218 }
 219
 220 /* Signed saturate.  */
 221 uint32_t HELPER(ssat)(CPUARMState *env, uint32_t x, uint32_t shift)
 222 {
 223     return do_ssat(env, x, shift);
 224 }
 225
 226 /* Dual halfword signed saturate.  */
 227 uint32_t HELPER(ssat16)(CPUARMState *env, uint32_t x, uint32_t shift)
 228 {
 229     uint32_t res;
 230
 231     res = (uint16_t)do_ssat(env, (int16_t)x, shift);
 232     res |= do_ssat(env, ((int32_t)x) >> 16, shift) << 16;
 233     return res;
 234 }
 235
 236 /* Unsigned saturate.  */
 237 uint32_t HELPER(usat)(CPUARMState *env, uint32_t x, uint32_t shift)
 238 {
 239     return do_usat(env, x, shift);
 240 }
 241
 242 /* Dual halfword unsigned saturate.  */
 243 uint32_t HELPER(usat16)(CPUARMState *env, uint32_t x, uint32_t shift)
 244 {
 245     uint32_t res;
 246
 247     res = (uint16_t)do_usat(env, (int16_t)x, shift);
 248     res |= do_usat(env, ((int32_t)x) >> 16, shift) << 16;
 249     return res;
 250 }
 251
 252 /* Function checks whether WFx (WFI/WFE) instructions are set up to be trapped.
 253  * The function returns the target EL (1-3) if the instruction is to be trapped;
 254  * otherwise it returns 0 indicating it is not trapped.
 255  */
 256 static inline int check_wfx_trap(CPUARMState *env, bool is_wfe)
 257 {
 258     int cur_el = arm_current_el(env);
 259     uint64_t mask;
 260
 261     /* If we are currently in EL0 then we need to check if SCTLR is set up for
 262      * WFx instructions being trapped to EL1. These trap bits don't exist in v7.
 263      */
 264     if (cur_el < 1 && arm_feature(env, ARM_FEATURE_V8)) {
 265         int target_el;
 266
 267         mask = is_wfe ? SCTLR_nTWE : SCTLR_nTWI;
 268         if (arm_is_secure_below_el3(env) && !arm_el_is_aa64(env, 3)) {
 269             /* Secure EL0 and Secure PL1 is at EL3 */
 270             target_el = 3;
 271         } else {
 272             target_el = 1;
 273         }
 274
 275         if (!(env->cp15.sctlr_el[target_el] & mask)) {
 276             return target_el;
 277         }
 278     }
 279
 280     /* We are not trapping to EL1; trap to EL2 if HCR_EL2 requires it
 281      * No need for ARM_FEATURE check as if HCR_EL2 doesn't exist the
 282      * bits will be zero indicating no trap.
 283      */
 284     if (cur_el < 2 && !arm_is_secure(env)) {
 285         mask = (is_wfe) ? HCR_TWE : HCR_TWI;
 286         if (env->cp15.hcr_el2 & mask) {
 287             return 2;
 288         }
 289     }
 290
 291     /* We are not trapping to EL1 or EL2; trap to EL3 if SCR_EL3 requires it */
 292     if (cur_el < 3) {
 293         mask = (is_wfe) ? SCR_TWE : SCR_TWI;
 294         if (env->cp15.scr_el3 & mask) {
 295             return 3;
 296         }
 297     }
 298
 299     return 0;
 300 }
 301
 302 void HELPER(wfi)(CPUARMState *env)
 303 {
 304     CPUState *cs = CPU(arm_env_get_cpu(env));
 305     int target_el = check_wfx_trap(env, false);
 306
 307     if (cpu_has_work(cs)) {
 308         /* Don't bother to go into our "low power state" if
 309          * we would just wake up immediately.
 310          */
 311         return;
 312     }
 313
 314     if (target_el) {
 315         env->pc -= 4;
 316         raise_exception(env, EXCP_UDEF, syn_wfx(1, 0xe, 0), target_el);
 317     }
 318
 319     cs->exception_index = EXCP_HLT;
 320     cs->halted = 1;
 321     cpu_loop_exit(cs);
 322 }
 323
 324 void HELPER(wfe)(CPUARMState *env)
 325 {
 326     /* This is a hint instruction that is semantically different
 327      * from YIELD even though we currently implement it identically.
 328      * Don't actually halt the CPU, just yield back to top
 329      * level loop. This is not going into a "low power state"
 330      * (ie halting until some event occurs), so we never take
 331      * a configurable trap to a different exception level.
 332      */
 333     HELPER(yield)(env);
 334 }
 335
 336 void HELPER(yield)(CPUARMState *env)
 337 {
 338     ARMCPU *cpu = arm_env_get_cpu(env);
 339     CPUState *cs = CPU(cpu);
 340
 341     /* This is a non-trappable hint instruction that generally indicates
 342      * that the guest is currently busy-looping. Yield control back to the
 343      * top level loop so that a more deserving VCPU has a chance to run.
 344      */
 345     cs->exception_index = EXCP_YIELD;
 346     cpu_loop_exit(cs);
 347 }
 348
 349 /* Raise an internal-to-QEMU exception. This is limited to only
 350  * those EXCP values which are special cases for QEMU to interrupt
 351  * execution and not to be used for exceptions which are passed to
 352  * the guest (those must all have syndrome information and thus should
 353  * use exception_with_syndrome).
 354  */
 355 void HELPER(exception_internal)(CPUARMState *env, uint32_t excp)
 356 {
 357     CPUState *cs = CPU(arm_env_get_cpu(env));
 358
 359     assert(excp_is_internal(excp));
 360     cs->exception_index = excp;
 361     cpu_loop_exit(cs);
 362 }
 363
 364 /* Raise an exception with the specified syndrome register value */
 365 void HELPER(exception_with_syndrome)(CPUARMState *env, uint32_t excp,
 366                                      uint32_t syndrome, uint32_t target_el)
 367 {
 368     raise_exception(env, excp, syndrome, target_el);
 369 }
 370
 371 uint32_t HELPER(cpsr_read)(CPUARMState *env)
 372 {
 373     return cpsr_read(env) & ~(CPSR_EXEC | CPSR_RESERVED);
 374 }
 375
 376 void HELPER(cpsr_write)(CPUARMState *env, uint32_t val, uint32_t mask)
 377 {
 378     cpsr_write(env, val, mask);
 379 }
 380
 381 /* Access to user mode registers from privileged modes.  */
 382 uint32_t HELPER(get_user_reg)(CPUARMState *env, uint32_t regno)
 383 {
 384     uint32_t val;
 385
 386     if (regno == 13) {
 387         val = env->banked_r13[0];
 388     } else if (regno == 14) {
 389         val = env->banked_r14[0];
 390     } else if (regno >= 8
 391                && (env->uncached_cpsr & 0x1f) == ARM_CPU_MODE_FIQ) {
 392         val = env->usr_regs[regno - 8];
 393     } else {
 394         val = env->regs[regno];
 395     }
 396     return val;
 397 }
 398
 399 void HELPER(set_user_reg)(CPUARMState *env, uint32_t regno, uint32_t val)
 400 {
 401     if (regno == 13) {
 402         env->banked_r13[0] = val;
 403     } else if (regno == 14) {
 404         env->banked_r14[0] = val;
 405     } else if (regno >= 8
 406                && (env->uncached_cpsr & 0x1f) == ARM_CPU_MODE_FIQ) {
 407         env->usr_regs[regno - 8] = val;
 408     } else {
 409         env->regs[regno] = val;
 410     }
 411 }
 412
 413 void HELPER(access_check_cp_reg)(CPUARMState *env, void *rip, uint32_t syndrome)
 414 {
 415     const ARMCPRegInfo *ri = rip;
 416     int target_el;
 417
 418     if (arm_feature(env, ARM_FEATURE_XSCALE) && ri->cp < 14
 419         && extract32(env->cp15.c15_cpar, ri->cp, 1) == 0) {
 420         raise_exception(env, EXCP_UDEF, syndrome, exception_target_el(env));
 421     }
 422
 423     if (!ri->accessfn) {
 424         return;
 425     }
 426
 427     switch (ri->accessfn(env, ri)) {
 428     case CP_ACCESS_OK:
 429         return;
 430     case CP_ACCESS_TRAP:
 431         target_el = exception_target_el(env);
 432         break;
 433     case CP_ACCESS_TRAP_EL2:
 434         /* Requesting a trap to EL2 when we're in EL3 or S-EL0/1 is
 435          * a bug in the access function.
 436          */
 437         assert(!arm_is_secure(env) && arm_current_el(env) != 3);
 438         target_el = 2;
 439         break;
 440     case CP_ACCESS_TRAP_EL3:
 441         target_el = 3;
 442         break;
 443     case CP_ACCESS_TRAP_UNCATEGORIZED:
 444         target_el = exception_target_el(env);
 445         syndrome = syn_uncategorized();
 446         break;
 447     default:
 448         g_assert_not_reached();
 449     }
 450
 451     raise_exception(env, EXCP_UDEF, syndrome, target_el);
 452 }
 453
 454 void HELPER(set_cp_reg)(CPUARMState *env, void *rip, uint32_t value)
 455 {
 456     const ARMCPRegInfo *ri = rip;
 457
 458     ri->writefn(env, ri, value);
 459 }
 460
 461 uint32_t HELPER(get_cp_reg)(CPUARMState *env, void *rip)
 462 {
 463     const ARMCPRegInfo *ri = rip;
 464
 465     return ri->readfn(env, ri);
 466 }
 467
 468 void HELPER(set_cp_reg64)(CPUARMState *env, void *rip, uint64_t value)
 469 {
 470     const ARMCPRegInfo *ri = rip;
 471
 472     ri->writefn(env, ri, value);
 473 }
 474
 475 uint64_t HELPER(get_cp_reg64)(CPUARMState *env, void *rip)
 476 {
 477     const ARMCPRegInfo *ri = rip;
 478
 479     return ri->readfn(env, ri);
 480 }
 481
 482 void HELPER(msr_i_pstate)(CPUARMState *env, uint32_t op, uint32_t imm)
 483 {
 484     /* MSR_i to update PSTATE. This is OK from EL0 only if UMA is set.
 485      * Note that SPSel is never OK from EL0; we rely on handle_msr_i()
 486      * to catch that case at translate time.
 487      */
 488     if (arm_current_el(env) == 0 && !(env->cp15.sctlr_el[1] & SCTLR_UMA)) {
 489         uint32_t syndrome = syn_aa64_sysregtrap(0, extract32(op, 0, 3),
 490                                                 extract32(op, 3, 3), 4,
 491                                                 imm, 0x1f, 0);
 492         raise_exception(env, EXCP_UDEF, syndrome, exception_target_el(env));
 493     }
 494
 495     switch (op) {
 496     case 0x05: /* SPSel */
 497         update_spsel(env, imm);
 498         break;
 499     case 0x1e: /* DAIFSet */
 500         env->daif |= (imm << 6) & PSTATE_DAIF;
 501         break;
 502     case 0x1f: /* DAIFClear */
 503         env->daif &= ~((imm << 6) & PSTATE_DAIF);
 504         break;
 505     default:
 506         g_assert_not_reached();
 507     }
 508 }
 509
 510 void HELPER(clear_pstate_ss)(CPUARMState *env)
 511 {
 512     env->pstate &= ~PSTATE_SS;
 513 }
 514
 515 void HELPER(pre_hvc)(CPUARMState *env)
 516 {
 517     ARMCPU *cpu = arm_env_get_cpu(env);
 518     int cur_el = arm_current_el(env);
 519     /* FIXME: Use actual secure state.  */
 520     bool secure = false;
 521     bool undef;
 522
 523     if (arm_is_psci_call(cpu, EXCP_HVC)) {
 524         /* If PSCI is enabled and this looks like a valid PSCI call then
 525          * that overrides the architecturally mandated HVC behaviour.
 526          */
 527         return;
 528     }
 529
 530     if (!arm_feature(env, ARM_FEATURE_EL2)) {
 531         /* If EL2 doesn't exist, HVC always UNDEFs */
 532         undef = true;
 533     } else if (arm_feature(env, ARM_FEATURE_EL3)) {
 534         /* EL3.HCE has priority over EL2.HCD. */
 535         undef = !(env->cp15.scr_el3 & SCR_HCE);
 536     } else {
 537         undef = env->cp15.hcr_el2 & HCR_HCD;
 538     }
 539
 540     /* In ARMv7 and ARMv8/AArch32, HVC is undef in secure state.
 541      * For ARMv8/AArch64, HVC is allowed in EL3.
 542      * Note that we've already trapped HVC from EL0 at translation
 543      * time.
 544      */
 545     if (secure && (!is_a64(env) || cur_el == 1)) {
 546         undef = true;
 547     }
 548
 549     if (undef) {
 550         raise_exception(env, EXCP_UDEF, syn_uncategorized(),
 551                         exception_target_el(env));
 552     }
 553 }
 554
 555 void HELPER(pre_smc)(CPUARMState *env, uint32_t syndrome)
 556 {
 557     ARMCPU *cpu = arm_env_get_cpu(env);
 558     int cur_el = arm_current_el(env);
 559     bool secure = arm_is_secure(env);
 560     bool smd = env->cp15.scr_el3 & SCR_SMD;
 561     /* On ARMv8 AArch32, SMD only applies to NS state.
 562      * On ARMv7 SMD only applies to NS state and only if EL2 is available.
 563      * For ARMv7 non EL2, we force SMD to zero so we don't need to re-check
 564      * the EL2 condition here.
 565      */
 566     bool undef = is_a64(env) ? smd : (!secure && smd);
 567
 568     if (arm_is_psci_call(cpu, EXCP_SMC)) {
 569         /* If PSCI is enabled and this looks like a valid PSCI call then
 570          * that overrides the architecturally mandated SMC behaviour.
 571          */
 572         return;
 573     }
 574
 575     if (!arm_feature(env, ARM_FEATURE_EL3)) {
 576         /* If we have no EL3 then SMC always UNDEFs */
 577         undef = true;
 578     } else if (!secure && cur_el == 1 && (env->cp15.hcr_el2 & HCR_TSC)) {
 579         /* In NS EL1, HCR controlled routing to EL2 has priority over SMD. */
 580         raise_exception(env, EXCP_HYP_TRAP, syndrome, 2);
 581     }
 582
 583     if (undef) {
 584         raise_exception(env, EXCP_UDEF, syn_uncategorized(),
 585                         exception_target_el(env));
 586     }
 587 }
 588
 589 void HELPER(exception_return)(CPUARMState *env)
 590 {
 591     int cur_el = arm_current_el(env);
 592     unsigned int spsr_idx = aarch64_banked_spsr_index(cur_el);
 593     uint32_t spsr = env->banked_spsr[spsr_idx];
 594     int new_el;
 595
 596     aarch64_save_sp(env, cur_el);
 597
 598     env->exclusive_addr = -1;
 599
 600     /* We must squash the PSTATE.SS bit to zero unless both of the
 601      * following hold:
 602      *  1. debug exceptions are currently disabled
 603      *  2. singlestep will be active in the EL we return to
 604      * We check 1 here and 2 after we've done the pstate/cpsr write() to
 605      * transition to the EL we're going to.
 606      */
 607     if (arm_generate_debug_exceptions(env)) {
 608         spsr &= ~PSTATE_SS;
 609     }
 610
 611     if (spsr & PSTATE_nRW) {
 612         /* TODO: We currently assume EL1/2/3 are running in AArch64.  */
 613         env->aarch64 = 0;
 614         new_el = 0;
 615         env->uncached_cpsr = 0x10;
 616         cpsr_write(env, spsr, ~0);
 617         if (!arm_singlestep_active(env)) {
 618             env->uncached_cpsr &= ~PSTATE_SS;
 619         }
 620         aarch64_sync_64_to_32(env);
 621
 622         env->regs[15] = env->elr_el[1] & ~0x1;
 623     } else {
 624         new_el = extract32(spsr, 2, 2);
 625         if (new_el > cur_el
 626             || (new_el == 2 && !arm_feature(env, ARM_FEATURE_EL2))) {
 627             /* Disallow return to an EL which is unimplemented or higher
 628              * than the current one.
 629              */
 630             goto illegal_return;
 631         }
 632         if (extract32(spsr, 1, 1)) {
 633             /* Return with reserved M[1] bit set */
 634             goto illegal_return;
 635         }
 636         if (new_el == 0 && (spsr & PSTATE_SP)) {
 637             /* Return to EL0 with M[0] bit set */
 638             goto illegal_return;
 639         }
 640         env->aarch64 = 1;
 641         pstate_write(env, spsr);
 642         if (!arm_singlestep_active(env)) {
 643             env->pstate &= ~PSTATE_SS;
 644         }
 645         aarch64_restore_sp(env, new_el);
 646         env->pc = env->elr_el[cur_el];
 647     }
 648
 649     return;
 650
 651 illegal_return:
 652     /* Illegal return events of various kinds have architecturally
 653      * mandated behaviour:
 654      * restore NZCV and DAIF from SPSR_ELx
 655      * set PSTATE.IL
 656      * restore PC from ELR_ELx
 657      * no change to exception level, execution state or stack pointer
 658      */
 659     env->pstate |= PSTATE_IL;
 660     env->pc = env->elr_el[cur_el];
 661     spsr &= PSTATE_NZCV | PSTATE_DAIF;
 662     spsr |= pstate_read(env) & ~(PSTATE_NZCV | PSTATE_DAIF);
 663     pstate_write(env, spsr);
 664     if (!arm_singlestep_active(env)) {
 665         env->pstate &= ~PSTATE_SS;
 666     }
 667 }
 668
 669 /* Return true if the linked breakpoint entry lbn passes its checks */
 670 static bool linked_bp_matches(ARMCPU *cpu, int lbn)
 671 {
 672     CPUARMState *env = &cpu->env;
 673     uint64_t bcr = env->cp15.dbgbcr[lbn];
 674     int brps = extract32(cpu->dbgdidr, 24, 4);
 675     int ctx_cmps = extract32(cpu->dbgdidr, 20, 4);
 676     int bt;
 677     uint32_t contextidr;
 678
 679     /* Links to unimplemented or non-context aware breakpoints are
 680      * CONSTRAINED UNPREDICTABLE: either behave as if disabled, or
 681      * as if linked to an UNKNOWN context-aware breakpoint (in which
 682      * case DBGWCR<n>_EL1.LBN must indicate that breakpoint).
 683      * We choose the former.
 684      */
 685     if (lbn > brps || lbn < (brps - ctx_cmps)) {
 686         return false;
 687     }
 688
 689     bcr = env->cp15.dbgbcr[lbn];
 690
 691     if (extract64(bcr, 0, 1) == 0) {
 692         /* Linked breakpoint disabled : generate no events */
 693         return false;
 694     }
 695
 696     bt = extract64(bcr, 20, 4);
 697
 698     /* We match the whole register even if this is AArch32 using the
 699      * short descriptor format (in which case it holds both PROCID and ASID),
 700      * since we don't implement the optional v7 context ID masking.
 701      */
 702     contextidr = extract64(env->cp15.contextidr_el[1], 0, 32);
 703
 704     switch (bt) {
 705     case 3: /* linked context ID match */
 706         if (arm_current_el(env) > 1) {
 707             /* Context matches never fire in EL2 or (AArch64) EL3 */
 708             return false;
 709         }
 710         return (contextidr == extract64(env->cp15.dbgbvr[lbn], 0, 32));
 711     case 5: /* linked address mismatch (reserved in AArch64) */
 712     case 9: /* linked VMID match (reserved if no EL2) */
 713     case 11: /* linked context ID and VMID match (reserved if no EL2) */
 714     default:
 715         /* Links to Unlinked context breakpoints must generate no
 716          * events; we choose to do the same for reserved values too.
 717          */
 718         return false;
 719     }
 720
 721     return false;
 722 }
 723
 724 static bool bp_wp_matches(ARMCPU *cpu, int n, bool is_wp)
 725 {
 726     CPUARMState *env = &cpu->env;
 727     uint64_t cr;
 728     int pac, hmc, ssc, wt, lbn;
 729     /* Note that for watchpoints the check is against the CPU security
 730      * state, not the S/NS attribute on the offending data access.
 731      */
 732     bool is_secure = arm_is_secure(env);
 733     int access_el = arm_current_el(env);
 734
 735     if (is_wp) {
 736         CPUWatchpoint *wp = env->cpu_watchpoint[n];
 737
 738         if (!wp || !(wp->flags & BP_WATCHPOINT_HIT)) {
 739             return false;
 740         }
 741         cr = env->cp15.dbgwcr[n];
 742         if (wp->hitattrs.user) {
 743             /* The LDRT/STRT/LDT/STT "unprivileged access" instructions should
 744              * match watchpoints as if they were accesses done at EL0, even if
 745              * the CPU is at EL1 or higher.
 746              */
 747             access_el = 0;
 748         }
 749     } else {
 750         uint64_t pc = is_a64(env) ? env->pc : env->regs[15];
 751
 752         if (!env->cpu_breakpoint[n] || env->cpu_breakpoint[n]->pc != pc) {
 753             return false;
 754         }
 755         cr = env->cp15.dbgbcr[n];
 756     }
 757     /* The WATCHPOINT_HIT flag guarantees us that the watchpoint is
 758      * enabled and that the address and access type match; for breakpoints
 759      * we know the address matched; check the remaining fields, including
 760      * linked breakpoints. We rely on WCR and BCR having the same layout
 761      * for the LBN, SSC, HMC, PAC/PMC and is-linked fields.
 762      * Note that some combinations of {PAC, HMC, SSC} are reserved and
 763      * must act either like some valid combination or as if the watchpoint
 764      * were disabled. We choose the former, and use this together with
 765      * the fact that EL3 must always be Secure and EL2 must always be
 766      * Non-Secure to simplify the code slightly compared to the full
 767      * table in the ARM ARM.
 768      */
 769     pac = extract64(cr, 1, 2);
 770     hmc = extract64(cr, 13, 1);
 771     ssc = extract64(cr, 14, 2);
 772
 773     switch (ssc) {
 774     case 0:
 775         break;
 776     case 1:
 777     case 3:
 778         if (is_secure) {
 779             return false;
 780         }
 781         break;
 782     case 2:
 783         if (!is_secure) {
 784             return false;
 785         }
 786         break;
 787     }
 788
 789     switch (access_el) {
 790     case 3:
 791     case 2:
 792         if (!hmc) {
 793             return false;
 794         }
 795         break;
 796     case 1:
 797         if (extract32(pac, 0, 1) == 0) {
 798             return false;
 799         }
 800         break;
 801     case 0:
 802         if (extract32(pac, 1, 1) == 0) {
 803             return false;
 804         }
 805         break;
 806     default:
 807         g_assert_not_reached();
 808     }
 809
 810     wt = extract64(cr, 20, 1);
 811     lbn = extract64(cr, 16, 4);
 812
 813     if (wt && !linked_bp_matches(cpu, lbn)) {
 814         return false;
 815     }
 816
 817     return true;
 818 }
 819
 820 static bool check_watchpoints(ARMCPU *cpu)
 821 {
 822     CPUARMState *env = &cpu->env;
 823     int n;
 824
 825     /* If watchpoints are disabled globally or we can't take debug
 826      * exceptions here then watchpoint firings are ignored.
 827      */
 828     if (extract32(env->cp15.mdscr_el1, 15, 1) == 0
 829         || !arm_generate_debug_exceptions(env)) {
 830         return false;
 831     }
 832
 833     for (n = 0; n < ARRAY_SIZE(env->cpu_watchpoint); n++) {
 834         if (bp_wp_matches(cpu, n, true)) {
 835             return true;
 836         }
 837     }
 838     return false;
 839 }
 840
 841 static bool check_breakpoints(ARMCPU *cpu)
 842 {
 843     CPUARMState *env = &cpu->env;
 844     int n;
 845
 846     /* If breakpoints are disabled globally or we can't take debug
 847      * exceptions here then breakpoint firings are ignored.
 848      */
 849     if (extract32(env->cp15.mdscr_el1, 15, 1) == 0
 850         || !arm_generate_debug_exceptions(env)) {
 851         return false;
 852     }
 853
 854     for (n = 0; n < ARRAY_SIZE(env->cpu_breakpoint); n++) {
 855         if (bp_wp_matches(cpu, n, false)) {
 856             return true;
 857         }
 858     }
 859     return false;
 860 }
 861
 862 void arm_debug_excp_handler(CPUState *cs)
 863 {
 864     /* Called by core code when a watchpoint or breakpoint fires;
 865      * need to check which one and raise the appropriate exception.
 866      */
 867     ARMCPU *cpu = ARM_CPU(cs);
 868     CPUARMState *env = &cpu->env;
 869     CPUWatchpoint *wp_hit = cs->watchpoint_hit;
 870
 871     if (wp_hit) {
 872         if (wp_hit->flags & BP_CPU) {
 873             cs->watchpoint_hit = NULL;
 874             if (check_watchpoints(cpu)) {
 875                 bool wnr = (wp_hit->flags & BP_WATCHPOINT_HIT_WRITE) != 0;
 876                 bool same_el = arm_debug_target_el(env) == arm_current_el(env);
 877
 878                 if (extended_addresses_enabled(env)) {
 879                     env->exception.fsr = (1 << 9) | 0x22;
 880                 } else {
 881                     env->exception.fsr = 0x2;
 882                 }
 883                 env->exception.vaddress = wp_hit->hitaddr;
 884                 raise_exception(env, EXCP_DATA_ABORT,
 885                                 syn_watchpoint(same_el, 0, wnr),
 886                                 arm_debug_target_el(env));
 887             } else {
 888                 cpu_resume_from_signal(cs, NULL);
 889             }
 890         }
 891     } else {
 892         if (check_breakpoints(cpu)) {
 893             bool same_el = (arm_debug_target_el(env) == arm_current_el(env));
 894             if (extended_addresses_enabled(env)) {
 895                 env->exception.fsr = (1 << 9) | 0x22;
 896             } else {
 897                 env->exception.fsr = 0x2;
 898             }
 899             /* FAR is UNKNOWN, so doesn't need setting */
 900             raise_exception(env, EXCP_PREFETCH_ABORT,
 901                             syn_breakpoint(same_el),
 902                             arm_debug_target_el(env));
 903         }
 904     }
 905 }
 906
 907 /* ??? Flag setting arithmetic is awkward because we need to do comparisons.
 908    The only way to do that in TCG is a conditional branch, which clobbers
 909    all our temporaries.  For now implement these as helper functions.  */
 910
 911 /* Similarly for variable shift instructions.  */
 912
 913 uint32_t HELPER(shl_cc)(CPUARMState *env, uint32_t x, uint32_t i)
 914 {
 915     int shift = i & 0xff;
 916     if (shift >= 32) {
 917         if (shift == 32)
 918             env->CF = x & 1;
 919         else
 920             env->CF = 0;
 921         return 0;
 922     } else if (shift != 0) {
 923         env->CF = (x >> (32 - shift)) & 1;
 924         return x << shift;
 925     }
 926     return x;
 927 }
 928
 929 uint32_t HELPER(shr_cc)(CPUARMState *env, uint32_t x, uint32_t i)
 930 {
 931     int shift = i & 0xff;
 932     if (shift >= 32) {
 933         if (shift == 32)
 934             env->CF = (x >> 31) & 1;
 935         else
 936             env->CF = 0;
 937         return 0;
 938     } else if (shift != 0) {
 939         env->CF = (x >> (shift - 1)) & 1;
 940         return x >> shift;
 941     }
 942     return x;
 943 }
 944
 945 uint32_t HELPER(sar_cc)(CPUARMState *env, uint32_t x, uint32_t i)
 946 {
 947     int shift = i & 0xff;
 948     if (shift >= 32) {
 949         env->CF = (x >> 31) & 1;
 950         return (int32_t)x >> 31;
 951     } else if (shift != 0) {
 952         env->CF = (x >> (shift - 1)) & 1;
 953         return (int32_t)x >> shift;
 954     }
 955     return x;
 956 }
 957
 958 uint32_t HELPER(ror_cc)(CPUARMState *env, uint32_t x, uint32_t i)
 959 {
 960     int shift1, shift;
 961     shift1 = i & 0xff;
 962     shift = shift1 & 0x1f;
 963     if (shift == 0) {
 964         if (shift1 != 0)
 965             env->CF = (x >> 31) & 1;
 966         return x;
 967     } else {
 968         env->CF = (x >> (shift - 1)) & 1;
 969         return ((uint32_t)x >> shift) | (x << (32 - shift));
 970     }
 971 }