Root/
Source at commit HEAD created 5 years 6 days ago. By ifabio, Few update to kernelPatcher (Credits to CrazyBirdy) | |
---|---|
1 | #ifdef __i386__␊ |
2 | ␊ |
3 | #include "libsa.h"␊ |
4 | #include "saio_internal.h"␊ |
5 | ␊ |
6 | #ifdef DEBUG_INTERRUPTS␊ |
7 | #define DBG(x...) printf(x)␊ |
8 | #else␊ |
9 | #define DBG(x...)␊ |
10 | #endif␊ |
11 | ␊ |
12 | #define CODE_SEGMENT_SELECTOR 0x28U␊ |
13 | #define CODED_EXCEPTION_MASK 0x27D00U␊ |
14 | #define IA32_APIC_BASE 27U␊ |
15 | #define IA32_APIC_BASE_BSP 0x100U␊ |
16 | #define IA32_APIC_BASE_EN 0x800U␊ |
17 | #define LAPIC_WANTED_FLAGS (IA32_APIC_BASE_EN | IA32_APIC_BASE_BSP)␊ |
18 | #define LAPIC_EOI_OFFSET 0xB0U␊ |
19 | #define LAPIC_ISR_OFFSET 0x100U␊ |
20 | #define PIC_READ_ISR_COMMAND 11U␊ |
21 | #define PIC_EOI_COMMAND 0x20U␊ |
22 | #define PIC_PORT0 0␊ |
23 | #define PIC_PORT1 1␊ |
24 | ␊ |
25 | #define BDA_TICK_COUNT 0x46CU␉␉// DWORD␊ |
26 | #define BDA_MIDNIGHT_FLAG 0x470U␉␉// BYTE␊ |
27 | #define BDA_24HR_TURNOVER 0x1800B0U␊ |
28 | ␊ |
29 | enum InterruptSources␊ |
30 | {␊ |
31 | ␉IS_Unknown = 0,␊ |
32 | ␉IS_APIC = 1,␉// Covers both LAPIC and IOAPIC␊ |
33 | ␉IS_PIC0 = 2,␊ |
34 | ␉IS_PIC1 = 3,␊ |
35 | ␉IS_Software = 4␊ |
36 | };␊ |
37 | ␊ |
38 | struct InterruptFrame␊ |
39 | {␊ |
40 | ␉uint32_t edx;␊ |
41 | ␉uint32_t ecx;␊ |
42 | ␉uint32_t eax;␊ |
43 | ␉uint32_t index;␊ |
44 | ␉uint32_t eip;␊ |
45 | ␉uint32_t cs;␊ |
46 | ␉uint32_t eflags;␊ |
47 | };␊ |
48 | ␊ |
49 | struct ExceptionFrame␊ |
50 | {␊ |
51 | ␉uint32_t edi;␊ |
52 | ␉uint32_t esi;␊ |
53 | ␉uint32_t ebp;␊ |
54 | ␉uint32_t esp;␊ |
55 | ␉uint32_t ebx;␊ |
56 | ␉uint32_t edx;␊ |
57 | ␉uint32_t ecx;␊ |
58 | ␉uint32_t eax;␊ |
59 | ␉uint32_t index;␊ |
60 | ␉uint32_t exception_code;␊ |
61 | ␉uint32_t eip;␊ |
62 | ␉uint32_t cs;␊ |
63 | ␉uint32_t eflags;␊ |
64 | };␊ |
65 | ␊ |
66 | struct InterruptGate␊ |
67 | {␊ |
68 | ␉uint16_t offset_0_15;␊ |
69 | ␉uint16_t selector;␊ |
70 | ␉uint16_t flags;␊ |
71 | ␉uint16_t offset_16_31;␊ |
72 | };␊ |
73 | ␊ |
74 | #pragma mark -␊ |
75 | #pragma mark Global Data␊ |
76 | #pragma mark -␊ |
77 | ␊ |
78 | extern␊ |
79 | uint16_t Idtr_prot[];␊ |
80 | ␊ |
81 | static␊ |
82 | uint32_t* counters = NULL;␊ |
83 | ␊ |
84 | static␊ |
85 | uint32_t lapic_base = 0U;␊ |
86 | ␊ |
87 | #pragma mark -␊ |
88 | #pragma mark Assembly Stubs␊ |
89 | #pragma mark -␊ |
90 | ␊ |
91 | static␊ |
92 | __attribute__((naked, noreturn))␊ |
93 | void InterruptStub(void)␊ |
94 | {␊ |
95 | ␉__asm__ volatile ("pushl %%eax\n\t"␊ |
96 | ␉␉␉␉␉ "pushl %%ecx\n\t"␊ |
97 | ␉␉␉␉␉ "pushl %%edx\n\t"␊ |
98 | ␉␉␉␉␉ "pushl %%esp\n\t"␊ |
99 | ␉␉␉␉␉ "calll _InterruptHandler\n\t"␊ |
100 | ␉␉␉␉␉ "addl $4, %%esp\n\t"␊ |
101 | ␉␉␉␉␉ "popl %%edx\n\t"␊ |
102 | ␉␉␉␉␉ "popl %%ecx\n\t"␊ |
103 | ␉␉␉␉␉ "popl %%eax\n\t"␊ |
104 | ␉␉␉␉␉ "addl $4, %%esp\n\t"␊ |
105 | ␉␉␉␉␉ "iretl"␊ |
106 | ␉␉␉␉␉ :);␊ |
107 | }␊ |
108 | ␊ |
109 | static␊ |
110 | __attribute__((naked, noreturn))␊ |
111 | void ExceptionWithCodeStub(void)␊ |
112 | {␊ |
113 | ␉__asm__ volatile("testl $-57, 8(%%esp)\n\t"␊ |
114 | ␉␉␉␉␉ "je 0f\n\t"␊ |
115 | ␉␉␉␉␉ "pushal\n\t"␊ |
116 | ␉␉␉␉␉ "jmp 1f\n"␊ |
117 | ␉␉␉␉␉ "_ExceptionNoCodeStub:\n"␊ |
118 | ␉␉␉␉␉ "0:\tsub $4, %%esp\n\t"␊ |
119 | ␉␉␉␉␉ "pushal\n\t"␊ |
120 | ␉␉␉␉␉ "xorl %%eax, %%eax\n\t"␊ |
121 | ␉␉␉␉␉ "xchgl %%eax, 36(%%esp)\n\t"␊ |
122 | ␉␉␉␉␉ "movl %%eax, 32(%%esp)\n"␊ |
123 | ␉␉␉␉␉ "1:\taddl $20, 12(%%esp)\n\t"␊ |
124 | ␉␉␉␉␉ "pushl %%esp\n\t"␊ |
125 | ␉␉␉␉␉ "calll _ExceptionHandler\n\t"␊ |
126 | ␉␉␉␉␉ "addl $4, %%esp\n\t"␊ |
127 | ␉␉␉␉␉ "popal\n\t"␊ |
128 | ␉␉␉␉␉ "addl $8, %%esp\n\t"␊ |
129 | ␉␉␉␉␉ "iretl"␊ |
130 | ␉␉␉␉␉ :);␊ |
131 | }␊ |
132 | ␊ |
133 | /*␊ |
134 | * Make _ExceptionNoCodeStub accessible to C␊ |
135 | */␊ |
136 | static␊ |
137 | __attribute__((naked, noreturn, weakref("ExceptionNoCodeStub")))␊ |
138 | void ExceptionNoCodeStubAlias(void);␊ |
139 | ␊ |
140 | static␊ |
141 | __attribute__((/* naked, */noinline, regparm(1), section("__INIT,__text")))␊ |
142 | void DispatchBiosVector(uint8_t vector)␊ |
143 | {␊ |
144 | ␉__asm__ volatile ("movb %0, 0f + 1\n\t"␊ |
145 | ␉␉␉␉␉ "calll __prot_to_real\n\t"␊ |
146 | ␉␉␉␉␉ ".code16\n"␊ |
147 | ␉␉␉␉␉ "0:\tint $0\n\t"␊ |
148 | ␉␉␉␉␉ "calll __real_to_prot\n\t"␊ |
149 | ␉␉␉␉␉ ".code32"␊ |
150 | ␉␉␉␉␉ : : "r"(vector));␊ |
151 | }␊ |
152 | ␊ |
153 | static␊ |
154 | __attribute__((noreturn, noinline, section("__INIT,__text")))␊ |
155 | void DisplayErrorAndStop(void)␊ |
156 | {␊ |
157 | ␉__asm__ volatile ("calll __prot_to_real\n\t"␊ |
158 | ␉␉␉␉␉ ".code16\n\t"␊ |
159 | ␉␉␉␉␉ "movw $2, %%ax\n\t"␊ |
160 | ␉␉␉␉␉ "int $0x10\n\t"␊ |
161 | ␉␉␉␉␉ "xorw %%ax, %%ax\n\t"␊ |
162 | ␉␉␉␉␉ "movw %%ax, %%ds\n\t"␊ |
163 | ␉␉␉␉␉ "movw $0x6000, %%si\n\t"␊ |
164 | ␉␉␉␉␉ "cld\n"␊ |
165 | ␉␉␉␉␉ "0:\tlodsb\n\t"␊ |
166 | ␉␉␉␉␉ "testb %%al, %%al\n\t"␊ |
167 | ␉␉␉␉␉ "je 1f\n\t"␊ |
168 | ␉␉␉␉␉ "movb $0xE, %%ah\n\t"␊ |
169 | ␉␉␉␉␉ "movw $0xFF, %%bx\n\t"␊ |
170 | ␉␉␉␉␉ "int $0x10\n\t"␊ |
171 | ␉␉␉␉␉ "jmp 0b\n"␊ |
172 | ␉␉␉␉␉ "1:\thlt\n\t"␊ |
173 | ␉␉␉␉␉ "jmp 1b\n\t"␊ |
174 | ␉␉␉␉␉ ".code32"␊ |
175 | ␉␉␉␉␉ :);␊ |
176 | ␉__builtin_unreachable();␊ |
177 | }␊ |
178 | ␊ |
179 | #pragma mark -␊ |
180 | #pragma mark Other Inline Assembly␊ |
181 | #pragma mark -␊ |
182 | ␊ |
183 | static inline␊ |
184 | uint32_t ReadLapic(uint32_t offset)␊ |
185 | {␊ |
186 | ␉return *(uint32_t const volatile*) (lapic_base + offset);␊ |
187 | }␊ |
188 | ␊ |
189 | static inline␊ |
190 | void WriteLapic(uint32_t offset, uint32_t value)␊ |
191 | {␊ |
192 | ␉*(uint32_t volatile*) (lapic_base + offset) = value;␊ |
193 | }␊ |
194 | ␊ |
195 | #define ChoosePicPort(pic, index) (pic == 0 ? (index == 0 ? 0x20U : 0x21U) : (index == 0 ? 0xA0U : 0xA1U))␊ |
196 | ␊ |
197 | #define ReadPic(pic, index) ({ \␊ |
198 | uint8_t value; \␊ |
199 | __asm__ volatile ("inb %1, %0" : "=a"(value) : "N"(ChoosePicPort(pic, index))); \␊ |
200 | value; \␊ |
201 | })␊ |
202 | ␊ |
203 | #define WritePic(pic, index, value) __asm__ volatile ("outb %0, %1" : : "a"((uint8_t)value), "N"(ChoosePicPort(pic, index)))␊ |
204 | ␊ |
205 | #pragma mark -␊ |
206 | #pragma mark Main Code␊ |
207 | #pragma mark -␊ |
208 | ␊ |
209 | static␊ |
210 | int IdentifyInterruptSource(uint8_t vector, uint32_t eip)␊ |
211 | {␊ |
212 | ␉if (lapic_base)␊ |
213 | ␉{␊ |
214 | ␉␉uint32_t value = ReadLapic(LAPIC_ISR_OFFSET + ((vector & 0xE0U) >> 1));␊ |
215 | ␉␉if (value & (1U << (vector & 31U)))␊ |
216 | ␉␉␉return IS_APIC;␊ |
217 | ␉}␊ |
218 | ␉if (vector >= 8U && vector < 16U)␊ |
219 | ␉{␊ |
220 | ␉␉uint8_t value;␊ |
221 | ␉␉WritePic(0, PIC_PORT0, PIC_READ_ISR_COMMAND);␊ |
222 | ␉␉value = ReadPic(0, PIC_PORT0);␊ |
223 | ␉␉if (value & (1U << (vector & 7U)))␊ |
224 | ␉␉␉return IS_PIC0;␊ |
225 | ␉}␊ |
226 | ␉if (vector >= 0x70U && vector < 0x78U)␊ |
227 | ␉{␊ |
228 | ␉␉uint8_t value;␊ |
229 | ␉␉WritePic(1, PIC_PORT0, PIC_READ_ISR_COMMAND);␊ |
230 | ␉␉value = ReadPic(1, PIC_PORT0);␊ |
231 | ␉␉if (value & (1U << (vector & 7U)))␊ |
232 | ␉␉␉return IS_PIC1;␊ |
233 | ␉}␊ |
234 | ␉if (eip)␊ |
235 | ␉{␊ |
236 | ␉␉uint8_t const volatile* pInstruction = (uint8_t const volatile*) (eip - 2U);␊ |
237 | ␉␉if ((*pInstruction) == 0xCDU && pInstruction[1] == vector)␊ |
238 | ␉␉␉return IS_Software;␊ |
239 | ␉␉/*␊ |
240 | ␉␉ * There are other software interrupt opcodes␊ |
241 | ␉␉ * debug breakpoint 0xCC␊ |
242 | ␉␉ * interrupt on overflow 0xCE␊ |
243 | ␉␉ * bound instruction 0x62␊ |
244 | ␉␉ * but those all trigger specific vectors, so are handled as exceptions.␊ |
245 | ␉␉ */␊ |
246 | ␉}␊ |
247 | ␉return IS_Unknown;␊ |
248 | }␊ |
249 | ␊ |
250 | static␊ |
251 | void SignalEOI(int source)␊ |
252 | {␊ |
253 | ␉switch (source)␊ |
254 | ␉{␊ |
255 | ␉␉case IS_APIC:␊ |
256 | ␉␉␉if (lapic_base)␊ |
257 | ␉␉␉␉WriteLapic(LAPIC_EOI_OFFSET, 0U);␊ |
258 | ␉␉␉break;␊ |
259 | ␉␉case IS_PIC1:␊ |
260 | ␉␉␉WritePic(1, PIC_PORT0, PIC_EOI_COMMAND);␊ |
261 | ␉␉case IS_PIC0:␊ |
262 | ␉␉␉WritePic(0, PIC_PORT0, PIC_EOI_COMMAND);␊ |
263 | ␉␉default:␊ |
264 | ␉␉␉break;␊ |
265 | ␉}␊ |
266 | }␊ |
267 | ␊ |
268 | static␊ |
269 | void HandleIRQ(int source, uint8_t vector)␊ |
270 | {␊ |
271 | ␉if (source == IS_PIC0 && vector == 8U)␊ |
272 | ␉{␊ |
273 | ␉␉uint32_t* pTickCount = (uint32_t*) BDA_TICK_COUNT;␊ |
274 | ␉␉if (++(*pTickCount) == BDA_24HR_TURNOVER)␊ |
275 | ␉␉{␊ |
276 | ␉␉␉*pTickCount = 0U;␊ |
277 | ␉␉␉++(*(uint8_t*)BDA_MIDNIGHT_FLAG);␊ |
278 | ␉␉}␊ |
279 | ␉␉SignalEOI(source);␊ |
280 | ␉␉return;␊ |
281 | ␉}␊ |
282 | ␉/*␊ |
283 | ␉ * Default Approach: send to bios␊ |
284 | ␉ */␊ |
285 | ␉DispatchBiosVector(vector);␊ |
286 | }␊ |
287 | ␊ |
288 | static␊ |
289 | __attribute__((used))␊ |
290 | void ExceptionHandler(struct ExceptionFrame* pFrame)␊ |
291 | {␊ |
292 | ␉uint8_t vector;␊ |
293 | ␉int interruptSource;␊ |
294 | ␉char* errorString;␊ |
295 | ␊ |
296 | ␉/*␊ |
297 | ␉ * FIXME: Should check if 0x10000U <= ESP <= 0x1FFFF0 here and switch stacks if not.␊ |
298 | ␉ */␊ |
299 | ␉if (!pFrame)␊ |
300 | ␉{␊ |
301 | ␉␉return;␊ |
302 | ␉}␊ |
303 | ␉vector = (uint8_t) pFrame->index;␊ |
304 | ␉if (counters)␊ |
305 | ␉␉++counters[vector];␊ |
306 | ␉interruptSource = IdentifyInterruptSource(vector, pFrame->eip);␊ |
307 | ␉switch (interruptSource)␊ |
308 | ␉{␊ |
309 | ␉␉case IS_APIC:␊ |
310 | ␉␉case IS_PIC0:␊ |
311 | ␉␉case IS_PIC1:␊ |
312 | ␉␉␉HandleIRQ(interruptSource, vector);␊ |
313 | ␉␉case IS_Software:␊ |
314 | ␉␉␉return;␊ |
315 | ␉␉default:␊ |
316 | ␉␉␉break;␊ |
317 | ␉}␊ |
318 | ␉errorString = (char*) 0x6000U;␊ |
319 | ␉switch (vector)␊ |
320 | ␉{␊ |
321 | ␉␉case 0U:␊ |
322 | ␉␉␉strcpy(errorString, "Division By Zero Exception");␊ |
323 | ␉␉␉break;␊ |
324 | ␉␉case 1U:␊ |
325 | ␉␉␉strcpy(errorString, "Debug Exception");␊ |
326 | ␉␉␉break;␊ |
327 | ␉␉case 2U:␊ |
328 | ␉␉␉strcpy(errorString, "NMI Interrupt");␊ |
329 | ␉␉␉break;␊ |
330 | ␉␉case 3U:␊ |
331 | ␉␉␉strcpy(errorString, "Debug Breakpoint");␊ |
332 | ␉␉␉break;␊ |
333 | ␉␉case 4U:␊ |
334 | ␉␉␉strcpy(errorString, "Overflow Exception");␊ |
335 | ␉␉␉break;␊ |
336 | ␉␉case 5U:␊ |
337 | ␉␉␉strcpy(errorString, "BOUND Range Exception");␊ |
338 | ␉␉␉break;␊ |
339 | ␉␉case 6U:␊ |
340 | ␉␉␉strcpy(errorString, "Invalid Opcode Exception");␊ |
341 | ␉␉␉break;␊ |
342 | ␉␉case 7U:␊ |
343 | ␉␉␉strcpy(errorString, "Math Coprocessor Unavailable Exception");␊ |
344 | ␉␉␉break;␊ |
345 | ␉␉case 8U:␊ |
346 | ␉␉␉strcpy(errorString, "Double Fault");␊ |
347 | ␉␉␉break;␊ |
348 | ␉␉case 9U:␊ |
349 | ␉␉␉strcpy(errorString, "Coprocessor Segment Overrun Exception");␊ |
350 | ␉␉␉break;␊ |
351 | ␉␉case 10U:␊ |
352 | ␉␉␉strcpy(errorString, "Invalid TSS Exception");␊ |
353 | ␉␉␉break;␊ |
354 | ␉␉case 11U:␊ |
355 | ␉␉␉strcpy(errorString, "Segment Not Present Exception");␊ |
356 | ␉␉␉break;␊ |
357 | ␉␉case 12U:␊ |
358 | ␉␉␉strcpy(errorString, "Stack-Segment Fault");␊ |
359 | ␉␉␉break;␊ |
360 | ␉␉case 13U:␊ |
361 | ␉␉␉strcpy(errorString, "General Protection Fault");␊ |
362 | ␉␉␉break;␊ |
363 | ␉␉case 14U:␊ |
364 | ␉␉␉strcpy(errorString, "Page Fault");␊ |
365 | ␉␉␉break;␊ |
366 | ␉␉case 16U:␊ |
367 | ␉␉␉strcpy(errorString, "x87 FPU Floating-Point Error");␊ |
368 | ␉␉␉break;␊ |
369 | ␉␉case 17U:␊ |
370 | ␉␉␉strcpy(errorString, "Alignment Check Exception");␊ |
371 | ␉␉␉break;␊ |
372 | ␉␉case 18U:␊ |
373 | ␉␉␉strcpy(errorString, "Machine Check Exception");␊ |
374 | ␉␉␉break;␊ |
375 | ␉␉case 19U:␊ |
376 | ␉␉␉strcpy(errorString, "SIMD Floating-Point Exception");␊ |
377 | ␉␉␉break;␊ |
378 | ␉␉case 20U:␊ |
379 | ␉␉␉strcpy(errorString, "Virtualization Exception");␊ |
380 | ␉␉␉break;␊ |
381 | ␉␉default:␊ |
382 | ␉␉␉sprintf(errorString, "Unknown Exception Vector %d", (int) vector);␊ |
383 | ␉␉␉break;␊ |
384 | ␉}␊ |
385 | ␉errorString += strlen(errorString);␊ |
386 | ␊ |
387 | ␉errorString += sprintf(errorString, "\r\nEDI 0x%x, ESI 0x%x, EBP 0x%x, ESP 0x%x",␊ |
388 | ␉␉␉␉␉␉ pFrame->edi, pFrame->esi, pFrame->ebp, pFrame->esp);␊ |
389 | ␉errorString += sprintf(errorString, "\r\nEBX 0x%x, EDX 0x%x, ECX 0x%x, EAX 0x%x",␊ |
390 | ␉␉␉␉␉␉ pFrame->ebx, pFrame->edx, pFrame->ecx, pFrame->eax);␊ |
391 | ␉errorString += sprintf(errorString, "\r\nException Code 0x%x, EIP 0x%x, CS 0x%x, EFLAGS 0x%x\r\nSystem Halted\r\n",␊ |
392 | ␉␉␉␉␉␉ pFrame->exception_code, pFrame->eip, pFrame->cs, pFrame->eflags);␊ |
393 | ␉DisplayErrorAndStop();␊ |
394 | }␊ |
395 | ␊ |
396 | static␊ |
397 | __attribute__((used))␊ |
398 | void InterruptHandler(struct InterruptFrame* pFrame)␊ |
399 | {␊ |
400 | ␉uint8_t vector;␊ |
401 | ␉int interruptSource;␊ |
402 | ␊ |
403 | ␉if (!pFrame)␊ |
404 | ␉{␊ |
405 | ␉␉return;␊ |
406 | ␉}␊ |
407 | ␉vector = (uint8_t) pFrame->index;␊ |
408 | ␉if (counters)␊ |
409 | ␉␉++counters[vector];␊ |
410 | ␉interruptSource = IdentifyInterruptSource(vector, pFrame->eip);␊ |
411 | ␉switch (interruptSource)␊ |
412 | ␉{␊ |
413 | ␉␉case IS_APIC:␊ |
414 | ␉␉case IS_PIC0:␊ |
415 | ␉␉case IS_PIC1:␊ |
416 | ␉␉␉HandleIRQ(interruptSource, vector);␊ |
417 | ␉␉default:␊ |
418 | ␉␉␉break;␊ |
419 | ␉}␊ |
420 | }␊ |
421 | ␊ |
422 | #if UNUSED␊ |
423 | void dumpMasks(void)␊ |
424 | {␊ |
425 | ␉int idx;␊ |
426 | ␉uint8_t port_val;␊ |
427 | ␉uint8_t volatile* apic_index;␊ |
428 | ␉uint32_t const volatile* apic_data;␊ |
429 | ␊ |
430 | ␉port_val = ReadPic(0, 1);␊ |
431 | ␉DBG("pic0 Masks 0x%x\n", port_val);␊ |
432 | ␉port_val = ReadPic(1, 1);␊ |
433 | ␉DBG("pic1 Masks 0x%x\n", port_val);␊ |
434 | ␉getchar();␊ |
435 | ␉DBG("IOAPIC vectors\n");␊ |
436 | ␉apic_index = (uint8_t volatile*) 0xFEC00000U;␊ |
437 | ␉apic_data = (uint32_t const volatile*) 0xFEC00010U;␊ |
438 | ␉for (idx = 0; idx != 24; ++idx)␊ |
439 | ␉{␊ |
440 | ␉␉uint32_t v1, v2;␊ |
441 | ␉␉*apic_index = (uint8_t) (16U + 2U * (unsigned) idx);␊ |
442 | ␉␉v1 = *apic_data;␊ |
443 | ␉␉if (v1 & 0x10000U)␊ |
444 | ␉␉␉continue;␊ |
445 | ␉␉*apic_index = (uint8_t) (16U + 2U * (unsigned) idx + 1U);␊ |
446 | ␉␉v2 = *apic_data;␊ |
447 | ␉␉DBG("index %d vector 0x%x%08x\n", idx, v2, v1);␊ |
448 | ␉}␊ |
449 | ␉getchar();␊ |
450 | ␉if (!lapic_base)␊ |
451 | ␉␉return;␊ |
452 | ␉DBG("LAPIC vectors\n");␊ |
453 | ␉for (idx = 0; idx != 7; ++idx)␊ |
454 | ␉{␊ |
455 | ␉␉uint32_t offs, v;␊ |
456 | ␉␉if (!idx)␊ |
457 | ␉␉␉offs = 0x2F0U;␊ |
458 | ␉␉else␊ |
459 | ␉␉␉offs = 0x320U + 16U * (unsigned) (idx - 1);␊ |
460 | ␉␉v = ReadLapic(offs);␊ |
461 | ␉␉if (v & 0x10000U)␊ |
462 | ␉␉␉continue;␊ |
463 | ␉␉DBG("index %d vector 0x%x\n", idx, v);␊ |
464 | ␉}␊ |
465 | }␊ |
466 | #endif␊ |
467 | ␊ |
468 | void ShowInterruptCounters(void)␊ |
469 | {␊ |
470 | ␉int j;␊ |
471 | ␊ |
472 | ␉if (!counters)␊ |
473 | ␉␉return;␊ |
474 | ␉msglog("Interrupt Counters\n");␊ |
475 | ␉for (j = 0; j != 256; ++j)␊ |
476 | ␉␉if (counters[j])␊ |
477 | ␉␉␉msglog("counters[%d] == %d\n", j, counters[j]);␊ |
478 | }␊ |
479 | ␊ |
480 | int SetupInterrupts(void)␊ |
481 | {␊ |
482 | ␉int idx;␊ |
483 | ␉uint32_t stub_address;␊ |
484 | ␉uint64_t ia32_apic_base;␊ |
485 | ␉size_t const total = 2048U + 2048U + 1024U;␊ |
486 | ␉uint8_t* workArea = (uint8_t*) malloc(total);␊ |
487 | ␉if (!workArea)␊ |
488 | ␉{␊ |
489 | ␉␉msglog("%s: Memory Allocation Failed\n", __FUNCTION__);␊ |
490 | ␉␉return 0;␊ |
491 | ␉}␊ |
492 | ␉counters = (uint32_t*) (workArea + 4096);␊ |
493 | ␉bzero(counters, 1024U);␊ |
494 | ␉for (idx = 0; idx != 256; ++idx)␊ |
495 | ␉{␊ |
496 | ␉␉struct InterruptGate* gate = (struct InterruptGate*) (workArea + idx * sizeof(struct InterruptGate));␊ |
497 | ␉␉uint8_t* thunk = workArea + 2048 + idx * 8;␊ |
498 | ␉␉gate->offset_0_15 = ((uint32_t) thunk) & 0xFFFFU;␊ |
499 | ␉␉gate->selector = CODE_SEGMENT_SELECTOR;␊ |
500 | ␉␉gate->flags = 0x8E00U; // Interrupt Gate, Present, DPL 0, 32-bit␊ |
501 | ␉␉gate->offset_16_31 = (((uint32_t) thunk) >> 16) & 0xFFFFU;␊ |
502 | ␉␉thunk[0] = 0x6AU;␉// push byte␊ |
503 | ␉␉thunk[1] = (uint8_t) idx;␊ |
504 | ␉␉thunk[2] = 0xE9U;␉// jmp rel32␊ |
505 | ␉␉if (idx >= 32)␊ |
506 | ␉␉␉stub_address = (uint32_t) &InterruptStub;␊ |
507 | ␉␉else if ((1U << idx) & CODED_EXCEPTION_MASK)␊ |
508 | ␉␉␉stub_address = (uint32_t) &ExceptionWithCodeStub;␊ |
509 | ␉␉else␊ |
510 | ␉␉␉stub_address = (uint32_t) &ExceptionNoCodeStubAlias;␊ |
511 | ␉␉*(uint32_t*) (&thunk[3]) = stub_address - (uint32_t) &thunk[7];␊ |
512 | ␉␉thunk[7] = 0x90U;␉// nop␊ |
513 | ␉}␊ |
514 | ␉Idtr_prot[0] = 0x7FFU;␊ |
515 | ␉Idtr_prot[1] = ((uint32_t) workArea) & 0xFFFFU;␊ |
516 | ␉Idtr_prot[2] = (((uint32_t) workArea) >> 16) & 0xFFFFU;␊ |
517 | ␉__asm__ volatile ("lidt %0" : : "m"(Idtr_prot[0]));␊ |
518 | ␉__asm__ volatile ("rdmsr" : "=A"(ia32_apic_base) : "c"((uint32_t) IA32_APIC_BASE));␊ |
519 | ␉if ((ia32_apic_base & LAPIC_WANTED_FLAGS) == LAPIC_WANTED_FLAGS &&␊ |
520 | ␉␉!((ia32_apic_base >> 32) & 255U))␊ |
521 | ␉␉lapic_base = ((uint32_t) ia32_apic_base) & ~0xFFFU;␊ |
522 | ␉DBG("%s: Work Area 0x%x, lapic_base 0x%x\n", __FUNCTION__, (uint32_t) workArea, lapic_base);␊ |
523 | ␉return 1;␊ |
524 | }␊ |
525 | #endif␊ |
526 |