fpu.c

来自「一个类似windows」· C语言 代码 · 共 605 行 · 第 1/2 页

        asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
        MxcsrFeatureMask = FxSaveArea->U.FxArea.MXCsrMask;
        if (MxcsrFeatureMask == 0)
        {
            MxcsrFeatureMask = 0x0000ffbf;
        }
    }
    /* FIXME: Check for SSE3 in Ke386CpuidFlags2! */
    if (Prcb->FeatureBits & (X86_FEATURE_SSE | X86_FEATURE_SSE2))
    {
        Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSXMMEXCPT);

        /* enable SSE */
        KeI386XMMIPresent = 1;
    }

    Ke386SetCr0(Ke386GetCr0() | X86_CR0_TS);
    Ke386RestoreFlags(Flags);
}


PFX_SAVE_AREA
KiGetFpuState(PKTHREAD Thread)
{
    PFX_SAVE_AREA FxSaveArea = NULL;
    KIRQL OldIrql;
    ULONG Cr0;

    KeRaiseIrql(DISPATCH_LEVEL, &OldIrql);
    if (Thread->NpxState & NPX_STATE_VALID)
    {
        FxSaveArea = (PFX_SAVE_AREA)((ULONG_PTR)Thread->InitialStack - sizeof (FX_SAVE_AREA));
        if (Thread->NpxState & NPX_STATE_DIRTY)
        {
            ASSERT(KeGetCurrentPrcb()->NpxThread == Thread);

            Cr0 = Ke386GetCr0();
            asm volatile("clts");
            if (KeI386FxsrPresent)
                asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
            else
            {
                asm volatile("fnsave %0" : : "m"(FxSaveArea->U.FnArea));
                /* FPU state has to be reloaded because fnsave changes it. */
                Cr0 |= X86_CR0_TS;
                KeGetCurrentPrcb()->NpxThread = NULL;
            }
            Ke386SetCr0(Cr0);
            Thread->NpxState = NPX_STATE_VALID;
        }
    }
    KeLowerIrql(OldIrql);

    return FxSaveArea;
}


NTSTATUS
KiHandleFpuFault(PKTRAP_FRAME Tf, ULONG ExceptionNr)
{
    if (ExceptionNr == 7) /* device not present */
    {
        BOOL FpuInitialized = FALSE;
        unsigned int cr0 = Ke386GetCr0();
        PKTHREAD CurrentThread;
        PFX_SAVE_AREA FxSaveArea;
        KIRQL oldIrql;
#ifndef CONFIG_SMP
        PKTHREAD NpxThread;
#endif

        (void) cr0;
        ASSERT((cr0 & X86_CR0_TS) == X86_CR0_TS);
        ASSERT((Tf->EFlags & X86_EFLAGS_VM) == 0);
        ASSERT((cr0 & X86_CR0_EM) == 0);

        /* disable scheduler, clear TS in cr0 */
        ASSERT_IRQL(DISPATCH_LEVEL);
        KeRaiseIrql(DISPATCH_LEVEL, &oldIrql);
        asm volatile("clts");

        CurrentThread = KeGetCurrentThread();
#ifndef CONFIG_SMP
        NpxThread = KeGetCurrentPrcb()->NpxThread;
#endif

        ASSERT(CurrentThread != NULL);
        DPRINT("Device not present exception happened! (Cr0 = 0x%x, NpxState = 0x%x)\n", cr0, CurrentThread->NpxState);

#ifndef CONFIG_SMP
        /* check if the current thread already owns the FPU */
        if (NpxThread != CurrentThread) /* FIXME: maybe this could be an assertation */
        {
            /* save the FPU state into the owner's save area */
            if (NpxThread != NULL)
            {
                KeGetCurrentPrcb()->NpxThread = NULL;
                FxSaveArea = (PFX_SAVE_AREA)((ULONG_PTR)NpxThread->InitialStack - sizeof (FX_SAVE_AREA));
                /* the fnsave might raise a delayed #MF exception */
                if (KeI386FxsrPresent)
                {
                    asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
                }
                else
                {
                    asm volatile("fnsave %0" : : "m"(FxSaveArea->U.FnArea));
                    FpuInitialized = TRUE;
                }
                NpxThread->NpxState = NPX_STATE_VALID;
            }
#endif /* !CONFIG_SMP */

            /* restore the state of the current thread */
            ASSERT((CurrentThread->NpxState & NPX_STATE_DIRTY) == 0);
            FxSaveArea = (PFX_SAVE_AREA)((ULONG_PTR)CurrentThread->InitialStack - sizeof (FX_SAVE_AREA));
            if (CurrentThread->NpxState & NPX_STATE_VALID)
            {
                if (KeI386FxsrPresent)
                {
                    FxSaveArea->U.FxArea.MXCsr &= MxcsrFeatureMask;
                    asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea));
                }
                else
                {
                    asm volatile("frstor %0" : : "m"(FxSaveArea->U.FnArea));
                }
            }
            else /* NpxState & NPX_STATE_INVALID */
            {
                DPRINT("Setting up clean FPU state\n");
                if (KeI386FxsrPresent)
                {
                    memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea));
                    FxSaveArea->U.FxArea.ControlWord = 0x037f;
                    if (KeI386XMMIPresent)
                    {
                        FxSaveArea->U.FxArea.MXCsr = 0x00001f80 & MxcsrFeatureMask;
                    }
                    asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea));
                }
                else if (!FpuInitialized)
                {
                    asm volatile("fninit");
                }
            }
            KeGetCurrentPrcb()->NpxThread = CurrentThread;
#ifndef CONFIG_SMP
        }
#endif

        CurrentThread->NpxState |= NPX_STATE_DIRTY;
        KeLowerIrql(oldIrql);
        DPRINT("Device not present exception handled!\n");

        return STATUS_SUCCESS;
    }
    else /* ExceptionNr == 16 || ExceptionNr == 19 */
    {
        EXCEPTION_RECORD Er;
        KPROCESSOR_MODE PreviousMode;
        PKTHREAD CurrentThread, NpxThread;
        KIRQL OldIrql;
        ULONG FpuEnvBuffer[7];
        PFNSAVE_FORMAT FpuEnv = (PFNSAVE_FORMAT)FpuEnvBuffer;

        ASSERT(ExceptionNr == 16 || ExceptionNr == 19); /* math fault or XMM fault*/

        KeRaiseIrql(DISPATCH_LEVEL, &OldIrql);

        NpxThread = KeGetCurrentPrcb()->NpxThread;
        CurrentThread = KeGetCurrentThread();
        if (NpxThread == NULL)
        {
            KeLowerIrql(OldIrql);
            DPRINT("Math/Xmm fault ignored! (NpxThread == NULL)\n");
            return STATUS_SUCCESS;
        }
        if (ExceptionNr == 16)
        {
            asm volatile("fnstenv %0" : : "m"(*FpuEnv));
            asm volatile("fldenv %0" : : "m"(*FpuEnv)); /* Stupid x87... */
            FpuEnv->StatusWord &= 0xffff;
        }
        KeLowerIrql(OldIrql);

        PreviousMode = ((Tf->SegCs & 0xffff) == (KGDT_R3_CODE | RPL_MASK)) ? (UserMode) : (KernelMode);
        DPRINT("Math/Xmm fault happened! (PreviousMode = %s)\n",
               (PreviousMode != KernelMode) ? ("UserMode") : ("KernelMode"));

        ASSERT(NpxThread == CurrentThread); /* FIXME: Is not always true I think */

        /* Get FPU/XMM state */
        KeLowerIrql(OldIrql);

        /* Determine exception code */
        if (ExceptionNr == 16)
        {
            DPRINT("FpuStatusWord = 0x%04x\n", FpuStatusWord);

            if (FpuEnv->StatusWord & X87_SW_IE)
                Er.ExceptionCode = STATUS_FLOAT_INVALID_OPERATION;
            else if (FpuEnv->StatusWord & X87_SW_DE)
                Er.ExceptionCode = STATUS_FLOAT_DENORMAL_OPERAND;
            else if (FpuEnv->StatusWord & X87_SW_ZE)
                Er.ExceptionCode = STATUS_FLOAT_DIVIDE_BY_ZERO;
            else if (FpuEnv->StatusWord & X87_SW_OE)
                Er.ExceptionCode = STATUS_FLOAT_OVERFLOW;
            else if (FpuEnv->StatusWord & X87_SW_UE)
                Er.ExceptionCode = STATUS_FLOAT_UNDERFLOW;
            else if (FpuEnv->StatusWord & X87_SW_PE)
                Er.ExceptionCode = STATUS_FLOAT_INEXACT_RESULT;
            else if (FpuEnv->StatusWord & X87_SW_SE)
                Er.ExceptionCode = STATUS_FLOAT_STACK_CHECK;
            else
                ASSERT(0); /* not reached */
            Er.ExceptionAddress = (PVOID)FpuEnv->ErrorOffset;
        }
        else /* ExceptionNr == 19 */
        {
            Er.ExceptionCode = STATUS_FLOAT_MULTIPLE_TRAPS;
            Er.ExceptionAddress = (PVOID)Tf->Eip;
        }

        Er.ExceptionFlags = 0;
        Er.ExceptionRecord = NULL;
        Er.NumberParameters = 0;

        /* Dispatch exception */
        DPRINT("Dispatching exception (ExceptionCode = 0x%08x)\n", Er.ExceptionCode);
        KiDispatchException(&Er, NULL, Tf, PreviousMode, TRUE);

        DPRINT("Math-fault handled!\n");
        return STATUS_SUCCESS;
    }

    return STATUS_UNSUCCESSFUL;
}


/* This is a rather naive implementation of Ke(Save/Restore)FloatingPointState
   which will not work for WDM drivers. Please feel free to improve */

NTSTATUS STDCALL
KeSaveFloatingPointState(OUT PKFLOATING_SAVE Save)
{
    PFNSAVE_FORMAT FpState;

    ASSERT_IRQL(DISPATCH_LEVEL);

    /* check if we are doing software emulation */
    if (!KeI386NpxPresent)
    {
        return STATUS_ILLEGAL_FLOAT_CONTEXT;
    }

    FpState = ExAllocatePool(NonPagedPool, sizeof (FNSAVE_FORMAT));
    if (NULL == FpState)
    {
        return STATUS_INSUFFICIENT_RESOURCES;
    }
    *((PVOID *) Save) = FpState;

#if defined(__GNUC__)
    asm volatile("fnsave %0\n\t" : "=m" (*FpState));
#elif defined(_MSC_VER)
    __asm mov eax, FpState;
    __asm fsave [eax];
#else
#error Unknown compiler for inline assembler
#endif

    KeGetCurrentThread()->DispatcherHeader.NpxIrql = KeGetCurrentIrql();

    return STATUS_SUCCESS;
}


NTSTATUS STDCALL
KeRestoreFloatingPointState(IN PKFLOATING_SAVE Save)
{
    PFNSAVE_FORMAT FpState = *((PVOID *) Save);

    if (KeGetCurrentThread()->DispatcherHeader.NpxIrql != KeGetCurrentIrql())
    {
        KEBUGCHECK(UNDEFINED_BUG_CODE);
    }

#if defined(__GNUC__)
    asm volatile("fnclex\n\t");
    asm volatile("frstor %0\n\t" : "=m" (*FpState));
#elif defined(_MSC_VER)
    __asm mov eax, FpState;
    __asm frstor [eax];
#else
#error Unknown compiler for inline assembler
#endif

    ExFreePool(FpState);

    return STATUS_SUCCESS;
}