diff --git a/os/common/oslib/include/chpreempt.h b/os/common/oslib/include/chpreempt.h
new file mode 100644
index 000000000..bfa1ce270
--- /dev/null
+++ b/os/common/oslib/include/chpreempt.h
@@ -0,0 +1,106 @@
+/*
+ ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio.
+
+ This file is part of ChibiOS.
+
+ ChibiOS 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 3 of the License, or
+ (at your option) any later version.
+
+ ChibiOS 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 this program. If not, see .
+*/
+
+/**
+ * @file chpreempt.h
+ * @brief Preemption enforcement structures and macros.
+ *
+ * @addtogroup preemption_enforcement
+ * @{
+ */
+
+#ifndef CHPREEMPT_H
+#define CHPREEMPT_H
+
+/* This header is included from chconf.h and that can be included by asm
+ modules.*/
+#if !defined(_FROM_ASM_)
+
+/*===========================================================================*/
+/* Module constants. */
+/*===========================================================================*/
+
+/**
+ * @brief Hooks function @p chSchIsPreemptionRequired().
+ */
+#define CH_SCH_IS_PREEMPTION_REQUIRED_HOOKED
+
+/**
+ * @brief Hooks function @p chSchDoReschedule().
+ */
+#define CH_SCH_DO_RESCHEDULE_HOOKED
+
+/*===========================================================================*/
+/* Module pre-compile time settings. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks. */
+/*===========================================================================*/
+
+#if !defined(CH_CFG_ROUND_ROBIN_QUANTUM)
+#error "CH_CFG_ROUND_ROBIN_QUANTUM not defined in chconf.h"
+#endif
+
+#if CH_CFG_ROUND_ROBIN_QUANTUM <= 0
+#error "CH_CFG_ROUND_ROBIN_QUANTUM must be greater than zero"
+#endif
+
+#if CH_CFG_TIME_QUANTUM > 0
+#error "CH_CFG_ROUND_ROBIN_QUANTUM is not compatible with CH_CFG_TIME_QUANTUM > 0"
+#endif
+
+/*===========================================================================*/
+/* Module data structures and types. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module macros. */
+/*===========================================================================*/
+
+/**
+ * @brief Fields to be added to the @p ch_system_t structure.
+ *
+ * @notapi
+ */
+#define CH_PREEMPT_SYSTEM_FIELDS \
+ virtual_timer_t preempt_vt;
+
+/*===========================================================================*/
+/* External declarations. */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+ void ch_preempt_system_init(void);
+ void ch_preempt_thread_switch(void);
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions. */
+/*===========================================================================*/
+
+#endif /* !defined(_FROM_ASM_) */
+
+#endif /* CHPREEMPT_H */
+
+/** @} */
diff --git a/os/common/oslib/src/chpreempt.c b/os/common/oslib/src/chpreempt.c
new file mode 100644
index 000000000..a11940596
--- /dev/null
+++ b/os/common/oslib/src/chpreempt.c
@@ -0,0 +1,153 @@
+/*
+ ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio.
+
+ This file is part of ChibiOS.
+
+ ChibiOS 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 3 of the License, or
+ (at your option) any later version.
+
+ ChibiOS 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 this program. If not, see .
+*/
+
+/**
+ * @file chpreempt.c
+ * @brief Preemption enforcement code.
+ *
+ * @addtogroup preemption_enforcement
+ * @details This modules export hook macros required for implementing
+ * a preemptive round robin mode for threads at the same priority
+ * level.
+ * This method is alternative to the ChibiOS/RT native implementation
+ * which is not compatible with the tick-less mode, however, this
+ * timers-based solution can decrease threads context-switch
+ * performance because the added overhead.
+ * @note This file is not included automatically by @p ch.h, you need
+ * to:
+ * - Define @p CH_CFG_ROUND_ROBIN_QUANTUM in chconf.h. It is the
+ * time quantum in ticks.
+ * - Include @p chpreempt.h from @p chconf.h.
+ * - Add all the macros and functions to the appropriate hooks in
+ * chconf.h.
+ * - Explicitely add @p chpreempt.c to your makefile.
+ * .
+ */
+
+#include "ch.h"
+
+/*===========================================================================*/
+/* Module exported variables. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local types. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local variables. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local functions. */
+/*===========================================================================*/
+
+static void preempt_cb(void *p) {
+
+ (void)p;
+}
+
+/*===========================================================================*/
+/* Module exported functions. */
+/*===========================================================================*/
+
+/**
+ * @brief Hook code for system initialization.
+ *
+ * @notapi
+ */
+void ch_preempt_system_init(void) {
+
+ chVTObjectInit(&ch.preempt_vt);
+}
+
+/**
+ * @brief Hook code for context switch.
+ *
+ * @notapi
+ */
+void ch_preempt_thread_switch(void) {
+
+ chVTSetI(&ch.preempt_vt, CH_CFG_ROUND_ROBIN_QUANTUM, preempt_cb, NULL);
+}
+
+/**
+ * @brief Evaluates if preemption is required.
+ * @details The decision is taken by comparing the relative priorities and
+ * depending on the state of the round robin timeout counter.
+ * @note Not a user function, it is meant to be invoked by the scheduler
+ * itself or from within the port layer.
+ *
+ * @retval true if there is a thread that must go in running state
+ * immediately.
+ * @retval false if preemption is not required.
+ *
+ * @special
+ */
+bool chSchIsPreemptionRequired(void) {
+ tprio_t p1 = firstprio(&ch.rlist.queue);
+ tprio_t p2 = currp->prio;
+
+ /* If the running thread has not reached its time quantum, reschedule only
+ if the first thread on the ready queue has a higher priority.
+ Otherwise, if the running thread has used up its time quantum, reschedule
+ if the first thread on the ready queue has equal or higher priority.*/
+ return chVTIsArmed(&ch.preempt_vt) ? (p1 > p2) : (p1 >= p2);
+}
+/**
+ * @brief Switches to the first thread on the runnable queue.
+ * @details The current thread is positioned in the ready list behind or
+ * ahead of all threads having the same priority depending on
+ * if it used its whole time slice.
+ * @note Not a user function, it is meant to be invoked by the scheduler
+ * itself or from within the port layer.
+ *
+ * @special
+ */
+void chSchDoReschedule(void) {
+ thread_t *otp = currp;
+
+ /* Picks the first thread from the ready queue and makes it current.*/
+ currp = queue_fifo_remove(&ch.rlist.queue);
+ currp->state = CH_STATE_CURRENT;
+
+ /* Handling idle-leave hook.*/
+ if (otp->prio == IDLEPRIO) {
+ CH_CFG_IDLE_LEAVE_HOOK();
+ }
+
+ /* There are two different scenarios to handle on preemption: time quantum
+ elapsed or not.*/
+ if (!chVTIsArmed(&ch.preempt_vt)) {
+
+ /* The thread consumed its time quantum so it is enqueued behind threads
+ with same priority level, however, it acquires a new time quantum.*/
+ otp = chSchReadyI(otp);
+ }
+ else {
+ /* The thread didn't consume all its time quantum so it is put ahead of
+ threads with equal priority and does not acquire a new time quantum.*/
+ otp = chSchReadyAheadI(otp);
+ }
+
+ /* Swap operation as tail call.*/
+ chSysSwitch(currp, otp);
+}
+
+/** @} */
diff --git a/os/rt/src/chschd.c b/os/rt/src/chschd.c
index 476b19836..62ca81d50 100644
--- a/os/rt/src/chschd.c
+++ b/os/rt/src/chschd.c
@@ -462,6 +462,7 @@ void chSchRescheduleS(void) {
}
}
+#if !defined(CH_SCH_IS_PREEMPTION_REQUIRED_HOOKED)
/**
* @brief Evaluates if preemption is required.
* @details The decision is taken by comparing the relative priorities and
@@ -491,6 +492,7 @@ bool chSchIsPreemptionRequired(void) {
return p1 > p2;
#endif
}
+#endif /* !defined(CH_SCH_IS_PREEMPTION_REQUIRED_HOOKED) */
/**
* @brief Switches to the first thread on the runnable queue.
@@ -554,6 +556,7 @@ void chSchDoRescheduleAhead(void) {
chSysSwitch(currp, otp);
}
+#if !defined(CH_SCH_DO_RESCHEDULE_HOOKED)
/**
* @brief Switches to the first thread on the runnable queue.
* @details The current thread is positioned in the ready list behind or
@@ -602,5 +605,6 @@ void chSchDoReschedule(void) {
/* Swap operation as tail call.*/
chSysSwitch(currp, otp);
}
+#endif /*!defined(CH_SCH_DO_RESCHEDULE_HOOKED) */
/** @} */