sys/arm/arm/debug_monitor.c

   1 /*
   2  * Copyright (c) 2015 Juniper Networks Inc.
   3  * All rights reserved.
   4  *
   5  * Developed by Semihalf.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  * 1. Redistributions of source code must retain the above copyright
  11  *    notice, this list of conditions and the following disclaimer.
  12  * 2. Redistributions in binary form must reproduce the above copyright
  13  *    notice, this list of conditions and the following disclaimer in the
  14  *    documentation and/or other materials provided with the distribution.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  18  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  19  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  20  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  21  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  22  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  24  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  25  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  26  * SUCH DAMAGE.
  27  */
  28
  29 #include <sys/cdefs.h>
  30 __FBSDID("$FreeBSD$");
  31
  32 #include "opt_ddb.h"
  33
  34 #include <sys/param.h>
  35 #include <sys/types.h>
  36 #include <sys/kdb.h>
  37 #include <sys/pcpu.h>
  38 #include <sys/smp.h>
  39 #include <sys/systm.h>
  40
  41 #include <machine/atomic.h>
  42 #include <machine/armreg.h>
  43 #include <machine/cpu.h>
  44 #include <machine/debug_monitor.h>
  45 #include <machine/kdb.h>
  46 #include <machine/pcb.h>
  47 #include <machine/reg.h>
  48
  49 #include <ddb/ddb.h>
  50 #include <ddb/db_access.h>
  51 #include <ddb/db_sym.h>
  52
  53 enum dbg_t {
  54         DBG_TYPE_BREAKPOINT = 0,
  55         DBG_TYPE_WATCHPOINT = 1,
  56 };
  57
  58 struct dbg_wb_conf {
  59         enum dbg_t              type;
  60         enum dbg_access_t       access;
  61         db_addr_t               address;
  62         db_expr_t               size;
  63         u_int                   slot;
  64 };
  65
  66 static int dbg_reset_state(void);
  67 static int dbg_setup_breakpoint(db_expr_t, db_expr_t, u_int);
  68 static int dbg_remove_breakpoint(u_int);
  69 static u_int dbg_find_slot(enum dbg_t, db_expr_t);
  70 static boolean_t dbg_check_slot_free(enum dbg_t, u_int);
  71
  72 static int dbg_remove_xpoint(struct dbg_wb_conf *);
  73 static int dbg_setup_xpoint(struct dbg_wb_conf *);
  74
  75 static int dbg_capable_var;     /* Indicates that machine is capable of using
  76                                    HW watchpoints/breakpoints */
  77
  78 static uint32_t dbg_model;      /* Debug Arch. Model */
  79 static boolean_t dbg_ossr;      /* OS Save and Restore implemented */
  80
  81 static uint32_t dbg_watchpoint_num;
  82 static uint32_t dbg_breakpoint_num;
  83
  84 /* ID_DFR0 - Debug Feature Register 0 */
  85 #define ID_DFR0_CP_DEBUG_M_SHIFT        0
  86 #define ID_DFR0_CP_DEBUG_M_MASK         (0xF << ID_DFR0_CP_DEBUG_M_SHIFT)
  87 #define ID_DFR0_CP_DEBUG_M_NS           (0x0) /* Not supported */
  88 #define ID_DFR0_CP_DEBUG_M_V6           (0x2) /* v6 Debug arch. CP14 access */
  89 #define ID_DFR0_CP_DEBUG_M_V6_1         (0x3) /* v6.1 Debug arch. CP14 access */
  90 #define ID_DFR0_CP_DEBUG_M_V7           (0x4) /* v7 Debug arch. CP14 access */
  91 #define ID_DFR0_CP_DEBUG_M_V7_1         (0x5) /* v7.1 Debug arch. CP14 access */
  92
  93 /* DBGDIDR - Debug ID Register */
  94 #define DBGDIDR_WRPS_SHIFT              28
  95 #define DBGDIDR_WRPS_MASK               (0xF << DBGDIDR_WRPS_SHIFT)
  96 #define DBGDIDR_WRPS_NUM(reg)           \
  97     ((((reg) & DBGDIDR_WRPS_MASK) >> DBGDIDR_WRPS_SHIFT) + 1)
  98
  99 #define DBGDIDR_BRPS_SHIFT              24
 100 #define DBGDIDR_BRPS_MASK               (0xF << DBGDIDR_BRPS_SHIFT)
 101 #define DBGDIDR_BRPS_NUM(reg)           \
 102     ((((reg) & DBGDIDR_BRPS_MASK) >> DBGDIDR_BRPS_SHIFT) + 1)
 103
 104 /* DBGPRSR - Device Powerdown and Reset Status Register */
 105 #define DBGPRSR_PU                      (1 << 0) /* Powerup status */
 106
 107 /* DBGOSLSR - OS Lock Status Register */
 108 #define DBGOSLSR_OSLM0                  (1 << 0)
 109
 110 /* DBGOSDLR - OS Double Lock Register */
 111 #define DBGPRSR_DLK                     (1 << 0) /* OS Double Lock set */
 112
 113 /* DBGDSCR - Debug Status and Control Register */
 114 #define DBGSCR_MDBG_EN                  (1 << 15) /* Monitor debug-mode enable */
 115
 116 /* DBGWVR - Watchpoint Value Register */
 117 #define DBGWVR_ADDR_MASK                (~0x3U)
 118
 119 /* Watchpoints/breakpoints control register bitfields */
 120 #define DBG_WB_CTRL_LEN_1               (0x1 << 5)
 121 #define DBG_WB_CTRL_LEN_2               (0x3 << 5)
 122 #define DBG_WB_CTRL_LEN_4               (0xf << 5)
 123 #define DBG_WB_CTRL_LEN_8               (0xff << 5)
 124 #define DBG_WB_CTRL_LEN_MASK(x) ((x) & (0xff << 5))
 125 #define DBG_WB_CTRL_EXEC                (0x0 << 3)
 126 #define DBG_WB_CTRL_LOAD                (0x1 << 3)
 127 #define DBG_WB_CTRL_STORE               (0x2 << 3)
 128 #define DBG_WB_CTRL_ACCESS_MASK(x)      ((x) & (0x3 << 3))
 129
 130 /* Common for breakpoint and watchpoint */
 131 #define DBG_WB_CTRL_PL1         (0x1 << 1)
 132 #define DBG_WB_CTRL_PL0         (0x2 << 1)
 133 #define DBG_WB_CTRL_PLX_MASK(x) ((x) & (0x3 << 1))
 134 #define DBG_WB_CTRL_E           (0x1 << 0)
 135
 136 /*
 137  * Watchpoint/breakpoint helpers
 138  */
 139 #define DBG_BKPT_BT_SLOT        0       /* Slot for branch taken */
 140 #define DBG_BKPT_BNT_SLOT       1       /* Slot for branch not taken */
 141
 142 #define OP2_SHIFT               4
 143
 144 /* Opc2 numbers for coprocessor instructions */
 145 #define DBG_WB_BVR      4
 146 #define DBG_WB_BCR      5
 147 #define DBG_WB_WVR      6
 148 #define DBG_WB_WCR      7
 149
 150 #define DBG_REG_BASE_BVR        (DBG_WB_BVR << OP2_SHIFT)
 151 #define DBG_REG_BASE_BCR        (DBG_WB_BCR << OP2_SHIFT)
 152 #define DBG_REG_BASE_WVR        (DBG_WB_WVR << OP2_SHIFT)
 153 #define DBG_REG_BASE_WCR        (DBG_WB_WCR << OP2_SHIFT)
 154
 155 #define DBG_WB_READ(cn, cm, op2, val) do {                                      \
 156         __asm __volatile("mrc p14, 0, %0, " #cn "," #cm "," #op2 : "=r" (val)); \
 157 } while (0)
 158
 159 #define DBG_WB_WRITE(cn, cm, op2, val) do {                                     \
 160         __asm __volatile("mcr p14, 0, %0, " #cn "," #cm "," #op2 :: "r" (val)); \
 161 } while (0)
 162
 163 #define READ_WB_REG_CASE(op2, m, val)                   \
 164         case (((op2) << OP2_SHIFT) + m):                \
 165                 DBG_WB_READ(c0, c ## m, op2, val);      \
 166                 break
 167
 168 #define WRITE_WB_REG_CASE(op2, m, val)                  \
 169         case (((op2) << OP2_SHIFT) + m):                \
 170                 DBG_WB_WRITE(c0, c ## m, op2, val);     \
 171                 break
 172
 173 #define SWITCH_CASES_READ_WB_REG(op2, val)      \
 174         READ_WB_REG_CASE(op2,  0, val);         \
 175         READ_WB_REG_CASE(op2,  1, val);         \
 176         READ_WB_REG_CASE(op2,  2, val);         \
 177         READ_WB_REG_CASE(op2,  3, val);         \
 178         READ_WB_REG_CASE(op2,  4, val);         \
 179         READ_WB_REG_CASE(op2,  5, val);         \
 180         READ_WB_REG_CASE(op2,  6, val);         \
 181         READ_WB_REG_CASE(op2,  7, val);         \
 182         READ_WB_REG_CASE(op2,  8, val);         \
 183         READ_WB_REG_CASE(op2,  9, val);         \
 184         READ_WB_REG_CASE(op2, 10, val);         \
 185         READ_WB_REG_CASE(op2, 11, val);         \
 186         READ_WB_REG_CASE(op2, 12, val);         \
 187         READ_WB_REG_CASE(op2, 13, val);         \
 188         READ_WB_REG_CASE(op2, 14, val);         \
 189         READ_WB_REG_CASE(op2, 15, val)
 190
 191 #define SWITCH_CASES_WRITE_WB_REG(op2, val)     \
 192         WRITE_WB_REG_CASE(op2,  0, val);        \
 193         WRITE_WB_REG_CASE(op2,  1, val);        \
 194         WRITE_WB_REG_CASE(op2,  2, val);        \
 195         WRITE_WB_REG_CASE(op2,  3, val);        \
 196         WRITE_WB_REG_CASE(op2,  4, val);        \
 197         WRITE_WB_REG_CASE(op2,  5, val);        \
 198         WRITE_WB_REG_CASE(op2,  6, val);        \
 199         WRITE_WB_REG_CASE(op2,  7, val);        \
 200         WRITE_WB_REG_CASE(op2,  8, val);        \
 201         WRITE_WB_REG_CASE(op2,  9, val);        \
 202         WRITE_WB_REG_CASE(op2, 10, val);        \
 203         WRITE_WB_REG_CASE(op2, 11, val);        \
 204         WRITE_WB_REG_CASE(op2, 12, val);        \
 205         WRITE_WB_REG_CASE(op2, 13, val);        \
 206         WRITE_WB_REG_CASE(op2, 14, val);        \
 207         WRITE_WB_REG_CASE(op2, 15, val)
 208
 209 static uint32_t
 210 dbg_wb_read_reg(int reg, int n)
 211 {
 212         uint32_t val;
 213
 214         val = 0;
 215
 216         switch (reg + n) {
 217         SWITCH_CASES_READ_WB_REG(DBG_WB_WVR, val);
 218         SWITCH_CASES_READ_WB_REG(DBG_WB_WCR, val);
 219         SWITCH_CASES_READ_WB_REG(DBG_WB_BVR, val);
 220         SWITCH_CASES_READ_WB_REG(DBG_WB_BCR, val);
 221         default:
 222                 db_printf(
 223                     "trying to read from CP14 reg. using wrong opc2 %d\n",
 224                     reg >> OP2_SHIFT);
 225         }
 226
 227         return (val);
 228 }
 229
 230 static void
 231 dbg_wb_write_reg(int reg, int n, uint32_t val)
 232 {
 233
 234         switch (reg + n) {
 235         SWITCH_CASES_WRITE_WB_REG(DBG_WB_WVR, val);
 236         SWITCH_CASES_WRITE_WB_REG(DBG_WB_WCR, val);
 237         SWITCH_CASES_WRITE_WB_REG(DBG_WB_BVR, val);
 238         SWITCH_CASES_WRITE_WB_REG(DBG_WB_BCR, val);
 239         default:
 240                 db_printf(
 241                     "trying to write to CP14 reg. using wrong opc2 %d\n",
 242                     reg >> OP2_SHIFT);
 243         }
 244         isb();
 245 }
 246
 247 static __inline boolean_t
 248 dbg_capable(void)
 249 {
 250
 251         return (atomic_cmpset_int(&dbg_capable_var, 0, 0) == 0);
 252 }
 253
 254 boolean_t
 255 kdb_cpu_pc_is_singlestep(db_addr_t pc)
 256 {
 257         /*
 258          * XXX: If the platform fails to enable its debug arch.
 259          *      there will be no stepping capabilities
 260          *      (SOFTWARE_SSTEP is not defined for __ARM_ARCH >= 6).
 261          */
 262         if (!dbg_capable())
 263                 return (FALSE);
 264
 265         if (dbg_find_slot(DBG_TYPE_BREAKPOINT, pc) != ~0U)
 266                 return (TRUE);
 267
 268         return (FALSE);
 269 }
 270
 271 void
 272 kdb_cpu_set_singlestep(void)
 273 {
 274         db_expr_t inst;
 275         db_addr_t pc, brpc;
 276         uint32_t wcr;
 277         u_int i;
 278
 279         if (!dbg_capable())
 280                 return;
 281
 282         /*
 283          * Disable watchpoints, e.g. stepping over watched instruction will
 284          * trigger break exception instead of single-step exception and locks
 285          * CPU on that instruction for ever.
 286          */
 287         for (i = 0; i < dbg_watchpoint_num; i++) {
 288                 wcr = dbg_wb_read_reg(DBG_REG_BASE_WCR, i);
 289                 if ((wcr & DBG_WB_CTRL_E) != 0) {
 290                         dbg_wb_write_reg(DBG_REG_BASE_WCR, i,
 291                             (wcr & ~DBG_WB_CTRL_E));
 292                 }
 293         }
 294
 295         pc = PC_REGS();
 296
 297         inst = db_get_value(pc, sizeof(pc), FALSE);
 298         if (inst_branch(inst) || inst_call(inst) || inst_return(inst)) {
 299                 brpc = branch_taken(inst, pc);
 300                 dbg_setup_breakpoint(brpc, INSN_SIZE, DBG_BKPT_BT_SLOT);
 301         }
 302         pc = next_instr_address(pc, 0);
 303         dbg_setup_breakpoint(pc, INSN_SIZE, DBG_BKPT_BNT_SLOT);
 304 }
 305
 306 void
 307 kdb_cpu_clear_singlestep(void)
 308 {
 309         uint32_t wvr, wcr;
 310         u_int i;
 311
 312         if (!dbg_capable())
 313                 return;
 314
 315         dbg_remove_breakpoint(DBG_BKPT_BT_SLOT);
 316         dbg_remove_breakpoint(DBG_BKPT_BNT_SLOT);
 317
 318         /* Restore all watchpoints */
 319         for (i = 0; i < dbg_watchpoint_num; i++) {
 320                 wcr = dbg_wb_read_reg(DBG_REG_BASE_WCR, i);
 321                 wvr = dbg_wb_read_reg(DBG_REG_BASE_WVR, i);
 322                 /* Watchpoint considered not empty if address value is not 0 */
 323                 if ((wvr & DBGWVR_ADDR_MASK) != 0) {
 324                         dbg_wb_write_reg(DBG_REG_BASE_WCR, i,
 325                             (wcr | DBG_WB_CTRL_E));
 326                 }
 327         }
 328 }
 329
 330 int
 331 dbg_setup_watchpoint(db_expr_t addr, db_expr_t size, enum dbg_access_t access)
 332 {
 333         struct dbg_wb_conf conf;
 334
 335         if (access == HW_BREAKPOINT_X) {
 336                 db_printf("Invalid access type for watchpoint: %d\n", access);
 337                 return (EINVAL);
 338         }
 339
 340         conf.address = addr;
 341         conf.size = size;
 342         conf.access = access;
 343         conf.type = DBG_TYPE_WATCHPOINT;
 344
 345         return (dbg_setup_xpoint(&conf));
 346 }
 347
 348 int
 349 dbg_remove_watchpoint(db_expr_t addr, db_expr_t size __unused)
 350 {
 351         struct dbg_wb_conf conf;
 352
 353         conf.address = addr;
 354         conf.type = DBG_TYPE_WATCHPOINT;
 355
 356         return (dbg_remove_xpoint(&conf));
 357 }
 358
 359 static int
 360 dbg_setup_breakpoint(db_expr_t addr, db_expr_t size, u_int slot)
 361 {
 362         struct dbg_wb_conf conf;
 363
 364         conf.address = addr;
 365         conf.size = size;
 366         conf.access = HW_BREAKPOINT_X;
 367         conf.type = DBG_TYPE_BREAKPOINT;
 368         conf.slot = slot;
 369
 370         return (dbg_setup_xpoint(&conf));
 371 }
 372
 373 static int
 374 dbg_remove_breakpoint(u_int slot)
 375 {
 376         struct dbg_wb_conf conf;
 377
 378         /* Slot already cleared. Don't recurse */
 379         if (dbg_check_slot_free(DBG_TYPE_BREAKPOINT, slot))
 380                 return (0);
 381
 382         conf.slot = slot;
 383         conf.type = DBG_TYPE_BREAKPOINT;
 384
 385         return (dbg_remove_xpoint(&conf));
 386 }
 387
 388 static const char *
 389 dbg_watchtype_str(uint32_t type)
 390 {
 391
 392         switch (type) {
 393                 case DBG_WB_CTRL_EXEC:
 394                         return ("execute");
 395                 case DBG_WB_CTRL_STORE:
 396                         return ("write");
 397                 case DBG_WB_CTRL_LOAD:
 398                         return ("read");
 399                 case DBG_WB_CTRL_LOAD | DBG_WB_CTRL_STORE:
 400                         return ("read/write");
 401                 default:
 402                         return ("invalid");
 403         }
 404 }
 405
 406 static int
 407 dbg_watchtype_len(uint32_t len)
 408 {
 409
 410         switch (len) {
 411         case DBG_WB_CTRL_LEN_1:
 412                 return (1);
 413         case DBG_WB_CTRL_LEN_2:
 414                 return (2);
 415         case DBG_WB_CTRL_LEN_4:
 416                 return (4);
 417         case DBG_WB_CTRL_LEN_8:
 418                 return (8);
 419         default:
 420                 return (0);
 421         }
 422 }
 423
 424 void
 425 dbg_show_watchpoint(void)
 426 {
 427         uint32_t wcr, len, type;
 428         uint32_t addr;
 429         boolean_t is_enabled;
 430         int i;
 431
 432         if (!dbg_capable()) {
 433                 db_printf("Architecture does not support HW "
 434                     "breakpoints/watchpoints\n");
 435                 return;
 436         }
 437
 438         db_printf("\nhardware watchpoints:\n");
 439         db_printf("  watch    status        type  len     address              symbol\n");
 440         db_printf("  -----  --------  ----------  ---  ----------  ------------------\n");
 441         for (i = 0; i < dbg_watchpoint_num; i++) {
 442                 wcr = dbg_wb_read_reg(DBG_REG_BASE_WCR, i);
 443                 if ((wcr & DBG_WB_CTRL_E) != 0)
 444                         is_enabled = TRUE;
 445                 else
 446                         is_enabled = FALSE;
 447
 448                 type = DBG_WB_CTRL_ACCESS_MASK(wcr);
 449                 len = DBG_WB_CTRL_LEN_MASK(wcr);
 450                 addr = dbg_wb_read_reg(DBG_REG_BASE_WVR, i) & DBGWVR_ADDR_MASK;
 451                 db_printf("  %-5d  %-8s  %10s  %3d  0x%08x  ", i,
 452                     is_enabled ? "enabled" : "disabled",
 453                     is_enabled ? dbg_watchtype_str(type) : "",
 454                     is_enabled ? dbg_watchtype_len(len) : 0,
 455                     addr);
 456                 db_printsym((db_addr_t)addr, DB_STGY_ANY);
 457                 db_printf("\n");
 458         }
 459 }
 460
 461 static boolean_t
 462 dbg_check_slot_free(enum dbg_t type, u_int slot)
 463 {
 464         uint32_t cr, vr;
 465         uint32_t max;
 466
 467         switch(type) {
 468         case DBG_TYPE_BREAKPOINT:
 469                 max = dbg_breakpoint_num;
 470                 cr = DBG_REG_BASE_BCR;
 471                 vr = DBG_REG_BASE_BVR;
 472                 break;
 473         case DBG_TYPE_WATCHPOINT:
 474                 max = dbg_watchpoint_num;
 475                 cr = DBG_REG_BASE_WCR;
 476                 vr = DBG_REG_BASE_WVR;
 477                 break;
 478         default:
 479                 db_printf("%s: Unsupported event type %d\n", __func__, type);
 480                 return (FALSE);
 481         }
 482
 483         if (slot >= max) {
 484                 db_printf("%s: Invalid slot number %d, max %d\n",
 485                     __func__, slot, max - 1);
 486                 return (FALSE);
 487         }
 488
 489         if ((dbg_wb_read_reg(cr, slot) & DBG_WB_CTRL_E) == 0 &&
 490             (dbg_wb_read_reg(vr, slot) & DBGWVR_ADDR_MASK) == 0)
 491                 return (TRUE);
 492
 493         return (FALSE);
 494 }
 495
 496 static u_int
 497 dbg_find_free_slot(enum dbg_t type)
 498 {
 499         u_int max, i;
 500
 501         switch(type) {
 502         case DBG_TYPE_BREAKPOINT:
 503                 max = dbg_breakpoint_num;
 504                 break;
 505         case DBG_TYPE_WATCHPOINT:
 506                 max = dbg_watchpoint_num;
 507                 break;
 508         default:
 509                 db_printf("Unsupported debug type\n");
 510                 return (~0U);
 511         }
 512
 513         for (i = 0; i < max; i++) {
 514                 if (dbg_check_slot_free(type, i))
 515                         return (i);
 516         }
 517
 518         return (~0U);
 519 }
 520
 521 static u_int
 522 dbg_find_slot(enum dbg_t type, db_expr_t addr)
 523 {
 524         uint32_t reg_addr, reg_ctrl;
 525         u_int max, i;
 526
 527         switch(type) {
 528         case DBG_TYPE_BREAKPOINT:
 529                 max = dbg_breakpoint_num;
 530                 reg_addr = DBG_REG_BASE_BVR;
 531                 reg_ctrl = DBG_REG_BASE_BCR;
 532                 break;
 533         case DBG_TYPE_WATCHPOINT:
 534                 max = dbg_watchpoint_num;
 535                 reg_addr = DBG_REG_BASE_WVR;
 536                 reg_ctrl = DBG_REG_BASE_WCR;
 537                 break;
 538         default:
 539                 db_printf("Unsupported debug type\n");
 540                 return (~0U);
 541         }
 542
 543         for (i = 0; i < max; i++) {
 544                 if ((dbg_wb_read_reg(reg_addr, i) == addr) &&
 545                     ((dbg_wb_read_reg(reg_ctrl, i) & DBG_WB_CTRL_E) != 0))
 546                         return (i);
 547         }
 548
 549         return (~0U);
 550 }
 551
 552 static __inline boolean_t
 553 dbg_monitor_is_enabled(void)
 554 {
 555
 556         return ((cp14_dbgdscrint_get() & DBGSCR_MDBG_EN) != 0);
 557 }
 558
 559 static int
 560 dbg_enable_monitor(void)
 561 {
 562         uint32_t dbg_dscr;
 563
 564         /* Already enabled? Just return */
 565         if (dbg_monitor_is_enabled())
 566                 return (0);
 567
 568         dbg_dscr = cp14_dbgdscrint_get();
 569
 570         switch (dbg_model) {
 571         case ID_DFR0_CP_DEBUG_M_V6:
 572         case ID_DFR0_CP_DEBUG_M_V6_1: /* fall through */
 573                 cp14_dbgdscr_v6_set(dbg_dscr | DBGSCR_MDBG_EN);
 574                 break;
 575         case ID_DFR0_CP_DEBUG_M_V7: /* fall through */
 576         case ID_DFR0_CP_DEBUG_M_V7_1:
 577                 cp14_dbgdscr_v7_set(dbg_dscr | DBGSCR_MDBG_EN);
 578                 break;
 579         default:
 580                 break;
 581         }
 582         isb();
 583
 584         /* Verify that Monitor mode is set */
 585         if (dbg_monitor_is_enabled())
 586                 return (0);
 587
 588         return (ENXIO);
 589 }
 590
 591 static int
 592 dbg_setup_xpoint(struct dbg_wb_conf *conf)
 593 {
 594         struct pcpu *pcpu;
 595         struct dbreg *d;
 596         const char *typestr;
 597         uint32_t cr_size, cr_priv, cr_access;
 598         uint32_t reg_ctrl, reg_addr, ctrl, addr;
 599         boolean_t is_bkpt;
 600         u_int cpu;
 601         u_int i;
 602
 603         if (!dbg_capable())
 604                 return (ENXIO);
 605
 606         is_bkpt = (conf->type == DBG_TYPE_BREAKPOINT);
 607         typestr = is_bkpt ? "breakpoint" : "watchpoint";
 608
 609         if (is_bkpt) {
 610                 if (dbg_breakpoint_num == 0) {
 611                         db_printf("Breakpoints not supported on this architecture\n");
 612                         return (ENXIO);
 613                 }
 614                 i = conf->slot;
 615                 if (!dbg_check_slot_free(DBG_TYPE_BREAKPOINT, i)) {
 616                         /*
 617                          * This should never happen. If it does it means that
 618                          * there is an erroneus scenario somewhere. Still, it can
 619                          * be done but let's inform the user.
 620                          */
 621                         db_printf("ERROR: Breakpoint already set. Replacing...\n");
 622                 }
 623         } else {
 624                 i = dbg_find_free_slot(DBG_TYPE_WATCHPOINT);
 625                 if (i == ~0U) {
 626                         db_printf("Can not find slot for %s, max %d slots supported\n",
 627                             typestr, dbg_watchpoint_num);
 628                         return (ENXIO);
 629                 }
 630         }
 631
 632         /* Kernel access only */
 633         cr_priv = DBG_WB_CTRL_PL1;
 634
 635         switch(conf->size) {
 636         case 1:
 637                 cr_size = DBG_WB_CTRL_LEN_1;
 638                 break;
 639         case 2:
 640                 cr_size = DBG_WB_CTRL_LEN_2;
 641                 break;
 642         case 4:
 643                 cr_size = DBG_WB_CTRL_LEN_4;
 644                 break;
 645         case 8:
 646                 cr_size = DBG_WB_CTRL_LEN_8;
 647                 break;
 648         default:
 649                 db_printf("Unsupported address size for %s\n", typestr);
 650                 return (EINVAL);
 651         }
 652
 653         if (is_bkpt) {
 654                 cr_access = DBG_WB_CTRL_EXEC;
 655                 reg_ctrl = DBG_REG_BASE_BCR;
 656                 reg_addr = DBG_REG_BASE_BVR;
 657                 /* Always unlinked BKPT */
 658                 ctrl = (cr_size | cr_access | cr_priv | DBG_WB_CTRL_E);
 659         } else {
 660                 switch(conf->access) {
 661                 case HW_WATCHPOINT_R:
 662                         cr_access = DBG_WB_CTRL_LOAD;
 663                         break;
 664                 case HW_WATCHPOINT_W:
 665                         cr_access = DBG_WB_CTRL_STORE;
 666                         break;
 667                 case HW_WATCHPOINT_RW:
 668                         cr_access = DBG_WB_CTRL_LOAD | DBG_WB_CTRL_STORE;
 669                         break;
 670                 default:
 671                         db_printf("Unsupported exception level for %s\n", typestr);
 672                         return (EINVAL);
 673                 }
 674
 675                 reg_ctrl = DBG_REG_BASE_WCR;
 676                 reg_addr = DBG_REG_BASE_WVR;
 677                 ctrl = (cr_size | cr_access | cr_priv | DBG_WB_CTRL_E);
 678         }
 679
 680         addr = conf->address;
 681
 682         dbg_wb_write_reg(reg_addr, i, addr);
 683         dbg_wb_write_reg(reg_ctrl, i, ctrl);
 684
 685         /*
 686          * Save watchpoint settings for all CPUs.
 687          * We don't need to do the same with breakpoints since HW breakpoints
 688          * are only used to perform single stepping.
 689          */
 690         if (!is_bkpt) {
 691                 CPU_FOREACH(cpu) {
 692                         pcpu = pcpu_find(cpu);
 693                         /* Fill out the settings for watchpoint */
 694                         d = (struct dbreg *)pcpu->pc_dbreg;
 695                         d->dbg_wvr[i] = addr;
 696                         d->dbg_wcr[i] = ctrl;
 697                         /* Skip update command for the current CPU */
 698                         if (cpu != PCPU_GET(cpuid))
 699                                 pcpu->pc_dbreg_cmd = PC_DBREG_CMD_LOAD;
 700                 }
 701         }
 702         /* Ensure all data is written before waking other CPUs */
 703         atomic_thread_fence_rel();
 704
 705         return (0);
 706 }
 707
 708 static int
 709 dbg_remove_xpoint(struct dbg_wb_conf *conf)
 710 {
 711         struct pcpu *pcpu;
 712         struct dbreg *d;
 713         uint32_t reg_ctrl, reg_addr, addr;
 714         boolean_t is_bkpt;
 715         u_int cpu;
 716         u_int i;
 717
 718         if (!dbg_capable())
 719                 return (ENXIO);
 720
 721         is_bkpt = (conf->type == DBG_TYPE_BREAKPOINT);
 722         addr = conf->address;
 723
 724         if (is_bkpt) {
 725                 i = conf->slot;
 726                 reg_ctrl = DBG_REG_BASE_BCR;
 727                 reg_addr = DBG_REG_BASE_BVR;
 728         } else {
 729                 i = dbg_find_slot(DBG_TYPE_WATCHPOINT, addr);
 730                 if (i == ~0U) {
 731                         db_printf("Can not find watchpoint for address 0%x\n", addr);
 732                         return (EINVAL);
 733                 }
 734                 reg_ctrl = DBG_REG_BASE_WCR;
 735                 reg_addr = DBG_REG_BASE_WVR;
 736         }
 737
 738         dbg_wb_write_reg(reg_ctrl, i, 0);
 739         dbg_wb_write_reg(reg_addr, i, 0);
 740
 741         /*
 742          * Save watchpoint settings for all CPUs.
 743          * We don't need to do the same with breakpoints since HW breakpoints
 744          * are only used to perform single stepping.
 745          */
 746         if (!is_bkpt) {
 747                 CPU_FOREACH(cpu) {
 748                         pcpu = pcpu_find(cpu);
 749                         /* Fill out the settings for watchpoint */
 750                         d = (struct dbreg *)pcpu->pc_dbreg;
 751                         d->dbg_wvr[i] = 0;
 752                         d->dbg_wcr[i] = 0;
 753                         /* Skip update command for the current CPU */
 754                         if (cpu != PCPU_GET(cpuid))
 755                                 pcpu->pc_dbreg_cmd = PC_DBREG_CMD_LOAD;
 756                 }
 757                 /* Ensure all data is written before waking other CPUs */
 758                 atomic_thread_fence_rel();
 759         }
 760
 761         return (0);
 762 }
 763
 764 static __inline uint32_t
 765 dbg_get_debug_model(void)
 766 {
 767         uint32_t dbg_m;
 768
 769         dbg_m = ((cpuinfo.id_dfr0 & ID_DFR0_CP_DEBUG_M_MASK) >>
 770             ID_DFR0_CP_DEBUG_M_SHIFT);
 771
 772         return (dbg_m);
 773 }
 774
 775 static __inline boolean_t
 776 dbg_get_ossr(void)
 777 {
 778
 779         switch (dbg_model) {
 780         case ID_DFR0_CP_DEBUG_M_V7:
 781                 if ((cp14_dbgoslsr_get() & DBGOSLSR_OSLM0) != 0)
 782                         return (TRUE);
 783
 784                 return (FALSE);
 785         case ID_DFR0_CP_DEBUG_M_V7_1:
 786                 return (TRUE);
 787         default:
 788                 return (FALSE);
 789         }
 790 }
 791
 792 static __inline boolean_t
 793 dbg_arch_supported(void)
 794 {
 795         uint32_t dbg_didr;
 796
 797         switch (dbg_model) {
 798         case ID_DFR0_CP_DEBUG_M_V6:
 799         case ID_DFR0_CP_DEBUG_M_V6_1:
 800                 dbg_didr = cp14_dbgdidr_get();
 801                 /*
 802                  * read-all-zeroes is used by QEMU
 803                  * to indicate that ARMv6 debug support
 804                  * is not implemented. Real hardware has at
 805                  * least version bits set
 806                  */
 807                 if (dbg_didr == 0)
 808                         return (FALSE);
 809                 return (TRUE);
 810         case ID_DFR0_CP_DEBUG_M_V7:
 811         case ID_DFR0_CP_DEBUG_M_V7_1:   /* fall through */
 812                 return (TRUE);
 813         default:
 814                 /* We only support valid v6.x/v7.x modes through CP14 */
 815                 return (FALSE);
 816         }
 817 }
 818
 819 static __inline uint32_t
 820 dbg_get_wrp_num(void)
 821 {
 822         uint32_t dbg_didr;
 823
 824         dbg_didr = cp14_dbgdidr_get();
 825
 826         return (DBGDIDR_WRPS_NUM(dbg_didr));
 827 }
 828
 829 static __inline uint32_t
 830 dgb_get_brp_num(void)
 831 {
 832         uint32_t dbg_didr;
 833
 834         dbg_didr = cp14_dbgdidr_get();
 835
 836         return (DBGDIDR_BRPS_NUM(dbg_didr));
 837 }
 838
 839 static int
 840 dbg_reset_state(void)
 841 {
 842         u_int cpuid;
 843         size_t i;
 844         int err;
 845
 846         cpuid = PCPU_GET(cpuid);
 847         err = 0;
 848
 849         switch (dbg_model) {
 850         case ID_DFR0_CP_DEBUG_M_V6:
 851         case ID_DFR0_CP_DEBUG_M_V6_1: /* fall through */
 852                 /*
 853                  * Arch needs monitor mode selected and enabled
 854                  * to be able to access breakpoint/watchpoint registers.
 855                  */
 856                 err = dbg_enable_monitor();
 857                 if (err != 0)
 858                         return (err);
 859                 goto vectr_clr;
 860         case ID_DFR0_CP_DEBUG_M_V7:
 861                 /* Is core power domain powered up? */
 862                 if ((cp14_dbgprsr_get() & DBGPRSR_PU) == 0)
 863                         err = ENXIO;
 864
 865                 if (err != 0)
 866                         break;
 867
 868                 if (dbg_ossr)
 869                         goto vectr_clr;
 870                 break;
 871         case ID_DFR0_CP_DEBUG_M_V7_1:
 872                 /* Is double lock set? */
 873                 if ((cp14_dbgosdlr_get() & DBGPRSR_DLK) != 0)
 874                         err = ENXIO;
 875
 876                 break;
 877         default:
 878                 break;
 879         }
 880
 881         if (err != 0) {
 882                 db_printf("Debug facility locked (CPU%d)\n", cpuid);
 883                 return (err);
 884         }
 885
 886         /*
 887          * DBGOSLAR is always implemented for v7.1 Debug Arch. however is
 888          * optional for v7 (depends on OS save and restore support).
 889          */
 890         if (((dbg_model & ID_DFR0_CP_DEBUG_M_V7_1) != 0) || dbg_ossr) {
 891                 /*
 892                  * Clear OS lock.
 893                  * Writing any other value than 0xC5ACCESS will unlock.
 894                  */
 895                 cp14_dbgoslar_set(0);
 896                 isb();
 897         }
 898
 899 vectr_clr:
 900         /*
 901          * After reset we must ensure that DBGVCR has a defined value.
 902          * Disable all vector catch events. Safe to use - required in all
 903          * implementations.
 904          */
 905         cp14_dbgvcr_set(0);
 906         isb();
 907
 908         /*
 909          * We have limited number of {watch,break}points, each consists of
 910          * two registers:
 911          * - wcr/bcr regsiter configurates corresponding {watch,break}point
 912          *   behaviour
 913          * - wvr/bvr register keeps address we are hunting for
 914          *
 915          * Reset all breakpoints and watchpoints.
 916          */
 917         for (i = 0; i < dbg_watchpoint_num; ++i) {
 918                 dbg_wb_write_reg(DBG_REG_BASE_WCR, i, 0);
 919                 dbg_wb_write_reg(DBG_REG_BASE_WVR, i, 0);
 920         }
 921
 922         for (i = 0; i < dbg_breakpoint_num; ++i) {
 923                 dbg_wb_write_reg(DBG_REG_BASE_BCR, i, 0);
 924                 dbg_wb_write_reg(DBG_REG_BASE_BVR, i, 0);
 925         }
 926
 927         return (0);
 928 }
 929
 930 void
 931 dbg_monitor_init(void)
 932 {
 933         int err;
 934
 935         /* Fetch ARM Debug Architecture model */
 936         dbg_model = dbg_get_debug_model();
 937
 938         if (!dbg_arch_supported()) {
 939                 db_printf("ARM Debug Architecture not supported\n");
 940                 return;
 941         }
 942
 943         if (bootverbose) {
 944                 db_printf("ARM Debug Architecture %s\n",
 945                     (dbg_model == ID_DFR0_CP_DEBUG_M_V6) ? "v6" :
 946                     (dbg_model == ID_DFR0_CP_DEBUG_M_V6_1) ? "v6.1" :
 947                     (dbg_model == ID_DFR0_CP_DEBUG_M_V7) ? "v7" :
 948                     (dbg_model == ID_DFR0_CP_DEBUG_M_V7_1) ? "v7.1" : "unknown");
 949         }
 950
 951         /* Do we have OS Save and Restore mechanism? */
 952         dbg_ossr = dbg_get_ossr();
 953
 954         /* Find out many breakpoints and watchpoints we can use */
 955         dbg_watchpoint_num = dbg_get_wrp_num();
 956         dbg_breakpoint_num = dgb_get_brp_num();
 957
 958         if (bootverbose) {
 959                 db_printf("%d watchpoints and %d breakpoints supported\n",
 960                     dbg_watchpoint_num, dbg_breakpoint_num);
 961         }
 962
 963         err = dbg_reset_state();
 964         if (err == 0) {
 965                 err = dbg_enable_monitor();
 966                 if (err == 0) {
 967                         atomic_set_int(&dbg_capable_var, 1);
 968                         return;
 969                 }
 970         }
 971
 972         db_printf("HW Breakpoints/Watchpoints not enabled on CPU%d\n",
 973             PCPU_GET(cpuid));
 974 }
 975
 976 CTASSERT(sizeof(struct dbreg) == sizeof(((struct pcpu *)NULL)->pc_dbreg));
 977
 978 void
 979 dbg_monitor_init_secondary(void)
 980 {
 981         u_int cpuid;
 982         int err;
 983         /*
 984          * This flag is set on the primary CPU
 985          * and its meaning is valid for other CPUs too.
 986          */
 987         if (!dbg_capable())
 988                 return;
 989
 990         cpuid = PCPU_GET(cpuid);
 991
 992         err = dbg_reset_state();
 993         if (err != 0) {
 994                 /*
 995                  * Something is very wrong.
 996                  * WPs/BPs will not work correctly on this CPU.
 997                  */
 998                 KASSERT(0, ("%s: Failed to reset Debug Architecture "
 999                     "state on CPU%d", __func__, cpuid));
1000                 /* Disable HW debug capabilities for all CPUs */
1001                 atomic_set_int(&dbg_capable_var, 0);
1002                 return;
1003         }
1004         err = dbg_enable_monitor();
1005         if (err != 0) {
1006                 KASSERT(0, ("%s: Failed to enable Debug Monitor"
1007                     " on CPU%d", __func__, cpuid));
1008                 atomic_set_int(&dbg_capable_var, 0);
1009         }
1010 }
1011
1012 void
1013 dbg_resume_dbreg(void)
1014 {
1015         struct dbreg *d;
1016         u_int i;
1017
1018         /*
1019          * This flag is set on the primary CPU
1020          * and its meaning is valid for other CPUs too.
1021          */
1022         if (!dbg_capable())
1023                 return;
1024
1025         atomic_thread_fence_acq();
1026
1027         switch (PCPU_GET(dbreg_cmd)) {
1028         case PC_DBREG_CMD_LOAD:
1029                 d = (struct dbreg *)PCPU_PTR(dbreg);
1030
1031                 /* Restore watchpoints */
1032                 for (i = 0; i < dbg_watchpoint_num; i++) {
1033                         dbg_wb_write_reg(DBG_REG_BASE_WVR, i, d->dbg_wvr[i]);
1034                         dbg_wb_write_reg(DBG_REG_BASE_WCR, i, d->dbg_wcr[i]);
1035                 }
1036
1037                 PCPU_SET(dbreg_cmd, PC_DBREG_CMD_NONE);
1038                 break;
1039         }
1040 }