ia32/exceptions: fix FPU exceptions

hal/ia32/_exceptions.S

@@ -30,14 +30,14 @@ exception_pushContext:
 	/* Save return address for ret */
 	movl (%esp), %eax
 	movl %eax, -(4 * CTXPUSHL)(%esp)
+	subl $FPU_CONTEXT_SIZE - 4, %esp
 	/* Check TS flag in CR0 register */
 	movl %cr0, %eax
 	andl $CR0_TS_BIT, %eax
-	subl $FPU_CONTEXT_SIZE - 4, %esp
 	xchgl FPU_CONTEXT_SIZE(%esp), %eax
 	jnz .exception_pushRegisters
 	/* Save FPU context */
-	fsave (%esp)
+	fnsave (%esp)
 .exception_pushRegisters:
 	pushw %ds
 	pushw %es
@@ -50,9 +50,7 @@ exception_pushContext:
 	pushl %ebp
 	pushl %esi
 	pushl %edi
-	movl %esp, %eax
-	subl $4, %eax
-	pushl %eax
+	pushl %esp
 
 	movl %dr7, %eax
 	pushl %eax
@@ -93,27 +91,24 @@ exception_popContext:
 	popl %edx
 	popl %ecx
 	popl %ebx
-	popl %eax
-	popw %gs
-	popw %fs
-	popw %es
-	popw %ds
 	/* FPU context */
-	testl $CR0_TS_BIT, FPU_CONTEXT_SIZE(%esp)
-	movl %eax, FPU_CONTEXT_SIZE(%esp)
+	testl $CR0_TS_BIT, (12 + FPU_CONTEXT_SIZE)(%esp)
+	jz .Linterrupts_popFPU
 	movl %cr0, %eax
-	jz .exception_popFPU
 	orl $CR0_TS_BIT, %eax
 	movl %eax, %cr0
-	addl $FPU_CONTEXT_SIZE, %esp
-	popl %eax
-	iret
-	.exception_popFPU:
-	andl $~CR0_TS_BIT, %eax
-	mov %eax, %cr0
-	frstor (%esp)
-	addl $FPU_CONTEXT_SIZE, %esp
+	jmp .Linterrupts_popFPUend
+.Linterrupts_popFPU:
+	clts
+	frstor 12(%esp)
+.Linterrupts_popFPUend:
 	popl %eax
+	popw %gs
+	popw %fs
+	popw %es
+	popw %ds
+	/* Skip over FPU context and cr0Bits. */
+	addl $FPU_CONTEXT_SIZE + 4, %esp
 	iret
 .size exception_popContext, .-exception_popContext
 
@@ -131,7 +126,7 @@ exceptions_exc7_handler:
 	movl %ecx, 176(%eax)
 	/* Init FPU */
 	fninit
-	fsave 68(%eax) /* Set ctx->fpuContext */
+	fnsave 68(%eax) /* Set ctx->fpuContext */
 	ret
 
 .size exceptions_exc7_handler, .-exceptions_exc7_handler
@@ -146,7 +141,6 @@ sym:
 
 /* Exception handling macro */
 #define EXCSTUB(exc)\
-	cli;\
 	call exception_pushContext;\
 	movl $SEL_KDATA, %eax;\
 	movw %ax, %ds;\
@@ -162,8 +156,7 @@ sym:
 	call *%eax;\
 	addl $8, %esp;\
 	/* cpu_context_t */;\
-	movl %esp, %eax;\
-	addl $28, %eax;\
+	leal 28(%esp), %eax;\
 	pushl %eax;\
 	call hal_cpuSupervisorMode;\
 	addl $4, %esp;\

hal/ia32/_interrupts.S

@@ -57,18 +57,17 @@ hal_lockScheduler:
 .global interrupts_pushContext
 .align 4, 0x90
 interrupts_pushContext:
-	xchgl (%esp), %edx
-	movl %edx, -(4 * CTXPUSHL)(%esp)
-	popl %edx
+	/* Save return address. */
+	xchgl (%esp), %eax
+	movl %eax, -(4 * CTXPUSHL)(%esp)
+	subl $FPU_CONTEXT_SIZE, %esp
 	/* Check TS flag in CR0 register */
-	pushl %eax
 	movl %cr0, %eax
-	subl $FPU_CONTEXT_SIZE, %esp
 	andl $CR0_TS_BIT, %eax
 	xchgl FPU_CONTEXT_SIZE(%esp), %eax
 	jnz .Linterrupts_pushRegisters
 	/* Save FPU context */
-	fsave (%esp)
+	fnsave (%esp)
 .Linterrupts_pushRegisters:
 	pushw %ds
 	pushw %es
@@ -108,27 +107,24 @@ interrupts_popContext:
 	popl %edx
 	popl %ecx
 	popl %ebx
-	popl %eax
-	popw %gs
-	popw %fs
-	popw %es
-	popw %ds
 	/* FPU context */
-	testl $CR0_TS_BIT, FPU_CONTEXT_SIZE(%esp)
-	movl %eax, FPU_CONTEXT_SIZE(%esp)
-	movl %cr0, %eax
+	testl $CR0_TS_BIT, (12 + FPU_CONTEXT_SIZE)(%esp)
 	jz .Linterrupts_popFPU
+	movl %cr0, %eax
 	orl $CR0_TS_BIT, %eax
 	movl %eax, %cr0
-	addl $FPU_CONTEXT_SIZE, %esp
-	popl %eax
-	iret
+	jmp .Linterrupts_popFPUend
 .Linterrupts_popFPU:
-	andl $~CR0_TS_BIT, %eax
-	mov %eax, %cr0
-	frstor (%esp)
-	addl $FPU_CONTEXT_SIZE, %esp
+	clts
+	frstor 12(%esp)
+.Linterrupts_popFPUend:
 	popl %eax
+	popw %gs
+	popw %fs
+	popw %es
+	popw %ds
+	/* Skip over FPU context and cr0Bits. */
+	addl $FPU_CONTEXT_SIZE + 4, %esp
 	iret
 interrupts_popContextUnlocked:
 	popl %esp

hal/ia32/interrupts.c

@@ -493,7 +493,6 @@ void _hal_interruptsInit(void)
 	}
 
 	/* Set stub for syscall */
-/*	_interrupts_setIDTEntry(0x80, _interrupts_syscall, IGBITS_TRAP); */
 	_interrupts_setIDTEntry(SYSCALL_IRQ, _interrupts_syscall, flags | IGBITS_DPL3);
 	_interrupts_setIDTEntry(TLB_IRQ, _interrupts_TLBShootdown, flags);