From 85f0adbb2de3faa3334232ebbbd3e8b022b67779 Mon Sep 17 00:00:00 2001
From: Sean Cross <sean@xobs.io>
Date: Fri, 2 Apr 2021 18:37:09 +0800
Subject: [PATCH] kernel: implement Disconnect() syscall

Signed-off-by: Sean Cross <sean@xobs.io>
---
 kernel/src/services.rs | 36 ++++++++++++++++++++++++++++++++++++
 kernel/src/syscall.rs  | 14 ++++++++++++--
 2 files changed, 48 insertions(+), 2 deletions(-)

diff --git a/kernel/src/services.rs b/kernel/src/services.rs
index 899d6c8e5..4aa30c7ec 100644
--- a/kernel/src/services.rs
+++ b/kernel/src/services.rs
@@ -1525,6 +1525,42 @@ impl SystemServices {
         })
     }
 
+    /// Invalidate the provided connection ID.
+    pub fn disconnect_from_server(&mut self, cid: CID) -> Result<(), xous_kernel::Error> {
+        // Check to see if we've already connected to this server.
+        // While doing this, find a free slot in case we haven't
+        // yet connected.
+
+        // Slot indices are offset by two. Ensure we don't underflow.
+        let slot_idx = cid;
+        if slot_idx < 2 {
+            klog!("CID {} is not valid", cid);
+            return Err(xous_kernel::Error::ServerNotFound);
+        }
+        let slot_idx = (slot_idx - 2) as usize;
+        let pid = crate::arch::process::current_pid();
+        // klog!("KERNEL({}): Server table: {:?}", pid.get(), self.servers);
+        ArchProcess::with_inner_mut(|process_inner| {
+            assert_eq!(pid, process_inner.pid);
+            if slot_idx >= process_inner.connection_map.len() {
+                klog!("Slot index exceeds map length");
+                return Err(xous_kernel::Error::ServerNotFound);
+            }
+
+            // If the server ID is None, then we weren't connected in the first place.
+            let idx = &mut process_inner.connection_map[slot_idx];
+            if idx.is_none() {
+                klog!("IDX[{}] is already None!", slot_idx);
+                return Err(xous_kernel::Error::ServerNotFound);
+            }
+
+            // Nullify this connection ID. It may now be reused.
+            *idx = None;
+            klog!("Removing server from table");
+            Ok(())
+        })
+    }
+
     /// Retrieve the server ID index from the specified SID.
     /// This may only be called if the SID is a server owned by
     /// the current process.
diff --git a/kernel/src/syscall.rs b/kernel/src/syscall.rs
index cc023bc04..d14c3ae65 100644
--- a/kernel/src/syscall.rs
+++ b/kernel/src/syscall.rs
@@ -341,7 +341,11 @@ fn return_memory(
             // print!(" [waking up PID {}:{}]", client_pid, client_tid);
             ss.ready_thread(client_pid, client_tid)?;
             ss.switch_to_thread(client_pid, Some(client_tid))?;
-            ss.set_thread_result(client_pid, client_tid, xous_kernel::Result::MemoryReturned(offset, valid))?;
+            ss.set_thread_result(
+                client_pid,
+                client_tid,
+                xous_kernel::Result::MemoryReturned(offset, valid),
+            )?;
 
             // Return success to the server
             Ok(xous_kernel::Result::Ok)
@@ -782,7 +786,9 @@ pub fn handle_inner(pid: PID, tid: TID, in_irq: bool, call: SysCall) -> SysCallR
             ss.connect_to_server(sid)
                 .map(xous_kernel::Result::ConnectionID)
         }),
-        SysCall::ReturnMemory(sender, buf, offset, valid) => return_memory(pid, tid, in_irq, sender, buf, offset, valid),
+        SysCall::ReturnMemory(sender, buf, offset, valid) => {
+            return_memory(pid, tid, in_irq, sender, buf, offset, valid)
+        }
         SysCall::ReturnScalar1(sender, arg) => return_scalar(pid, tid, in_irq, sender, arg),
         SysCall::ReturnScalar2(sender, arg1, arg2) => {
             return_scalar2(pid, tid, in_irq, sender, arg1, arg2)
@@ -834,6 +840,10 @@ pub fn handle_inner(pid: PID, tid: TID, in_irq: bool, call: SysCall) -> SysCallR
                 Err(e) => Err(e),
             }
         }
+        SysCall::Disconnect(cid) => SystemServices::with_mut(|ss| {
+            ss.disconnect_from_server(cid)
+                .and(Ok(xous_kernel::Result::Ok))
+        }),
         _ => Err(xous_kernel::Error::UnhandledSyscall),
     }
 }