10 files changed, 197 insertions, 11 deletions

diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt
index 0d3189d29..207f39707 100644
--- a/src/core/CMakeLists.txt
+++ b/src/core/CMakeLists.txt
@@ -34,6 +34,7 @@ set(SRCS    core.cpp
             hle/config_mem.cpp
             hle/coprocessor.cpp
             hle/svc.cpp
+            hle/kernel/address_arbiter.cpp
             hle/kernel/archive.cpp
             hle/kernel/event.cpp
             hle/kernel/kernel.cpp
@@ -83,6 +84,7 @@ set(HEADERS core.h
             hle/coprocessor.h
             hle/hle.h
             hle/svc.h
+            hle/kernel/address_arbiter.h
             hle/kernel/archive.h
             hle/kernel/kernel.h
             hle/kernel/mutex.h
diff --git a/src/core/core.vcxproj b/src/core/core.vcxproj
index a09a4a1f1..ddc174c2c 100644
--- a/src/core/core.vcxproj
+++ b/src/core/core.vcxproj
@@ -166,6 +166,7 @@
     <ClCompile Include="hle\config_mem.cpp" />
     <ClCompile Include="hle\coprocessor.cpp" />
     <ClCompile Include="hle\hle.cpp" />
+    <ClCompile Include="hle\kernel\address_arbiter.cpp" />
     <ClCompile Include="hle\kernel\archive.cpp" />
     <ClCompile Include="hle\kernel\event.cpp" />
     <ClCompile Include="hle\kernel\kernel.cpp" />
@@ -219,6 +220,7 @@
     <ClInclude Include="hle\coprocessor.h" />
     <ClInclude Include="hle\function_wrappers.h" />
     <ClInclude Include="hle\hle.h" />
+    <ClInclude Include="hle\kernel\address_arbiter.h" />
     <ClInclude Include="hle\kernel\archive.h" />
     <ClInclude Include="hle\kernel\event.h" />
     <ClInclude Include="hle\kernel\kernel.h" />
diff --git a/src/core/core.vcxproj.filters b/src/core/core.vcxproj.filters
index 7e6a7f91b..68ba9e50b 100644
--- a/src/core/core.vcxproj.filters
+++ b/src/core/core.vcxproj.filters
@@ -182,6 +182,9 @@
     <ClCompile Include="hle\kernel\shared_memory.cpp">
       <Filter>hle\kernel</Filter>
     </ClCompile>
+    <ClCompile Include="hle\kernel\address_arbiter.cpp">
+      <Filter>hle\kernel</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="arm\disassembler\arm_disasm.h">
@@ -326,6 +329,9 @@
     <ClInclude Include="hle\kernel\shared_memory.h">
       <Filter>hle\kernel</Filter>
     </ClInclude>
+    <ClInclude Include="hle\kernel\address_arbiter.h">
+      <Filter>hle\kernel</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <Text Include="CMakeLists.txt" />
diff --git a/src/core/hle/function_wrappers.h b/src/core/hle/function_wrappers.h
index 0bed78653..ea603a1bb 100644
--- a/src/core/hle/function_wrappers.h
+++ b/src/core/hle/function_wrappers.h
@@ -39,9 +39,16 @@ template<s32 func(s32*, u32*, s32, bool, s64)> void Wrap() {
     RETURN(retval);
 }
 
-// TODO(bunnei): Is this correct? Probably not
+// TODO(bunnei): Is this correct? Probably not - Last parameter looks wrong for ArbitrateAddress
 template<s32 func(u32, u32, u32, u32, s64)> void Wrap() {
-    RETURN(func(PARAM(5), PARAM(1), PARAM(2), PARAM(3), (((s64)PARAM(4) << 32) | PARAM(0))));
+    RETURN(func(PARAM(0), PARAM(1), PARAM(2), PARAM(3), (((s64)PARAM(5) << 32) | PARAM(4))));
+}
+
+template<s32 func(u32*)> void Wrap(){
+    u32 param_1 = 0;
+    u32 retval = func(&param_1);
+    Core::g_app_core->SetReg(1, param_1);
+    RETURN(retval);
 }
 
 template<s32 func(u32, s64)> void Wrap() {
diff --git a/src/core/hle/kernel/address_arbiter.cpp b/src/core/hle/kernel/address_arbiter.cpp
new file mode 100644
index 000000000..61717bbe4
--- /dev/null
+++ b/src/core/hle/kernel/address_arbiter.cpp
@@ -0,0 +1,87 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2
+// Refer to the license.txt file included.
+
+#include "common/common_types.h"
+
+#include "core/mem_map.h"
+
+#include "core/hle/hle.h"
+#include "core/hle/kernel/address_arbiter.h"
+#include "core/hle/kernel/thread.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Kernel namespace
+
+namespace Kernel {
+
+class AddressArbiter : public Object {
+public:
+    const char* GetTypeName() const { return "Arbiter"; }
+    const char* GetName() const { return name.c_str(); }
+
+    static Kernel::HandleType GetStaticHandleType() { return HandleType::AddressArbiter; }
+    Kernel::HandleType GetHandleType() const { return HandleType::AddressArbiter; }
+
+    std::string name;   ///< Name of address arbiter object (optional)
+
+    /**
+     * Wait for kernel object to synchronize
+     * @param wait Boolean wait set if current thread should wait as a result of sync operation
+     * @return Result of operation, 0 on success, otherwise error code
+     */
+    Result WaitSynchronization(bool* wait) {
+        // TODO(bunnei): ImplementMe
+        ERROR_LOG(OSHLE, "(UNIMPLEMENTED)");
+        return 0;
+    }
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/// Arbitrate an address
+Result ArbitrateAddress(Handle handle, ArbitrationType type, u32 address, s32 value) {
+    switch (type) {
+
+    // Signal thread(s) waiting for arbitrate address...
+    case ArbitrationType::Signal:
+        // Negative value means resume all threads
+        if (value < 0) {
+            ArbitrateAllThreads(handle, address);
+        } else {
+            // Resume first N threads
+            for(int i = 0; i < value; i++)
+                ArbitrateHighestPriorityThread(handle, address);
+        }
+        HLE::Reschedule(__func__);
+
+    // Wait current thread (acquire the arbiter)...
+    case ArbitrationType::WaitIfLessThan:
+        if ((s32)Memory::Read32(address) <= value) {
+            Kernel::WaitCurrentThread(WAITTYPE_ARB, handle);
+            HLE::Reschedule(__func__);
+        }
+
+    default:
+        ERROR_LOG(KERNEL, "unknown type=%d", type);
+        return -1;
+    }
+    return 0;
+}
+
+/// Create an address arbiter
+AddressArbiter* CreateAddressArbiter(Handle& handle, const std::string& name) {
+    AddressArbiter* address_arbiter = new AddressArbiter;
+    handle = Kernel::g_object_pool.Create(address_arbiter);
+    address_arbiter->name = name;
+    return address_arbiter;
+}
+
+/// Create an address arbiter
+Handle CreateAddressArbiter(const std::string& name) {
+    Handle handle;
+    CreateAddressArbiter(handle, name);
+    return handle;
+}
+
+} // namespace Kernel
diff --git a/src/core/hle/kernel/address_arbiter.h b/src/core/hle/kernel/address_arbiter.h
new file mode 100644
index 000000000..a483fe466
--- /dev/null
+++ b/src/core/hle/kernel/address_arbiter.h
@@ -0,0 +1,36 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "common/common_types.h"
+
+#include "core/hle/kernel/kernel.h"
+
+// Address arbiters are an underlying kernel synchronization object that can be created/used via
+// supervisor calls (SVCs). They function as sort of a global lock. Typically, games/other CTR
+// applications use them as an underlying mechanism to implement thread-safe barriers, events, and
+// semphores. 
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Kernel namespace
+
+namespace Kernel {
+
+/// Address arbitration types
+enum class ArbitrationType : u32 {
+    Signal,
+    WaitIfLessThan,
+    DecrementAndWaitIfLessThan,
+    WaitIfLessThanWithTimeout,
+    DecrementAndWaitIfLessThanWithTimeout,
+};
+
+/// Arbitrate an address
+Result ArbitrateAddress(Handle handle, ArbitrationType type, u32 address, s32 value);
+
+/// Create an address arbiter
+Handle CreateAddressArbiter(const std::string& name = "Unknown");
+
+} // namespace FileSys
diff --git a/src/core/hle/kernel/kernel.h b/src/core/hle/kernel/kernel.h
index 69f4ddd37..d9afcdd25 100644
--- a/src/core/hle/kernel/kernel.h
+++ b/src/core/hle/kernel/kernel.h
@@ -26,7 +26,7 @@ enum class HandleType : u32 {
     Redirection     = 6,
     Thread          = 7,
     Process         = 8,
-    Arbiter         = 9,
+    AddressArbiter  = 9,
     File            = 10,
     Semaphore       = 11,
     Archive         = 12,
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index ab5a5559e..86bbf29d0 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -188,6 +188,43 @@ void ChangeThreadState(Thread* t, ThreadStatus new_status) {
     }
 }
 
+/// Arbitrate the highest priority thread that is waiting
+Handle ArbitrateHighestPriorityThread(u32 arbiter, u32 address) {
+    Handle highest_priority_thread = 0;
+    s32 priority = THREADPRIO_LOWEST;
+
+    // Iterate through threads, find highest priority thread that is waiting to be arbitrated...
+    for (const auto& handle : g_thread_queue) {
+
+        // TODO(bunnei): Verify arbiter address...
+        if (!VerifyWait(handle, WAITTYPE_ARB, arbiter))
+            continue;
+
+        Thread* thread = g_object_pool.GetFast<Thread>(handle);
+        if(thread->current_priority <= priority) {
+            highest_priority_thread = handle;
+            priority = thread->current_priority;
+        }
+    }
+    // If a thread was arbitrated, resume it
+    if (0 != highest_priority_thread)
+        ResumeThreadFromWait(highest_priority_thread);
+
+    return highest_priority_thread;
+}
+
+/// Arbitrate all threads currently waiting
+void ArbitrateAllThreads(u32 arbiter, u32 address) {
+    
+    // Iterate through threads, find highest priority thread that is waiting to be arbitrated...
+    for (const auto& handle : g_thread_queue) {
+
+        // TODO(bunnei): Verify arbiter address...
+        if (VerifyWait(handle, WAITTYPE_ARB, arbiter))
+            ResumeThreadFromWait(handle);
+    }
+}
+
 /// Calls a thread by marking it as "ready" (note: will not actually execute until current thread yields)
 void CallThread(Thread* t) {
     // Stop waiting
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 04914ba90..f2bfdfa1a 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -39,6 +39,7 @@ enum WaitType {
     WAITTYPE_VBLANK,
     WAITTYPE_MUTEX,
     WAITTYPE_SYNCH,
+    WAITTYPE_ARB,
 };
 
 namespace Kernel {
@@ -59,6 +60,12 @@ void StopThread(Handle thread, const char* reason);
 /// Resumes a thread from waiting by marking it as "ready"
 void ResumeThreadFromWait(Handle handle);
 
+/// Arbitrate the highest priority thread that is waiting
+Handle ArbitrateHighestPriorityThread(u32 arbiter, u32 address);
+
+/// Arbitrate all threads currently waiting...
+void ArbitrateAllThreads(u32 arbiter, u32 address);
+
 /// Gets the current thread handle
 Handle GetCurrentThreadHandle();
 
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index 9130d77ec..17967f260 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -9,6 +9,7 @@
 
 #include "core/mem_map.h"
 
+#include "core/hle/kernel/address_arbiter.h"
 #include "core/hle/kernel/event.h"
 #include "core/hle/kernel/kernel.h"
 #include "core/hle/kernel/mutex.h"
@@ -175,18 +176,19 @@ Result WaitSynchronizationN(s32* out, Handle* handles, s32 handle_count, bool wa
 }
 
 /// Create an address arbiter (to allocate access to shared resources)
-Result CreateAddressArbiter(void* arbiter) {
-    ERROR_LOG(SVC, "(UNIMPLEMENTED) called");
-    Core::g_app_core->SetReg(1, 0xFABBDADD);
+Result CreateAddressArbiter(u32* arbiter) {
+    DEBUG_LOG(SVC, "called");
+    Handle handle = Kernel::CreateAddressArbiter();
+    *arbiter = handle;
     return 0;
 }
 
 /// Arbitrate address
-Result ArbitrateAddress(Handle arbiter, u32 addr, u32 _type, u32 value, s64 nanoseconds) {
-    ERROR_LOG(SVC, "(UNIMPLEMENTED) called");
-    ArbitrationType type = (ArbitrationType)_type;
-    Memory::Write32(addr, type);
-    return 0;
+Result ArbitrateAddress(Handle arbiter, u32 address, u32 type, u32 value, s64 nanoseconds) {
+    DEBUG_LOG(SVC, "called arbiter=0x%08X, address=0x%08X, type=0x%08X, value=0x%08X, "
+        "nanoseconds=%d", arbiter, address, type, value, nanoseconds);
+    return Kernel::ArbitrateAddress(arbiter, static_cast<Kernel::ArbitrationType>(type), address, 
+        value);
 }
 
 /// Used to output a message on a debug hardware unit - does nothing on a retail unit