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