Index: kernel/generic/src/proc/scheduler.c
===================================================================
--- kernel/generic/src/proc/scheduler.c	(revision 5663872d31151ed95c7dd593f31d76a61b79f008)
+++ kernel/generic/src/proc/scheduler.c	(revision 23f36a3cb9c57838fb51caaf810dcd9e271bfb86)
@@ -67,4 +67,5 @@
 
 static void scheduler_separated_stack(void);
+static void fpu_restore(void);
 
 atomic_size_t nrdy;  /**< Number of ready threads in the system. */
@@ -83,25 +84,5 @@
 	before_thread_runs_arch();
 
-#ifdef CONFIG_FPU_LAZY
-	/*
-	 * The only concurrent modification possible for fpu_owner here is
-	 * another thread changing it from itself to NULL in its destructor.
-	 */
-	thread_t *owner = atomic_load_explicit(&CPU->fpu_owner,
-	    memory_order_relaxed);
-
-	if (THREAD == owner)
-		fpu_enable();
-	else
-		fpu_disable();
-#elif defined CONFIG_FPU
-	fpu_enable();
-	if (THREAD->fpu_context_exists)
-		fpu_context_restore(&THREAD->fpu_context);
-	else {
-		fpu_init();
-		THREAD->fpu_context_exists = true;
-	}
-#endif
+	fpu_restore();
 
 #ifdef CONFIG_UDEBUG
@@ -341,4 +322,44 @@
 }
 
+/**
+ * Do whatever needs to be done with current FPU state before we switch to
+ * another thread.
+ */
+static void fpu_cleanup(void)
+{
+#if (defined CONFIG_FPU) && (!defined CONFIG_FPU_LAZY)
+	fpu_context_save(&THREAD->fpu_context);
+#endif
+}
+
+/**
+ * Set correct FPU state for this thread after switch from another thread.
+ */
+static void fpu_restore(void)
+{
+#ifdef CONFIG_FPU_LAZY
+	/*
+	 * The only concurrent modification possible for fpu_owner here is
+	 * another thread changing it from itself to NULL in its destructor.
+	 */
+	thread_t *owner = atomic_load_explicit(&CPU->fpu_owner,
+	    memory_order_relaxed);
+
+	if (THREAD == owner)
+		fpu_enable();
+	else
+		fpu_disable();
+
+#elif defined CONFIG_FPU
+	fpu_enable();
+	if (THREAD->fpu_context_exists)
+		fpu_context_restore(&THREAD->fpu_context);
+	else {
+		fpu_init();
+		THREAD->fpu_context_exists = true;
+	}
+#endif
+}
+
 void scheduler(void)
 {
@@ -370,7 +391,6 @@
 		THREAD->kcycles += get_cycle() - THREAD->last_cycle;
 
-#if (defined CONFIG_FPU) && (!defined CONFIG_FPU_LAZY)
-		fpu_context_save(&THREAD->fpu_context);
-#endif
+		fpu_cleanup();
+
 		if (!context_save(&THREAD->saved_context)) {
 			/*