3 * Copyright (C) 2007 Tomas 'ZeXx86' Jedrzejek (zexx86@zexos.org)
5 * This program is free software: you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation, either version 3 of the License, or
8 * (at your option) any later version.
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this program. If not, see <http://www.gnu.org/licenses/>.
24 #define KRNL_CODE_SEL 0x18
27 extern unsigned char g_tss_iopb
[];
28 extern unsigned char g_tss_end
[];
29 extern unsigned g_kvirt_to_phys
;
31 /*****************************************************************************
32 Sets up V86 mode stack and initializes some registers
33 *****************************************************************************/
34 void init_v86_regs(uregs_t
*regs
)
36 /* V86 mode stack. This MUST be in conventional memory (below 1 meg)
37 but still be accessible to the pmode kernel: */
38 static char _v86_stack
[4096];
43 /* start with all registers zeroed */
44 memset(regs
, 0, sizeof(uregs_t
));
45 /* point to BOTTOM (highest addess) of stack */
46 v86_stack
= _v86_stack
+ sizeof(_v86_stack
);
47 /* v86_stack is virtual address. Convert it to physical address
48 and align it so bottom 4 bits == 0x0F
49 's' must be >= 0x10000 so we can set ESP=0xFFFF, below. */
50 s
= ((unsigned)v86_stack
+ g_kvirt_to_phys
- 16) | 0x0F;
52 regs
->user_esp
= 0xFFFF;
53 regs
->user_ss
= (s
- 0xFFFF) / 16;
54 /* init kernel data segment registers */
55 regs
->ds
= regs
->es
= regs
->fs
= regs
->gs
= KRNL_CODE_SEL
;
56 /* init EFLAGS: set RF=0, NT=0, IF=0, and reserved bits */
58 regs
->eflags
= 0x00020002L
; /* VM=1, IOPL=0 */
60 regs
->eflags
= 0x00023002L
; /* VM=1, IOPL=3 */
64 /*****************************************************************************
65 *****************************************************************************/
66 static unsigned long to_linear(unsigned seg
, unsigned off
)
68 return (seg
& 0xFFFF) * 16L + off
;
70 /*****************************************************************************
71 *****************************************************************************/
72 unsigned peekb(unsigned seg
, unsigned off
)
74 return *(unsigned int *)(to_linear(seg
, off
) - g_kvirt_to_phys
);
76 /*****************************************************************************
77 *****************************************************************************/
78 static unsigned peekw(unsigned seg
, unsigned off
)
80 return *(unsigned short *)(to_linear(seg
, off
) - g_kvirt_to_phys
);
82 /*****************************************************************************
83 *****************************************************************************/
84 static unsigned long peekl(unsigned seg
, unsigned off
)
86 return *(unsigned int *)(to_linear(seg
, off
) - g_kvirt_to_phys
);
88 /*****************************************************************************
89 *****************************************************************************/
90 void pokeb(unsigned seg
, unsigned off
, unsigned val
)
92 *(unsigned int *)(to_linear(seg
, off
) - g_kvirt_to_phys
) = val
;
94 /*****************************************************************************
95 *****************************************************************************/
96 void pokew2 (unsigned seg
, unsigned off
, unsigned val
)
98 *(unsigned short *)(to_linear(seg
, off
) - g_kvirt_to_phys
) = val
;
100 /*****************************************************************************
101 *****************************************************************************/
102 static void pokel(unsigned seg
, unsigned off
, unsigned long val
)
104 *(unsigned int *)(to_linear(seg
, off
) - g_kvirt_to_phys
) = val
;
106 /*****************************************************************************
107 *****************************************************************************/
108 static void v86_enable_port(unsigned port
)
112 mask
= 0x01 << (port
& 7);
114 g_tss_iopb
[port
] &= ~mask
;
116 /*****************************************************************************
117 xxx - GPF if EIP > 0xFFFF ?
118 *****************************************************************************/
119 static unsigned v86_fetch8(uregs_t
*regs
)
123 byte
= peekb(regs
->cs
, regs
->eip
);
124 regs
->eip
= (regs
->eip
+ 1) & 0xFFFF;
127 /*****************************************************************************
128 xxx - next four functions should fault (stack fault; exception 0Ch)
129 if stack straddles 0xFFFF
130 *****************************************************************************/
131 void v86_push16(uregs_t
*regs
, unsigned value
)
133 regs
->user_esp
= (regs
->user_esp
- 2) & 0xFFFF;
134 pokew2(regs
->user_ss
, regs
->user_esp
, value
);
136 /*****************************************************************************
137 *****************************************************************************/
138 static unsigned v86_pop16(uregs_t
*regs
)
142 rv
= peekw(regs
->user_ss
, regs
->user_esp
);
143 regs
->user_esp
= (regs
->user_esp
+ 2) & 0xFFFF;
146 /*****************************************************************************
147 *****************************************************************************/
148 static void v86_push32(uregs_t
*regs
, unsigned long value
)
150 regs
->user_esp
= (regs
->user_esp
- 4) & 0xFFFF;
151 pokel(regs
->user_ss
, regs
->user_esp
, value
);
153 /*****************************************************************************
154 *****************************************************************************/
155 static unsigned long v86_pop32(uregs_t
*regs
)
159 rv
= peekl(regs
->user_ss
, regs
->user_esp
);
160 regs
->user_esp
= (regs
->user_esp
+ 4) & 0xFFFF;
163 /*****************************************************************************
164 *****************************************************************************/
165 void v86_int(uregs_t
*regs
, unsigned int_num
)
167 /* push return IP, CS, and FLAGS onto V86 mode stack */
168 v86_push16(regs
, regs
->eflags
);
169 v86_push16(regs
, regs
->cs
);
170 v86_push16(regs
, regs
->eip
);
171 /* disable interrupts */
172 regs
->eflags
&= ~0x200;
173 /* load new CS and IP from IVT */
175 regs
->eip
= (regs
->eip
& ~0xFFFF) | peekw(0, int_num
+ 0);
176 regs
->cs
= peekw(0, int_num
+ 2);
179 /*****************************************************************************
180 Call real-mode interrupt handler in V86 mode
181 *****************************************************************************/
182 void do_v86_int(uregs_t
*regs
, unsigned int_num
)
186 /* convert int_num to IVT index */
187 ivt_off
= (int_num
& 0xFF) * 4;
188 /* fetch CS:IP of real-mode interrupt handler */
189 regs
->cs
= peekw(0, ivt_off
+ 2);
190 regs
->eip
= peekw(0, ivt_off
+ 0);
196 /*****************************************************************************
197 *****************************************************************************/
205 #define PFX_OP32 0x040
206 #define PFX_ADR32 0x080
207 #define PFX_LOCK 0x100
208 #define PFX_REPNE 0x200
209 #define PFX_REP 0x400
211 int v86_emulate(uregs_t
*regs
)
213 unsigned init_eip
, prefix
, i
;
215 /* save current EIP so we can re-try instructions
216 instead of skipping over or emulating them */
217 init_eip
= regs
->eip
;
218 /* consume prefix bytes */
222 i
= v86_fetch8(regs
);
267 if(prefix
& PFX_OP32
)
268 v86_push32(regs
, regs
->eflags
);
270 v86_push16(regs
, regs
->eflags
);
274 if(prefix
& PFX_OP32
)
276 if(regs
->user_esp
> 0xFFFC)
278 regs
->eflags
= v86_pop32(regs
);
282 if(regs
->user_esp
> 0xFFFE)
284 /* tarnation! regs->eflags = v86_pop16(regs); */
285 regs
->eflags
= (regs
->eflags
& 0xFFFF0000L
) |
291 i
= v86_fetch8(regs
); /* get interrupt number */
296 /* pop (E)IP, CS, (E)FLAGS */
297 if(prefix
& PFX_OP32
)
299 if(regs
->user_esp
> 0xFFF4)
301 regs
->eip
= v86_pop32(regs
);
302 regs
->cs
= v86_pop32(regs
);
303 regs
->eflags
= v86_pop32(regs
);
307 if(regs
->user_esp
> 0xFFFA)
309 regs
->eip
= v86_pop16(regs
);
310 regs
->cs
= v86_pop16(regs
);
311 regs
->eflags
= (regs
->eflags
& 0xFFFF0000L
) |
315 /************************************
316 I/O functions are not (yet) emulated. We just enable the
317 appropriate port in the IOPB and retry the instruction.
318 **************************************/
323 i
= v86_fetch8(regs
);
325 /* restore original EIP -- we will re-try the instruction */
326 regs
->eip
= init_eip
;
332 i
= v86_fetch8(regs
);
334 v86_enable_port(i
+ 1);
335 if(prefix
& PFX_OP32
)
337 v86_enable_port(i
+ 2);
338 v86_enable_port(i
+ 3);
340 regs
->eip
= init_eip
;
350 i
= regs
->edx
& 0xFFFF;
352 regs
->eip
= init_eip
;
362 i
= regs
->edx
& 0xFFFF;
364 v86_enable_port(i
+ 1);
365 if(prefix
& PFX_OP32
)
367 v86_enable_port(i
+ 2);
368 v86_enable_port(i
+ 3);
370 regs
->eip
= init_eip
;
374 regs
->eflags
&= ~0x200;
378 regs
->eflags
|= 0x200;
382 kprintf("Error in V86 mode at CS:IP=%04X:%04X\n",
384 kprintf("Dump of bytes at CS:EIP:\n");
385 //dump((void *)(to_linear(regs->cs, init_eip) - g_kvirt_to_phys), 16);