1 #include "libcflat.h" 2 #include "desc.h" 3 #include "processor.h" 4 #include <setjmp.h> 5 6 #ifndef __x86_64__ 7 __attribute__((regparm(1))) 8 #endif 9 void do_handle_exception(struct ex_regs *regs); 10 11 void set_idt_entry(int vec, void *addr, int dpl) 12 { 13 idt_entry_t *e = &boot_idt[vec]; 14 memset(e, 0, sizeof *e); 15 e->offset0 = (unsigned long)addr; 16 e->selector = read_cs(); 17 e->ist = 0; 18 e->type = 14; 19 e->dpl = dpl; 20 e->p = 1; 21 e->offset1 = (unsigned long)addr >> 16; 22 #ifdef __x86_64__ 23 e->offset2 = (unsigned long)addr >> 32; 24 #endif 25 } 26 27 void set_idt_dpl(int vec, u16 dpl) 28 { 29 idt_entry_t *e = &boot_idt[vec]; 30 e->dpl = dpl; 31 } 32 33 void set_idt_sel(int vec, u16 sel) 34 { 35 idt_entry_t *e = &boot_idt[vec]; 36 e->selector = sel; 37 } 38 39 struct ex_record { 40 unsigned long rip; 41 unsigned long handler; 42 }; 43 44 extern struct ex_record exception_table_start, exception_table_end; 45 46 static const char* exception_mnemonic(int vector) 47 { 48 switch(vector) { 49 case 0: return "#DE"; 50 case 1: return "#DB"; 51 case 2: return "#NMI"; 52 case 3: return "#BP"; 53 case 4: return "#OF"; 54 case 5: return "#BR"; 55 case 6: return "#UD"; 56 case 7: return "#NM"; 57 case 8: return "#DF"; 58 case 10: return "#TS"; 59 case 11: return "#NP"; 60 case 12: return "#SS"; 61 case 13: return "#GP"; 62 case 14: return "#PF"; 63 case 16: return "#MF"; 64 case 17: return "#AC"; 65 case 18: return "#MC"; 66 case 19: return "#XM"; 67 default: return "#??"; 68 } 69 } 70 71 void unhandled_exception(struct ex_regs *regs, bool cpu) 72 { 73 printf("Unhandled %sexception %ld %s at ip %016lx\n", 74 cpu ? "cpu " : "", regs->vector, 75 exception_mnemonic(regs->vector), regs->rip); 76 if (regs->vector == 14) 77 printf("PF at %#lx addr %#lx\n", regs->rip, read_cr2()); 78 79 printf("error_code=%04lx rflags=%08lx cs=%08lx\n" 80 "rax=%016lx rcx=%016lx rdx=%016lx rbx=%016lx\n" 81 "rbp=%016lx rsi=%016lx rdi=%016lx\n" 82 #ifdef __x86_64__ 83 " r8=%016lx r9=%016lx r10=%016lx r11=%016lx\n" 84 "r12=%016lx r13=%016lx r14=%016lx r15=%016lx\n" 85 #endif 86 "cr0=%016lx cr2=%016lx cr3=%016lx cr4=%016lx\n" 87 #ifdef __x86_64__ 88 "cr8=%016lx\n" 89 #endif 90 , 91 regs->error_code, regs->rflags, regs->cs, 92 regs->rax, regs->rcx, regs->rdx, regs->rbx, 93 regs->rbp, regs->rsi, regs->rdi, 94 #ifdef __x86_64__ 95 regs->r8, regs->r9, regs->r10, regs->r11, 96 regs->r12, regs->r13, regs->r14, regs->r15, 97 #endif 98 read_cr0(), read_cr2(), read_cr3(), read_cr4() 99 #ifdef __x86_64__ 100 , read_cr8() 101 #endif 102 ); 103 dump_frame_stack((void*) regs->rip, (void*) regs->rbp); 104 abort(); 105 } 106 107 static void check_exception_table(struct ex_regs *regs) 108 { 109 struct ex_record *ex; 110 unsigned ex_val; 111 112 ex_val = regs->vector | (regs->error_code << 16) | 113 (((regs->rflags >> 16) & 1) << 8); 114 asm("mov %0, %%gs:4" : : "r"(ex_val)); 115 116 for (ex = &exception_table_start; ex != &exception_table_end; ++ex) { 117 if (ex->rip == regs->rip) { 118 regs->rip = ex->handler; 119 return; 120 } 121 } 122 unhandled_exception(regs, false); 123 } 124 125 static handler exception_handlers[32]; 126 127 handler handle_exception(u8 v, handler fn) 128 { 129 handler old; 130 131 old = exception_handlers[v]; 132 if (v < 32) 133 exception_handlers[v] = fn; 134 return old; 135 } 136 137 #ifndef __x86_64__ 138 __attribute__((regparm(1))) 139 #endif 140 void do_handle_exception(struct ex_regs *regs) 141 { 142 if (regs->vector < 32 && exception_handlers[regs->vector]) { 143 exception_handlers[regs->vector](regs); 144 return; 145 } 146 unhandled_exception(regs, true); 147 } 148 149 #define EX(NAME, N) extern char NAME##_fault; \ 150 asm (".pushsection .text \n\t" \ 151 #NAME"_fault: \n\t" \ 152 "push"W" $0 \n\t" \ 153 "push"W" $"#N" \n\t" \ 154 "jmp __handle_exception \n\t" \ 155 ".popsection") 156 157 #define EX_E(NAME, N) extern char NAME##_fault; \ 158 asm (".pushsection .text \n\t" \ 159 #NAME"_fault: \n\t" \ 160 "push"W" $"#N" \n\t" \ 161 "jmp __handle_exception \n\t" \ 162 ".popsection") 163 164 EX(de, 0); 165 EX(db, 1); 166 EX(nmi, 2); 167 EX(bp, 3); 168 EX(of, 4); 169 EX(br, 5); 170 EX(ud, 6); 171 EX(nm, 7); 172 EX_E(df, 8); 173 EX_E(ts, 10); 174 EX_E(np, 11); 175 EX_E(ss, 12); 176 EX_E(gp, 13); 177 EX_E(pf, 14); 178 EX(mf, 16); 179 EX_E(ac, 17); 180 EX(mc, 18); 181 EX(xm, 19); 182 EX_E(cp, 21); 183 184 asm (".pushsection .text \n\t" 185 "__handle_exception: \n\t" 186 #ifdef __x86_64__ 187 "push %r15; push %r14; push %r13; push %r12 \n\t" 188 "push %r11; push %r10; push %r9; push %r8 \n\t" 189 #endif 190 "push %"R "di; push %"R "si; push %"R "bp; sub $"S", %"R "sp \n\t" 191 "push %"R "bx; push %"R "dx; push %"R "cx; push %"R "ax \n\t" 192 #ifdef __x86_64__ 193 "mov %"R "sp, %"R "di \n\t" 194 #else 195 "mov %"R "sp, %"R "ax \n\t" 196 #endif 197 "call do_handle_exception \n\t" 198 "pop %"R "ax; pop %"R "cx; pop %"R "dx; pop %"R "bx \n\t" 199 "add $"S", %"R "sp; pop %"R "bp; pop %"R "si; pop %"R "di \n\t" 200 #ifdef __x86_64__ 201 "pop %r8; pop %r9; pop %r10; pop %r11 \n\t" 202 "pop %r12; pop %r13; pop %r14; pop %r15 \n\t" 203 #endif 204 "add $"S", %"R "sp \n\t" 205 "add $"S", %"R "sp \n\t" 206 "iret"W" \n\t" 207 ".popsection"); 208 209 static void *idt_handlers[32] = { 210 [0] = &de_fault, 211 [1] = &db_fault, 212 [2] = &nmi_fault, 213 [3] = &bp_fault, 214 [4] = &of_fault, 215 [5] = &br_fault, 216 [6] = &ud_fault, 217 [7] = &nm_fault, 218 [8] = &df_fault, 219 [10] = &ts_fault, 220 [11] = &np_fault, 221 [12] = &ss_fault, 222 [13] = &gp_fault, 223 [14] = &pf_fault, 224 [16] = &mf_fault, 225 [17] = &ac_fault, 226 [18] = &mc_fault, 227 [19] = &xm_fault, 228 [21] = &cp_fault, 229 }; 230 231 void setup_idt(void) 232 { 233 int i; 234 static bool idt_initialized = false; 235 236 if (idt_initialized) { 237 return; 238 } 239 idt_initialized = true; 240 for (i = 0; i < 32; i++) 241 if (idt_handlers[i]) 242 set_idt_entry(i, idt_handlers[i], 0); 243 handle_exception(0, check_exception_table); 244 handle_exception(6, check_exception_table); 245 handle_exception(13, check_exception_table); 246 } 247 248 unsigned exception_vector(void) 249 { 250 unsigned char vector; 251 252 asm volatile("movb %%gs:4, %0" : "=q"(vector)); 253 return vector; 254 } 255 256 int write_cr4_checking(unsigned long val) 257 { 258 asm volatile(ASM_TRY("1f") 259 "mov %0,%%cr4\n\t" 260 "1:": : "r" (val)); 261 return exception_vector(); 262 } 263 264 unsigned exception_error_code(void) 265 { 266 unsigned short error_code; 267 268 asm volatile("mov %%gs:6, %0" : "=r"(error_code)); 269 return error_code; 270 } 271 272 bool exception_rflags_rf(void) 273 { 274 unsigned char rf_flag; 275 276 asm volatile("movb %%gs:5, %b0" : "=q"(rf_flag)); 277 return rf_flag & 1; 278 } 279 280 static char intr_alt_stack[4096]; 281 282 #ifndef __x86_64__ 283 void set_gdt_entry(int sel, u32 base, u32 limit, u8 access, u8 gran) 284 { 285 int num = sel >> 3; 286 287 /* Setup the descriptor base address */ 288 gdt32[num].base_low = (base & 0xFFFF); 289 gdt32[num].base_middle = (base >> 16) & 0xFF; 290 gdt32[num].base_high = (base >> 24) & 0xFF; 291 292 /* Setup the descriptor limits */ 293 gdt32[num].limit_low = (limit & 0xFFFF); 294 gdt32[num].granularity = ((limit >> 16) & 0x0F); 295 296 /* Finally, set up the granularity and access flags */ 297 gdt32[num].granularity |= (gran & 0xF0); 298 gdt32[num].access = access; 299 } 300 301 void set_gdt_task_gate(u16 sel, u16 tss_sel) 302 { 303 set_gdt_entry(sel, tss_sel, 0, 0x85, 0); // task, present 304 } 305 306 void set_idt_task_gate(int vec, u16 sel) 307 { 308 idt_entry_t *e = &boot_idt[vec]; 309 310 memset(e, 0, sizeof *e); 311 312 e->selector = sel; 313 e->ist = 0; 314 e->type = 5; 315 e->dpl = 0; 316 e->p = 1; 317 } 318 319 /* 320 * 0 - main task 321 * 1 - interrupt task 322 */ 323 324 tss32_t tss_intr; 325 326 void setup_tss32(void) 327 { 328 u16 desc_size = sizeof(tss32_t); 329 330 tss.cr3 = read_cr3(); 331 tss_intr.cr3 = read_cr3(); 332 tss_intr.ss0 = tss_intr.ss1 = tss_intr.ss2 = 0x10; 333 tss_intr.esp = tss_intr.esp0 = tss_intr.esp1 = tss_intr.esp2 = 334 (u32)intr_alt_stack + 4096; 335 tss_intr.cs = 0x08; 336 tss_intr.ds = tss_intr.es = tss_intr.fs = tss_intr.ss = 0x10; 337 tss_intr.gs = read_gs(); 338 tss_intr.iomap_base = (u16)desc_size; 339 set_gdt_entry(TSS_INTR, (u32)&tss_intr, desc_size - 1, 0x89, 0); 340 } 341 342 void set_intr_task_gate(int e, void *fn) 343 { 344 tss_intr.eip = (u32)fn; 345 set_idt_task_gate(e, TSS_INTR); 346 } 347 348 void setup_alt_stack(void) 349 { 350 setup_tss32(); 351 } 352 353 void set_intr_alt_stack(int e, void *fn) 354 { 355 set_intr_task_gate(e, fn); 356 } 357 358 void print_current_tss_info(void) 359 { 360 u16 tr = str(); 361 362 if (tr != TSS_MAIN && tr != TSS_INTR) 363 printf("Unknown TSS %x\n", tr); 364 else 365 printf("TR=%x (%s) Main TSS back link %x. Intr TSS back link %x\n", 366 tr, tr ? "interrupt" : "main", tss.prev, tss_intr.prev); 367 } 368 #else 369 void set_intr_alt_stack(int e, void *addr) 370 { 371 set_idt_entry(e, addr, 0); 372 boot_idt[e].ist = 1; 373 } 374 375 void setup_alt_stack(void) 376 { 377 tss.ist1 = (u64)intr_alt_stack + 4096; 378 } 379 #endif 380 381 static bool exception; 382 static jmp_buf *exception_jmpbuf; 383 384 static void exception_handler_longjmp(void) 385 { 386 longjmp(*exception_jmpbuf, 1); 387 } 388 389 static void exception_handler(struct ex_regs *regs) 390 { 391 /* longjmp must happen after iret, so do not do it now. */ 392 exception = true; 393 regs->rip = (unsigned long)&exception_handler_longjmp; 394 regs->cs = read_cs(); 395 } 396 397 bool test_for_exception(unsigned int ex, void (*trigger_func)(void *data), 398 void *data) 399 { 400 handler old; 401 jmp_buf jmpbuf; 402 int ret; 403 404 old = handle_exception(ex, exception_handler); 405 ret = set_exception_jmpbuf(jmpbuf); 406 if (ret == 0) 407 trigger_func(data); 408 handle_exception(ex, old); 409 return ret; 410 } 411 412 void __set_exception_jmpbuf(jmp_buf *addr) 413 { 414 exception_jmpbuf = addr; 415 } 416