9 files changed, 346 insertions, 49 deletions

diff --git a/src/core/arm/dynarmic/arm_dynarmic_64.cpp b/src/core/arm/dynarmic/arm_dynarmic_64.cpp
index 1d46f6d40..22b5d5656 100644
--- a/src/core/arm/dynarmic/arm_dynarmic_64.cpp
+++ b/src/core/arm/dynarmic/arm_dynarmic_64.cpp
@@ -111,6 +111,7 @@ public:
         LOG_ERROR(Core_ARM,
                   "Unimplemented instruction @ 0x{:X} for {} instructions (instr = {:08X})", pc,
                   num_instructions, memory.Read32(pc));
+        ReturnException(pc, ARM_Interface::no_execute);
     }
 
     void InstructionCacheOperationRaised(Dynarmic::A64::InstructionCacheOperation op,
diff --git a/src/core/hle/ipc_helpers.h b/src/core/hle/ipc_helpers.h
index d631c0357..0cc26a211 100644
--- a/src/core/hle/ipc_helpers.h
+++ b/src/core/hle/ipc_helpers.h
@@ -152,7 +152,8 @@ public:
                 Kernel::LimitableResource::Sessions, 1);
 
             auto* session = Kernel::KSession::Create(kernel);
-            session->Initialize(nullptr, iface->GetServiceName());
+            session->Initialize(nullptr, iface->GetServiceName(),
+                                std::make_shared<Kernel::SessionRequestManager>(kernel));
 
             context->AddMoveObject(&session->GetClientSession());
             iface->ClientConnected(&session->GetServerSession());
diff --git a/src/core/hle/kernel/k_client_session.cpp b/src/core/hle/kernel/k_client_session.cpp
index b2a887b14..8892c5b7c 100644
--- a/src/core/hle/kernel/k_client_session.cpp
+++ b/src/core/hle/kernel/k_client_session.cpp
@@ -21,10 +21,9 @@ void KClientSession::Destroy() {
 
 void KClientSession::OnServerClosed() {}
 
-Result KClientSession::SendSyncRequest(KThread* thread, Core::Memory::Memory& memory,
-                                       Core::Timing::CoreTiming& core_timing) {
+Result KClientSession::SendSyncRequest() {
     // Signal the server session that new data is available
-    return parent->GetServerSession().HandleSyncRequest(thread, memory, core_timing);
+    return parent->GetServerSession().OnRequest();
 }
 
 } // namespace Kernel
diff --git a/src/core/hle/kernel/k_client_session.h b/src/core/hle/kernel/k_client_session.h
index 0c750d756..b4a19c546 100644
--- a/src/core/hle/kernel/k_client_session.h
+++ b/src/core/hle/kernel/k_client_session.h
@@ -46,8 +46,7 @@ public:
         return parent;
     }
 
-    Result SendSyncRequest(KThread* thread, Core::Memory::Memory& memory,
-                           Core::Timing::CoreTiming& core_timing);
+    Result SendSyncRequest();
 
     void OnServerClosed();
 
diff --git a/src/core/hle/kernel/k_server_session.cpp b/src/core/hle/kernel/k_server_session.cpp
index 802c646a6..4252c9adb 100644
--- a/src/core/hle/kernel/k_server_session.cpp
+++ b/src/core/hle/kernel/k_server_session.cpp
@@ -7,6 +7,8 @@
 #include "common/assert.h"
 #include "common/common_types.h"
 #include "common/logging/log.h"
+#include "common/scope_exit.h"
+#include "core/core.h"
 #include "core/core_timing.h"
 #include "core/hle/ipc_helpers.h"
 #include "core/hle/kernel/hle_ipc.h"
@@ -18,13 +20,19 @@
 #include "core/hle/kernel/k_server_session.h"
 #include "core/hle/kernel/k_session.h"
 #include "core/hle/kernel/k_thread.h"
+#include "core/hle/kernel/k_thread_queue.h"
 #include "core/hle/kernel/kernel.h"
 #include "core/hle/kernel/service_thread.h"
 #include "core/memory.h"
 
 namespace Kernel {
 
-KServerSession::KServerSession(KernelCore& kernel_) : KSynchronizationObject{kernel_} {}
+using ThreadQueueImplForKServerSessionRequest = KThreadQueue;
+
+static constexpr u32 MessageBufferSize = 0x100;
+
+KServerSession::KServerSession(KernelCore& kernel_)
+    : KSynchronizationObject{kernel_}, m_lock{kernel_} {}
 
 KServerSession::~KServerSession() = default;
 
@@ -33,17 +41,14 @@ void KServerSession::Initialize(KSession* parent_session_, std::string&& name_,
     // Set member variables.
     parent = parent_session_;
     name = std::move(name_);
-
-    if (manager_) {
-        manager = manager_;
-    } else {
-        manager = std::make_shared<SessionRequestManager>(kernel);
-    }
+    manager = manager_;
 }
 
 void KServerSession::Destroy() {
     parent->OnServerClosed();
 
+    this->CleanupRequests();
+
     parent->Close();
 
     // Release host emulation members.
@@ -54,13 +59,13 @@ void KServerSession::Destroy() {
 }
 
 void KServerSession::OnClientClosed() {
-    if (manager->HasSessionHandler()) {
+    if (manager && manager->HasSessionHandler()) {
         manager->SessionHandler().ClientDisconnected(this);
     }
 }
 
 bool KServerSession::IsSignaled() const {
-    ASSERT(kernel.GlobalSchedulerContext().IsLocked());
+    ASSERT(KScheduler::IsSchedulerLockedByCurrentThread(kernel));
 
     // If the client is closed, we're always signaled.
     if (parent->IsClientClosed()) {
@@ -68,7 +73,7 @@ bool KServerSession::IsSignaled() const {
     }
 
     // Otherwise, we're signaled if we have a request and aren't handling one.
-    return false;
+    return !m_thread_request_list.empty() && m_current_thread_request == nullptr;
 }
 
 void KServerSession::AppendDomainHandler(SessionRequestHandlerPtr handler) {
@@ -173,9 +178,221 @@ Result KServerSession::CompleteSyncRequest(HLERequestContext& context) {
     return result;
 }
 
-Result KServerSession::HandleSyncRequest(KThread* thread, Core::Memory::Memory& memory,
-                                         Core::Timing::CoreTiming& core_timing) {
-    return QueueSyncRequest(thread, memory);
+Result KServerSession::OnRequest() {
+    // Create the wait queue.
+    ThreadQueueImplForKServerSessionRequest wait_queue{kernel};
+
+    {
+        // Lock the scheduler.
+        KScopedSchedulerLock sl{kernel};
+
+        // Ensure that we can handle new requests.
+        R_UNLESS(!parent->IsServerClosed(), ResultSessionClosed);
+
+        // Check that we're not terminating.
+        R_UNLESS(!GetCurrentThread(kernel).IsTerminationRequested(), ResultTerminationRequested);
+
+        if (manager) {
+            // HLE request.
+            auto& memory{kernel.System().Memory()};
+            this->QueueSyncRequest(GetCurrentThreadPointer(kernel), memory);
+        } else {
+            // Non-HLE request.
+            auto* thread{GetCurrentThreadPointer(kernel)};
+
+            // Get whether we're empty.
+            const bool was_empty = m_thread_request_list.empty();
+
+            // Add the thread to the list.
+            thread->Open();
+            m_thread_request_list.push_back(thread);
+
+            // If we were empty, signal.
+            if (was_empty) {
+                this->NotifyAvailable();
+            }
+        }
+
+        // This is a synchronous request, so we should wait for our request to complete.
+        GetCurrentThread(kernel).SetWaitReasonForDebugging(ThreadWaitReasonForDebugging::IPC);
+        GetCurrentThread(kernel).BeginWait(&wait_queue);
+    }
+
+    return GetCurrentThread(kernel).GetWaitResult();
+}
+
+Result KServerSession::SendReply() {
+    // Lock the session.
+    KScopedLightLock lk(m_lock);
+
+    // Get the request.
+    KThread* client_thread;
+    {
+        KScopedSchedulerLock sl{kernel};
+
+        // Get the current request.
+        client_thread = m_current_thread_request;
+        R_UNLESS(client_thread != nullptr, ResultInvalidState);
+
+        // Clear the current request, since we're processing it.
+        m_current_thread_request = nullptr;
+        if (!m_thread_request_list.empty()) {
+            this->NotifyAvailable();
+        }
+    }
+
+    // Close reference to the request once we're done processing it.
+    SCOPE_EXIT({ client_thread->Close(); });
+
+    // Extract relevant information from the request.
+    // const uintptr_t client_message  = request->GetAddress();
+    // const size_t client_buffer_size = request->GetSize();
+    // KThread *client_thread          = request->GetThread();
+    // KEvent *event                   = request->GetEvent();
+
+    // Check whether we're closed.
+    const bool closed = (client_thread == nullptr || parent->IsClientClosed());
+
+    Result result = ResultSuccess;
+    if (!closed) {
+        // If we're not closed, send the reply.
+        Core::Memory::Memory& memory{kernel.System().Memory()};
+        KThread* server_thread{GetCurrentThreadPointer(kernel)};
+        UNIMPLEMENTED_IF(server_thread->GetOwnerProcess() != client_thread->GetOwnerProcess());
+
+        auto* src_msg_buffer = memory.GetPointer(server_thread->GetTLSAddress());
+        auto* dst_msg_buffer = memory.GetPointer(client_thread->GetTLSAddress());
+        std::memcpy(dst_msg_buffer, src_msg_buffer, MessageBufferSize);
+    } else {
+        result = ResultSessionClosed;
+    }
+
+    // Select a result for the client.
+    Result client_result = result;
+    if (closed && R_SUCCEEDED(result)) {
+        result = ResultSessionClosed;
+        client_result = ResultSessionClosed;
+    } else {
+        result = ResultSuccess;
+    }
+
+    // If there's a client thread, update it.
+    if (client_thread != nullptr) {
+        // End the client thread's wait.
+        KScopedSchedulerLock sl{kernel};
+
+        if (!client_thread->IsTerminationRequested()) {
+            client_thread->EndWait(client_result);
+        }
+    }
+
+    return result;
+}
+
+Result KServerSession::ReceiveRequest() {
+    // Lock the session.
+    KScopedLightLock lk(m_lock);
+
+    // Get the request and client thread.
+    // KSessionRequest *request;
+    KThread* client_thread;
+
+    {
+        KScopedSchedulerLock sl{kernel};
+
+        // Ensure that we can service the request.
+        R_UNLESS(!parent->IsClientClosed(), ResultSessionClosed);
+
+        // Ensure we aren't already servicing a request.
+        R_UNLESS(m_current_thread_request == nullptr, ResultNotFound);
+
+        // Ensure we have a request to service.
+        R_UNLESS(!m_thread_request_list.empty(), ResultNotFound);
+
+        // Pop the first request from the list.
+        client_thread = m_thread_request_list.front();
+        m_thread_request_list.pop_front();
+
+        // Get the thread for the request.
+        R_UNLESS(client_thread != nullptr, ResultSessionClosed);
+
+        // Open the client thread.
+        client_thread->Open();
+    }
+
+    // SCOPE_EXIT({ client_thread->Close(); });
+
+    // Set the request as our current.
+    m_current_thread_request = client_thread;
+
+    // Receive the message.
+    Core::Memory::Memory& memory{kernel.System().Memory()};
+    KThread* server_thread{GetCurrentThreadPointer(kernel)};
+    UNIMPLEMENTED_IF(server_thread->GetOwnerProcess() != client_thread->GetOwnerProcess());
+
+    auto* src_msg_buffer = memory.GetPointer(client_thread->GetTLSAddress());
+    auto* dst_msg_buffer = memory.GetPointer(server_thread->GetTLSAddress());
+    std::memcpy(dst_msg_buffer, src_msg_buffer, MessageBufferSize);
+
+    // We succeeded.
+    return ResultSuccess;
+}
+
+void KServerSession::CleanupRequests() {
+    KScopedLightLock lk(m_lock);
+
+    // Clean up any pending requests.
+    while (true) {
+        // Get the next request.
+        // KSessionRequest *request = nullptr;
+        KThread* client_thread = nullptr;
+        {
+            KScopedSchedulerLock sl{kernel};
+
+            if (m_current_thread_request) {
+                // Choose the current request if we have one.
+                client_thread = m_current_thread_request;
+                m_current_thread_request = nullptr;
+            } else if (!m_thread_request_list.empty()) {
+                // Pop the request from the front of the list.
+                client_thread = m_thread_request_list.front();
+                m_thread_request_list.pop_front();
+            }
+        }
+
+        // If there's no request, we're done.
+        if (client_thread == nullptr) {
+            break;
+        }
+
+        // Close a reference to the request once it's cleaned up.
+        SCOPE_EXIT({ client_thread->Close(); });
+
+        // Extract relevant information from the request.
+        // const uintptr_t client_message  = request->GetAddress();
+        // const size_t client_buffer_size = request->GetSize();
+        // KThread *client_thread          = request->GetThread();
+        // KEvent *event                   = request->GetEvent();
+
+        // KProcess *server_process             = request->GetServerProcess();
+        // KProcess *client_process             = (client_thread != nullptr) ?
+        //                                         client_thread->GetOwnerProcess() : nullptr;
+        // KProcessPageTable *client_page_table = (client_process != nullptr) ?
+        //                                         &client_process->GetPageTable() : nullptr;
+
+        // Cleanup the mappings.
+        // Result result = CleanupMap(request, server_process, client_page_table);
+
+        // If there's a client thread, update it.
+        if (client_thread != nullptr) {
+            // End the client thread's wait.
+            KScopedSchedulerLock sl{kernel};
+
+            if (!client_thread->IsTerminationRequested()) {
+                client_thread->EndWait(ResultSessionClosed);
+            }
+        }
+    }
 }
 
 } // namespace Kernel
diff --git a/src/core/hle/kernel/k_server_session.h b/src/core/hle/kernel/k_server_session.h
index 6d0821945..748d52826 100644
--- a/src/core/hle/kernel/k_server_session.h
+++ b/src/core/hle/kernel/k_server_session.h
@@ -3,6 +3,7 @@
 
 #pragma once
 
+#include <list>
 #include <memory>
 #include <string>
 #include <utility>
@@ -10,6 +11,7 @@
 #include <boost/intrusive/list.hpp>
 
 #include "core/hle/kernel/hle_ipc.h"
+#include "core/hle/kernel/k_light_lock.h"
 #include "core/hle/kernel/k_synchronization_object.h"
 #include "core/hle/result.h"
 
@@ -59,25 +61,15 @@ public:
     void OnClientClosed();
 
     void ClientConnected(SessionRequestHandlerPtr handler) {
-        manager->SetSessionHandler(std::move(handler));
+        if (manager) {
+            manager->SetSessionHandler(std::move(handler));
+        }
     }
 
     void ClientDisconnected() {
         manager = nullptr;
     }
 
-    /**
-     * Handle a sync request from the emulated application.
-     *
-     * @param thread      Thread that initiated the request.
-     * @param memory      Memory context to handle the sync request under.
-     * @param core_timing Core timing context to schedule the request event under.
-     *
-     * @returns Result from the operation.
-     */
-    Result HandleSyncRequest(KThread* thread, Core::Memory::Memory& memory,
-                             Core::Timing::CoreTiming& core_timing);
-
     /// Adds a new domain request handler to the collection of request handlers within
     /// this ServerSession instance.
     void AppendDomainHandler(SessionRequestHandlerPtr handler);
@@ -88,7 +80,7 @@ public:
 
     /// Returns true if the session has been converted to a domain, otherwise False
     bool IsDomain() const {
-        return manager->IsDomain();
+        return manager && manager->IsDomain();
     }
 
     /// Converts the session to a domain at the end of the current command
@@ -101,7 +93,15 @@ public:
         return manager;
     }
 
+    /// TODO: flesh these out to match the real kernel
+    Result OnRequest();
+    Result SendReply();
+    Result ReceiveRequest();
+
 private:
+    /// Frees up waiting client sessions when this server session is about to die
+    void CleanupRequests();
+
     /// Queues a sync request from the emulated application.
     Result QueueSyncRequest(KThread* thread, Core::Memory::Memory& memory);
 
@@ -112,7 +112,7 @@ private:
     /// object handle.
     Result HandleDomainSyncRequest(Kernel::HLERequestContext& context);
 
-    /// This session's HLE request handlers
+    /// This session's HLE request handlers; if nullptr, this is not an HLE server
     std::shared_ptr<SessionRequestManager> manager;
 
     /// When set to True, converts the session to a domain at the end of the command
@@ -120,6 +120,13 @@ private:
 
     /// KSession that owns this KServerSession
     KSession* parent{};
+
+    /// List of threads which are pending a reply.
+    /// FIXME: KSessionRequest
+    std::list<KThread*> m_thread_request_list;
+    KThread* m_current_thread_request{};
+
+    KLightLock m_lock;
 };
 
 } // namespace Kernel
diff --git a/src/core/hle/kernel/svc.cpp b/src/core/hle/kernel/svc.cpp
index 3a89511aa..510a9b3e3 100644
--- a/src/core/hle/kernel/svc.cpp
+++ b/src/core/hle/kernel/svc.cpp
@@ -323,7 +323,7 @@ Result CreateSession(Core::System& system, Handle* out_server, Handle* out_clien
     // Add the server session to the handle table.
     R_TRY(handle_table.Add(out_server, &session->GetServerSession()));
 
-    // Add the client session to the handle table. */
+    // Add the client session to the handle table.
     const auto result = handle_table.Add(out_client, &session->GetClientSession());
 
     if (!R_SUCCEEDED(result)) {
@@ -383,7 +383,8 @@ static Result ConnectToNamedPort(Core::System& system, Handle* out, VAddr port_n
 
     // Create a session.
     KClientSession* session{};
-    R_TRY(port->CreateSession(std::addressof(session)));
+    R_TRY(port->CreateSession(std::addressof(session),
+                              std::make_shared<SessionRequestManager>(kernel)));
     port->Close();
 
     // Register the session in the table, close the extra reference.
@@ -401,7 +402,7 @@ static Result ConnectToNamedPort32(Core::System& system, Handle* out_handle,
     return ConnectToNamedPort(system, out_handle, port_name_address);
 }
 
-/// Makes a blocking IPC call to an OS service.
+/// Makes a blocking IPC call to a service.
 static Result SendSyncRequest(Core::System& system, Handle handle) {
     auto& kernel = system.Kernel();
 
@@ -415,22 +416,75 @@ static Result SendSyncRequest(Core::System& system, Handle handle) {
 
     LOG_TRACE(Kernel_SVC, "called handle=0x{:08X}({})", handle, session->GetName());
 
-    {
-        KScopedSchedulerLock lock(kernel);
-
-        // This is a synchronous request, so we should wait for our request to complete.
-        GetCurrentThread(kernel).BeginWait(std::addressof(wait_queue));
-        GetCurrentThread(kernel).SetWaitReasonForDebugging(ThreadWaitReasonForDebugging::IPC);
-        session->SendSyncRequest(&GetCurrentThread(kernel), system.Memory(), system.CoreTiming());
-    }
-
-    return GetCurrentThread(kernel).GetWaitResult();
+    return session->SendSyncRequest();
 }
 
 static Result SendSyncRequest32(Core::System& system, Handle handle) {
     return SendSyncRequest(system, handle);
 }
 
+static Result ReplyAndReceive(Core::System& system, s32* out_index, Handle* handles,
+                              s32 num_handles, Handle reply_target, s64 timeout_ns) {
+    auto& kernel = system.Kernel();
+    auto& handle_table = GetCurrentThread(kernel).GetOwnerProcess()->GetHandleTable();
+
+    // Convert handle list to object table.
+    std::vector<KSynchronizationObject*> objs(num_handles);
+    R_UNLESS(
+        handle_table.GetMultipleObjects<KSynchronizationObject>(objs.data(), handles, num_handles),
+        ResultInvalidHandle);
+
+    // Ensure handles are closed when we're done.
+    SCOPE_EXIT({
+        for (auto i = 0; i < num_handles; ++i) {
+            objs[i]->Close();
+        }
+    });
+
+    // Reply to the target, if one is specified.
+    if (reply_target != InvalidHandle) {
+        KScopedAutoObject session = handle_table.GetObject<KServerSession>(reply_target);
+        R_UNLESS(session.IsNotNull(), ResultInvalidHandle);
+
+        // If we fail to reply, we want to set the output index to -1.
+        // ON_RESULT_FAILURE { *out_index = -1; };
+
+        // Send the reply.
+        // R_TRY(session->SendReply());
+
+        Result rc = session->SendReply();
+        if (!R_SUCCEEDED(rc)) {
+            *out_index = -1;
+            return rc;
+        }
+    }
+
+    // Wait for a message.
+    while (true) {
+        // Wait for an object.
+        s32 index;
+        Result result = KSynchronizationObject::Wait(kernel, &index, objs.data(),
+                                                     static_cast<s32>(objs.size()), timeout_ns);
+        if (result == ResultTimedOut) {
+            return result;
+        }
+
+        // Receive the request.
+        if (R_SUCCEEDED(result)) {
+            KServerSession* session = objs[index]->DynamicCast<KServerSession*>();
+            if (session != nullptr) {
+                result = session->ReceiveRequest();
+                if (result == ResultNotFound) {
+                    continue;
+                }
+            }
+        }
+
+        *out_index = index;
+        return result;
+    }
+}
+
 /// Get the ID for the specified thread.
 static Result GetThreadId(Core::System& system, u64* out_thread_id, Handle thread_handle) {
     // Get the thread from its handle.
@@ -2951,7 +3005,7 @@ static const FunctionDef SVC_Table_64[] = {
     {0x40, SvcWrap64<CreateSession>, "CreateSession"},
     {0x41, nullptr, "AcceptSession"},
     {0x42, nullptr, "ReplyAndReceiveLight"},
-    {0x43, nullptr, "ReplyAndReceive"},
+    {0x43, SvcWrap64<ReplyAndReceive>, "ReplyAndReceive"},
     {0x44, nullptr, "ReplyAndReceiveWithUserBuffer"},
     {0x45, SvcWrap64<CreateEvent>, "CreateEvent"},
     {0x46, nullptr, "MapIoRegion"},
diff --git a/src/core/hle/kernel/svc_wrap.h b/src/core/hle/kernel/svc_wrap.h
index 16bf65802..272c54cf7 100644
--- a/src/core/hle/kernel/svc_wrap.h
+++ b/src/core/hle/kernel/svc_wrap.h
@@ -8,6 +8,7 @@
 #include "core/core.h"
 #include "core/hle/kernel/svc_types.h"
 #include "core/hle/result.h"
+#include "core/memory.h"
 
 namespace Kernel {
 
@@ -360,6 +361,23 @@ void SvcWrap64(Core::System& system) {
     FuncReturn(system, retval);
 }
 
+// Used by ReplyAndReceive
+template <Result func(Core::System&, s32*, Handle*, s32, Handle, s64)>
+void SvcWrap64(Core::System& system) {
+    s32 param_1 = 0;
+    s32 num_handles = static_cast<s32>(Param(system, 2));
+
+    std::vector<Handle> handles(num_handles);
+    system.Memory().ReadBlock(Param(system, 1), handles.data(), num_handles * sizeof(Handle));
+
+    const u32 retval = func(system, &param_1, handles.data(), num_handles,
+                            static_cast<s32>(Param(system, 3)), static_cast<s64>(Param(system, 4)))
+                           .raw;
+
+    system.CurrentArmInterface().SetReg(1, param_1);
+    FuncReturn(system, retval);
+}
+
 // Used by WaitForAddress
 template <Result func(Core::System&, u64, Svc::ArbitrationType, s32, s64)>
 void SvcWrap64(Core::System& system) {
diff --git a/src/core/hle/service/sm/sm.cpp b/src/core/hle/service/sm/sm.cpp
index 246c94623..48e70f93c 100644
--- a/src/core/hle/service/sm/sm.cpp
+++ b/src/core/hle/service/sm/sm.cpp
@@ -156,7 +156,8 @@ ResultVal<Kernel::KClientSession*> SM::GetServiceImpl(Kernel::HLERequestContext&
 
     // Create a new session.
     Kernel::KClientSession* session{};
-    if (const auto result = port->GetClientPort().CreateSession(std::addressof(session));
+    if (const auto result = port->GetClientPort().CreateSession(
+            std::addressof(session), std::make_shared<Kernel::SessionRequestManager>(kernel));
         result.IsError()) {
         LOG_ERROR(Service_SM, "called service={} -> error 0x{:08X}", name, result.raw);
         return result;