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