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