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