3 files changed, 76 insertions, 1 deletions

diff --git a/src/core/hle/syscall.cpp b/src/core/hle/syscall.cpp
index c5b887795..0cb563955 100644
--- a/src/core/hle/syscall.cpp
+++ b/src/core/hle/syscall.cpp
@@ -4,6 +4,10 @@
 
 #include <map>
 
+#include "core/mem_map.h"
+
+#include "core/hw/hw_lcd.h"
+
 #include "core/hle/function_wrappers.h"
 #include "core/hle/syscall.h"
 #include "core/hle/service/service.h"
@@ -13,6 +17,26 @@
 
 namespace Syscall {
 
+/// Map application or GSP heap memory
+Result ControlMemory(void* outaddr, u32 addr0, u32 addr1, u32 size, u32 operation, u32 permissions) {
+    u32 virtual_address = 0x00000000;
+
+    switch (operation) {
+
+    // Map GSP heap memory?
+    case 0x00010003:
+        virtual_address = Memory::MapBlock_HeapGSP(size, operation, permissions);
+        break;
+
+    // Unknown ControlMemory operation
+    default:
+        ERROR_LOG(OSHLE, "Unknown ControlMemory operation %08X", operation);
+    }
+
+    Core::g_app_core->SetReg(1,  Memory::MapBlock_HeapGSP(size, operation, permissions));
+    return 0;
+}
+
 /// Connect to an OS service given the port name, returns the handle to the port to out
 Result ConnectToPort(void* out, const char* port_name) {
     Service::Interface* service = Service::g_manager->FetchFromPortName(port_name);
@@ -41,7 +65,7 @@ Result WaitSynchronization1(Handle handle, s64 nanoseconds) {
 
 const HLE::FunctionDef Syscall_Table[] = {
     {0x00,  NULL,                               "Unknown"},
-    {0x01,  NULL,                               "ControlMemory"},
+    {0x01,  WrapI_VUUUUU<ControlMemory>,        "ControlMemory"},
     {0x02,  NULL,                               "QueryMemory"},
     {0x03,  NULL,                               "ExitProcess"},
     {0x04,  NULL,                               "GetProcessAffinityMask"},
diff --git a/src/core/mem_map.h b/src/core/mem_map.h
index 9931daece..ab1eb2606 100644
--- a/src/core/mem_map.h
+++ b/src/core/mem_map.h
@@ -49,6 +49,23 @@ enum {
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
+/// Represents a block of heap memory mapped by ControlMemory
+struct HeapBlock {
+    HeapBlock() : base_address(0), address(0), size(0), operation(0), permissions(0) {
+    }
+    u32 base_address;
+    u32 address;
+    u32 size;
+    u32 operation;
+    u32 permissions;
+
+    const u32 GetVirtualAddress() const{
+        return base_address + address;
+    }
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
 // Base is a pointer to the base of the memory map. Yes, some MMU tricks
 // are used to set up a full GC or Wii memory map in process memory.  on
 // 32-bit, you have to mask your offsets with 0x3FFFFFFF. This means that
@@ -81,6 +98,14 @@ void Write32(const u32 addr, const u32 data);
 
 u8* GetPointer(const u32 Address);
 
+/**
+ * Maps a block of memory on the GSP heap
+ * @param size Size of block in bytes
+ * @param operation Control memory operation
+ * @param permissions Control memory permissions
+ */
+u32 MapBlock_HeapGSP(u32 size, u32 operation, u32 permissions);
+
 inline const char* GetCharPointer(const u32 address) {
     return (const char *)GetPointer(address);
 }
diff --git a/src/core/mem_map_funcs.cpp b/src/core/mem_map_funcs.cpp
index 8e97ef111..af4cfacbd 100644
--- a/src/core/mem_map_funcs.cpp
+++ b/src/core/mem_map_funcs.cpp
@@ -2,6 +2,8 @@
 // Licensed under GPLv2
 // Refer to the license.txt file included.
 
+#include <map>
+
 #include "common/common.h"
 
 #include "core/mem_map.h"
@@ -10,6 +12,8 @@
 
 namespace Memory {
 
+std::map<u32, HeapBlock> g_heap_gsp_map;
+
 /// Convert a physical address to virtual address
 u32 _AddressPhysicalToVirtual(const u32 addr) {
     // Our memory interface read/write functions assume virtual addresses. Put any physical address 
@@ -116,6 +120,28 @@ u8 *GetPointer(const u32 addr) {
     }
 }
 
+/**
+ * Maps a block of memory on the GSP heap
+ * @param size Size of block in bytes
+ * @param flags Memory allocation flags
+ */
+u32 MapBlock_HeapGSP(u32 size, u32 operation, u32 permissions) {
+    HeapBlock block;
+    
+    block.base_address  = HEAP_GSP_VADDR;
+    block.size          = size;
+    block.operation     = operation;
+    block.permissions   = permissions;
+    
+    if (g_heap_gsp_map.size() > 0) {
+        const HeapBlock last_block = g_heap_gsp_map.rbegin()->second;
+        block.address = last_block.address + last_block.size;
+    }
+    g_heap_gsp_map[block.GetVirtualAddress()] = block;
+
+    return block.GetVirtualAddress();
+}
+
 u8 Read8(const u32 addr) {
     u8 _var = 0;
     _Read<u8>(_var, addr);