evenless: nothing but yet another co-kernel

Signed-off-by: Philippe Gerum <rpm@xenomai.org>

arch/arm/Kconfig

@@ -291,6 +291,8 @@ config PGTABLE_LEVELS
 	default 3 if ARM_LPAE
 	default 2
 
+source "kernel/Kconfig.evenless"
+
 menu "System Type"
 
 config MMU
@@ -568,6 +570,10 @@ config ARCH_MULTI_V6_V7
 	select MIGHT_HAVE_CACHE_L2X0
 	select HAVE_IRQ_PIPELINE
 	select HAVE_DOVETAIL if CPU_HAS_ASID
+	select HAVE_ARCH_EVENLESS
+	select WARN_CPUFREQ_GOVERNOR if CPU_FREQ && \
+	       !(CPU_FREQ_DEFAULT_GOV_PERFORMANCE || \
+	        CPU_FREQ_DEFAULT_GOV_POWERSAVE)
 
 config ARCH_MULTI_CPU_AUTO
 	def_bool !(ARCH_MULTI_V4 || ARCH_MULTI_V4T || ARCH_MULTI_V6_V7)

arch/arm/Makefile

@@ -250,6 +250,10 @@ KBUILD_CPPFLAGS += $(patsubst %,-I$(srctree)/%include,$(machdirs) $(platdirs))
 endif
 endif
 
+ifeq ($(CONFIG_EVENLESS),y)
+KBUILD_CFLAGS += -Iarch/$(SRCARCH)/evenless/include -Iinclude/evenless
+endif
+
 export	TEXT_OFFSET GZFLAGS MMUEXT
 
 core-y				+= arch/arm/

arch/arm/include/asm/evenless/fptest.h

+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _EVENLESS_ARM_ASM_FPTEST_H
+#define _EVENLESS_ARM_ASM_FPTEST_H
+
+#include <linux/cpufeature.h>
+#include <uapi/asm/evenless/fptest.h>
+
+static inline bool evl_begin_fpu(void)
+{
+	return false;
+}
+
+static inline void evl_end_fpu(void) { }
+
+static inline u32 evl_detect_fpu(void)
+{
+	u32 features = 0;
+
+	if (elf_hwcap & HWCAP_VFP)
+		return features |= evl_arm_vfp;
+
+	return features;
+}
+
+#endif /* _EVENLESS_ARM_ASM_FPTEST_H */

arch/arm/include/asm/evenless/syscall.h

+/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
+#ifndef _EVENLESS_ARM_ASM_SYSCALL_H
+#define _EVENLESS_ARM_ASM_SYSCALL_H
+
+#include <linux/uaccess.h>
+#include <asm/unistd.h>
+#include <asm/ptrace.h>
+#include <asm/syscall.h>
+#include <uapi/asm/dovetail.h>
+
+#define raw_put_user(src, dst)  __put_user(src, dst)
+#define raw_get_user(dst, src)  __get_user(dst, src)
+
+#define is_oob_syscall(__regs)	((__regs)->ARM_r7 == __ARM_NR_dovetail)
+#define oob_syscall_nr(__regs)	((__regs)->ARM_ORIG_r0)
+
+#define oob_retval(__regs)	((__regs)->ARM_r0)
+#define oob_arg1(__regs)	((__regs)->ARM_r1)
+#define oob_arg2(__regs)	((__regs)->ARM_r2)
+#define oob_arg3(__regs)	((__regs)->ARM_r3)
+#define oob_arg4(__regs)	((__regs)->ARM_r4)
+#define oob_arg5(__regs)	((__regs)->ARM_r5)
+
+/*
+ * Fetch and test inband syscall number (valid only if
+ * !is_oob_syscall(__regs)).
+ */
+#define inband_syscall_nr(__regs, __nr)					\
+	({								\
+		*(__nr) = (__regs)->ARM_r7;				\
+		*(__nr) < NR_syscalls || *(__nr) >= __ARM_NR_BASE;	\
+	})
+
+static inline void
+set_oob_error(struct pt_regs *regs, int err)
+{
+	oob_retval(regs) = err;
+}
+
+static inline
+void set_oob_retval(struct pt_regs *regs, long ret)
+{
+	oob_retval(regs) = ret;
+}
+
+#endif /* !_EVENLESS_ARM_ASM_SYSCALL_H */

arch/arm/include/asm/evenless/thread.h

+/*
+ * Copyright (C) 2005 Stelian Pop
+ *
+ * Xenomai is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * Xenomai is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Xenomai; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
+ * 02111-1307, USA.
+ */
+#ifndef _EVENLESS_ARM_ASM_THREAD_H
+#define _EVENLESS_ARM_ASM_THREAD_H
+
+#define xnarch_fault_pf_p(__trapnr)	((__trapnr) == ARM_TRAP_ACCESS)
+#define xnarch_fault_bp_p(__trapnr)	((current->ptrace & PT_PTRACED) && \
+					 (__trapnr == ARM_TRAP_BREAK || \
+					  (__trapnr) == ARM_TRAP_UNDEFINSTR))
+
+#define xnarch_fault_notify(__trapnr) (!xnarch_fault_bp_p(__trapnr))
+
+#endif /* !_EVENLESS_ARM_ASM_THREAD_H */

arch/arm/include/dovetail/thread_info.h

+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _EVENLESS_DOVETAIL_THREAD_INFO_H
+#define _EVENLESS_DOVETAIL_THREAD_INFO_H
+
+#include <asm-generic/evenless/thread_info.h>
+
+#endif /* !_EVENLESS_DOVETAIL_THREAD_INFO_H */

arch/arm/include/uapi/asm/evenless/fptest.h

+/*
+ * SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note
+ * Derived from Xenomai's Cobalt core:
+ * Copyright (C) 2006 Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>.
+ */
+#ifndef _EVENLESS_ARM_ASM_UAPI_FPTEST_H
+#define _EVENLESS_ARM_ASM_UAPI_FPTEST_H
+
+#define evl_arm_vfp  0x1
+
+#define evl_set_fpregs(__features, __val)				\
+	do {								\
+		unsigned int __i;					\
+		__u64 __e[16];						\
+									\
+		if (__features & evl_arm_vfp) {				\
+			for (__i = 0; __i < 16; __i++)			\
+				__e[__i] = (__val);			\
+			/* vldm %0!, {d0-d15}, AKA fldmiax %0!, {d0-d15} */ \
+			__asm__ __volatile__("ldc p11, cr0, [%0],#32*4": \
+					     "=r"(__i):			\
+					     "0"(&__e[0]): "memory");	\
+		}							\
+	} while (0)
+
+#define evl_check_fpregs(__features, __val, __bad)			\
+	({								\
+		unsigned int __result = (__val), __i;			\
+		__u64 __e[16];						\
+									\
+		if (__features & evl_arm_vfp) {				\
+			/* vstm %0!, {d0-d15}, AKA fstmiax %0!, {d0-d15} */ \
+			__asm__ __volatile__("stc p11, cr0, [%0],#32*4": \
+					     "=r"(__i):			\
+					     "0"(&__e[0]): "memory");	\
+			for (__i = 0; __i < 16; __i++)			\
+				if (__e[__i] != __val) {		\
+					__result = __e[__i];		\
+					(__bad) = __i;			\
+					break;				\
+				}					\
+		}							\
+		__result;						\
+	})
+
+#endif /* !_EVENLESS_ARM_ASM_UAPI_FPTEST_H */

arch/arm64/Kconfig

@@ -152,6 +152,7 @@ config ARM64
 	select HAVE_ARCH_TRACEHOOK
 	select HAVE_ARCH_TRANSPARENT_HUGEPAGE
 	select HAVE_ARCH_VMAP_STACK
+	select HAVE_ARCH_EVENLESS
 	select HAVE_ARM_SMCCC
 	select HAVE_ASM_MODVERSIONS
 	select HAVE_EBPF_JIT
@@ -1838,6 +1839,8 @@ config STACKPROTECTOR_PER_TASK
 
 endmenu
 
+source "kernel/Kconfig.evenless"
+
 menu "Boot options"
 
 config ARM64_ACPI_PARKING_PROTOCOL

arch/arm64/Makefile

@@ -131,6 +131,10 @@ endif
 
 CHECKFLAGS	+= -D__aarch64__
 
+ifeq ($(CONFIG_EVENLESS),y)
+KBUILD_CFLAGS += -Iarch/$(SRCARCH)/evenless/include -Iinclude/evenless
+endif
+
 ifeq ($(CONFIG_DYNAMIC_FTRACE_WITH_REGS),y)
   KBUILD_CPPFLAGS += -DCC_USING_PATCHABLE_FUNCTION_ENTRY
   CC_FLAGS_FTRACE := -fpatchable-function-entry=2

arch/arm64/include/asm/evenless/fptest.h

+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _EVENLESS_ARM64_ASM_FPTEST_H
+#define _EVENLESS_ARM64_ASM_FPTEST_H
+
+#include <linux/cpufeature.h>
+#include <uapi/asm/evenless/fptest.h>
+
+static inline bool evl_begin_fpu(void)
+{
+	return false;
+}
+
+static inline void evl_end_fpu(void) { }
+
+static inline u32 evl_detect_fpu(void)
+{
+	u32 features = 0;
+
+	if (system_supports_fpsimd())
+		return features |= evl_arm64_fpsimd;
+
+	if (system_supports_sve())
+		return features |= evl_arm64_sve;
+
+	return features;
+}
+
+#endif /* _EVENLESS_ARM64_ASM_FPTEST_H */

arch/arm64/include/asm/evenless/syscall.h

+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _EVENLESS_ARM64_ASM_SYSCALL_H
+#define _EVENLESS_ARM64_ASM_SYSCALL_H
+
+#include <linux/uaccess.h>
+#include <asm/unistd.h>
+#include <asm/ptrace.h>
+#include <uapi/asm/evenless/syscall.h>
+
+#define raw_put_user(src, dst)  __put_user(src, dst)
+#define raw_get_user(dst, src)  __get_user(dst, src)
+
+#define is_oob_syscall(__regs)	((__regs)->syscallno & __EVENLESS_SYSCALL_BIT)
+#define oob_syscall_nr(__regs)	((__regs)->syscallno & ~__EVENLESS_SYSCALL_BIT)
+
+#define oob_retval(__regs)	((__regs)->regs[0])
+#define oob_arg1(__regs)	((__regs)->regs[0])
+#define oob_arg2(__regs)	((__regs)->regs[1])
+#define oob_arg3(__regs)	((__regs)->regs[2])
+#define oob_arg4(__regs)	((__regs)->regs[3])
+#define oob_arg5(__regs)	((__regs)->regs[4])
+
+/*
+ * Fetch and test inband syscall number (valid only if
+ * !is_oob_syscall(__regs)).
+ */
+#define inband_syscall_nr(__regs, __nr)			\
+	({						\
+		*(__nr) = oob_syscall_nr(__regs);	\
+		!is_oob_syscall(__regs);		\
+	})
+
+static inline void
+set_oob_error(struct pt_regs *regs, int err)
+{
+	oob_retval(regs) = err;
+}
+
+static inline
+void set_oob_retval(struct pt_regs *regs, long ret)
+{
+	oob_retval(regs) = ret;
+}
+
+#endif /* !_EVENLESS_ARM64_ASM_SYSCALL_H */

arch/arm64/include/asm/evenless/thread.h

+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _EVENLESS_ARM64_ASM_THREAD_H
+#define _EVENLESS_ARM64_ASM_THREAD_H
+
+#define xnarch_fault_pf_p(__trapnr)	((__trapnr) == ARM64_TRAP_ACCESS)
+#define xnarch_fault_bp_p(__trapnr)	((current->ptrace & PT_PTRACED) && \
+					 (__trapnr == ARM64_TRAP_DEBUG || \
+					  (__trapnr) == ARM64_TRAP_UNDI))
+
+#define xnarch_fault_notify(__trapnr) (!xnarch_fault_bp_p(__trapnr))
+
+#endif /* !_EVENLESS_ARM64_ASM_THREAD_H */

arch/arm64/include/dovetail/thread_info.h

+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _EVENLESS_DOVETAIL_THREAD_INFO_H
+#define _EVENLESS_DOVETAIL_THREAD_INFO_H
+
+#include <asm-generic/evenless/thread_info.h>
+
+#endif /* !_EVENLESS_DOVETAIL_THREAD_INFO_H */

arch/arm64/include/uapi/asm/evenless/fptest.h

+/*
+ * SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note
+ * Derived from Xenomai's Cobalt core:
+ * Copyright (C) 2006 Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>.
+ */
+#ifndef _EVENLESS_ARM64_ASM_UAPI_FPTEST_H
+#define _EVENLESS_ARM64_ASM_UAPI_FPTEST_H
+
+#define evl_arm64_fpsimd  0x1
+#define evl_arm64_sve     0x2
+
+/*
+ * CAUTION: keep this code strictly inlined in macros: we don't want
+ * GCC to apply any callee-saved logic to fpsimd registers in
+ * evl_set_fpregs() before evl_check_fpregs() can verify their
+ * contents, but we still want GCC to know about the registers we have
+ * clobbered.
+ */
+
+#define evl_set_fpregs(__features, __val)				\
+	do {								\
+		unsigned int __i;					\
+		__u64 __e[32];						\
+									\
+		if (__features & evl_arm64_fpsimd) {			\
+			for (__i = 0; __i < 32; __i++)			\
+				__e[__i] = (__val);			\
+			__asm__ __volatile__("ldp  d0, d1, [%0, #8 * 0] \n"	\
+					     "ldp  d2, d3, [%0, #8 * 2] \n"	\
+					     "ldp  d4, d5, [%0, #8 * 4]\n"	\
+					     "ldp  d6, d7, [%0, #8 * 6]\n"	\
+					     "ldp  d8, d9, [%0, #8 * 8]\n"	\
+					     "ldp  d10, d11, [%0, #8 * 10]\n"	\
+					     "ldp  d12, d13, [%0, #8 * 12]\n"	\
+					     "ldp  d14, d15, [%0, #8 * 14]\n"	\
+					     "ldp  d16, d17, [%0, #8 * 16]\n"	\
+					     "ldp  d18, d19, [%0, #8 * 18]\n"	\
+					     "ldp  d20, d21, [%0, #8 * 20]\n"	\
+					     "ldp  d22, d23, [%0, #8 * 22]\n"	\
+					     "ldp  d24, d25, [%0, #8 * 24]\n"	\
+					     "ldp  d26, d27, [%0, #8 * 26]\n"	\
+					     "ldp  d28, d29, [%0, #8 * 28]\n"	\
+					     "ldp  d30, d31, [%0, #8 * 30]\n"	\
+					     : /* No outputs. */	\
+					     : "r"(&__e[0])		\
+					     : "d0", "d1", "d2", "d3", "d4", "d5", "d6",	\
+					       "d7", "d8", "d9", "d10", "d11", "d12", "d13",	\
+					       "d14", "d15", "d16", "d17", "d18", "d19",	\
+					       "d20", "d21", "d22", "d23", "d24", "d25",	\
+					       "d26", "d27", "d28", "d29", "d30", "d31",	\
+					       "memory");		\
+		}							\
+	} while (0)
+
+#define evl_check_fpregs(__features, __val, __bad)			\
+	({								\
+		unsigned int __result = (__val), __i;			\
+		__u64 __e[32];						\
+									\
+		if (__features & evl_arm64_fpsimd) {			\
+			__asm__ __volatile__("stp  d0, d1, [%0, #8 * 0] \n"	\
+					     "stp  d2, d3, [%0, #8 * 2] \n"	\
+					     "stp  d4, d5, [%0, #8 * 4]\n"	\
+					     "stp  d6, d7, [%0, #8 * 6]\n"	\
+					     "stp  d8, d9, [%0, #8 * 8]\n"	\
+					     "stp  d10, d11, [%0, #8 * 10]\n"	\
+					     "stp  d12, d13, [%0, #8 * 12]\n"	\
+					     "stp  d14, d15, [%0, #8 * 14]\n"	\
+					     "stp  d16, d17, [%0, #8 * 16]\n"	\
+					     "stp  d18, d19, [%0, #8 * 18]\n"	\
+					     "stp  d20, d21, [%0, #8 * 20]\n"	\
+					     "stp  d22, d23, [%0, #8 * 22]\n"	\
+					     "stp  d24, d25, [%0, #8 * 24]\n"	\
+					     "stp  d26, d27, [%0, #8 * 26]\n"	\
+					     "stp  d28, d29, [%0, #8 * 28]\n"	\
+					     "stp  d30, d31, [%0, #8 * 30]\n"	\
+					     :  /* No outputs. */	\
+					     : "r"(&__e[0])		\
+					     : "memory");		\
+									\
+			for (__i = 0; __i < 32; __i++)			\
+				if (__e[__i] != __val) {		\
+					__result = __e[__i];		\
+					(__bad) = __i;			\
+					break;				\
+				}					\
+		}							\
+		__result;						\
+	})
+
+#endif /* !_EVENLESS_ARM64_ASM_UAPI_FPTEST_H */

arch/arm64/include/uapi/asm/evenless/syscall.h

+/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
+#ifndef _EVENLESS_ARM64_ASM_UAPI_SYSCALL_H
+#define _EVENLESS_ARM64_ASM_UAPI_SYSCALL_H
+
+#define __EVENLESS_SYSCALL_BIT	0x10000000
+
+#endif /* !_EVENLESS_ARM64_ASM_UAPI_SYSCALL_H */

drivers/Kconfig

@@ -31,6 +31,8 @@ source "drivers/block/Kconfig"
 
 source "drivers/nvme/Kconfig"
 
+source "drivers/evenless/Kconfig"
+
 source "drivers/misc/Kconfig"
 
 source "drivers/ide/Kconfig"

drivers/Makefile

@@ -157,6 +157,8 @@ obj-$(CONFIG_REMOTEPROC)	+= remoteproc/
 obj-$(CONFIG_RPMSG)		+= rpmsg/
 obj-$(CONFIG_SOUNDWIRE)		+= soundwire/
 
+obj-$(CONFIG_EVENLESS)		+= evenless/
+
 # Virtualization drivers
 obj-$(CONFIG_VIRT_DRIVERS)	+= virt/
 obj-$(CONFIG_HYPERV)		+= hv/

drivers/evenless/Kconfig

+menu "Evenless OOB devices"
+
+config EVENLESS_LATMUS
+	bool "Latency calibration and measurement"
+	depends on EVENLESS
+	default y
+
+config EVENLESS_HECTIC
+	bool "OOB context switching validator"
+	depends on EVENLESS
+	default y
+
+endmenu

drivers/evenless/Makefile

+obj-$(CONFIG_EVENLESS_LATMUS)	+= latmus.o
+obj-$(CONFIG_EVENLESS_HECTIC)	+= hectic.o

drivers/evenless/hectic.c

+/*
+ * SPDX-License-Identifier: GPL-2.0
+ *
+ * Derived from Xenomai Cobalt's switchtest driver
+ * (http://git.xenomai.org/xenomai-3.git/)
+ * Copyright (C) 2010 Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>.
+ * Copyright (C) 2018 Philippe Gerum  <rpm@xenomai.org>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/vmalloc.h>
+#include <linux/slab.h>
+#include <linux/semaphore.h>
+#include <linux/irq_work.h>
+#include <evenless/synch.h>
+#include <evenless/thread.h>
+#include <evenless/wait.h>
+#include <evenless/file.h>
+#include <asm/evenless/fptest.h>
+#include <uapi/evenless/devices/hectic.h>
+
+#define RTSWITCH_RT      0x10000
+#define RTSWITCH_NRT     0
+#define RTSWITCH_KERNEL  0x20000
+
+struct rtswitch_context;
+
+struct rtswitch_task {
+	struct hectic_task_index base;
+	struct evl_wait_flag rt_synch;
+	struct semaphore nrt_synch;
+	struct evl_kthread kthread; /* For kernel-space real-time tasks. */
+	unsigned int last_switch;
+	struct rtswitch_context *ctx;
+};
+
+struct rtswitch_context {
+	struct rtswitch_task *tasks;
+	unsigned int tasks_count;
+	unsigned int next_index;
+	struct semaphore lock;
+	unsigned int cpu;
+	unsigned int switches_count;
+
+	unsigned long pause_us;
+	unsigned int next_task;
+	struct evl_timer wake_up_delay;
+
+	bool failed;
+	struct hectic_error error;
+
+	struct rtswitch_task *utask;
+	struct irq_work wake_utask;
+	struct evl_file efile;
+};
+
+static u32 fp_features;
+
+static void handle_fpu_error(struct rtswitch_context *ctx,
+			     unsigned int fp_in, unsigned int fp_out,
+			     int bad_reg)
+{
+	struct rtswitch_task *cur = &ctx->tasks[ctx->error.last_switch.to];
+	unsigned int i;
+
+	printk(EVL_ERR "fpreg%d trashed: in=%u, out=%u\n",
+	       bad_reg, fp_in, fp_out);
+
+	ctx->failed = true;
+	ctx->error.fp_val = fp_out;
+
+	if ((cur->base.flags & RTSWITCH_RT) == RTSWITCH_RT)
+		for (i = 0; i < ctx->tasks_count; i++) {
+			struct rtswitch_task *task = &ctx->tasks[i];
+
+			/* Find the first non kernel-space task. */
+			if ((task->base.flags & RTSWITCH_KERNEL))
+				continue;
+
+			/* Unblock it. */
+			switch(task->base.flags & RTSWITCH_RT) {
+			case RTSWITCH_NRT:
+				ctx->utask = task;
+				irq_work_queue(&ctx->wake_utask);
+				break;
+
+			case RTSWITCH_RT:
+				evl_raise_flag(&task->rt_synch);
+				break;
+			}
+
+			evl_stop_thread(&cur->kthread.thread, T_SUSP);
+		}
+}
+
+static int rtswitch_pend_rt(struct rtswitch_context *ctx,
+			    unsigned int idx)
+{
+	struct rtswitch_task *task;
+	int rc;
+
+	if (idx > ctx->tasks_count)
+		return -EINVAL;
+
+	task = &ctx->tasks[idx];
+	task->base.flags |= RTSWITCH_RT;
+
+	rc = evl_wait_flag(&task->rt_synch);
+	if (rc < 0)
+		return rc;
+
+	if (ctx->failed)
+		return 1;
+
+	return 0;
+}
+
+static void timed_wake_up(struct evl_timer *timer) /* hard irqs off */
+{
+	struct rtswitch_context *ctx;
+	struct rtswitch_task *task;
+
+	ctx = container_of(timer, struct rtswitch_context, wake_up_delay);
+	task = &ctx->tasks[ctx->next_task];
+
+	switch (task->base.flags & RTSWITCH_RT) {
+	case RTSWITCH_NRT:
+		ctx->utask = task;
+		irq_work_queue(&ctx->wake_utask);
+		break;
+
+	case RTSWITCH_RT:
+		evl_raise_flag(&task->rt_synch);
+	}
+}
+
+static int rtswitch_to_rt(struct rtswitch_context *ctx,
+			  unsigned int from_idx,
+			  unsigned int to_idx)
+{
+	struct rtswitch_task *from, *to;
+	int rc;
+
+	if (from_idx > ctx->tasks_count || to_idx > ctx->tasks_count)
+		return -EINVAL;
+
+	/* to == from is a special case which means
+	   "return to the previous task". */
+	if (to_idx == from_idx)
+		to_idx = ctx->error.last_switch.from;
+
+	from = &ctx->tasks[from_idx];
+	to = &ctx->tasks[to_idx];
+
+	from->base.flags |= RTSWITCH_RT;
+	from->last_switch = ++ctx->switches_count;
+	ctx->error.last_switch.from = from_idx;
+	ctx->error.last_switch.to = to_idx;
+	barrier();
+
+	if (ctx->pause_us) {
+		ctx->next_task = to_idx;
+		barrier();
+		evl_start_timer(&ctx->wake_up_delay,
+				evl_abs_timeout(&ctx->wake_up_delay,
+						ctx->pause_us * 1000),
+				EVL_INFINITE);