cleaning up simulation -- more tests pass

Signed-off-by: Sean Cross <sean@xobs.io>

crates/riscv-cpu/src/cpu.rs

@@ -1,4 +1,4 @@
-use std::sync::{Arc, RwLock};
+use std::sync::{Arc, RwLock, mpsc::Receiver};
 
 pub use super::mmu::Memory;
 use super::mmu::{AddressingMode, Mmu};
@@ -54,6 +54,13 @@ pub const MIP_SEIP: u64 = 0x200;
 const MIP_STIP: u64 = 0x020;
 const MIP_SSIP: u64 = 0x002;
 
+pub enum TickResult {
+    Ok,
+    ExitThread(u64),
+    PauseEmulation(Receiver<([i64; 8], Option<(Vec<u8>, u64)>)>),
+}
+
+
 /// Emulates a RISC-V CPU core
 pub struct Cpu {
     clock: u64,
@@ -120,6 +127,7 @@ pub enum TrapType {
     UserExternalInterrupt,
     SupervisorExternalInterrupt,
     MachineExternalInterrupt,
+    PauseEmulation(std::sync::mpsc::Receiver<([i64; 8], Option<(Vec<u8>, u64)>)>),
 }
 
 fn _get_privilege_mode_name(mode: &PrivilegeMode) -> &'static str {
@@ -176,6 +184,7 @@ fn _get_trap_type_name(trap_type: &TrapType) -> &'static str {
         TrapType::UserExternalInterrupt => "UserExternalInterrupt",
         TrapType::SupervisorExternalInterrupt => "SupervisorExternalInterrupt",
         TrapType::MachineExternalInterrupt => "MachineExternalInterrupt",
+        TrapType::PauseEmulation(_) => "PauseEmulation",
     }
 }
 
@@ -199,6 +208,7 @@ fn get_trap_cause(trap: &Trap, xlen: &Xlen) -> u64 {
         TrapType::InstructionPageFault => 12,
         TrapType::LoadPageFault => 13,
         TrapType::StorePageFault => 15,
+        TrapType::PauseEmulation(_) => 16,
         TrapType::UserSoftwareInterrupt => interrupt_bit,
         TrapType::SupervisorSoftwareInterrupt => interrupt_bit + 1,
         TrapType::MachineSoftwareInterrupt => interrupt_bit + 3,
@@ -213,6 +223,8 @@ fn get_trap_cause(trap: &Trap, xlen: &Xlen) -> u64 {
 
 pub struct CpuBuilder {
     xlen: Xlen,
+    pc: u64,
+    sp: u64,
     memory: Arc<RwLock<dyn Memory + Send + Sync>>,
 }
 
@@ -221,6 +233,8 @@ impl CpuBuilder {
         CpuBuilder {
             xlen: Xlen::Bit64,
             memory,
+            pc: 0,
+            sp: 0,
         }
     }
 
@@ -229,9 +243,20 @@ impl CpuBuilder {
         self
     }
 
+    pub fn pc(mut self, pc: u64) -> Self {
+        self.pc = pc;
+        self
+    }
+
+    pub fn sp(mut self, sp: u64) -> Self {
+        self.sp = sp;
+        self
+    }
     pub fn build(self) -> Cpu {
         let mut cpu = Cpu::new(self.memory);
         cpu.update_xlen(self.xlen.clone());
+        cpu.update_pc(self.pc);
+        cpu.write_register(2, self.sp as i64);
         cpu
     }
 }
@@ -317,10 +342,22 @@ impl Cpu {
     }
 
     /// Runs program one cycle. Fetch, decode, and execution are completed in a cycle so far.
-    pub fn tick(&mut self) {
+    pub fn tick(&mut self) -> TickResult {
         let instruction_address = self.pc;
         match self.tick_operate() {
             Ok(()) => {}
+            Err(Trap {
+                trap_type: TrapType::PauseEmulation(rx),
+                ..
+            }) => {
+                return TickResult::PauseEmulation(rx);
+            }
+            Err(Trap {
+                trap_type: TrapType::InstructionPageFault,
+                value: 0xff803000,
+            }) => {
+                return TickResult::ExitThread(self.read_register(10) as u64);
+            }
             Err(e) => self.handle_exception(e, instruction_address),
         }
         self.mmu.tick(&mut self.csr[CSR_MIP_ADDRESS as usize]);
@@ -331,6 +368,8 @@ impl Cpu {
         // just an arbiraty ratio.
         // @TODO: Implement more properly
         self.write_csr_raw(CSR_CYCLE_ADDRESS, self.clock * 8);
+
+        TickResult::Ok
     }
 
     // @TODO: Rename?
@@ -742,7 +781,7 @@ impl Cpu {
         // println!("Fetching word from {:08x}...", self.pc);
         self.mmu.fetch_word(self.pc).map_err(|e| {
             self.pc = self.pc.wrapping_add(4); // @TODO: What if instruction is compressed?
-            println!("Fetch error: {:x?}", e);
+            // println!("Fetch error: {:x?}", e);
             e
         })
     }
@@ -1465,14 +1504,6 @@ impl Cpu {
         &mut self.mmu
     }
 
-    // pub fn memory_base(&self) -> u64 {
-    //     self.memory_base
-    // }
-
-    // pub fn memory_size(&self) -> u64 {
-    //     self.mmu.memory_size()
-    // }
-
     pub fn phys_read_u32(&self, address: u64) -> u32 {
         self.mmu.load_word_raw(address)
     }
@@ -2413,16 +2444,23 @@ const INSTRUCTIONS: [Instruction; INSTRUCTION_NUM] = [
         mask: 0xffffffff,
         data: 0x00000073,
         name: "ECALL",
-        operation: |cpu, _word, _address| {
+        operation: |cpu, _word, address| {
             let mut args = [0i64; 8];
             for (src, dest) in cpu.x[10..].iter().zip(args.iter_mut()) {
                 *dest = *src;
             }
-            let result = cpu.memory.write().unwrap().syscall(args);
+            match cpu.memory.write().unwrap().syscall(args) {
-            for (src, dest) in result.iter().zip(cpu.x[10..].iter_mut()) {
+                super::mmu::SyscallResult::Ok(result) => {
-                *dest = *src;
+                    for (src, dest) in result.iter().zip(cpu.x[10..].iter_mut()) {
+                        *dest = *src;
+                    }
+                    Ok(())
+                }
+                super::mmu::SyscallResult::Defer(receiver) => Err(Trap {
+                    trap_type: TrapType::PauseEmulation(receiver),
+                    value: address,
+                }),
             }
-            Ok(())
 
             // let exception_type = match cpu.privilege_mode {
             //     PrivilegeMode::User => TrapType::EnvironmentCallFromUMode,

crates/riscv-cpu/src/mmu.rs

@@ -5,6 +5,23 @@ use std::{
 
 use crate::cpu::{decode_privilege_mode, PrivilegeMode, Trap, TrapType, Xlen};
 
+pub enum SyscallResult {
+    Ok([i64; 8]),
+    Defer(std::sync::mpsc::Receiver<([i64; 8], Option<(Vec<u8>, u64)>)>),
+}
+
+impl From<[i64; 8]> for SyscallResult {
+    fn from(args: [i64; 8]) -> Self {
+        SyscallResult::Ok(args)
+    }
+}
+
+impl From<std::sync::mpsc::Receiver<([i64; 8], Option<(Vec<u8>, u64)>)>> for SyscallResult {
+    fn from(receiver: std::sync::mpsc::Receiver<([i64; 8], Option<(Vec<u8>, u64)>)>) -> Self {
+        SyscallResult::Defer(receiver)
+    }
+}
+
 pub trait Memory {
     fn read_u8(&self, p_address: u64) -> u8;
     fn read_u16(&self, p_address: u64) -> u16;
@@ -15,7 +32,7 @@ pub trait Memory {
     fn write_u32(&mut self, p_address: u64, value: u32);
     fn write_u64(&mut self, p_address: u64, value: u64);
     fn validate_address(&self, address: u64) -> bool;
-    fn syscall(&mut self, args: [i64; 8]) -> [i64; 8];
+    fn syscall(&mut self, args: [i64; 8]) -> SyscallResult;
 }
 
 /// Emulates Memory Management Unit. It holds the Main memory and peripheral

src/xous.rs

@@ -1,10 +1,12 @@
-use riscv_cpu::cpu::Memory as OtherMemory;
+use riscv_cpu::{cpu::Memory as OtherMemory, mmu::SyscallResult};
 mod definitions;
+mod services;
 
 use definitions::{Syscall, SyscallNumber, SyscallResultNumber};
 use std::{
-    collections::{BTreeSet, HashMap},
+    collections::{BTreeSet, HashMap, HashSet},
     sync::{
+        atomic::{AtomicI64, Ordering},
         mpsc::{Receiver, Sender},
         Arc, RwLock,
     },
@@ -44,6 +46,22 @@ const MMUFLAG_DIRTY: u32 = 0x80;
 
 impl std::error::Error for LoadError {}
 
+enum MemoryCommand {
+    Exit,
+    ExitThread(u32 /* tid */, u32 /* result */),
+    CreateThread(
+        u32,         /* entry point */
+        u32,         /* stack pointer */
+        u32,         /* stack length */
+        u32,         /* argument 1 */
+        u32,         /* argument 2 */
+        u32,         /* argument 3 */
+        u32,         /* argument 4 */
+        Sender<i64>, /* Thread ID */
+    ),
+    JoinThread(u32, Sender<([i64; 8], Option<(Vec<u8>, u64)>)>),
+}
+
 struct Memory {
     base: u32,
     data: HashMap<usize, [u8; 4096]>,
@@ -55,57 +73,57 @@ struct Memory {
     allocation_previous: u32,
     l1_pt: u32,
     satp: u32,
-}
+    connections: HashMap<u32, Box<dyn services::Service + Send + Sync>>,
-
+    memory_cmd: Sender<MemoryCommand>,
-enum WorkerCommand {
-    Start,
-    // MemoryRange(u32 /* address */, u32 /* size */),
-}
-
-enum WorkerResponse {
-    // Started,
-    Exited(u32),
-    // AllocateMemory(
-    //     u32, /* phys */
-    //     u32, /* virt */
-    //     u32, /* size */
-    //     u32, /* flags */
-    // ),
 }
 
 struct Worker {
     cpu: riscv_cpu::Cpu,
-    tx: Sender<WorkerResponse>,
+    cmd: Sender<MemoryCommand>,
-    rx: Receiver<WorkerCommand>,
+    tid: i64,
 }
 
 impl Worker {
-    fn new(
+    fn new(cpu: riscv_cpu::Cpu, cmd: Sender<MemoryCommand>, tid: i64) -> Self {
-        cpu: riscv_cpu::Cpu,
+        Self { cpu, cmd, tid }
-        rx: Receiver<WorkerCommand>,
-        worker_response_tx: Sender<WorkerResponse>,
-    ) -> Self {
-        Self {
-            cpu,
-            tx: worker_response_tx,
-            rx,
-        }
     }
     fn run(&mut self) {
-        self.rx.recv().unwrap();
+        use riscv_cpu::cpu::TickResult;
-        for _tick in 0..1000 {
+        loop {
-            self.cpu.tick();
+            match self.cpu.tick() {
+                TickResult::PauseEmulation(e) => {
+                    let (result, data) = e.recv().unwrap();
+                    if let Some(data) = data {
+                        let start = data.1;
+                        let data = data.0;
+                        let mmu = self.cpu.get_mut_mmu();
+                        for (offset, byte) in data.into_iter().enumerate() {
+                            mmu.store(offset as u64 + start, byte).unwrap();
+                        }
+                    }
+                    for (index, value) in result.iter().enumerate() {
+                        self.cpu.write_register(10 + index as u8, *value);
+                    }
+                }
+                TickResult::ExitThread(val) => {
+                    self.cmd
+                        .send(MemoryCommand::ExitThread(self.tid as u32, val as u32))
+                        .unwrap();
+                    // println!("Thread {} exited", self.tid);
+                    break;
+                }
+                TickResult::Ok => {}
+            }
         }
-        self.tx.send(WorkerResponse::Exited(1)).unwrap();
     }
 }
 
 struct WorkerHandle {
-    tx: Sender<WorkerCommand>,
+    joiner: std::thread::JoinHandle<()>,
 }
 
 impl Memory {
-    pub fn new(base: u32, size: usize) -> Self {
+    pub fn new(base: u32, size: usize) -> (Self, Receiver<MemoryCommand>) {
         let mut data = HashMap::new();
         let mut free_pages = BTreeSet::new();
         let mut allocated_pages = BTreeSet::new();
@@ -116,18 +134,23 @@ impl Memory {
         // Remove the l0 page table
         free_pages.remove(&(MEMORY_BASE as usize + 4096));
         allocated_pages.insert(MEMORY_BASE as usize + 4096);
-        Self {
+        let (memory_cmd, memory_cmd_rx) = std::sync::mpsc::channel();
-            base,
+        (
-            data,
+            Self {
-            allocated_pages,
+                base,
-            free_pages,
+                data,
-            l1_pt: MEMORY_BASE + 4096,
+                allocated_pages,
-            satp: ((4096 + MEMORY_BASE) >> 12) | 0x8000_0000,
+                free_pages,
-            heap_start: 0x6000_0000,
+                l1_pt: MEMORY_BASE + 4096,
-            heap_size: 0,
+                satp: ((4096 + MEMORY_BASE) >> 12) | 0x8000_0000,
-            allocation_previous: 0x4000_0000,
+                heap_start: 0x6000_0000,
-            // allocation_start: 0x4000_0000,
+                heap_size: 0,
-        }
+                allocation_previous: 0x4000_0000,
+                connections: HashMap::new(),
+                memory_cmd,
+            },
+            memory_cmd_rx,
+        )
     }
 
     fn allocate_page(&mut self) -> u32 {
@@ -136,6 +159,31 @@ impl Memory {
         page as u32
     }
 
+    fn free_page(&mut self, page: u32) -> Result<(), ()> {
+        let phys = self.virt_to_phys(page).ok_or(())?;
+        if !self.allocated_pages.remove(&(phys as usize)) {
+            panic!("Page wasn't allocated!");
+        }
+        self.free_pages.insert(phys as usize);
+
+        let vpn1 = ((page >> 22) & ((1 << 10) - 1)) as usize * 4;
+        let vpn0 = ((page >> 12) & ((1 << 10) - 1)) as usize * 4;
+
+        // The root (l1) pagetable is defined to be mapped into our virtual
+        // address space at this address.
+
+        // If the level 1 pagetable doesn't exist, then this address is invalid
+        let l1_pt_entry = self.read_u32(self.l1_pt as u64 + vpn1 as u64);
+        if l1_pt_entry & MMUFLAG_VALID == 0 {
+            return Ok(());
+        }
+
+        let l0_pt_phys = ((l1_pt_entry >> 10) << 12) + vpn0 as u32;
+        self.write_u32(l0_pt_phys as u64, 0);
+
+        Ok(())
+    }
+
     fn allocate_virt_region(&mut self, size: usize) -> Option<u32> {
         let mut start = self.allocation_previous;
         // Find a free region that will fit this page.
@@ -373,14 +421,13 @@ impl riscv_cpu::cpu::Memory for Memory {
         address < self.data.len()
     }
 
-    fn syscall(&mut self, args: [i64; 8]) -> [i64; 8] {
+    fn syscall(&mut self, args: [i64; 8]) -> SyscallResult {
         let syscall: Syscall = args.into();
-        // println!("Syscall {:?} with args: {:?}", syscall, &args[1..]);
 
-        print!("Syscall: ");
+        // print!("Syscall: ");
         match syscall {
             Syscall::IncreaseHeap(bytes, _flags) => {
-                println!("IncreaseHeap({} bytes, flags: {:02x})", bytes, _flags);
+                // println!("IncreaseHeap({} bytes, flags: {:02x})", bytes, _flags);
                 let heap_address = self.heap_start + self.heap_size;
                 match bytes {
                     bytes if bytes < 0 => {
@@ -407,13 +454,14 @@ impl riscv_cpu::cpu::Memory for Memory {
                     0,
                     0,
                 ]
+                .into()
             }
 
             Syscall::MapMemory(phys, virt, size, _flags) => {
-                println!(
+                // println!(
-                    "MapMemory(phys: {:08x}, virt: {:08x}, bytes: {}, flags: {:02x})",
+                //     "MapMemory(phys: {:08x}, virt: {:08x}, bytes: {}, flags: {:02x})",
-                    phys, virt, size, _flags
+                //     phys, virt, size, _flags
-                );
+                // );
                 if virt != 0 {
                     unimplemented!("Non-zero virt address");
                 }
@@ -433,6 +481,227 @@ impl riscv_cpu::cpu::Memory for Memory {
                     0,
                     0,
                 ]
+                .into()
+            }
+            Syscall::Connect(id) => {
+                // println!(
+                //     "Connect([0x{:08x}, 0x{:08x}, 0x{:08x}, 0x{:08x}])",
+                //     id[0], id[1], id[2], id[3]
+                // );
+                if let Some(service) = services::get_service(&id) {
+                    let connection_id = self.connections.len() as u32 + 1;
+                    self.connections.insert(connection_id, service);
+                    [
+                        SyscallResultNumber::ConnectionId as i64,
+                        connection_id as i64,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                    ]
+                    .into()
+                } else {
+                    [
+                        SyscallResultNumber::ConnectionId as i64,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                    ]
+                    .into()
+                }
+            }
+            Syscall::SendMessage(connection_id, kind, opcode, args) => {
+                // println!(
+                //     "SendMessage({}, {}, {}: {:x?})",
+                //     connection_id, kind, opcode, args
+                // );
+                let memory_region = if kind == 1 || kind == 2 || kind == 3 {
+                    let mut memory_region = vec![0; args[1] as usize];
+                    for (offset, value) in memory_region.iter_mut().enumerate() {
+                        *value = self.read_u8(
+                            self.virt_to_phys(args[0] + offset as u32)
+                                .expect("invalid memory address")
+                                as u64,
+                        );
+                    }
+                    Some(memory_region)
+                } else {
+                    None
+                };
+                let Some(service) = self.connections.get_mut(&connection_id) else {
+                    println!("Unhandled connection ID {}", connection_id);
+                    return [
+                        SyscallResultNumber::Error as i64,
+                        9, /* ServerNotFound */
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                    ]
+                    .into();
+                };
+                match kind {
+                    1..=3 => {
+                        let mut memory_region = memory_region.unwrap();
+                        let extra = [args[2], args[3]];
+                        match kind {
+                            1 => match service.lend_mut(0, opcode, &mut memory_region, extra) {
+                                services::LendResult::WaitForResponse(msg) => msg.into(),
+                                services::LendResult::MemoryReturned(result) => {
+                                    for (offset, value) in memory_region.into_iter().enumerate() {
+                                        self.write_u8(args[0] as u64 + offset as u64, value);
+                                    }
+                                    [
+                                        SyscallResultNumber::MemoryReturned as i64,
+                                        result[0] as i64,
+                                        result[1] as i64,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                    ]
+                                    .into()
+                                }
+                            },
+                            2 => match service.lend(0, opcode, &memory_region, extra) {
+                                services::LendResult::WaitForResponse(msg) => msg.into(),
+                                services::LendResult::MemoryReturned(result) => [
+                                    SyscallResultNumber::MemoryReturned as i64,
+                                    result[0] as i64,
+                                    result[1] as i64,
+                                    0,
+                                    0,
+                                    0,
+                                    0,
+                                    0,
+                                ]
+                                .into(),
+                            },
+                            3 => {
+                                service.send(0, opcode, &memory_region, extra);
+                                [SyscallResultNumber::Ok as i64, 0, 0, 0, 0, 0, 0, 0].into()
+                            }
+                            _ => unreachable!(),
+                        }
+                    }
+                    4 => {
+                        service.scalar(0, opcode, args);
+                        [SyscallResultNumber::Ok as i64, 0, 0, 0, 0, 0, 0, 0].into()
+                    }
+                    5 => match service.blocking_scalar(0, opcode, args) {
+                        services::ScalarResult::Scalar1(result) => [
+                            SyscallResultNumber::Scalar1 as i64,
+                            result as i64,
+                            0,
+                            0,
+                            0,
+                            0,
+                            0,
+                            0,
+                        ]
+                        .into(),
+                        services::ScalarResult::Scalar2(result) => [
+                            SyscallResultNumber::Scalar2 as i64,
+                            result[0] as i64,
+                            result[1] as i64,
+                            0,
+                            0,
+                            0,
+                            0,
+                            0,
+                        ]
+                        .into(),
+                        services::ScalarResult::Scalar5(result) => [
+                            SyscallResultNumber::Scalar5 as i64,
+                            result[0] as i64,
+                            result[1] as i64,
+                            result[2] as i64,
+                            result[3] as i64,
+                            result[4] as i64,
+                            0,
+                            0,
+                        ]
+                        .into(),
+                        services::ScalarResult::WaitForResponse(msg) => msg.into(),
+                    },
+                    _ => {
+                        println!("Unknown message kind {}", kind);
+                        [
+                            SyscallResultNumber::Error as i64,
+                            9, /* ServerNotFound */
+                            0,
+                            0,
+                            0,
+                            0,
+                            0,
+                            0,
+                        ]
+                        .into()
+                    }
+                }
+            }
+            Syscall::UpdateMemoryFlags(_address, _range, _value) => {
+                [SyscallResultNumber::Ok as i64, 0, 0, 0, 0, 0, 0, 0].into()
+            }
+            Syscall::Yield => [SyscallResultNumber::Ok as i64, 0, 0, 0, 0, 0, 0, 0].into(),
+            Syscall::CreateThread(
+                entry_point,
+                stack_pointer,
+                stack_length,
+                argument_1,
+                argument_2,
+                argument_3,
+                argument_4,
+            ) => {
+                let (tx, rx) = std::sync::mpsc::channel();
+                self.memory_cmd
+                    .send(MemoryCommand::CreateThread(
+                        entry_point as _,
+                        stack_pointer as _,
+                        stack_length as _,
+                        argument_1 as _,
+                        argument_2 as _,
+                        argument_3 as _,
+                        argument_4 as _,
+                        tx,
+                    ))
+                    .unwrap();
+                let thread_id = rx.recv().unwrap();
+                [
+                    SyscallResultNumber::ThreadId as i64,
+                    thread_id,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                ]
+                .into()
+            }
+            Syscall::UnmapMemory(address, size) => {
+                // println!("UnmapMemory({:08x}, {})", address, size);
+                for offset in (address..address + size).step_by(4096) {
+                    self.free_page(offset as u32).unwrap();
+                }
+                [SyscallResultNumber::Ok as i64, 0, 0, 0, 0, 0, 0, 0].into()
+            }
+            Syscall::JoinThread(thread_id) => {
+                // println!("JoinThread({})", thread_id);
+                let (tx, rx) = std::sync::mpsc::channel();
+                self.memory_cmd
+                    .send(MemoryCommand::JoinThread(thread_id as _, tx))
+                    .unwrap();
+                rx.into()
             }
             Syscall::Unknown(args) => {
                 println!(
@@ -440,7 +709,8 @@ impl riscv_cpu::cpu::Memory for Memory {
                     SyscallNumber::from(args[0]),
                     &args[1..]
                 );
-                [SyscallResultNumber::Unimplemented as _, 0, 0, 0, 0, 0, 0, 0]
+                unimplemented!("Unhandled syscall");
+                // [SyscallResultNumber::Unimplemented as _, 0, 0, 0, 0, 0, 0, 0]
             }
         }
     }
@@ -449,20 +719,25 @@ impl riscv_cpu::cpu::Memory for Memory {
 pub struct Machine {
     memory: Arc<RwLock<Memory>>,
     workers: Vec<WorkerHandle>,
-    worker_response: Receiver<WorkerResponse>,
+    satp: u64,
-    worker_response_tx: Sender<WorkerResponse>,
+    memory_cmd_sender: Sender<MemoryCommand>,
+    memory_cmd: Receiver<MemoryCommand>,
+    thread_id: AtomicI64,
 }
 
 impl Machine {
     pub fn new(program: &[u8]) -> Result<Self, LoadError> {
-        let memory = Arc::new(RwLock::new(Memory::new(MEMORY_BASE, 16 * 1024 * 1024)));
+        let (memory, memory_cmd) = Memory::new(MEMORY_BASE, 16 * 1024 * 1024);
+        let memory_cmd_sender = memory.memory_cmd.clone();
+        let memory = Arc::new(RwLock::new(memory));
 
-        let (worker_response_tx, worker_response) = std::sync::mpsc::channel();
         let mut machine = Self {
             memory,
             workers: vec![],
-            worker_response_tx,
+            satp: 0,
-            worker_response,
+            memory_cmd,
+            memory_cmd_sender,
+            thread_id: AtomicI64::new(1),
         };
 
         machine.load_program(program)?;
@@ -529,37 +804,116 @@ impl Machine {
         // Update the stack pointer
         cpu.write_register(2, 0xc002_0000 - 4);
 
-        let (tx, rx) = std::sync::mpsc::channel();
+        let cmd = self.memory_cmd_sender.clone();
-        let worker_tx = self.worker_response_tx.clone();
+        let joiner = std::thread::spawn(move || Worker::new(cpu, cmd, 0).run());
-        std::thread::spawn(move || Worker::new(cpu, rx, worker_tx).run());
 
-        self.workers.push(WorkerHandle { tx });
+        self.workers.push(WorkerHandle { joiner });
+        self.satp = satp;
 
         Ok(())
     }
 
     pub fn run(&mut self) -> Result<(), Box<dyn std::error::Error>> {
-        self.workers[0].tx.send(WorkerCommand::Start)?;
+        let (join_tx, rx) = std::sync::mpsc::channel();
-        self.worker_response.recv().unwrap();
+        let main_worker: WorkerHandle = self.workers.pop().unwrap();
+        join_tx.send(main_worker.joiner).unwrap();
+        let memory_cmd_sender = self.memory_cmd_sender.clone();
+        std::thread::spawn(move || {
+            while let Ok(msg) = rx.try_recv() {
+                if let Err(_e) = msg.join() {}
+            }
+            memory_cmd_sender.send(MemoryCommand::Exit).unwrap();
+        });
+        let mut joining_threads = HashMap::new();
+        let mut exited_threads = HashSet::new();
+        while let Ok(msg) = self.memory_cmd.recv() {
+            match msg {
+                MemoryCommand::JoinThread(tid, sender) => {
+                    if exited_threads.contains(&tid) {
+                        sender
+                            .send((
+                                [SyscallResultNumber::Scalar1 as i64, 0, 0, 0, 0, 0, 0, 0],
+                                None,
+                            ))
+                            .unwrap();
+                    } else {
+                        joining_threads
+                            .entry(tid)
+                            .or_insert_with(Vec::new)
+                            .push(sender);
+                    }
+                }
+                MemoryCommand::ExitThread(tid, result) => {
+                    exited_threads.insert(tid);
+                    if let Some(joiners) = joining_threads.remove(&tid) {
+                        for joiner in joiners {
+                            joiner
+                                .send((
+                                    [
+                                        SyscallResultNumber::Scalar1 as i64,
+                                        result.into(),
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                    ],
+                                    None,
+                                ))
+                                .unwrap();
+                        }
+                    }
+                }
+                MemoryCommand::CreateThread(
+                    entry_point,
+                    stack_pointer,
+                    _stack_length,
+                    argument_1,
+                    argument_2,
+                    argument_3,
+                    argument_4,
+                    tx,
+                ) => {
+                    let mut cpu = riscv_cpu::CpuBuilder::new(self.memory.clone())
+                        .xlen(riscv_cpu::Xlen::Bit32)
+                        .build();
+
+                    cpu.write_csr(riscv_cpu::cpu::CSR_SATP_ADDRESS, self.satp)
+                        .map_err(|_| LoadError::SatpWriteError)?;
+                    cpu.update_pc(entry_point as u64);
+
+                    // Return to User Mode (0 << 11) with interrupts disabled (1 << 5)
+                    cpu.write_csr(riscv_cpu::cpu::CSR_MSTATUS_ADDRESS, 1 << 5)
+                        .map_err(|_| LoadError::MstatusWriteError)?;
+
+                    cpu.write_csr(riscv_cpu::cpu::CSR_SEPC_ADDRESS, entry_point as u64)
+                        .unwrap();
+
+                    // SRET to return to user mode
+                    cpu.execute_opcode(0x10200073).map_err(LoadError::CpuTrap)?;
+
+                    // Update the stack pointer
+                    cpu.write_register(2, stack_pointer as i64 - 16);
+                    cpu.write_register(10, argument_1 as i64);
+                    cpu.write_register(11, argument_2 as i64);
+                    cpu.write_register(12, argument_3 as i64);
+                    cpu.write_register(13, argument_4 as i64);
+
+                    let cmd = self.memory_cmd_sender.clone();
+                    let tid = self.thread_id.fetch_add(1, Ordering::SeqCst);
+                    join_tx
+                        .send(std::thread::spawn(move || Worker::new(cpu, cmd, tid).run()))
+                        .unwrap();
+                    tx.send(tid).unwrap();
+                }
+                MemoryCommand::Exit => {
+                    break;
+                }
+            }
+        }
+        println!("Done! memory_cmd returned error");
 
         Ok(())
     }
 }
-
-// impl SyscallHandler for Worker {
-//     fn syscall(&mut self, cpu: &mut riscv_cpu::Cpu, args: [i64; 8]) -> [i64; 8] {
-//         let syscall: Syscall = args.into();
-//         println!("Syscall {:?} with args: {:?}", syscall, &args[1..]);
-//         // self.syscall(cpu, syscall)
-//         [
-//             SyscallResultNumber::Unimplemented as i64,
-//             0,
-//             0,
-//             0,
-//             0,
-//             0,
-//             0,
-//             0,
-//         ]
-//     }
-// }

src/xous/definitions.rs

@@ -59,6 +59,8 @@ pub enum Syscall {
         i64, /* argument 3 */
         i64, /* argument 4 */
     ),
+    JoinThread(i64 /* thread ID */),
+    UnmapMemory(i64, /* address */ i64 /* size */),
 }
 
 #[derive(Debug)]
@@ -88,6 +90,7 @@ impl From<[i64; 8]> for Syscall {
         match value[0].into() {
             SyscallNumber::IncreaseHeap => Syscall::IncreaseHeap(value[1], value[2]),
             SyscallNumber::MapMemory => Syscall::MapMemory(value[1], value[2], value[3], value[4]),
+            SyscallNumber::UnmapMemory => Syscall::UnmapMemory(value[1], value[2]),
             SyscallNumber::Connect => Syscall::Connect([
                 value[1] as u32,
                 value[2] as u32,
@@ -112,6 +115,7 @@ impl From<[i64; 8]> for Syscall {
                 value[1], value[2], value[3], value[4], value[5], value[6], value[7],
             ),
             SyscallNumber::Yield => Syscall::Yield,
+            SyscallNumber::JoinThread => Syscall::JoinThread(value[1]),
             _ => Syscall::Unknown(value),
         }
     }

src/xous/services.rs

@@ -1,3 +1,4 @@
+use std::sync::mpsc::Receiver;
 pub mod log;
 pub mod ticktimer;
 
@@ -5,6 +6,12 @@ pub enum ScalarResult {
     Scalar1(u32),
     Scalar2([u32; 2]),
     Scalar5([u32; 5]),
+    WaitForResponse(Receiver<([i64; 8], Option<(Vec<u8>, u64)>)>),
+}
+
+pub enum LendResult {
+    MemoryReturned([u32; 2]),
+    WaitForResponse(Receiver<([i64; 8], Option<(Vec<u8>, u64)>)>),
 }
 
 pub trait Service {
@@ -14,14 +21,33 @@ pub trait Service {
     fn blocking_scalar(&mut self, sender: u32, opcode: u32, _args: [u32; 4]) -> ScalarResult {
         panic!("Unknown scalar to service {}: {}", sender, opcode);
     }
-    fn lend(&mut self, sender: u32, opcode: u32, _buf: &[u8], extra: [u32; 2]) -> [u32; 2] {
+    fn lend(&mut self, sender: u32, opcode: u32, _buf: &[u8], extra: [u32; 2]) -> LendResult {
-        panic!("Unknown lend to service {}: {} ({:?})", sender, opcode, extra);
+        panic!(
+            "Unknown lend to service {}: {} ({:?})",
+            sender, opcode, extra
+        );
     }
-    fn lend_mut(&mut self, sender: u32, opcode: u32, _buf: &mut [u8], extra: [u32; 2]) -> [u32; 2] {
+
-        panic!("Unknown lend_mut to service {}: {} ({:?})", sender, opcode, extra);
+    /// Mutable lend messages may block
+    fn lend_mut(
+        &mut self,
+        sender: u32,
+        opcode: u32,
+        _buf: &mut [u8],
+        extra: [u32; 2],
+    ) -> LendResult {
+        panic!(
+            "Unknown lend_mut to service {}: {} ({:?})",
+            sender, opcode, extra
+        );
     }
+
+    /// Send-type messages return immediately, and memory is detached from the host process.
     fn send(&mut self, sender: u32, opcode: u32, _buf: &[u8], extra: [u32; 2]) {
-        panic!("Unknown send to service {}: {} ({:?})", sender, opcode, extra);
+        panic!(
+            "Unknown send to service {}: {} ({:?})",
+            sender, opcode, extra
+        );
     }
 }
 

src/xous/services/log.rs

@@ -1,4 +1,4 @@
-use super::{ScalarResult, Service};
+use super::{LendResult, ScalarResult, Service};
 use std::io::Write;
 
 enum LogLendOpcode {
@@ -28,11 +28,13 @@ impl Service for Log {
     fn scalar(&mut self, sender: u32, opcode: u32, args: [u32; 4]) {
         println!("Log scalar {}: {} {:x?}", sender, opcode, args);
     }
+
     fn blocking_scalar(&mut self, sender: u32, opcode: u32, args: [u32; 4]) -> ScalarResult {
         println!("Log blocking_scalar {}: {} {:x?}", sender, opcode, args);
         ScalarResult::Scalar1(0)
     }
-    fn lend(&mut self, sender: u32, opcode: u32, buf: &[u8], extra: [u32; 2]) -> [u32; 2] {
+
+    fn lend(&mut self, sender: u32, opcode: u32, buf: &[u8], extra: [u32; 2]) -> LendResult {
         if opcode == LogLendOpcode::StandardOutput as u32 {
             let print_buffer = &buf[0..extra[1] as usize];
             // println!("Log stdout:");
@@ -44,12 +46,20 @@ impl Service for Log {
         } else {
             panic!("Log lend {}: {} {:x?}", sender, opcode, buf);
         }
-        [0, 0]
+        LendResult::MemoryReturned([0, 0])
     }
-    fn lend_mut(&mut self, sender: u32, opcode: u32, _buf: &mut [u8], extra: [u32; 2]) -> [u32; 2] {
+
+    fn lend_mut(
+        &mut self,
+        sender: u32,
+        opcode: u32,
+        _buf: &mut [u8],
+        extra: [u32; 2],
+    ) -> LendResult {
         println!("Log lend_mut {}: {} {:x?}", sender, opcode, extra);
-        [0, 0]
+        LendResult::MemoryReturned([0, 0])
     }
+
     fn send(&mut self, sender: u32, opcode: u32, _buf: &[u8], extra: [u32; 2]) {
         println!("Log send {}: {} {:x?}", sender, opcode, extra);
     }

src/xous/services/ticktimer.rs

@@ -1,10 +1,21 @@
+use std::{
+    collections::HashMap,
+    sync::{atomic::AtomicUsize, Arc, Condvar, Mutex},
+    thread,
+};
+
+use super::LendResult;
+
 pub struct Ticktimer {
     start: std::time::SystemTime,
+    condvars: HashMap<usize, Arc<(Condvar, AtomicUsize)>>,
 }
 
 enum ScalarOpcode {
     ElapsedMs = 0,
-    WaitForCondition = 8
+    WaitForCondition = 8,
+    NotifyCondition = 9,
+    FreeCondition = 11,
 }
 
 impl Ticktimer {
@@ -12,6 +23,7 @@ impl Ticktimer {
         // println!("Constructing a ticktimer");
         Ticktimer {
             start: std::time::SystemTime::now(),
+            condvars: HashMap::new(),
         }
     }
 }
@@ -23,9 +35,15 @@ impl Default for Ticktimer {
 }
 
 impl super::Service for Ticktimer {
-    fn scalar(&mut self, sender: u32, opcode: u32, args: [u32; 4]) {
+    fn scalar(&mut self, _sender: u32, opcode: u32, args: [u32; 4]) {
-        println!("Ticktimer scalar {}: {} {:x?}", sender, opcode, args);
+        if opcode == ScalarOpcode::FreeCondition as u32 {
+            let condition_index = args[0] as usize;
+            if let Some(condvar) = self.condvars.remove(&condition_index) {
+                assert!(condvar.1.load(std::sync::atomic::Ordering::Relaxed) == 0);
+            }
+        }
     }
+
     fn blocking_scalar(&mut self, sender: u32, opcode: u32, args: [u32; 4]) -> super::ScalarResult {
         if opcode == ScalarOpcode::ElapsedMs as u32 {
             let elapsed_ms = std::time::SystemTime::now()
@@ -34,21 +52,70 @@ impl super::Service for Ticktimer {
                 .as_millis() as u64;
             super::ScalarResult::Scalar2([elapsed_ms as u32, (elapsed_ms >> 32) as u32])
         } else if opcode == ScalarOpcode::WaitForCondition as u32 {
-            let start = std::time::SystemTime::now();
+            let condition_index = args[0] as usize;
-            let mut elapsed_ms = start
+            let wait_count = args[1] as u64;
-                .duration_since(self.start)
+
-                .unwrap()
+            let (tx, rx) = std::sync::mpsc::channel();
-                .as_millis() as u64;
+            let condvar = self
-            let mut condition = args[0];
+                .condvars
-            while condition != 0 {
+                .entry(condition_index)
-                std::thread::sleep(std::time::Duration::from_millis(1));
+                .or_insert_with(|| Arc::new((Condvar::new(), AtomicUsize::new(0))))
-                elapsed_ms = start
+                .clone();
-                    .duration_since(self.start)
+            condvar.1.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
+
+            thread::spawn(move || {
+                let dummy_mutex = Mutex::new(());
+                let guard = dummy_mutex.lock().unwrap();
+                let timeout_value = if wait_count == 0 {
+                    let _ignored = condvar.0.wait(guard).unwrap();
+                    0
+                } else if condvar
+                    .0
+                    .wait_timeout(guard, std::time::Duration::from_millis(wait_count))
                     .unwrap()
-                    .as_millis() as u64;
+                    .1
-                condition = args[0];
+                    .timed_out()
+                {
+                    1
+                } else {
+                    0
+                };
+                condvar.1.fetch_sub(1, std::sync::atomic::Ordering::Relaxed);
+                tx.send((
+                    [
+                        super::super::definitions::SyscallResultNumber::Scalar1 as i64,
+                        timeout_value,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                    ],
+                    None,
+                ))
+                .unwrap();
+            });
+            super::ScalarResult::WaitForResponse(rx)
+        } else if opcode == ScalarOpcode::NotifyCondition as u32 {
+            let condition_index = args[0] as usize;
+            let condition_count = args[1] as usize;
+            if condition_count == 0 || !self.condvars.contains_key(&condition_index) {
+                return super::ScalarResult::Scalar5([0, 0, 0, 0, 0]);
             }
-            super::ScalarResult::Scalar2([elapsed_ms as u32, (elapsed_ms >> 32) as u32])
+            let mut notify_count = 0;
+            if let Some(condvar) = self.condvars.get(&condition_index) {
+                if condition_count == 0 {
+                    notify_count = condvar.1.load(std::sync::atomic::Ordering::Relaxed);
+                    condvar.0.notify_all();
+                } else {
+                    for _ in 0..condition_count {
+                        notify_count += 1;
+                        condvar.0.notify_one();
+                    }
+                }
+            }
+            super::ScalarResult::Scalar1(notify_count as u32)
         } else {
             panic!(
                 "Ticktimer blocking_scalar {}: {} {:x?}",
@@ -56,14 +123,23 @@ impl super::Service for Ticktimer {
             );
         }
     }
-    fn lend(&mut self, sender: u32, opcode: u32, _buf: &[u8], extra: [u32; 2]) -> [u32; 2] {
+
+    fn lend(&mut self, sender: u32, opcode: u32, _buf: &[u8], extra: [u32; 2]) -> LendResult {
         println!("Ticktimer lend {}: {} {:x?}", sender, opcode, extra);
-        [0, 0]
+        LendResult::MemoryReturned([0, 0])
     }
-    fn lend_mut(&mut self, sender: u32, opcode: u32, _buf: &mut [u8], extra: [u32; 2]) -> [u32; 2] {
+
+    fn lend_mut(
+        &mut self,
+        sender: u32,
+        opcode: u32,
+        _buf: &mut [u8],
+        extra: [u32; 2],
+    ) -> LendResult {
         println!("Ticktimer lend_mut {}: {} {:x?}", sender, opcode, extra);
-        [0, 0]
+        LendResult::MemoryReturned([0, 0])
     }
+
     fn send(&mut self, sender: u32, opcode: u32, _buf: &[u8], extra: [u32; 2]) {
         println!("Ticktimer send {}: {} {:x?}", sender, opcode, extra);
     }