src/arch/arm/self_debug.cc

   1 /*
   2  * Copyright (c) 2019 Metempsy Technology LSC
   3  * All rights reserved
   4  *
   5  * The license below extends only to copyright in the software and shall
   6  * not be construed as granting a license to any other intellectual
   7  * property including but not limited to intellectual property relating
   8  * to a hardware implementation of the functionality of the software
   9  * licensed hereunder.  You may use the software subject to the license
  10  * terms below provided that you ensure that this notice is replicated
  11  * unmodified and in its entirety in all distributions of the software,
  12  * modified or unmodified, in source code or in binary form.
  13  *
  14  * Redistribution and use in source and binary forms, with or without
  15  * modification, are permitted provided that the following conditions are
  16  * met: redistributions of source code must retain the above copyright
  17  * notice, this list of conditions and the following disclaimer;
  18  * redistributions in binary form must reproduce the above copyright
  19  * notice, this list of conditions and the following disclaimer in the
  20  * documentation and/or other materials provided with the distribution;
  21  * neither the name of the copyright holders nor the names of its
  22  * contributors may be used to endorse or promote products derived from
  23  * this software without specific prior written permission.
  24  *
  25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  28  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  29  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  30  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  31  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  32  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  33  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  34  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  35  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  36  */
  37
  38 #include "arch/arm/self_debug.hh"
  39
  40 #include "arch/arm/faults.hh"
  41 #include "arch/arm/miscregs_types.hh"
  42 #include "base/bitfield.hh"
  43
  44 using namespace ArmISA;
  45 using namespace std;
  46
  47 Fault
  48 SelfDebug::testDebug(ThreadContext *tc, const RequestPtr &req,
  49                      BaseTLB::Mode mode)
  50 {
  51     Fault fault = NoFault;
  52
  53     if (mode == BaseTLB::Execute) {
  54         const bool d_step = softStep->advanceSS(tc);
  55         if (!d_step) {
  56             fault = testVectorCatch(tc, req->getVaddr(), nullptr);
  57             if (fault == NoFault)
  58                 fault = testBreakPoints(tc, req->getVaddr());
  59         }
  60     } else if (!req->isCacheMaintenance() ||
  61              (req->isCacheInvalidate() && !req->isCacheClean())) {
  62         bool md = mode == BaseTLB::Write ? true: false;
  63         fault = testWatchPoints(tc, req->getVaddr(), md,
  64                                 req->isAtomic(),
  65                                 req->getSize(),
  66                                 req->isCacheMaintenance());
  67     }
  68
  69     return fault;
  70 }
  71
  72 Fault
  73 SelfDebug::testBreakPoints(ThreadContext *tc, Addr vaddr)
  74 {
  75     if (!enableFlag)
  76         return NoFault;
  77
  78     setAArch32(tc);
  79
  80     to32 = targetAArch32(tc);
  81
  82     init(tc);
  83
  84     if (!isDebugEnabled(tc))
  85         return NoFault;
  86
  87     ExceptionLevel el = (ExceptionLevel) currEL(tc);
  88     for (auto &p: arBrkPoints){
  89         PCState pcst = tc->pcState();
  90         Addr pc = vaddr;
  91         if (pcst.itstate() != 0x0)
  92             pc = pcst.pc();
  93         if (p.enable && p.isActive(pc) &&(!to32 || !p.onUse)) {
  94             const DBGBCR ctr = p.getControlReg(tc);
  95             if (p.isEnabled(tc, el, ctr.hmc, ctr.ssc, ctr.pmc)) {
  96                 bool debug = p.test(tc, pc, el, ctr, false);
  97                 if (debug){
  98                     if (to32)
  99                         p.onUse = true;
 100                     return triggerException(tc, pc);
 101                 }
 102             }
 103         }
 104     }
 105     return NoFault;
 106 }
 107
 108
 109 Fault
 110 SelfDebug::triggerException(ThreadContext *tc, Addr vaddr)
 111 {
 112     if (to32) {
 113         return std::make_shared<PrefetchAbort>(vaddr,
 114                                    ArmFault::DebugEvent, false,
 115                                    ArmFault::UnknownTran,
 116                                    ArmFault::BRKPOINT);
 117     } else {
 118         return std::make_shared<HardwareBreakpoint>(vaddr, 0x22);
 119     }
 120 }
 121
 122 Fault
 123 SelfDebug::testWatchPoints(ThreadContext *tc, Addr vaddr, bool write,
 124                            bool atomic, unsigned size, bool cm)
 125 {
 126     setAArch32(tc);
 127     to32 = targetAArch32(tc);
 128     if (!initialized)
 129         init(tc);
 130     if (!isDebugEnabled(tc) || !enableFlag)
 131         return NoFault;
 132
 133     ExceptionLevel el = (ExceptionLevel) currEL(tc);
 134     int idxtmp = -1;
 135     for (auto &p: arWatchPoints){
 136         idxtmp ++;
 137         if (p.enable) {
 138             bool debug = p.test(tc, vaddr, el, write, atomic, size);
 139             if (debug){
 140                 return triggerWatchpointException(tc, vaddr, write, cm);
 141             }
 142         }
 143     }
 144     return NoFault;
 145 }
 146
 147 Fault
 148 SelfDebug::triggerWatchpointException(ThreadContext *tc, Addr vaddr,
 149                                       bool write, bool cm)
 150 {
 151     if (to32) {
 152         ArmFault::DebugType d = cm? ArmFault::WPOINT_CM:
 153                                     ArmFault::WPOINT_NOCM;
 154         return std::make_shared<DataAbort>(vaddr,
 155                                            TlbEntry::DomainType::NoAccess,
 156                                            write, ArmFault::DebugEvent, cm,
 157                                            ArmFault::UnknownTran, d);
 158     } else {
 159         return std::make_shared<Watchpoint>(0, vaddr, write, cm);
 160     }
 161 }
 162
 163 bool
 164 SelfDebug::isDebugEnabledForEL64(ThreadContext *tc, ExceptionLevel el,
 165                          bool secure, bool mask)
 166 {
 167     bool route_to_el2 =  ArmSystem::haveEL(tc, EL2) &&
 168         !secure && enableTdeTge;
 169
 170     ExceptionLevel target_el = route_to_el2 ? EL2 : EL1;
 171     if (oslk || (bSDD && secure && ArmSystem::haveEL(tc, EL3))) {
 172         return false;
 173     }
 174
 175     if (el == target_el) {
 176         return bKDE  && !mask;
 177     } else {
 178         return target_el > el;
 179     }
 180 }
 181
 182 bool
 183 SelfDebug::isDebugEnabledForEL32(ThreadContext *tc, ExceptionLevel el,
 184                          bool secure, bool mask)
 185 {
 186     if (el == EL0 && !ELStateUsingAArch32(tc, EL1, secure)) {
 187         return isDebugEnabledForEL64(tc, el, secure, mask);
 188     }
 189
 190     if (oslk) {
 191         return false;
 192     }
 193
 194     bool enabled;
 195     if (secure && ArmSystem::haveEL(tc, EL3)) {
 196         // We ignore the check for invasive External debug checking SPIDEN
 197         // and DBGEN signals. They are not implemented
 198         bool spd32 = bits(tc->readMiscReg(MISCREG_MDCR_EL3), 14);
 199         enabled = spd32;
 200
 201         bool suiden = bits(tc->readMiscReg(MISCREG_SDER), 0);
 202         enabled  = el == EL0 ? (enabled || suiden) : enabled;
 203     } else {
 204         enabled = el != EL2;
 205     }
 206     return enabled;
 207 }
 208
 209 bool
 210 BrkPoint::testLinkedBk(ThreadContext *tc, Addr vaddr, ExceptionLevel el)
 211 {
 212     bool debug = false;
 213     const DBGBCR ctr = getControlReg(tc);
 214     if ((ctr.bt & 0x1) && enable) {
 215         debug = test(tc, vaddr, el, ctr, true);
 216     }
 217     return debug;
 218 }
 219
 220 bool
 221 BrkPoint::test(ThreadContext *tc, Addr pc, ExceptionLevel el, DBGBCR ctr,
 222                bool from_link)
 223 {
 224     bool v = false;
 225     switch (ctr.bt) {
 226       case 0x0:
 227         v = testAddrMatch(tc, pc, ctr.bas);
 228         break;
 229
 230       case 0x1:
 231         v = testAddrMatch(tc, pc, ctr.bas); // linked
 232         if (v) {
 233             v = (conf->getBrkPoint(ctr.lbn))->testLinkedBk(tc, pc, el);
 234         }
 235         break;
 236
 237       case 0x2:
 238         {
 239             bool host = ELIsInHost(tc, el);
 240             v = testContextMatch(tc, !host, true);
 241         }
 242         break;
 243
 244       case 0x3:
 245         if (from_link){
 246             bool host = ELIsInHost(tc, el);
 247             v = testContextMatch(tc, !host, true);
 248         }
 249         break;
 250
 251       case 0x4:
 252         v = testAddrMissMatch(tc, pc, ctr.bas);
 253         break;
 254
 255       case 0x5:
 256         v = testAddrMissMatch(tc, pc, ctr.bas); // linked
 257         if (v && !from_link)
 258             v = v && (conf->getBrkPoint(ctr.lbn))->testLinkedBk(tc, pc, el);
 259         break;
 260
 261       case 0x6:
 262         if (HaveVirtHostExt(tc) && !ELIsInHost(tc, el))
 263              v = testContextMatch(tc, true);
 264         break;
 265
 266       case 0x7:
 267         if (HaveVirtHostExt(tc) && !ELIsInHost(tc, el) && from_link)
 268             v = testContextMatch(tc, true);
 269         break;
 270
 271       case 0x8:
 272         if (ArmSystem::haveEL(tc, EL2) && !ELIsInHost(tc, el)) {
 273             v = testVMIDMatch(tc);
 274         }
 275         break;
 276
 277       case 0x9:
 278         if (from_link && ArmSystem::haveEL(tc, EL2) &&
 279             !ELIsInHost(tc, el)) {
 280             v = testVMIDMatch(tc);
 281         }
 282         break;
 283
 284       case 0xa:
 285         if (ArmSystem::haveEL(tc, EL2) && !ELIsInHost(tc, el)) {
 286             v = testContextMatch(tc, true);
 287             if (v && !from_link)
 288                  v = v && testVMIDMatch(tc);
 289         }
 290         break;
 291       case 0xb:
 292         if (from_link && ArmSystem::haveEL(tc, EL2) &&
 293             !ELIsInHost(tc, el)) {
 294             v = testContextMatch(tc, true);
 295             v = v && testVMIDMatch(tc);
 296         }
 297         break;
 298
 299       case 0xc:
 300         if (HaveVirtHostExt(tc) && !inSecureState(tc))
 301             v = testContextMatch(tc, false);
 302         break;
 303
 304       case 0xd:
 305         if (HaveVirtHostExt(tc) && from_link && !inSecureState(tc))
 306             v = testContextMatch(tc, false);
 307         break;
 308
 309       case 0xe:
 310         if (HaveVirtHostExt(tc) && !ELIsInHost(tc, el)
 311                 && !inSecureState(tc) ) {
 312             v = testContextMatch(tc, true); // CONTEXTIDR_EL1
 313             v = v && testContextMatch(tc, false); // CONTEXTIDR_EL2
 314         }
 315         break;
 316       case 0xf:
 317         if (HaveVirtHostExt(tc) && !ELIsInHost(tc, el) && from_link
 318                 && !inSecureState(tc) ) {
 319             v = testContextMatch(tc, true); // CONTEXTIDR_EL1
 320             v = v && testContextMatch(tc, false); // CONTEXTIDR_EL2
 321         }
 322         break;
 323       default:
 324         break;
 325     }
 326     return v;
 327 }
 328
 329 void
 330 SelfDebug::init(ThreadContext *tc)
 331 {
 332     if (initialized)
 333         return;
 334     CPSR cpsr = tc->readMiscReg(MISCREG_CPSR);
 335     aarch32 = cpsr.width == 1;
 336
 337     const AA64DFR0 dfr = tc->readMiscReg(MISCREG_ID_AA64DFR0_EL1);
 338     const AA64MMFR2 mm_fr2 = tc->readMiscReg(MISCREG_ID_AA64MMFR2_EL1);
 339     const AA64MMFR1 mm_fr1 = tc->readMiscReg(MISCREG_ID_AA64MMFR1_EL1);
 340
 341     for (int i = 0; i <= dfr.brps; i++) {
 342         const bool isctxaw = i >= (dfr.brps - dfr.ctx_cmps);
 343
 344         BrkPoint bkp = BrkPoint((MiscRegIndex)(MISCREG_DBGBCR0_EL1 + i),
 345                                 (MiscRegIndex)(MISCREG_DBGBVR0_EL1 + i),
 346                                 this, isctxaw, (bool)mm_fr2.varange,
 347                                 mm_fr1.vmidbits, aarch32);
 348         const DBGBCR ctr = tc->readMiscReg(MISCREG_DBGBCR0_EL1 + i);
 349
 350         bkp.updateControl(ctr);
 351         arBrkPoints.push_back(bkp);
 352     }
 353
 354     for (int i = 0; i <= dfr.wrps; i++) {
 355         WatchPoint wtp = WatchPoint((MiscRegIndex)(MISCREG_DBGWCR0 + i),
 356                                     (MiscRegIndex)(MISCREG_DBGWVR0 + i),
 357                                     this, (bool)mm_fr2.varange, aarch32);
 358         const DBGWCR ctr = tc->readMiscReg(MISCREG_DBGWCR0 + i);
 359
 360         wtp.updateControl(ctr);
 361         arWatchPoints.push_back(wtp);
 362     }
 363
 364     initialized = true;
 365
 366     RegVal oslar_el1 = tc->readMiscReg(MISCREG_OSLAR_EL1);
 367     updateOSLock(oslar_el1);
 368     // Initialize preloaded control booleans
 369     uint64_t mdscr_el1 = tc->readMiscReg(MISCREG_MDSCR_EL1);
 370     setMDSCRvals(mdscr_el1);
 371
 372     const uint64_t mdcr_el3 = tc->readMiscReg(MISCREG_MDCR_EL3);
 373     setbSDD(mdcr_el3);
 374
 375     const HCR hcr = tc->readMiscReg(MISCREG_HCR_EL2);
 376     const HDCR mdcr  = tc->readMiscRegNoEffect(MISCREG_MDCR_EL2);
 377     setenableTDETGE(hcr, mdcr);
 378
 379     // Enable Vector Catch Exceptions
 380     const DEVID dvid = tc->readMiscReg(MISCREG_DBGDEVID0);
 381     vcExcpt = new VectorCatch(dvid.vectorcatch==0x0, this);
 382 }
 383
 384 bool
 385 BrkPoint::testAddrMatch(ThreadContext *tc, Addr in_pc, uint8_t bas)
 386 {
 387     Addr pc_tocmp = getAddrfromReg(tc);
 388     Addr pc = bits(in_pc, maxAddrSize, 2);
 389
 390     bool prs = true;
 391     CPSR cpsr = tc->readMiscReg(MISCREG_CPSR);
 392     bool thumb = cpsr.t;
 393
 394     if (thumb) {
 395         if (bas == 0xc)
 396             prs = bits(in_pc, 1, 0) == 0x2;
 397         else if (bas == 0x3)
 398             prs = bits(in_pc, 1, 0) == 0x0;
 399     }
 400     return (pc == pc_tocmp) && prs;
 401 }
 402
 403 bool
 404 BrkPoint::testAddrMissMatch(ThreadContext *tc, Addr in_pc, uint8_t bas)
 405 {
 406     if (bas == 0x0)
 407         return true;
 408     Addr pc_tocmp = getAddrfromReg(tc);
 409     Addr pc = bits(in_pc, maxAddrSize, 2);
 410     bool prs = false;
 411     CPSR cpsr = tc->readMiscReg(MISCREG_CPSR);
 412     bool thumb = cpsr.t;
 413
 414     if (thumb) {
 415         if (bas == 0xc)
 416             prs = bits(in_pc, 1, 0) == 0x2;
 417         else if (bas == 0x3)
 418             prs = bits(in_pc, 1, 0) == 0x0;
 419     }
 420     return (pc != pc_tocmp) && !prs;
 421 }
 422
 423 bool
 424 BrkPoint::testContextMatch(ThreadContext *tc, bool ctx1)
 425 {
 426     return testContextMatch(tc, ctx1, ctx1);
 427 }
 428
 429 bool
 430 BrkPoint::testContextMatch(ThreadContext *tc, bool ctx1, bool low_ctx)
 431 {
 432     if (!isCntxtAware)
 433         return false;
 434     MiscRegIndex miscridx;
 435     ExceptionLevel el = currEL(tc);
 436     bool a32 = conf->isAArch32();
 437
 438     if (ctx1) {
 439         miscridx = a32? MISCREG_CONTEXTIDR : MISCREG_CONTEXTIDR_EL1;
 440         if ((el == EL3 && !a32) || el == EL2)
 441             return false;
 442     } else {
 443         miscridx = MISCREG_CONTEXTIDR_EL2;
 444         if (el == EL2 && a32)
 445             return false;
 446     }
 447
 448     RegVal ctxid = bits(tc->readMiscReg(miscridx), 31, 0);
 449     RegVal v = getContextfromReg(tc, low_ctx);
 450     return (v == ctxid);
 451 }
 452
 453 bool
 454 BrkPoint::testVMIDMatch(ThreadContext *tc)
 455 {
 456     uint32_t vmid_index = 55;
 457     if (VMID16enabled)
 458         vmid_index = 63;
 459     ExceptionLevel el = currEL(tc);
 460     if (el == EL2)
 461         return false;
 462
 463     uint32_t vmid = bits(tc->readMiscReg(MISCREG_VTTBR_EL2), vmid_index, 48);
 464     uint32_t v = getVMIDfromReg(tc);
 465     return (v == vmid);
 466 }
 467
 468
 469 bool
 470 BrkPoint::isEnabled(ThreadContext *tc, ExceptionLevel el,
 471                     uint8_t hmc, uint8_t ssc, uint8_t pmc)
 472 {
 473     bool v;
 474     bool aarch32 = conf->isAArch32();
 475     bool no_el2 = !ArmSystem::haveEL(tc, EL2);
 476     bool no_el3 = !ArmSystem::haveEL(tc, EL3);
 477
 478     if (no_el3 && !no_el2 && (ssc == 0x1 || ssc == 0x2) &&
 479         !(hmc && ssc == 0x1 && pmc == 0x0)) {
 480         return false;
 481     } else if (no_el3 && no_el2 && (hmc != 0x0 || ssc != 0x0) &&
 482         !(!aarch32 && ((hmc && ssc == 0x1  && pmc == 0x0) || ssc == 0x3))) {
 483         return false;
 484     } else if (no_el2 && hmc && ssc == 0x3 && pmc == 0x0) {
 485         return false;
 486     } else if (ssc == 0x11 && pmc == 0x1 &&
 487         !(!aarch32 && hmc && ssc == 0x3 && pmc == 0x0)) {
 488         // AND secureEL2 not implemented
 489         return false;
 490     } else if (hmc && ssc == 0x1 && pmc == 0x0) {
 491         //AND secureEL2 not implemented
 492         return false;
 493     }
 494     switch (el) {
 495         case EL0:
 496             v = (pmc == 0x3) || (pmc == 0x2 && hmc == 0x0);
 497             if (aarch32)
 498                 v = v || (pmc == 0x0 && ssc != 0x3 && hmc == 0x0);
 499             if (v && ssc == 0x3)
 500                 panic("Unexpected EL in SelfDebug::isDebugEnabled.\n");
 501             break;
 502         case EL1:
 503             v = (pmc == 0x3) || (pmc == 0x1);
 504             if (aarch32)
 505                 v = v || (pmc == 0x0 && hmc == 0x0 && ssc !=0x3);
 506             break;
 507         case EL2:
 508             v = (ssc == 0x3) ||
 509                 ((hmc == 0x1) && !((ssc == 0x2) && (pmc == 0x0)));
 510             if (v && pmc == 0x2)
 511                 panic("Unexpected EL in SelfDebug::isDebugEnabled.\n");
 512             break;
 513         case EL3:
 514             if (ssc == 0x1)
 515                 panic("Unexpected EL in SelfDebug::isDebugEnabled.\n");
 516             v = (hmc == 0x1) & (ssc != 0x3);
 517             break;
 518         default:
 519             panic("Unexpected EL %d in BrkPoint::isEnabled.\n", el);
 520     }
 521     return v && SelfDebug::securityStateMatch(tc, ssc, hmc || !aarch32);
 522 }
 523
 524 uint32_t
 525 BrkPoint::getVMIDfromReg(ThreadContext *tc)
 526 {
 527     uint32_t vmid_index = 39;
 528     if (VMID16enabled)
 529         vmid_index = 47;
 530     return bits(tc->readMiscReg(valRegIndex), vmid_index, 32);
 531 }
 532
 533
 534 bool
 535 WatchPoint::isEnabled(ThreadContext* tc, ExceptionLevel el,
 536                       bool hmc, uint8_t ssc, uint8_t pac)
 537 {
 538
 539     bool v;
 540     bool aarch32 = conf->isAArch32();
 541     bool no_el2 = !ArmSystem::haveEL(tc, EL2);
 542     bool no_el3 = !ArmSystem::haveEL(tc, EL3);
 543
 544     if (aarch32) {
 545         // WatchPoint PL2 using aarch32 is disabled except for
 546         // debug state. Check G2-5395 table G2-15.
 547         if (el == EL2)
 548             return false;
 549         if (no_el3) {
 550             if (ssc == 0x01 || ssc == 0x02 ){
 551                 return false;
 552             } else if (no_el2 &&
 553                       ((!hmc && ssc == 0x3) || (hmc && ssc == 0x0))) {
 554                 return false;
 555             }
 556         }
 557         if (no_el2 && hmc && ssc == 0x03 && pac == 0)
 558             return false;
 559     }
 560     switch (el) {
 561       case EL0:
 562         v = (pac == 0x3 || (pac == 0x2 && !hmc && ssc != 0x3));
 563         break;
 564       case EL1:
 565         v = (pac == 0x1 || pac == 0x3);
 566         break;
 567       case EL2:
 568         v = (hmc && (ssc != 0x2 || pac != 0x0));
 569         break;
 570       case EL3:
 571         v = (hmc && (ssc == 0x2 ||
 572             (ssc == 0x1 && (pac == 0x1 || pac == 0x3))));
 573         break;
 574       default:
 575         panic("Unexpected EL in WatchPoint::isEnabled.\n");
 576     }
 577     return v && SelfDebug::securityStateMatch(tc, ssc, hmc);
 578 }
 579
 580 bool
 581 WatchPoint::test(ThreadContext *tc, Addr addr, ExceptionLevel el, bool& wrt,
 582                  bool atomic, unsigned size)
 583 {
 584
 585     bool v = false;
 586     const DBGWCR ctr = tc->readMiscReg(ctrlRegIndex);
 587     if (isEnabled(tc, el, ctr.hmc, ctr.ssc, ctr.pac) &&
 588         ((wrt && (ctr.lsv & 0x2)) || (!wrt && (ctr.lsv & 0x1)) || atomic)) {
 589         v = compareAddress(tc, addr, ctr.bas, ctr.mask, size);
 590         if (ctr.wt) {
 591             v = v && (conf->getBrkPoint(ctr.lbn))->testLinkedBk(tc, addr, el);
 592         }
 593     }
 594     if (atomic && (ctr.lsv & 0x1)) {
 595         wrt = false;
 596     }
 597     return v;
 598 }
 599
 600 bool
 601 WatchPoint::compareAddress(ThreadContext *tc, Addr in_addr, uint8_t bas,
 602         uint8_t mask, unsigned size)
 603 {
 604     Addr addr_tocmp = getAddrfromReg(tc);
 605     int maxbits = isDoubleAligned(addr_tocmp) ? 4: 8;
 606     int bottom = isDoubleAligned(addr_tocmp) ? 2: 3;
 607     Addr addr = bits(in_addr, maxAddrSize, 0);
 608
 609     if (bas == 0x0)
 610         return false;
 611
 612     if (mask == 0x0) {
 613         for (int i = 0; i < maxbits; i++) {
 614             uint8_t bas_m = 0x1 << i;
 615             uint8_t masked_bas = bas & bas_m;
 616             if (masked_bas == bas_m) {
 617                 uint8_t off = log2(masked_bas);
 618                 Addr cmpaddr = addr_tocmp | off;
 619                 for (int j = 0; j < size; j++) {
 620                     if ((addr + j) == cmpaddr) {
 621                         return true;
 622                     }
 623                 }
 624             }
 625         }
 626         return false;
 627     } else {
 628         bool v = false;
 629         for (int j = 0; j < size; j++) {
 630             Addr compaddr;
 631             if (mask > bottom) {
 632                 addr = bits((in_addr+j), maxAddrSize, mask);
 633                 compaddr = bits(addr_tocmp, maxAddrSize, mask);
 634             } else {
 635                 addr = bits((in_addr+j), maxAddrSize, bottom);
 636                 compaddr = bits(addr_tocmp, maxAddrSize, bottom);
 637             }
 638             v = v || (addr == compaddr);
 639         }
 640         return v;
 641     }
 642 }
 643
 644 bool
 645 SoftwareStep::debugExceptionReturnSS(ThreadContext *tc, CPSR spsr,
 646                                      ExceptionLevel dest, bool aarch32)
 647 {
 648     bool SS_bit = false;
 649     bool enabled_src = false;
 650     if (bSS) {
 651         enabled_src = conf->isDebugEnabled(tc);
 652
 653         bool enabled_dst = false;
 654         bool secure = isSecureBelowEL3(tc) || dest == EL3;
 655         CPSR cpsr = tc->readMiscReg(MISCREG_CPSR);
 656         if (cpsr.width) {
 657             enabled_dst = conf->isDebugEnabledForEL32(tc, dest, secure,
 658                                                       spsr.d == 1);
 659         } else {
 660             enabled_dst = conf->isDebugEnabledForEL64(tc, dest, secure,
 661                                                       spsr.d == 1);
 662         }
 663         ExceptionLevel ELd = debugTargetFrom(tc, secure);
 664
 665         if (!ELIs32(tc, ELd) && !enabled_src && enabled_dst) {
 666             SS_bit = spsr.ss;
 667             if (SS_bit == 0x0) {
 668                 stateSS = ACTIVE_PENDING_STATE;
 669             } else {
 670                 stateSS = ACTIVE_NOT_PENDING_STATE;
 671             }
 672         }
 673     }
 674     return SS_bit;
 675 }
 676
 677 bool
 678 SoftwareStep::advanceSS(ThreadContext * tc)
 679 {
 680
 681     PCState pc = tc->pcState();
 682     bool res = false;
 683     switch (stateSS) {
 684       case INACTIVE_STATE:
 685         pc.debugStep(false);
 686         break;
 687
 688       case ACTIVE_NOT_PENDING_STATE:
 689         pc.debugStep(false);
 690         if (cpsrD == 1 || !bSS) {
 691             stateSS = INACTIVE_STATE;
 692         } else {
 693             pc.stepped(true);
 694             stateSS = ACTIVE_PENDING_STATE;
 695             tc->pcState(pc);
 696         }
 697         break;
 698
 699       case ACTIVE_PENDING_STATE:
 700         if (!cpsrD && bSS) {
 701             pc.debugStep(true);
 702             res = true;
 703             tc->pcState(pc);
 704         }
 705         stateSS = INACTIVE_STATE;
 706         clearLdx();
 707         break;
 708
 709       default:
 710         break;
 711     }
 712     return res;
 713 }
 714
 715 Fault
 716 SelfDebug::testVectorCatch(ThreadContext *tc, Addr addr,
 717                            ArmFault *fault)
 718 {
 719
 720     setAArch32(tc);
 721     to32 = targetAArch32(tc);
 722     if (!initialized)
 723         init(tc);
 724     if (!isDebugEnabled(tc) || !enableFlag || !aarch32)
 725         return NoFault;
 726
 727     ExceptionLevel el = (ExceptionLevel) currEL(tc);
 728     bool debug;
 729     if (fault == nullptr)
 730         debug = vcExcpt->addressMatching(tc, addr, el);
 731     else
 732         debug = vcExcpt->exceptionTrapping(tc, el, fault);
 733     if (debug) {
 734         if (enableTdeTge) {
 735             return std::make_shared<HypervisorTrap>(0, 0x22,
 736                                         EC_PREFETCH_ABORT_TO_HYP);
 737         } else {
 738             return std::make_shared<PrefetchAbort>(addr,
 739                                        ArmFault::DebugEvent, false,
 740                                        ArmFault::UnknownTran,
 741                                        ArmFault::VECTORCATCH);
 742         }
 743     }
 744
 745     return NoFault;
 746 }
 747
 748 bool
 749 VectorCatch::addressMatching(ThreadContext *tc, Addr addr, ExceptionLevel el)
 750 {
 751     // Each bit position in this string corresponds to a bit in DBGVCR
 752     // and an exception vector.
 753     bool enabled;
 754     if (conf->isAArch32() && ELIs32(tc, EL1) &&
 755         (addr & 0x3) == 0 && el != EL2 ) {
 756
 757         DBGVCR match_word = 0x0;
 758
 759         Addr vbase = getVectorBase(tc, false);
 760         Addr vaddress = addr & ~ 0x1f;
 761         Addr low_addr = bits(addr, 5, 2);
 762         if (vaddress == vbase) {
 763             if (ArmSystem::haveEL(tc, EL3) && !inSecureState(tc)) {
 764                 uint32_t bmask = 1UL << (low_addr + 24);
 765                 match_word = match_word | (DBGVCR) bmask;
 766                 // Non-secure vectors
 767             } else {
 768                 uint32_t bmask = 1UL << (low_addr);
 769                 match_word = match_word | (DBGVCR) bmask;
 770                 // Secure vectors (or no EL3)
 771             }
 772         }
 773         uint32_t mvbase = getVectorBase(tc, true);
 774         if (ArmSystem::haveEL(tc, EL3) && ELIs32(tc, EL3) &&
 775             inSecureState(tc) && (vaddress == mvbase)) {
 776             uint32_t bmask = 1UL << (low_addr + 8);
 777             match_word = match_word | (DBGVCR) bmask;
 778             // Monitor vectors
 779         }
 780
 781         DBGVCR mask;
 782
 783         // Mask out bits not corresponding to vectors.
 784         if (!ArmSystem::haveEL(tc, EL3)) {
 785             mask = (DBGVCR) 0xDE;
 786         } else if (!ELIs32(tc, EL3)) {
 787             mask = (DBGVCR) 0xDE0000DE;
 788         } else {
 789             mask = (DBGVCR) 0xDE00DEDE;
 790         }
 791         DBGVCR dbgvcr = tc->readMiscReg(MISCREG_DBGVCR);
 792         match_word = match_word & dbgvcr & mask;
 793         enabled = match_word != 0x0;
 794         // Check for UNPREDICTABLE case - match on Prefetch Abort and
 795         // Data Abort vectors
 796         ExceptionLevel ELd = debugTargetFrom(tc, inSecureState(tc));
 797         if (((match_word & 0x18001818) != 0x0) && ELd == el) {
 798             enabled = false;
 799         }
 800     } else {
 801         enabled = false;
 802     }
 803     return enabled;
 804 }
 805
 806 bool
 807 VectorCatch::exceptionTrapping(ThreadContext *tc, ExceptionLevel el,
 808                                ArmFault* fault)
 809 {
 810     if (conf->isAArch32() && ELIs32(tc, EL1) && el != EL2) {
 811
 812         DBGVCR dbgvcr = tc->readMiscReg(MISCREG_DBGVCR);
 813         DBGVCR match_type = fault->vectorCatchFlag();
 814         DBGVCR mask;
 815
 816         if (!ArmSystem::haveEL(tc, EL3)) {
 817             mask = (DBGVCR) 0xDE;
 818         } else if (ELIs32(tc, EL3) && fault->getToMode() == MODE_MON) {
 819             mask = (DBGVCR) 0x0000DE00;
 820         } else {
 821             if (inSecureState(tc))
 822                 mask = (DBGVCR) 0x000000DE;
 823             else
 824                 mask = (DBGVCR) 0xDE000000;
 825         }
 826         match_type = match_type & mask & dbgvcr;
 827
 828         if (match_type != 0x0) {
 829             return true;
 830         }
 831     }
 832     return false;
 833 }
 834