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          */
 261         if (!dbg_capable())
 262                 return (FALSE);
 263
 264         if (dbg_find_slot(DBG_TYPE_BREAKPOINT, pc) != ~0U)
 265                 return (TRUE);
 266
 267         return (FALSE);
 268 }
 269
 270 void
 271 kdb_cpu_set_singlestep(void)
 272 {
 273         db_expr_t inst;
 274         db_addr_t pc, brpc;
 275         uint32_t wcr;
 276         u_int i;
 277
 278         if (!dbg_capable())
 279                 return;
 280
 281         /*
 282          * Disable watchpoints, e.g. stepping over watched instruction will
 283          * trigger break exception instead of single-step exception and locks
 284          * CPU on that instruction for ever.
 285          */
 286         for (i = 0; i < dbg_watchpoint_num; i++) {
 287                 wcr = dbg_wb_read_reg(DBG_REG_BASE_WCR, i);
 288                 if ((wcr & DBG_WB_CTRL_E) != 0) {
 289                         dbg_wb_write_reg(DBG_REG_BASE_WCR, i,
 290                             (wcr & ~DBG_WB_CTRL_E));
 291                 }
 292         }
 293
 294         pc = PC_REGS();
 295
 296         inst = db_get_value(pc, sizeof(pc), FALSE);
 297         if (inst_branch(inst) || inst_call(inst) || inst_return(inst)) {
 298                 brpc = branch_taken(inst, pc);
 299                 dbg_setup_breakpoint(brpc, INSN_SIZE, DBG_BKPT_BT_SLOT);
 300         }
 301         pc = next_instr_address(pc, 0);
 302         dbg_setup_breakpoint(pc, INSN_SIZE, DBG_BKPT_BNT_SLOT);
 303 }
 304
 305 void
 306 kdb_cpu_clear_singlestep(void)
 307 {
 308         uint32_t wvr, wcr;
 309         u_int i;
 310
 311         if (!dbg_capable())
 312                 return;
 313
 314         dbg_remove_breakpoint(DBG_BKPT_BT_SLOT);
 315         dbg_remove_breakpoint(DBG_BKPT_BNT_SLOT);
 316
 317         /* Restore all watchpoints */
 318         for (i = 0; i < dbg_watchpoint_num; i++) {
 319                 wcr = dbg_wb_read_reg(DBG_REG_BASE_WCR, i);
 320                 wvr = dbg_wb_read_reg(DBG_REG_BASE_WVR, i);
 321                 /* Watchpoint considered not empty if address value is not 0 */
 322                 if ((wvr & DBGWVR_ADDR_MASK) != 0) {
 323                         dbg_wb_write_reg(DBG_REG_BASE_WCR, i,
 324                             (wcr | DBG_WB_CTRL_E));
 325                 }
 326         }
 327 }
 328
 329 int
 330 kdb_cpu_set_watchpoint(vm_offset_t addr, size_t size, int access)
 331 {
 332         enum dbg_access_t dbg_access;
 333
 334         switch (access) {
 335         case KDB_DBG_ACCESS_R:
 336                 dbg_access = HW_WATCHPOINT_R;
 337                 break;
 338         case KDB_DBG_ACCESS_W:
 339                 dbg_access = HW_WATCHPOINT_W;
 340                 break;
 341         case KDB_DBG_ACCESS_RW:
 342                 dbg_access = HW_WATCHPOINT_RW;
 343                 break;
 344         default:
 345                 return (EINVAL);
 346         }
 347
 348         return (dbg_setup_watchpoint(addr, size, (enum dbg_access_t)access));
 349 }
 350
 351 int
 352 kdb_cpu_clr_watchpoint(vm_offset_t addr, size_t size)
 353 {
 354
 355         return (dbg_remove_watchpoint(addr, size));
 356 }
 357
 358 int
 359 dbg_setup_watchpoint(db_expr_t addr, db_expr_t size, enum dbg_access_t access)
 360 {
 361         struct dbg_wb_conf conf;
 362
 363         if (access == HW_BREAKPOINT_X) {
 364                 db_printf("Invalid access type for watchpoint: %d\n", access);
 365                 return (EINVAL);
 366         }
 367
 368         conf.address = addr;
 369         conf.size = size;
 370         conf.access = access;
 371         conf.type = DBG_TYPE_WATCHPOINT;
 372
 373         return (dbg_setup_xpoint(&conf));
 374 }
 375
 376 int
 377 dbg_remove_watchpoint(db_expr_t addr, db_expr_t size __unused)
 378 {
 379         struct dbg_wb_conf conf;
 380
 381         conf.address = addr;
 382         conf.type = DBG_TYPE_WATCHPOINT;
 383
 384         return (dbg_remove_xpoint(&conf));
 385 }
 386
 387 static int
 388 dbg_setup_breakpoint(db_expr_t addr, db_expr_t size, u_int slot)
 389 {
 390         struct dbg_wb_conf conf;
 391
 392         conf.address = addr;
 393         conf.size = size;
 394         conf.access = HW_BREAKPOINT_X;
 395         conf.type = DBG_TYPE_BREAKPOINT;
 396         conf.slot = slot;
 397
 398         return (dbg_setup_xpoint(&conf));
 399 }
 400
 401 static int
 402 dbg_remove_breakpoint(u_int slot)
 403 {
 404         struct dbg_wb_conf conf;
 405
 406         /* Slot already cleared. Don't recurse */
 407         if (dbg_check_slot_free(DBG_TYPE_BREAKPOINT, slot))
 408                 return (0);
 409
 410         conf.slot = slot;
 411         conf.type = DBG_TYPE_BREAKPOINT;
 412
 413         return (dbg_remove_xpoint(&conf));
 414 }
 415
 416 static const char *
 417 dbg_watchtype_str(uint32_t type)
 418 {
 419
 420         switch (type) {
 421                 case DBG_WB_CTRL_EXEC:
 422                         return ("execute");
 423                 case DBG_WB_CTRL_STORE:
 424                         return ("write");
 425                 case DBG_WB_CTRL_LOAD:
 426                         return ("read");
 427                 case DBG_WB_CTRL_LOAD | DBG_WB_CTRL_STORE:
 428                         return ("read/write");
 429                 default:
 430                         return ("invalid");
 431         }
 432 }
 433
 434 static int
 435 dbg_watchtype_len(uint32_t len)
 436 {
 437
 438         switch (len) {
 439         case DBG_WB_CTRL_LEN_1:
 440                 return (1);
 441         case DBG_WB_CTRL_LEN_2:
 442                 return (2);
 443         case DBG_WB_CTRL_LEN_4:
 444                 return (4);
 445         case DBG_WB_CTRL_LEN_8:
 446                 return (8);
 447         default:
 448                 return (0);
 449         }
 450 }
 451
 452 void
 453 dbg_show_watchpoint(void)
 454 {
 455         uint32_t wcr, len, type;
 456         uint32_t addr;
 457         boolean_t is_enabled;
 458         int i;
 459
 460         if (!dbg_capable()) {
 461                 db_printf("Architecture does not support HW "
 462                     "breakpoints/watchpoints\n");
 463                 return;
 464         }
 465
 466         db_printf("\nhardware watchpoints:\n");
 467         db_printf("  watch    status        type  len     address              symbol\n");
 468         db_printf("  -----  --------  ----------  ---  ----------  ------------------\n");
 469         for (i = 0; i < dbg_watchpoint_num; i++) {
 470                 wcr = dbg_wb_read_reg(DBG_REG_BASE_WCR, i);
 471                 if ((wcr & DBG_WB_CTRL_E) != 0)
 472                         is_enabled = TRUE;
 473                 else
 474                         is_enabled = FALSE;
 475
 476                 type = DBG_WB_CTRL_ACCESS_MASK(wcr);
 477                 len = DBG_WB_CTRL_LEN_MASK(wcr);
 478                 addr = dbg_wb_read_reg(DBG_REG_BASE_WVR, i) & DBGWVR_ADDR_MASK;
 479                 db_printf("  %-5d  %-8s  %10s  %3d  0x%08x  ", i,
 480                     is_enabled ? "enabled" : "disabled",
 481                     is_enabled ? dbg_watchtype_str(type) : "",
 482                     is_enabled ? dbg_watchtype_len(len) : 0,
 483                     addr);
 484                 db_printsym((db_addr_t)addr, DB_STGY_ANY);
 485                 db_printf("\n");
 486         }
 487 }
 488
 489 static boolean_t
 490 dbg_check_slot_free(enum dbg_t type, u_int slot)
 491 {
 492         uint32_t cr, vr;
 493         uint32_t max;
 494
 495         switch(type) {
 496         case DBG_TYPE_BREAKPOINT:
 497                 max = dbg_breakpoint_num;
 498                 cr = DBG_REG_BASE_BCR;
 499                 vr = DBG_REG_BASE_BVR;
 500                 break;
 501         case DBG_TYPE_WATCHPOINT:
 502                 max = dbg_watchpoint_num;
 503                 cr = DBG_REG_BASE_WCR;
 504                 vr = DBG_REG_BASE_WVR;
 505                 break;
 506         default:
 507                 db_printf("%s: Unsupported event type %d\n", __func__, type);
 508                 return (FALSE);
 509         }
 510
 511         if (slot >= max) {
 512                 db_printf("%s: Invalid slot number %d, max %d\n",
 513                     __func__, slot, max - 1);
 514                 return (FALSE);
 515         }
 516
 517         if ((dbg_wb_read_reg(cr, slot) & DBG_WB_CTRL_E) == 0 &&
 518             (dbg_wb_read_reg(vr, slot) & DBGWVR_ADDR_MASK) == 0)
 519                 return (TRUE);
 520
 521         return (FALSE);
 522 }
 523
 524 static u_int
 525 dbg_find_free_slot(enum dbg_t type)
 526 {
 527         u_int max, i;
 528
 529         switch(type) {
 530         case DBG_TYPE_BREAKPOINT:
 531                 max = dbg_breakpoint_num;
 532                 break;
 533         case DBG_TYPE_WATCHPOINT:
 534                 max = dbg_watchpoint_num;
 535                 break;
 536         default:
 537                 db_printf("Unsupported debug type\n");
 538                 return (~0U);
 539         }
 540
 541         for (i = 0; i < max; i++) {
 542                 if (dbg_check_slot_free(type, i))
 543                         return (i);
 544         }
 545
 546         return (~0U);
 547 }
 548
 549 static u_int
 550 dbg_find_slot(enum dbg_t type, db_expr_t addr)
 551 {
 552         uint32_t reg_addr, reg_ctrl;
 553         u_int max, i;
 554
 555         switch(type) {
 556         case DBG_TYPE_BREAKPOINT:
 557                 max = dbg_breakpoint_num;
 558                 reg_addr = DBG_REG_BASE_BVR;
 559                 reg_ctrl = DBG_REG_BASE_BCR;
 560                 break;
 561         case DBG_TYPE_WATCHPOINT:
 562                 max = dbg_watchpoint_num;
 563                 reg_addr = DBG_REG_BASE_WVR;
 564                 reg_ctrl = DBG_REG_BASE_WCR;
 565                 break;
 566         default:
 567                 db_printf("Unsupported debug type\n");
 568                 return (~0U);
 569         }
 570
 571         for (i = 0; i < max; i++) {
 572                 if ((dbg_wb_read_reg(reg_addr, i) == addr) &&
 573                     ((dbg_wb_read_reg(reg_ctrl, i) & DBG_WB_CTRL_E) != 0))
 574                         return (i);
 575         }
 576
 577         return (~0U);
 578 }
 579
 580 static __inline boolean_t
 581 dbg_monitor_is_enabled(void)
 582 {
 583
 584         return ((cp14_dbgdscrint_get() & DBGSCR_MDBG_EN) != 0);
 585 }
 586
 587 static int
 588 dbg_enable_monitor(void)
 589 {
 590         uint32_t dbg_dscr;
 591
 592         /* Already enabled? Just return */
 593         if (dbg_monitor_is_enabled())
 594                 return (0);
 595
 596         dbg_dscr = cp14_dbgdscrint_get();
 597
 598         switch (dbg_model) {
 599         case ID_DFR0_CP_DEBUG_M_V6:
 600         case ID_DFR0_CP_DEBUG_M_V6_1: /* fall through */
 601                 cp14_dbgdscr_v6_set(dbg_dscr | DBGSCR_MDBG_EN);
 602                 break;
 603         case ID_DFR0_CP_DEBUG_M_V7: /* fall through */
 604         case ID_DFR0_CP_DEBUG_M_V7_1:
 605                 cp14_dbgdscr_v7_set(dbg_dscr | DBGSCR_MDBG_EN);
 606                 break;
 607         default:
 608                 break;
 609         }
 610         isb();
 611
 612         /* Verify that Monitor mode is set */
 613         if (dbg_monitor_is_enabled())
 614                 return (0);
 615
 616         return (ENXIO);
 617 }
 618
 619 static int
 620 dbg_setup_xpoint(struct dbg_wb_conf *conf)
 621 {
 622         struct pcpu *pcpu;
 623         struct dbreg *d;
 624         const char *typestr;
 625         uint32_t cr_size, cr_priv, cr_access;
 626         uint32_t reg_ctrl, reg_addr, ctrl, addr;
 627         boolean_t is_bkpt;
 628         u_int cpu;
 629         u_int i;
 630
 631         if (!dbg_capable())
 632                 return (ENXIO);
 633
 634         is_bkpt = (conf->type == DBG_TYPE_BREAKPOINT);
 635         typestr = is_bkpt ? "breakpoint" : "watchpoint";
 636
 637         if (is_bkpt) {
 638                 if (dbg_breakpoint_num == 0) {
 639                         db_printf("Breakpoints not supported on this architecture\n");
 640                         return (ENXIO);
 641                 }
 642                 i = conf->slot;
 643                 if (!dbg_check_slot_free(DBG_TYPE_BREAKPOINT, i)) {
 644                         /*
 645                          * This should never happen. If it does it means that
 646                          * there is an erroneus scenario somewhere. Still, it can
 647                          * be done but let's inform the user.
 648                          */
 649                         db_printf("ERROR: Breakpoint already set. Replacing...\n");
 650                 }
 651         } else {
 652                 i = dbg_find_free_slot(DBG_TYPE_WATCHPOINT);
 653                 if (i == ~0U) {
 654                         db_printf("Can not find slot for %s, max %d slots supported\n",
 655                             typestr, dbg_watchpoint_num);
 656                         return (EBUSY);
 657                 }
 658         }
 659
 660         /* Kernel access only */
 661         cr_priv = DBG_WB_CTRL_PL1;
 662
 663         switch(conf->size) {
 664         case 1:
 665                 cr_size = DBG_WB_CTRL_LEN_1;
 666                 break;
 667         case 2:
 668                 cr_size = DBG_WB_CTRL_LEN_2;
 669                 break;
 670         case 4:
 671                 cr_size = DBG_WB_CTRL_LEN_4;
 672                 break;
 673         case 8:
 674                 cr_size = DBG_WB_CTRL_LEN_8;
 675                 break;
 676         default:
 677                 db_printf("Unsupported address size for %s: %zu\n", typestr,
 678                     conf->size);
 679                 return (EINVAL);
 680         }
 681
 682         if (is_bkpt) {
 683                 cr_access = DBG_WB_CTRL_EXEC;
 684                 reg_ctrl = DBG_REG_BASE_BCR;
 685                 reg_addr = DBG_REG_BASE_BVR;
 686                 /* Always unlinked BKPT */
 687                 ctrl = (cr_size | cr_access | cr_priv | DBG_WB_CTRL_E);
 688         } else {
 689                 switch(conf->access) {
 690                 case HW_WATCHPOINT_R:
 691                         cr_access = DBG_WB_CTRL_LOAD;
 692                         break;
 693                 case HW_WATCHPOINT_W:
 694                         cr_access = DBG_WB_CTRL_STORE;
 695                         break;
 696                 case HW_WATCHPOINT_RW:
 697                         cr_access = DBG_WB_CTRL_LOAD | DBG_WB_CTRL_STORE;
 698                         break;
 699                 default:
 700                         db_printf("Unsupported access type for %s: %d\n",
 701                             typestr, conf->access);
 702                         return (EINVAL);
 703                 }
 704
 705                 reg_ctrl = DBG_REG_BASE_WCR;
 706                 reg_addr = DBG_REG_BASE_WVR;
 707                 ctrl = (cr_size | cr_access | cr_priv | DBG_WB_CTRL_E);
 708         }
 709
 710         addr = conf->address;
 711
 712         dbg_wb_write_reg(reg_addr, i, addr);
 713         dbg_wb_write_reg(reg_ctrl, i, ctrl);
 714
 715         /*
 716          * Save watchpoint settings for all CPUs.
 717          * We don't need to do the same with breakpoints since HW breakpoints
 718          * are only used to perform single stepping.
 719          */
 720         if (!is_bkpt) {
 721                 CPU_FOREACH(cpu) {
 722                         pcpu = pcpu_find(cpu);
 723                         /* Fill out the settings for watchpoint */
 724                         d = (struct dbreg *)pcpu->pc_dbreg;
 725                         d->dbg_wvr[i] = addr;
 726                         d->dbg_wcr[i] = ctrl;
 727                         /* Skip update command for the current CPU */
 728                         if (cpu != PCPU_GET(cpuid))
 729                                 pcpu->pc_dbreg_cmd = PC_DBREG_CMD_LOAD;
 730                 }
 731         }
 732         /* Ensure all data is written before waking other CPUs */
 733         atomic_thread_fence_rel();
 734
 735         return (0);
 736 }
 737
 738 static int
 739 dbg_remove_xpoint(struct dbg_wb_conf *conf)
 740 {
 741         struct pcpu *pcpu;
 742         struct dbreg *d;
 743         uint32_t reg_ctrl, reg_addr, addr;
 744         boolean_t is_bkpt;
 745         u_int cpu;
 746         u_int i;
 747
 748         if (!dbg_capable())
 749                 return (ENXIO);
 750
 751         is_bkpt = (conf->type == DBG_TYPE_BREAKPOINT);
 752         addr = conf->address;
 753
 754         if (is_bkpt) {
 755                 i = conf->slot;
 756                 reg_ctrl = DBG_REG_BASE_BCR;
 757                 reg_addr = DBG_REG_BASE_BVR;
 758         } else {
 759                 i = dbg_find_slot(DBG_TYPE_WATCHPOINT, addr);
 760                 if (i == ~0U) {
 761                         db_printf("Can not find watchpoint for address 0%x\n", addr);
 762                         return (EINVAL);
 763                 }
 764                 reg_ctrl = DBG_REG_BASE_WCR;
 765                 reg_addr = DBG_REG_BASE_WVR;
 766         }
 767
 768         dbg_wb_write_reg(reg_ctrl, i, 0);
 769         dbg_wb_write_reg(reg_addr, i, 0);
 770
 771         /*
 772          * Save watchpoint settings for all CPUs.
 773          * We don't need to do the same with breakpoints since HW breakpoints
 774          * are only used to perform single stepping.
 775          */
 776         if (!is_bkpt) {
 777                 CPU_FOREACH(cpu) {
 778                         pcpu = pcpu_find(cpu);
 779                         /* Fill out the settings for watchpoint */
 780                         d = (struct dbreg *)pcpu->pc_dbreg;
 781                         d->dbg_wvr[i] = 0;
 782                         d->dbg_wcr[i] = 0;
 783                         /* Skip update command for the current CPU */
 784                         if (cpu != PCPU_GET(cpuid))
 785                                 pcpu->pc_dbreg_cmd = PC_DBREG_CMD_LOAD;
 786                 }
 787                 /* Ensure all data is written before waking other CPUs */
 788                 atomic_thread_fence_rel();
 789         }
 790
 791         return (0);
 792 }
 793
 794 static __inline uint32_t
 795 dbg_get_debug_model(void)
 796 {
 797         uint32_t dbg_m;
 798
 799         dbg_m = ((cpuinfo.id_dfr0 & ID_DFR0_CP_DEBUG_M_MASK) >>
 800             ID_DFR0_CP_DEBUG_M_SHIFT);
 801
 802         return (dbg_m);
 803 }
 804
 805 static __inline boolean_t
 806 dbg_get_ossr(void)
 807 {
 808
 809         switch (dbg_model) {
 810         case ID_DFR0_CP_DEBUG_M_V7:
 811                 if ((cp14_dbgoslsr_get() & DBGOSLSR_OSLM0) != 0)
 812                         return (TRUE);
 813
 814                 return (FALSE);
 815         case ID_DFR0_CP_DEBUG_M_V7_1:
 816                 return (TRUE);
 817         default:
 818                 return (FALSE);
 819         }
 820 }
 821
 822 static __inline boolean_t
 823 dbg_arch_supported(void)
 824 {
 825         uint32_t dbg_didr;
 826
 827         switch (dbg_model) {
 828         case ID_DFR0_CP_DEBUG_M_V6:
 829         case ID_DFR0_CP_DEBUG_M_V6_1:
 830                 dbg_didr = cp14_dbgdidr_get();
 831                 /*
 832                  * read-all-zeroes is used by QEMU
 833                  * to indicate that ARMv6 debug support
 834                  * is not implemented. Real hardware has at
 835                  * least version bits set
 836                  */
 837                 if (dbg_didr == 0)
 838                         return (FALSE);
 839                 return (TRUE);
 840         case ID_DFR0_CP_DEBUG_M_V7:
 841         case ID_DFR0_CP_DEBUG_M_V7_1:   /* fall through */
 842                 return (TRUE);
 843         default:
 844                 /* We only support valid v6.x/v7.x modes through CP14 */
 845                 return (FALSE);
 846         }
 847 }
 848
 849 static __inline uint32_t
 850 dbg_get_wrp_num(void)
 851 {
 852         uint32_t dbg_didr;
 853
 854         dbg_didr = cp14_dbgdidr_get();
 855
 856         return (DBGDIDR_WRPS_NUM(dbg_didr));
 857 }
 858
 859 static __inline uint32_t
 860 dgb_get_brp_num(void)
 861 {
 862         uint32_t dbg_didr;
 863
 864         dbg_didr = cp14_dbgdidr_get();
 865
 866         return (DBGDIDR_BRPS_NUM(dbg_didr));
 867 }
 868
 869 static int
 870 dbg_reset_state(void)
 871 {
 872         u_int cpuid;
 873         size_t i;
 874         int err;
 875
 876         cpuid = PCPU_GET(cpuid);
 877         err = 0;
 878
 879         switch (dbg_model) {
 880         case ID_DFR0_CP_DEBUG_M_V6:
 881         case ID_DFR0_CP_DEBUG_M_V6_1: /* fall through */
 882                 /*
 883                  * Arch needs monitor mode selected and enabled
 884                  * to be able to access breakpoint/watchpoint registers.
 885                  */
 886                 err = dbg_enable_monitor();
 887                 if (err != 0)
 888                         return (err);
 889                 goto vectr_clr;
 890         case ID_DFR0_CP_DEBUG_M_V7:
 891                 /* Is core power domain powered up? */
 892                 if ((cp14_dbgprsr_get() & DBGPRSR_PU) == 0)
 893                         err = ENXIO;
 894
 895                 if (err != 0)
 896                         break;
 897
 898                 if (dbg_ossr)
 899                         goto vectr_clr;
 900                 break;
 901         case ID_DFR0_CP_DEBUG_M_V7_1:
 902                 /* Is double lock set? */
 903                 if ((cp14_dbgosdlr_get() & DBGPRSR_DLK) != 0)
 904                         err = ENXIO;
 905
 906                 break;
 907         default:
 908                 break;
 909         }
 910
 911         if (err != 0) {
 912                 db_printf("Debug facility locked (CPU%d)\n", cpuid);
 913                 return (err);
 914         }
 915
 916         /*
 917          * DBGOSLAR is always implemented for v7.1 Debug Arch. however is
 918          * optional for v7 (depends on OS save and restore support).
 919          */
 920         if (((dbg_model & ID_DFR0_CP_DEBUG_M_V7_1) != 0) || dbg_ossr) {
 921                 /*
 922                  * Clear OS lock.
 923                  * Writing any other value than 0xC5ACCESS will unlock.
 924                  */
 925                 cp14_dbgoslar_set(0);
 926                 isb();
 927         }
 928
 929 vectr_clr:
 930         /*
 931          * After reset we must ensure that DBGVCR has a defined value.
 932          * Disable all vector catch events. Safe to use - required in all
 933          * implementations.
 934          */
 935         cp14_dbgvcr_set(0);
 936         isb();
 937
 938         /*
 939          * We have limited number of {watch,break}points, each consists of
 940          * two registers:
 941          * - wcr/bcr regsiter configurates corresponding {watch,break}point
 942          *   behaviour
 943          * - wvr/bvr register keeps address we are hunting for
 944          *
 945          * Reset all breakpoints and watchpoints.
 946          */
 947         for (i = 0; i < dbg_watchpoint_num; ++i) {
 948                 dbg_wb_write_reg(DBG_REG_BASE_WCR, i, 0);
 949                 dbg_wb_write_reg(DBG_REG_BASE_WVR, i, 0);
 950         }
 951
 952         for (i = 0; i < dbg_breakpoint_num; ++i) {
 953                 dbg_wb_write_reg(DBG_REG_BASE_BCR, i, 0);
 954                 dbg_wb_write_reg(DBG_REG_BASE_BVR, i, 0);
 955         }
 956
 957         return (0);
 958 }
 959
 960 void
 961 dbg_monitor_init(void)
 962 {
 963         int err;
 964
 965         /* Fetch ARM Debug Architecture model */
 966         dbg_model = dbg_get_debug_model();
 967
 968         if (!dbg_arch_supported()) {
 969                 db_printf("ARM Debug Architecture not supported\n");
 970                 return;
 971         }
 972
 973         if (bootverbose) {
 974                 db_printf("ARM Debug Architecture %s\n",
 975                     (dbg_model == ID_DFR0_CP_DEBUG_M_V6) ? "v6" :
 976                     (dbg_model == ID_DFR0_CP_DEBUG_M_V6_1) ? "v6.1" :
 977                     (dbg_model == ID_DFR0_CP_DEBUG_M_V7) ? "v7" :
 978                     (dbg_model == ID_DFR0_CP_DEBUG_M_V7_1) ? "v7.1" : "unknown");
 979         }
 980
 981         /* Do we have OS Save and Restore mechanism? */
 982         dbg_ossr = dbg_get_ossr();
 983
 984         /* Find out many breakpoints and watchpoints we can use */
 985         dbg_watchpoint_num = dbg_get_wrp_num();
 986         dbg_breakpoint_num = dgb_get_brp_num();
 987
 988         if (bootverbose) {
 989                 db_printf("%d watchpoints and %d breakpoints supported\n",
 990                     dbg_watchpoint_num, dbg_breakpoint_num);
 991         }
 992
 993         err = dbg_reset_state();
 994         if (err == 0) {
 995                 err = dbg_enable_monitor();
 996                 if (err == 0) {
 997                         atomic_set_int(&dbg_capable_var, 1);
 998                         return;
 999                 }
1000         }
1001
1002         db_printf("HW Breakpoints/Watchpoints not enabled on CPU%d\n",
1003             PCPU_GET(cpuid));
1004 }
1005
1006 CTASSERT(sizeof(struct dbreg) == sizeof(((struct pcpu *)NULL)->pc_dbreg));
1007
1008 void
1009 dbg_monitor_init_secondary(void)
1010 {
1011         u_int cpuid;
1012         int err;
1013         /*
1014          * This flag is set on the primary CPU
1015          * and its meaning is valid for other CPUs too.
1016          */
1017         if (!dbg_capable())
1018                 return;
1019
1020         cpuid = PCPU_GET(cpuid);
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__, 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__, 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 }