fpu.c

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

/*
 * COPYRIGHT:       See COPYING in the top level directory
 * PROJECT:         ReactOS kernel
 * FILE:            ntoskrnl/ke/i386/fpu.c
 * PURPOSE:         Handles the FPU
 * PROGRAMMERS:     David Welch ([e-mail])
 *                  Gregor Anich
 */

/* INCLUDES *****************************************************************/

#include <roscfg.h>
#include <ntoskrnl.h>
#define NDEBUG
#include <internal/debug.h>

/* DEFINES *******************************************************************/

/* x87 Status Word exception flags */
#define X87_SW_IE       (1<<0)   /* Invalid Operation */
#define X87_SW_DE       (1<<1)   /* Denormalized Operand */
#define X87_SW_ZE       (1<<2)   /* Zero Devide */
#define X87_SW_OE       (1<<3)   /* Overflow */
#define X87_SW_UE       (1<<4)   /* Underflow */
#define X87_SW_PE       (1<<5)   /* Precision */
#define X87_SW_SE       (1<<6)   /* Stack Fault */

#define X87_SW_ES       (1<<7)   /* Error Summary */

/* MXCSR exception flags */
#define MXCSR_IE        (1<<0)   /* Invalid Operation */
#define MXCSR_DE        (1<<1)   /* Denormalized Operand */
#define MXCSR_ZE        (1<<2)   /* Zero Devide */
#define MXCSR_OE        (1<<3)   /* Overflow */
#define MXCSR_UE        (1<<4)   /* Underflow */
#define MXCSR_PE        (1<<5)   /* Precision */
#define MXCSR_DAZ       (1<<6)   /* Denormals Are Zeros (P4 only) */

/* GLOBALS *******************************************************************/

extern ULONG KeI386NpxPresent;
extern ULONG KeI386XMMIPresent;
extern ULONG KeI386FxsrPresent;

static ULONG MxcsrFeatureMask = 0;

/* FUNCTIONS *****************************************************************/

STATIC USHORT
KiTagWordFnsaveToFxsave(USHORT TagWord)
{
    INT tmp;

    /*
     * Converts the tag-word. 11 (Empty) is converted into 0, everything else into 1
     */
    tmp = ~TagWord; /* Empty is now 00, any 2 bits containing 1 mean valid */
    tmp = (tmp | (tmp >> 1)) & 0x5555; /* 0V0V0V0V0V0V0V0V */
    tmp = (tmp | (tmp >> 1)) & 0x3333; /* 00VV00VV00VV00VV */
    tmp = (tmp | (tmp >> 2)) & 0x0f0f; /* 0000VVVV0000VVVV */
    tmp = (tmp | (tmp >> 4)) & 0x00ff; /* 00000000VVVVVVVV */

    return tmp;
}


STATIC USHORT
KiTagWordFxsaveToFnsave(PFXSAVE_FORMAT FxSave)
{
    USHORT TagWord = 0;
    UCHAR Tag;
    INT i;
    struct FPREG { USHORT Significand[4]; USHORT Exponent; } *FpReg;

    for (i = 0; i < 8; i++)
    {
        if (FxSave->TagWord & (1 << i)) /* valid */
        {
            FpReg = (struct FPREG *)(FxSave->RegisterArea + (i * 16));
            switch (FpReg->Exponent & 0x00007fff)
            {
            case 0x0000:
                if (FpReg->Significand[0] == 0 && FpReg->Significand[1] == 0 &&
                    FpReg->Significand[2] == 0 && FpReg->Significand[3] == 0)
                    Tag = 1;  /* Zero */
                else
                    Tag = 2;  /* Special */
                break;

            case 0x7fff:
                Tag = 2;      /* Special */
                break;

            default:
                if (FpReg->Significand[3] & 0x00008000)
                    Tag = 0;  /* Valid */
                else
                    Tag = 2;  /* Special */
                break;
            }
        }
        else /* empty */
        {
           Tag = 3;
        }
        TagWord |= Tag << (i * 2);
    }

    return TagWord;
}


STATIC VOID
KiFnsaveToFxsaveFormat(PFXSAVE_FORMAT FxSave, PFNSAVE_FORMAT FnSave)
{
    INT i;

    FxSave->ControlWord = (USHORT)FnSave->ControlWord;
    FxSave->StatusWord = (USHORT)FnSave->StatusWord;
    FxSave->TagWord = KiTagWordFnsaveToFxsave((USHORT)FnSave->TagWord);
    FxSave->ErrorOpcode = (USHORT)(FnSave->ErrorSelector >> 16);
    FxSave->ErrorOffset = FnSave->ErrorOffset;
    FxSave->ErrorSelector = FnSave->ErrorSelector & 0x0000ffff;
    FxSave->DataOffset = FnSave->DataOffset;
    FxSave->DataSelector = FnSave->DataSelector & 0x0000ffff;
    if (KeI386XMMIPresent)
        FxSave->MXCsr = 0x00001f80 & MxcsrFeatureMask;
    else
        FxSave->MXCsr = 0;
    FxSave->MXCsrMask = MxcsrFeatureMask;
    memset(FxSave->Reserved3, 0, sizeof(FxSave->Reserved3) +
           sizeof(FxSave->Reserved4)); /* Don't zero Align16Byte because Context->ExtendedRegisters
                                          is only 512 bytes, not 520 */
    for (i = 0; i < 8; i++)
    {
        memcpy(FxSave->RegisterArea + (i * 16), FnSave->RegisterArea + (i * 10), 10);
        memset(FxSave->RegisterArea + (i * 16) + 10, 0, 6);
    }
}

STATIC VOID
KiFxsaveToFnsaveFormat(PFNSAVE_FORMAT FnSave, PFXSAVE_FORMAT FxSave)
{
    INT i;

    FnSave->ControlWord = 0xffff0000 | FxSave->ControlWord;
    FnSave->StatusWord = 0xffff0000 | FxSave->StatusWord;
    FnSave->TagWord = 0xffff0000 | KiTagWordFxsaveToFnsave(FxSave);
    FnSave->ErrorOffset = FxSave->ErrorOffset;
    FnSave->ErrorSelector = FxSave->ErrorSelector & 0x0000ffff;
    FnSave->ErrorSelector |= FxSave->ErrorOpcode << 16;
    FnSave->DataOffset = FxSave->DataOffset;
    FnSave->DataSelector = FxSave->DataSelector | 0xffff0000;
    for (i = 0; i < 8; i++)
    {
        memcpy(FnSave->RegisterArea + (i * 10), FxSave->RegisterArea + (i * 16), 10);
    }
}


STATIC VOID
KiFloatingSaveAreaToFxSaveArea(PFX_SAVE_AREA FxSaveArea, FLOATING_SAVE_AREA *FloatingSaveArea)
{
    if (KeI386FxsrPresent)
    {
        KiFnsaveToFxsaveFormat(&FxSaveArea->U.FxArea, (PFNSAVE_FORMAT)FloatingSaveArea);
    }
    else
    {
        memcpy(&FxSaveArea->U.FnArea, FloatingSaveArea, sizeof(FxSaveArea->U.FnArea));
    }
    FxSaveArea->NpxSavedCpu = 0;
    FxSaveArea->Cr0NpxState = FloatingSaveArea->Cr0NpxState;
}


VOID
KiFxSaveAreaToFloatingSaveArea(FLOATING_SAVE_AREA *FloatingSaveArea, CONST PFX_SAVE_AREA FxSaveArea)
{
    if (KeI386FxsrPresent)
    {
        KiFxsaveToFnsaveFormat((PFNSAVE_FORMAT)FloatingSaveArea, &FxSaveArea->U.FxArea);
    }
    else
    {
        memcpy(FloatingSaveArea, &FxSaveArea->U.FnArea, sizeof(FxSaveArea->U.FnArea));
    }
    FloatingSaveArea->Cr0NpxState = FxSaveArea->Cr0NpxState;
}


BOOL
KiContextToFxSaveArea(PFX_SAVE_AREA FxSaveArea, PCONTEXT Context)
{
    BOOL FpuContextChanged = FALSE;

    /* First of all convert the FLOATING_SAVE_AREA into the FX_SAVE_AREA */
    if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) == CONTEXT_FLOATING_POINT)
    {
        KiFloatingSaveAreaToFxSaveArea(FxSaveArea, &Context->FloatSave);
        FpuContextChanged = TRUE;
    }

    /* Now merge the FX_SAVE_AREA from the context with the destination area */
    if ((Context->ContextFlags & CONTEXT_EXTENDED_REGISTERS) == CONTEXT_EXTENDED_REGISTERS)
    {
        if (KeI386FxsrPresent)
        {
            PFXSAVE_FORMAT src = (PFXSAVE_FORMAT)Context->ExtendedRegisters;
            PFXSAVE_FORMAT dst = &FxSaveArea->U.FxArea;
            dst->MXCsr = src->MXCsr & MxcsrFeatureMask;
            memcpy(dst->Reserved3, src->Reserved3,
                   sizeof(src->Reserved3) + sizeof(src->Reserved4));

            if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) != CONTEXT_FLOATING_POINT)
            {
                dst->ControlWord = src->ControlWord;
                dst->StatusWord = src->StatusWord;
                dst->TagWord = src->TagWord;
                dst->ErrorOpcode = src->ErrorOpcode;
                dst->ErrorOffset = src->ErrorOffset;
                dst->ErrorSelector = src->ErrorSelector;
                dst->DataOffset = src->DataOffset;
                dst->DataSelector = src->DataSelector;
                memcpy(dst->RegisterArea, src->RegisterArea, sizeof(src->RegisterArea));

                FxSaveArea->NpxSavedCpu = 0;
                FxSaveArea->Cr0NpxState = 0;
            }
            FpuContextChanged = TRUE;
        }
    }

    return FpuContextChanged;
}


VOID INIT_FUNCTION
KiCheckFPU(VOID)
{
    unsigned short int status;
    int cr0;
    ULONG Flags;
    PKPRCB Prcb = KeGetCurrentPrcb();

    Ke386SaveFlags(Flags);
    Ke386DisableInterrupts();

    KeI386NpxPresent = 0;
    KeI386FxsrPresent = 0;
    KeI386XMMIPresent = 0;

    cr0 = Ke386GetCr0();
    cr0 |= X86_CR0_NE | X86_CR0_MP;
    cr0 &= ~(X86_CR0_EM | X86_CR0_TS);
    Ke386SetCr0(cr0);

#if defined(__GNUC__)
    asm volatile("fninit\n\t");
    asm volatile("fstsw %0\n\t" : "=a" (status));
#elif defined(_MSC_VER)
    __asm
    {
	   fninit;
	   fstsw status
    }
#else
#error Unknown compiler for inline assembler
#endif

    if (status != 0)
    {
        /* Set the EM flag in CR0 so any FPU instructions cause a trap. */
        Ke386SetCr0(Ke386GetCr0() | X86_CR0_EM);
        Ke386RestoreFlags(Flags);
        return;
    }

    /* fsetpm for i287, ignored by i387 */
#if defined(__GNUC__)
    asm volatile(".byte 0xDB, 0xE4\n\t");
#elif defined(_MSC_VER)
    __asm _emit 0xDB __asm _emit 0xe4
#else
#error Unknown compiler for inline assembler
#endif

    KeI386NpxPresent = 1;

    /* check for and enable MMX/SSE support if possible */
    if ((Prcb->FeatureBits & X86_FEATURE_FXSR) != 0)
    {
        BYTE DummyArea[sizeof(FX_SAVE_AREA) + 15];
        PFX_SAVE_AREA FxSaveArea;

        /* enable FXSR */
        KeI386FxsrPresent = 1;

        /* we need a 16 byte aligned FX_SAVE_AREA */
        FxSaveArea = (PFX_SAVE_AREA)(((ULONG_PTR)DummyArea + 0xf) & (~0x0f));

        Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSFXSR);
        memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea));