16 files changed, 3464 insertions, 0 deletions
diff --git a/os/kernel/src/chcond.c b/os/kernel/src/chcond.c
new file mode 100644
index 000000000..ef58f1ddf
--- /dev/null
+++ b/os/kernel/src/chcond.c
@@ -0,0 +1,227 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+/*
+   Concepts and parts of this file are contributed by and Copyright (C) 2008
+   of Leon Woestenberg.
+ */
+
+/**
+ * @file chcond.c
+ * @brief Condition Variables code.
+ * @addtogroup CondVars
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_USE_CONDVARS && CH_USE_MUTEXES
+
+/**
+ * @brief Initializes s @p CondVar structure.
+ *
+ * @param[out] cp pointer to a @p CondVar structure
+ * @note This function can be invoked from within an interrupt handler even if
+ *       it is not an I-Class API because it does not touch any critical kernel
+ *       data structure.
+ */
+void chCondInit(CondVar *cp) {
+
+  chDbgCheck(cp != NULL, "chCondInit");
+
+  queue_init(&cp->c_queue);
+}
+
+/**
+ * @brief Signals one thread that is waiting on the condition variable.
+ *
+ * @param[in] cp pointer to the @p CondVar structure
+ */
+void chCondSignal(CondVar *cp) {
+
+  chDbgCheck(cp != NULL, "chCondSignal");
+
+  chSysLock();
+  if (notempty(&cp->c_queue))                           /* any thread ? */
+    chSchWakeupS(fifo_remove(&cp->c_queue), RDY_OK);
+  chSysUnlock();
+}
+
+/**
+ * @brief Signals one thread that is waiting on the condition variable.
+ *
+ * @param[in] cp pointer to the @p CondVar structure
+ */
+void chCondSignalI(CondVar *cp) {
+
+  chDbgCheck(cp != NULL, "chCondSignalI");
+
+  if (notempty(&cp->c_queue))                           /* any thread ? */
+    chSchReadyI(fifo_remove(&cp->c_queue))->p_rdymsg = RDY_OK;
+}
+
+/**
+ * @brief Signals all threads that are waiting on the condition variable.
+ *
+ * @param[in] cp pointer to the @p CondVar structure
+ */
+void chCondBroadcast(CondVar *cp) {
+
+  chSysLock();
+  chCondBroadcastI(cp);
+  chSchRescheduleS();
+  chSysUnlock();
+}
+
+/**
+ * @brief Signals all threads that are waiting on the condition variable.
+ *
+ * @param[in] cp pointer to the @p CondVar structure
+ */
+void chCondBroadcastI(CondVar *cp) {
+
+  chDbgCheck(cp != NULL, "chCondBroadcastI");
+
+  /* empties the condition variable queue and inserts all the Threads into the
+   * ready list in FIFO order. The wakeup message is set to @p RDY_RESET in
+   * order to make a chCondBroadcast() detectable from a chCondSignal(). */
+  while (cp->c_queue.p_next != (void *)&cp->c_queue)
+    chSchReadyI(fifo_remove(&cp->c_queue))->p_rdymsg = RDY_RESET;
+}
+
+/**
+ * @brief Waits on the condition variable releasing the mutex lock.
+ * @details Releases the mutex, waits on the condition variable, and finally
+ *          acquires the mutex again. This is done atomically.
+ *
+ * @param[in] cp pointer to the @p CondVar structure
+ * @return The wakep mode.
+ * @retval RDY_OK if the condvar was signaled using chCondSignal().
+ * @retval RDY_RESET if the condvar was signaled using chCondBroadcast().
+ * @note  The thread MUST already have locked the mutex when calling
+ *        @p chCondWait().
+ */
+msg_t chCondWait(CondVar *cp) {
+  msg_t msg;
+
+  chSysLock();
+  msg = chCondWaitS(cp);
+  chSysUnlock();
+  return msg;
+}
+
+/**
+ * @brief Waits on the condition variable releasing the mutex lock.
+ * @details Releases the mutex, waits on the condition variable, and finally
+ *          acquires the mutex again. This is done atomically.
+ *
+ * @param[in] cp pointer to the @p CondVar structure
+ * @return The wakep mode.
+ * @retval RDY_OK if the condvar was signaled using chCondSignal().
+ * @retval RDY_RESET if the condvar was signaled using chCondBroadcast().
+ * @note  The thread MUST already have locked the mutex when calling
+ *        @p chCondWaitS().
+ */
+msg_t chCondWaitS(CondVar *cp) {
+  Mutex *mp;
+  msg_t msg;
+
+  chDbgCheck(cp != NULL, "chCondWaitS");
+  chDbgAssert(currp->p_mtxlist != NULL,
+              "chCondWaitS(), #1",
+              "not owning a mutex");
+
+  mp = chMtxUnlockS();                  /* unlocks the condvar mutex */
+  prio_insert(currp, &cp->c_queue);     /* enters the condvar queue */
+  currp->p_wtcondp = cp;                /* needed by the tracer */
+  chSchGoSleepS(PRWTCOND);              /* waits on the condvar */
+  msg = currp->p_rdymsg;                /* fetches the wakeup message */
+  chMtxLockS(mp);                       /* atomically relocks the mutex */
+  return msg;                           /* returns the wakeup message */
+}
+
+#if CH_USE_CONDVARS_TIMEOUT
+/**
+ * @brief Waits on the condition variable releasing the mutex lock.
+ * @details Releases the mutex, waits on the condition variable, and finally
+ *          acquires the mutex again. This is done atomically.
+ *
+ * @param[in] cp pointer to the @p CondVar structure
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the special value @p TIME_INFINITE is allowed.
+ *                 It is not possible to specify zero @p TIME_IMMEDIATE
+ *                 as timeout specification because it would make no sense
+ *                 in this function.
+ * @return The wakep mode.
+ * @retval RDY_OK if the condvar was signaled using chCondSignal().
+ * @retval RDY_RESET if the condvar was signaled using chCondBroadcast().
+ * @retval RDY_TIMEOUT if the condvar was not signaled within the specified
+ *         timeout.
+ * @note  The thread MUST already have locked the mutex when calling
+ *        @p chCondWaitTimeout().
+ */
+msg_t chCondWaitTimeout(CondVar *cp, systime_t time) {
+  msg_t msg;
+
+  chSysLock();
+  msg = chCondWaitTimeoutS(cp, time);
+  chSysUnlock();
+  return msg;
+}
+
+/**
+ * @brief Waits on the condition variable releasing the mutex lock.
+ * @details Releases the mutex, waits on the condition variable, and finally
+ *          acquires the mutex again. This is done atomically.
+ *
+ * @param[in] cp pointer to the @p CondVar structure
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the special value @p TIME_INFINITE is allowed.
+ *                 It is not possible to specify zero @p TIME_IMMEDIATE
+ *                 as timeout specification because it would make no sense
+ *                 in this function.
+ * @return The wakep mode.
+ * @retval RDY_OK if the condvar was signaled using chCondSignal().
+ * @retval RDY_RESET if the condvar was signaled using chCondBroadcast().
+ * @retval RDY_TIMEOUT if the condvar was not signaled within the specified
+ *         timeout.
+ * @note  The thread MUST already have locked the mutex when calling
+ *        @p chCondWaitTimeoutS().
+ */
+msg_t chCondWaitTimeoutS(CondVar *cp, systime_t time) {
+  Mutex *mp;
+  msg_t msg;
+
+  chDbgCheck(cp != NULL, "chCondWaitTimeoutS");
+  chDbgAssert(currp->p_mtxlist != NULL,
+              "chCondWaitTimeoutS(), #1",
+              "not owning a mutex");
+
+  mp = chMtxUnlockS();                  /* unlocks the condvar mutex */
+  prio_insert(currp, &cp->c_queue);     /* enters the condvar queue */
+  currp->p_wtcondp = cp;                /* needed by the tracer */
+  chSchGoSleepTimeoutS(PRWTCOND, time); /* waits on the condvar */
+  msg = currp->p_rdymsg;                /* fetches the wakeup message */
+  chMtxLockS(mp);                       /* atomically relocks the mutex */
+  return msg;                           /* returns the wakeup message */
+}
+#endif /* CH_USE_CONDVARS_TIMEOUT */
+
+#endif /* CH_USE_CONDVARS && CH_USE_MUTEXES */
+
+/** @} */
diff --git a/os/kernel/src/chdebug.c b/os/kernel/src/chdebug.c
new file mode 100644
index 000000000..9a8120b29
--- /dev/null
+++ b/os/kernel/src/chdebug.c
@@ -0,0 +1,81 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chdebug.c
+ * @brief ChibiOS/RT Debug code.
+ * @addtogroup Debug
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_DBG_ENABLE_TRACE
+/**
+ * @brief Public trace buffer.
+ */
+TraceBuffer trace_buffer;
+
+/**
+ * @brief Trace circular buffer subsystem initialization.
+ */
+void trace_init(void) {
+
+  trace_buffer.tb_size = TRACE_BUFFER_SIZE;
+  trace_buffer.tb_ptr = &trace_buffer.tb_buffer[0];
+}
+
+/**
+ * @brief Inserts in the circular debug trace buffer a context switch record.
+ *
+ * @param[in] otp the thread being switched out
+ * @param[in] ntp the thread to be switched in
+ */
+void chDbgTrace(Thread *otp, Thread *ntp) {
+
+  trace_buffer.tb_ptr->cse_wtobjp = otp->p_wtobjp;
+  trace_buffer.tb_ptr->cse_time = chTimeNow();
+  trace_buffer.tb_ptr->cse_state = otp->p_state;
+  trace_buffer.tb_ptr->cse_tid = (unsigned)ntp >> 4;
+  if (++trace_buffer.tb_ptr >= &trace_buffer.tb_buffer[TRACE_BUFFER_SIZE])
+    trace_buffer.tb_ptr = &trace_buffer.tb_buffer[0];
+}
+#endif /* CH_DBG_ENABLE_TRACE */
+
+#if CH_DBG_ENABLE_ASSERTS || CH_DBG_ENABLE_CHECKS || CH_DBG_ENABLE_STACK_CHECK
+/**
+ * @brief Pointer to the panic message.
+ * @details This pointer is meant to be accessed through the debugger, it is
+ *          written once and then the system is halted.
+ */
+char *panic_msg;
+
+/**
+ * @brief Prints a panic message on the console and then halts the system.
+ *
+ * @param[in] msg the pointer to the panic message string
+ */
+void chDbgPanic(char *msg) {
+
+  panic_msg = msg;
+  chSysHalt();
+}
+#endif /* CH_DBG_ENABLE_ASSERTS || CH_DBG_ENABLE_CHECKS || CH_DBG_ENABLE_STACK_CHECK */
+
+/** @} */
diff --git a/os/kernel/src/chevents.c b/os/kernel/src/chevents.c
new file mode 100644
index 000000000..098adce5e
--- /dev/null
+++ b/os/kernel/src/chevents.c
@@ -0,0 +1,392 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chevents.c
+ * @brief Events code.
+ * @addtogroup Events
+ * @{
+ */
+#include <ch.h>
+
+#if CH_USE_EVENTS
+/**
+ * @brief Registers an Event Listener on an Event Source.
+ *
+ * @param[in] esp pointer to the  @p EventSource structure
+ * @param[in] elp pointer to the @p EventListener structure
+ * @param[in] emask the mask of event flags to be pended to the thread when the
+ *                  event source is broadcasted
+ * @note Multiple Event Listeners can specify the same bits to be pended.
+ */
+void chEvtRegisterMask(EventSource *esp, EventListener *elp, eventmask_t emask) {
+
+  chDbgCheck((esp != NULL) && (elp != NULL), "chEvtRegisterMask");
+
+  chSysLock();
+  elp->el_next = esp->es_next;
+  esp->es_next = elp;
+  elp->el_listener = currp;
+  elp->el_mask = emask;
+  chSysUnlock();
+}
+
+/**
+ * @brief Unregisters an Event Listener from its Event Source.
+ *
+ * @param[in] esp pointer to the  @p EventSource structure
+ * @param[in] elp pointer to the @p EventListener structure
+ * @note If the event listener is not registered on the specified event source
+ *       then the function does nothing.
+ * @note For optimal performance it is better to perform the unregister
+ *       operations in inverse order of the register operations (elements are
+ *       found on top of the list).
+ */
+void chEvtUnregister(EventSource *esp, EventListener *elp) {
+  EventListener *p;
+
+  chDbgCheck((esp != NULL) && (elp != NULL), "chEvtUnregister");
+
+  p = (EventListener *)esp;
+  chSysLock();
+  while (p->el_next != (EventListener *)esp) {
+    if (p->el_next == elp) {
+      p->el_next = elp->el_next;
+      break;
+    }
+    p = p->el_next;
+  }
+  chSysUnlock();
+}
+
+/**
+ * @brief Clears the pending events specified in the mask.
+ *
+ * @param[in] mask the events to be cleared
+ * @return The pending events that were cleared.
+ */
+eventmask_t chEvtClear(eventmask_t mask) {
+  eventmask_t m;
+
+  chSysLock();
+
+  m = currp->p_epending & mask;
+  currp->p_epending &= ~mask;
+
+  chSysUnlock();
+  return m;
+}
+
+/**
+ * @brief Pends a set of event flags on the current thread, this is @b much
+ *        faster than using @p chEvtBroadcast() or @p chEvtSignal().
+ *
+ * @param[in] mask the events to be pended
+ * @return The current pending events mask.
+ */
+eventmask_t chEvtPend(eventmask_t mask) {
+
+  chSysLock();
+
+  mask = (currp->p_epending |= mask);
+
+  chSysUnlock();
+  return mask;
+}
+
+/**
+ * @brief Pends a set of event flags on the specified @p Thread.
+ *
+ * @param[in] tp the thread to be signaled
+ * @param[in] mask the event flags set to be pended
+ */
+void chEvtSignal(Thread *tp, eventmask_t mask) {
+
+  chDbgCheck(tp != NULL, "chEvtSignal");
+
+  chSysLock();
+  chEvtSignalI(tp, mask);
+  chSysUnlock();
+}
+
+/**
+ * @brief Pends a set of event flags on the specified @p Thread.
+ *
+ * @param[in] tp the thread to be signaled
+ * @param[in] mask the event flags set to be pended
+ */
+void chEvtSignalI(Thread *tp, eventmask_t mask) {
+
+  chDbgCheck(tp != NULL, "chEvtSignalI");
+
+  tp->p_epending |= mask;
+  /* Test on the AND/OR conditions wait states.*/
+  if (((tp->p_state == PRWTOREVT) && ((tp->p_epending & tp->p_ewmask) != 0)) ||
+      ((tp->p_state == PRWTANDEVT) && ((tp->p_epending & tp->p_ewmask) == tp->p_ewmask)))
+    chSchReadyI(tp)->p_rdymsg = RDY_OK;
+}
+
+/**
+ * @brief Signals all the Event Listeners registered on the specified Event
+ *        Source.
+ *
+ * @param[in] esp pointer to the @p EventSource structure
+ */
+void chEvtBroadcast(EventSource *esp) {
+
+  chSysLock();
+  chEvtBroadcastI(esp);
+  chSchRescheduleS();
+  chSysUnlock();
+}
+
+/**
+ * @brief Signals all the Event Listeners registered on the specified Event
+ *        Source.
+ *
+ * @param[in] esp pointer to the @p EventSource structure
+ */
+void chEvtBroadcastI(EventSource *esp) {
+  EventListener *elp;
+
+  chDbgCheck(esp != NULL, "chEvtBroadcastI");
+
+  elp = esp->es_next;
+  while (elp != (EventListener *)esp) {
+    chEvtSignalI(elp->el_listener, elp->el_mask);
+    elp = elp->el_next;
+  }
+}
+
+/**
+ * @brief Invokes the event handlers associated with a mask.
+ *
+ * @param[in] mask mask of the events to be dispatched
+ * @param[in] handlers an array of @p evhandler_t. The array must have size
+ *                     equal to the number of bits in eventmask_t.
+ */
+void chEvtDispatch(const evhandler_t handlers[], eventmask_t mask) {
+  eventid_t eid;
+
+  chDbgCheck(handlers != NULL, "chEvtDispatch");
+
+  eid = 0;
+  while (mask) {
+    if (mask & EVENT_MASK(eid)) {
+      chDbgAssert(handlers[eid] != NULL,
+                  "chEvtDispatch(), #1",
+                  "null handler");
+      mask &= ~EVENT_MASK(eid);
+      handlers[eid](eid);
+    }
+    eid++;
+  }
+}
+
+#if CH_OPTIMIZE_SPEED || !CH_USE_EVENTS_TIMEOUT || defined(__DOXYGEN__)
+/**
+ * @brief Waits for exactly one of the specified events.
+ * @details The function waits for one event among those specified in
+ *          @p ewmask to become pending then the event is cleared and returned.
+ *
+ * @param[in] ewmask mask of the events that the function should wait for,
+ *                   @p ALL_EVENTS enables all the events
+ * @return The mask of the lowest id served and cleared event.
+ * @note One and only one event is served in the function, the one with the
+ *       lowest event id. The function is meant to be invoked into a loop in
+ *       order to serve all the pending events.<br>
+ *       This means that Event Listeners with a lower event identifier have
+ *       an higher priority.
+ */
+eventmask_t chEvtWaitOne(eventmask_t ewmask) {
+  eventmask_t m;
+
+  chSysLock();
+
+  if ((m = (currp->p_epending & ewmask)) == 0) {
+    currp->p_ewmask = ewmask;
+    chSchGoSleepS(PRWTOREVT);
+    m = currp->p_epending & ewmask;
+  }
+  m &= -m;
+  currp->p_epending &= ~m;
+
+  chSysUnlock();
+  return m;
+}
+
+/**
+ * @brief Waits for any of the specified events.
+ * @details The function waits for any event among those specified in
+ *          @p ewmask to become pending then the events are cleared and returned.
+ *
+ * @param[in] ewmask mask of the events that the function should wait for,
+ *                   @p ALL_EVENTS enables all the events
+ * @return The mask of the served and cleared events.
+ */
+eventmask_t chEvtWaitAny(eventmask_t ewmask) {
+  eventmask_t m;
+
+  chSysLock();
+
+  if ((m = (currp->p_epending & ewmask)) == 0) {
+    currp->p_ewmask = ewmask;
+    chSchGoSleepS(PRWTOREVT);
+    m = currp->p_epending & ewmask;
+  }
+  currp->p_epending &= ~m;
+
+  chSysUnlock();
+  return m;
+}
+
+/**
+ * @brief Waits for all the specified events.
+ * @details The function waits for all the events specified in @p ewmask to
+ *          become pending then the events are cleared and returned.
+ *
+ * @param[in] ewmask mask of the event ids that the function should wait for
+ * @return The mask of the served and cleared events.
+ */
+eventmask_t chEvtWaitAll(eventmask_t ewmask) {
+
+  chSysLock();
+
+  if ((currp->p_epending & ewmask) != ewmask) {
+    currp->p_ewmask = ewmask;
+    chSchGoSleepS(PRWTANDEVT);
+  }
+  currp->p_epending &= ~ewmask;
+
+  chSysUnlock();
+  return ewmask;
+}
+#endif /* CH_OPTIMIZE_SPEED || !CH_USE_EVENTS_TIMEOUT */
+
+#if CH_USE_EVENTS_TIMEOUT
+/**
+ * @brief Waits for exactly one of the specified events.
+ * @details The function waits for one event among those specified in
+ *          @p ewmask to become pending then the event is cleared and returned.
+ *
+ * @param[in] ewmask mask of the events that the function should wait for,
+ *                   @p ALL_EVENTS enables all the events
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @return The mask of the lowest id served and cleared event.
+ * @retval 0 if the specified timeout expired.
+ * @note One and only one event is served in the function, the one with the
+ *       lowest event id. The function is meant to be invoked into a loop in
+ *       order to serve all the pending events.<br>
+ *       This means that Event Listeners with a lower event identifier have
+ *       an higher priority.
+ */
+eventmask_t chEvtWaitOneTimeout(eventmask_t ewmask, systime_t time) {
+  eventmask_t m;
+
+  chSysLock();
+
+  if ((m = (currp->p_epending & ewmask)) == 0) {
+    if (TIME_IMMEDIATE == time)
+      return (eventmask_t)0;
+    currp->p_ewmask = ewmask;
+    if (chSchGoSleepTimeoutS(PRWTOREVT, time) < RDY_OK)
+      return (eventmask_t)0;
+    m = currp->p_epending & ewmask;
+  }
+  m &= -m;
+  currp->p_epending &= ~m;
+
+  chSysUnlock();
+  return m;
+}
+
+/**
+ * @brief Waits for any of the specified events.
+ * @details The function waits for any event among those specified in
+ *          @p ewmask to become pending then the events are cleared and
+ *          returned.
+ *
+ * @param[in] ewmask mask of the events that the function should wait for,
+ *                   @p ALL_EVENTS enables all the events
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @return The mask of the served and cleared events.
+ * @retval 0 if the specified timeout expired.
+ */
+eventmask_t chEvtWaitAnyTimeout(eventmask_t ewmask, systime_t time) {
+  eventmask_t m;
+
+  chSysLock();
+
+  if ((m = (currp->p_epending & ewmask)) == 0) {
+    if (TIME_IMMEDIATE == time)
+      return (eventmask_t)0;
+    currp->p_ewmask = ewmask;
+    if (chSchGoSleepTimeoutS(PRWTOREVT, time) < RDY_OK)
+      return (eventmask_t)0;
+    m = currp->p_epending & ewmask;
+  }
+  currp->p_epending &= ~m;
+
+  chSysUnlock();
+  return m;
+}
+
+/**
+ * @brief Waits for all the specified events.
+ * @details The function waits for all the events specified in @p ewmask to
+ *          become pending then the events are cleared and returned.
+ *
+ * @param[in] ewmask mask of the event ids that the function should wait for
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @return The mask of the served and cleared events.
+ * @retval 0 if the specified timeout expired.
+ */
+eventmask_t chEvtWaitAllTimeout(eventmask_t ewmask, systime_t time) {
+
+  chSysLock();
+
+  if ((currp->p_epending & ewmask) != ewmask) {
+    if (TIME_IMMEDIATE == time)
+      return (eventmask_t)0;
+    currp->p_ewmask = ewmask;
+    if (chSchGoSleepTimeoutS(PRWTANDEVT, time) < RDY_OK)
+      return (eventmask_t)0;
+  }
+  currp->p_epending &= ~ewmask;
+
+  chSysUnlock();
+  return ewmask;
+}
+#endif /* CH_USE_EVENTS_TIMEOUT */
+
+#endif /* CH_USE_EVENTS */
+
+/** @} */
diff --git a/os/kernel/src/chheap.c b/os/kernel/src/chheap.c
new file mode 100644
index 000000000..82b1ba785
--- /dev/null
+++ b/os/kernel/src/chheap.c
@@ -0,0 +1,276 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chheap.c
+ * @brief Heap code.
+ * @addtogroup Heap
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_USE_HEAP
+
+#if !CH_USE_MALLOC_HEAP
+
+#define MAGIC 0xF5A0
+#define ALIGN_TYPE      void *
+#define ALIGN_MASK      (sizeof(ALIGN_TYPE) - 1)
+#define ALIGN_SIZE(p)   (((size_t)(p) + ALIGN_MASK) & ~ALIGN_MASK)
+
+struct header {
+  union {
+    struct header       *h_next;
+    size_t              h_magic;
+  };
+  size_t                h_size;
+};
+
+static struct {
+  struct header         free;   /* Guaranteed to be not adjacent to the heap */
+#if CH_USE_MUTEXES
+#define H_LOCK()        chMtxLock(&heap.hmtx)
+#define H_UNLOCK()      chMtxUnlock()
+  Mutex                 hmtx;
+#elif CH_USE_SEMAPHORES
+#define H_LOCK()        chSemWait(&heap.hsem)
+#define H_UNLOCK()      chSemSignal(&heap.hsem)
+  Semaphore             hsem;
+#else
+#error "The heap allocator requires mutexes or semaphores to be enabled"
+#endif
+#if CH_HEAP_SIZE > 0
+  union {
+    ALIGN_TYPE          alignment;
+    char                buffer[ALIGN_SIZE(CH_HEAP_SIZE)];
+  };
+#endif
+} heap;
+
+/**
+ * @brief Initializes the allocator subsystem.
+ *
+ * @note Internal use only.
+ */
+void heap_init(void) {
+  struct header *hp;
+
+#if CH_HEAP_SIZE == 0
+  extern char __heap_base__;
+  extern char __heap_end__;
+
+  hp = (void *)&__heap_base__;
+  hp->h_size = &__heap_end__ - &__heap_base__ - sizeof(struct header);
+#else
+  hp = (void *)&heap.buffer[0];
+  hp->h_size = (&heap.buffer[ALIGN_SIZE(CH_HEAP_SIZE)] - &heap.buffer[0]) -
+               sizeof(struct header);
+#endif
+  hp->h_next = NULL;
+  heap.free.h_next = hp;
+  heap.free.h_size = 0;
+#if CH_USE_MUTEXES
+  chMtxInit(&heap.hmtx);
+#else
+  chSemInit(&heap.hsem, 1);
+#endif
+}
+
+/**
+ * @brief Allocates a block of memory from the heap by using the first-fit
+ *        algorithm.
+ * @details The allocated block is guaranteed to be properly aligned for a
+ *          pointer data type.
+ *
+ * @param[in] size the size of the block to be allocated. Note that the
+ *                 allocated block may be a bit bigger than the requested
+ *                 size for alignment and fragmentation reasons.
+ * @return A pointer to the allocated block.
+ * @retval NULL if the block cannot be allocated.
+ */
+void *chHeapAlloc(size_t size) {
+  struct header *qp, *hp, *fp;
+
+  size = ALIGN_SIZE(size);
+  qp = &heap.free;
+  H_LOCK();
+
+  while (qp->h_next != NULL) {
+    hp = qp->h_next;
+    if (hp->h_size >= size) {
+      if (hp->h_size < size + sizeof(struct header)) {
+        /* Gets the whole block even if it is slightly bigger than the
+           requested size because the fragment would be too small to be
+           useful */
+        qp->h_next = hp->h_next;
+      }
+      else {
+        /* Block bigger enough, must split it */
+        fp = (void *)((char *)(hp) + sizeof(struct header) + size);
+        fp->h_next = hp->h_next;
+        fp->h_size = hp->h_size - sizeof(struct header) - size;
+        qp->h_next = fp;
+        hp->h_size = size;
+      }
+      hp->h_magic = MAGIC;
+
+      H_UNLOCK();
+      return (void *)(hp + 1);
+    }
+    qp = hp;
+  }
+
+  H_UNLOCK();
+  return NULL;
+}
+
+#define LIMIT(p) (struct header *)((char *)(p) + \
+                                   sizeof(struct header) + \
+                                   (p)->h_size)
+
+/**
+ * @brief Frees a previously allocated memory block.
+ *
+ * @param[in] p the memory block pointer
+ */
+void chHeapFree(void *p) {
+  struct header *qp, *hp;
+
+  chDbgCheck(p != NULL, "chHeapFree");
+
+  hp = (struct header *)p - 1;
+  chDbgAssert(hp->h_magic == MAGIC,
+              "chHeapFree(), #1",
+              "it is not magic");
+  qp = &heap.free;
+  H_LOCK();
+
+  while (TRUE) {
+
+    chDbgAssert((hp < qp) || (hp >= LIMIT(qp)),
+                "chHeapFree(), #2",
+                "within free block");
+
+    if (((qp == &heap.free) || (hp > qp)) &&
+        ((qp->h_next == NULL) || (hp < qp->h_next))) {
+      /* Insertion after qp */
+      hp->h_next = qp->h_next;
+      qp->h_next = hp;
+      /* Verifies if the newly inserted block should be merged */
+      if (LIMIT(hp) == hp->h_next) {
+        /* Merge with the next block */
+        hp->h_size += hp->h_next->h_size + sizeof(struct header);
+        hp->h_next = hp->h_next->h_next;
+      }
+      if ((LIMIT(qp) == hp)) {  /* Cannot happen when qp == &heap.free */
+        /* Merge with the previous block */
+        qp->h_size += hp->h_size + sizeof(struct header);
+        qp->h_next = hp->h_next;
+      }
+
+      H_UNLOCK();
+      return;
+    }
+    qp = qp->h_next;
+  }
+}
+
+/**
+ * @brief Reports the heap status.
+ *
+ * @param[in] sizep pointer to a variable that will receive the total
+ *                  fragmented free space
+ * @return The number of fragments in the heap.
+ * @note This function is meant to be used in the test suite, it should not be
+ *       really useful for the application code.
+ * @note This function is not implemented when the @p CH_USE_MALLOC_HEAP
+ *       configuration option is used (it always returns zero).
+ */
+size_t chHeapStatus(size_t *sizep) {
+  struct header *qp;
+  size_t n, sz;
+
+  H_LOCK();
+
+  sz = 0;
+  for (n = 0, qp = &heap.free; qp->h_next; n++, qp = qp->h_next)
+    sz += qp->h_next->h_size;
+  if (sizep)
+    *sizep = sz;
+
+  H_UNLOCK();
+  return n;
+}
+
+#else /* CH_USE_MALLOC_HEAP */
+
+#include <stdlib.h>
+
+#if CH_USE_MUTEXES
+#define H_LOCK()        chMtxLock(&hmtx)
+#define H_UNLOCK()      chMtxLock(&hmtx)
+static Mutex            hmtx;
+#elif CH_USE_SEMAPHORES
+#define H_LOCK()        chSemWait(&hsem)
+#define H_UNLOCK()      chSemSignal(&hsem)
+static Semaphore        hsem;
+#else
+#error "The heap allocator requires mutexes or semaphores to be enabled"
+#endif
+
+void heap_init(void) {
+
+#if CH_USE_MUTEXES
+  chMtxInit(&hmtx);
+#else
+  chSemInit(&hsem, 1);
+#endif
+}
+
+void *chHeapAlloc(size_t size) {
+  void *p;
+
+  H_LOCK();
+  p = malloc(size);
+  H_UNLOCK();
+  return p;
+}
+
+void chHeapFree(void *p) {
+
+  chDbgCheck(p != NULL, "chHeapFree");
+
+  H_LOCK();
+  free(p);
+  H_UNLOCK();
+}
+
+size_t chHeapStatus(size_t *sizep) {
+
+  if (sizep)
+    *sizep = 0;
+  return 0;
+}
+
+#endif /* CH_USE_MALLOC_HEAP */
+
+#endif /* CH_USE_HEAP */
+
+/** @} */
diff --git a/os/kernel/src/chlists.c b/os/kernel/src/chlists.c
new file mode 100644
index 000000000..a2177ca63
--- /dev/null
+++ b/os/kernel/src/chlists.c
@@ -0,0 +1,111 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chlists.c
+ * @brief Thread queues/lists code.
+ * @addtogroup ThreadLists
+ * @{
+ */
+#include <ch.h>
+
+#if !CH_OPTIMIZE_SPEED || defined(__DOXYGEN__)
+/**
+ * @brief Inserts a thread into a priority ordered queue.
+ *
+ * @param[in] tp the pointer to the thread to be inserted in the list
+ * @param[in] tqp the pointer to the threads list header
+ * @note The insertion is done by scanning the list from the highest priority
+ *       toward the lowest.
+ * @note This function is @b not an API.
+ */
+void prio_insert(Thread *tp, ThreadsQueue *tqp) {
+
+  /* cp iterates over the queue */
+  Thread *cp = (Thread *)tqp;
+  do {
+    /* iterate to next thread in queue */
+    cp = cp->p_next;
+    /* not end of queue? and cp has equal or higher priority than tp? */
+  } while ((cp != (Thread *)tqp) && (cp->p_prio >= tp->p_prio));
+  /* insert before cp, point tp to next and prev in queue */
+  tp->p_prev = (tp->p_next = cp)->p_prev;
+  /* make prev point to tp, and cp point back to tp */
+  tp->p_prev->p_next = cp->p_prev = tp;
+}
+
+/**
+ * @brief Inserts a Thread into a queue.
+ *
+ * @param[in] tp the pointer to the thread to be inserted in the list
+ * @param[in] tqp the pointer to the threads list header
+ * @note This function is @b not an API.
+ */
+void queue_insert(Thread *tp, ThreadsQueue *tqp) {
+
+  tp->p_prev = (tp->p_next = (Thread *)tqp)->p_prev;
+  tp->p_prev->p_next = tqp->p_prev = tp;
+}
+
+/**
+ * @brief Removes the first-out Thread from a queue and returns it.
+ *
+ * @param[in] tqp the pointer to the threads list header
+ * @return The removed thread pointer.
+ * @note This function is @b not an API.
+ */
+Thread *fifo_remove(ThreadsQueue *tqp) {
+  Thread *tp = tqp->p_next;
+
+  (tqp->p_next = tp->p_next)->p_prev = (Thread *)tqp;
+  return tp;
+}
+
+/**
+ * @brief Removes the last-out Thread from a queue and returns it.
+ *
+ * @param[in] tqp the pointer to the threads list header
+ * @return The removed thread pointer.
+ * @note This function is @b not an API.
+ */
+Thread *lifo_remove(ThreadsQueue *tqp) {
+  Thread *tp = tqp->p_next;
+
+  (tqp->p_next = tp->p_next)->p_prev = (Thread *)tqp;
+  return tp;
+}
+
+/**
+ * @brief Removes a Thread from a queue and returns it.
+ * @details The thread is removed from the queue regardless of its relative
+ *          position and regardless the used insertion method.
+ *
+ * @param[in] tp the pointer to the thread to be removed from the queue
+ * @return The removed thread pointer.
+ * @note This function is @b not an API.
+ */
+Thread *dequeue(Thread *tp) {
+
+  tp->p_prev->p_next = tp->p_next;
+  tp->p_next->p_prev = tp->p_prev;
+  return tp;
+}
+#endif /* CH_OPTIMIZE_SPEED */
+
+/** @} */
diff --git a/os/kernel/src/chmboxes.c b/os/kernel/src/chmboxes.c
new file mode 100644
index 000000000..8a791a984
--- /dev/null
+++ b/os/kernel/src/chmboxes.c
@@ -0,0 +1,244 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chmboxes.c
+ * @brief Mailboxes code.
+ * @addtogroup Mailboxes
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_USE_MAILBOXES
+/**
+ * @brief Initializes a Mailbox object.
+ *
+ * @param[out] mbp the pointer to the Mailbox structure to be initialized
+ * @param[in] buf the circular messages buffer
+ * @param[in] n the buffer size as number of @p msg_t
+ */
+void chMBInit(Mailbox *mbp, msg_t *buf, cnt_t n) {
+
+  chDbgCheck((mbp != NULL) && (buf != NULL) && (n > 0), "chMBInit");
+
+  mbp->mb_buffer = mbp->mb_wrptr = mbp->mb_rdptr = buf;
+  mbp->mb_top = &buf[n];
+  chSemInit(&mbp->mb_emptysem, n);
+  chSemInit(&mbp->mb_fullsem, 0);
+}
+
+/**
+ * @brief Resets a Mailbox object.
+ * @details All the waiting threads are resumed with status @p RDY_RESET and
+ *          the queued messages are lost.
+ *
+ * @param[in] mbp the pointer to an initialized Mailbox object
+ */
+void chMBReset(Mailbox *mbp) {
+
+  chDbgCheck(mbp != NULL, "chMBReset");
+
+  chSysLock();
+  mbp->mb_wrptr = mbp->mb_rdptr = mbp->mb_buffer;
+  chSemResetI(&mbp->mb_emptysem, mbp->mb_top - mbp->mb_buffer);
+  chSemResetI(&mbp->mb_fullsem, 0);
+  chSchRescheduleS();
+  chSysUnlock();
+}
+
+/**
+ * @brief Posts a message into a mailbox.
+ * @details The invoking thread waits until a empty slot in the mailbox becomes
+ *          available or the specified time runs out.
+ *
+ * @param[in] mbp the pointer to an initialized Mailbox object
+ * @param[in] msg the message to be posted on the mailbox
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @return The operation status.
+ * @retval RDY_OK if the message was correctly posted.
+ * @retval RDY_RESET if the mailbox was reset while waiting.
+ * @retval RDY_TIMEOUT if the operation timed out.
+ */
+msg_t chMBPost(Mailbox *mbp, msg_t msg, systime_t time) {
+  msg_t rdymsg;
+
+  chSysLock();
+  rdymsg = chMBPostS(mbp, msg, time);
+  chSysUnlock();
+  return rdymsg;
+}
+
+/**
+ * @brief Posts a message into a mailbox.
+ * @details The invoking thread waits until a empty slot in the mailbox becomes
+ *          available or the specified time runs out.
+ *
+ * @param[in] mbp the pointer to an initialized Mailbox object
+ * @param[in] msg the message to be posted on the mailbox
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @return The operation status.
+ * @retval RDY_OK if the message was correctly posted.
+ * @retval RDY_RESET if the mailbox was reset while waiting.
+ * @retval RDY_TIMEOUT if the operation timed out.
+ */
+msg_t chMBPostS(Mailbox *mbp, msg_t msg, systime_t time) {
+  msg_t rdymsg;
+
+  chDbgCheck(mbp != NULL, "chMBPostS");
+
+  rdymsg = chSemWaitTimeoutS(&mbp->mb_emptysem, time);
+  if (rdymsg == RDY_OK) {
+    *mbp->mb_wrptr++ = msg;
+    if (mbp->mb_wrptr >= mbp->mb_top)
+      mbp->mb_wrptr = mbp->mb_buffer;
+    chSemSignalI(&mbp->mb_fullsem);
+    chSchRescheduleS();
+  }
+  return rdymsg;
+}
+
+/**
+ * @brief Posts an high priority message into a mailbox.
+ * @details The invoking thread waits until a empty slot in the mailbox becomes
+ *          available or the specified time runs out.
+ *
+ * @param[in] mbp the pointer to an initialized Mailbox object
+ * @param[in] msg the message to be posted on the mailbox
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @return The operation status.
+ * @retval RDY_OK if the message was correctly posted.
+ * @retval RDY_RESET if the mailbox was reset while waiting.
+ * @retval RDY_TIMEOUT if the operation timed out.
+ */
+msg_t chMBPostAhead(Mailbox *mbp, msg_t msg, systime_t time) {
+  msg_t rdymsg;
+
+  chSysLock();
+  rdymsg = chMBPostAheadS(mbp, msg, time);
+  chSysUnlock();
+  return rdymsg;
+}
+
+/**
+ * @brief Posts an high priority message into a mailbox.
+ * @details The invoking thread waits until a empty slot in the mailbox becomes
+ *          available or the specified time runs out.
+ *
+ * @param[in] mbp the pointer to an initialized Mailbox object
+ * @param[in] msg the message to be posted on the mailbox
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @return The operation status.
+ * @retval RDY_OK if the message was correctly posted.
+ * @retval RDY_RESET if the mailbox was reset while waiting.
+ * @retval RDY_TIMEOUT if the operation timed out.
+ */
+msg_t chMBPostAheadS(Mailbox *mbp, msg_t msg, systime_t time) {
+  msg_t rdymsg;
+
+  chDbgCheck(mbp != NULL, "chMBPostAheadS");
+
+  rdymsg = chSemWaitTimeoutS(&mbp->mb_emptysem, time);
+  if (rdymsg == RDY_OK) {
+    if (--mbp->mb_rdptr < mbp->mb_buffer)
+      mbp->mb_rdptr = mbp->mb_top - 1;
+    *mbp->mb_rdptr = msg;
+    chSemSignalI(&mbp->mb_fullsem);
+    chSchRescheduleS();
+  }
+  return rdymsg;
+}
+
+/**
+ * @brief Retrieves a message from a mailbox.
+ * @details The invoking thread waits until a message is posted in the mailbox
+ *          or the specified time runs out.
+ *
+ * @param[in] mbp the pointer to an initialized Mailbox object
+ * @param[out] msgp pointer to a message variable for the received message
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @return The operation status.
+ * @retval RDY_OK if a message was correctly fetched.
+ * @retval RDY_RESET if the mailbox was reset while waiting.
+ * @retval RDY_TIMEOUT if the operation timed out.
+ */
+msg_t chMBFetch(Mailbox *mbp, msg_t *msgp, systime_t time) {
+  msg_t rdymsg;
+
+  chSysLock();
+  rdymsg = chMBFetchS(mbp, msgp, time);
+  chSysUnlock();
+  return rdymsg;
+}
+
+/**
+ * @brief Retrieves a message from a mailbox.
+ * @details The invoking thread waits until a message is posted in the mailbox
+ *          or the specified time runs out.
+ *
+ * @param[in] mbp the pointer to an initialized Mailbox object
+ * @param[out] msgp pointer to a message variable for the received message
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @return The operation status.
+ * @retval RDY_OK if a message was correctly fetched.
+ * @retval RDY_RESET if the mailbox was reset while waiting.
+ * @retval RDY_TIMEOUT if the operation timed out.
+ */
+msg_t chMBFetchS(Mailbox *mbp, msg_t *msgp, systime_t time) {
+  msg_t rdymsg;
+
+  chDbgCheck((mbp != NULL) && (msgp != NULL), "chMBFetchS");
+
+  rdymsg = chSemWaitTimeoutS(&mbp->mb_fullsem, time);
+  if (rdymsg == RDY_OK) {
+    *msgp = *mbp->mb_rdptr++;
+    if (mbp->mb_rdptr >= mbp->mb_top)
+      mbp->mb_rdptr = mbp->mb_buffer;
+    chSemSignalI(&mbp->mb_emptysem);
+    chSchRescheduleS();
+  }
+  return rdymsg;
+}
+#endif /* CH_USE_MAILBOXES */
+
+/** @} */
diff --git a/os/kernel/src/chmempools.c b/os/kernel/src/chmempools.c
new file mode 100644
index 000000000..dd6f3d1ba
--- /dev/null
+++ b/os/kernel/src/chmempools.c
@@ -0,0 +1,112 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chmempools.c
+ * @brief Memory Pools code.
+ * @addtogroup MemoryPools
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_USE_MEMPOOLS
+/**
+ * @brief Initializes an empty memory pool.
+ *
+ * @param[out] mp pointer to a @p MemoryPool structure
+ * @param[in] size the size of the objects contained in this memory pool,
+ *                 the minimum accepted size is the size of a pointer to void
+ */
+void chPoolInit(MemoryPool *mp, size_t size) {
+
+  chDbgCheck((mp != NULL) && (size >= sizeof(void *)), "chPoolInit");
+
+  mp->mp_next = NULL;
+  mp->mp_object_size = size;
+}
+
+/**
+ * @brief Allocates an object from a memory pool.
+ *
+ * @param[in] mp pointer to a @p MemoryPool structure
+ * @return The pointer to the allocated object.
+ * @retval NULL if pool is empty.
+ */
+void *chPoolAllocI(MemoryPool *mp) {
+  void *objp;
+
+  chDbgCheck(mp != NULL, "chPoolAllocI");
+
+  if ((objp = mp->mp_next) != NULL)
+    mp->mp_next = mp->mp_next->ph_next;
+
+  return objp;
+}
+
+/**
+ * @brief Allocates an object from a memory pool.
+ *
+ * @param[in] mp pointer to a @p MemoryPool structure
+ * @return The pointer to the allocated object.
+ * @retval NULL if pool is empty.
+ */
+void *chPoolAlloc(MemoryPool *mp) {
+  void *objp;
+
+  chSysLock();
+  objp = chPoolAllocI(mp);
+  chSysUnlock();
+  return objp;
+}
+
+/**
+ * @brief Releases (or adds) an object into (to) a memory pool.
+ *
+ * @param[in] mp pointer to a @p MemoryPool structure
+ * @param[in] objp the pointer to the object to be released or added
+ * @note the object is assumed to be of the right size for the specified
+ *       memory pool.
+ */
+void chPoolFreeI(MemoryPool *mp, void *objp) {
+  struct pool_header *php = objp;
+
+  chDbgCheck((mp != NULL) && (objp != NULL), "chPoolFreeI");
+
+  php->ph_next = mp->mp_next;
+  mp->mp_next = php;
+}
+
+/**
+ * @brief Releases (or adds) an object into (to) a memory pool.
+ *
+ * @param[in] mp pointer to a @p MemoryPool structure
+ * @param[in] objp the pointer to the object to be released or added
+ * @note the object is assumed to be of the right size for the specified
+ *       memory pool.
+ */
+void chPoolFree(MemoryPool *mp, void *objp) {
+
+  chSysLock();
+  chPoolFreeI(mp, objp);
+  chSysUnlock();
+}
+#endif /* CH_USE_MEMPOOLS */
+
+/** @} */
diff --git a/os/kernel/src/chmsg.c b/os/kernel/src/chmsg.c
new file mode 100644
index 000000000..393ab8dad
--- /dev/null
+++ b/os/kernel/src/chmsg.c
@@ -0,0 +1,125 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chmsg.c
+ * @brief Messages code.
+ * @addtogroup Messages
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_USE_MESSAGES
+
+#if CH_USE_MESSAGES_PRIORITY
+#define msg_insert(tp, qp) prio_insert(tp, qp)
+#else
+#define msg_insert(tp, qp) queue_insert(tp, qp)
+#endif
+
+/**
+ * @brief Sends a message to the specified thread.
+ * @details The sender is stopped until the receiver executes a
+ *          @p chMsgRelease()after receiving the message.
+ *
+ * @param[in] tp the pointer to the thread
+ * @param[in] msg the message
+ * @return The return message from @p chMsgRelease().
+ */
+msg_t chMsgSend(Thread *tp, msg_t msg) {
+
+  chDbgCheck(tp != NULL, "chMsgSend");
+
+  chSysLock();
+  msg_insert(currp, &tp->p_msgqueue);
+  currp->p_msg = msg;
+  currp->p_wtthdp = tp;
+  if (tp->p_state == PRWTMSG)
+    chSchReadyI(tp);
+  chSchGoSleepS(PRSNDMSG);
+  msg = currp->p_rdymsg;
+  chSysUnlock();
+  return msg;
+}
+
+/**
+ * @brief Suspends the thread and waits for an incoming message.
+ *
+ * @return The pointer to the message structure. Note, it is always the
+ *         message associated to the thread on the top of the messages queue.
+ * @note You can assume that the data contained in the message is stable until
+ *       you invoke @p chMsgRelease() because the sending thread is
+ *       suspended until then.
+ */
+msg_t chMsgWait(void) {
+  msg_t msg;
+
+  chSysLock();
+  if (!chMsgIsPendingI(currp))
+    chSchGoSleepS(PRWTMSG);
+  msg = chMsgGetI(currp);
+  chSysUnlock();
+  return msg;
+}
+
+/**
+ * @brief Returns the next message in the queue.
+ *
+ * @return The pointer to the message structure. Note, it is always the
+ *         message associated to the thread on the top of the messages queue.
+ *         If the queue is empty then @p NULL is returned.
+ * @note You can assume that the data pointed by the message is stable until
+ *       you invoke @p chMsgRelease() because the sending thread is
+ *       suspended until then. Always remember that the message data is not
+ *       copied between the sender and the receiver, just a pointer is passed.
+ */
+msg_t chMsgGet(void) {
+  msg_t msg;
+
+  chSysLock();
+  msg = chMsgIsPendingI(currp) ? chMsgGetI(currp) : (msg_t)NULL;
+  chSysUnlock();
+  return msg;
+}
+
+/**
+ * @brief Releases the thread waiting on top of the messages queue.
+ *
+ * @param[in] msg the message returned to the message sender
+ * @note You can call this function only if there is a message already in the
+ *       queue else the result will be unpredictable (a crash most likely).
+ *       Exiting from the @p chMsgWait() ensures you have at least one
+ *       message in the queue so it is not a big deal.<br>
+ *       The condition is only tested in debug mode in order to make this code
+ *       as fast as possible.
+ */
+void chMsgRelease(msg_t msg) {
+
+  chSysLock();
+  chDbgAssert(chMsgIsPendingI(currp),
+              "chMsgRelease(), #1",
+              "no message pending");
+  chSchWakeupS(fifo_remove(&currp->p_msgqueue), msg);
+  chSysUnlock();
+}
+
+#endif /* CH_USE_MESSAGES */
+
+/** @} */
diff --git a/os/kernel/src/chmtx.c b/os/kernel/src/chmtx.c
new file mode 100644
index 000000000..5fcb6fd5e
--- /dev/null
+++ b/os/kernel/src/chmtx.c
@@ -0,0 +1,299 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chmtx.c
+ * @brief Mutexes code.
+ * @addtogroup Mutexes
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_USE_MUTEXES
+
+/**
+ * @brief Initializes s @p Mutex structure.
+ *
+ * @param[out] mp pointer to a @p Mutex structure
+ * @note This function can be invoked from within an interrupt handler even if
+ *       it is not an I-Class API because it does not touch any critical kernel
+ *       data structure.
+ */
+void chMtxInit(Mutex *mp) {
+
+  chDbgCheck(mp != NULL, "chMtxInit");
+
+  queue_init(&mp->m_queue);
+  mp->m_owner = NULL;
+}
+
+/**
+ * @brief Locks the specified mutex.
+ *
+ * @param[in] mp pointer to the @p Mutex structure
+ */
+void chMtxLock(Mutex *mp) {
+
+  chSysLock();
+
+  chMtxLockS(mp);
+
+  chSysUnlock();
+}
+
+/**
+ * @brief Locks the specified mutex.
+ *
+ * @param[in] mp pointer to the @p Mutex structure
+ * @note This function must be called within a @p chSysLock() / @p chSysUnlock()
+ *       block.
+ */
+void chMtxLockS(Mutex *mp) {
+
+  chDbgCheck(mp != NULL, "chMtxLockS");
+
+  /* the mutex is already locked? */
+  if (mp->m_owner != NULL) {
+    /*
+     * Priority inheritance protocol; explores the thread-mutex dependencies
+     * boosting the priority of all the affected threads to equal the priority
+     * of the running thread requesting the mutex.
+     */
+    Thread *tp = mp->m_owner;
+    /* { tp is the thread currently owning the mutex } */
+    /* the running thread has higher priority than tp? */
+    while (tp->p_prio < currp->p_prio) {
+      /* make priority of thread tp match the running thread's priority */
+      tp->p_prio = currp->p_prio;
+      /*
+       * The following states need priority queues reordering.
+       */
+      switch (tp->p_state) {
+      /* thread tp is waiting on a mutex? */
+      case PRWTMTX:
+        /* Requeues tp with its new priority on the mutex wait queue. */
+        prio_insert(dequeue(tp), &tp->p_wtmtxp->m_queue);
+        /* boost the owner of this mutex if needed */
+        tp = tp->p_wtmtxp->m_owner;
+        continue;
+#if CH_USE_CONDVARS
+      case PRWTCOND:
+        /* Requeues tp with its new priority on the condvar queue. */
+        prio_insert(dequeue(tp), &tp->p_wtcondp->c_queue);
+        break;
+#endif
+#if CH_USE_SEMAPHORES_PRIORITY
+      case PRWTSEM:
+        /* Requeues tp with its new priority on the semaphore queue. */
+        prio_insert(dequeue(tp), &tp->p_wtsemp->s_queue);
+        break;
+#endif
+#if CH_USE_MESSAGES_PRIORITY
+      case PRSNDMSG:
+        /* Requeues tp with its new priority on the server thread queue. */
+        prio_insert(dequeue(tp), &tp->p_wtthdp->p_msgqueue);
+        break;
+#endif
+      /* thread tp is ready? */
+      case PRREADY:
+        /* Requeue tp with its new priority on the ready list. */
+        chSchReadyI(dequeue(tp));
+      }
+      break;
+    }
+    /* sleep on the mutex */
+    prio_insert(currp, &mp->m_queue);
+    /* thread remembers the mutex where it is waiting on */
+    currp->p_wtmtxp = mp;
+    chSchGoSleepS(PRWTMTX);
+    chDbgAssert(mp->m_owner == NULL, "chMtxLockS(), #1", "still owned");
+  }
+  /*
+   * The mutex is now inserted in the owned mutexes list.
+   */
+  mp->m_owner = currp;
+  mp->m_next = currp->p_mtxlist;
+  currp->p_mtxlist = mp;
+}
+
+/**
+ * @brief Tries to lock a mutex.
+ * @details This function does not have any overhead related to
+ *          the priority inheritance mechanism because it does not try to
+ *          enter a sleep state on the mutex.
+ *
+ * @param[in] mp pointer to the @p Mutex structure
+ * @retval TRUE if the mutex was successfully acquired
+ * @retval FALSE if the lock attempt failed.
+ */
+bool_t chMtxTryLock(Mutex *mp) {
+  bool_t b;
+
+  chSysLock();
+
+  b = chMtxTryLockS(mp);
+
+  chSysUnlock();
+  return b;
+}
+
+/**
+ * @brief Tries to lock a mutex.
+ * @details This function does not have any overhead related to
+ *          the priority inheritance mechanism because it does not try to
+ *          enter a sleep state on the mutex.
+ * @param[in] mp pointer to the @p Mutex structure
+ * @retval TRUE if the mutex was successfully acquired
+ * @retval FALSE if the lock attempt failed.
+ * @note This function must be called within a @p chSysLock() / @p chSysUnlock()
+ *       block.
+ */
+bool_t chMtxTryLockS(Mutex *mp) {
+
+  chDbgCheck(mp != NULL, "chMtxTryLockS");
+
+  if (mp->m_owner != NULL)
+    return FALSE;
+  mp->m_owner = currp;
+  mp->m_next = currp->p_mtxlist;
+  currp->p_mtxlist = mp;
+  return TRUE;
+}
+
+/**
+ * @brief Unlocks the next owned mutex in reverse lock order.
+ *
+ * @return The pointer to the unlocked mutex.
+ */
+Mutex *chMtxUnlock(void) {
+  Mutex *ump, *mp;
+
+  chSysLock();
+  chDbgAssert(currp->p_mtxlist != NULL,
+              "chMtxUnlock(), #1",
+              "owned mutexes list empty");
+  chDbgAssert(currp->p_mtxlist->m_owner == currp,
+              "chMtxUnlock(), #2",
+              "ownership failure");
+  /* remove the top Mutex from the Threads's owned mutexes list */
+  ump = currp->p_mtxlist;
+  currp->p_mtxlist = ump->m_next;
+  /* mark the Mutex as not owned */
+  ump->m_owner = NULL;
+  /*
+   * If a thread is waiting on the mutex then the hard part begins.
+   */
+  if (chMtxQueueNotEmptyS(ump)) {
+    /* get the highest priority thread waiting for the unlocked mutex */
+    Thread *tp = fifo_remove(&ump->m_queue);
+    /*
+     * Recalculates the optimal thread priority by scanning the owned mutexes list.
+     */
+    tprio_t newprio = currp->p_realprio;
+    /* iterate mp over all the (other) mutexes the current thread still owns */
+    mp = currp->p_mtxlist;
+    while (mp != NULL) {
+      /* mutex mp has a higher priority thread pending? */
+      if (chMtxQueueNotEmptyS(mp) && (mp->m_queue.p_next->p_prio > newprio))
+        /* boost current thread's priority to waiting thread */
+        newprio = mp->m_queue.p_next->p_prio;
+      mp = mp->m_next;
+    }
+    /* (possibly) boost the priority of the current thread */
+    currp->p_prio = newprio;
+    /* awaken the highest priority thread waiting for the unlocked mutex */
+    chSchWakeupS(tp, RDY_OK);
+  }
+  chSysUnlock();
+  return ump;
+}
+
+/**
+ * @brief Unlocks the next owned mutex in reverse lock order.
+ *
+ * @return The pointer to the unlocked mutex.
+ * @note This function must be called within a @p chSysLock() / @p chSysUnlock()
+ *       block.
+ * @note This function does not reschedule internally.
+ */
+Mutex *chMtxUnlockS(void) {
+  Mutex *ump, *mp;
+
+  chDbgAssert(currp->p_mtxlist != NULL,
+              "chMtxUnlockS(), #1",
+              "owned mutexes list empty");
+  chDbgAssert(currp->p_mtxlist->m_owner == currp,
+              "chMtxUnlockS(), #2",
+              "ownership failure");
+
+  /*
+   * Removes the top Mutex from the owned mutexes list and marks it as not owned.
+   */
+  ump = currp->p_mtxlist;
+  currp->p_mtxlist = ump->m_next;
+  ump->m_owner = NULL;
+  /*
+   * If a thread is waiting on the mutex then the hard part begins.
+   */
+  if (chMtxQueueNotEmptyS(ump)) {
+    Thread *tp = fifo_remove(&ump->m_queue);
+    /*
+     * Recalculates the optimal thread priority by scanning the owned mutexes list.
+     */
+    tprio_t newprio = currp->p_realprio;
+    mp = currp->p_mtxlist;
+    while (mp != NULL) {
+      if (chMtxQueueNotEmptyS(mp) && (mp->m_queue.p_next->p_prio > newprio))
+        newprio = mp->m_queue.p_next->p_prio;
+      mp = mp->m_next;
+    }
+    currp->p_prio = newprio;
+    chSchReadyI(tp);
+  }
+  return ump;
+}
+
+/**
+ * @brief Unlocks all the mutexes owned by the invoking thread.
+ * @details This function is <b>MUCH MORE</b> efficient than releasing the
+ *          mutexes one by one and not just because the call overhead,
+ *          this function does not have any overhead related to the priority
+ *          inheritance mechanism.
+ */
+void chMtxUnlockAll(void) {
+
+  chSysLock();
+  if (currp->p_mtxlist != NULL) {
+    do {
+      Mutex *mp = currp->p_mtxlist;
+      currp->p_mtxlist = mp->m_next;
+      mp->m_owner = NULL;
+      if (chMtxQueueNotEmptyS(mp))
+        chSchReadyI(fifo_remove(&mp->m_queue));
+    } while (currp->p_mtxlist != NULL);
+    currp->p_prio = currp->p_realprio;
+    chSchRescheduleS();
+  }
+  chSysUnlock();
+}
+
+#endif /* CH_USE_MUTEXES */
+
+/** @} */
diff --git a/os/kernel/src/chqueues.c b/os/kernel/src/chqueues.c
new file mode 100644
index 000000000..a72b83696
--- /dev/null
+++ b/os/kernel/src/chqueues.c
@@ -0,0 +1,304 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chqueues.c
+ * @brief I/O Queues code.
+ * @addtogroup IOQueues
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_USE_QUEUES
+
+/**
+ * @brief Initializes an input queue.
+ * @details A Semaphore is internally initialized and works as a counter of
+ *          the bytes contained in the queue.
+ *
+ * @param[out] iqp pointer to an @p InputQueue structure
+ * @param[in] buffer pointer to a memory area allocated as queue buffer
+ * @param[in] size size of the queue buffer
+ * @param[in] inotify pointer to a callback function that is invoked when
+ *                    some data is read from the queue. The value can be
+ *                    @p NULL.
+ *
+ * @note The callback is invoked from within the S-Locked system state,
+ *       see @ref system_states.
+ */
+void chIQInit(InputQueue *iqp, uint8_t *buffer,
+              size_t size, qnotify_t inotify) {
+
+  iqp->q_buffer = iqp->q_rdptr = iqp->q_wrptr = buffer;
+  iqp->q_top = buffer + size;
+  chSemInit(&iqp->q_sem, 0);
+  iqp->q_notify = inotify;
+}
+
+/**
+ * @brief Resets an input queue.
+ * @details All the data in the input queue is erased and lost, any waiting
+ *          thread is resumed with status @p Q_RESET.
+ *
+ * @param[in] iqp pointer to an @p InputQueue structure
+ *
+ * @note A reset operation can be used by a low level driver in order to obtain
+ *       immediate attention from the high level layers.
+ */
+void chIQResetI(InputQueue *iqp) {
+
+  iqp->q_rdptr = iqp->q_wrptr = iqp->q_buffer;
+  chSemResetI(&iqp->q_sem, 0);
+}
+
+/**
+ * @brief Input queue write.
+ * @details A byte value is written into the low end of an input queue.
+ *
+ * @param[in] iqp pointer to an @p InputQueue structure
+ * @param[in] b the byte value to be written in the queue
+ * @return The operation status, it can be one of:
+ * @retval Q_OK if the operation has been completed with success.
+ * @retval Q_FULL if the queue is full and the operation cannot be completed.
+ */
+msg_t chIQPutI(InputQueue *iqp, uint8_t b) {
+
+  if (chIQIsFull(iqp))
+    return Q_FULL;
+
+  *iqp->q_wrptr++ = b;
+  if (iqp->q_wrptr >= iqp->q_top)
+    iqp->q_wrptr = iqp->q_buffer;
+  chSemSignalI(&iqp->q_sem);
+  return Q_OK;
+}
+
+/**
+ * @brief Input queue read with timeout.
+ * @details This function reads a byte value from an input queue. If the queue
+ *          is empty then the calling thread is suspended until a byte arrives
+ *          in the queue or a timeout occurs.
+ *
+ * @param[in] iqp pointer to an @p InputQueue structure
+ * @param[in] timeout the number of ticks before the operation timeouts,
+ *             the following special values are allowed:
+ *             - @a TIME_IMMEDIATE immediate timeout.
+ *             - @a TIME_INFINITE no timeout.
+ *             .
+ * @return A byte value from the queue or:
+ * @retval Q_TIMEOUT if the specified time expired.
+ * @retval Q_RESET if the queue was reset.
+ */
+msg_t chIQGetTimeout(InputQueue *iqp, systime_t timeout) {
+  uint8_t b;
+  msg_t msg;
+
+  chSysLock();
+  if ((msg = chSemWaitTimeoutS(&iqp->q_sem, timeout)) < RDY_OK) {
+    chSysUnlock();
+    return msg;
+  }
+  b = *iqp->q_rdptr++;
+  if (iqp->q_rdptr >= iqp->q_top)
+    iqp->q_rdptr = iqp->q_buffer;
+
+  if (iqp->q_notify)
+    iqp->q_notify();
+
+  chSysUnlock();
+  return b;
+}
+
+/**
+ * @brief Non-blocking read.
+ * @details The function reads data from an input queue into a buffer. The
+ *          transfer is non-blocking and can return zero if the queue is
+ *          empty.
+ *
+ * @param[in] iqp pointer to an @p InputQueue structure
+ * @param[out] buffer pointer to the buffer where the input data is copied
+ * @param[in] n the maximum amount of data to be transferred
+ * @return The number of bytes transferred.
+ *
+ * @note The function is not atomic, if you need atomicity it is suggested
+ *       to use a semaphore or a mutex for mutual exclusion.
+ */
+size_t chIQRead(InputQueue *iqp, uint8_t *buffer, size_t n) {
+  size_t r = 0;
+
+  while (n--) {
+    chSysLock();
+    if (chIQIsEmpty(iqp)) {
+      chSysUnlock();
+      break;
+    }
+    chSemFastWaitI(&iqp->q_sem);
+    *buffer++ = *iqp->q_rdptr++;
+    if (iqp->q_rdptr >= iqp->q_top)
+      iqp->q_rdptr = iqp->q_buffer;
+    chSysUnlock();
+    r++;
+  }
+  if (r && iqp->q_notify) {
+    chSysLock();
+    iqp->q_notify();
+    chSysUnlock();
+  }
+  return r;
+}
+
+/**
+ * @brief Initializes an output queue.
+ * @details A Semaphore is internally initialized and works as a counter of
+ *          the free bytes in the queue.
+ *
+ * @param[out] oqp pointer to an @p OutputQueue structure
+ * @param[in] buffer pointer to a memory area allocated as queue buffer
+ * @param[in] size size of the queue buffer
+ * @param[in] onotify pointer to a callback function that is invoked when
+ *                    some data is written to the queue. The value can be
+ *                    @p NULL.
+ *
+ * @note The callback is invoked from within the S-Locked system state,
+ *       see @ref system_states.
+ */
+void chOQInit(OutputQueue *oqp, uint8_t *buffer,
+              size_t size, qnotify_t onotify) {
+
+  oqp->q_buffer = oqp->q_rdptr = oqp->q_wrptr = buffer;
+  oqp->q_top = buffer + size;
+  chSemInit(&oqp->q_sem, size);
+  oqp->q_notify = onotify;
+}
+
+/**
+ * @brief Resets an output queue.
+ * @details All the data in the output queue is erased and lost, any waiting
+ *          thread is resumed with status @p Q_RESET.
+ *
+ * @param[in] oqp pointer to an @p OutputQueue structure
+ *
+ * @note A reset operation can be used by a low level driver in order to obtain
+ *       immediate attention from the high level layers.
+ */
+void chOQResetI(OutputQueue *oqp) {
+
+  oqp->q_rdptr = oqp->q_wrptr = oqp->q_buffer;
+  chSemResetI(&oqp->q_sem, (cnt_t)(oqp->q_top - oqp->q_buffer));
+}
+
+/**
+ * @brief Output queue write with timeout.
+ * @details This function writes a byte value to an output queue. If the queue
+ *          is full then the calling thread is suspended until there is space
+ *          in the queue or a timeout occurs.
+ *
+ * @param[in] oqp pointer to an @p OutputQueue structure
+ * @param[in] b the byte value to be written in the queue
+ * @param[in] timeout the number of ticks before the operation timeouts,
+ *             the following special values are allowed:
+ *             - @a TIME_IMMEDIATE immediate timeout.
+ *             - @a TIME_INFINITE no timeout.
+ *             .
+ * @return The operation status:
+ * @retval Q_OK if the operation succeeded.
+ * @retval Q_TIMEOUT if the specified time expired.
+ * @retval Q_RESET if the queue was reset.
+ */
+msg_t chOQPutTimeout(OutputQueue *oqp, uint8_t b, systime_t timeout) {
+  msg_t msg;
+
+  chSysLock();
+  if ((msg = chSemWaitTimeoutS(&oqp->q_sem, timeout)) < RDY_OK) {
+    chSysUnlock();
+    return msg;
+  }
+  *oqp->q_wrptr++ = b;
+  if (oqp->q_wrptr >= oqp->q_top)
+    oqp->q_wrptr = oqp->q_buffer;
+
+  if (oqp->q_notify)
+    oqp->q_notify();
+
+  chSysUnlock();
+  return Q_OK;
+}
+
+/**
+ * @brief Output queue read.
+ * @details A byte value is read from the low end of an output queue.
+ *
+ * @param[in] oqp pointer to an @p OutputQueue structure
+ * @return The byte value from the queue or:
+ * @retval Q_EMPTY if the queue is empty.
+ */
+msg_t chOQGetI(OutputQueue *oqp) {
+  uint8_t b;
+
+  if (chOQIsEmpty(oqp))
+    return Q_EMPTY;
+
+  b = *oqp->q_rdptr++;
+  if (oqp->q_rdptr >= oqp->q_top)
+    oqp->q_rdptr = oqp->q_buffer;
+  chSemSignalI(&oqp->q_sem);
+  return b;
+}
+
+/**
+ * @brief Non-blocking write.
+ * @details The function writes data from a buffer to an output queue. The
+ *          transfer is non-blocking and can return zero if the queue is
+ *          already full.
+ *
+ * @param[in] oqp pointer to an @p OutputQueue structure
+ * @param[out] buffer pointer to the buffer where the output data is stored
+ * @param[in] n the maximum amount of data to be transferred
+ * @return The number of bytes transferred.
+ *
+ * @note The function is not atomic, if you need atomicity it is suggested
+ *       to use a semaphore or a mutex for mutual exclusion.
+ */
+size_t chOQWrite(OutputQueue *oqp, uint8_t *buffer, size_t n) {
+
+  size_t w = 0;
+  while (n--) {
+    chSysLock();
+    if (chOQIsFull(oqp)) {
+      chSysUnlock();
+      break;
+    }
+    chSemFastWaitI(&oqp->q_sem);
+    *oqp->q_wrptr++ = *buffer++;
+    if (oqp->q_wrptr >= oqp->q_top)
+      oqp->q_wrptr = oqp->q_buffer;
+    chSysUnlock();
+    w++;
+  }
+  if (w && oqp->q_notify) {
+    chSysLock();
+    oqp->q_notify();
+    chSysUnlock();
+  }
+  return w;
+}
+#endif  /* CH_USE_QUEUES */
+
+/** @} */
diff --git a/os/kernel/src/chschd.c b/os/kernel/src/chschd.c
new file mode 100644
index 000000000..3ba8b29e9
--- /dev/null
+++ b/os/kernel/src/chschd.c
@@ -0,0 +1,242 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chschd.c
+ * @brief Scheduler code.
+ * @addtogroup Scheduler
+ * @{
+ */
+
+#include <ch.h>
+
+/** @cond never */
+ReadyList rlist;
+/** @endcond */
+
+/**
+ * @brief Scheduler initialization.
+ *
+ * @note Internally invoked by the @p chSysInit().
+ */
+void scheduler_init(void) {
+
+  queue_init(&rlist);
+  rlist.r_prio = NOPRIO;
+#if CH_USE_ROUNDROBIN
+  rlist.r_preempt = CH_TIME_QUANTUM;
+#endif
+}
+
+/**
+ * @brief Inserts a thread in the Ready List.
+ *
+ * @param[in] tp the Thread to be made ready
+ * @return The Thread pointer.
+ * @note The function does not reschedule, the @p chSchRescheduleS() should
+ *       be called soon after.
+ */
+#if CH_OPTIMIZE_SPEED
+/* NOTE: it is inlined in this module only.*/
+INLINE Thread *chSchReadyI(Thread *tp) {
+#else
+Thread *chSchReadyI(Thread *tp) {
+#endif
+  Thread *cp;
+
+  tp->p_state = PRREADY;
+  cp = (Thread *)&rlist;
+  do {
+    cp = cp->p_next;
+  } while (cp->p_prio >= tp->p_prio);
+  /* Insertion on p_prev.*/
+  tp->p_prev = (tp->p_next = cp)->p_prev;
+  tp->p_prev->p_next = cp->p_prev = tp;
+  return tp;
+}
+
+/**
+ * @brief Puts the current thread to sleep into the specified state.
+ * @details The thread goes into a sleeping state. The @ref thread_states are
+ *          described into @p threads.h.
+ *
+ * @param[in] newstate the new thread state
+ */
+void chSchGoSleepS(tstate_t newstate) {
+  Thread *otp;
+
+  (otp = currp)->p_state = newstate;
+  (currp = fifo_remove((void *)&rlist))->p_state = PRCURR;
+#if CH_USE_ROUNDROBIN
+  rlist.r_preempt = CH_TIME_QUANTUM;
+#endif
+  chDbgTrace(otp, currp);
+  chSysSwitchI(otp, currp);
+}
+
+/*
+ * Timeout wakeup callback.
+ */
+static void wakeup(void *p) {
+  Thread *tp = (Thread *)p;
+
+#if CH_USE_SEMAPHORES || CH_USE_MUTEXES || CH_USE_CONDVARS
+  switch (tp->p_state) {
+#if CH_USE_SEMAPHORES
+  case PRWTSEM:
+    chSemFastSignalI(tp->p_wtsemp);
+    /* Falls into, intentional. */
+#endif
+#if CH_USE_MUTEXES
+  case PRWTMTX:
+#endif
+#if CH_USE_CONDVARS
+  case PRWTCOND:
+#endif
+    /* States requiring dequeuing. */
+    dequeue(tp);
+  }
+#endif
+  chSchReadyI(tp)->p_rdymsg = RDY_TIMEOUT;
+}
+
+/**
+ * @brief Puts the current thread to sleep into the specified state with
+ *        timeout specification.
+ * @details The thread goes into a sleeping state, if it is not awakened
+ *          explicitly within the specified timeout then it is forcibly
+ *          awakened with a @p RDY_TIMEOUT low level message. The @ref
+ *          thread_states are described into @p threads.h.
+ *
+ * @param[in] newstate the new thread state
+ * @param[in] time the number of ticks before the operation timeouts, the
+ *                 special values are handled as follow:
+ *                 - @a TIME_INFINITE the thread enters an infinite sleep
+ *                   state, this is equivalent to invoking @p chSchGoSleepS()
+ *                   but, of course, less efficient.
+ *                 - @a TIME_IMMEDIATE this value is accepted but interpreted
+ *                   as a normal time specification not as an immediate timeout
+ *                   specification.
+ *                 .
+ * @return The wakeup message.
+ * @retval RDY_TIMEOUT if a timeout occurs.
+ */
+msg_t chSchGoSleepTimeoutS(tstate_t newstate, systime_t time) {
+
+  if (TIME_INFINITE != time) {
+    VirtualTimer vt;
+
+    chVTSetI(&vt, time, wakeup, currp);
+    chSchGoSleepS(newstate);
+    if (chVTIsArmedI(&vt))
+      chVTResetI(&vt);
+  }
+  else
+    chSchGoSleepS(newstate);
+  return currp->p_rdymsg;
+}
+
+/**
+ * @brief Wakes up a thread.
+ * @details The thread is inserted into the ready list or immediately made
+ *          running depending on its relative priority compared to the current
+ *          thread.
+ *
+ * @param[in] ntp the Thread to be made ready
+ * @param[in] msg message to the awakened thread
+ * @note It is equivalent to a @p chSchReadyI() followed by a
+ *       @p chSchRescheduleS() but much more efficient.
+ */
+void chSchWakeupS(Thread *ntp, msg_t msg) {
+
+  ntp->p_rdymsg = msg;
+  /* If the waken thread has a not-greater priority than the current
+   * one then it is just inserted in the ready list else it made
+   * running immediately and the invoking thread goes in the ready
+   * list instead.*/
+  if (ntp->p_prio <= currp->p_prio)
+    chSchReadyI(ntp);
+  else {
+    Thread *otp = currp;
+    chSchReadyI(otp);
+    (currp = ntp)->p_state = PRCURR;
+#if CH_USE_ROUNDROBIN
+    rlist.r_preempt = CH_TIME_QUANTUM;
+#endif
+    chDbgTrace(otp, ntp);
+    chSysSwitchI(otp, ntp);
+  }
+}
+
+/**
+ * @brief Switches to the first thread on the runnable queue.
+ *
+ * @note It is intended to be called if @p chSchRescRequiredI() evaluates to
+ *       @p TRUE.
+ */
+void chSchDoRescheduleI(void) {
+
+  Thread *otp = currp;
+  /* pick the first thread from the ready queue and makes it current */
+  (currp = fifo_remove((void *)&rlist))->p_state = PRCURR;
+  chSchReadyI(otp);
+#if CH_USE_ROUNDROBIN
+  rlist.r_preempt = CH_TIME_QUANTUM;
+#endif
+  chDbgTrace(otp, currp);
+  chSysSwitchI(otp, currp);
+}
+
+/**
+ * @brief Performs a reschedulation if a higher priority thread is runnable.
+ * @details If a thread with a higher priority than the current thread is in
+ *          the ready list then make the higher priority thread running.
+ */
+void chSchRescheduleS(void) {
+  /* first thread in the runnable queue has higher priority than the running
+   * thread? */
+  if (firstprio(&rlist) > currp->p_prio)
+    chSchDoRescheduleI();
+}
+
+/**
+ * @brief Evaluates if a reschedulation is required.
+ * @details The decision is taken by comparing the relative priorities and
+ *          depending on the state of the round robin timeout counter.
+ *
+ * @retval TRUE if there is a thread that should go in running state.
+ * @retval FALSE if a reschedulation is not required.
+ */
+bool_t chSchRescRequiredI(void) {
+  tprio_t p1 = firstprio(&rlist);
+  tprio_t p2 = currp->p_prio;
+#if CH_USE_ROUNDROBIN
+  /* 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 rlist.r_preempt ? p1 > p2 : p1 >= p2;
+#else
+  /* If the round robin feature is not enabled then performs a simpler
+   * comparison.*/
+  return p1 > p2;
+#endif
+}
+
+/** @} */
diff --git a/os/kernel/src/chsem.c b/os/kernel/src/chsem.c
new file mode 100644
index 000000000..9a8570580
--- /dev/null
+++ b/os/kernel/src/chsem.c
@@ -0,0 +1,257 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chsem.c
+ * @brief Semaphores code.
+ * @addtogroup Semaphores
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_USE_SEMAPHORES
+
+#if CH_USE_SEMAPHORES_PRIORITY
+#define sem_insert(tp, qp) prio_insert(tp, qp)
+#else
+#define sem_insert(tp, qp) queue_insert(tp, qp)
+#endif
+
+/**
+ * @brief Initializes a semaphore with the specified counter value.
+ *
+ * @param[out] sp pointer to a @p Semaphore structure
+ * @param[in] n initial value of the semaphore counter. Must be non-negative.
+ * @note This function can be invoked from within an interrupt handler even if
+ *       it is not an I-Class API because it does not touch any critical kernel
+ *       data structure.
+ */
+void chSemInit(Semaphore *sp, cnt_t n) {
+
+  chDbgCheck((sp != NULL) && (n >= 0), "chSemInit");
+
+  queue_init(&sp->s_queue);
+  sp->s_cnt = n;
+}
+
+/**
+ * @brief Performs a reset operation on the semaphore.
+ *
+ * @param[in] sp pointer to a @p Semaphore structure
+ * @param[in] n the new value of the semaphore counter. The value must be non-negative.
+ * @note The released threads can recognize they were waked up by a reset
+ *       instead than a signal because the @p chSemWait() will return
+ *       @p RDY_RESET instead of @p RDY_OK.
+ */
+void chSemReset(Semaphore *sp, cnt_t n) {
+
+  chSysLock();
+  chSemResetI(sp, n);
+  chSchRescheduleS();
+  chSysUnlock();
+}
+
+/**
+ * @brief Performs a reset operation on the semaphore.
+ *
+ * @param[in] sp pointer to a @p Semaphore structure
+ * @param[in] n the new value of the semaphore counter. The value must be non-negative.
+ * @note The released threads can recognize they were waked up by a reset
+ *       instead than a signal because the @p chSemWait() will return
+ *       @p RDY_RESET instead of @p RDY_OK.
+ * @note This function does not reschedule.
+ */
+void chSemResetI(Semaphore *sp, cnt_t n) {
+  cnt_t cnt;
+
+  chDbgCheck((sp != NULL) && (n >= 0), "chSemResetI");
+
+  cnt = sp->s_cnt;
+  sp->s_cnt = n;
+  while (cnt++ < 0)
+    chSchReadyI(lifo_remove(&sp->s_queue))->p_rdymsg = RDY_RESET;
+}
+
+/**
+ * @brief Performs a wait operation on a semaphore.
+ *
+ * @param[in] sp pointer to a @p Semaphore structure
+ * @retval RDY_OK if the semaphore was signaled or not taken.
+ * @retval RDY_RESET if the semaphore was reset using @p chSemReset().
+ */
+msg_t chSemWait(Semaphore *sp) {
+  msg_t msg;
+
+  chSysLock();
+  msg = chSemWaitS(sp);
+  chSysUnlock();
+  return msg;
+}
+
+/**
+ * @brief Performs a wait operation on a semaphore.
+ *
+ * @param[in] sp pointer to a @p Semaphore structure
+ * @retval RDY_OK if the semaphore was signaled or not taken.
+ * @retval RDY_RESET if the semaphore was reset using @p chSemReset().
+ * @note This function must be called with interrupts disabled.
+ * @note This function cannot be called by an interrupt handler.
+ */
+msg_t chSemWaitS(Semaphore *sp) {
+
+  chDbgCheck(sp != NULL, "chSemWaitS");
+
+  if (--sp->s_cnt < 0) {
+    sem_insert(currp, &sp->s_queue);
+    currp->p_wtsemp = sp;
+    chSchGoSleepS(PRWTSEM);
+    return currp->p_rdymsg;
+  }
+  return RDY_OK;
+}
+
+/**
+ * @brief Performs a wait operation on a semaphore with timeout specification.
+ *
+ * @param[in] sp pointer to a @p Semaphore structure
+ * @param[in] time the number of ticks before the operation timeouts,
+ *             the following special values are allowed:
+ *             - @a TIME_IMMEDIATE immediate timeout.
+ *             - @a TIME_INFINITE no timeout.
+ *             .
+ * @retval RDY_OK if the semaphore was signaled or not taken.
+ * @retval RDY_RESET if the semaphore was reset using @p chSemReset().
+ * @retval RDY_TIMEOUT if the semaphore was not signaled or reset within the
+ *         specified timeout.
+ */
+msg_t chSemWaitTimeout(Semaphore *sp, systime_t time) {
+  msg_t msg;
+
+  chSysLock();
+  msg = chSemWaitTimeoutS(sp, time);
+  chSysUnlock();
+  return msg;
+}
+
+/**
+ * @brief Performs a wait operation on a semaphore with timeout specification.
+ *
+ * @param[in] sp pointer to a @p Semaphore structure
+ * @param[in] time the number of ticks before the operation timeouts,
+ *                 the following special values are allowed:
+ *                 - @a TIME_IMMEDIATE immediate timeout.
+ *                 - @a TIME_INFINITE no timeout.
+ *                 .
+ * @retval RDY_OK if the semaphore was signaled or not taken.
+ * @retval RDY_RESET if the semaphore was reset using @p chSemReset().
+ * @retval RDY_TIMEOUT if the semaphore was not signaled or reset within the specified
+ *         timeout.
+ */
+msg_t chSemWaitTimeoutS(Semaphore *sp, systime_t time) {
+
+  chDbgCheck(sp != NULL, "chSemWaitTimeoutS");
+
+  if (--sp->s_cnt < 0) {
+    if (TIME_IMMEDIATE == time) {
+      sp->s_cnt++;
+      return RDY_TIMEOUT;
+    }
+    sem_insert(currp, &sp->s_queue);
+    currp->p_wtsemp = sp;
+    return chSchGoSleepTimeoutS(PRWTSEM, time);
+  }
+  return RDY_OK;
+}
+
+/**
+ * @brief Performs a signal operation on a semaphore.
+ *
+ * @param[in] sp pointer to a @p Semaphore structure
+ * @note The function is available only if the @p CH_USE_SEMAPHORES
+ *       option is enabled in @p chconf.h.
+ */
+void chSemSignal(Semaphore *sp) {
+
+  chDbgCheck(sp != NULL, "chSemSignal");
+
+  chSysLock();
+  if (sp->s_cnt++ < 0)
+    chSchWakeupS(fifo_remove(&sp->s_queue), RDY_OK);
+  chSysUnlock();
+}
+
+/**
+ * @brief Performs a signal operation on a semaphore.
+ *
+ * @param[in] sp pointer to a @p Semaphore structure
+ * @note The function is available only if the @p CH_USE_SEMAPHORES
+ *       option is enabled in @p chconf.h.
+ * @note This function does not reschedule.
+ */
+void chSemSignalI(Semaphore *sp) {
+
+  chDbgCheck(sp != NULL, "chSemSignalI");
+
+  if (sp->s_cnt++ < 0) {
+    /* NOTE: It is done this way in order to allow a tail call on
+             chSchReadyI().*/
+    Thread *tp = fifo_remove(&sp->s_queue);
+    tp->p_rdymsg = RDY_OK;
+    chSchReadyI(tp);
+  }
+}
+
+#if CH_USE_SEMSW
+/**
+ * @brief Performs atomic signal and wait operations on two semaphores.
+ *
+ * @param[in] sps pointer to a @p Semaphore structure to be signaled
+ * @param[in] spw pointer to a @p Semaphore structure to be wait on
+ * @retval RDY_OK if the semaphore was signaled or not taken.
+ * @retval RDY_RESET if the semaphore was reset using @p chSemReset().
+ * @note The function is available only if the @p CH_USE_SEMSW
+ *       option is enabled in @p chconf.h.
+ */
+msg_t chSemSignalWait(Semaphore *sps, Semaphore *spw) {
+  msg_t msg;
+
+  chDbgCheck((sps != NULL) && (spw != NULL), "chSemSignalWait");
+
+  chSysLock();
+  if (sps->s_cnt++ < 0)
+    chSchReadyI(fifo_remove(&sps->s_queue))->p_rdymsg = RDY_OK;
+  if (--spw->s_cnt < 0) {
+    sem_insert(currp, &spw->s_queue);
+    currp->p_wtsemp = spw;
+    chSchGoSleepS(PRWTSEM);
+    msg = currp->p_rdymsg;
+  }
+  else {
+    chSchRescheduleS();
+    msg = RDY_OK;
+  }
+  chSysUnlock();
+  return msg;
+}
+#endif /* CH_USE_SEMSW */
+
+#endif /* CH_USE_SEMAPHORES */
+
+/** @} */
diff --git a/os/kernel/src/chserial.c b/os/kernel/src/chserial.c
new file mode 100644
index 000000000..65179689d
--- /dev/null
+++ b/os/kernel/src/chserial.c
@@ -0,0 +1,168 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chserial.c
+ * @brief Serial Drivers code.
+ * @addtogroup Serial
+ * @{
+ */
+
+#include <ch.h>
+
+#if CH_USE_SERIAL_FULLDUPLEX
+
+/*
+ * Interface implementation, the following functions just invoke the equivalent
+ * queue-level function or macro.
+ */
+static bool_t putwouldblock(void *instance) {
+
+  return chOQIsFull(&((FullDuplexDriver *)instance)->d2.oqueue);
+}
+
+static bool_t getwouldblock(void *instance) {
+
+  return chIQIsEmpty(&((FullDuplexDriver *)instance)->d2.iqueue);
+}
+
+static msg_t put(void *instance, uint8_t b, systime_t timeout) {
+
+  return chOQPutTimeout(&((FullDuplexDriver *)instance)->d2.oqueue, b, timeout);
+}
+
+static msg_t get(void *instance, systime_t timeout) {
+
+  return chIQGetTimeout(&((FullDuplexDriver *)instance)->d2.iqueue, timeout);
+}
+
+static size_t write(void *instance, uint8_t *buffer, size_t n) {
+
+  return chOQWrite(&((FullDuplexDriver *)instance)->d2.oqueue, buffer, n);
+}
+
+static size_t read(void *instance, uint8_t *buffer, size_t n) {
+
+  return chIQRead(&((FullDuplexDriver *)instance)->d2.iqueue, buffer, n);
+}
+
+static const struct FullDuplexDriverVMT vmt = {
+  {putwouldblock, getwouldblock, put, get},
+  {write, read},
+  {}
+};
+
+/**
+ * @brief Initializes a generic full duplex driver.
+ * @details The HW dependent part of the initialization has to be performed
+ *          outside, usually in the hardware initialization code.
+ *
+ * @param[out] sd pointer to a @p FullDuplexDriver structure
+ * @param[in] ib pointer to a memory area allocated for the Input Queue buffer
+ * @param[in] isize size of the Input Queue buffer
+ * @param[in] inotify pointer to a callback function that is invoked when
+ *                    some data is read from the Queue. The value can be
+ *                    @p NULL.
+ * @param[in] ob pointer to a memory area allocated for the Output Queue buffer
+ * @param[in] osize size of the Output Queue buffer
+ * @param[in] onotify pointer to a callback function that is invoked when
+ *                    some data is written in the Queue. The value can be
+ *                    @p NULL.
+ */
+void chFDDInit(FullDuplexDriver *sd,
+               uint8_t *ib, size_t isize, qnotify_t inotify,
+               uint8_t *ob, size_t osize, qnotify_t onotify) {
+
+  chDbgCheck((sd != NULL) && (ib != NULL) && (ob != NULL) &&
+             (isize > 0) && (osize > 0), "chFDDInit");
+
+  sd->vmt = &vmt;
+  chEvtInit(&sd->d1.ievent);
+  chEvtInit(&sd->d1.oevent);
+  chEvtInit(&sd->d2.sevent);
+  sd->d2.flags = SD_NO_ERROR;
+  chIQInit(&sd->d2.iqueue, ib, isize, inotify);
+  chOQInit(&sd->d2.oqueue, ob, osize, onotify);
+}
+
+/**
+ * @brief Handles incoming data.
+ * @details This function must be called from the input interrupt service
+ *          routine in order to enqueue incoming data and generate the
+ *          related events.
+ * @param[in] sd pointer to a @p FullDuplexDriver structure
+ * @param[in] b the byte to be written in the driver's Input Queue
+ */
+void chFDDIncomingDataI(FullDuplexDriver *sd, uint8_t b) {
+
+  if (chIQPutI(&sd->d2.iqueue, b) < Q_OK)
+    chFDDAddFlagsI(sd, SD_OVERRUN_ERROR);
+  else
+    chEvtBroadcastI(&sd->d1.ievent);
+}
+
+/**
+ * @brief Handles outgoing data.
+ * @details Must be called from the output interrupt service routine in order
+ *          to get the next byte to be transmitted.
+ *
+ * @param[in] sd pointer to a @p FullDuplexDriver structure
+ * @return The byte value read from the driver's output queue.
+ * @retval Q_EMPTY if the queue is empty (the lower driver usually disables
+ *                 the interrupt source when this happens).
+ */
+msg_t chFDDRequestDataI(FullDuplexDriver *sd) {
+
+  msg_t b = chOQGetI(&sd->d2.oqueue);
+  if (b < Q_OK)
+    chEvtBroadcastI(&sd->d1.oevent);
+  return b;
+}
+
+/**
+ * @brief Handles communication events/errors.
+ * @details Must be called from the I/O interrupt service routine in order to
+ *          notify I/O conditions as errors, signals change etc.
+ *
+ * @param[in] sd pointer to a @p FullDuplexDriver structure
+ * @param[in] mask condition flags to be added to the mask
+ */
+void chFDDAddFlagsI(FullDuplexDriver *sd, dflags_t mask) {
+
+  sd->d2.flags |= mask;
+  chEvtBroadcastI(&sd->d2.sevent);
+}
+
+/**
+ * @brief Returns and clears the errors mask associated to the driver.
+ *
+ * @param[in] sd pointer to a @p FullDuplexDriver structure
+ * @return The condition flags modified since last time this function was
+ *         invoked.
+ */
+dflags_t chFDDGetAndClearFlags(FullDuplexDriver *sd) {
+  dflags_t mask;
+
+  mask = sd->d2.flags;
+  sd->d2.flags = SD_NO_ERROR;
+  return mask;
+}
+#endif /* CH_USE_SERIAL_FULLDUPLEX */
+
+/** @} */
diff --git a/os/kernel/src/chsys.c b/os/kernel/src/chsys.c
new file mode 100644
index 000000000..217e2f2da
--- /dev/null
+++ b/os/kernel/src/chsys.c
@@ -0,0 +1,129 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chsys.c
+ * @brief System related code.
+ * @addtogroup System
+ * @{
+ */
+
+#include <ch.h>
+
+static WORKING_AREA(idle_thread_wa, IDLE_THREAD_STACK_SIZE);
+
+/**
+ * @brief This function implements the idle thread infinite loop.
+ * @details The function puts the processor in the lowest power mode capable
+ *          to serve interrupts.<br>
+ *          The priority is internally set to the minimum system value so
+ *          that this thread is executed only if there are no other ready
+ *          threads in the system.
+ *
+ * @param[in] p the thread parameter, unused in this scenario
+ */
+static void idle_thread(void *p) {
+
+  while (TRUE) {
+    port_wait_for_interrupt();
+    IDLE_LOOP_HOOK();
+  }
+}
+
+/**
+ * @brief ChibiOS/RT initialization.
+ * @details After executing this function the current instructions stream
+ *          becomes the main thread.
+ *
+ * @note Interrupts should be still disabled when @p chSysInit() is invoked
+ *       and are internally enabled.
+ * @note The main thread is created with priority @p NORMALPRIO.
+ */
+void chSysInit(void) {
+  static Thread mainthread;
+
+  port_init();
+  scheduler_init();
+  vt_init();
+#if CH_USE_HEAP
+  heap_init();
+#endif
+#if CH_DBG_ENABLE_TRACE
+  trace_init();
+#endif
+
+  /*
+   * Now this instructions flow becomes the main thread.
+   */
+  (currp = init_thread(&mainthread, NORMALPRIO))->p_state = PRCURR;
+  chSysEnable();
+
+  /*
+   * This thread has the lowest priority in the system, its role is just to
+   * serve interrupts in its context while keeping the lowest energy saving
+   * mode compatible with the system status.
+   */
+  chThdCreateStatic(idle_thread_wa, sizeof(idle_thread_wa), IDLEPRIO,
+                    (tfunc_t)idle_thread, NULL);
+}
+
+/**
+ * @brief Handles time ticks for round robin preemption and timer increments.
+ * @details Decrements the remaining time quantum of the running thread
+ *          and preempts it when the quantum is used up. Increments system
+ *          time and manages the timers.
+ *
+ * @note The frequency of the timer determines the system tick granularity and,
+ *       together with the @p CH_TIME_QUANTUM macro, the round robin interval.
+ */
+void chSysTimerHandlerI(void) {
+
+#if CH_USE_ROUNDROBIN
+  /* running thread has not used up quantum yet? */
+  if (rlist.r_preempt > 0)
+    /* decrement remaining quantum */
+    rlist.r_preempt--;
+#endif
+#if CH_DBG_THREADS_PROFILING
+  currp->p_time++;
+#endif
+  chVTDoTickI();
+}
+
+#if CH_USE_NESTED_LOCKS && !CH_OPTIMIZE_SPEED
+void chSysLock(void) {
+
+  chDbgAssert(currp->p_locks >= 0,
+              "chSysLock(), #1",
+              "negative nesting counter");
+  if (currp->p_locks++ == 0)
+    port_lock();
+}
+
+void chSysUnlock(void) {
+
+  chDbgAssert(currp->p_locks > 0,
+              "chSysUnlock(), #1",
+              "non-positive nesting counter");
+  if (--currp->p_locks == 0)
+    port_unlock();
+}
+#endif /* CH_USE_NESTED_LOCKS && !CH_OPTIMIZE_SPEED */
+
+/** @} */
diff --git a/os/kernel/src/chthreads.c b/os/kernel/src/chthreads.c
new file mode 100644
index 000000000..f8bb3b869
--- /dev/null
+++ b/os/kernel/src/chthreads.c
@@ -0,0 +1,381 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chthreads.c
+ * @brief Threads code.
+ * @addtogroup Threads
+ * @{
+ */
+
+#include <ch.h>
+
+/*
+ * Initializes a thread structure.
+ */
+Thread *init_thread(Thread *tp, tprio_t prio) {
+
+  tp->p_flags = P_MEM_MODE_STATIC;
+  tp->p_prio = prio;
+  tp->p_state = PRSUSPENDED;
+#if CH_USE_NESTED_LOCKS
+  tp->p_locks = 0;
+#endif
+#if CH_DBG_THREADS_PROFILING
+  tp->p_time = 0;
+#endif
+#if CH_USE_MUTEXES
+  /* realprio is the thread's own, non-inherited, priority */
+  tp->p_realprio = prio;
+  tp->p_mtxlist = NULL;
+#endif
+#if CH_USE_WAITEXIT
+  tp->p_waiting = NULL;
+#endif
+#if CH_USE_MESSAGES
+  queue_init(&tp->p_msgqueue);
+#endif
+#if CH_USE_EVENTS
+  tp->p_epending = 0;
+#endif
+  THREAD_EXT_INIT(tp);
+  return tp;
+}
+
+#if CH_DBG_FILL_THREADS
+static void memfill(uint8_t *startp, uint8_t *endp, uint8_t v) {
+
+  while (startp < endp)
+    *startp++ = v;
+}
+#endif
+
+/**
+ * @brief Initializes a new thread.
+ * @details The new thread is initialized but not inserted in the ready list,
+ *          the initial state is @p PRSUSPENDED.
+ *
+ * @param[out] wsp pointer to a working area dedicated to the thread stack
+ * @param[in] size size of the working area
+ * @param[in] prio the priority level for the new thread
+ * @param[in] pf the thread function
+ * @param[in] arg an argument passed to the thread function. It can be @p NULL.
+ * @return The pointer to the @p Thread structure allocated for the
+ *         thread into the working space area.
+ * @note A thread can terminate by calling @p chThdExit() or by simply
+ *       returning from its main function.
+ * @note This function can be invoked from within an interrupt handler even if
+ *       it is not an I-Class API because it does not touch any critical kernel
+ *       data structure.
+ */
+Thread *chThdInit(void *wsp, size_t size, tprio_t prio, tfunc_t pf, void *arg) {
+  /* Thread structure is layed out in the lower part of the thread workspace */
+  Thread *tp = wsp;
+
+  chDbgCheck((wsp != NULL) && (size >= THD_WA_SIZE(0)) &&
+             (prio <= HIGHPRIO) && (pf != NULL),
+             "chThdInit");
+#if CH_DBG_FILL_THREADS
+  memfill((uint8_t *)wsp, (uint8_t *)wsp + sizeof(Thread), THREAD_FILL_VALUE);
+  memfill((uint8_t *)wsp + sizeof(Thread),
+          (uint8_t *)wsp + size, STACK_FILL_VALUE);
+#endif
+  SETUP_CONTEXT(wsp, size, pf, arg);
+  return init_thread(tp, prio);
+}
+
+/**
+ * @brief Creates a new thread into a static memory area.
+ *
+ * @param[out] wsp pointer to a working area dedicated to the thread
+ *                       stack
+ * @param[in] size size of the working area
+ * @param[in] prio the priority level for the new thread
+ * @param[in] pf the thread function
+ * @param[in] arg an argument passed to the thread function. It can be @p NULL.
+ * @return The pointer to the @p Thread structure allocated for the
+ *         thread into the working space area.
+ * @note A thread can terminate by calling @p chThdExit() or by simply
+ *       returning from its main function.
+ */
+Thread *chThdCreateStatic(void *wsp, size_t size,
+                          tprio_t prio, tfunc_t pf, void *arg) {
+
+  return chThdResume(chThdInit(wsp, size, prio, pf, arg));
+}
+
+#if CH_USE_DYNAMIC && CH_USE_WAITEXIT && CH_USE_HEAP
+/**
+ * @brief Creates a new thread allocating the memory from the heap.
+ *
+ * @param[in] size size of the working area to be allocated
+ * @param[in] prio the priority level for the new thread
+ * @param[in] pf the thread function
+ * @param[in] arg an argument passed to the thread function. It can be @p NULL.
+ * @return The pointer to the @p Thread structure allocated for the
+ *         thread into the working space area.
+ * @retval NULL if the memory cannot be allocated.
+ * @note A thread can terminate by calling @p chThdExit() or by simply
+ *       returning from its main function.
+ * @note The memory allocated for the thread is not released when the thread
+ *       terminates but when a @p chThdWait() is performed.
+ * @note The function is available only if the @p CH_USE_DYNAMIC,
+ *       @p CH_USE_HEAP and @p CH_USE_WAITEXIT options are enabled
+ *       in @p chconf.h.
+ */
+Thread *chThdCreateFromHeap(size_t size, tprio_t prio, tfunc_t pf, void *arg) {
+  void *wsp;
+  Thread *tp;
+
+  wsp = chHeapAlloc(size);
+  if (wsp == NULL)
+    return NULL;
+  tp = chThdInit(wsp, size, prio, pf, arg);
+  tp->p_flags = P_MEM_MODE_HEAP;
+  return chThdResume(tp);
+}
+#endif /* CH_USE_DYNAMIC && CH_USE_WAITEXIT && CH_USE_HEAP */
+
+#if CH_USE_DYNAMIC && CH_USE_WAITEXIT && CH_USE_MEMPOOLS
+/**
+ * @brief Creates a new thread allocating the memory from the specified Memory
+ *        Pool.
+ *
+ * @param[in] mp the memory pool
+ * @param[in] prio the priority level for the new thread
+ * @param[in] pf the thread function
+ * @param[in] arg an argument passed to the thread function. It can be @p NULL.
+ * @return The pointer to the @p Thread structure allocated for the
+ *         thread into the working space area or @p NULL if the memory cannot
+ *         be allocated.
+ * @retval NULL if the memory pool is empty.
+ * @note A thread can terminate by calling @p chThdExit() or by simply
+ *       returning from its main function.
+ * @note The memory allocated for the thread is not released when the thread
+ *       terminates but when a @p chThdWait() is performed.
+ * @note The function is available only if the @p CH_USE_DYNAMIC,
+ *       @p CH_USE_MEMPOOLS and @p CH_USE_WAITEXIT options are enabled
+ *       in @p chconf.h.
+ */
+Thread *chThdCreateFromMemoryPool(MemoryPool *mp, tprio_t prio,
+                                  tfunc_t pf, void *arg) {
+  void *wsp;
+  Thread *tp;
+
+  chDbgCheck(mp != NULL, "chThdCreateFromMemoryPool");
+
+  wsp = chPoolAlloc(mp);
+  if (wsp == NULL)
+    return NULL;
+  tp = chThdInit(wsp, mp->mp_object_size, prio, pf, arg);
+  tp->p_flags = P_MEM_MODE_MEMPOOL;
+  tp->p_mpool = mp;
+  return chThdResume(tp);
+}
+#endif /* CH_USE_DYNAMIC && CH_USE_WAITEXIT && CH_USE_MEMPOOLS */
+
+/**
+ * @brief Changes the running thread priority level then reschedules if
+ *        necessary.
+ *
+ * @param[in] newprio the new priority level of the running thread
+ * @return The old priority level.
+ * @note The function returns the real thread priority regardless of the
+ *       current priority that could be higher than the real priority because
+ *       the priority inheritance mechanism.
+ */
+tprio_t chThdSetPriority(tprio_t newprio) {
+  tprio_t oldprio;
+
+  chDbgCheck((newprio >= LOWPRIO) && (newprio <= HIGHPRIO),
+              "chThdSetPriority");
+
+  chSysLock();
+#if CH_USE_MUTEXES
+  oldprio = currp->p_realprio;
+  if ((currp->p_prio == currp->p_realprio) || (newprio > currp->p_prio))
+    currp->p_prio = newprio;
+  currp->p_realprio = newprio;
+#else
+  oldprio = currp->p_prio;
+  currp->p_prio = newprio;
+#endif
+  chSchRescheduleS();
+  chSysUnlock();
+  return oldprio;
+}
+
+/**
+ * @brief Resumes a suspended thread.
+ *
+ * @param[in] tp the pointer to the thread
+ * @return The pointer to the thread.
+ * @note This call is supposed to resume threads created with @p chThdInit().
+ *       It should not be used on threads suspended using @p chThdSuspend().
+ */
+Thread *chThdResume(Thread *tp) {
+
+  chSysLock();
+  chDbgAssert(tp->p_state == PRSUSPENDED,
+              "chThdResume(), #1",
+              "thread not in PRSUSPENDED state");
+  chSchWakeupS(tp, RDY_OK);
+  chSysUnlock();
+  return tp;
+}
+
+/**
+ * @brief Requests a thread termination.
+ *
+ * @param[in] tp the pointer to the thread
+ * @note The thread is not termitated but a termination request is added to
+ *       its @p p_flags field. The thread can read this status by
+ *       invoking @p chThdShouldTerminate() and then terminate cleanly.
+ */
+void chThdTerminate(Thread *tp) {
+
+  chSysLock();
+  tp->p_flags |= P_TERMINATE;
+  chSysUnlock();
+}
+
+/**
+ * @brief Suspends the invoking thread for the specified time.
+ *
+ * @param[in] time the delay in system ticks, the special values are handled as
+ *                 follow:
+ *                 - @a TIME_INFINITE the thread enters an infinite sleep
+ *                   state.
+ *                 - @a TIME_IMMEDIATE this value is accepted but interpreted
+ *                   as a normal time specification not as an immediate timeout
+ *                   specification.
+ *                 .
+ */
+void chThdSleep(systime_t time) {
+
+  chDbgCheck(time != TIME_INFINITE, "chThdSleep");
+
+  chSysLock();
+  chThdSleepS(time);
+  chSysUnlock();
+}
+
+/**
+ * @brief Suspends the invoking thread until the system time arrives to the
+ *        specified value.
+ *
+ * @param[in] time the absolute system time
+ */
+void chThdSleepUntil(systime_t time) {
+
+  chSysLock();
+  if ((time -= chTimeNow()) > 0)
+    chThdSleepS(time);
+  chSysUnlock();
+}
+
+/**
+ * @brief Terminates the current thread by specifying an exit status code.
+ *
+ * @param[in] msg the thread exit code. The code can be retrieved by using
+ *                @p chThdWait().
+ */
+void chThdExit(msg_t msg) {
+  Thread *tp = currp;
+
+  chSysLock();
+  tp->p_exitcode = msg;
+  THREAD_EXT_EXIT(tp);
+#if CH_USE_WAITEXIT
+  if (tp->p_waiting != NULL)
+    chSchReadyI(tp->p_waiting);
+#endif
+  chSchGoSleepS(PREXIT);
+}
+
+#if CH_USE_WAITEXIT
+/**
+ * @brief Blocks the execution of the invoking thread until the specified
+ *        thread terminates then the exit code is returned.
+ * @details The memory used by the exited thread is handled in different ways
+ *          depending on the API that spawned the thread:
+ *          - If the thread was spawned by @p chThdCreateStatic() or by
+ *            @p chThdInit() then nothing happens and the thread working area
+ *            is not released or modified in any way. This is the default,
+ *            totally static, behavior.
+ *          - If the thread was spawned by @p chThdCreateFromHeap() then
+ *            the working area is returned to the system heap.
+ *          - If the thread was spawned by @p chThdCreateFromMemoryPool()
+ *            then the working area is returned to the owning memory pool.
+ *          .
+ * @param[in] tp the thread pointer
+ * @return The exit code from the terminated thread
+ * @note After invoking @p chThdWait() the thread pointer becomes invalid and
+ *       must not be used as parameter for further system calls.
+ * @note The function is available only if the @p CH_USE_WAITEXIT
+ *       option is enabled in @p chconf.h.
+ * @note Only one thread can be waiting for another thread at any time. You
+ *       should imagine the threads as having a reference counter that is set
+ *       to one when the thread is created, chThdWait() decreases the reference
+ *       and the memory is freed when the counter reaches zero. In the current
+ *       implementation there is no real reference counter in the thread
+ *       structure but it is a planned extension.
+ */
+msg_t chThdWait(Thread *tp) {
+  msg_t msg;
+
+  chDbgCheck(tp != NULL, "chThdWait");
+
+  chSysLock();
+
+  chDbgAssert(tp != currp, "chThdWait(), #1", "waiting self");
+  chDbgAssert(tp->p_waiting == NULL, "chThdWait(), #2", "some other thread waiting");
+
+  if (tp->p_state != PREXIT) {
+    tp->p_waiting = currp;
+    chSchGoSleepS(PRWAIT);
+  }
+  msg = tp->p_exitcode;
+#if !CH_USE_DYNAMIC
+  chSysUnlock();
+  return msg;
+#else /* CH_USE_DYNAMIC */
+
+  /* Returning memory.*/
+  tmode_t mode = tp->p_flags & P_MEM_MODE_MASK;
+  chSysUnlock();
+
+  switch (mode) {
+#if CH_USE_HEAP
+  case P_MEM_MODE_HEAP:
+    chHeapFree(tp);
+    break;
+#endif
+#if CH_USE_MEMPOOLS
+  case P_MEM_MODE_MEMPOOL:
+    chPoolFree(tp->p_mpool, tp);
+    break;
+#endif
+  }
+  return msg;
+#endif /* CH_USE_DYNAMIC */
+}
+#endif /* CH_USE_WAITEXIT */
+
+/** @} */
diff --git a/os/kernel/src/chvt.c b/os/kernel/src/chvt.c
new file mode 100644
index 000000000..c1d8734ec
--- /dev/null
+++ b/os/kernel/src/chvt.c
@@ -0,0 +1,116 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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/RT 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 <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file chvt.c
+ * @brief Time and Virtual Timers related code.
+ * @addtogroup Time
+ * @{
+ */
+
+#include <ch.h>
+
+VTList vtlist;
+
+/**
+ * @brief Virtual Timers initialization.
+ *
+ * @note Internal use only.
+ */
+void vt_init(void) {
+
+  vtlist.vt_next = vtlist.vt_prev = (void *)&vtlist;
+  vtlist.vt_time = (systime_t)-1;
+  vtlist.vt_systime = 0;
+}
+
+/**
+ * @brief Enables a virtual timer.
+ *
+ * @param[out] vtp the @p VirtualTimer structure pointer
+ * @param[in] time the number of time ticks, the value @p TIME_INFINITE is not
+ *                 allowed. The value @p TIME_IMMEDIATE is allowed but
+ *                 interpreted as a normal time specification not as an
+ *                 immediate timeout specification.
+ * @param[in] vtfunc the timer callback function. After invoking the callback
+ *                   the timer is disabled and the structure can be disposed or
+ *                   reused.
+ * @param[in] par a parameter that will be passed to the callback function
+ * @note The associated function is invoked by an interrupt handler within
+ *       the I-Locked state, see @ref system_states.
+ */
+void chVTSetI(VirtualTimer *vtp, systime_t time, vtfunc_t vtfunc, void *par) {
+  VirtualTimer *p;
+
+  chDbgCheck((vtp != NULL) && (vtfunc != NULL) && (time != TIME_INFINITE),
+             "chVTSetI");
+
+  vtp->vt_par = par;
+  vtp->vt_func = vtfunc;
+  p = vtlist.vt_next;
+  while (p->vt_time < time) {
+    time -= p->vt_time;
+    p = p->vt_next;
+  }
+
+  vtp->vt_prev = (vtp->vt_next = p)->vt_prev;
+  vtp->vt_prev->vt_next = p->vt_prev = vtp;
+  vtp->vt_time = time;
+  if (p != (void *)&vtlist)
+    p->vt_time -= time;
+}
+
+/**
+ * @brief Disables a Virtual Timer.
+ *
+ * @param[in] vtp the @p VirtualTimer structure pointer
+ * @note The timer MUST be active when this function is invoked.
+ */
+void chVTResetI(VirtualTimer *vtp) {
+
+  chDbgCheck(vtp != NULL, "chVTResetI");
+  chDbgAssert(vtp->vt_func != NULL,
+              "chVTResetI(), #1",
+              "timer not set or already triggered");
+
+  if (vtp->vt_next != (void *)&vtlist)
+    vtp->vt_next->vt_time += vtp->vt_time;
+  vtp->vt_prev->vt_next = vtp->vt_next;
+  vtp->vt_next->vt_prev = vtp->vt_prev;
+  vtp->vt_func = NULL;
+}
+
+/**
+ * @brief Checks if the current system time is within the specified time window.
+ *
+ * @param[in] start the start of the time window (inclusive)
+ * @param[in] end the end of the time window (non inclusive)
+ * @retval TRUE current time within the specified time window.
+ * @retval FALSE current time not within the specified time window.
+ * @note When start==end then the function returns always true because the
+ *       whole time range is specified.
+ */
+bool_t chTimeIsWithin(systime_t start, systime_t end) {
+
+  systime_t time = chTimeNow();
+  return end > start ? (time >= start) && (time < end) :
+                       (time >= start) || (time < end);
+}
+
+/** @} */