Index: libcfa/src/concurrency/kernel.cfa
===================================================================
--- libcfa/src/concurrency/kernel.cfa	(revision 9f575eacc1e359b231c369fde8ace4f941121f26)
+++ libcfa/src/concurrency/kernel.cfa	(revision 3381ed7a703d0c964d6e94bffab7c58f0cdf0d17)
@@ -258,6 +258,6 @@
 // Kernel Scheduling logic
 //=============================================================================================
+static thread_desc * nextThread(cluster * this);
 static void runThread(processor * this, thread_desc * dst);
-static void finishRunning(processor * this);
 static void halt(processor * this);
 
@@ -286,17 +286,16 @@
 
 			if(readyThread) {
-				verify( ! kernelTLS.preemption_state.enabled );
+				/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
+				/* paranoid */ verifyf( readyThread->state == Inactive || readyThread->state == Start || readyThread->preempted != __NO_PREEMPTION, "state : %d, preempted %d\n", readyThread->state, readyThread->preempted);
+				/* paranoid */ verifyf( readyThread->next == 0p, "Expected null got %p", readyThread->next );
 
 				runThread(this, readyThread);
 
-				verify( ! kernelTLS.preemption_state.enabled );
-
-				//Some actions need to be taken from the kernel
-				finishRunning(this);
+				/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
 
 				spin_count = 0;
 			} else {
 				// spin(this, &spin_count);
-				halt(this);
+				// halt(this);
 			}
 		}
@@ -332,11 +331,14 @@
 
 	// Actually run the thread
-	RUN:
-	{
+	RUNNING:  while(true) {
 		if(unlikely(thrd_dst->preempted)) {
-			thrd_dst->preempted = false;
+			thrd_dst->preempted = __NO_PREEMPTION;
+			verify(thrd_dst->state == Active || thrd_dst->state == Rerun || thrd_dst->state == Reschedule);
 		} else {
+			verify(thrd_dst->state == Start || thrd_dst->state == Primed || thrd_dst->state == Inactive);
 			thrd_dst->state = Active;
 		}
+
+		/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
 
 		// set context switch to the thread that the processor is executing
@@ -344,50 +346,48 @@
 		CtxSwitch( &proc_cor->context, &thrd_dst->context );
 		// when CtxSwitch returns we are back in the processor coroutine
-	}
-
-	// We just finished running a thread, there are a few things that could have happened.
-	// 1 - Regular case : the thread has blocked and now one has scheduled it yet.
-	// 2 - Racy case    : the thread has blocked but someone has already tried to schedule it.
-	// 3 - Polite Racy case : the thread has blocked, someone has already tried to schedule it, but the thread is nice and wants to go through the ready-queue any way
-	// 4 - Preempted
-	// In case 1, we may have won a race so we can't write to the state again.
-	// In case 2, we lost the race so we now own the thread.
-	// In case 3, we lost the race but can just reschedule the thread.
-
-	if(unlikely(thrd_dst->preempted)) {
-		// The thread was preempted, reschedule it and reset the flag
-		ScheduleThread( thrd_dst );
-
-		// Just before returning to the processor, set the processor coroutine to active
-		proc_cor->state = Active;
-		return;
-	}
-
-	// set state of processor coroutine to active and the thread to inactive
-	enum coroutine_state old_state = __atomic_exchange_n(&thrd_dst->state, Inactive, __ATOMIC_SEQ_CST);
-	switch(old_state) {
-		case Halted:
-			// The thread has halted, it should never be scheduled/run again, leave it back to Halted and move on
-			thrd_dst->state = Halted;
-			break;
-		case Active:
-			// This is case 1, the regular case, nothing more is needed
-			break;
-		case Rerun:
-			// This is case 2, the racy case, someone tried to run this thread before it finished blocking
-			// In this case, just run it again.
-			goto RUN;
-		case Reschedule:
-			// This is case 3, someone tried to run this before it finished blocking
-			// but it must go through the ready-queue
-			thrd_dst->state = Inactive;  /*restore invariant */
+
+		/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
+
+
+		// We just finished running a thread, there are a few things that could have happened.
+		// 1 - Regular case : the thread has blocked and now one has scheduled it yet.
+		// 2 - Racy case    : the thread has blocked but someone has already tried to schedule it.
+		// 3 - Polite Racy case : the thread has blocked, someone has already tried to schedule it, but the thread is nice and wants to go through the ready-queue any way
+		// 4 - Preempted
+		// In case 1, we may have won a race so we can't write to the state again.
+		// In case 2, we lost the race so we now own the thread.
+		// In case 3, we lost the race but can just reschedule the thread.
+
+		if(unlikely(thrd_dst->preempted != __NO_PREEMPTION)) {
+			// The thread was preempted, reschedule it and reset the flag
 			ScheduleThread( thrd_dst );
-			break;
-		case Inactive:
-		case Start:
-		case Primed:
-		default:
-			// This makes no sense, something is wrong abort
-			abort("Finished running a thread that was Inactive/Start/Primed %d\n", old_state);
+			break RUNNING;
+		}
+
+		// set state of processor coroutine to active and the thread to inactive
+		static_assert(sizeof(thrd_dst->state) == sizeof(int));
+		enum coroutine_state old_state = __atomic_exchange_n(&thrd_dst->state, Inactive, __ATOMIC_SEQ_CST);
+		switch(old_state) {
+			case Halted:
+				// The thread has halted, it should never be scheduled/run again, leave it back to Halted and move on
+				thrd_dst->state = Halted;
+				break RUNNING;
+			case Active:
+				// This is case 1, the regular case, nothing more is needed
+				break RUNNING;
+			case Rerun:
+				// This is case 2, the racy case, someone tried to run this thread before it finished blocking
+				// In this case, just run it again.
+				continue RUNNING;
+			case Reschedule:
+				// This is case 3, someone tried to run this before it finished blocking
+				// but it must go through the ready-queue
+				thrd_dst->state = Inactive;  /*restore invariant */
+				ScheduleThread( thrd_dst );
+				break RUNNING;
+			default:
+				// This makes no sense, something is wrong abort
+				abort("Finished running a thread that was Inactive/Start/Primed %d\n", old_state);
+		}
 	}
 
@@ -398,5 +398,5 @@
 // KERNEL_ONLY
 static void returnToKernel() {
-	verify( ! kernelTLS.preemption_state.enabled );
+	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
 	coroutine_desc * proc_cor = get_coroutine(kernelTLS.this_processor->runner);
 	thread_desc * thrd_src = kernelTLS.this_thread;
@@ -416,39 +416,5 @@
 	}
 
-	verify( ! kernelTLS.preemption_state.enabled );
-}
-
-// KERNEL_ONLY
-// Once a thread has finished running, some of
-// its final actions must be executed from the kernel
-static void finishRunning(processor * this) with( this->finish ) {
-	verify( ! kernelTLS.preemption_state.enabled );
-	verify( action_code == No_Action );
-	choose( action_code ) {
-	case No_Action:
-		break;
-	case Release:
-		unlock( *lock );
-	case Schedule:
-		ScheduleThread( thrd );
-	case Release_Schedule:
-		unlock( *lock );
-		ScheduleThread( thrd );
-	case Release_Multi:
-		for(int i = 0; i < lock_count; i++) {
-			unlock( *locks[i] );
-		}
-	case Release_Multi_Schedule:
-		for(int i = 0; i < lock_count; i++) {
-			unlock( *locks[i] );
-		}
-		for(int i = 0; i < thrd_count; i++) {
-			ScheduleThread( thrds[i] );
-		}
-	case Callback:
-		callback();
-	default:
-		abort("KERNEL ERROR: Unexpected action to run after thread");
-	}
+	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
 }
 
@@ -581,9 +547,13 @@
 //-----------------------------------------------------------------------------
 // Scheduler routines
-
 // KERNEL ONLY
 void ScheduleThread( thread_desc * thrd ) with( *thrd->curr_cluster ) {
 	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
-	/* paranoid */ verifyf( thrd->state == Inactive || thrd->state == Start || thrd->preempted, "state : %d, preempted %d\n", thrd->state, thrd->preempted);
+	/* paranoid */ #if defined( __CFA_WITH_VERIFY__ )
+	/* paranoid */ if( thrd->state == Inactive || thrd->state == Start ) assertf( thrd->preempted == __NO_PREEMPTION,
+	                  "Error inactive thread marked as preempted, state %d, preemption %d\n", thrd->state, thrd->preempted );
+	/* paranoid */ if( thrd->preempted != __NO_PREEMPTION ) assertf(thrd->state == Active || thrd->state == Rerun || thrd->state == Reschedule,
+	                  "Error preempted thread marked as not currently running, state %d, preemption %d\n", thrd->state, thrd->preempted );
+	/* paranoid */ #endif
 	/* paranoid */ verifyf( thrd->next == 0p, "Expected null got %p", thrd->next );
 
@@ -608,102 +578,93 @@
 
 // KERNEL ONLY
-thread_desc * nextThread(cluster * this) with( *this ) {
-	verify( ! kernelTLS.preemption_state.enabled );
+static thread_desc * nextThread(cluster * this) with( *this ) {
+	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
+
 	lock( ready_queue_lock __cfaabi_dbg_ctx2 );
 	thread_desc * head = pop_head( ready_queue );
 	unlock( ready_queue_lock );
-	verify( ! kernelTLS.preemption_state.enabled );
+
+	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
 	return head;
 }
 
-void BlockInternal() {
+void unpark( thread_desc * thrd, bool must_yield ) {
+	if( !thrd ) return;
+
+	enum coroutine_state new_state = must_yield ? Reschedule : Rerun;
+
 	disable_interrupts();
-	verify( ! kernelTLS.preemption_state.enabled );
+	static_assert(sizeof(thrd->state) == sizeof(int));
+	enum coroutine_state old_state = __atomic_exchange_n(&thrd->state, new_state, __ATOMIC_SEQ_CST);
+	switch(old_state) {
+		case Active:
+			// Wake won the race, the thread will reschedule/rerun itself
+			break;
+		case Inactive:
+			/* paranoid */ verify( ! thrd->preempted != __NO_PREEMPTION );
+
+			// Wake lost the race,
+			thrd->state = Inactive;
+			ScheduleThread( thrd );
+			break;
+		case Rerun:
+		case Reschedule:
+			abort("More than one thread attempted to schedule thread %p\n", thrd);
+			break;
+		case Halted:
+		case Start:
+		case Primed:
+		default:
+			// This makes no sense, something is wrong abort
+			abort();
+	}
+	enable_interrupts( __cfaabi_dbg_ctx );
+}
+
+void park( void ) {
+	/* paranoid */ verify( kernelTLS.preemption_state.enabled );
+	disable_interrupts();
+	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
+	/* paranoid */ verify( kernelTLS.this_thread->preempted == __NO_PREEMPTION );
+
 	returnToKernel();
-	verify( ! kernelTLS.preemption_state.enabled );
+
+	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
 	enable_interrupts( __cfaabi_dbg_ctx );
-}
-
-void BlockInternal( __spinlock_t * lock ) {
+	/* paranoid */ verify( kernelTLS.preemption_state.enabled );
+
+}
+
+// KERNEL ONLY
+void LeaveThread() {
+	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
+	returnToKernel();
+}
+
+// KERNEL ONLY
+bool force_yield( __Preemption_Reason reason ) {
+	/* paranoid */ verify( kernelTLS.preemption_state.enabled );
 	disable_interrupts();
-	unlock( *lock );
-
-	verify( ! kernelTLS.preemption_state.enabled );
-	returnToKernel();
-	verify( ! kernelTLS.preemption_state.enabled );
-
-	enable_interrupts( __cfaabi_dbg_ctx );
-}
-
-void BlockInternal( thread_desc * thrd ) {
-	disable_interrupts();
-	WakeThread( thrd, false );
-
-	verify( ! kernelTLS.preemption_state.enabled );
-	returnToKernel();
-	verify( ! kernelTLS.preemption_state.enabled );
-
-	enable_interrupts( __cfaabi_dbg_ctx );
-}
-
-void BlockInternal( __spinlock_t * lock, thread_desc * thrd ) {
-	disable_interrupts();
-	unlock( *lock );
-	WakeThread( thrd, false );
-
-	verify( ! kernelTLS.preemption_state.enabled );
-	returnToKernel();
-	verify( ! kernelTLS.preemption_state.enabled );
-
-	enable_interrupts( __cfaabi_dbg_ctx );
-}
-
-void BlockInternal(__spinlock_t * locks [], unsigned short count) {
-	disable_interrupts();
-	for(int i = 0; i < count; i++) {
-		unlock( *locks[i] );
-	}
-
-	verify( ! kernelTLS.preemption_state.enabled );
-	returnToKernel();
-	verify( ! kernelTLS.preemption_state.enabled );
-
-	enable_interrupts( __cfaabi_dbg_ctx );
-}
-
-void BlockInternal(__spinlock_t * locks [], unsigned short lock_count, thread_desc * thrds [], unsigned short thrd_count) {
-	disable_interrupts();
-	for(int i = 0; i < lock_count; i++) {
-		unlock( *locks[i] );
-	}
-	for(int i = 0; i < thrd_count; i++) {
-		WakeThread( thrds[i], false );
-	}
-
-	verify( ! kernelTLS.preemption_state.enabled );
-	returnToKernel();
-	verify( ! kernelTLS.preemption_state.enabled );
-
-	enable_interrupts( __cfaabi_dbg_ctx );
-}
-
-void BlockInternal(__finish_callback_fptr_t callback) {
-	disable_interrupts();
-	callback();
-
-	verify( ! kernelTLS.preemption_state.enabled );
-	returnToKernel();
-	verify( ! kernelTLS.preemption_state.enabled );
-
-	enable_interrupts( __cfaabi_dbg_ctx );
-}
-
-// KERNEL ONLY
-void LeaveThread(__spinlock_t * lock, thread_desc * thrd) {
-	verify( ! kernelTLS.preemption_state.enabled );
-	unlock( *lock );
-	WakeThread( thrd, false );
-
-	returnToKernel();
+	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
+
+	thread_desc * thrd = kernelTLS.this_thread;
+	/* paranoid */ verify(thrd->state == Active || thrd->state == Rerun || thrd->state == Reschedule);
+
+	// SKULLDUGGERY: It is possible that we are preempting this thread just before
+	// it was going to park itself. If that is the case and it is already using the
+	// intrusive fields then we can't use them to preempt the thread
+	// If that is the case, abandon the preemption.
+	bool preempted = false;
+	if(thrd->next == 0p) {
+		preempted = true;
+		thrd->preempted = reason;
+		returnToKernel();
+	}
+
+	/* paranoid */ verify( ! kernelTLS.preemption_state.enabled );
+	enable_interrupts_noPoll();
+	/* paranoid */ verify( kernelTLS.preemption_state.enabled );
+
+	return preempted;
 }
 
@@ -939,5 +900,6 @@
 
 		// atomically release spin lock and block
-		BlockInternal( &lock );
+		unlock( lock );
+		park();
 	}
 	else {
@@ -958,5 +920,5 @@
 
 	// make new owner
-	WakeThread( thrd, false );
+	unpark( thrd );
 }
 
