8 files changed, 225 insertions, 30 deletions

diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt
index 02c178441..eab08207d 100644
--- a/src/core/CMakeLists.txt
+++ b/src/core/CMakeLists.txt
@@ -137,6 +137,8 @@ add_library(core STATIC
     hle/service/nvdrv/devices/nvmap.h
     hle/service/nvdrv/interface.cpp
     hle/service/nvdrv/interface.h
+    hle/service/nvdrv/memory_manager.cpp
+    hle/service/nvdrv/memory_manager.h
     hle/service/nvdrv/nvdrv.cpp
     hle/service/nvdrv/nvdrv.h
     hle/service/nvdrv/nvmemp.cpp
diff --git a/src/core/hle/service/nvdrv/devices/nvhost_as_gpu.cpp b/src/core/hle/service/nvdrv/devices/nvhost_as_gpu.cpp
index 11ab25545..cf3601f02 100644
--- a/src/core/hle/service/nvdrv/devices/nvhost_as_gpu.cpp
+++ b/src/core/hle/service/nvdrv/devices/nvhost_as_gpu.cpp
@@ -5,6 +5,7 @@
 #include "common/assert.h"
 #include "common/logging/log.h"
 #include "core/hle/service/nvdrv/devices/nvhost_as_gpu.h"
+#include "core/hle/service/nvdrv/devices/nvmap.h"
 
 namespace Service {
 namespace Nvidia {
@@ -40,9 +41,16 @@ u32 nvhost_as_gpu::InitalizeEx(const std::vector<u8>& input, std::vector<u8>& ou
 u32 nvhost_as_gpu::AllocateSpace(const std::vector<u8>& input, std::vector<u8>& output) {
     IoctlAllocSpace params{};
     std::memcpy(&params, input.data(), input.size());
-    LOG_WARNING(Service_NVDRV, "(STUBBED) called, pages=%x, page_size=%x, flags=%x", params.pages,
-                params.page_size, params.flags);
-    params.offset = 0xdeadbeef; // TODO(ogniK): Actually allocate space and give a real offset
+    LOG_DEBUG(Service_NVDRV, "called, pages=%x, page_size=%x, flags=%x", params.pages,
+              params.page_size, params.flags);
+
+    const u64 size{static_cast<u64>(params.pages) * static_cast<u64>(params.page_size)};
+    if (params.flags & 1) {
+        params.offset = memory_manager->AllocateSpace(params.offset, size, 1);
+    } else {
+        params.offset = memory_manager->AllocateSpace(size, params.align);
+    }
+
     std::memcpy(output.data(), &params, output.size());
     return 0;
 }
@@ -51,12 +59,24 @@ u32 nvhost_as_gpu::MapBufferEx(const std::vector<u8>& input, std::vector<u8>& ou
     IoctlMapBufferEx params{};
     std::memcpy(&params, input.data(), input.size());
 
-    LOG_WARNING(Service_NVDRV,
-                "(STUBBED) called, flags=%x, nvmap_handle=%x, buffer_offset=%lx, mapping_size=%lx, "
-                "offset=%lx",
-                params.flags, params.nvmap_handle, params.buffer_offset, params.mapping_size,
-                params.offset);
-    params.offset = 0x0; // TODO(ogniK): Actually map and give a real offset
+    LOG_DEBUG(Service_NVDRV,
+              "called, flags=%x, nvmap_handle=%x, buffer_offset=%lx, mapping_size=%lx, offset=%lx",
+              params.flags, params.nvmap_handle, params.buffer_offset, params.mapping_size,
+              params.offset);
+
+    if (!params.nvmap_handle) {
+        return 0;
+    }
+
+    auto object = nvmap_dev->GetObject(params.nvmap_handle);
+    ASSERT(object);
+
+    if (params.flags & 1) {
+        params.offset = memory_manager->MapBufferEx(object->addr, params.offset, object->size);
+    } else {
+        params.offset = memory_manager->MapBufferEx(object->addr, object->size);
+    }
+
     std::memcpy(output.data(), &params, output.size());
     return 0;
 }
diff --git a/src/core/hle/service/nvdrv/devices/nvhost_as_gpu.h b/src/core/hle/service/nvdrv/devices/nvhost_as_gpu.h
index 06c256d5d..9d37b971a 100644
--- a/src/core/hle/service/nvdrv/devices/nvhost_as_gpu.h
+++ b/src/core/hle/service/nvdrv/devices/nvhost_as_gpu.h
@@ -4,18 +4,25 @@
 
 #pragma once
 
+#include <memory>
+#include <utility>
 #include <vector>
 #include "common/common_types.h"
 #include "common/swap.h"
 #include "core/hle/service/nvdrv/devices/nvdevice.h"
+#include "core/hle/service/nvdrv/memory_manager.h"
 
 namespace Service {
 namespace Nvidia {
 namespace Devices {
 
+class nvmap;
+
 class nvhost_as_gpu final : public nvdevice {
 public:
-    nvhost_as_gpu() = default;
+    nvhost_as_gpu(std::shared_ptr<nvmap> nvmap_dev) : nvdevice(), nvmap_dev(std::move(nvmap_dev)) {
+        memory_manager = std::make_shared<MemoryManager>();
+    }
     ~nvhost_as_gpu() override = default;
 
     u32 ioctl(Ioctl command, const std::vector<u8>& input, std::vector<u8>& output) override;
@@ -92,6 +99,9 @@ private:
     u32 MapBufferEx(const std::vector<u8>& input, std::vector<u8>& output);
     u32 BindChannel(const std::vector<u8>& input, std::vector<u8>& output);
     u32 GetVARegions(const std::vector<u8>& input, std::vector<u8>& output);
+
+    std::shared_ptr<nvmap> nvmap_dev;
+    std::shared_ptr<MemoryManager> memory_manager;
 };
 
 } // namespace Devices
diff --git a/src/core/hle/service/nvdrv/devices/nvmap.cpp b/src/core/hle/service/nvdrv/devices/nvmap.cpp
index cb3692689..02b33374a 100644
--- a/src/core/hle/service/nvdrv/devices/nvmap.cpp
+++ b/src/core/hle/service/nvdrv/devices/nvmap.cpp
@@ -13,10 +13,8 @@ namespace Nvidia {
 namespace Devices {
 
 VAddr nvmap::GetObjectAddress(u32 handle) const {
-    auto itr = handles.find(handle);
-    ASSERT(itr != handles.end());
-
-    auto object = itr->second;
+    auto object = GetObject(handle);
+    ASSERT(object);
     ASSERT(object->status == Object::Status::Allocated);
     return object->addr;
 }
@@ -52,7 +50,7 @@ u32 nvmap::IocCreate(const std::vector<u8>& input, std::vector<u8>& output) {
     u32 handle = next_handle++;
     handles[handle] = std::move(object);
 
-    LOG_WARNING(Service_NVDRV, "(STUBBED) size 0x%08X", params.size);
+    LOG_DEBUG(Service_NVDRV, "size=0x%08X", params.size);
 
     params.handle = handle;
 
@@ -64,17 +62,16 @@ u32 nvmap::IocAlloc(const std::vector<u8>& input, std::vector<u8>& output) {
     IocAllocParams params;
     std::memcpy(&params, input.data(), sizeof(params));
 
-    auto itr = handles.find(params.handle);
-    ASSERT(itr != handles.end());
+    auto object = GetObject(params.handle);
+    ASSERT(object);
 
-    auto object = itr->second;
     object->flags = params.flags;
     object->align = params.align;
     object->kind = params.kind;
     object->addr = params.addr;
     object->status = Object::Status::Allocated;
 
-    LOG_WARNING(Service_NVDRV, "(STUBBED) Allocated address 0x%llx", params.addr);
+    LOG_DEBUG(Service_NVDRV, "called, addr=0x%llx", params.addr);
 
     std::memcpy(output.data(), &params, sizeof(params));
     return 0;
@@ -86,10 +83,10 @@ u32 nvmap::IocGetId(const std::vector<u8>& input, std::vector<u8>& output) {
 
     LOG_WARNING(Service_NVDRV, "called");
 
-    auto itr = handles.find(params.handle);
-    ASSERT(itr != handles.end());
+    auto object = GetObject(params.handle);
+    ASSERT(object);
 
-    params.id = itr->second->id;
+    params.id = object->id;
 
     std::memcpy(output.data(), &params, sizeof(params));
     return 0;
@@ -123,10 +120,8 @@ u32 nvmap::IocParam(const std::vector<u8>& input, std::vector<u8>& output) {
 
     LOG_WARNING(Service_NVDRV, "(STUBBED) called type=%u", params.type);
 
-    auto itr = handles.find(params.handle);
-    ASSERT(itr != handles.end());
-
-    auto object = itr->second;
+    auto object = GetObject(params.handle);
+    ASSERT(object);
     ASSERT(object->status == Object::Status::Allocated);
 
     switch (static_cast<ParamTypes>(params.type)) {
diff --git a/src/core/hle/service/nvdrv/devices/nvmap.h b/src/core/hle/service/nvdrv/devices/nvmap.h
index 1591ac8ff..4681e438b 100644
--- a/src/core/hle/service/nvdrv/devices/nvmap.h
+++ b/src/core/hle/service/nvdrv/devices/nvmap.h
@@ -26,8 +26,7 @@ public:
 
     u32 ioctl(Ioctl command, const std::vector<u8>& input, std::vector<u8>& output) override;
 
-private:
-    // Represents an nvmap object.
+    /// Represents an nvmap object.
     struct Object {
         enum class Status { Created, Allocated };
         u32 id;
@@ -39,10 +38,19 @@ private:
         Status status;
     };
 
+    std::shared_ptr<Object> GetObject(u32 handle) const {
+        auto itr = handles.find(handle);
+        if (itr != handles.end()) {
+            return itr->second;
+        }
+        return {};
+    }
+
+private:
     /// Id to use for the next handle that is created.
     u32 next_handle = 1;
 
-    // Id to use for the next object that is created.
+    /// Id to use for the next object that is created.
     u32 next_id = 1;
 
     /// Mapping of currently allocated handles to the objects they represent.
diff --git a/src/core/hle/service/nvdrv/memory_manager.cpp b/src/core/hle/service/nvdrv/memory_manager.cpp
new file mode 100644
index 000000000..55a8675d5
--- /dev/null
+++ b/src/core/hle/service/nvdrv/memory_manager.cpp
@@ -0,0 +1,112 @@
+// Copyright 2018 yuzu emulator team
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/assert.h"
+#include "core/hle/service/nvdrv/memory_manager.h"
+
+namespace Service {
+namespace Nvidia {
+
+PAddr MemoryManager::AllocateSpace(u64 size, u64 align) {
+    boost::optional<PAddr> paddr = FindFreeBlock(size, align);
+    ASSERT(paddr);
+
+    for (u64 offset = 0; offset < size; offset += Memory::PAGE_SIZE) {
+        PageSlot(*paddr + offset) = static_cast<u64>(PageStatus::Allocated);
+    }
+
+    return *paddr;
+}
+
+PAddr MemoryManager::AllocateSpace(PAddr paddr, u64 size, u64 align) {
+    for (u64 offset = 0; offset < size; offset += Memory::PAGE_SIZE) {
+        if (IsPageMapped(paddr + offset)) {
+            return AllocateSpace(size, align);
+        }
+    }
+
+    for (u64 offset = 0; offset < size; offset += Memory::PAGE_SIZE) {
+        PageSlot(paddr + offset) = static_cast<u64>(PageStatus::Allocated);
+    }
+
+    return paddr;
+}
+
+PAddr MemoryManager::MapBufferEx(VAddr vaddr, u64 size) {
+    vaddr &= ~Memory::PAGE_MASK;
+
+    boost::optional<PAddr> paddr = FindFreeBlock(size);
+    ASSERT(paddr);
+
+    for (u64 offset = 0; offset < size; offset += Memory::PAGE_SIZE) {
+        PageSlot(*paddr + offset) = vaddr + offset;
+    }
+
+    return *paddr;
+}
+
+PAddr MemoryManager::MapBufferEx(VAddr vaddr, PAddr paddr, u64 size) {
+    vaddr &= ~Memory::PAGE_MASK;
+    paddr &= ~Memory::PAGE_MASK;
+
+    for (u64 offset = 0; offset < size; offset += Memory::PAGE_SIZE) {
+        if (PageSlot(paddr + offset) != static_cast<u64>(PageStatus::Allocated)) {
+            return MapBufferEx(vaddr, size);
+        }
+    }
+
+    for (u64 offset = 0; offset < size; offset += Memory::PAGE_SIZE) {
+        PageSlot(paddr + offset) = vaddr + offset;
+    }
+
+    return paddr;
+}
+
+boost::optional<PAddr> MemoryManager::FindFreeBlock(u64 size, u64 align) {
+    PAddr paddr{};
+    u64 free_space{};
+    align = (align + Memory::PAGE_MASK) & ~Memory::PAGE_MASK;
+
+    while (paddr + free_space < MAX_ADDRESS) {
+        if (!IsPageMapped(paddr + free_space)) {
+            free_space += Memory::PAGE_SIZE;
+            if (free_space >= size) {
+                return paddr;
+            }
+        } else {
+            paddr += free_space + Memory::PAGE_SIZE;
+            free_space = 0;
+            const u64 remainder{paddr % align};
+            if (!remainder) {
+                paddr = (paddr - remainder) + align;
+            }
+        }
+    }
+
+    return {};
+}
+
+VAddr MemoryManager::PhysicalToVirtualAddress(PAddr paddr) {
+    VAddr base_addr = PageSlot(paddr);
+    ASSERT(base_addr != static_cast<u64>(PageStatus::Unmapped));
+    return base_addr + (paddr & Memory::PAGE_MASK);
+}
+
+bool MemoryManager::IsPageMapped(PAddr paddr) {
+    return PageSlot(paddr) != static_cast<u64>(PageStatus::Unmapped);
+}
+
+VAddr& MemoryManager::PageSlot(PAddr paddr) {
+    auto& block = page_table[(paddr >> (Memory::PAGE_BITS + PAGE_TABLE_BITS)) & PAGE_TABLE_MASK];
+    if (!block) {
+        block = std::make_unique<PageBlock>();
+        for (unsigned index = 0; index < PAGE_BLOCK_SIZE; index++) {
+            (*block)[index] = static_cast<u64>(PageStatus::Unmapped);
+        }
+    }
+    return (*block)[(paddr >> Memory::PAGE_BITS) & PAGE_BLOCK_MASK];
+}
+
+} // namespace Nvidia
+} // namespace Service
diff --git a/src/core/hle/service/nvdrv/memory_manager.h b/src/core/hle/service/nvdrv/memory_manager.h
new file mode 100644
index 000000000..4ba1a3952
--- /dev/null
+++ b/src/core/hle/service/nvdrv/memory_manager.h
@@ -0,0 +1,48 @@
+// Copyright 2018 yuzu emulator team
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include <array>
+#include <memory>
+#include "common/common_types.h"
+#include "core/memory.h"
+
+namespace Service {
+namespace Nvidia {
+
+class MemoryManager final {
+public:
+    MemoryManager() = default;
+
+    PAddr AllocateSpace(u64 size, u64 align);
+    PAddr AllocateSpace(PAddr paddr, u64 size, u64 align);
+    PAddr MapBufferEx(VAddr vaddr, u64 size);
+    PAddr MapBufferEx(VAddr vaddr, PAddr paddr, u64 size);
+    VAddr PhysicalToVirtualAddress(PAddr paddr);
+
+private:
+    boost::optional<PAddr> FindFreeBlock(u64 size, u64 align = 1);
+    bool IsPageMapped(PAddr paddr);
+    VAddr& PageSlot(PAddr paddr);
+
+    enum class PageStatus : u64 {
+        Unmapped = 0xFFFFFFFFFFFFFFFFULL,
+        Allocated = 0xFFFFFFFFFFFFFFFEULL,
+    };
+
+    static constexpr u64 MAX_ADDRESS{0x10000000000ULL};
+    static constexpr u64 PAGE_TABLE_BITS{14};
+    static constexpr u64 PAGE_TABLE_SIZE{1 << PAGE_TABLE_BITS};
+    static constexpr u64 PAGE_TABLE_MASK{PAGE_TABLE_SIZE - 1};
+    static constexpr u64 PAGE_BLOCK_BITS{14};
+    static constexpr u64 PAGE_BLOCK_SIZE{1 << PAGE_BLOCK_BITS};
+    static constexpr u64 PAGE_BLOCK_MASK{PAGE_BLOCK_SIZE - 1};
+
+    using PageBlock = std::array<VAddr, PAGE_BLOCK_SIZE>;
+    std::array<std::unique_ptr<PageBlock>, PAGE_TABLE_SIZE> page_table{};
+};
+
+} // namespace Nvidia
+} // namespace Service
diff --git a/src/core/hle/service/nvdrv/nvdrv.cpp b/src/core/hle/service/nvdrv/nvdrv.cpp
index 1cf8f3175..a98634422 100644
--- a/src/core/hle/service/nvdrv/nvdrv.cpp
+++ b/src/core/hle/service/nvdrv/nvdrv.cpp
@@ -31,7 +31,7 @@ void InstallInterfaces(SM::ServiceManager& service_manager) {
 
 Module::Module() {
     auto nvmap_dev = std::make_shared<Devices::nvmap>();
-    devices["/dev/nvhost-as-gpu"] = std::make_shared<Devices::nvhost_as_gpu>();
+    devices["/dev/nvhost-as-gpu"] = std::make_shared<Devices::nvhost_as_gpu>(nvmap_dev);
     devices["/dev/nvhost-ctrl-gpu"] = std::make_shared<Devices::nvhost_ctrl_gpu>();
     devices["/dev/nvmap"] = nvmap_dev;
     devices["/dev/nvdisp_disp0"] = std::make_shared<Devices::nvdisp_disp0>(nvmap_dev);