Index: kernel/generic/include/cpu.h
===================================================================
--- kernel/generic/include/cpu.h	(revision c1eaec452eea24d23cfcba31035c7fa507f2a011)
+++ kernel/generic/include/cpu.h	(revision 6a0e568427978c583cc81ec4a2e002711f89b081)
@@ -77,4 +77,5 @@
 	context_t scheduler_context;
 
+	struct thread *prev_thread;
 	state_t exiting_state;
 } cpu_local_t;
Index: kernel/generic/include/proc/scheduler.h
===================================================================
--- kernel/generic/include/proc/scheduler.h	(revision c1eaec452eea24d23cfcba31035c7fa507f2a011)
+++ kernel/generic/include/proc/scheduler.h	(revision 6a0e568427978c583cc81ec4a2e002711f89b081)
@@ -64,4 +64,6 @@
 extern void scheduler_enter(state_t);
 
+extern void thread_main_func(void);
+
 /*
  * To be defined by architectures.
Index: kernel/generic/src/proc/scheduler.c
===================================================================
--- kernel/generic/src/proc/scheduler.c	(revision c1eaec452eea24d23cfcba31035c7fa507f2a011)
+++ kernel/generic/src/proc/scheduler.c	(revision 6a0e568427978c583cc81ec4a2e002711f89b081)
@@ -425,4 +425,15 @@
 		halt();
 
+	/* Check if we have a thread to switch to. */
+
+	int rq_index;
+	thread_t *new_thread = try_find_thread(&rq_index);
+
+	if (new_thread == NULL && new_state == Running) {
+		/* No other thread to run, but we still have work to do here. */
+		interrupts_restore(ipl);
+		return;
+	}
+
 	irq_spinlock_lock(&THREAD->lock, false);
 	THREAD->state = new_state;
@@ -446,9 +457,30 @@
 	CPU_LOCAL->exiting_state = new_state;
 
-	current_copy(CURRENT, (current_t *) CPU_LOCAL->stack);
-	context_swap(&THREAD->saved_context, &CPU_LOCAL->scheduler_context);
+	if (new_thread) {
+		thread_t *old_thread = THREAD;
+		CPU_LOCAL->prev_thread = old_thread;
+		THREAD = new_thread;
+		/* No waiting necessary, we can switch to the new thread directly. */
+		prepare_to_run_thread(rq_index);
+
+		current_copy(CURRENT, (current_t *) new_thread->kstack);
+		context_swap(&old_thread->saved_context, &new_thread->saved_context);
+	} else {
+		/*
+		 * A new thread isn't immediately available, switch to a separate
+		 * stack to sleep or do other idle stuff.
+		 */
+		current_copy(CURRENT, (current_t *) CPU_LOCAL->stack);
+		context_swap(&THREAD->saved_context, &CPU_LOCAL->scheduler_context);
+	}
 
 	assert(CURRENT->mutex_locks == 0);
 	assert(interrupts_disabled());
+
+	/* Check if we need to clean up after another thread. */
+	if (CPU_LOCAL->prev_thread) {
+		cleanup_after_thread(CPU_LOCAL->prev_thread, CPU_LOCAL->exiting_state);
+		CPU_LOCAL->prev_thread = NULL;
+	}
 
 	interrupts_restore(ipl);
@@ -504,4 +536,33 @@
 
 	halt();
+}
+
+/** Thread wrapper.
+ *
+ * This wrapper is provided to ensure that a starting thread properly handles
+ * everything it needs to do when first scheduled, and when it exits.
+ */
+void thread_main_func(void)
+{
+	assert(interrupts_disabled());
+
+	void (*f)(void *) = THREAD->thread_code;
+	void *arg = THREAD->thread_arg;
+
+	/* This is where each thread wakes up after its creation */
+
+	/* Check if we need to clean up after another thread. */
+	if (CPU_LOCAL->prev_thread) {
+		cleanup_after_thread(CPU_LOCAL->prev_thread, CPU_LOCAL->exiting_state);
+		CPU_LOCAL->prev_thread = NULL;
+	}
+
+	interrupts_enable();
+
+	f(arg);
+
+	thread_exit();
+
+	/* Not reached */
 }
 
Index: kernel/generic/src/proc/thread.c
===================================================================
--- kernel/generic/src/proc/thread.c	(revision c1eaec452eea24d23cfcba31035c7fa507f2a011)
+++ kernel/generic/src/proc/thread.c	(revision 6a0e568427978c583cc81ec4a2e002711f89b081)
@@ -108,27 +108,4 @@
 static int threads_cmp(void *, void *);
 
-/** Thread wrapper.
- *
- * This wrapper is provided to ensure that every thread makes a call to
- * thread_exit() when its implementing function returns.
- *
- * interrupts_disable() is assumed.
- *
- */
-static void cushion(void)
-{
-	void (*f)(void *) = THREAD->thread_code;
-	void *arg = THREAD->thread_arg;
-
-	/* This is where each thread wakes up after its creation */
-	interrupts_enable();
-
-	f(arg);
-
-	thread_exit();
-
-	/* Not reached */
-}
-
 /** Initialization and allocation for thread_t structure
  *
@@ -309,5 +286,6 @@
 	irq_spinlock_unlock(&tidlock, true);
 
-	context_create(&thread->saved_context, cushion, thread->kstack, STACK_SIZE);
+	context_create(&thread->saved_context, thread_main_func,
+	    thread->kstack, STACK_SIZE);
 
 	current_initialize((current_t *) thread->kstack);