VFP bug fixes for Raspberry Pi (which has VFPv2).

We weren't being quite careful enough about the structure alignment part
of CurrentThread.FpuContext. Some areas honored it, others didn't. This
messed up ARMv6 where flags have to be set and stay set in the FPSCR. Also
figured out exactly what those flags are.

apps/libc/dynamic/armv7/fenvc.c

@@ -141,7 +141,7 @@ Return Value:
 
     Mask &= FE_ALL_EXCEPT;
     fegetenv(&Environment);
-    Environment.Fpscr &= Mask;
+    Environment.Fpscr |= Mask;
     return fesetenv(&Environment);
 }
 

include/minoca/kernel/arm.h

@@ -468,6 +468,7 @@ Author:
 //
 
 #define ARM_FPSCR_FLUSH_TO_ZERO (1 << 24)
+#define ARM_FPSCR_DEFAULT_NAN (1 << 25)
 
 //
 // Define the required alignment for FPU context.
@@ -2505,6 +2506,48 @@ Return Value:
 
 --*/
 
+ULONG
+ArGetVfpInstructionRegister (
+    VOID
+    );
+
+/*++
+
+Routine Description:
+
+    This routine gets the floating point instruction register (FPINST).
+
+Arguments:
+
+    None.
+
+Return Value:
+
+    Returns the contents of the register.
+
+--*/
+
+ULONG
+ArGetFpscr (
+    VOID
+    );
+
+/*++
+
+Routine Description:
+
+    This routine gets the floating point status and control register (FPSCR).
+
+Arguments:
+
+    None.
+
+Return Value:
+
+    Returns the contents of the register.
+
+--*/
+
 VOID
 ArSaveVfp (
     PFPU_CONTEXT Context,

include/minoca/kernel/ps.h

@@ -417,10 +417,6 @@ Author:
 #define THREAD_FPU_FLAG_IN_USE      0x0001
 #define THREAD_FPU_FLAG_OWNER       0x0002
 
-#define THREAD_FLAG_HAS_FPU2 0x00010000
-#define THREAD_FLAG_FPU_DISABLED 0x00020000
-#define THREAD_FLAG_FPU_ENABLED 0x00040000
-
 //
 // Define the set of thread flags that can be specified on creation. This is
 // also the set of flags that will propagate when a thread is copied.

kernel/armv7/commsup.S

@@ -2040,6 +2040,64 @@ FUNCTION ArSetVfpExceptionRegister
 
 END_FUNCTION ArSetVfpExceptionRegister
 
+##
+## ULONG
+## ArGetVfpInstructionRegister (
+##     VOID
+##     )
+##
+
+/*++
+
+Routine Description:
+
+    This routine gets the floating point instruction register (FPINST).
+
+Arguments:
+
+    None.
+
+Return Value:
+
+    Returns the contents of the register.
+
+--*/
+
+FUNCTION ArGetVfpInstructionRegister
+    mrc     p10, 7, %r0, c9, c0, 0
+    bx      %lr
+
+END_FUNCTION ArGetVfpInstructionRegister
+
+##
+## ULONG
+## ArGetFpscr (
+##     VOID
+##     )
+##
+
+/*++
+
+Routine Description:
+
+    This routine gets the floating point status and control register (FPSCR).
+
+Arguments:
+
+    None.
+
+Return Value:
+
+    Returns the contents of the register.
+
+--*/
+
+FUNCTION ArGetFpscr
+    vmrs    %r0, FPSCR                  @ Get FPSCR.
+    bx      %lr
+
+END_FUNCTION ArGetFpscr
+
 ##
 ## VOID
 ## ArSaveVfp (

kernel/armv7/vfp.c

@@ -63,6 +63,12 @@ ArpHandleVfpException (
     PTRAP_FRAME TrapFrame
     );
 
+VOID
+ArpRestoreFpuState (
+    PFPU_CONTEXT Context,
+    BOOL SimdSupport
+    );
+
 BOOL
 ArpDummyVfpExceptionHandler (
     PTRAP_FRAME TrapFrame
@@ -199,7 +205,12 @@ Return Value:
 
 {
 
-    ArSaveVfp(Buffer, ArVfpRegisters32);
+    PFPU_CONTEXT AlignedContext;
+
+    AlignedContext = (PVOID)(UINTN)ALIGN_RANGE_UP((UINTN)Buffer,
+                                                  FPU_CONTEXT_ALIGNMENT);
+
+    ArSaveVfp(AlignedContext, ArVfpRegisters32);
     return;
 }
 
@@ -305,11 +316,7 @@ Return Value:
 
 {
 
-    ULONG Control;
-
-    Control = ArGetVfpExceptionRegister();
-    Control &= ~ARM_FPEXC_ENABLE;
-    ArSetVfpExceptionRegister(Control);
+    ArSetVfpExceptionRegister(0);
     return;
 }
 
@@ -363,7 +370,8 @@ Return Value:
 
     UserSharedData = MmGetUserSharedData();
     if ((UserSharedData->ProcessorFeatures & ARM_FEATURE_VFP3) == 0) {
-        ContextStructure->Fpscr |= ARM_FPSCR_FLUSH_TO_ZERO;
+        ContextStructure->Fpscr = ARM_FPSCR_FLUSH_TO_ZERO |
+                                  ARM_FPSCR_DEFAULT_NAN;
     }
 
     return Context;
@@ -426,11 +434,13 @@ Return Value:
 {
 
     ULONG Control;
+    BOOL Handled;
     RUNLEVEL OldRunLevel;
     PKTHREAD Thread;
 
     ASSERT(ArAreInterruptsEnabled() != FALSE);
 
+    Handled = FALSE;
     Thread = KeGetCurrentThread();
 
     //
@@ -467,7 +477,21 @@ Return Value:
 
     Control = ArGetVfpExceptionRegister();
     if ((Control & ARM_FPEXC_ENABLE) != 0) {
-        return FALSE;
+        if ((Control & ARM_FPEXC_EXCEPTION) != 0) {
+            RtlDebugPrint("VFP Exception: %x\n", Control);
+
+        } else {
+            RtlDebugPrint("Unsupported VFP instruction.\n");
+        }
+
+        RtlDebugPrint("FPINST %x FPSCR %x\n",
+                      ArGetVfpInstructionRegister(),
+                      ArGetFpscr());
+
+        Control &= ~ARM_FPEXC_EXCEPTION;
+        ArSetVfpExceptionRegister(Control);
+        KeLowerRunLevel(OldRunLevel);
+        goto HandleVfpExceptionEnd;
     }
 
     Control |= ARM_FPEXC_ENABLE;
@@ -479,14 +503,52 @@ Return Value:
     //
 
     if ((Thread->FpuFlags & THREAD_FPU_FLAG_OWNER) == 0) {
-        ArRestoreVfp(Thread->FpuContext, ArVfpRegisters32);
+        ArpRestoreFpuState(Thread->FpuContext, ArVfpRegisters32);
     }
 
     Thread->FpuFlags |= THREAD_FPU_FLAG_OWNER | THREAD_FPU_FLAG_IN_USE;
     KeLowerRunLevel(OldRunLevel);
+    Handled = TRUE;
 
 HandleVfpExceptionEnd:
-    return TRUE;
+    return Handled;
+}
+
+VOID
+ArpRestoreFpuState (
+    PFPU_CONTEXT Context,
+    BOOL SimdSupport
+    )
+
+/*++
+
+Routine Description:
+
+    This routine restores the Vector Floating Point unit state into the
+    hardware.
+
+Arguments:
+
+    Context - Supplies a pointer to the context to restore.
+
+    SimdSupport - Supplies a boolean indicating whether the VFP unit contains
+        32 64-bit registers (TRUE) or 16 64-bit registers (FALSE).
+
+Return Value:
+
+    None.
+
+--*/
+
+{
+
+    PFPU_CONTEXT AlignedContext;
+
+    AlignedContext = (PVOID)(UINTN)ALIGN_RANGE_UP((UINTN)Context,
+                                                  FPU_CONTEXT_ALIGNMENT);
+
+    ArRestoreVfp(AlignedContext, SimdSupport);
+    return;
 }
 
 BOOL

kernel/ke/armv7/ctxswapc.c

@@ -117,8 +117,8 @@ Return Value:
             }
         }
 
-        ArDisableFpu();
         CurrentThread->FpuFlags &= ~THREAD_FPU_FLAG_OWNER;
+        ArDisableFpu();
     }
 
     return;