From b0c6956a173d31b8aefd30e40f759d90c4c0c9ba Mon Sep 17 00:00:00 2001
From: Aleksandar Andrejevic <aandrejevic@reactos.org>
Date: Sun, 8 Mar 2015 00:36:46 +0000
Subject: [PATCH] [NTVDM] Implement EMS function AH = 57h.

svn path=/trunk/; revision=66602
---
 reactos/subsystems/mvdm/ntvdm/ems.c | 125 ++++++++++++++++++++++++----
 reactos/subsystems/mvdm/ntvdm/ems.h |  19 +++++
 2 files changed, 128 insertions(+), 16 deletions(-)

diff --git a/reactos/subsystems/mvdm/ntvdm/ems.c b/reactos/subsystems/mvdm/ntvdm/ems.c
index 5f39ffe6e2f..72d77400ad7 100644
--- a/reactos/subsystems/mvdm/ntvdm/ems.c
+++ b/reactos/subsystems/mvdm/ntvdm/ems.c
@@ -10,6 +10,7 @@
 
 #define NDEBUG
 
+#include "emulator.h"
 #include "bios/bios32/bios32p.h"
 #include <ndk/rtltypes.h>
 #include <ndk/rtlfuncs.h>
@@ -104,12 +105,24 @@ static UCHAR EmsAlloc(USHORT NumPages, PUSHORT Handle)
     return EMS_STATUS_OK;
 }
 
+static PEMS_PAGE GetLogicalPage(PEMS_HANDLE Handle, USHORT LogicalPage)
+{
+    PLIST_ENTRY Entry = Handle->PageList.Flink;
+
+    while (LogicalPage)
+    {
+        if (Entry == &Handle->PageList) return NULL;
+        LogicalPage--;
+        Entry = Entry->Flink;
+    }
+
+    return (PEMS_PAGE)CONTAINING_RECORD(Entry, EMS_PAGE, Entry);
+}
+
 static USHORT EmsMap(USHORT Handle, UCHAR PhysicalPage, USHORT LogicalPage)
 {
-    PLIST_ENTRY Entry;
     PEMS_PAGE PageEntry;
     PEMS_HANDLE HandleEntry = &HandleTable[Handle];
-    ULONG PageNumber;
 
     if (PhysicalPage >= EMS_PHYSICAL_PAGES) return EMS_STATUS_INV_PHYSICAL_PAGE;
     if (LogicalPage == 0xFFFF)
@@ -121,21 +134,10 @@ static USHORT EmsMap(USHORT Handle, UCHAR PhysicalPage, USHORT LogicalPage)
 
     if (Handle >= EMS_MAX_HANDLES || !HandleEntry->Allocated) return EMS_STATUS_INVALID_HANDLE;
 
-    Entry = HandleEntry->PageList.Flink;
-    while (LogicalPage)
-    {
-        if (Entry == &HandleEntry->PageList) break;
-
-        LogicalPage--;
-        Entry = Entry->Flink;
-    }
-
-    if (Entry == &HandleEntry->PageList) return EMS_STATUS_INV_LOGICAL_PAGE; 
-
-    PageEntry = (PEMS_PAGE)CONTAINING_RECORD(Entry, EMS_PAGE, Entry);
-    PageNumber = (ULONG)(((ULONG_PTR)PageEntry - (ULONG_PTR)PageTable) / sizeof(EMS_PAGE));
-    Mapping[PhysicalPage] = (PVOID)(EMS_ADDRESS + PageNumber * EMS_PAGE_SIZE);
+    PageEntry = GetLogicalPage(HandleEntry, LogicalPage);
+    if (!PageEntry) return EMS_STATUS_INV_LOGICAL_PAGE; 
 
+    Mapping[PhysicalPage] = (PVOID)(EMS_ADDRESS + ARRAY_INDEX(PageEntry, PageTable) * EMS_PAGE_SIZE);
     return EMS_STATUS_OK;
 }
 
@@ -192,6 +194,97 @@ static VOID WINAPI EmsIntHandler(LPWORD Stack)
             break;
         }
 
+        /* Move/Exchange Memory */
+        case 0x57:
+        {
+            PUCHAR SourcePtr, DestPtr;
+            PEMS_HANDLE HandleEntry;
+            PEMS_PAGE PageEntry;
+            BOOLEAN Exchange = getAL();
+            PEMS_COPY_DATA Data = (PEMS_COPY_DATA)SEG_OFF_TO_PTR(getDS(), getSI());
+
+            if (Data->SourceType)
+            {
+                /* Expanded memory */
+                HandleEntry = &HandleTable[Data->SourceHandle];
+
+                if (Data->SourceHandle >= EMS_MAX_HANDLES || !HandleEntry->Allocated)
+                {
+                    setAL(EMS_STATUS_INVALID_HANDLE);
+                    break;
+                }
+
+                PageEntry = GetLogicalPage(HandleEntry, Data->SourceSegment);
+
+                if (!PageEntry)
+                {
+                    setAL(EMS_STATUS_INV_LOGICAL_PAGE);
+                    break;
+                }
+
+                SourcePtr = (PUCHAR)(EMS_ADDRESS
+                                     + ARRAY_INDEX(PageEntry, PageTable)
+                                     * EMS_PAGE_SIZE
+                                     + Data->SourceOffset);
+            }
+            else
+            {
+                /* Conventional memory */
+                SourcePtr = (PUCHAR)SEG_OFF_TO_PTR(Data->SourceSegment, Data->SourceOffset);
+            }
+
+            if (Data->DestType)
+            {
+                /* Expanded memory */
+                HandleEntry = &HandleTable[Data->DestHandle];
+
+                if (Data->SourceHandle >= EMS_MAX_HANDLES || !HandleEntry->Allocated)
+                {
+                    setAL(EMS_STATUS_INVALID_HANDLE);
+                    break;
+                }
+
+                PageEntry = GetLogicalPage(HandleEntry, Data->DestSegment);
+
+                if (!PageEntry)
+                {
+                    setAL(EMS_STATUS_INV_LOGICAL_PAGE);
+                    break;
+                }
+
+                DestPtr = (PUCHAR)(EMS_ADDRESS
+                                   + ARRAY_INDEX(PageEntry, PageTable)
+                                   * EMS_PAGE_SIZE
+                                   + Data->DestOffset);
+            }
+            else
+            {
+                /* Conventional memory */
+                DestPtr = (PUCHAR)SEG_OFF_TO_PTR(Data->DestSegment, Data->DestOffset);
+            }
+
+            if (Exchange)
+            {
+                ULONG i;
+
+                /* Exchange */
+                for (i = 0; i < Data->RegionLength; i++)
+                {
+                    UCHAR Temp = DestPtr[i];
+                    DestPtr[i] = SourcePtr[i];
+                    SourcePtr[i] = Temp;
+                }
+            }
+            else
+            {
+                /* Move */
+                RtlMoveMemory(DestPtr, SourcePtr, Data->RegionLength);
+            }
+
+            setAL(EMS_STATUS_OK);
+            break;
+        }
+
         default:
         {
             DPRINT1("EMS function AH = %02X NOT IMPLEMENTED\n", getAH());
diff --git a/reactos/subsystems/mvdm/ntvdm/ems.h b/reactos/subsystems/mvdm/ntvdm/ems.h
index b56b4361446..e89c85af983 100644
--- a/reactos/subsystems/mvdm/ntvdm/ems.h
+++ b/reactos/subsystems/mvdm/ntvdm/ems.h
@@ -32,6 +32,8 @@
 #define EMS_STATUS_INV_PHYSICAL_PAGE    0x8B
 #define EMS_STATUS_UNKNOWN_FUNCTION     0x8F
 
+#define ARRAY_INDEX(ptr, array) ((ULONG)(((ULONG_PTR)(ptr) - (ULONG_PTR)(array)) / sizeof(*array)))
+
 typedef struct _EMS_HANDLE
 {
     BOOLEAN Allocated;
@@ -45,6 +47,23 @@ typedef struct _EMS_PAGE
     USHORT Handle;
 } EMS_PAGE, *PEMS_PAGE;
 
+#pragma pack(push, 1)
+
+typedef struct _EMS_COPY_DATA
+{
+    ULONG RegionLength;
+    UCHAR SourceType;
+    USHORT SourceHandle;
+    USHORT SourceOffset;
+    USHORT SourceSegment;
+    UCHAR DestType;
+    USHORT DestHandle;
+    USHORT DestOffset;
+    USHORT DestSegment;
+} EMS_COPY_DATA, *PEMS_COPY_DATA;
+
+#pragma pack(pop)
+
 /* FUNCTIONS ******************************************************************/
 
 VOID EmsReadMemory(ULONG Address, PVOID Buffer, ULONG Size);