Robert Zieba | 3f01cd1 | 2022-04-14 10:36:15 -0600 | [diff] [blame] | 1 | /* SPDX-License-Identifier: GPL-2.0-only */ |
| 2 | #include <arch/registers.h> |
| 3 | #include <arch/breakpoint.h> |
Robert Zieba | 3f01cd1 | 2022-04-14 10:36:15 -0600 | [diff] [blame] | 4 | #include <types.h> |
| 5 | |
| 6 | #define DEBUG_REGISTER_COUNT 4 |
| 7 | |
| 8 | /* Each enable field is 2 bits and starts at bit 0 */ |
| 9 | #define DEBUG_CTRL_ENABLE_SHIFT(index) (2 * (index)) |
| 10 | #define DEBUG_CTRL_ENABLE_MASK(index) (0x3 << DEBUG_CTRL_ENABLE_SHIFT(index)) |
| 11 | #define DEBUG_CTRL_ENABLE(index, enable) ((enable) << DEBUG_CTRL_ENABLE_SHIFT(index)) |
| 12 | |
| 13 | /* Each breakpoint has a length and type, each is two bits and start at bit 16 */ |
| 14 | #define DEBUG_CTRL_LT_SHIFT(index) (4 * (index) + 16) |
| 15 | #define DEBUG_CTRL_LT_MASK(index) (0xf << DEBUG_CTRL_LT_SHIFT(index)) |
| 16 | #define DEBUG_CTRL_LT(index, len, type) ((((len) << 2 | (type))) << DEBUG_CTRL_LT_SHIFT(index)) |
| 17 | |
| 18 | /* Each field is one bit, starting at bit 0 */ |
| 19 | #define DEBUG_STATUS_BP_HIT_MASK(index) (1 << (index)) |
| 20 | #define DEBUG_STATUS_GET_BP_HIT(index, value) \ |
| 21 | (((value) & DEBUG_STATUS_BP_HIT_MASK(index)) >> (index)) |
| 22 | |
| 23 | /* Breakpoint lengths values */ |
| 24 | #define DEBUG_CTRL_LEN_1 0x0 |
| 25 | #define DEBUG_CTRL_LEN_2 0x1 |
| 26 | #define DEBUG_CTRL_LEN_8 0x2 |
| 27 | #define DEBUG_CTRL_LEN_4 0x3 |
| 28 | |
| 29 | /* Breakpoint enable values */ |
| 30 | #define DEBUG_CTRL_ENABLE_LOCAL 0x1 |
| 31 | #define DEBUG_CTRL_ENABLE_GLOBAL 0x2 |
| 32 | |
| 33 | /* eflags/rflags bit to continue execution after hitting an instruction breakpoint */ |
| 34 | #define FLAGS_RESUME (1 << 16) |
| 35 | |
| 36 | struct breakpoint { |
| 37 | bool allocated; |
| 38 | enum breakpoint_type type; |
| 39 | breakpoint_handler handler; |
| 40 | }; |
| 41 | |
| 42 | static struct breakpoint breakpoints[DEBUG_REGISTER_COUNT]; |
| 43 | |
| 44 | static inline bool debug_write_addr_reg(int index, uintptr_t value) |
| 45 | { |
| 46 | switch (index) { |
| 47 | case 0: |
| 48 | asm("mov %0, %%dr0" ::"r"(value)); |
| 49 | break; |
| 50 | |
| 51 | case 1: |
| 52 | asm("mov %0, %%dr1" ::"r"(value)); |
| 53 | break; |
| 54 | |
| 55 | case 2: |
| 56 | asm("mov %0, %%dr2" ::"r"(value)); |
| 57 | break; |
| 58 | |
| 59 | case 3: |
| 60 | asm("mov %0, %%dr3" ::"r"(value)); |
| 61 | break; |
| 62 | |
| 63 | default: |
| 64 | return false; |
| 65 | } |
| 66 | |
| 67 | return true; |
| 68 | } |
| 69 | |
| 70 | static inline uintptr_t debug_read_status(void) |
| 71 | { |
| 72 | uintptr_t ret = 0; |
| 73 | |
| 74 | asm("mov %%dr6, %0" : "=r"(ret)); |
| 75 | return ret; |
| 76 | } |
| 77 | |
| 78 | static inline void debug_write_status(uintptr_t value) |
| 79 | { |
| 80 | asm("mov %0, %%dr6" ::"r"(value)); |
| 81 | } |
| 82 | |
| 83 | static inline uintptr_t debug_read_control(void) |
| 84 | { |
| 85 | uintptr_t ret = 0; |
| 86 | |
| 87 | asm("mov %%dr7, %0" : "=r"(ret)); |
| 88 | return ret; |
| 89 | } |
| 90 | |
| 91 | static inline void debug_write_control(uintptr_t value) |
| 92 | { |
| 93 | asm("mov %0, %%dr7" ::"r"(value)); |
| 94 | } |
| 95 | |
| 96 | static enum breakpoint_result allocate_breakpoint(struct breakpoint_handle *out_handle, |
| 97 | enum breakpoint_type type) |
| 98 | { |
| 99 | for (int i = 0; i < DEBUG_REGISTER_COUNT; i++) { |
| 100 | if (breakpoints[i].allocated) |
| 101 | continue; |
| 102 | |
| 103 | breakpoints[i].allocated = true; |
| 104 | breakpoints[i].handler = NULL; |
| 105 | breakpoints[i].type = type; |
| 106 | out_handle->bp = i; |
| 107 | return BREAKPOINT_RES_OK; |
| 108 | } |
| 109 | |
| 110 | return BREAKPOINT_RES_NONE_AVAILABLE; |
| 111 | } |
| 112 | |
| 113 | static enum breakpoint_result validate_handle(struct breakpoint_handle handle) |
| 114 | { |
| 115 | int bp = handle.bp; |
| 116 | |
| 117 | if (bp < 0 || bp >= DEBUG_REGISTER_COUNT || !breakpoints[bp].allocated) |
| 118 | return BREAKPOINT_RES_INVALID_HANDLE; |
| 119 | |
| 120 | return BREAKPOINT_RES_OK; |
| 121 | } |
| 122 | |
| 123 | enum breakpoint_result breakpoint_create_instruction(struct breakpoint_handle *out_handle, |
| 124 | void *virt_addr) |
| 125 | { |
| 126 | enum breakpoint_result res = |
| 127 | allocate_breakpoint(out_handle, BREAKPOINT_TYPE_INSTRUCTION); |
| 128 | |
| 129 | if (res != BREAKPOINT_RES_OK) |
| 130 | return res; |
| 131 | |
| 132 | int bp = out_handle->bp; |
| 133 | if (!debug_write_addr_reg(bp, (uintptr_t)virt_addr)) |
| 134 | return BREAKPOINT_RES_INVALID_HANDLE; |
| 135 | |
| 136 | uintptr_t control = debug_read_control(); |
| 137 | control &= ~DEBUG_CTRL_LT_MASK(bp); |
| 138 | control |= DEBUG_CTRL_LT(bp, DEBUG_CTRL_LEN_1, BREAKPOINT_TYPE_INSTRUCTION); |
| 139 | debug_write_control(control); |
| 140 | return BREAKPOINT_RES_OK; |
| 141 | } |
| 142 | |
| 143 | enum breakpoint_result breakpoint_create_data(struct breakpoint_handle *out_handle, |
| 144 | void *virt_addr, size_t len, bool write_only) |
| 145 | { |
| 146 | uintptr_t len_value = 0; |
| 147 | |
| 148 | switch (len) { |
| 149 | case 1: |
| 150 | len_value = DEBUG_CTRL_LEN_1; |
| 151 | break; |
| 152 | |
| 153 | case 2: |
| 154 | len_value = DEBUG_CTRL_LEN_2; |
| 155 | break; |
| 156 | |
| 157 | case 4: |
| 158 | len_value = DEBUG_CTRL_LEN_4; |
| 159 | break; |
| 160 | |
| 161 | case 8: |
| 162 | /* Only supported on 64-bit CPUs */ |
| 163 | if (!ENV_X86_64) |
| 164 | return BREAKPOINT_RES_INVALID_LENGTH; |
| 165 | len_value = DEBUG_CTRL_LEN_8; |
| 166 | break; |
| 167 | |
| 168 | default: |
| 169 | return BREAKPOINT_RES_INVALID_LENGTH; |
| 170 | } |
| 171 | |
| 172 | enum breakpoint_type type = |
| 173 | write_only ? BREAKPOINT_TYPE_DATA_WRITE : BREAKPOINT_TYPE_DATA_RW; |
| 174 | enum breakpoint_result res = allocate_breakpoint(out_handle, type); |
| 175 | if (res != BREAKPOINT_RES_OK) |
| 176 | return res; |
| 177 | |
| 178 | int bp = out_handle->bp; |
| 179 | if (!debug_write_addr_reg(bp, (uintptr_t)virt_addr)) |
| 180 | return BREAKPOINT_RES_INVALID_HANDLE; |
| 181 | |
| 182 | uintptr_t control = debug_read_control(); |
| 183 | control &= ~DEBUG_CTRL_LT_MASK(bp); |
| 184 | control |= DEBUG_CTRL_LT(bp, len_value, type); |
| 185 | debug_write_control(control); |
| 186 | return BREAKPOINT_RES_OK; |
| 187 | } |
| 188 | |
| 189 | enum breakpoint_result breakpoint_remove(struct breakpoint_handle handle) |
| 190 | { |
| 191 | enum breakpoint_result res = validate_handle(handle); |
| 192 | |
| 193 | if (res != BREAKPOINT_RES_OK) |
| 194 | return res; |
| 195 | breakpoint_enable(handle, false); |
| 196 | |
| 197 | int bp = handle.bp; |
| 198 | breakpoints[bp].allocated = false; |
| 199 | return BREAKPOINT_RES_OK; |
| 200 | } |
| 201 | |
| 202 | enum breakpoint_result breakpoint_enable(struct breakpoint_handle handle, bool enabled) |
| 203 | { |
| 204 | enum breakpoint_result res = validate_handle(handle); |
| 205 | |
| 206 | if (res != BREAKPOINT_RES_OK) |
| 207 | return res; |
| 208 | |
| 209 | uintptr_t control = debug_read_control(); |
| 210 | int bp = handle.bp; |
| 211 | control &= ~DEBUG_CTRL_ENABLE_MASK(bp); |
| 212 | if (enabled) |
| 213 | control |= DEBUG_CTRL_ENABLE(bp, DEBUG_CTRL_ENABLE_GLOBAL); |
| 214 | debug_write_control(control); |
| 215 | return BREAKPOINT_RES_OK; |
| 216 | } |
| 217 | |
| 218 | enum breakpoint_result breakpoint_get_type(struct breakpoint_handle handle, |
| 219 | enum breakpoint_type *type) |
| 220 | { |
| 221 | enum breakpoint_result res = validate_handle(handle); |
| 222 | |
| 223 | if (res != BREAKPOINT_RES_OK) |
| 224 | return res; |
| 225 | |
| 226 | *type = breakpoints[handle.bp].type; |
| 227 | return BREAKPOINT_RES_OK; |
| 228 | } |
| 229 | |
| 230 | enum breakpoint_result breakpoint_set_handler(struct breakpoint_handle handle, |
| 231 | breakpoint_handler handler) |
| 232 | { |
| 233 | enum breakpoint_result res = validate_handle(handle); |
| 234 | |
| 235 | if (res != BREAKPOINT_RES_OK) |
| 236 | return res; |
| 237 | |
| 238 | breakpoints[handle.bp].handler = handler; |
| 239 | return BREAKPOINT_RES_OK; |
| 240 | } |
| 241 | |
| 242 | static enum breakpoint_result is_breakpoint_hit(struct breakpoint_handle handle, bool *out_hit) |
| 243 | { |
| 244 | enum breakpoint_result res = validate_handle(handle); |
| 245 | |
| 246 | if (res != BREAKPOINT_RES_OK) |
| 247 | return res; |
| 248 | |
| 249 | uintptr_t status = debug_read_status(); |
| 250 | *out_hit = DEBUG_STATUS_GET_BP_HIT(handle.bp, status); |
| 251 | |
| 252 | return BREAKPOINT_RES_OK; |
| 253 | } |
| 254 | |
| 255 | int breakpoint_dispatch_handler(struct eregs *info) |
| 256 | { |
| 257 | bool instr_bp_hit = 0; |
| 258 | |
| 259 | for (int i = 0; i < DEBUG_REGISTER_COUNT; i++) { |
| 260 | struct breakpoint_handle handle = { i }; |
| 261 | bool hit = false; |
| 262 | enum breakpoint_type type; |
| 263 | |
| 264 | if (is_breakpoint_hit(handle, &hit) != BREAKPOINT_RES_OK || !hit) |
| 265 | continue; |
| 266 | |
| 267 | if (breakpoint_get_type(handle, &type) != BREAKPOINT_RES_OK) |
| 268 | continue; |
| 269 | |
| 270 | instr_bp_hit |= type == BREAKPOINT_TYPE_INSTRUCTION; |
| 271 | |
| 272 | /* Call the breakpoint handler. */ |
| 273 | if (breakpoints[handle.bp].handler) { |
| 274 | int ret = breakpoints[handle.bp].handler(handle, info); |
| 275 | /* A non-zero return value indicates a fatal error. */ |
| 276 | if (ret) |
| 277 | return ret; |
| 278 | } |
| 279 | } |
| 280 | |
| 281 | /* Clear hit breakpoints. */ |
| 282 | uintptr_t status = debug_read_status(); |
| 283 | for (int i = 0; i < DEBUG_REGISTER_COUNT; i++) { |
| 284 | status &= ~DEBUG_STATUS_BP_HIT_MASK(i); |
| 285 | } |
| 286 | debug_write_status(status); |
| 287 | |
| 288 | if (instr_bp_hit) { |
| 289 | /* Set the resume flag so the same breakpoint won't be hit immediately. */ |
| 290 | #if ENV_X86_64 |
| 291 | info->rflags |= FLAGS_RESUME; |
| 292 | #else |
| 293 | info->eflags |= FLAGS_RESUME; |
| 294 | #endif |
| 295 | } |
| 296 | |
| 297 | return 0; |
| 298 | } |