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 typedef struct { 22 u16 limit_low; 23 u16 base_low; 24 u8 base_middle; 25 u8 access; 26 u8 granularity; 27 u8 base_high; 28 } gdt_entry_t; 29 30 typedef struct { 31 u16 prev; 32 u16 res1; 33 u32 esp0; 34 u16 ss0; 35 u16 res2; 36 u32 esp1; 37 u16 ss1; 38 u16 res3; 39 u32 esp2; 40 u16 ss2; 41 u16 res4; 42 u32 cr3; 43 u32 eip; 44 u32 eflags; 45 u32 eax, ecx, edx, ebx, esp, ebp, esi, edi; 46 u16 es; 47 u16 res5; 48 u16 cs; 49 u16 res6; 50 u16 ss; 51 u16 res7; 52 u16 ds; 53 u16 res8; 54 u16 fs; 55 u16 res9; 56 u16 gs; 57 u16 res10; 58 u16 ldt; 59 u16 res11; 60 u16 t:1; 61 u16 res12:15; 62 u16 iomap_base; 63 } tss32_t; 64 65 static idt_entry_t idt[256]; 66 67 void load_lidt(idt_entry_t *idt, int nentries) 68 { 69 struct descriptor_table_ptr dt; 70 71 dt.limit = nentries * sizeof(*idt) - 1; 72 dt.base = (unsigned long)idt; 73 lidt(&dt); 74 asm volatile ("lidt %0" : : "m"(dt)); 75 } 76 77 void set_idt_entry(int vec, void *addr, int dpl) 78 { 79 idt_entry_t *e = &idt[vec]; 80 memset(e, 0, sizeof *e); 81 e->offset0 = (unsigned long)addr; 82 e->selector = read_cs(); 83 e->ist = 0; 84 e->type = 14; 85 e->dpl = dpl; 86 e->p = 1; 87 e->offset1 = (unsigned long)addr >> 16; 88 #ifdef __x86_64__ 89 e->offset2 = (unsigned long)addr >> 32; 90 #endif 91 } 92 93 struct ex_regs { 94 unsigned long rax, rcx, rdx, rbx; 95 unsigned long dummy, rbp, rsi, rdi; 96 #ifdef __x86_64__ 97 unsigned long r8, r9, r10, r11; 98 unsigned long r12, r13, r14, r15; 99 #endif 100 unsigned long vector; 101 unsigned long error_code; 102 unsigned long rip; 103 unsigned long cs; 104 unsigned long rflags; 105 }; 106 107 struct ex_record { 108 unsigned long rip; 109 unsigned long handler; 110 }; 111 112 extern struct ex_record exception_table_start, exception_table_end; 113 114 #ifndef __x86_64__ 115 __attribute__((regparm(1))) 116 #endif 117 void do_handle_exception(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 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 printf("unhandled excecption\n"); 133 exit(7); 134 } 135 136 #ifdef __x86_64__ 137 # define R "r" 138 # define W "q" 139 # define S "8" 140 #else 141 # define R "e" 142 # define W "l" 143 # define S "4" 144 #endif 145 146 asm (".pushsection .text \n\t" 147 "ud_fault: \n\t" 148 "push"W" $0 \n\t" 149 "push"W" $6 \n\t" 150 "jmp handle_exception \n\t" 151 152 "gp_fault: \n\t" 153 "push"W" $13 \n\t" 154 "jmp handle_exception \n\t" 155 156 "de_fault: \n\t" 157 "push"W" $0 \n\t" 158 "push"W" $0 \n\t" 159 "jmp handle_exception \n\t" 160 161 "handle_exception: \n\t" 162 #ifdef __x86_64__ 163 "push %r15; push %r14; push %r13; push %r12 \n\t" 164 "push %r11; push %r10; push %r9; push %r8 \n\t" 165 #endif 166 "push %"R"di; push %"R"si; push %"R"bp; sub $"S", %"R"sp \n\t" 167 "push %"R"bx; push %"R"dx; push %"R"cx; push %"R"ax \n\t" 168 #ifdef __x86_64__ 169 "mov %"R"sp, %"R"di \n\t" 170 #else 171 "mov %"R"sp, %"R"ax \n\t" 172 #endif 173 "call do_handle_exception \n\t" 174 "pop %"R"ax; pop %"R"cx; pop %"R"dx; pop %"R"bx \n\t" 175 "add $"S", %"R"sp; pop %"R"bp; pop %"R"si; pop %"R"di \n\t" 176 #ifdef __x86_64__ 177 "pop %r8; pop %r9; pop %r10; pop %r11 \n\t" 178 "pop %r12; pop %r13; pop %r14; pop %r15 \n\t" 179 #endif 180 "add $"S", %"R"sp \n\t" 181 "add $"S", %"R"sp \n\t" 182 "iret"W" \n\t" 183 ".popsection"); 184 185 186 void setup_idt(void) 187 { 188 extern char ud_fault, gp_fault, de_fault; 189 190 load_lidt(idt, 256); 191 set_idt_entry(0, &de_fault, 0); 192 set_idt_entry(6, &ud_fault, 0); 193 set_idt_entry(13, &gp_fault, 0); 194 } 195 196 unsigned exception_vector(void) 197 { 198 unsigned short vector; 199 200 asm("mov %%gs:4, %0" : "=rm"(vector)); 201 return vector; 202 } 203 204 unsigned exception_error_code(void) 205 { 206 unsigned short error_code; 207 208 asm("mov %%gs:6, %0" : "=rm"(error_code)); 209 return error_code; 210 } 211 212 #ifndef __x86_64__ 213 /* 214 * GDT, with 6 entries: 215 * 0x00 - NULL descriptor 216 * 0x08 - Code segment 217 * 0x10 - Data segment 218 * 0x18 - Not presend code segment 219 * 0x20 - Primery task 220 * 0x28 - Interrupt task 221 */ 222 223 static gdt_entry_t gdt[6]; 224 #define TSS_GDT_OFFSET 4 225 226 void set_gdt_entry(int num, u32 base, u32 limit, u8 access, u8 gran) 227 { 228 /* Setup the descriptor base address */ 229 gdt[num].base_low = (base & 0xFFFF); 230 gdt[num].base_middle = (base >> 16) & 0xFF; 231 gdt[num].base_high = (base >> 24) & 0xFF; 232 233 /* Setup the descriptor limits */ 234 gdt[num].limit_low = (limit & 0xFFFF); 235 gdt[num].granularity = ((limit >> 16) & 0x0F); 236 237 /* Finally, set up the granularity and access flags */ 238 gdt[num].granularity |= (gran & 0xF0); 239 gdt[num].access = access; 240 } 241 242 void setup_gdt(void) 243 { 244 struct descriptor_table_ptr gp; 245 /* Setup the GDT pointer and limit */ 246 gp.limit = sizeof(gdt) - 1; 247 gp.base = (ulong)&gdt; 248 249 memset(gdt, 0, sizeof(gdt)); 250 251 /* Our NULL descriptor */ 252 set_gdt_entry(0, 0, 0, 0, 0); 253 254 /* The second entry is our Code Segment. The base address 255 * is 0, the limit is 4GBytes, it uses 4KByte granularity, 256 * uses 32-bit opcodes, and is a Code Segment descriptor. */ 257 set_gdt_entry(1, 0, 0xFFFFFFFF, 0x9A, 0xcf); 258 259 /* The third entry is our Data Segment. It's EXACTLY the 260 * same as our code segment, but the descriptor type in 261 * this entry's access byte says it's a Data Segment */ 262 set_gdt_entry(2, 0, 0xFFFFFFFF, 0x92, 0xcf); 263 264 /* Same as code register above but not present */ 265 set_gdt_entry(3, 0, 0xFFFFFFFF, 0x1A, 0xcf); 266 267 268 /* Flush out the old GDT and install the new changes! */ 269 lgdt(&gp); 270 271 asm volatile ("mov %0, %%ds\n\t" 272 "mov %0, %%es\n\t" 273 "mov %0, %%fs\n\t" 274 "mov %0, %%gs\n\t" 275 "mov %0, %%ss\n\t" 276 "jmp $0x08, $.Lflush2\n\t" 277 ".Lflush2: "::"r"(0x10)); 278 } 279 280 static void set_idt_task_gate(int vec, u16 sel) 281 { 282 idt_entry_t *e = &idt[vec]; 283 284 memset(e, 0, sizeof *e); 285 286 e->selector = sel; 287 e->ist = 0; 288 e->type = 5; 289 e->dpl = 0; 290 e->p = 1; 291 } 292 293 /* 294 * 0 - main task 295 * 1 - interrupt task 296 */ 297 298 static tss32_t tss[2]; 299 static char tss_stack[2][4096]; 300 301 void setup_tss32(void) 302 { 303 u16 desc_size = sizeof(tss32_t); 304 int i; 305 306 for (i = 0; i < 2; i++) { 307 tss[i].cr3 = read_cr3(); 308 tss[i].ss0 = tss[i].ss1 = tss[i].ss2 = 0x10; 309 tss[i].esp = tss[i].esp0 = tss[i].esp1 = tss[i].esp2 = 310 (u32)tss_stack[i]; 311 tss[i].cs = 0x08; 312 tss[i].ds = tss[i].es = tss[i].fs = tss[i].gs = tss[i].ss = 0x10; 313 tss[i].iomap_base = (u16)desc_size; 314 set_gdt_entry(TSS_GDT_OFFSET + i, (u32)&tss[i], 315 desc_size - 1, 0x89, 0x0f); 316 } 317 318 ltr(TSS_MAIN); 319 } 320 321 void set_intr_task_gate(int e, void *fn) 322 { 323 tss[1].eip = (u32)fn; 324 set_idt_task_gate(e, TSS_INTR); 325 } 326 327 void print_current_tss_info(void) 328 { 329 u16 tr = str(); 330 int i = (tr == TSS_MAIN) ? 0 : 1; 331 332 if (tr != TSS_MAIN && tr != TSS_INTR) 333 printf("Unknown TSS %x\n", tr); 334 else 335 printf("TR=%x Main TSS back link %x. Current TSS back link %x\n", 336 tr, tss[0].prev, tss[i].prev); 337 } 338 #endif 339