diff --git a/programs/lib/flexpret_lock.c b/programs/lib/flexpret_lock.c
index cb9b373d..b21a62b1 100644
--- a/programs/lib/flexpret_lock.c
+++ b/programs/lib/flexpret_lock.c
@@ -1,5 +1,12 @@
 #include "flexpret.h"
 
+
+/*
+linked list
+*/
+static bool has_next_waiter[NUM_THREADS];
+static uint32_t next_waiter[NUM_THREADS];
+
 /**
  * Acquire a hardware lock.
  */
@@ -18,19 +25,25 @@ void hwlock_release() {
 }
 
 int do_acquire(lock_t* lock) {
+    uint32_t hartid = read_hartid();
     hwlock_acquire();
     if (lock->locked) {
+        // append to front of lock's linked list
+        has_next_waiter[hartid] = lock->has_first_waiter;
+        next_waiter[hartid] = lock->first_waiter;
+        lock->has_first_waiter = true;
+        lock->first_waiter = hartid;
         hwlock_release();
+        fp_sleep(); // sleep until lock is availible
         return 1;
     }
     lock->locked = true;
-    lock->owner  = read_hartid();
+    lock->owner  = hartid;
     hwlock_release();
     return 0;
 }
 
 void lock_acquire(lock_t* lock) {
-    // Spin lock
     while(do_acquire(lock));
 }
 
@@ -45,5 +58,13 @@ void lock_release(lock_t* lock) {
     }
     lock->locked = false;
     lock->owner  = UINT32_MAX;
+    if (lock->has_first_waiter) {
+        // pop first waiter off of linked list
+        uint32_t fw = lock->first_waiter;
+        lock->has_first_waiter = has_next_waiter[fw];
+        lock->first_waiter = next_waiter[fw];
+        // wake first waiter
+        fp_wake(fw);
+    }
     hwlock_release();
 }
diff --git a/programs/lib/flexpret_thread.c b/programs/lib/flexpret_thread.c
index 1fc707df..a4694648 100644
--- a/programs/lib/flexpret_thread.c
+++ b/programs/lib/flexpret_thread.c
@@ -4,6 +4,7 @@
 #include <flexpret_lock.h>
 #include <flexpret_thread.h>
 #include <flexpret_config.h>
+#include <flexpret_sleep.h>
 
 /*************************************************
  * FlexPRET's hardware thread scheduling functions
@@ -235,8 +236,7 @@ int thread_create(
             num_threads_busy += 1;
             // Signal the worker thread to do work.
             in_use[i] = true;
-            // FIXME: If the thread is asleep,
-            // wake up the thread.
+            fp_wake(i);
             hwlock_release();
             return 0;
         }
@@ -264,8 +264,7 @@ int thread_map(
         num_threads_busy += 1;
         // Signal the worker thread to do work.
         in_use[*hartid] = true;
-        // FIXME: If the thread is asleep,
-        // wake up the thread.
+        fp_wake(*hartid);
         hwlock_release();
         return 0;
     }
@@ -371,6 +370,7 @@ void worker_main() {
             num_threads_busy -= 1;
             in_use[hartid] = false;
             hwlock_release();
+            fp_sleep();
         }
     }
 
diff --git a/programs/lib/include/flexpret.h b/programs/lib/include/flexpret.h
index 500e2d7e..7122a746 100644
--- a/programs/lib/include/flexpret.h
+++ b/programs/lib/include/flexpret.h
@@ -5,6 +5,7 @@
 #include "flexpret_csrs.h"
 #include "flexpret_io.h"
 #include "flexpret_lock.h"
+#include "flexpret_sleep.h"
 #include "flexpret_thread.h"
 
 #include <stdbool.h>
diff --git a/programs/lib/include/flexpret_lock.h b/programs/lib/include/flexpret_lock.h
index e6bf1919..df6f3a84 100644
--- a/programs/lib/include/flexpret_lock.h
+++ b/programs/lib/include/flexpret_lock.h
@@ -4,10 +4,12 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#define LOCK_INITIALIZER { .locked = false, .owner = UINT32_MAX }
+#define LOCK_INITIALIZER { .locked = false, .owner = UINT32_MAX, .first_waiter = 0, .has_first_waiter = false }
 typedef struct _lock_t {
     bool     locked;
     uint32_t owner;
+    bool has_first_waiter;
+    uint32_t first_waiter;
 } lock_t;
 
 /**
diff --git a/programs/lib/include/flexpret_sleep.h b/programs/lib/include/flexpret_sleep.h
new file mode 100644
index 00000000..1b14da95
--- /dev/null
+++ b/programs/lib/include/flexpret_sleep.h
@@ -0,0 +1,17 @@
+#ifndef FLEXPRET_SLEEP_H
+#define FLEXPRET_SLEEP_H
+
+#include "flexpret_io.h"
+
+/**
+ * Put the current thread to sleep.
+ */
+static inline int fp_sleep() { return swap_csr(0x530, 0x0); }
+
+/**
+ * Wake up thread with given hartid
+ */
+static inline int fp_wake(uint32_t hartid) { return swap_csr(0x531, hartid); }
+
+
+#endif // FLEXPRET_SLEEP_H
diff --git a/programs/tests/c-tests/sleeper/Makefile b/programs/tests/c-tests/sleeper/Makefile
new file mode 100644
index 00000000..3dded651
--- /dev/null
+++ b/programs/tests/c-tests/sleeper/Makefile
@@ -0,0 +1,4 @@
+FLEXPRET_ROOT_DIR = ../../../../
+NAME = sleeper
+APP_SOURCES = *.c
+include $(FLEXPRET_ROOT_DIR)/Makefrag
diff --git a/programs/tests/c-tests/sleeper/sleeper.c b/programs/tests/c-tests/sleeper/sleeper.c
new file mode 100644
index 00000000..0aa43479
--- /dev/null
+++ b/programs/tests/c-tests/sleeper/sleeper.c
@@ -0,0 +1,20 @@
+#include <stdint.h>
+#include <flexpret.h>
+#include "flexpret_sleep.h"
+
+int main() {
+    
+    // Print the hardware thread id.
+    int x = read_hartid();
+    _fp_print(x);
+
+    _fp_print(fp_wake(0x0));
+    _fp_print(fp_wake(0x0));
+    _fp_print(fp_wake(0x0));
+    _fp_print(fp_sleep());
+    // _fp_print(fp_sleep());
+
+    _fp_print(9);
+    return 0;
+}
+
diff --git a/programs/tests/mt-c-tests/add/Makefile b/programs/tests/mt-c-tests/add/Makefile
new file mode 100644
index 00000000..44dce22c
--- /dev/null
+++ b/programs/tests/mt-c-tests/add/Makefile
@@ -0,0 +1,4 @@
+FLEXPRET_ROOT_DIR = ../../../../
+NAME = add
+APP_SOURCES = *.c
+include $(FLEXPRET_ROOT_DIR)/Makefrag
diff --git a/programs/tests/mt-c-tests/lockowner/Makefile b/programs/tests/mt-c-tests/lockowner/Makefile
new file mode 100644
index 00000000..7102ea1c
--- /dev/null
+++ b/programs/tests/mt-c-tests/lockowner/Makefile
@@ -0,0 +1,4 @@
+FLEXPRET_ROOT_DIR = ../../../../
+NAME = lockowner
+APP_SOURCES = *.c
+include $(FLEXPRET_ROOT_DIR)/Makefrag
diff --git a/programs/tests/mt-c-tests/swlock/Makefile b/programs/tests/mt-c-tests/swlock/Makefile
new file mode 100644
index 00000000..4b630f24
--- /dev/null
+++ b/programs/tests/mt-c-tests/swlock/Makefile
@@ -0,0 +1,4 @@
+FLEXPRET_ROOT_DIR = ../../../../
+NAME = swlock
+APP_SOURCES = *.c
+include $(FLEXPRET_ROOT_DIR)/Makefrag
diff --git a/programs/tests/mt-c-tests/threadcancel/Makefile b/programs/tests/mt-c-tests/threadcancel/Makefile
new file mode 100644
index 00000000..7150c418
--- /dev/null
+++ b/programs/tests/mt-c-tests/threadcancel/Makefile
@@ -0,0 +1,4 @@
+FLEXPRET_ROOT_DIR = ../../../../
+NAME = threadcancel
+APP_SOURCES = *.c
+include $(FLEXPRET_ROOT_DIR)/Makefrag
diff --git a/src/main/scala/Core/CSR.scala b/src/main/scala/Core/CSR.scala
index aa377284..c6ae7388 100644
--- a/src/main/scala/Core/CSR.scala
+++ b/src/main/scala/Core/CSR.scala
@@ -340,6 +340,29 @@ class CSR(implicit val conf: FlexpretConfiguration) extends Module {
     }
   }
 
+  val sleeper = Module(new Sleeper()).io
+  sleeper.tid := 0.U
+  sleeper.wake := false.B
+  sleeper.sleep := false.B
+  when (write && compare_addr(CSRs.wake)) {
+    sleeper.tid := io.rw.data_in
+    sleeper.wake := true.B
+    data_out := sleeper.state
+    when (sleeper.state =/= 0.U) {
+      wake(io.rw.data_in) := true.B
+    }
+  }
+  when (write && compare_addr(CSRs.sleep)) {
+    sleeper.tid := io.rw.thread
+    sleeper.sleep := true.B
+    data_out := sleeper.state
+    when (sleeper.state === 0.U) {
+      sleep := true.B
+    }
+  }
+
+
+
   // exception handling
   if (conf.exceptions) {
     when(io.exception) {
diff --git a/src/main/scala/Core/instructions.scala b/src/main/scala/Core/instructions.scala
index 3f4c7212..b8d101d7 100644
--- a/src/main/scala/Core/instructions.scala
+++ b/src/main/scala/Core/instructions.scala
@@ -260,6 +260,8 @@ object CSRs {
   val tohost    = 0x51e
   val fromhost  = 0x51f
   val hwlock    = 0x520
+  val sleep     = 0x530
+  val wake      = 0x531
   val cycle     = 0xc00
   val time      = 0xc01
   val instret   = 0xc02
diff --git a/src/main/scala/Core/sleep.scala b/src/main/scala/Core/sleep.scala
new file mode 100644
index 00000000..a292c8b5
--- /dev/null
+++ b/src/main/scala/Core/sleep.scala
@@ -0,0 +1,45 @@
+package flexpret.core
+
+import chisel3._
+import chisel3.util._
+
+
+
+class SleeperIO(implicit val conf: FlexpretConfiguration) extends Bundle {
+    val wake = Input(Bool())
+    val sleep = Input(Bool())
+    val tid = Input(UInt(conf.threadBits.W))
+    val state = Output(UInt(2.W)) // resulting state of tid
+}
+
+
+class Sleeper(implicit val conf: FlexpretConfiguration) extends Module {
+    val io = IO(new SleeperIO())
+    io.state := 3.U // invalid state
+
+    // in regStates, 0 = awake, 1 = asleep, 2 = caffeinated 
+    // (because initializing to non-zero is not possible?)
+    // in io.state, 0 = asleep, 1 = awake, 2 = caffeinated
+    val regStates = RegInit(VecInit(Seq.fill(conf.threads) { 0.U(2.W) }))
+    val asleep = regStates(io.tid)(0);
+    val caffeinated = regStates(io.tid)(1);
+
+    when(io.wake) {
+        when (!asleep) { // set state to caffeinated
+            regStates(io.tid) := 2.U
+            io.state := 2.U
+        } .otherwise { // set state to awake
+            regStates(io.tid) := 0.U
+            io.state := 1.U
+        }
+    }
+    when(io.sleep) {
+        when (caffeinated) { // set state to awake
+            regStates(io.tid) := 0.U
+            io.state := 1.U
+        } .otherwise { // set state to asleep
+            regStates(io.tid) := 1.U
+            io.state := 0.U
+        }
+    }
+}
\ No newline at end of file