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