[FAST486]: fixes for CountLeadingZeros64:

[reactos.git] / reactos / lib / fast486 / common.inl

diff --git a/reactos/lib/fast486/common.inl b/reactos/lib/fast486/common.inl
index a558ed4..e31f40b 100644 (file)
--- a/reactos/lib/fast486/common.inl
+++ b/reactos/lib/fast486/common.inl
@@ -2,7 +2,7 @@
  * Fast486 386/486 CPU Emulation Library
  * common.inl
  *
- * Copyright (C) 2013 Aleksandar Andrejevic <theflash AT sdf DOT lonestar DOT org>
+ * Copyright (C) 2014 Aleksandar Andrejevic <theflash AT sdf DOT lonestar DOT org>
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -20,9 +20,34 @@
  */
 
 #include "common.h"
+#include "fpu.h"
 
 /* PUBLIC FUNCTIONS ***********************************************************/
 
+#if defined (__GNUC__)
+    #define CountLeadingZeros64(x) __builtin_clzll(x)
+#elif (_MSC_VER >= 1500) && defined(_WIN64)
+    #define CountLeadingZeros64(x) __lzcnt64(x)
+#elif (_MSC_VER >= 1500)
+    #define CountLeadingZeros64(x) ((x) > 0xFFFFFFFFULL) ? __lzcnt((x) >> 32) \
+                                                         : (__lzcnt(x) + 32)
+#else
+    static FORCEINLINE
+    ULONG CountLeadingZeros64(ULONGLONG Value)
+    {
+        ULONG Count = 0;
+        ULONGLONG Mask = 1ULL << 63;
+
+        while (!(Value & Mask))
+        {
+            Count++;
+            Mask >>= 1;
+        }
+
+        return Count;
+    }
+#endif
+
 FORCEINLINE
 INT
 Fast486GetCurrentPrivLevel(PFAST486_STATE State)
@@ -690,53 +715,6 @@ Fast486FetchDword(PFAST486_STATE State,
     return TRUE;
 }
 
-FORCEINLINE
-BOOLEAN
-Fast486GetIntVector(PFAST486_STATE State,
-                    UCHAR Number,
-                    PFAST486_IDT_ENTRY IdtEntry)
-{
-    ULONG FarPointer;
-
-    /* Check for protected mode */
-    if (State->ControlRegisters[FAST486_REG_CR0] & FAST486_CR0_PE)
-    {
-        /* Read from the IDT */
-        if (!Fast486ReadLinearMemory(State,
-                                     State->Idtr.Address
-                                     + Number * sizeof(*IdtEntry),
-                                     IdtEntry,
-                                     sizeof(*IdtEntry)))
-        {
-            /* Exception occurred */
-            return FALSE;
-        }
-    }
-    else
-    {
-        /* Read from the real-mode IVT */
-
-        /* Paging is always disabled in real mode */
-        State->MemReadCallback(State,
-                               State->Idtr.Address
-                               + Number * sizeof(FarPointer),
-                               &FarPointer,
-                               sizeof(FarPointer));
-
-        /* Fill a fake IDT entry */
-        IdtEntry->Offset = LOWORD(FarPointer);
-        IdtEntry->Selector = HIWORD(FarPointer);
-        IdtEntry->Zero = 0;
-        IdtEntry->Type = FAST486_IDT_INT_GATE;
-        IdtEntry->Storage = FALSE;
-        IdtEntry->Dpl = 0;
-        IdtEntry->Present = TRUE;
-        IdtEntry->OffsetHigh = 0;
-    }
-
-    return TRUE;
-}
-
 FORCEINLINE
 BOOLEAN
 Fast486CalculateParity(UCHAR Number)
@@ -1330,4 +1308,75 @@ Fast486WriteModrmDwordOperands(PFAST486_STATE State,
     return TRUE;
 }
 
+#ifndef FAST486_NO_FPU
+
+FORCEINLINE
+VOID
+Fast486FpuNormalize(PFAST486_STATE State, PFAST486_FPU_DATA_REG Data)
+{
+    UINT LeadingZeros;
+
+    if (FPU_IS_NORMALIZED(Data)) return;
+    if (FPU_IS_ZERO(Data))
+    {
+        Data->Exponent = 0;
+        return;
+    }
+
+    LeadingZeros = CountLeadingZeros64(Data->Mantissa);
+
+    if (LeadingZeros < Data->Exponent)
+    {
+        Data->Mantissa <<= LeadingZeros;
+        Data->Exponent -= LeadingZeros;
+    }
+    else
+    {
+        /* Make it denormalized */
+        Data->Mantissa <<= Data->Exponent - 1;
+        Data->Exponent = 1;
+
+        /* Underflow */
+        State->FpuStatus.Ue = TRUE;
+    }
+}
+
+FORCEINLINE
+USHORT
+Fast486GetValueTag(PFAST486_FPU_DATA_REG Data)
+{
+    if (FPU_IS_ZERO(Data)) return FPU_TAG_ZERO;
+    else if (FPU_IS_NAN(Data)) return FPU_TAG_SPECIAL;
+    else return FPU_TAG_VALID;
+}
+
+FORCEINLINE
+VOID
+Fast486FpuPush(PFAST486_STATE State,
+               PFAST486_FPU_DATA_REG Data)
+{
+    State->FpuStatus.Top--;
+
+    if (FPU_GET_TAG(0) == FPU_TAG_EMPTY)
+    {
+        FPU_ST(0) = *Data;
+        FPU_SET_TAG(0, Fast486GetValueTag(Data));
+    }
+    else State->FpuStatus.Ie = TRUE;
+}
+
+FORCEINLINE
+VOID
+Fast486FpuPop(PFAST486_STATE State)
+{
+    if (FPU_GET_TAG(0) != FPU_TAG_EMPTY)
+    {
+        FPU_SET_TAG(0, FPU_TAG_EMPTY);
+        State->FpuStatus.Top++;
+    }
+    else State->FpuStatus.Ie = TRUE;
+}
+
+#endif
+
 /* EOF */