1 #include "libcflat.h" 2 #include "desc.h" 3 #include "processor.h" 4 5 typedef struct { 6 unsigned short offset0; 7 unsigned short selector; 8 unsigned short ist : 3; 9 unsigned short : 5; 10 unsigned short type : 4; 11 unsigned short : 1; 12 unsigned short dpl : 2; 13 unsigned short p : 1; 14 unsigned short offset1; 15 #ifdef __x86_64__ 16 unsigned offset2; 17 unsigned reserved; 18 #endif 19 } idt_entry_t; 20 21 static idt_entry_t idt[256]; 22 23 void load_lidt(idt_entry_t *idt, int nentries) 24 { 25 struct descriptor_table_ptr dt; 26 27 dt.limit = nentries * sizeof(*idt) - 1; 28 dt.base = (unsigned long)idt; 29 lidt(&dt); 30 asm volatile ("lidt %0" : : "m"(dt)); 31 } 32 33 void set_idt_entry(int vec, void *addr, int dpl) 34 { 35 idt_entry_t *e = &idt[vec]; 36 memset(e, 0, sizeof *e); 37 e->offset0 = (unsigned long)addr; 38 e->selector = read_cs(); 39 e->ist = 0; 40 e->type = 14; 41 e->dpl = dpl; 42 e->p = 1; 43 e->offset1 = (unsigned long)addr >> 16; 44 #ifdef __x86_64__ 45 e->offset2 = (unsigned long)addr >> 32; 46 #endif 47 } 48 49 struct ex_regs { 50 unsigned long rax, rcx, rdx, rbx; 51 unsigned long dummy, rbp, rsi, rdi; 52 #ifdef __x86_64__ 53 unsigned long r8, r9, r10, r11; 54 unsigned long r12, r13, r14, r15; 55 #endif 56 unsigned long vector; 57 unsigned long error_code; 58 unsigned long rip; 59 unsigned long cs; 60 unsigned long rflags; 61 }; 62 63 struct ex_record { 64 unsigned long rip; 65 unsigned long handler; 66 }; 67 68 extern struct ex_record exception_table_start, exception_table_end; 69 70 void do_handle_exception(struct ex_regs *regs) 71 { 72 struct ex_record *ex; 73 unsigned ex_val; 74 75 ex_val = regs->vector | (regs->error_code << 16); 76 77 asm("mov %0, %%gs:4" : : "r"(ex_val)); 78 79 for (ex = &exception_table_start; ex != &exception_table_end; ++ex) { 80 if (ex->rip == regs->rip) { 81 regs->rip = ex->handler; 82 return; 83 } 84 } 85 printf("unhandled excecption\n"); 86 exit(7); 87 } 88 89 #ifdef __x86_64__ 90 # define R "r" 91 # define W "q" 92 # define S "8" 93 #else 94 # define R "e" 95 # define W "l" 96 # define S "4" 97 #endif 98 99 asm (".pushsection .text \n\t" 100 "ud_fault: \n\t" 101 "push"W" $0 \n\t" 102 "push"W" $6 \n\t" 103 "jmp handle_exception \n\t" 104 105 "gp_fault: \n\t" 106 "push"W" $13 \n\t" 107 "jmp handle_exception \n\t" 108 109 "de_fault: \n\t" 110 "push"W" $0 \n\t" 111 "push"W" $0 \n\t" 112 "jmp handle_exception \n\t" 113 114 "handle_exception: \n\t" 115 #ifdef __x86_64__ 116 "push %r15; push %r14; push %r13; push %r12 \n\t" 117 "push %r11; push %r10; push %r9; push %r8 \n\t" 118 #endif 119 "push %"R"di; push %"R"si; push %"R"bp; sub $"S", %"R"sp \n\t" 120 "push %"R"bx; push %"R"dx; push %"R"cx; push %"R"ax \n\t" 121 "mov %"R"sp, %"R"di \n\t" 122 "call do_handle_exception \n\t" 123 "pop %"R"ax; pop %"R"cx; pop %"R"dx; pop %"R"bx \n\t" 124 "add $"S", %"R"sp; pop %"R"bp; pop %"R"si; pop %"R"di \n\t" 125 #ifdef __x86_64__ 126 "pop %r8; pop %r9; pop %r10; pop %r11 \n\t" 127 "pop %r12; pop %r13; pop %r14; pop %r15 \n\t" 128 #endif 129 "add $"S", %"R"sp \n\t" 130 "add $"S", %"R"sp \n\t" 131 "iret"W" \n\t" 132 ".popsection"); 133 134 135 void setup_idt(void) 136 { 137 extern char ud_fault, gp_fault, de_fault; 138 139 load_lidt(idt, 256); 140 set_idt_entry(0, &de_fault, 0); 141 set_idt_entry(6, &ud_fault, 0); 142 set_idt_entry(13, &gp_fault, 0); 143 } 144 145 unsigned exception_vector(void) 146 { 147 unsigned short vector; 148 149 asm("mov %%gs:4, %0" : "=rm"(vector)); 150 return vector; 151 } 152 153 unsigned exception_error_code(void) 154 { 155 unsigned short error_code; 156 157 asm("mov %%gs:6, %0" : "=rm"(error_code)); 158 return error_code; 159 } 160