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