23 files changed, 7218 insertions, 0 deletions

diff --git a/lib/chibios/os/oslib/dox/lib.dox b/lib/chibios/os/oslib/dox/lib.dox
new file mode 100644
index 000000000..7386c1365
--- /dev/null
+++ b/lib/chibios/os/oslib/dox/lib.dox
@@ -0,0 +1,100 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+/**
+ * @defgroup oslib OS Library
+ * @details The OS Library is a set of RTOS extensions compatible with both
+ *          the RT and NIL RTOSes.
+ */
+
+/**
+ * @defgroup oslib_info Version Numbers and Identification
+ * @ingroup oslib
+ */
+
+/**
+ * @defgroup oslib_synchronization Synchronization
+ * @details Synchronization services.
+ * @ingroup oslib
+ */
+
+/**
+ * @defgroup oslib_binary_semaphores Binary Semaphores
+ * @ingroup oslib_synchronization
+ */
+
+/**
+ * @defgroup oslib_mailboxes Mailboxes
+ * @ingroup oslib_synchronization
+ */
+
+/**
+ * @defgroup oslib_pipes Pipes
+ * @ingroup oslib_synchronization
+ */
+
+/**
+ * @defgroup oslib_delegates Delegate Threads
+ * @ingroup oslib_synchronization
+ */
+
+/**
+ * @defgroup oslib_jobs_queues Jobs Queues
+ * @ingroup oslib_synchronization
+ */
+
+/**
+ * @defgroup oslib_memory Memory Management
+ * @details Memory Management services.
+ * @ingroup oslib
+ */
+
+/**
+ * @defgroup oslib_memcore Core Memory Manager
+ * @ingroup oslib_memory
+ */
+
+/**
+ * @defgroup oslib_memheaps Memory Heaps
+ * @ingroup oslib_memory
+ */
+
+/**
+ * @defgroup oslib_mempools Memory Pools
+ * @ingroup oslib_memory
+ */
+
+/**
+ * @defgroup oslib_complex Complex Services
+ * @ingroup oslib
+ */
+
+/**
+ * @defgroup oslib_objects_fifos Objects FIFOs
+ * @ingroup oslib_complex
+ */
+
+/**
+ * @defgroup oslib_objchaches Objects Caches
+ * @ingroup oslib_complex
+ */
+
+/**
+ * @defgroup oslib_objects_factory Dynamic Objects Factory
+ * @ingroup oslib_complex
+ */
diff --git a/lib/chibios/os/oslib/include/chbsem.h b/lib/chibios/os/oslib/include/chbsem.h
new file mode 100644
index 000000000..7969d4fd0
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chbsem.h
@@ -0,0 +1,311 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chbsem.h
+ * @brief   Binary semaphores structures and macros.
+ * @details Binary semaphores related APIs and services.
+ *          <h2>Operation mode</h2>
+ *          Binary semaphores are implemented as a set of inline functions
+ *          that use the existing counting semaphores primitives. The
+ *          difference between counting and binary semaphores is that the
+ *          counter of binary semaphores is not allowed to grow above the
+ *          value 1. Repeated signal operation are ignored. A binary
+ *          semaphore can thus have only two defined states:
+ *          - <b>Taken</b>, when its counter has a value of zero or lower
+ *            than zero. A negative number represent the number of threads
+ *            queued on the binary semaphore.
+ *          - <b>Not taken</b>, when its counter has a value of one.
+ *          .
+ *          Binary semaphores are different from mutexes because there is no
+ *          concept of ownership, a binary semaphore can be taken by a
+ *          thread and signaled by another thread or an interrupt handler,
+ *          mutexes can only be taken and released by the same thread. Another
+ *          difference is that binary semaphores, unlike mutexes, do not
+ *          implement the priority inheritance protocol.<br>
+ *          In order to use the binary semaphores APIs the
+ *          @p CH_CFG_USE_SEMAPHORES option must be enabled in @p chconf.h.
+ *
+ * @addtogroup oslib_binary_semaphores
+ * @{
+ */
+
+#ifndef CHBSEM_H
+#define CHBSEM_H
+
+#if (CH_CFG_USE_SEMAPHORES == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @extends semaphore_t
+ *
+ * @brief   Binary semaphore type.
+ */
+typedef struct ch_binary_semaphore {
+  semaphore_t           sem;
+} binary_semaphore_t;
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/**
+ * @brief   Data part of a static semaphore initializer.
+ * @details This macro should be used when statically initializing a semaphore
+ *          that is part of a bigger structure.
+ *
+ * @param[in] name      the name of the semaphore variable
+ * @param[in] taken     the semaphore initial state
+ */
+#define _BSEMAPHORE_DATA(name, taken)                                       \
+  {_SEMAPHORE_DATA(name.sem, ((taken) ? 0 : 1))}
+
+/**
+ * @brief   Static semaphore initializer.
+ * @details Statically initialized semaphores require no explicit
+ *          initialization using @p chBSemInit().
+ *
+ * @param[in] name      the name of the semaphore variable
+ * @param[in] taken     the semaphore initial state
+ */
+#define BSEMAPHORE_DECL(name, taken)                                        \
+    binary_semaphore_t name = _BSEMAPHORE_DATA(name, taken)
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Initializes a binary semaphore.
+ *
+ * @param[out] bsp      pointer to a @p binary_semaphore_t structure
+ * @param[in] taken     initial state of the binary semaphore:
+ *                      - @a false, the initial state is not taken.
+ *                      - @a true, the initial state is taken.
+ *                      .
+ *
+ * @init
+ */
+static inline void chBSemObjectInit(binary_semaphore_t *bsp, bool taken) {
+
+  chSemObjectInit(&bsp->sem, taken ? (cnt_t)0 : (cnt_t)1);
+}
+
+/**
+ * @brief   Wait operation on the binary semaphore.
+ *
+ * @param[in] bsp       pointer to a @p binary_semaphore_t structure
+ * @return              A message specifying how the invoking thread has been
+ *                      released from the semaphore.
+ * @retval MSG_OK       if the binary semaphore has been successfully taken.
+ * @retval MSG_RESET    if the binary semaphore has been reset using
+ *                      @p bsemReset().
+ *
+ * @api
+ */
+static inline msg_t chBSemWait(binary_semaphore_t *bsp) {
+
+  return chSemWait(&bsp->sem);
+}
+
+/**
+ * @brief   Wait operation on the binary semaphore.
+ *
+ * @param[in] bsp       pointer to a @p binary_semaphore_t structure
+ * @return              A message specifying how the invoking thread has been
+ *                      released from the semaphore.
+ * @retval MSG_OK       if the binary semaphore has been successfully taken.
+ * @retval MSG_RESET    if the binary semaphore has been reset using
+ *                      @p bsemReset().
+ *
+ * @sclass
+ */
+static inline msg_t chBSemWaitS(binary_semaphore_t *bsp) {
+
+  chDbgCheckClassS();
+
+  return chSemWaitS(&bsp->sem);
+}
+
+/**
+ * @brief   Wait operation on the binary semaphore.
+ *
+ * @param[in] bsp       pointer to a @p binary_semaphore_t 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 message specifying how the invoking thread has been
+ *                      released from the semaphore.
+ * @retval MSG_OK       if the binary semaphore has been successfully taken.
+ * @retval MSG_RESET    if the binary semaphore has been reset using
+ *                      @p bsemReset().
+ * @retval MSG_TIMEOUT  if the binary semaphore has not been signaled or reset
+ *                      within the specified timeout.
+ *
+ * @sclass
+ */
+static inline msg_t chBSemWaitTimeoutS(binary_semaphore_t *bsp,
+                                       sysinterval_t timeout) {
+
+  chDbgCheckClassS();
+
+  return chSemWaitTimeoutS(&bsp->sem, timeout);
+}
+
+/**
+ * @brief   Wait operation on the binary semaphore.
+ *
+ * @param[in] bsp       pointer to a @p binary_semaphore_t 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 message specifying how the invoking thread has been
+ *                      released from the semaphore.
+ * @retval MSG_OK       if the binary semaphore has been successfully taken.
+ * @retval MSG_RESET    if the binary semaphore has been reset using
+ *                      @p bsemReset().
+ * @retval MSG_TIMEOUT  if the binary semaphore has not been signaled or reset
+ *                      within the specified timeout.
+ *
+ * @api
+ */
+static inline msg_t chBSemWaitTimeout(binary_semaphore_t *bsp,
+                                      sysinterval_t timeout) {
+
+  return chSemWaitTimeout(&bsp->sem, timeout);
+}
+
+/**
+ * @brief   Reset operation on the binary semaphore.
+ * @note    The released threads can recognize they were waked up by a reset
+ *          rather than a signal because the @p bsemWait() will return
+ *          @p MSG_RESET instead of @p MSG_OK.
+ * @note    This function does not reschedule.
+ *
+ * @param[in] bsp       pointer to a @p binary_semaphore_t structure
+ * @param[in] taken     new state of the binary semaphore
+ *                      - @a false, the new state is not taken.
+ *                      - @a true, the new state is taken.
+ *                      .
+ *
+ * @iclass
+ */
+static inline void chBSemResetI(binary_semaphore_t *bsp, bool taken) {
+
+  chDbgCheckClassI();
+
+  chSemResetI(&bsp->sem, taken ? (cnt_t)0 : (cnt_t)1);
+}
+
+/**
+ * @brief   Reset operation on the binary semaphore.
+ * @note    The released threads can recognize they were waked up by a reset
+ *          rather than a signal because the @p bsemWait() will return
+ *          @p MSG_RESET instead of @p MSG_OK.
+ *
+ * @param[in] bsp       pointer to a @p binary_semaphore_t structure
+ * @param[in] taken     new state of the binary semaphore
+ *                      - @a false, the new state is not taken.
+ *                      - @a true, the new state is taken.
+ *                      .
+ *
+ * @api
+ */
+static inline void chBSemReset(binary_semaphore_t *bsp, bool taken) {
+
+  chSemReset(&bsp->sem, taken ? (cnt_t)0 : (cnt_t)1);
+}
+
+/**
+ * @brief   Performs a signal operation on a binary semaphore.
+ * @note    This function does not reschedule.
+ *
+ * @param[in] bsp       pointer to a @p binary_semaphore_t structure
+ *
+ * @iclass
+ */
+static inline void chBSemSignalI(binary_semaphore_t *bsp) {
+
+  chDbgCheckClassI();
+
+  if (bsp->sem.cnt < (cnt_t)1) {
+    chSemSignalI(&bsp->sem);
+  }
+}
+
+/**
+ * @brief   Performs a signal operation on a binary semaphore.
+ *
+ * @param[in] bsp       pointer to a @p binary_semaphore_t structure
+ *
+ * @api
+ */
+static inline void chBSemSignal(binary_semaphore_t *bsp) {
+
+  chSysLock();
+  chBSemSignalI(bsp);
+  chSchRescheduleS();
+  chSysUnlock();
+}
+
+/**
+ * @brief   Returns the binary semaphore current state.
+ *
+ * @param[in] bsp       pointer to a @p binary_semaphore_t structure
+ * @return              The binary semaphore current state.
+ * @retval false        if the binary semaphore is not taken.
+ * @retval true         if the binary semaphore is taken.
+ *
+ * @iclass
+ */
+static inline bool chBSemGetStateI(const binary_semaphore_t *bsp) {
+
+  chDbgCheckClassI();
+
+  return (bsp->sem.cnt > (cnt_t)0) ? false : true;
+}
+
+#endif /* CH_CFG_USE_SEMAPHORES == TRUE */
+
+#endif /* CHBSEM_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chdelegates.h b/lib/chibios/os/oslib/include/chdelegates.h
new file mode 100644
index 000000000..b5b62f991
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chdelegates.h
@@ -0,0 +1,208 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chdelegates.h
+ * @brief   Delegate threads macros and structures.
+ *
+ * @addtogroup oslib_delegates
+ * @{
+ */
+
+#ifndef CHDELEGATES_H
+#define CHDELEGATES_H
+
+#if (CH_CFG_USE_DELEGATES == TRUE) || defined(__DOXYGEN__)
+
+/*lint -save -e829 [17.1] Required by design.*/
+#include <stdarg.h>
+/*lint -restore*/
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if CH_CFG_USE_MESSAGES == FALSE
+#error "CH_CFG_USE_DELEGATES requires CH_CFG_USE_MESSAGES"
+#endif
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Type of a delegate veneer function.
+ */
+typedef msg_t (*delegate_veneer_t)(va_list *argsp);
+
+/**
+ * @brief   Type of a delegate function with no parameters.
+ */
+typedef msg_t (*delegate_fn0_t)(void);
+
+/**
+ * @brief   Type of a delegate function with one parameter.
+ */
+typedef msg_t (*delegate_fn1_t)(msg_t p1);
+
+/**
+ * @brief   Type of a delegate function with two parameters.
+ */
+typedef msg_t (*delegate_fn2_t)(msg_t p1, msg_t p2);
+
+/**
+ * @brief   Type of a delegate function with three parameters.
+ */
+typedef msg_t (*delegate_fn3_t)(msg_t p1, msg_t p2, msg_t p3);
+
+/**
+ * @brief   Type of a delegate function with four parameters.
+ */
+typedef msg_t (*delegate_fn4_t)(msg_t p1, msg_t p2, msg_t p3, msg_t p4);
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  msg_t __ch_delegate_fn0(va_list *argsp);
+  msg_t __ch_delegate_fn1(va_list *argsp);
+  msg_t __ch_delegate_fn2(va_list *argsp);
+  msg_t __ch_delegate_fn3(va_list *argsp);
+  msg_t __ch_delegate_fn4(va_list *argsp);
+  void chDelegateDispatch(void);
+  msg_t chDelegateDispatchTimeout(sysinterval_t timeout);
+  msg_t chDelegateCallVeneer(thread_t *tp, delegate_veneer_t veneer, ...);
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Direct call to a function with no parameters.
+ * @note    The return value is assumed to be not larger than a data
+ *          pointer type. If you need a portable function then use
+ *          @p chDelegateCallVeneer() instead.
+ *
+ * @param[in] tp        pointer to the delegate thread
+ * @param[in] func      pointer to the function to be called
+ * @return              The function return value as a @p msg_t.
+ */
+static inline msg_t chDelegateCallDirect0(thread_t *tp, delegate_fn0_t func) {
+
+  return chDelegateCallVeneer(tp, __ch_delegate_fn0, func);
+}
+
+/**
+ * @brief   Direct call to a function with one parameter.
+ * @note    The return value and parameters are assumed to be not larger
+ *          than a data pointer type. If you need a portable function then use
+ *          @p chDelegateCallVeneer() instead.
+ *
+ * @param[in] tp        pointer to the delegate thread
+ * @param[in] func      pointer to the function to be called
+ * @param[in] p1        parameter 1 passed as a @p msg_t
+ * @return              The function return value as a @p msg_t.
+ */
+static inline msg_t chDelegateCallDirect1(thread_t *tp, delegate_fn1_t func,
+                                          msg_t p1) {
+
+  return chDelegateCallVeneer(tp, __ch_delegate_fn1, func, p1);
+}
+
+/**
+ * @brief   Direct call to a function with two parameters.
+ * @note    The return value and parameters are assumed to be not larger
+ *          than a data pointer type. If you need a portable function then use
+ *          @p chDelegateCallVeneer() instead.
+ *
+ * @param[in] tp        pointer to the delegate thread
+ * @param[in] func      pointer to the function to be called
+ * @param[in] p1        parameter 1 passed as a @p msg_t
+ * @param[in] p2        parameter 2 passed as a @p msg_t
+ * @return              The function return value as a @p msg_t.
+ */
+static inline msg_t chDelegateCallDirect2(thread_t *tp, delegate_fn2_t func,
+                                          msg_t p1, msg_t p2) {
+
+  return chDelegateCallVeneer(tp, __ch_delegate_fn2, func, p1, p2);
+}
+
+/**
+ * @brief   Direct call to a function with three parameters.
+ * @note    The return value and parameters are assumed to be not larger
+ *          than a data pointer type. If you need a portable function then use
+ *          @p chDelegateCallVeneer() instead.
+ *
+ * @param[in] tp        pointer to the delegate thread
+ * @param[in] func      pointer to the function to be called
+ * @param[in] p1        parameter 1 passed as a @p msg_t
+ * @param[in] p2        parameter 2 passed as a @p msg_t
+ * @param[in] p3        parameter 3 passed as a @p msg_t
+ * @return              The function return value as a @p msg_t.
+ */
+static inline msg_t chDelegateCallDirect3(thread_t *tp, delegate_fn3_t func,
+                                          msg_t p1, msg_t p2, msg_t p3) {
+
+  return chDelegateCallVeneer(tp, __ch_delegate_fn3, func, p1, p2, p3);
+}
+
+/**
+ * @brief   Direct call to a function with four parameters.
+ * @note    The return value and parameters are assumed to be not larger
+ *          than a data pointer type. If you need a portable function then use
+ *          @p chDelegateCallVeneer() instead.
+ *
+ * @param[in] tp        pointer to the delegate thread
+ * @param[in] func      pointer to the function to be called
+ * @param[in] p1        parameter 1 passed as a @p msg_t
+ * @param[in] p2        parameter 2 passed as a @p msg_t
+ * @param[in] p3        parameter 3 passed as a @p msg_t
+ * @param[in] p4        parameter 4 passed as a @p msg_t
+ * @return              The function return value as a @p msg_t.
+ */
+static inline msg_t chDelegateCallDirect4(thread_t *tp, delegate_fn4_t func,
+                                          msg_t p1, msg_t p2, msg_t p3,
+                                          msg_t p4) {
+
+  return chDelegateCallVeneer(tp, __ch_delegate_fn4, func, p1, p2, p3, p4);
+}
+
+#endif /* CH_CFG_USE_DELEGATES == TRUE */
+
+#endif /* CHDELEGATES_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chfactory.h b/lib/chibios/os/oslib/include/chfactory.h
new file mode 100644
index 000000000..c751c6fd1
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chfactory.h
@@ -0,0 +1,523 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chfactory.h
+ * @brief   ChibiOS objects factory structures and macros.
+ *
+ * @addtogroup oslib_objects_factory
+ * @{
+ */
+
+#ifndef CHFACTORY_H
+#define CHFACTORY_H
+
+#if (CH_CFG_USE_FACTORY == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Maximum length for object names.
+ * @details If the specified length is zero then the name is stored by
+ *          pointer but this could have unintended side effects.
+ */
+#if !defined(CH_CFG_FACTORY_MAX_NAMES_LENGTH) || defined(__DOXYGEN__)
+#define CH_CFG_FACTORY_MAX_NAMES_LENGTH     8
+#endif
+
+/**
+ * @brief   Enables the registry of generic objects.
+ */
+#if !defined(CH_CFG_FACTORY_OBJECTS_REGISTRY) || defined(__DOXYGEN__)
+#define CH_CFG_FACTORY_OBJECTS_REGISTRY     TRUE
+#endif
+
+/**
+ * @brief   Enables factory for generic buffers.
+ */
+#if !defined(CH_CFG_FACTORY_GENERIC_BUFFERS) || defined(__DOXYGEN__)
+#define CH_CFG_FACTORY_GENERIC_BUFFERS      TRUE
+#endif
+
+/**
+ * @brief   Enables factory for semaphores.
+ */
+#if !defined(CH_CFG_FACTORY_SEMAPHORES) || defined(__DOXYGEN__)
+#define CH_CFG_FACTORY_SEMAPHORES           TRUE
+#endif
+
+/**
+ * @brief   Enables factory for mailboxes.
+ */
+#if !defined(CH_CFG_FACTORY_MAILBOXES) || defined(__DOXYGEN__)
+#define CH_CFG_FACTORY_MAILBOXES            TRUE
+#endif
+
+/**
+ * @brief   Enables factory for objects FIFOs.
+ */
+#if !defined(CH_CFG_FACTORY_OBJ_FIFOS) || defined(__DOXYGEN__)
+#define CH_CFG_FACTORY_OBJ_FIFOS            TRUE
+#endif
+
+/**
+ * @brief   Enables factory for objects FIFOs.
+ */
+#if !defined(CH_CFG_FACTORY_OBJ_FIFOS) || defined(__DOXYGEN__)
+#define CH_CFG_FACTORY_OBJ_FIFOS            TRUE
+#endif
+
+/**
+ * @brief   Enables factory for Pipes.
+ */
+#if !defined(CH_CFG_FACTORY_PIPES) || defined(__DOXYGEN__)
+#define CH_CFG_FACTORY_PIPES                TRUE
+#endif
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if (CH_CFG_FACTORY_SEMAPHORES == TRUE) && (CH_CFG_USE_SEMAPHORES == FALSE)
+/*lint -save -e767 [20.5] Valid because the #undef.*/
+#undef CH_CFG_FACTORY_SEMAPHORES
+#define CH_CFG_FACTORY_SEMAPHORES           FALSE
+/*lint restore*/
+#endif
+
+#if (CH_CFG_FACTORY_MAILBOXES == TRUE) && (CH_CFG_USE_MAILBOXES == FALSE)
+/*lint -save -e767 [20.5] Valid because the #undef.*/
+#undef CH_CFG_FACTORY_MAILBOXES
+#define CH_CFG_FACTORY_MAILBOXES            FALSE
+/*lint restore*/
+#endif
+
+#if (CH_CFG_FACTORY_OBJ_FIFOS == TRUE) && (CH_CFG_USE_OBJ_FIFOS == FALSE)
+/*lint -save -e767 [20.5] Valid because the #undef.*/
+#undef CH_CFG_FACTORY_OBJ_FIFOS
+#define CH_CFG_FACTORY_OBJ_FIFOS            FALSE
+/*lint restore*/
+#endif
+
+#if (CH_CFG_FACTORY_PIPES == TRUE) && (CH_CFG_USE_PIPES == FALSE)
+/*lint -save -e767 [20.5] Valid because the #undef.*/
+#undef CH_CFG_FACTORY_PIPES
+#define CH_CFG_FACTORY_PIPES                FALSE
+/*lint restore*/
+#endif
+
+#define CH_FACTORY_REQUIRES_POOLS                                           \
+  ((CH_CFG_FACTORY_OBJECTS_REGISTRY == TRUE) ||                             \
+   (CH_CFG_FACTORY_SEMAPHORES == TRUE))
+
+#define CH_FACTORY_REQUIRES_HEAP                                            \
+  ((CH_CFG_FACTORY_GENERIC_BUFFERS == TRUE) ||                              \
+   (CH_CFG_FACTORY_MAILBOXES == TRUE) ||                                    \
+   (CH_CFG_FACTORY_OBJ_FIFOS == TRUE) ||                                    \
+   (CH_CFG_FACTORY_PIPES == TRUE))
+
+#if (CH_CFG_FACTORY_MAX_NAMES_LENGTH < 0) ||                                \
+    (CH_CFG_FACTORY_MAX_NAMES_LENGTH > 32)
+#error "invalid CH_CFG_FACTORY_MAX_NAMES_LENGTH value"
+#endif
+
+#if (CH_CFG_USE_MUTEXES == FALSE) && (CH_CFG_USE_SEMAPHORES == FALSE)
+#error "CH_CFG_USE_FACTORY requires CH_CFG_USE_MUTEXES and/or CH_CFG_USE_SEMAPHORES"
+#endif
+
+#if CH_CFG_USE_MEMCORE == FALSE
+#error "CH_CFG_USE_FACTORY requires CH_CFG_USE_MEMCORE"
+#endif
+
+#if CH_FACTORY_REQUIRES_POOLS && (CH_CFG_USE_MEMPOOLS == FALSE)
+#error "CH_CFG_USE_MEMPOOLS is required"
+#endif
+
+#if CH_FACTORY_REQUIRES_HEAP && (CH_CFG_USE_HEAP == FALSE)
+#error "CH_CFG_USE_HEAP is required"
+#endif
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Type of a dynamic object list element.
+ */
+typedef struct ch_dyn_element {
+  /**
+   * @brief   Next dynamic object in the list.
+   */
+  struct ch_dyn_element *next;
+  /**
+   * @brief   Number of references to this object.
+   */
+  ucnt_t                refs;
+#if (CH_CFG_FACTORY_MAX_NAMES_LENGTH > 0) || defined(__DOXYGEN__)
+  char                  name[CH_CFG_FACTORY_MAX_NAMES_LENGTH];
+#else
+  const char            *name;
+#endif
+} dyn_element_t;
+
+/**
+ * @brief   Type of a dynamic object list.
+ */
+typedef struct ch_dyn_list {
+    dyn_element_t       *next;
+} dyn_list_t;
+
+#if (CH_CFG_FACTORY_OBJECTS_REGISTRY == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Type of a registered object.
+ */
+typedef struct ch_registered_static_object {
+  /**
+   * @brief   List element of the registered object.
+   */
+  dyn_element_t         element;
+  /**
+   * @brief   Pointer to the object.
+   * @note    The type of the object is not stored in anyway.
+   */
+  void                  *objp;
+} registered_object_t;
+#endif
+
+#if (CH_CFG_FACTORY_GENERIC_BUFFERS == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Type of a dynamic buffer object.
+ */
+typedef struct ch_dyn_object {
+  /**
+   * @brief   List element of the dynamic buffer object.
+   */
+  dyn_element_t         element;
+} dyn_buffer_t;
+#endif
+
+#if (CH_CFG_FACTORY_SEMAPHORES == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Type of a dynamic semaphore.
+ */
+typedef struct ch_dyn_semaphore {
+  /**
+   * @brief   List element of the dynamic semaphore.
+   */
+  dyn_element_t         element;
+  /**
+   * @brief   The semaphore.
+   */
+  semaphore_t           sem;
+} dyn_semaphore_t;
+#endif
+
+#if (CH_CFG_FACTORY_MAILBOXES == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Type of a dynamic buffer object.
+ */
+typedef struct ch_dyn_mailbox {
+  /**
+   * @brief   List element of the dynamic buffer object.
+   */
+  dyn_element_t         element;
+  /**
+   * @brief   The mailbox.
+   */
+  mailbox_t             mbx;
+} dyn_mailbox_t;
+#endif
+
+#if (CH_CFG_FACTORY_OBJ_FIFOS == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Type of a dynamic buffer object.
+ */
+typedef struct ch_dyn_objects_fifo {
+  /**
+   * @brief   List element of the dynamic buffer object.
+   */
+  dyn_element_t         element;
+  /**
+   * @brief   The objects FIFO.
+   */
+  objects_fifo_t        fifo;
+} dyn_objects_fifo_t;
+#endif
+
+#if (CH_CFG_FACTORY_PIPES == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Type of a dynamic pipe object.
+ */
+typedef struct ch_dyn_pipe {
+  /**
+   * @brief   List element of the dynamic pipe object.
+   */
+  dyn_element_t         element;
+  /**
+   * @brief   The pipe.
+   */
+  pipe_t                pipe;
+} dyn_pipe_t;
+#endif
+
+/**
+ * @brief   Type of the factory main object.
+ */
+typedef struct ch_objects_factory {
+  /**
+   * @brief   Factory access mutex or semaphore.
+   */
+#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
+  mutex_t               mtx;
+#else
+  semaphore_t           sem;
+#endif
+  /**
+   * @brief   List of the registered objects.
+   */
+  dyn_list_t            obj_list;
+  /**
+   * @brief   Pool of the available registered objects.
+   */
+  memory_pool_t         obj_pool;
+#if (CH_CFG_FACTORY_GENERIC_BUFFERS == TRUE) || defined(__DOXYGEN__)
+  /**
+   * @brief   List of the allocated buffer objects.
+   */
+  dyn_list_t            buf_list;
+#endif /* CH_CFG_FACTORY_GENERIC_BUFFERS = TRUE */
+#if (CH_CFG_FACTORY_SEMAPHORES == TRUE) || defined(__DOXYGEN__)
+  /**
+   * @brief   List of the allocated semaphores.
+   */
+  dyn_list_t            sem_list;
+  /**
+   * @brief   Pool of the available semaphores.
+   */
+  memory_pool_t         sem_pool;
+#endif /* CH_CFG_FACTORY_SEMAPHORES = TRUE */
+#if (CH_CFG_FACTORY_MAILBOXES == TRUE) || defined(__DOXYGEN__)
+  /**
+   * @brief   List of the allocated buffer objects.
+   */
+  dyn_list_t            mbx_list;
+#endif /* CH_CFG_FACTORY_MAILBOXES = TRUE */
+#if (CH_CFG_FACTORY_OBJ_FIFOS == TRUE) || defined(__DOXYGEN__)
+  /**
+   * @brief   List of the allocated "objects FIFO" objects.
+   */
+  dyn_list_t            fifo_list;
+#endif /* CH_CFG_FACTORY_OBJ_FIFOS = TRUE */
+#if (CH_CFG_FACTORY_PIPES == TRUE) || defined(__DOXYGEN__)
+  /**
+   * @brief   List of the allocated pipe objects.
+   */
+  dyn_list_t            pipe_list;
+#endif /* CH_CFG_FACTORY_PIPES = TRUE */
+} objects_factory_t;
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#if !defined(__DOXYGEN__)
+extern objects_factory_t ch_factory;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void _factory_init(void);
+#if (CH_CFG_FACTORY_OBJECTS_REGISTRY == TRUE) || defined(__DOXYGEN__)
+  registered_object_t *chFactoryRegisterObject(const char *name,
+                                               void *objp);
+  registered_object_t *chFactoryFindObject(const char *name);
+  registered_object_t *chFactoryFindObjectByPointer(void *objp);
+  void chFactoryReleaseObject(registered_object_t *rop);
+#endif
+#if (CH_CFG_FACTORY_GENERIC_BUFFERS == TRUE) || defined(__DOXYGEN__)
+  dyn_buffer_t *chFactoryCreateBuffer(const char *name, size_t size);
+  dyn_buffer_t *chFactoryFindBuffer(const char *name);
+  void chFactoryReleaseBuffer(dyn_buffer_t *dbp);
+#endif
+#if (CH_CFG_FACTORY_SEMAPHORES == TRUE) || defined(__DOXYGEN__)
+  dyn_semaphore_t *chFactoryCreateSemaphore(const char *name, cnt_t n);
+  dyn_semaphore_t *chFactoryFindSemaphore(const char *name);
+  void chFactoryReleaseSemaphore(dyn_semaphore_t *dsp);
+#endif
+#if (CH_CFG_FACTORY_MAILBOXES == TRUE) || defined(__DOXYGEN__)
+  dyn_mailbox_t *chFactoryCreateMailbox(const char *name, size_t n);
+  dyn_mailbox_t *chFactoryFindMailbox(const char *name);
+  void chFactoryReleaseMailbox(dyn_mailbox_t *dmp);
+#endif
+#if (CH_CFG_FACTORY_OBJ_FIFOS == TRUE) || defined(__DOXYGEN__)
+  dyn_objects_fifo_t *chFactoryCreateObjectsFIFO(const char *name,
+                                                 size_t objsize,
+                                                 size_t objn,
+                                                 unsigned objalign);
+  dyn_objects_fifo_t *chFactoryFindObjectsFIFO(const char *name);
+  void chFactoryReleaseObjectsFIFO(dyn_objects_fifo_t *dofp);
+#endif
+#if (CH_CFG_FACTORY_PIPES == TRUE) || defined(__DOXYGEN__)
+  dyn_pipe_t *chFactoryCreatePipe(const char *name, size_t size);
+  dyn_pipe_t *chFactoryFindPipe(const char *name);
+  void chFactoryReleasePipe(dyn_pipe_t *dpp);
+#endif
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Duplicates an object reference.
+ * @note    This function can be used on any kind of dynamic object.
+ *
+ * @param[in] dep       pointer to the element field of the object
+ * @return              The duplicated object reference.
+ *
+ * @api
+ */
+static inline dyn_element_t *chFactoryDuplicateReference(dyn_element_t *dep) {
+
+  dep->refs++;
+
+  return dep;
+}
+
+#if (CH_CFG_FACTORY_OBJECTS_REGISTRY == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Returns the pointer to the inner registered object.
+ *
+ * @param[in] rop       registered object reference
+ * @return              The pointer to the registered object.
+ *
+ * @api
+ */
+static inline void *chFactoryGetObject(registered_object_t *rop) {
+
+  return rop->objp;
+}
+#endif /* CH_CFG_FACTORY_OBJECTS_REGISTRY == TRUE */
+
+#if (CH_CFG_FACTORY_GENERIC_BUFFERS == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Returns the size of a generic dynamic buffer object.
+ *
+ * @param[in] dbp       dynamic buffer object reference
+ * @return              The size of the buffer object in bytes.
+ *
+ * @api
+ */
+static inline size_t chFactoryGetBufferSize(dyn_buffer_t *dbp) {
+
+  return chHeapGetSize(dbp) - sizeof (dyn_element_t);
+}
+
+/**
+ * @brief   Returns the pointer to the inner buffer.
+ *
+ * @param[in] dbp       dynamic buffer object reference
+ * @return              The pointer to the dynamic buffer.
+ *
+ * @api
+ */
+static inline uint8_t *chFactoryGetBuffer(dyn_buffer_t *dbp) {
+
+  return (uint8_t *)(dbp + 1);
+}
+#endif /* CH_CFG_FACTORY_GENERIC_BUFFERS == TRUE */
+
+#if (CH_CFG_FACTORY_SEMAPHORES == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Returns the pointer to the inner semaphore.
+ *
+ * @param[in] dsp       dynamic semaphore object reference
+ * @return              The pointer to the semaphore.
+ *
+ * @api
+ */
+static inline semaphore_t *chFactoryGetSemaphore(dyn_semaphore_t *dsp) {
+
+  return &dsp->sem;
+}
+#endif /* CH_CFG_FACTORY_SEMAPHORES == TRUE */
+
+#if (CH_CFG_FACTORY_MAILBOXES == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Returns the pointer to the inner mailbox.
+ *
+ * @param[in] dmp       dynamic mailbox object reference
+ * @return              The pointer to the mailbox.
+ *
+ * @api
+ */
+static inline mailbox_t *chFactoryGetMailbox(dyn_mailbox_t *dmp) {
+
+  return &dmp->mbx;
+}
+#endif /* CH_CFG_FACTORY_MAILBOXES == TRUE */
+
+#if (CH_CFG_FACTORY_OBJ_FIFOS == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Returns the pointer to the inner objects FIFO.
+ *
+ * @param[in] dofp      dynamic "objects FIFO" object reference
+ * @return              The pointer to the objects FIFO.
+ *
+ * @api
+ */
+static inline objects_fifo_t *chFactoryGetObjectsFIFO(dyn_objects_fifo_t *dofp) {
+
+  return &dofp->fifo;
+}
+#endif /* CH_CFG_FACTORY_OBJ_FIFOS == TRUE */
+
+#if (CH_CFG_FACTORY_PIPES == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Returns the pointer to the inner pipe.
+ *
+ * @param[in] dpp       dynamic pipe object reference
+ * @return              The pointer to the pipe.
+ *
+ * @api
+ */
+static inline pipe_t *chFactoryGetPipe(dyn_pipe_t *dpp) {
+
+  return &dpp->pipe;
+}
+#endif /* CH_CFG_FACTORY_PIPES == TRUE */
+
+#endif /* CH_CFG_USE_FACTORY == TRUE */
+
+#endif /* CHFACTORY_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chjobs.h b/lib/chibios/os/oslib/include/chjobs.h
new file mode 100644
index 000000000..5eaa0e4ec
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chjobs.h
@@ -0,0 +1,397 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chjobs.h
+ * @brief   Jobs Queues structures and macros.
+ * @details This module implements queues of generic jobs to be delegated
+ *          asynchronously to a pool of dedicated threads.
+ *          Operations defined for Jobs Queues
+ *          - <b>Get</b>: An job object is taken from the pool of the
+ *            available jobs.
+ *          - <b>Post</b>: A job is posted to the queue, it will be
+ *            returned to the pool after execution.
+ *          .
+ *
+ * @addtogroup oslib_jobs_queues
+ * @{
+ */
+
+#ifndef CHJOBS_H
+#define CHJOBS_H
+
+#if (CH_CFG_USE_JOBS == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Dispatcher return code in case of a @p JOB_NUL has been received.
+ */
+#define MSG_JOB_NULL    ((msg_t)-2)
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if CH_CFG_USE_MEMPOOLS == FALSE
+#error "CH_CFG_USE_JOBS requires CH_CFG_USE_MEMPOOLS"
+#endif
+
+#if CH_CFG_USE_SEMAPHORES == FALSE
+#error "CH_CFG_USE_JOBS requires CH_CFG_USE_SEMAPHORES"
+#endif
+
+#if CH_CFG_USE_MAILBOXES == FALSE
+#error "CH_CFG_USE_JOBS requires CH_CFG_USE_MAILBOXES"
+#endif
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Type of a jobs queue.
+ */
+typedef struct ch_jobs_queue {
+  /**
+   * @brief   Pool of the free jobs.
+   */
+  guarded_memory_pool_t     free;
+  /**
+   * @brief   Mailbox of the sent jobs.
+   */
+  mailbox_t                 mbx;
+} jobs_queue_t;
+
+/**
+ * @brief   Type of a job function.
+ */
+typedef void (*job_function_t)(void *arg);
+
+/**
+ * @brief   Type of a job descriptor.
+ */
+typedef struct ch_job_descriptor {
+  /**
+   * @brief   Job function.
+   */
+  job_function_t            jobfunc;
+  /**
+   * @brief   Argument to be passed to the job function.
+   */
+  void                      *jobarg;
+} job_descriptor_t;
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Initializes a jobs queue object.
+ *
+ * @param[out] jqp      pointer to a @p jobs_queue_t structure
+ * @param[in] jobsn     number of jobs available
+ * @param[in] jobsbuf   pointer to the buffer of jobs, it must be able
+ *                      to hold @p jobsn @p job_descriptor_t structures
+ * @param[in] msgbuf    pointer to the buffer of messages, it must be able
+ *                      to hold @p jobsn @p msg_t messages
+ *
+ * @init
+ */
+static inline void chJobObjectInit(jobs_queue_t *jqp,
+                                   size_t jobsn,
+                                   job_descriptor_t *jobsbuf,
+                                   msg_t *msgbuf) {
+
+  chDbgCheck((jobsn > 0U) && (jobsbuf != NULL) && (msgbuf != NULL));
+
+  chGuardedPoolObjectInit(&jqp->free, sizeof (job_descriptor_t));
+  chGuardedPoolLoadArray(&jqp->free, (void *)jobsbuf, jobsn);
+  chMBObjectInit(&jqp->mbx, msgbuf, jobsn);
+}
+
+/**
+ * @brief   Allocates a free job object.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t structure
+ * @return              The pointer to the allocated job object.
+ *
+ * @api
+ */
+static inline job_descriptor_t *chJobGet(jobs_queue_t *jqp) {
+
+  return (job_descriptor_t *)chGuardedPoolAllocTimeout(&jqp->free, TIME_INFINITE);
+}
+
+/**
+ * @brief   Allocates a free job object.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t structure
+ * @return              The pointer to the allocated job object.
+ * @retval NULL         if a job object is not immediately available.
+ *
+ * @iclass
+ */
+static inline job_descriptor_t *chJobGetI(jobs_queue_t *jqp) {
+
+  return (job_descriptor_t *)chGuardedPoolAllocI(&jqp->free);
+}
+
+/**
+ * @brief   Allocates a free job object.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t 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              The pointer to the allocated job object.
+ * @retval NULL         if a job object is not available within the specified
+ *                      timeout.
+ *
+ * @sclass
+ */
+static inline job_descriptor_t *chJobGetTimeoutS(jobs_queue_t *jqp,
+                                                 sysinterval_t timeout) {
+
+  return (job_descriptor_t *)chGuardedPoolAllocTimeoutS(&jqp->free, timeout);
+}
+
+/**
+ * @brief   Allocates a free job object.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t 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              The pointer to the allocated job object.
+ * @retval NULL         if a job object is not available within the specified
+ *                      timeout.
+ *
+ * @api
+ */
+static inline job_descriptor_t *chJobGetTimeout(jobs_queue_t *jqp,
+                                                sysinterval_t timeout) {
+
+  return (job_descriptor_t *)chGuardedPoolAllocTimeout(&jqp->free, timeout);
+}
+
+/**
+ * @brief   Posts a job object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t structure
+ * @param[in] jp        pointer to the job object to be posted
+ *
+ * @iclass
+ */
+static inline void chJobPostI(jobs_queue_t *jqp, job_descriptor_t *jp) {
+  msg_t msg;
+
+  msg = chMBPostI(&jqp->mbx, (msg_t)jp);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts a job object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t structure
+ * @param[in] jp        pointer to the job object to be posted
+ *
+ * @sclass
+ */
+static inline void chJobPostS(jobs_queue_t *jqp, job_descriptor_t *jp) {
+  msg_t msg;
+
+  msg = chMBPostTimeoutS(&jqp->mbx, (msg_t)jp, TIME_IMMEDIATE);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts a job object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t structure
+ * @param[in] jp        pointer to the job object to be posted
+ *
+ * @api
+ */
+static inline void chJobPost(jobs_queue_t *jqp, job_descriptor_t *jp) {
+  msg_t msg;
+
+  msg = chMBPostTimeout(&jqp->mbx, (msg_t)jp, TIME_IMMEDIATE);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts an high priority job object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t structure
+ * @param[in] jp        pointer to the job object to be posted
+ *
+ * @iclass
+ */
+static inline void chJobPostAheadI(jobs_queue_t *jqp, job_descriptor_t *jp) {
+  msg_t msg;
+
+  msg = chMBPostAheadI(&jqp->mbx, (msg_t)jp);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts an high priority job object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t structure
+ * @param[in] jp        pointer to the job object to be posted
+ *
+ * @sclass
+ */
+static inline void chJobPostAheadS(jobs_queue_t *jqp, job_descriptor_t *jp) {
+  msg_t msg;
+
+  msg = chMBPostAheadTimeoutS(&jqp->mbx, (msg_t)jp, TIME_IMMEDIATE);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts an high priority job object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t structure
+ * @param[in] jp        pointer to the job object to be posted
+ *
+ * @api
+ */
+static inline void chJobPostAhead(jobs_queue_t *jqp, job_descriptor_t *jp) {
+  msg_t msg;
+
+  msg = chMBPostAheadTimeout(&jqp->mbx, (msg_t)jp, TIME_IMMEDIATE);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Waits for a job then executes it.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t structure
+ * @return              The function outcome.
+ * @retval MSG_OK       if a job has been executed.
+ * @retval MSG_RESET    if the internal mailbox has been reset.
+ * @retval MSG_JOB_NULL if a @p JOB_NULL has been received.
+ */
+static inline msg_t chJobDispatch(jobs_queue_t *jqp) {
+  msg_t msg, jmsg;
+
+  /* Waiting for a job.*/
+  msg = chMBFetchTimeout(&jqp->mbx, &jmsg, TIME_INFINITE);
+  if (msg == MSG_OK) {
+    job_descriptor_t *jp = (job_descriptor_t *)jmsg;
+
+    chDbgAssert(jp != NULL, "is NULL");
+
+    if (jp->jobfunc != NULL) {
+
+      /* Invoking the job function.*/
+      jp->jobfunc(jp->jobarg);
+
+      /* Returning the job descriptor object.*/
+      chGuardedPoolFree(&jqp->free, (void *)jp);
+    }
+    else {
+      msg = MSG_JOB_NULL;
+    }
+  }
+
+  return msg;
+}
+
+/**
+ * @brief   Waits for a job then executes it.
+ *
+ * @param[in] jqp       pointer to a @p jobs_queue_t 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              The function outcome.
+ * @retval MSG_OK       if a job has been executed.
+ * @retval MSG_TIMEOUT  if a timeout occurred.
+ * @retval MSG_RESET    if the internal mailbox has been reset.
+ * @retval MSG_JOB_NULL if a @p JOB_NULL has been received.
+ */
+static inline msg_t chJobDispatchTimeout(jobs_queue_t *jqp,
+                                         sysinterval_t timeout) {
+  msg_t msg, jmsg;
+
+  /* Waiting for a job or a timeout.*/
+  msg = chMBFetchTimeout(&jqp->mbx, &jmsg, timeout);
+  if (msg == MSG_OK) {
+    job_descriptor_t *jp = (job_descriptor_t *)jmsg;
+
+    chDbgAssert(jp != NULL, "is NULL");
+
+    if (jp->jobfunc != NULL) {
+
+      /* Invoking the job function.*/
+      jp->jobfunc(jp->jobarg);
+
+      /* Returning the job descriptor object.*/
+      chGuardedPoolFree(&jqp->free, (void *)jp);
+    }
+    else {
+      msg = MSG_JOB_NULL;
+    }
+  }
+
+  return msg;
+}
+
+#endif /* CH_CFG_USE_JOBS == TRUE */
+
+#endif /* CHJOBS_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chlib.h b/lib/chibios/os/oslib/include/chlib.h
new file mode 100644
index 000000000..e8ad22f8e
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chlib.h
@@ -0,0 +1,264 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chlib.h
+ * @brief   ChibiOS/LIB main include file.
+ * @details This header includes all the required library headers. This file
+ *          is meant do be included by @p ch.h not directly by user.
+ *
+ * @addtogroup oslib_info
+ * @details OS Library related info.
+ * @{
+ */
+
+#ifndef CHLIB_H
+#define CHLIB_H
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   ChibiOS/LIB identification macro.
+ */
+#define _CHIBIOS_OSLIB_
+
+/**
+ * @brief   Stable release flag.
+ */
+#define CH_OSLIB_STABLE         1
+
+/**
+ * @name    ChibiOS/LIB version identification
+ * @{
+ */
+/**
+ * @brief   OS Library version string.
+ */
+#define CH_OSLIB_VERSION        "1.2.1"
+
+/**
+ * @brief   OS Library version major number.
+ */
+#define CH_OSLIB_MAJOR          1
+
+/**
+ * @brief   OS Library version minor number.
+ */
+#define CH_OSLIB_MINOR          2
+
+/**
+ * @brief   OS Library version patch number.
+ */
+#define CH_OSLIB_PATCH          1
+/** @} */
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/* Host OS checks.*/
+#if !defined(_CHIBIOS_RT_) && !defined(_CHIBIOS_NIL_)
+#error "OS check failed, must be included after ch.h"
+#endif
+
+/* Configuration file checks.*/
+#if !defined(CH_CFG_USE_MAILBOXES)
+#error "CH_CFG_USE_MAILBOXES not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_USE_MEMCORE)
+#error "CH_CFG_USE_MEMCORE not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_USE_HEAP)
+#error "CH_CFG_USE_HEAP not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_USE_MEMPOOLS)
+#error "CH_CFG_USE_MEMPOOLS not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_USE_OBJ_FIFOS)
+#error "CH_CFG_USE_OBJ_FIFOS not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_USE_PIPES)
+#error "CH_CFG_USE_PIPES not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_USE_OBJ_CACHES)
+#error "CH_CFG_USE_OBJ_CACHES not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_USE_DELEGATES)
+#error "CH_CFG_USE_DELEGATES not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_USE_JOBS)
+#error "CH_CFG_USE_JOBS not defined in chconf.h"
+#endif
+
+/* Objects factory options checks.*/
+#if !defined(CH_CFG_USE_FACTORY)
+#error "CH_CFG_USE_FACTORY not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_FACTORY_MAX_NAMES_LENGTH)
+#error "CH_CFG_FACTORY_MAX_NAMES_LENGTH not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_FACTORY_OBJECTS_REGISTRY)
+#error "CH_CFG_FACTORY_OBJECTS_REGISTRY not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_FACTORY_GENERIC_BUFFERS)
+#error "CH_CFG_FACTORY_GENERIC_BUFFERS not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_FACTORY_SEMAPHORES)
+#error "CH_CFG_FACTORY_SEMAPHORES not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_FACTORY_MAILBOXES)
+#error "CH_CFG_FACTORY_MAILBOXES not defined in chconf.h"
+#endif
+
+#if !defined(CH_CFG_FACTORY_OBJ_FIFOS)
+#error "CH_CFG_FACTORY_OBJ_FIFOS not defined in chconf.h"
+#endif
+
+/* License checks.*/
+#if !defined(CH_CUSTOMER_LIC_OSLIB) || !defined(CH_LICENSE_FEATURES)
+#error "malformed chlicense.h"
+#endif
+
+#if (CH_LICENSE_FEATURES != CH_FEATURES_FULL) &&                            \
+    (CH_LICENSE_FEATURES != CH_FEATURES_INTERMEDIATE) &&                    \
+    (CH_LICENSE_FEATURES != CH_FEATURES_BASIC)
+#error "invalid CH_LICENSE_FEATURES setting"
+#endif
+
+/* Restrictions in basic and intermediate modes.*/
+#if (CH_CUSTOMER_LIC_OSLIB == FALSE) ||                                     \
+    (CH_LICENSE_FEATURES == CH_FEATURES_INTERMEDIATE) ||                    \
+    (CH_LICENSE_FEATURES == CH_FEATURES_BASIC)
+
+/* Restricted subsystems.*/
+#undef CH_CFG_USE_FACTORY
+
+#define CH_CFG_USE_FACTORY                  FALSE
+
+#endif /* (CH_CUSTOMER_LIC_OSLIB == FALSE) ||
+          (CH_LICENSE_FEATURES == CH_FEATURES_INTERMEDIATE) ||
+          (CH_LICENSE_FEATURES == CH_FEATURES_BASIC) */
+
+/* Restrictions in basic mode.*/
+#if (CH_CUSTOMER_LIC_OSLIB == FALSE) ||                                     \
+    (CH_LICENSE_FEATURES == CH_FEATURES_BASIC)
+
+/* Restricted subsystems.*/
+#undef CH_CFG_USE_HEAP
+#undef CH_CFG_USE_MEMPOOLS
+#undef CH_CFG_USE_OBJ_FIFOS
+#undef CH_CFG_USE_PIPES
+#undef CH_CFG_USE_OBJ_CACHES
+#undef CH_CFG_USE_DELEGATES
+#undef CH_CFG_USE_JOBS
+
+#define CH_CFG_USE_HEAP                     FALSE
+#define CH_CFG_USE_MEMPOOLS                 FALSE
+#define CH_CFG_USE_OBJ_FIFOS                FALSE
+#define CH_CFG_USE_PIPES                    FALSE
+#define CH_CFG_USE_OBJ_CACHES               FALSE
+#define CH_CFG_USE_DELEGATES                FALSE
+#define CH_CFG_USE_JOBS                     FALSE
+
+#endif /* (CH_CUSTOMER_LIC_OSLIB == FALSE) ||
+          (CH_LICENSE_FEATURES == CH_FEATURES_BASIC) */
+
+/* Restrictions in unlicensed mode.*/
+#if (CH_CUSTOMER_LIC_OSLIB == FALSE)
+
+/* Restricted subsystems.*/
+#undef CH_CFG_USE_MAILBOXES
+
+#define CH_CFG_USE_MAILBOXES                FALSE
+
+#endif /* CH_CUSTOMER_LIC_OSLIB == FALSE */
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Late inclusions.                                                          */
+/*===========================================================================*/
+
+/* OS Library headers.*/
+#include "chbsem.h"
+#include "chmboxes.h"
+#include "chmemcore.h"
+#include "chmemheaps.h"
+#include "chmempools.h"
+#include "chobjfifos.h"
+#include "chpipes.h"
+#include "chobjcaches.h"
+#include "chdelegates.h"
+#include "chjobs.h"
+#include "chfactory.h"
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Initialization of all library modules.
+ *
+ * @notapi
+ */
+static inline void _oslib_init(void) {
+
+#if CH_CFG_USE_MEMCORE == TRUE
+  _core_init();
+#endif
+#if CH_CFG_USE_HEAP == TRUE
+  _heap_init();
+#endif
+#if CH_CFG_USE_FACTORY == TRUE
+  _factory_init();
+#endif
+}
+
+#endif /* CHLIB_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chmboxes.h b/lib/chibios/os/oslib/include/chmboxes.h
new file mode 100644
index 000000000..d925f219e
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chmboxes.h
@@ -0,0 +1,209 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chmboxes.h
+ * @brief   Mailboxes macros and structures.
+ *
+ * @addtogroup oslib_mailboxes
+ * @{
+ */
+
+#ifndef CHMBOXES_H
+#define CHMBOXES_H
+
+#if (CH_CFG_USE_MAILBOXES == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Structure representing a mailbox object.
+ */
+typedef struct {
+  msg_t                 *buffer;        /**< @brief Pointer to the mailbox
+                                                    buffer.                 */
+  msg_t                 *top;           /**< @brief Pointer to the location
+                                                    after the buffer.       */
+  msg_t                 *wrptr;         /**< @brief Write pointer.          */
+  msg_t                 *rdptr;         /**< @brief Read pointer.           */
+  size_t                cnt;            /**< @brief Messages in queue.      */
+  bool                  reset;          /**< @brief True in reset state.    */
+  threads_queue_t       qw;             /**< @brief Queued writers.         */
+  threads_queue_t       qr;             /**< @brief Queued readers.         */
+} mailbox_t;
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/**
+ * @brief   Data part of a static mailbox initializer.
+ * @details This macro should be used when statically initializing a
+ *          mailbox that is part of a bigger structure.
+ *
+ * @param[in] name      the name of the mailbox variable
+ * @param[in] buffer    pointer to the mailbox buffer array of @p msg_t
+ * @param[in] size      number of @p msg_t elements in the buffer array
+ */
+#define _MAILBOX_DATA(name, buffer, size) {                                 \
+  (msg_t *)(buffer),                                                        \
+  (msg_t *)(buffer) + size,                                                 \
+  (msg_t *)(buffer),                                                        \
+  (msg_t *)(buffer),                                                        \
+  (size_t)0,                                                                \
+  false,                                                                    \
+  _THREADS_QUEUE_DATA(name.qw),                                             \
+  _THREADS_QUEUE_DATA(name.qr),                                             \
+}
+
+/**
+ * @brief   Static mailbox initializer.
+ * @details Statically initialized mailboxes require no explicit
+ *          initialization using @p chMBObjectInit().
+ *
+ * @param[in] name      the name of the mailbox variable
+ * @param[in] buffer    pointer to the mailbox buffer array of @p msg_t
+ * @param[in] size      number of @p msg_t elements in the buffer array
+ */
+#define MAILBOX_DECL(name, buffer, size)                                    \
+  mailbox_t name = _MAILBOX_DATA(name, buffer, size)
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void chMBObjectInit(mailbox_t *mbp, msg_t *buf, size_t n);
+  void chMBReset(mailbox_t *mbp);
+  void chMBResetI(mailbox_t *mbp);
+  msg_t chMBPostTimeout(mailbox_t *mbp, msg_t msg, sysinterval_t timeout);
+  msg_t chMBPostTimeoutS(mailbox_t *mbp, msg_t msg, sysinterval_t timeout);
+  msg_t chMBPostI(mailbox_t *mbp, msg_t msg);
+  msg_t chMBPostAheadTimeout(mailbox_t *mbp, msg_t msg, sysinterval_t timeout);
+  msg_t chMBPostAheadTimeoutS(mailbox_t *mbp, msg_t msg, sysinterval_t timeout);
+  msg_t chMBPostAheadI(mailbox_t *mbp, msg_t msg);
+  msg_t chMBFetchTimeout(mailbox_t *mbp, msg_t *msgp, sysinterval_t timeout);
+  msg_t chMBFetchTimeoutS(mailbox_t *mbp, msg_t *msgp, sysinterval_t timeout);
+  msg_t chMBFetchI(mailbox_t *mbp, msg_t *msgp);
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Returns the mailbox buffer size as number of messages.
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ * @return              The size of the mailbox.
+ *
+ * @iclass
+ */
+static inline size_t chMBGetSizeI(const mailbox_t *mbp) {
+
+  /*lint -save -e9033 [10.8] Perfectly safe pointers
+    arithmetic.*/
+  return (size_t)(mbp->top - mbp->buffer);
+  /*lint -restore*/
+}
+
+/**
+ * @brief   Returns the number of used message slots into a mailbox.
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ * @return              The number of queued messages.
+ *
+ * @iclass
+ */
+static inline size_t chMBGetUsedCountI(const mailbox_t *mbp) {
+
+  chDbgCheckClassI();
+
+  return mbp->cnt;
+}
+
+/**
+ * @brief   Returns the number of free message slots into a mailbox.
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ * @return              The number of empty message slots.
+ *
+ * @iclass
+ */
+static inline size_t chMBGetFreeCountI(const mailbox_t *mbp) {
+
+  chDbgCheckClassI();
+
+  return chMBGetSizeI(mbp) - chMBGetUsedCountI(mbp);
+}
+
+/**
+ * @brief   Returns the next message in the queue without removing it.
+ * @pre     A message must be waiting in the queue for this function to work
+ *          or it would return garbage. The correct way to use this macro is
+ *          to use @p chMBGetUsedCountI() and then use this macro, all within
+ *          a lock state.
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ * @return              The next message in queue.
+ *
+ * @iclass
+ */
+static inline msg_t chMBPeekI(const mailbox_t *mbp) {
+
+  chDbgCheckClassI();
+
+  return *mbp->rdptr;
+}
+
+/**
+ * @brief   Terminates the reset state.
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ *
+ * @xclass
+ */
+static inline void chMBResumeX(mailbox_t *mbp) {
+
+  mbp->reset = false;
+}
+
+#endif /* CH_CFG_USE_MAILBOXES == TRUE */
+
+#endif /* CHMBOXES_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chmemcore.h b/lib/chibios/os/oslib/include/chmemcore.h
new file mode 100644
index 000000000..4004f70b7
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chmemcore.h
@@ -0,0 +1,211 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chmemcore.h
+ * @brief   Core memory manager macros and structures.
+ *
+ * @addtogroup oslib_memcore
+ * @{
+ */
+
+#ifndef CHMEMCORE_H
+#define CHMEMCORE_H
+
+#if (CH_CFG_USE_MEMCORE == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Managed RAM size.
+ * @details Size of the RAM area to be managed by the OS. If set to zero
+ *          then the whole available RAM is used. The core memory is made
+ *          available to the heap allocator and/or can be used directly through
+ *          the simplified core memory allocator.
+ *
+ * @note    In order to let the OS manage the whole RAM the linker script must
+ *          provide the @p __heap_base__ and @p __heap_end__ symbols.
+ * @note    Requires @p CH_CFG_USE_MEMCORE.
+ */
+#if !defined(CH_CFG_MEMCORE_SIZE) || defined(__DOXYGEN__)
+#define CH_CFG_MEMCORE_SIZE                 0
+#endif
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if CH_CFG_MEMCORE_SIZE < 0
+#error "invalid CH_CFG_MEMCORE_SIZE value specified"
+#endif
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Memory get function.
+ */
+typedef void *(*memgetfunc_t)(size_t size, unsigned align);
+
+/**
+ * @brief   Enhanced memory get function.
+ */
+typedef void *(*memgetfunc2_t)(size_t size, unsigned align, size_t offset);
+
+/**
+ * @brief   Type of memory core object.
+ */
+typedef struct {
+  /**
+   * @brief   Next free address.
+   */
+  uint8_t *basemem;
+  /**
+   * @brief   Final address.
+   */
+  uint8_t *topmem;
+} memcore_t;
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/**
+ * @brief   Allocates a memory block.
+ * @note    This is a generic form with unspecified allocation position.
+ *
+ * @iclass
+ */
+#define chCoreAllocAlignedWithOffsetI chCoreAllocFromTopI
+
+/**
+ * @brief   Allocates a memory block.
+ * @note    This is a generic form with unspecified allocation position.
+ *
+ * @api
+ */
+#define chCoreAllocAlignedWithOffset chCoreAllocFromTop
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#if !defined(__DOXYGEN__)
+extern memcore_t ch_memcore;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void _core_init(void);
+  void *chCoreAllocFromBaseI(size_t size, unsigned align, size_t offset);
+  void *chCoreAllocFromTopI(size_t size, unsigned align, size_t offset);
+  void *chCoreAllocFromBase(size_t size, unsigned align, size_t offset);
+  void *chCoreAllocFromTop(size_t size, unsigned align, size_t offset);
+  size_t chCoreGetStatusX(void);
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Allocates a memory block.
+ * @details The allocated block is guaranteed to be properly aligned to the
+ *          specified alignment.
+ * @note    This is a generic form with unspecified allocation position.
+ *
+ * @param[in] size      the size of the block to be allocated.
+ * @param[in] align     desired memory alignment
+ * @return              A pointer to the allocated memory block.
+ * @retval NULL         allocation failed, core memory exhausted.
+ *
+ * @iclass
+ */
+static inline void *chCoreAllocAlignedI(size_t size, unsigned align) {
+
+  return chCoreAllocAlignedWithOffsetI(size, align, 0U);
+}
+
+/**
+ * @brief   Allocates a memory block.
+ * @details The allocated block is guaranteed to be properly aligned to the
+ *          specified alignment.
+ * @note    This is a generic form with unspecified allocation position.
+ *
+ * @param[in] size      the size of the block to be allocated
+ * @param[in] align     desired memory alignment
+ * @return              A pointer to the allocated memory block.
+ * @retval NULL         allocation failed, core memory exhausted.
+ *
+ * @api
+ */
+static inline void *chCoreAllocAligned(size_t size, unsigned align) {
+
+  return chCoreAllocAlignedWithOffset(size, align, 0U);
+}
+
+/**
+ * @brief   Allocates a memory block.
+ * @details The allocated block is guaranteed to be properly aligned for a
+ *          pointer data type.
+ * @note    This is a generic form with unspecified allocation position.
+ *
+ * @param[in] size      the size of the block to be allocated.
+ * @return              A pointer to the allocated memory block.
+ * @retval NULL         allocation failed, core memory exhausted.
+ *
+ * @iclass
+ */
+static inline void *chCoreAllocI(size_t size) {
+
+  return chCoreAllocAlignedWithOffsetI(size, PORT_NATURAL_ALIGN, 0U);
+}
+
+/**
+ * @brief   Allocates a memory block.
+ * @details The allocated block is guaranteed to be properly aligned for a
+ *          pointer data type.
+ * @note    This is a generic form with unspecified allocation position.
+ *
+ * @param[in] size      the size of the block to be allocated.
+ * @return              A pointer to the allocated memory block.
+ * @retval NULL         allocation failed, core memory exhausted.
+ *
+ * @api
+ */
+static inline void *chCoreAlloc(size_t size) {
+
+  return chCoreAllocAlignedWithOffset(size, PORT_NATURAL_ALIGN, 0U);
+}
+
+#endif /* CH_CFG_USE_MEMCORE == TRUE */
+
+#endif /* CHMEMCORE_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chmemheaps.h b/lib/chibios/os/oslib/include/chmemheaps.h
new file mode 100644
index 000000000..c0c4b97e0
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chmemheaps.h
@@ -0,0 +1,178 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chmemheaps.h
+ * @brief   Memory heaps macros and structures.
+ *
+ * @addtogroup oslib_memheaps
+ * @{
+ */
+
+#ifndef CHMEMHEAPS_H
+#define CHMEMHEAPS_H
+
+#if (CH_CFG_USE_HEAP == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Minimum alignment used for heap.
+ * @note    Cannot use the sizeof operator in this macro.
+ */
+#if (SIZEOF_PTR == 4) || defined(__DOXYGEN__)
+#define CH_HEAP_ALIGNMENT   8U
+#elif (SIZEOF_PTR == 2)
+#define CH_HEAP_ALIGNMENT   4U
+#else
+#error "unsupported pointer size"
+#endif
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if CH_CFG_USE_MEMCORE == FALSE
+#error "CH_CFG_USE_HEAP requires CH_CFG_USE_MEMCORE"
+#endif
+
+#if (CH_CFG_USE_MUTEXES == FALSE) && (CH_CFG_USE_SEMAPHORES == FALSE)
+#error "CH_CFG_USE_HEAP requires CH_CFG_USE_MUTEXES and/or CH_CFG_USE_SEMAPHORES"
+#endif
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Type of a memory heap.
+ */
+typedef struct memory_heap memory_heap_t;
+
+/**
+ * @brief   Type of a memory heap header.
+ */
+typedef union heap_header heap_header_t;
+
+/**
+ * @brief   Memory heap block header.
+ */
+union heap_header {
+  struct {
+    heap_header_t       *next;      /**< @brief Next block in free list.    */
+    size_t              pages;      /**< @brief Size of the area in pages.  */
+  } free;
+  struct {
+    memory_heap_t       *heap;      /**< @brief Block owner heap.           */
+    size_t              size;       /**< @brief Size of the area in bytes.  */
+  } used;
+};
+
+/**
+ * @brief   Structure describing a memory heap.
+ */
+struct memory_heap {
+  memgetfunc2_t         provider;   /**< @brief Memory blocks provider for
+                                                this heap.                  */
+  heap_header_t         header;     /**< @brief Free blocks list header.    */
+#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
+  mutex_t               mtx;        /**< @brief Heap access mutex.          */
+#else
+  semaphore_t           sem;        /**< @brief Heap access semaphore.      */
+#endif
+};
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/**
+ * @brief   Allocation of an aligned static heap buffer.
+ */
+#define CH_HEAP_AREA(name, size)                                            \
+  ALIGNED_VAR(CH_HEAP_ALIGNMENT)                                            \
+  uint8_t name[MEM_ALIGN_NEXT((size), CH_HEAP_ALIGNMENT)]
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void _heap_init(void);
+  void chHeapObjectInit(memory_heap_t *heapp, void *buf, size_t size);
+  void *chHeapAllocAligned(memory_heap_t *heapp, size_t size, unsigned align);
+  void chHeapFree(void *p);
+  size_t chHeapStatus(memory_heap_t *heapp, size_t *totalp, size_t *largestp);
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @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] heapp     pointer to a heap descriptor or @p NULL in order to
+ *                      access the default heap.
+ * @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.
+ *
+ * @api
+ */
+static inline void *chHeapAlloc(memory_heap_t *heapp, size_t size) {
+
+  return chHeapAllocAligned(heapp, size, CH_HEAP_ALIGNMENT);
+}
+
+/**
+ * @brief   Returns the size of an allocated block.
+ * @note    The returned value is the requested size, the real size is the
+ *          same value aligned to the next @p CH_HEAP_ALIGNMENT multiple.
+ *
+ * @param[in] p         pointer to the memory block
+ * @return              Size of the block.
+ *
+ * @api
+ */
+static inline size_t chHeapGetSize(const void *p) {
+
+  return ((heap_header_t *)p - 1U)->used.size;
+}
+
+#endif /* CH_CFG_USE_HEAP == TRUE */
+
+#endif /* CHMEMHEAPS_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chmempools.h b/lib/chibios/os/oslib/include/chmempools.h
new file mode 100644
index 000000000..52f78e004
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chmempools.h
@@ -0,0 +1,385 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chmempools.h
+ * @brief   Memory Pools macros and structures.
+ *
+ * @addtogroup oslib_mempools
+ * @{
+ */
+
+#ifndef CHMEMPOOLS_H
+#define CHMEMPOOLS_H
+
+#if (CH_CFG_USE_MEMPOOLS == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if CH_CFG_USE_MEMCORE == FALSE
+#error "CH_CFG_USE_MEMPOOLS requires CH_CFG_USE_MEMCORE"
+#endif
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Memory pool free object header.
+ */
+struct pool_header {
+  struct pool_header    *next;          /**< @brief Pointer to the next pool
+                                                    header in the list.     */
+};
+
+/**
+ * @brief   Memory pool descriptor.
+ */
+typedef struct {
+  struct pool_header    *next;          /**< @brief Pointer to the header.  */
+  size_t                object_size;    /**< @brief Memory pool objects
+                                                    size.                   */
+  unsigned              align;          /**< @brief Required alignment.     */
+  memgetfunc_t          provider;       /**< @brief Memory blocks provider
+                                                    for this pool.          */
+} memory_pool_t;
+
+#if (CH_CFG_USE_SEMAPHORES == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Guarded memory pool descriptor.
+ */
+typedef struct {
+  semaphore_t           sem;            /**< @brief Counter semaphore guarding
+                                                    the memory pool.        */
+  memory_pool_t         pool;           /**< @brief The memory pool itself. */
+} guarded_memory_pool_t;
+#endif /* CH_CFG_USE_SEMAPHORES == TRUE */
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/**
+ * @brief   Data part of a static memory pool initializer.
+ * @details This macro should be used when statically initializing a
+ *          memory pool that is part of a bigger structure.
+ *
+ * @param[in] name      the name of the memory pool variable
+ * @param[in] size      size of the memory pool contained objects
+ * @param[in] align     required memory alignment
+ * @param[in] provider  memory provider function for the memory pool
+ */
+#define _MEMORYPOOL_DATA(name, size, align, provider)                       \
+  {NULL, size, align, provider}
+
+/**
+ * @brief   Static memory pool initializer.
+ * @details Statically initialized memory pools require no explicit
+ *          initialization using @p chPoolInit().
+ *
+ * @param[in] name      the name of the memory pool variable
+ * @param[in] size      size of the memory pool contained objects
+ * @param[in] align     required memory alignment
+ * @param[in] provider  memory provider function for the memory pool or @p NULL
+ *                      if the pool is not allowed to grow automatically
+ */
+#define MEMORYPOOL_DECL(name, size, align, provider)                        \
+  memory_pool_t name = _MEMORYPOOL_DATA(name, size, align, provider)
+
+#if (CH_CFG_USE_SEMAPHORES == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Data part of a static guarded memory pool initializer.
+ * @details This macro should be used when statically initializing a
+ *          memory pool that is part of a bigger structure.
+ *
+ * @param[in] name      the name of the memory pool variable
+ * @param[in] size      size of the memory pool contained objects
+ * @param[in] align     required memory alignment
+ */
+#define _GUARDEDMEMORYPOOL_DATA(name, size, align) {                        \
+  _SEMAPHORE_DATA(name.sem, (cnt_t)0),                                      \
+  _MEMORYPOOL_DATA(NULL, size, align, NULL)                                 \
+}
+
+/**
+ * @brief   Static guarded memory pool initializer.
+ * @details Statically initialized guarded memory pools require no explicit
+ *          initialization using @p chGuardedPoolInit().
+ *
+ * @param[in] name      the name of the guarded memory pool variable
+ * @param[in] size      size of the memory pool contained objects
+ * @param[in] align     required memory alignment
+ */
+#define GUARDEDMEMORYPOOL_DECL(name, size, align)                           \
+  guarded_memory_pool_t name = _GUARDEDMEMORYPOOL_DATA(name, size, align)
+#endif /* CH_CFG_USE_SEMAPHORES == TRUE */
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void chPoolObjectInitAligned(memory_pool_t *mp, size_t size,
+                               unsigned align, memgetfunc_t provider);
+  void chPoolLoadArray(memory_pool_t *mp, void *p, size_t n);
+  void *chPoolAllocI(memory_pool_t *mp);
+  void *chPoolAlloc(memory_pool_t *mp);
+  void chPoolFreeI(memory_pool_t *mp, void *objp);
+  void chPoolFree(memory_pool_t *mp, void *objp);
+#if CH_CFG_USE_SEMAPHORES == TRUE
+  void chGuardedPoolObjectInitAligned(guarded_memory_pool_t *gmp,
+                                      size_t size,
+                                      unsigned align);
+  void chGuardedPoolLoadArray(guarded_memory_pool_t *gmp, void *p, size_t n);
+  void *chGuardedPoolAllocTimeoutS(guarded_memory_pool_t *gmp,
+                                   sysinterval_t timeout);
+  void *chGuardedPoolAllocTimeout(guarded_memory_pool_t *gmp,
+                                  sysinterval_t timeout);
+  void chGuardedPoolFree(guarded_memory_pool_t *gmp, void *objp);
+#endif
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Initializes an empty memory pool.
+ *
+ * @param[out] mp       pointer to a @p memory_pool_t 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.
+ * @param[in] provider  memory provider function for the memory pool or
+ *                      @p NULL if the pool is not allowed to grow
+ *                      automatically
+ *
+ * @init
+ */
+static inline void chPoolObjectInit(memory_pool_t *mp,
+                                    size_t size,
+                                    memgetfunc_t provider) {
+
+  chPoolObjectInitAligned(mp, size, PORT_NATURAL_ALIGN, provider);
+}
+
+/**
+ * @brief   Adds an object to a memory pool.
+ * @pre     The memory pool must be already been initialized.
+ * @pre     The added object must be of the right size for the specified
+ *          memory pool.
+ * @pre     The added object must be properly aligned.
+ * @note    This function is just an alias for @p chPoolFree() and has been
+ *          added for clarity.
+ *
+ * @param[in] mp        pointer to a @p memory_pool_t structure
+ * @param[in] objp      the pointer to the object to be added
+ *
+ * @api
+ */
+static inline void chPoolAdd(memory_pool_t *mp, void *objp) {
+
+  chPoolFree(mp, objp);
+}
+
+/**
+ * @brief   Adds an object to a memory pool.
+ * @pre     The memory pool must be already been initialized.
+ * @pre     The added object must be of the right size for the specified
+ *          memory pool.
+ * @pre     The added object must be properly aligned.
+ * @note    This function is just an alias for @p chPoolFreeI() and has been
+ *          added for clarity.
+ *
+ * @param[in] mp        pointer to a @p memory_pool_t structure
+ * @param[in] objp      the pointer to the object to be added
+ *
+ * @iclass
+ */
+static inline void chPoolAddI(memory_pool_t *mp, void *objp) {
+
+  chPoolFreeI(mp, objp);
+}
+
+#if (CH_CFG_USE_SEMAPHORES == TRUE) || defined(__DOXYGEN__)
+/**
+ * @brief   Initializes an empty guarded memory pool.
+ *
+ * @param[out] gmp      pointer to a @p guarded_memory_pool_t structure
+ * @param[in] size      the size of the objects contained in this guarded
+ *                      memory pool, the minimum accepted size is the size
+ *                      of a pointer to void.
+ *
+ * @init
+ */
+static inline void chGuardedPoolObjectInit(guarded_memory_pool_t *gmp,
+                                           size_t size) {
+
+  chGuardedPoolObjectInitAligned(gmp, size, PORT_NATURAL_ALIGN);
+}
+
+/**
+ * @brief   Gets the count of objects in a guarded memory pool.
+ * @pre     The guarded memory pool must be already been initialized.
+ *
+ * @param[in] gmp       pointer to a @p guarded_memory_pool_t structure
+ * @return              The counter of the guard semaphore.
+ *
+ * @iclass
+ */
+static inline cnt_t chGuardedPoolGetCounterI(guarded_memory_pool_t *gmp) {
+
+  return chSemGetCounterI(&gmp->sem);
+}
+
+/**
+ * @brief   Allocates an object from a guarded memory pool.
+ * @pre     The guarded memory pool must be already been initialized.
+ *
+ * @param[in] gmp       pointer to a @p guarded_memory_pool_t structure
+ * @return              The pointer to the allocated object.
+ * @retval NULL         if the pool is empty.
+ *
+ * @iclass
+ */
+static inline void *chGuardedPoolAllocI(guarded_memory_pool_t *gmp) {
+  void *p;
+
+  p = chPoolAllocI(&gmp->pool);
+  if (p != NULL) {
+    chSemFastWaitI(&gmp->sem);
+    chDbgAssert(chSemGetCounterI(&gmp->sem) >= (cnt_t)0,
+                "semaphore out of sync");
+  }
+  return p;
+}
+
+/**
+ * @brief   Releases an object into a guarded memory pool.
+ * @pre     The guarded memory pool must already be initialized.
+ * @pre     The freed object must be of the right size for the specified
+ *          guarded memory pool.
+ * @pre     The added object must be properly aligned.
+ *
+ * @param[in] gmp       pointer to a @p guarded_memory_pool_t structure
+ * @param[in] objp      the pointer to the object to be released
+ *
+ * @iclass
+ */
+static inline void chGuardedPoolFreeI(guarded_memory_pool_t *gmp, void *objp) {
+
+  chPoolFreeI(&gmp->pool, objp);
+  chSemSignalI(&gmp->sem);
+}
+
+/**
+ * @brief   Releases an object into a guarded memory pool.
+ * @pre     The guarded memory pool must already be initialized.
+ * @pre     The freed object must be of the right size for the specified
+ *          guarded memory pool.
+ * @pre     The added object must be properly aligned.
+ *
+ * @param[in] gmp       pointer to a @p guarded_memory_pool_t structure
+ * @param[in] objp      the pointer to the object to be released
+ *
+ * @sclass
+ */
+static inline void chGuardedPoolFreeS(guarded_memory_pool_t *gmp, void *objp) {
+
+  chGuardedPoolFreeI(gmp, objp);
+  chSchRescheduleS();
+}
+
+/**
+ * @brief   Adds an object to a guarded memory pool.
+ * @pre     The guarded memory pool must be already been initialized.
+ * @pre     The added object must be of the right size for the specified
+ *          guarded memory pool.
+ * @pre     The added object must be properly aligned.
+ * @note    This function is just an alias for @p chGuardedPoolFree() and
+ *          has been added for clarity.
+ *
+ * @param[in] gmp       pointer to a @p guarded_memory_pool_t structure
+ * @param[in] objp      the pointer to the object to be added
+ *
+ * @api
+ */
+static inline void chGuardedPoolAdd(guarded_memory_pool_t *gmp, void *objp) {
+
+  chGuardedPoolFree(gmp, objp);
+}
+
+/**
+ * @brief   Adds an object to a guarded memory pool.
+ * @pre     The guarded memory pool must be already been initialized.
+ * @pre     The added object must be of the right size for the specified
+ *          guarded memory pool.
+ * @pre     The added object must be properly aligned.
+ * @note    This function is just an alias for @p chGuardedPoolFreeI() and
+ *          has been added for clarity.
+ *
+ * @param[in] gmp       pointer to a @p guarded_memory_pool_t structure
+ * @param[in] objp      the pointer to the object to be added
+ *
+ * @iclass
+ */
+static inline void chGuardedPoolAddI(guarded_memory_pool_t *gmp, void *objp) {
+
+  chGuardedPoolFreeI(gmp, objp);
+}
+
+/**
+ * @brief   Adds an object to a guarded memory pool.
+ * @pre     The guarded memory pool must be already been initialized.
+ * @pre     The added object must be of the right size for the specified
+ *          guarded memory pool.
+ * @pre     The added object must be properly aligned.
+ * @note    This function is just an alias for @p chGuardedPoolFreeI() and
+ *          has been added for clarity.
+ *
+ * @param[in] gmp       pointer to a @p guarded_memory_pool_t structure
+ * @param[in] objp      the pointer to the object to be added
+ *
+ * @sclass
+ */
+static inline void chGuardedPoolAddS(guarded_memory_pool_t *gmp, void *objp) {
+
+  chGuardedPoolFreeS(gmp, objp);
+}
+#endif /* CH_CFG_USE_SEMAPHORES == TRUE */
+
+#endif /* CH_CFG_USE_MEMPOOLS == TRUE */
+
+#endif /* CHMEMPOOLS_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chobjcaches.h b/lib/chibios/os/oslib/include/chobjcaches.h
new file mode 100644
index 000000000..9d8768e21
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chobjcaches.h
@@ -0,0 +1,304 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chobjcaches.h
+ * @brief   Objects Caches macros and structures.
+ *
+ * @addtogroup oslib_objchaches
+ * @{
+ */
+
+#ifndef CHOBJCACHES_H
+#define CHOBJCACHES_H
+
+#if (CH_CFG_USE_OBJ_CACHES == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/**
+ * @name    Cached objects flags
+ * @{
+ */
+#define OC_FLAG_INLRU                       0x00000001U
+#define OC_FLAG_INHASH                      0x00000002U
+#define OC_FLAG_SHARED                      0x00000004U
+#define OC_FLAG_NOTSYNC                     0x00000008U
+#define OC_FLAG_LAZYWRITE                   0x00000010U
+#define OC_FLAG_FORGET                      0x00000020U
+/** @} */
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Flags of cached objects.
+ */
+typedef uint32_t oc_flags_t;
+
+/**
+ * @brief   Type of an hash element header.
+ */
+typedef struct ch_oc_hash_header oc_hash_header_t;
+
+/**
+ * @brief   Type of an LRU element header.
+ */
+typedef struct ch_oc_lru_header oc_lru_header_t;
+
+/**
+ * @brief   Type of a cached object.
+ */
+typedef struct ch_oc_object oc_object_t;
+
+/**
+ * @brief   Type of a cache object.
+ */
+typedef struct ch_objects_cache objects_cache_t;
+
+/**
+ * @brief   Object read function.
+ *
+ * @param[in] ocp       pointer to the @p objects_cache_t structure
+ * @param[in] async     requests an asynchronous operation if supported, the
+ *                      function is then responsible for releasing the
+ *                      object
+ */
+typedef bool (*oc_readf_t)(objects_cache_t *ocp,
+                           oc_object_t *objp,
+                           bool async);
+
+/**
+ * @brief   Object write function.
+ *
+ * @param[in] ocp       pointer to the @p objects_cache_t structure
+ * @param[in] async     requests an asynchronous operation if supported, the
+ *                      function is then responsible for releasing the
+ *                      object
+ */
+typedef bool (*oc_writef_t)(objects_cache_t *ocp,
+                            oc_object_t *objp,
+                            bool async);
+
+/**
+ * @brief   Structure representing an hash table element.
+ */
+struct ch_oc_hash_header {
+  /**
+   * @brief   Next in the collisions list.
+   */
+  oc_object_t           *hash_next;
+  /**
+   * @brief   Previous in the collisions list.
+   */
+  oc_object_t           *hash_prev;
+};
+
+/**
+ * @brief   Structure representing an hash table element.
+ */
+struct ch_oc_lru_header {
+  /**
+   * @brief   Next in the collisions list.
+   */
+  oc_object_t           *hash_next;
+  /**
+   * @brief   Previous in the collisions list.
+   */
+  oc_object_t           *hash_prev;
+  /**
+   * @brief   Next in the LRU list.
+   */
+  oc_object_t           *lru_next;
+  /**
+   * @brief   Previous in the LRU list.
+   */
+  oc_object_t           *lru_prev;
+};
+
+/**
+ * @brief   Structure representing a cached object.
+ */
+struct ch_oc_object {
+  /**
+   * @brief   Next in the collisions list.
+   */
+  oc_object_t           *hash_next;
+  /**
+   * @brief   Previous in the collisions list.
+   */
+  oc_object_t           *hash_prev;
+  /**
+   * @brief   Next in the LRU list.
+   */
+  oc_object_t           *lru_next;
+  /**
+   * @brief   Previous in the LRU list.
+   */
+  oc_object_t           *lru_prev;
+  /**
+   * @brief   Object group.
+   */
+  uint32_t              obj_group;
+  /**
+   * @brief   Object key.
+   */
+  uint32_t              obj_key;
+  /**
+   * @brief   Semaphore for object access.
+   */
+  semaphore_t           obj_sem;
+  /**
+   * @brief   Object flags.
+   */
+  oc_flags_t            obj_flags;
+  /**
+   * @brief   User pointer.
+   * @note    This pointer can be used to refer to external buffers,
+   *          @p chCacheObjectInit() initializes it to @p NULL.
+   */
+  void                  *dptr;
+};
+
+/**
+ * @brief   Structure representing a cache object.
+ */
+struct ch_objects_cache {
+  /**
+   * @brief   Number of elements in the hash table.
+   */
+  ucnt_t                hashn;
+  /**
+   * @brief   Pointer to the hash table.
+   */
+  oc_hash_header_t      *hashp;
+  /**
+   * @brief   Number of elements in the objects table.
+   */
+  ucnt_t                objn;
+  /**
+   * @brief   Size of elements in the objects table.
+   */
+  size_t                objsz;
+  /**
+   * @brief   Pointer to the objects table.
+   */
+  void                  *objvp;
+  /**
+   * @brief   LRU list header.
+   */
+  oc_lru_header_t       lru;
+  /**
+   * @brief   Semaphore for cache access.
+   */
+  semaphore_t           cache_sem;
+  /**
+   * @brief   Semaphore for LRU access.
+   */
+  semaphore_t           lru_sem;
+  /**
+   * @brief   Reader functions for cached objects.
+   */
+  oc_readf_t            readf;
+  /**
+   * @brief   Writer functions for cached objects.
+   */
+  oc_writef_t           writef;
+};
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void chCacheObjectInit(objects_cache_t *ocp,
+                         ucnt_t hashn,
+                         oc_hash_header_t *hashp,
+                         ucnt_t objn,
+                         size_t objsz,
+                         void *objvp,
+                         oc_readf_t readf,
+                         oc_writef_t writef);
+  oc_object_t *chCacheGetObject(objects_cache_t *ocp,
+                                uint32_t group,
+                                uint32_t key);
+  void chCacheReleaseObjectI(objects_cache_t *ocp,
+                             oc_object_t *objp);
+  bool chCacheReadObject(objects_cache_t *ocp,
+                         oc_object_t *objp,
+                         bool async);
+  bool chCacheWriteObject(objects_cache_t *ocp,
+                          oc_object_t *objp,
+                          bool async);
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Releases an object into the cache.
+ * @note    This function gives a meaning to the following flags:
+ *          - @p OC_FLAG_INLRU must be cleared.
+ *          - @p OC_FLAG_INHASH must be set.
+ *          - @p OC_FLAG_SHARED must be cleared.
+ *          - @p OC_FLAG_NOTSYNC invalidates the object and queues it on
+ *            the LRU tail.
+ *          - @p OC_FLAG_LAZYWRITE is ignored and kept, a write will occur
+ *            when the object is removed from the LRU list (lazy write).
+ *          .
+ *
+ * @param[in] ocp       pointer to the @p objects_cache_t structure
+ * @param[in] objp      pointer to the @p oc_object_t structure
+ *
+ * @api
+ */
+static inline void chCacheReleaseObject(objects_cache_t *ocp,
+                                        oc_object_t *objp) {
+
+  chSysLock();
+  chCacheReleaseObjectI(ocp, objp);
+  chSchRescheduleS();
+  chSysUnlock();
+}
+
+#endif /* CH_CFG_USE_OBJ_CACHES == TRUE */
+
+#endif /* CHOBJCACHES_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chobjfifos.h b/lib/chibios/os/oslib/include/chobjfifos.h
new file mode 100644
index 000000000..56cec3634
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chobjfifos.h
@@ -0,0 +1,430 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chobjfifos.h
+ * @brief   Objects FIFO structures and macros.
+ * @details This module implements a generic FIFO queue of objects by
+ *          coupling a Guarded Memory Pool (for objects storage) and
+ *          a MailBox.<br>
+ *          On the sender side free objects are taken from the pool, filled
+ *          and then sent to the receiver, on the receiver side objects are
+ *          fetched, used and then returned to the pool.
+ *          Operations defined for object FIFOs:
+ *          - <b>Take</b>: An object is taken from the pool of the free
+ *            objects, can be blocking.
+ *          - <b>Return</b>: An object is returned to the pool of the
+ *            free objects, it is guaranteed to be non-blocking.
+ *          - <b>Send</b>: An object is sent through the mailbox, it is
+ *            guaranteed to be non-blocking
+ *          - <b>Receive</b>: An object is received from the mailbox,
+ *            can be blocking.
+ *          .
+ *
+ * @addtogroup oslib_objects_fifos
+ * @{
+ */
+
+#ifndef CHOBJFIFOS_H
+#define CHOBJFIFOS_H
+
+#if (CH_CFG_USE_OBJ_FIFOS == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if CH_CFG_USE_MEMPOOLS == FALSE
+#error "CH_CFG_USE_OBJ_FIFOS requires CH_CFG_USE_MEMPOOLS"
+#endif
+
+#if CH_CFG_USE_SEMAPHORES == FALSE
+#error "CH_CFG_USE_OBJ_FIFOS requires CH_CFG_USE_SEMAPHORES"
+#endif
+
+#if CH_CFG_USE_MAILBOXES == FALSE
+#error "CH_CFG_USE_OBJ_FIFOS requires CH_CFG_USE_MAILBOXES"
+#endif
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Type of an objects FIFO.
+ */
+typedef struct ch_objects_fifo {
+  /**
+   * @brief   Pool of the free objects.
+   */
+  guarded_memory_pool_t     free;
+  /**
+   * @brief   Mailbox of the sent objects.
+   */
+  mailbox_t                 mbx;
+} objects_fifo_t;
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Initializes a FIFO object.
+ * @pre     The messages size must be a multiple of the alignment
+ *          requirement.
+ *
+ * @param[out] ofp      pointer to a @p objects_fifo_t structure
+ * @param[in] objsize   size of objects
+ * @param[in] objn      number of objects available
+ * @param[in] objalign  required objects alignment
+ * @param[in] objbuf    pointer to the buffer of objects, it must be able
+ *                      to hold @p objn objects of @p objsize size with
+ *                      @p objalign alignment
+ * @param[in] msgbuf    pointer to the buffer of messages, it must be able
+ *                      to hold @p objn messages
+ *
+ * @init
+ */
+static inline void chFifoObjectInitAligned(objects_fifo_t *ofp, size_t objsize,
+                                           size_t objn, unsigned objalign,
+                                           void *objbuf, msg_t *msgbuf) {
+
+  chDbgCheck((objsize >= objalign) && ((objsize % objalign) == 0U));
+
+  chGuardedPoolObjectInitAligned(&ofp->free, objsize, objalign);
+  chGuardedPoolLoadArray(&ofp->free, objbuf, objn);
+  chMBObjectInit(&ofp->mbx, msgbuf, objn);
+}
+
+/**
+ * @brief   Initializes a FIFO object.
+ * @pre     The messages size must be a multiple of the alignment
+ *          requirement.
+ *
+ * @param[out] ofp      pointer to a @p objects_fifo_t structure
+ * @param[in] objsize   size of objects
+ * @param[in] objn      number of objects available
+ * @param[in] objbuf    pointer to the buffer of objects, it must be able
+ *                      to hold @p objn objects of @p objsize size
+ * @param[in] msgbuf    pointer to the buffer of messages, it must be able
+ *                      to hold @p objn messages
+ *
+ * @init
+ */
+static inline void chFifoObjectInit(objects_fifo_t *ofp, size_t objsize,
+                                    size_t objn, void *objbuf,
+                                    msg_t *msgbuf) {
+
+  chFifoObjectInitAligned(ofp, objsize, objn,
+                          PORT_NATURAL_ALIGN,
+                          objbuf, msgbuf);
+}
+
+/**
+ * @brief   Allocates a free object.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @return              The pointer to the allocated object.
+ * @retval NULL         if an object is not immediately available.
+ *
+ * @iclass
+ */
+static inline void *chFifoTakeObjectI(objects_fifo_t *ofp) {
+
+  return chGuardedPoolAllocI(&ofp->free);
+}
+
+/**
+ * @brief   Allocates a free object.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t 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              The pointer to the allocated object.
+ * @retval NULL         if an object is not available within the specified
+ *                      timeout.
+ *
+ * @sclass
+ */
+static inline void *chFifoTakeObjectTimeoutS(objects_fifo_t *ofp,
+                                             sysinterval_t timeout) {
+
+  return chGuardedPoolAllocTimeoutS(&ofp->free, timeout);
+}
+
+/**
+ * @brief   Allocates a free object.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t 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              The pointer to the allocated object.
+ * @retval NULL         if an object is not available within the specified
+ *                      timeout.
+ *
+ * @api
+ */
+static inline void *chFifoTakeObjectTimeout(objects_fifo_t *ofp,
+                                            sysinterval_t timeout) {
+
+  return chGuardedPoolAllocTimeout(&ofp->free, timeout);
+}
+
+/**
+ * @brief   Releases a fetched object.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objp      pointer to the object to be released
+ *
+ * @iclass
+ */
+static inline void chFifoReturnObjectI(objects_fifo_t *ofp,
+                                       void *objp) {
+
+  chGuardedPoolFreeI(&ofp->free, objp);
+}
+
+/**
+ * @brief   Releases a fetched object.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objp      pointer to the object to be released
+ *
+ * @sclass
+ */
+static inline void chFifoReturnObjectS(objects_fifo_t *ofp,
+                                       void *objp) {
+
+  chGuardedPoolFreeS(&ofp->free, objp);
+}
+
+/**
+ * @brief   Releases a fetched object.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objp      pointer to the object to be released
+ *
+ * @api
+ */
+static inline void chFifoReturnObject(objects_fifo_t *ofp,
+                                      void *objp) {
+
+  chGuardedPoolFree(&ofp->free, objp);
+}
+
+/**
+ * @brief   Posts an object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objp      pointer to the object to be posted
+ *
+ * @iclass
+ */
+static inline void chFifoSendObjectI(objects_fifo_t *ofp,
+                                     void *objp) {
+  msg_t msg;
+
+  msg = chMBPostI(&ofp->mbx, (msg_t)objp);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts an object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objp      pointer to the object to be posted
+ *
+ * @sclass
+ */
+static inline void chFifoSendObjectS(objects_fifo_t *ofp,
+                                     void *objp) {
+  msg_t msg;
+
+  msg = chMBPostTimeoutS(&ofp->mbx, (msg_t)objp, TIME_IMMEDIATE);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts an object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objp      pointer to the object to be released
+ *
+ * @api
+ */
+static inline void chFifoSendObject(objects_fifo_t *ofp, void *objp) {
+
+  msg_t msg;
+
+  msg = chMBPostTimeout(&ofp->mbx, (msg_t)objp, TIME_IMMEDIATE);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts an high priority object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objp      pointer to the object to be posted
+ *
+ * @iclass
+ */
+static inline void chFifoSendObjectAheadI(objects_fifo_t *ofp,
+                                          void *objp) {
+  msg_t msg;
+
+  msg = chMBPostAheadI(&ofp->mbx, (msg_t)objp);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts an high priority object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objp      pointer to the object to be posted
+ *
+ * @sclass
+ */
+static inline void chFifoSendObjectAheadS(objects_fifo_t *ofp,
+                                          void *objp) {
+  msg_t msg;
+
+  msg = chMBPostAheadTimeoutS(&ofp->mbx, (msg_t)objp, TIME_IMMEDIATE);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Posts an high priority object.
+ * @note    By design the object can be always immediately posted.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objp      pointer to the object to be released
+ *
+ * @api
+ */
+static inline void chFifoSendObjectAhead(objects_fifo_t *ofp, void *objp) {
+
+  msg_t msg;
+
+  msg = chMBPostAheadTimeout(&ofp->mbx, (msg_t)objp, TIME_IMMEDIATE);
+  chDbgAssert(msg == MSG_OK, "post failed");
+}
+
+/**
+ * @brief   Fetches an object.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objpp     pointer to the fetched object reference
+ * @return              The operation status.
+ * @retval MSG_OK       if an object has been correctly fetched.
+ * @retval MSG_TIMEOUT  if the FIFO is empty and a message cannot be fetched.
+ *
+ * @iclass
+ */
+static inline msg_t chFifoReceiveObjectI(objects_fifo_t *ofp,
+                                         void **objpp) {
+
+  return chMBFetchI(&ofp->mbx, (msg_t *)objpp);
+}
+
+/**
+ * @brief   Fetches an object.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objpp     pointer to the fetched object reference
+ * @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 MSG_OK       if an object has been correctly fetched.
+ * @retval MSG_TIMEOUT  if the operation has timed out.
+ *
+ * @sclass
+ */
+static inline msg_t chFifoReceiveObjectTimeoutS(objects_fifo_t *ofp,
+                                                void **objpp,
+                                                sysinterval_t timeout) {
+
+  return chMBFetchTimeoutS(&ofp->mbx, (msg_t *)objpp, timeout);
+}
+
+/**
+ * @brief   Fetches an object.
+ *
+ * @param[in] ofp       pointer to a @p objects_fifo_t structure
+ * @param[in] objpp     pointer to the fetched object reference
+ * @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 MSG_OK       if an object has been correctly fetched.
+ * @retval MSG_TIMEOUT  if the operation has timed out.
+ *
+ * @api
+ */
+static inline msg_t chFifoReceiveObjectTimeout(objects_fifo_t *ofp,
+                                               void **objpp,
+                                               sysinterval_t timeout) {
+
+  return chMBFetchTimeout(&ofp->mbx, (msg_t *)objpp, timeout);
+}
+
+#endif /* CH_CFG_USE_OBJ_FIFOS == TRUE */
+
+#endif /* CHOBJFIFOS_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/include/chpipes.h b/lib/chibios/os/oslib/include/chpipes.h
new file mode 100644
index 000000000..66fb70284
--- /dev/null
+++ b/lib/chibios/os/oslib/include/chpipes.h
@@ -0,0 +1,208 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/include/chpipes.h
+ * @brief   Pipes macros and structures.
+ *
+ * @addtogroup oslib_pipes
+ * @{
+ */
+
+#ifndef CHPIPES_H
+#define CHPIPES_H
+
+#if (CH_CFG_USE_PIPES == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Structure representing a pipe object.
+ */
+typedef struct {
+  uint8_t               *buffer;        /**< @brief Pointer to the pipe
+                                                    buffer.                 */
+  uint8_t               *top;           /**< @brief Pointer to the location
+                                                    after the buffer.       */
+  uint8_t               *wrptr;         /**< @brief Write pointer.          */
+  uint8_t               *rdptr;         /**< @brief Read pointer.           */
+  size_t                cnt;            /**< @brief Bytes in the pipe.      */
+  bool                  reset;          /**< @brief True if in reset state. */
+  thread_reference_t    wtr;            /**< @brief Waiting writer.         */
+  thread_reference_t    rtr;            /**< @brief Waiting reader.         */
+#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
+  mutex_t               cmtx;           /**< @brief Common access mutex.    */
+  mutex_t               wmtx;           /**< @brief Write access mutex.     */
+  mutex_t               rmtx;           /**< @brief Read access mutex.      */
+#else
+  semaphore_t           csem;           /**< @brief Common access semaphore.*/
+  semaphore_t           wsem;           /**< @brief Write access semaphore. */
+  semaphore_t           rsem;           /**< @brief Read access semaphore.  */
+#endif
+} pipe_t;
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/**
+ * @brief   Data part of a static pipe initializer.
+ * @details This macro should be used when statically initializing a
+ *          pipe that is part of a bigger structure.
+ *
+ * @param[in] name      the name of the pipe variable
+ * @param[in] buffer    pointer to the pipe buffer array of @p uint8_t
+ * @param[in] size      number of @p uint8_t elements in the buffer array
+ */
+#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
+#define _PIPE_DATA(name, buffer, size) {                                    \
+  (uint8_t *)(buffer),                                                      \
+  (uint8_t *)(buffer) + size,                                               \
+  (uint8_t *)(buffer),                                                      \
+  (uint8_t *)(buffer),                                                      \
+  (size_t)0,                                                                \
+  false,                                                                    \
+  NULL,                                                                     \
+  NULL,                                                                     \
+  _MUTEX_DATA(name.cmtx),                                                   \
+  _MUTEX_DATA(name.wmtx),                                                   \
+  _MUTEX_DATA(name.rmtx),                                                   \
+}
+#else /* CH_CFG_USE_MUTEXES == FALSE */
+#define _PIPE_DATA(name, buffer, size) {                                    \
+  (uint8_t *)(buffer),                                                      \
+  (uint8_t *)(buffer) + size,                                               \
+  (uint8_t *)(buffer),                                                      \
+  (uint8_t *)(buffer),                                                      \
+  (size_t)0,                                                                \
+  false,                                                                    \
+  NULL,                                                                     \
+  NULL,                                                                     \
+  _SEMAPHORE_DATA(name.csem, (cnt_t)1),                                     \
+  _SEMAPHORE_DATA(name.wsem, (cnt_t)1),                                     \
+  _SEMAPHORE_DATA(name.rsem, (cnt_t)1),                                     \
+}
+#endif /* CH_CFG_USE_MUTEXES == FALSE */
+
+/**
+ * @brief   Static pipe initializer.
+ * @details Statically initialized pipes require no explicit
+ *          initialization using @p chPipeObjectInit().
+ *
+ * @param[in] name      the name of the pipe variable
+ * @param[in] buffer    pointer to the pipe buffer array of @p uint8_t
+ * @param[in] size      number of @p uint8_t elements in the buffer array
+ */
+#define PIPE_DECL(name, buffer, size)                                       \
+  pipe_t name = _PIPE_DATA(name, buffer, size)
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void chPipeObjectInit(pipe_t *pp, uint8_t *buf, size_t n);
+  void chPipeReset(pipe_t *pp);
+  size_t chPipeWriteTimeout(pipe_t *pp, const uint8_t *bp,
+                            size_t n, sysinterval_t timeout);
+  size_t chPipeReadTimeout(pipe_t *pp, uint8_t *bp,
+                           size_t n, sysinterval_t timeout);
+#ifdef __cplusplus
+}
+#endif
+
+/*===========================================================================*/
+/* Module inline functions.                                                  */
+/*===========================================================================*/
+
+/**
+ * @brief   Returns the pipe buffer size as number of bytes.
+ *
+ * @param[in] pp        the pointer to an initialized @p pipe_t object
+ * @return              The size of the pipe.
+ *
+ * @api
+ */
+static inline size_t chPipeGetSize(const pipe_t *pp) {
+
+  /*lint -save -e9033 [10.8] Perfectly safe pointers
+    arithmetic.*/
+  return (size_t)(pp->top - pp->buffer);
+  /*lint -restore*/
+}
+
+/**
+ * @brief   Returns the number of used byte slots into a pipe.
+ *
+ * @param[in] pp        the pointer to an initialized @p pipe_t object
+ * @return              The number of queued bytes.
+ *
+ * @api
+ */
+static inline size_t chPipeGetUsedCount(const pipe_t *pp) {
+
+  return pp->cnt;
+}
+
+/**
+ * @brief   Returns the number of free byte slots into a pipe.
+ *
+ * @param[in] pp        the pointer to an initialized @p pipe_t object
+ * @return              The number of empty byte slots.
+ *
+ * @api
+ */
+static inline size_t chPipeGetFreeCount(const pipe_t *pp) {
+
+  return chPipeGetSize(pp) - chPipeGetUsedCount(pp);
+}
+
+/**
+ * @brief   Terminates the reset state.
+ *
+ * @param[in] pp        the pointer to an initialized @p pipe_t object
+ *
+ * @api
+ */
+static inline void chPipeResume(pipe_t *pp) {
+
+  pp->reset = false;
+}
+
+#endif /* CH_CFG_USE_PIPES == TRUE */
+
+#endif /* CHPIPES_H */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/oslib.mk b/lib/chibios/os/oslib/oslib.mk
new file mode 100644
index 000000000..e9fe2db9c
--- /dev/null
+++ b/lib/chibios/os/oslib/oslib.mk
@@ -0,0 +1,57 @@
+# List of all the ChibiOS/LIB files, there is no need to remove the files
+# from this list, you can disable parts of the kernel by editing chlibconf.h.
+ifeq ($(USE_SMART_BUILD),yes)
+
+# Configuration files directory
+ifeq ($(CHCONFDIR),)
+  ifeq ($(CONFDIR),)
+    CHCONFDIR = .
+  else
+    CHCONFDIR := $(CONFDIR)
+  endif
+endif
+
+CHLIBCONF := $(strip $(shell cat $(CHCONFDIR)/chconf.h | egrep -e "\#define"))
+
+LIBSRC :=
+ifneq ($(findstring CH_CFG_USE_MAILBOXES TRUE,$(CHLIBCONF)),)
+LIBSRC += $(CHIBIOS)/os/oslib/src/chmboxes.c
+endif
+ifneq ($(findstring CH_CFG_USE_MEMCORE TRUE,$(CHLIBCONF)),)
+LIBSRC += $(CHIBIOS)/os/oslib/src/chmemcore.c
+endif
+ifneq ($(findstring CH_CFG_USE_HEAP TRUE,$(CHLIBCONF)),)
+LIBSRC += $(CHIBIOS)/os/oslib/src/chmemheaps.c
+endif
+ifneq ($(findstring CH_CFG_USE_MEMPOOLS TRUE,$(CHLIBCONF)),)
+LIBSRC += $(CHIBIOS)/os/oslib/src/chmempools.c
+endif
+ifneq ($(findstring CH_CFG_USE_PIPES TRUE,$(CHLIBCONF)),)
+LIBSRC += $(CHIBIOS)/os/oslib/src/chpipes.c
+endif
+ifneq ($(findstring CH_CFG_USE_OBJ_CACHES TRUE,$(CHLIBCONF)),)
+LIBSRC += $(CHIBIOS)/os/oslib/src/chobjcaches.c
+endif
+ifneq ($(findstring CH_CFG_USE_DELEGATES TRUE,$(CHLIBCONF)),)
+LIBSRC += $(CHIBIOS)/os/oslib/src/chdelegates.c
+endif
+ifneq ($(findstring CH_CFG_USE_FACTORY TRUE,$(CHLIBCONF)),)
+LIBSRC += $(CHIBIOS)/os/oslib/src/chfactory.c
+endif
+else
+LIBSRC := $(CHIBIOS)/os/oslib/src/chmboxes.c \
+          $(CHIBIOS)/os/oslib/src/chmemcore.c \
+          $(CHIBIOS)/os/oslib/src/chmemheaps.c \
+          $(CHIBIOS)/os/oslib/src/chmempools.c \
+          $(CHIBIOS)/os/oslib/src/chpipes.c \
+          $(CHIBIOS)/os/oslib/src/chobjcaches.c \
+          $(CHIBIOS)/os/oslib/src/chdelegates.c \
+          $(CHIBIOS)/os/oslib/src/chfactory.c
+endif
+
+# Required include directories
+LIBINC := $(CHIBIOS)/os/oslib/include
+
+# Shared variables
+ALLCSRC += $(LIBSRC)
+ALLINC  += $(LIBINC)
diff --git a/lib/chibios/os/oslib/readme.txt b/lib/chibios/os/oslib/readme.txt
new file mode 100644
index 000000000..ec43d5af2
--- /dev/null
+++ b/lib/chibios/os/oslib/readme.txt
@@ -0,0 +1,2 @@
+All the code contained under ./os/lib are high level RTOS extension modules
+compatible with both RT and NIL.
\ No newline at end of file
diff --git a/lib/chibios/os/oslib/src/chdelegates.c b/lib/chibios/os/oslib/src/chdelegates.c
new file mode 100644
index 000000000..648276eb6
--- /dev/null
+++ b/lib/chibios/os/oslib/src/chdelegates.c
@@ -0,0 +1,239 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/src/chdelegates.c
+ * @brief   Delegate threads code.
+ * @details Delegate threads.
+ *          <h2>Operation mode</h2>
+ *          A delegate thread is a thread performing function calls triggered
+ *          by other threads. This functionality is especially useful when
+ *          encapsulating a library not designed for threading into a
+ *          delegate thread. Other threads have access to the library without
+ *          having to worry about mutual exclusion.
+ * @pre     In order to use the pipes APIs the @p CH_CFG_USE_DELEGATES
+ *          option must be enabled in @p chconf.h.
+ * @note    Compatible with RT and NIL.
+ *
+ * @addtogroup oslib_delegates
+ * @{
+ */
+
+#include "ch.h"
+
+#if (CH_CFG_USE_DELEGATES == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local types.                                                       */
+/*===========================================================================*/
+
+/**
+ * @brief   Type of a structure representing a delegate call.
+ */
+typedef struct {
+  /**
+   * @brief   The delegate veneer function.
+   */
+  delegate_veneer_t veneer;
+  /**
+   * @brief   Pointer to the caller @p va_list object.
+   */
+  va_list           *argsp;
+} call_message_t;
+
+/*===========================================================================*/
+/* Module local variables.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module exported functions.                                                */
+/*===========================================================================*/
+
+/*lint -save -e586 [17.1] Required by design.*/
+
+/**
+ * @brief   Veneer for functions with no parameters.
+ *
+ * @param[in] argsp     the list of arguments
+ * @return              The function return value.
+ */
+msg_t __ch_delegate_fn0(va_list *argsp) {
+  delegate_fn0_t fn0 = (delegate_fn0_t)va_arg(*argsp, delegate_fn0_t);
+  return fn0();
+}
+
+/**
+ * @brief   Veneer for functions with one parameter.
+ *
+ * @param[in] argsp     the list of arguments
+ * @return              The function return value.
+ */
+msg_t __ch_delegate_fn1(va_list *argsp) {
+  delegate_fn1_t fn1 = (delegate_fn1_t)va_arg(*argsp, delegate_fn1_t);
+  msg_t p1 = (msg_t)va_arg(*argsp, msg_t);
+  return fn1(p1);
+}
+
+/**
+ * @brief   Veneer for functions with two parameters.
+ *
+ * @param[in] argsp     the list of arguments
+ * @return              The function return value.
+ */
+msg_t __ch_delegate_fn2(va_list *argsp) {
+  delegate_fn2_t fn2 = (delegate_fn2_t)va_arg(*argsp, delegate_fn2_t);
+  msg_t p1 = (msg_t)va_arg(*argsp, msg_t);
+  msg_t p2 = (msg_t)va_arg(*argsp, msg_t);
+  return fn2(p1, p2);
+}
+
+/**
+ * @brief   Veneer for functions with three parameters.
+ *
+ * @param[in] argsp     the list of arguments
+ * @return              The function return value.
+ */
+msg_t __ch_delegate_fn3(va_list *argsp) {
+  delegate_fn3_t fn3 = (delegate_fn3_t)va_arg(*argsp, delegate_fn3_t);
+  msg_t p1 = (msg_t)va_arg(*argsp, msg_t);
+  msg_t p2 = (msg_t)va_arg(*argsp, msg_t);
+  msg_t p3 = (msg_t)va_arg(*argsp, msg_t);
+  return fn3(p1, p2, p3);
+}
+
+/**
+ * @brief   Veneer for functions with four parameters.
+ *
+ * @param[in] argsp     the list of arguments
+ * @return              The function return value.
+ */
+msg_t __ch_delegate_fn4(va_list *argsp) {
+  delegate_fn4_t fn4 = (delegate_fn4_t)va_arg(*argsp, delegate_fn4_t);
+  msg_t p1 = (msg_t)va_arg(*argsp, msg_t);
+  msg_t p2 = (msg_t)va_arg(*argsp, msg_t);
+  msg_t p3 = (msg_t)va_arg(*argsp, msg_t);
+  msg_t p4 = (msg_t)va_arg(*argsp, msg_t);
+  return fn4(p1, p2, p3, p4);
+}
+
+/**
+ * @brief   Triggers a function call on a delegate thread.
+ * @note    The thread must be executing @p chDelegateDispatchTimeout() in
+ *          order to have the functions called.
+ *
+ * @param[in] tp        pointer to the delegate thread
+ * @param[in] veneer    pointer to the veneer function to be called
+ * @param[in] ...       variable number of parameters
+ * @return              The function return value casted to msg_t. It is
+ *                      garbage for functions returning @p void.
+ */
+msg_t chDelegateCallVeneer(thread_t *tp, delegate_veneer_t veneer, ...) {
+  va_list args;
+  call_message_t cm;
+  msg_t msg;
+
+  va_start(args, veneer);
+
+  /* Preparing the call message.*/
+  cm.veneer = veneer;
+  cm.argsp  = &args;
+  (void)cm; /* Suppresses a lint warning.*/
+
+  /* Sending the message to the dispatcher thread, the return value is
+     contained in the returned message.*/
+  msg = chMsgSend(tp, (msg_t)&cm);
+
+  va_end(args);
+
+  return msg;
+}
+
+/*lint -restore*/
+
+/**
+ * @brief   Call messages dispatching.
+ * @details The function awaits for an incoming call messages and calls the
+ *          specified functions, then it returns. In case multiple threads
+ *          are sending messages then the requests are served in priority
+ *          order.
+ *
+ * @api
+ */
+void chDelegateDispatch(void) {
+  thread_t *tp;
+  const call_message_t *cmp;
+  msg_t ret;
+
+  tp = chMsgWait();
+  cmp = (const call_message_t *)chMsgGet(tp);
+  ret = cmp->veneer(cmp->argsp);
+
+  chMsgRelease(tp, ret);
+}
+
+/**
+ * @brief   Call messages dispatching with timeout.
+ * @details The function awaits for an incoming call messages and calls the
+ *          specified functions, then it returns. In case multiple threads
+ *          are sending messages then the requests are served in priority
+ *          order.
+ *
+ * @param[in] timeout   the number of ticks before the operation timeouts,
+ *                      the following special values are allowed:
+ *                      - @a TIME_INFINITE no timeout.
+ *                      .
+ * @return              The function outcome.
+ * @retval MSG_OK       if a function has been called.
+ * @retval MSG_TIMEOUT  if a timeout occurred.
+ *
+ * @api
+ */
+msg_t chDelegateDispatchTimeout(sysinterval_t timeout) {
+  thread_t *tp;
+  const call_message_t *cmp;
+  msg_t ret;
+
+  tp = chMsgWaitTimeout(timeout);
+  if (tp == NULL) {
+    return MSG_TIMEOUT;
+  }
+
+  cmp = (const call_message_t *)chMsgGet(tp);
+  ret = cmp->veneer(cmp->argsp);
+
+  chMsgRelease(tp, ret);
+
+  return MSG_OK;
+}
+
+#endif /* CH_CFG_USE_DELEGATES == TRUE */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/src/chfactory.c b/lib/chibios/os/oslib/src/chfactory.c
new file mode 100644
index 000000000..9351ab151
--- /dev/null
+++ b/lib/chibios/os/oslib/src/chfactory.c
@@ -0,0 +1,829 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/src/chfactory.c
+ * @brief   ChibiOS objects factory and registry code.
+ *
+ * @addtogroup oslib_objects_factory
+ * @details The object factory is a subsystem that allows to:
+ *          - Register static objects by name.
+ *          - Dynamically create objects and assign them a name.
+ *          - Retrieve existing objects by name.
+ *          - Free objects by reference.
+ *          .
+ *          Allocated OS objects are handled using a reference counter, only
+ *          when all references have been released then the object memory is
+ *          freed in a pool.<br>
+ * @pre     This subsystem requires the @p CH_CFG_USE_MEMCORE and
+ *          @p CH_CFG_USE_MEMPOOLS options to be set to @p TRUE. The
+ *          option @p CH_CFG_USE_HEAP is also required if the support
+ *          for variable length objects is enabled.
+ * @note    Compatible with RT and NIL.
+ * @{
+ */
+
+#include <string.h>
+
+#include "ch.h"
+
+#if (CH_CFG_USE_FACTORY == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module local definitions.                                                 */
+/*===========================================================================*/
+
+/*
+ * Defaults on the best synchronization mechanism available.
+ */
+#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
+#define F_LOCK()        chMtxLock(&ch_factory.mtx)
+#define F_UNLOCK()      chMtxUnlock(&ch_factory.mtx)
+#else
+#define F_LOCK()        (void) chSemWait(&ch_factory.sem)
+#define F_UNLOCK()      chSemSignal(&ch_factory.sem)
+#endif
+
+/*===========================================================================*/
+/* Module exported variables.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Factory object static instance.
+ * @note    It is a global object because it could be accessed through
+ *          a specific debugger plugin.
+ */
+objects_factory_t ch_factory;
+
+/*===========================================================================*/
+/* Module local types.                                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local variables.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local functions.                                                   */
+/*===========================================================================*/
+
+static void copy_name(const char *sp, char *dp) {
+  unsigned i;
+  char c;
+
+  i = CH_CFG_FACTORY_MAX_NAMES_LENGTH;
+  do {
+    c = *sp++;
+    *dp++ = c;
+    i--;
+  } while ((c != (char)0) && (i > 0U));
+}
+
+static inline void dyn_list_init(dyn_list_t *dlp) {
+
+  dlp->next = (dyn_element_t *)dlp;
+}
+
+static dyn_element_t *dyn_list_find(const char *name, dyn_list_t *dlp) {
+  dyn_element_t *p = dlp->next;
+
+  while (p != (dyn_element_t *)dlp) {
+    if (strncmp(p->name, name, CH_CFG_FACTORY_MAX_NAMES_LENGTH) == 0) {
+      return p;
+    }
+    p = p->next;
+  }
+
+  return NULL;
+}
+
+static dyn_element_t *dyn_list_unlink(dyn_element_t *element,
+                                      dyn_list_t *dlp) {
+  dyn_element_t *prev = (dyn_element_t *)dlp;
+
+  /* Scanning the list.*/
+  while (prev->next != (dyn_element_t *)dlp) {
+    if (prev->next == element) {
+      /* Found.*/
+      prev->next = element->next;
+      return element;
+    }
+
+    /* Next element in the list.*/
+    prev = prev->next;
+  }
+
+  return NULL;
+}
+
+#if CH_FACTORY_REQUIRES_HEAP || defined(__DOXYGEN__)
+static dyn_element_t *dyn_create_object_heap(const char *name,
+                                             dyn_list_t *dlp,
+                                             size_t size,
+                                             unsigned align) {
+  dyn_element_t *dep;
+
+  chDbgCheck(name != NULL);
+
+  /* Checking if an object with this name has already been created.*/
+  dep = dyn_list_find(name, dlp);
+  if (dep != NULL) {
+    return NULL;
+  }
+
+  /* Allocating space for the new buffer object.*/
+  dep = (dyn_element_t *)chHeapAllocAligned(NULL, size, align);
+  if (dep == NULL) {
+    return NULL;
+  }
+
+  /* Initializing object list element.*/
+  copy_name(name, dep->name);
+  dep->refs = (ucnt_t)1;
+  dep->next = dlp->next;
+
+  /* Updating factory list.*/
+  dlp->next = dep;
+
+  return dep;
+}
+
+static void dyn_release_object_heap(dyn_element_t *dep,
+                                    dyn_list_t *dlp) {
+
+  chDbgCheck(dep != NULL);
+  chDbgAssert(dep->refs > (ucnt_t)0, "invalid references number");
+
+  dep->refs--;
+  if (dep->refs == (ucnt_t)0) {
+    dep = dyn_list_unlink(dep, dlp);
+    chHeapFree((void *)dep);
+  }
+}
+#endif /* CH_FACTORY_REQUIRES_HEAP */
+
+#if CH_FACTORY_REQUIRES_POOLS || defined(__DOXYGEN__)
+static dyn_element_t *dyn_create_object_pool(const char *name,
+                                             dyn_list_t *dlp,
+                                             memory_pool_t *mp) {
+  dyn_element_t *dep;
+
+  chDbgCheck(name != NULL);
+
+  /* Checking if an object object with this name has already been created.*/
+  dep = dyn_list_find(name, dlp);
+  if (dep != NULL) {
+    return NULL;
+  }
+
+  /* Allocating space for the new object.*/
+  dep = (dyn_element_t *)chPoolAlloc(mp);
+  if (dep == NULL) {
+    return NULL;
+  }
+
+  /* Initializing object list element.*/
+  copy_name(name, dep->name);
+  dep->refs = (ucnt_t)1;
+  dep->next = dlp->next;
+
+  /* Updating factory list.*/
+  dlp->next = (dyn_element_t *)dep;
+
+  return dep;
+}
+
+static void dyn_release_object_pool(dyn_element_t *dep,
+                                    dyn_list_t *dlp,
+                                    memory_pool_t *mp) {
+
+  chDbgCheck(dep != NULL);
+  chDbgAssert(dep->refs > (ucnt_t)0, "invalid references number");
+
+  dep->refs--;
+  if (dep->refs == (ucnt_t)0) {
+    dep = dyn_list_unlink(dep, dlp);
+    chPoolFree(mp, (void *)dep);
+  }
+}
+#endif /* CH_FACTORY_REQUIRES_POOLS */
+
+static dyn_element_t *dyn_find_object(const char *name, dyn_list_t *dlp) {
+  dyn_element_t *dep;
+
+  chDbgCheck(name != NULL);
+
+  /* Checking if an object with this name has already been created.*/
+  dep = dyn_list_find(name, dlp);
+  if (dep != NULL) {
+    /* Increasing references counter.*/
+    dep->refs++;
+  }
+
+  return dep;
+}
+
+/*===========================================================================*/
+/* Module exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Initializes the objects factory.
+ *
+ * @init
+ */
+void _factory_init(void) {
+
+#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
+  chMtxObjectInit(&ch_factory.mtx);
+#else
+  chSemObjectInit(&ch_factory.sem, (cnt_t)1);
+#endif
+
+#if CH_CFG_FACTORY_OBJECTS_REGISTRY == TRUE
+  dyn_list_init(&ch_factory.obj_list);
+  chPoolObjectInit(&ch_factory.obj_pool,
+                   sizeof (registered_object_t),
+                   chCoreAllocAlignedI);
+#endif
+#if CH_CFG_FACTORY_GENERIC_BUFFERS == TRUE
+  dyn_list_init(&ch_factory.buf_list);
+#endif
+#if CH_CFG_FACTORY_SEMAPHORES == TRUE
+  dyn_list_init(&ch_factory.sem_list);
+  chPoolObjectInit(&ch_factory.sem_pool,
+                   sizeof (dyn_semaphore_t),
+                   chCoreAllocAlignedI);
+#endif
+#if CH_CFG_FACTORY_MAILBOXES == TRUE
+  dyn_list_init(&ch_factory.mbx_list);
+#endif
+#if CH_CFG_FACTORY_OBJ_FIFOS == TRUE
+  dyn_list_init(&ch_factory.fifo_list);
+#endif
+#if CH_CFG_FACTORY_PIPES == TRUE
+  dyn_list_init(&ch_factory.pipe_list);
+#endif
+}
+
+#if (CH_CFG_FACTORY_OBJECTS_REGISTRY == TRUE) || defined(__DOXIGEN__)
+/**
+ * @brief   Registers a generic object.
+ * @post    A reference to the registered object is returned and the
+ *          reference counter is initialized to one.
+ *
+ * @param[in] name      name to be assigned to the registered object
+ * @param[in] objp      pointer to the object to be registered
+ *
+ * @return              The reference to the registered object.
+ * @retval NULL         if the object to be registered cannot be allocated or
+ *                      a registered object with the same name exists.
+ *
+ * @api
+ */
+registered_object_t *chFactoryRegisterObject(const char *name,
+                                             void *objp) {
+  registered_object_t *rop;
+
+  F_LOCK();
+
+  rop = (registered_object_t *)dyn_create_object_pool(name,
+                                                      &ch_factory.obj_list,
+                                                      &ch_factory.obj_pool);
+  if (rop != NULL) {
+    /* Initializing registered object data.*/
+    rop->objp = objp;
+  }
+
+  F_UNLOCK();
+
+  return rop;
+}
+
+/**
+ * @brief   Retrieves a registered object.
+ * @post    A reference to the registered object is returned with the
+ *          reference counter increased by one.
+ *
+ * @param[in] name      name of the registered object
+ *
+ * @return              The reference to the found registered object.
+ * @retval NULL         if a registered object with the specified name
+ *                      does not exist.
+ *
+ * @api
+ */
+registered_object_t *chFactoryFindObject(const char *name) {
+  registered_object_t *rop;
+
+  F_LOCK();
+
+  rop = (registered_object_t *)dyn_find_object(name, &ch_factory.obj_list);
+
+  F_UNLOCK();
+
+  return rop;
+}
+
+/**
+ * @brief   Retrieves a registered object by pointer.
+ * @post    A reference to the registered object is returned with the
+ *          reference counter increased by one.
+ *
+ * @param[in] objp      pointer to the object to be retrieved
+ *
+ * @return              The reference to the found registered object.
+ * @retval NULL         if a registered object with the specified pointer
+ *                      does not exist.
+ *
+ * @api
+ */
+registered_object_t *chFactoryFindObjectByPointer(void *objp) {
+  registered_object_t *rop = (registered_object_t *)ch_factory.obj_list.next;
+
+  F_LOCK();
+
+  while ((void *)rop != (void *)&ch_factory.obj_list) {
+    if (rop->objp == objp) {
+      rop->element.refs++;
+
+      F_UNLOCK();
+
+      return rop;
+    }
+    rop = (registered_object_t *)rop->element.next;
+  }
+
+  F_UNLOCK();
+
+  return NULL;
+}
+
+/**
+ * @brief   Releases a registered object.
+ * @details The reference counter of the registered object is decreased
+ *          by one, if reaches zero then the registered object memory
+ *          is freed.
+ * @note    The object itself is not freed, it could be static, only the
+ *          allocated list element is freed.
+ *
+ * @param[in] rop       registered object reference
+ *
+ * @api
+ */
+void chFactoryReleaseObject(registered_object_t *rop) {
+
+  F_LOCK();
+
+  dyn_release_object_pool(&rop->element,
+                          &ch_factory.obj_list,
+                          &ch_factory.obj_pool);
+
+  F_UNLOCK();
+}
+#endif /* CH_CFG_FACTORY_OBJECTS_REGISTRY == TRUE */
+
+#if (CH_CFG_FACTORY_GENERIC_BUFFERS == TRUE) || defined(__DOXIGEN__)
+/**
+ * @brief   Creates a generic dynamic buffer object.
+ * @post    A reference to the dynamic buffer object is returned and the
+ *          reference counter is initialized to one.
+ * @post    The dynamic buffer object is filled with zeros.
+ *
+ * @param[in] name      name to be assigned to the new dynamic buffer object
+ * @param[in] size      payload size of the dynamic buffer object to be created
+ *
+ * @return              The reference to the created dynamic buffer object.
+ * @retval NULL         if the dynamic buffer object cannot be allocated or
+ *                      a dynamic buffer object with the same name exists.
+ *
+ * @api
+ */
+dyn_buffer_t *chFactoryCreateBuffer(const char *name, size_t size) {
+  dyn_buffer_t *dbp;
+
+  F_LOCK();
+
+  dbp = (dyn_buffer_t *)dyn_create_object_heap(name,
+                                               &ch_factory.buf_list,
+                                               sizeof (dyn_buffer_t) + size,
+                                               CH_HEAP_ALIGNMENT);
+  if (dbp != NULL) {
+    /* Initializing buffer object data.*/
+    memset((void *)(dbp + 1), 0, size);
+  }
+
+  F_UNLOCK();
+
+  return dbp;
+}
+
+/**
+ * @brief   Retrieves a dynamic buffer object.
+ * @post    A reference to the dynamic buffer object is returned with the
+ *          reference counter increased by one.
+ *
+ * @param[in] name      name of the dynamic buffer object
+ *
+ * @return              The reference to the found dynamic buffer object.
+ * @retval NULL         if a dynamic buffer object with the specified name
+ *                      does not exist.
+ *
+ * @api
+ */
+dyn_buffer_t *chFactoryFindBuffer(const char *name) {
+  dyn_buffer_t *dbp;
+
+  F_LOCK();
+
+  dbp = (dyn_buffer_t *)dyn_find_object(name, &ch_factory.buf_list);
+
+  F_UNLOCK();
+
+  return dbp;
+}
+
+/**
+ * @brief   Releases a dynamic buffer object.
+ * @details The reference counter of the dynamic buffer object is decreased
+ *          by one, if reaches zero then the dynamic buffer object memory
+ *          is freed.
+ *
+ * @param[in] dbp       dynamic buffer object reference
+ *
+ * @api
+ */
+void chFactoryReleaseBuffer(dyn_buffer_t *dbp) {
+
+  F_LOCK();
+
+  dyn_release_object_heap(&dbp->element, &ch_factory.buf_list);
+
+  F_UNLOCK();
+}
+#endif /* CH_CFG_FACTORY_GENERIC_BUFFERS = TRUE */
+
+#if (CH_CFG_FACTORY_SEMAPHORES == TRUE) || defined(__DOXIGEN__)
+/**
+ * @brief   Creates a dynamic semaphore object.
+ * @post    A reference to the dynamic semaphore object is returned and the
+ *          reference counter is initialized to one.
+ * @post    The dynamic semaphore object is initialized and ready to use.
+ *
+ * @param[in] name      name to be assigned to the new dynamic semaphore object
+ * @param[in] n         dynamic semaphore object counter initialization value
+ *
+ * @return              The reference to the created dynamic semaphore object.
+ * @retval NULL         if the dynamic semaphore object cannot be allocated or
+ *                      a dynamic semaphore with the same name exists.
+ *
+ * @api
+ */
+dyn_semaphore_t *chFactoryCreateSemaphore(const char *name, cnt_t n) {
+  dyn_semaphore_t *dsp;
+
+  F_LOCK();
+
+  dsp = (dyn_semaphore_t *)dyn_create_object_pool(name,
+                                                  &ch_factory.sem_list,
+                                                  &ch_factory.sem_pool);
+  if (dsp != NULL) {
+    /* Initializing semaphore object dataa.*/
+    chSemObjectInit(&dsp->sem, n);
+  }
+
+  F_UNLOCK();
+
+  return dsp;
+}
+
+/**
+ * @brief   Retrieves a dynamic semaphore object.
+ * @post    A reference to the dynamic semaphore object is returned with the
+ *          reference counter increased by one.
+ *
+ * @param[in] name      name of the dynamic semaphore object
+ *
+ * @return              The reference to the found dynamic semaphore object.
+ * @retval NULL         if a dynamic semaphore object with the specified name
+ *                      does not exist.
+ *
+ * @api
+ */
+dyn_semaphore_t *chFactoryFindSemaphore(const char *name) {
+  dyn_semaphore_t *dsp;
+
+  F_LOCK();
+
+  dsp = (dyn_semaphore_t *)dyn_find_object(name, &ch_factory.sem_list);
+
+  F_UNLOCK();
+
+  return dsp;
+}
+
+/**
+ * @brief   Releases a dynamic semaphore object.
+ * @details The reference counter of the dynamic semaphore object is decreased
+ *          by one, if reaches zero then the dynamic semaphore object memory
+ *          is freed.
+ *
+ * @param[in] dsp       dynamic semaphore object reference
+ *
+ * @api
+ */
+void chFactoryReleaseSemaphore(dyn_semaphore_t *dsp) {
+
+  F_LOCK();
+
+  dyn_release_object_pool(&dsp->element,
+                          &ch_factory.sem_list,
+                          &ch_factory.sem_pool);
+
+  F_UNLOCK();
+}
+#endif /* CH_CFG_FACTORY_SEMAPHORES = TRUE */
+
+#if (CH_CFG_FACTORY_MAILBOXES == TRUE) || defined(__DOXIGEN__)
+/**
+ * @brief   Creates a dynamic mailbox object.
+ * @post    A reference to the dynamic mailbox object is returned and the
+ *          reference counter is initialized to one.
+ * @post    The dynamic mailbox object is initialized and ready to use.
+ *
+ * @param[in] name      name to be assigned to the new dynamic mailbox object
+ * @param[in] n         mailbox buffer size as number of messages
+ *
+ * @return              The reference to the created dynamic mailbox object.
+ * @retval NULL         if the dynamic mailbox object cannot be allocated or
+ *                      a dynamic mailbox object with the same name exists.
+ *
+ * @api
+ */
+dyn_mailbox_t *chFactoryCreateMailbox(const char *name, size_t n) {
+  dyn_mailbox_t *dmp;
+
+  F_LOCK();
+
+  dmp = (dyn_mailbox_t *)dyn_create_object_heap(name,
+                                                &ch_factory.mbx_list,
+                                                sizeof (dyn_mailbox_t) +
+                                                (n * sizeof (msg_t)),
+                                                CH_HEAP_ALIGNMENT);
+  if (dmp != NULL) {
+    /* Initializing mailbox object data.*/
+    chMBObjectInit(&dmp->mbx, (msg_t *)(dmp + 1), n);
+  }
+
+  F_UNLOCK();
+
+  return dmp;
+}
+
+/**
+ * @brief   Retrieves a dynamic mailbox object.
+ * @post    A reference to the dynamic mailbox object is returned with the
+ *          reference counter increased by one.
+ *
+ * @param[in] name      name of the dynamic mailbox object
+ *
+ * @return              The reference to the found dynamic mailbox object.
+ * @retval NULL         if a dynamic mailbox object with the specified name
+ *                      does not exist.
+ *
+ * @api
+ */
+dyn_mailbox_t *chFactoryFindMailbox(const char *name) {
+  dyn_mailbox_t *dmp;
+
+  F_LOCK();
+
+  dmp = (dyn_mailbox_t *)dyn_find_object(name, &ch_factory.mbx_list);
+
+  F_UNLOCK();
+
+  return dmp;
+}
+
+/**
+ * @brief   Releases a dynamic mailbox object.
+ * @details The reference counter of the dynamic mailbox object is decreased
+ *          by one, if reaches zero then the dynamic mailbox object memory
+ *          is freed.
+ *
+ * @param[in] dmp       dynamic mailbox object reference
+ *
+ * @api
+ */
+void chFactoryReleaseMailbox(dyn_mailbox_t *dmp) {
+
+  F_LOCK();
+
+  dyn_release_object_heap(&dmp->element, &ch_factory.mbx_list);
+
+  F_UNLOCK();
+}
+#endif /* CH_CFG_FACTORY_MAILBOXES = TRUE */
+
+#if (CH_CFG_FACTORY_OBJ_FIFOS == TRUE) || defined(__DOXIGEN__)
+/**
+ * @brief   Creates a dynamic "objects FIFO" object.
+ * @post    A reference to the dynamic "objects FIFO" object is returned and
+ *          the reference counter is initialized to one.
+ * @post    The dynamic "objects FIFO" object is initialized and ready to use.
+ *
+ * @param[in] name      name to be assigned to the new dynamic "objects FIFO"
+ *                      object
+ * @param[in] objsize   size of objects
+ * @param[in] objn      number of objects available
+ * @param[in] objalign  required objects alignment
+ * @return              The reference to the created dynamic "objects FIFO"
+ *                      object.
+ * @retval NULL         if the dynamic "objects FIFO" object cannot be
+ *                      allocated or a dynamic "objects FIFO" object with
+ *                      the same name exists.
+ *
+ * @api
+ */
+dyn_objects_fifo_t *chFactoryCreateObjectsFIFO(const char *name,
+                                               size_t objsize,
+                                               size_t objn,
+                                               unsigned objalign) {
+  size_t size1, size2;
+  dyn_objects_fifo_t *dofp;
+
+  F_LOCK();
+
+  /* Enforcing alignment for the objects array.*/
+  objsize = MEM_ALIGN_NEXT(objsize, objalign);
+  size1   = MEM_ALIGN_NEXT(sizeof (dyn_objects_fifo_t) + (objn * sizeof (msg_t)),
+                           objalign);
+  size2   = objn * objsize;
+
+  /* Allocating the FIFO object with messages buffer and objects buffer.*/
+  dofp = (dyn_objects_fifo_t *)dyn_create_object_heap(name,
+                                                      &ch_factory.fifo_list,
+                                                      size1 + size2,
+                                                      objalign);
+  if (dofp != NULL) {
+    msg_t *msgbuf = (msg_t *)(dofp + 1);
+    uint8_t *objbuf = (uint8_t *)dofp + size1;
+
+    /* Initializing mailbox object data.*/
+    chFifoObjectInitAligned(&dofp->fifo, objsize, objn, objalign,
+                            (void *)objbuf, msgbuf);
+  }
+
+  F_UNLOCK();
+
+  return dofp;
+}
+
+/**
+ * @brief   Retrieves a dynamic "objects FIFO" object.
+ * @post    A reference to the dynamic "objects FIFO" object is returned with
+ *          the reference counter increased by one.
+ *
+ * @param[in] name      name of the dynamic "objects FIFO" object
+ *
+ * @return              The reference to the found dynamic "objects FIFO"
+ *                      object.
+ * @retval NULL         if a dynamic "objects FIFO" object with the specified
+ *                      name does not exist.
+ *
+ * @api
+ */
+dyn_objects_fifo_t *chFactoryFindObjectsFIFO(const char *name) {
+  dyn_objects_fifo_t *dofp;
+
+  F_LOCK();
+
+  dofp = (dyn_objects_fifo_t *)dyn_find_object(name, &ch_factory.fifo_list);
+
+  F_UNLOCK();
+
+  return dofp;
+}
+
+/**
+ * @brief   Releases a dynamic "objects FIFO" object.
+ * @details The reference counter of the dynamic "objects FIFO" object is
+ *          decreased by one, if reaches zero then the dynamic "objects FIFO"
+ *          object memory is freed.
+ *
+ * @param[in] dofp      dynamic "objects FIFO" object reference
+ *
+ * @api
+ */
+void chFactoryReleaseObjectsFIFO(dyn_objects_fifo_t *dofp) {
+
+  F_LOCK();
+
+  dyn_release_object_heap(&dofp->element, &ch_factory.fifo_list);
+
+  F_UNLOCK();
+}
+#endif /* CH_CFG_FACTORY_OBJ_FIFOS = TRUE */
+
+#if (CH_CFG_FACTORY_PIPES == TRUE) || defined(__DOXIGEN__)
+/**
+ * @brief   Creates a dynamic pipe object.
+ * @post    A reference to the dynamic pipe object is returned and
+ *          the reference counter is initialized to one.
+ * @post    The dynamic pipe object is initialized and ready to use.
+ *
+ * @param[in] name      name to be assigned to the new dynamic pipe
+ *                      object
+ * @param[in] size      pipe buffer size
+ * @return              The reference to the created dynamic pipe
+ *                      object.
+ * @retval NULL         if the dynamic pipe object cannot be
+ *                      allocated or a dynamic pipe object with
+ *                      the same name exists.
+ *
+ * @api
+ */
+dyn_pipe_t *chFactoryCreatePipe(const char *name, size_t size) {
+  dyn_pipe_t *dpp;
+
+  F_LOCK();
+
+  dpp = (dyn_pipe_t *)dyn_create_object_heap(name,
+                                             &ch_factory.pipe_list,
+                                             sizeof (dyn_pipe_t) + size,
+                                             CH_HEAP_ALIGNMENT);
+  if (dpp != NULL) {
+    /* Initializing mailbox object data.*/
+    chPipeObjectInit(&dpp->pipe, (uint8_t *)(dpp + 1), size);
+  }
+
+  F_UNLOCK();
+
+  return dpp;
+}
+
+/**
+ * @brief   Retrieves a dynamic pipe object.
+ * @post    A reference to the dynamic pipe object is returned with
+ *          the reference counter increased by one.
+ *
+ * @param[in] name      name of the pipe object
+ *
+ * @return              The reference to the found dynamic pipe
+ *                      object.
+ * @retval NULL         if a dynamic pipe object with the specified
+ *                      name does not exist.
+ *
+ * @api
+ */
+dyn_pipe_t *chFactoryFindPipe(const char *name) {
+  dyn_pipe_t *dpp;
+
+  F_LOCK();
+
+  dpp = (dyn_pipe_t *)dyn_find_object(name, &ch_factory.pipe_list);
+
+  F_UNLOCK();
+
+  return dpp;
+}
+
+/**
+ * @brief   Releases a dynamic pipe object.
+ * @details The reference counter of the dynamic pipe object is
+ *          decreased by one, if reaches zero then the dynamic pipe
+ *          object memory is freed.
+ *
+ * @param[in] dpp       dynamic pipe object reference
+ *
+ * @api
+ */
+void chFactoryReleasePipe(dyn_pipe_t *dpp) {
+
+  F_LOCK();
+
+  dyn_release_object_heap(&dpp->element, &ch_factory.pipe_list);
+
+  F_UNLOCK();
+}
+#endif /* CH_CFG_FACTORY_PIPES = TRUE */
+
+#endif /* CH_CFG_USE_FACTORY == TRUE */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/src/chmboxes.c b/lib/chibios/os/oslib/src/chmboxes.c
new file mode 100644
index 000000000..a9f738109
--- /dev/null
+++ b/lib/chibios/os/oslib/src/chmboxes.c
@@ -0,0 +1,522 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/src/chmboxes.c
+ * @brief   Mailboxes code.
+ *
+ * @addtogroup oslib_mailboxes
+ * @details Asynchronous messages.
+ *          <h2>Operation mode</h2>
+ *          A mailbox is an asynchronous communication mechanism.<br>
+ *          Operations defined for mailboxes:
+ *          - <b>Post</b>: Posts a message on the mailbox in FIFO order.
+ *          - <b>Post Ahead</b>: Posts a message on the mailbox with urgent
+ *            priority.
+ *          - <b>Fetch</b>: A message is fetched from the mailbox and removed
+ *            from the queue.
+ *          - <b>Reset</b>: The mailbox is emptied and all the stored messages
+ *            are lost.
+ *          .
+ *          A message is a variable of type msg_t that is guaranteed to have
+ *          the same size of and be compatible with (data) pointers (anyway an
+ *          explicit cast is needed).
+ *          If larger messages need to be exchanged then a pointer to a
+ *          structure can be posted in the mailbox but the posting side has
+ *          no predefined way to know when the message has been processed. A
+ *          possible approach is to allocate memory (from a memory pool for
+ *          example) from the posting side and free it on the fetching side.
+ *          Another approach is to set a "done" flag into the structure pointed
+ *          by the message.
+ * @pre     In order to use the mailboxes APIs the @p CH_CFG_USE_MAILBOXES
+ *          option must be enabled in @p chconf.h.
+ * @note    Compatible with RT and NIL.
+ * @{
+ */
+
+#include "ch.h"
+
+#if (CH_CFG_USE_MAILBOXES == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local types.                                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local variables.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Initializes a @p mailbox_t object.
+ *
+ * @param[out] mbp      the pointer to the @p mailbox_t structure to be
+ *                      initialized
+ * @param[in] buf       pointer to the messages buffer as an array of @p msg_t
+ * @param[in] n         number of elements in the buffer array
+ *
+ * @init
+ */
+void chMBObjectInit(mailbox_t *mbp, msg_t *buf, size_t n) {
+
+  chDbgCheck((mbp != NULL) && (buf != NULL) && (n > (size_t)0));
+
+  mbp->buffer = buf;
+  mbp->rdptr  = buf;
+  mbp->wrptr  = buf;
+  mbp->top    = &buf[n];
+  mbp->cnt    = (size_t)0;
+  mbp->reset  = false;
+  chThdQueueObjectInit(&mbp->qw);
+  chThdQueueObjectInit(&mbp->qr);
+}
+
+/**
+ * @brief   Resets a @p mailbox_t object.
+ * @details All the waiting threads are resumed with status @p MSG_RESET and
+ *          the queued messages are lost.
+ * @post    The mailbox is in reset state, all operations will fail and
+ *          return @p MSG_RESET until the mailbox is enabled again using
+ *          @p chMBResumeX().
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ *
+ * @api
+ */
+void chMBReset(mailbox_t *mbp) {
+
+  chSysLock();
+  chMBResetI(mbp);
+  chSchRescheduleS();
+  chSysUnlock();
+}
+
+/**
+ * @brief   Resets a @p mailbox_t object.
+ * @details All the waiting threads are resumed with status @p MSG_RESET and
+ *          the queued messages are lost.
+ * @post    The mailbox is in reset state, all operations will fail and
+ *          return @p MSG_RESET until the mailbox is enabled again using
+ *          @p chMBResumeX().
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ *
+ * @api
+ */
+void chMBResetI(mailbox_t *mbp) {
+
+  chDbgCheckClassI();
+  chDbgCheck(mbp != NULL);
+
+  mbp->wrptr = mbp->buffer;
+  mbp->rdptr = mbp->buffer;
+  mbp->cnt   = (size_t)0;
+  mbp->reset = true;
+  chThdDequeueAllI(&mbp->qw, MSG_RESET);
+  chThdDequeueAllI(&mbp->qr, MSG_RESET);
+}
+
+/**
+ * @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 @p mailbox_t object
+ * @param[in] msg       the message to be posted on the mailbox
+ * @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 MSG_OK       if a message has been correctly posted.
+ * @retval MSG_RESET    if the mailbox has been reset.
+ * @retval MSG_TIMEOUT  if the operation has timed out.
+ *
+ * @api
+ */
+msg_t chMBPostTimeout(mailbox_t *mbp, msg_t msg, sysinterval_t timeout) {
+  msg_t rdymsg;
+
+  chSysLock();
+  rdymsg = chMBPostTimeoutS(mbp, msg, timeout);
+  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 @p mailbox_t object
+ * @param[in] msg       the message to be posted on the mailbox
+ * @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 MSG_OK       if a message has been correctly posted.
+ * @retval MSG_RESET    if the mailbox has been reset.
+ * @retval MSG_TIMEOUT  if the operation has timed out.
+ *
+ * @sclass
+ */
+msg_t chMBPostTimeoutS(mailbox_t *mbp, msg_t msg, sysinterval_t timeout) {
+  msg_t rdymsg;
+
+  chDbgCheckClassS();
+  chDbgCheck(mbp != NULL);
+
+  do {
+    /* If the mailbox is in reset state then returns immediately.*/
+    if (mbp->reset) {
+      return MSG_RESET;
+    }
+
+    /* Is there a free message slot in queue? if so then post.*/
+    if (chMBGetFreeCountI(mbp) > (size_t)0) {
+      *mbp->wrptr++ = msg;
+      if (mbp->wrptr >= mbp->top) {
+        mbp->wrptr = mbp->buffer;
+      }
+      mbp->cnt++;
+
+      /* If there is a reader waiting then makes it ready.*/
+      chThdDequeueNextI(&mbp->qr, MSG_OK);
+      chSchRescheduleS();
+
+      return MSG_OK;
+    }
+
+    /* No space in the queue, waiting for a slot to become available.*/
+    rdymsg = chThdEnqueueTimeoutS(&mbp->qw, timeout);
+  } while (rdymsg == MSG_OK);
+
+  return rdymsg;
+}
+
+/**
+ * @brief   Posts a message into a mailbox.
+ * @details This variant is non-blocking, the function returns a timeout
+ *          condition if the queue is full.
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ * @param[in] msg       the message to be posted on the mailbox
+ * @return              The operation status.
+ * @retval MSG_OK       if a message has been correctly posted.
+ * @retval MSG_RESET    if the mailbox has been reset.
+ * @retval MSG_TIMEOUT  if the mailbox is full and the message cannot be
+ *                      posted.
+ *
+ * @iclass
+ */
+msg_t chMBPostI(mailbox_t *mbp, msg_t msg) {
+
+  chDbgCheckClassI();
+  chDbgCheck(mbp != NULL);
+
+  /* If the mailbox is in reset state then returns immediately.*/
+  if (mbp->reset) {
+    return MSG_RESET;
+  }
+
+  /* Is there a free message slot in queue? if so then post.*/
+  if (chMBGetFreeCountI(mbp) > (size_t)0) {
+    *mbp->wrptr++ = msg;
+    if (mbp->wrptr >= mbp->top) {
+      mbp->wrptr = mbp->buffer;
+    }
+    mbp->cnt++;
+
+    /* If there is a reader waiting then makes it ready.*/
+    chThdDequeueNextI(&mbp->qr, MSG_OK);
+
+    return MSG_OK;
+  }
+
+  /* No space, immediate timeout.*/
+  return MSG_TIMEOUT;
+}
+
+/**
+ * @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 @p mailbox_t object
+ * @param[in] msg       the message to be posted on the mailbox
+ * @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 MSG_OK       if a message has been correctly posted.
+ * @retval MSG_RESET    if the mailbox has been reset.
+ * @retval MSG_TIMEOUT  if the operation has timed out.
+ *
+ * @api
+ */
+msg_t chMBPostAheadTimeout(mailbox_t *mbp, msg_t msg, sysinterval_t timeout) {
+  msg_t rdymsg;
+
+  chSysLock();
+  rdymsg = chMBPostAheadTimeoutS(mbp, msg, timeout);
+  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 @p mailbox_t object
+ * @param[in] msg       the message to be posted on the mailbox
+ * @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 MSG_OK       if a message has been correctly posted.
+ * @retval MSG_RESET    if the mailbox has been reset.
+ * @retval MSG_TIMEOUT  if the operation has timed out.
+ *
+ * @sclass
+ */
+msg_t chMBPostAheadTimeoutS(mailbox_t *mbp, msg_t msg, sysinterval_t timeout) {
+  msg_t rdymsg;
+
+  chDbgCheckClassS();
+  chDbgCheck(mbp != NULL);
+
+  do {
+    /* If the mailbox is in reset state then returns immediately.*/
+    if (mbp->reset) {
+      return MSG_RESET;
+    }
+
+    /* Is there a free message slot in queue? if so then post.*/
+    if (chMBGetFreeCountI(mbp) > (size_t)0) {
+      if (--mbp->rdptr < mbp->buffer) {
+        mbp->rdptr = mbp->top - 1;
+      }
+      *mbp->rdptr = msg;
+      mbp->cnt++;
+
+      /* If there is a reader waiting then makes it ready.*/
+      chThdDequeueNextI(&mbp->qr, MSG_OK);
+      chSchRescheduleS();
+
+      return MSG_OK;
+    }
+
+    /* No space in the queue, waiting for a slot to become available.*/
+    rdymsg = chThdEnqueueTimeoutS(&mbp->qw, timeout);
+  } while (rdymsg == MSG_OK);
+
+  return rdymsg;
+}
+
+/**
+ * @brief   Posts an high priority message into a mailbox.
+ * @details This variant is non-blocking, the function returns a timeout
+ *          condition if the queue is full.
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ * @param[in] msg       the message to be posted on the mailbox
+ * @return              The operation status.
+ * @retval MSG_OK       if a message has been correctly posted.
+ * @retval MSG_RESET    if the mailbox has been reset.
+ * @retval MSG_TIMEOUT  if the mailbox is full and the message cannot be
+ *                      posted.
+ *
+ * @iclass
+ */
+msg_t chMBPostAheadI(mailbox_t *mbp, msg_t msg) {
+
+  chDbgCheckClassI();
+  chDbgCheck(mbp != NULL);
+
+  /* If the mailbox is in reset state then returns immediately.*/
+  if (mbp->reset) {
+    return MSG_RESET;
+  }
+
+  /* Is there a free message slot in queue? if so then post.*/
+  if (chMBGetFreeCountI(mbp) > (size_t)0) {
+    if (--mbp->rdptr < mbp->buffer) {
+      mbp->rdptr = mbp->top - 1;
+    }
+    *mbp->rdptr = msg;
+    mbp->cnt++;
+
+    /* If there is a reader waiting then makes it ready.*/
+    chThdDequeueNextI(&mbp->qr, MSG_OK);
+
+    return MSG_OK;
+  }
+
+  /* No space, immediate timeout.*/
+  return MSG_TIMEOUT;
+}
+
+/**
+ * @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 @p mailbox_t object
+ * @param[out] msgp     pointer to a message variable for the received message
+ * @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 MSG_OK       if a message has been correctly fetched.
+ * @retval MSG_RESET    if the mailbox has been reset.
+ * @retval MSG_TIMEOUT  if the operation has timed out.
+ *
+ * @api
+ */
+msg_t chMBFetchTimeout(mailbox_t *mbp, msg_t *msgp, sysinterval_t timeout) {
+  msg_t rdymsg;
+
+  chSysLock();
+  rdymsg = chMBFetchTimeoutS(mbp, msgp, timeout);
+  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 @p mailbox_t object
+ * @param[out] msgp     pointer to a message variable for the received message
+ * @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 MSG_OK       if a message has been correctly fetched.
+ * @retval MSG_RESET    if the mailbox has been reset.
+ * @retval MSG_TIMEOUT  if the operation has timed out.
+ *
+ * @sclass
+ */
+msg_t chMBFetchTimeoutS(mailbox_t *mbp, msg_t *msgp, sysinterval_t timeout) {
+  msg_t rdymsg;
+
+  chDbgCheckClassS();
+  chDbgCheck((mbp != NULL) && (msgp != NULL));
+
+  do {
+    /* If the mailbox is in reset state then returns immediately.*/
+    if (mbp->reset) {
+      return MSG_RESET;
+    }
+
+    /* Is there a message in queue? if so then fetch.*/
+    if (chMBGetUsedCountI(mbp) > (size_t)0) {
+      *msgp = *mbp->rdptr++;
+      if (mbp->rdptr >= mbp->top) {
+        mbp->rdptr = mbp->buffer;
+      }
+      mbp->cnt--;
+
+      /* If there is a writer waiting then makes it ready.*/
+      chThdDequeueNextI(&mbp->qw, MSG_OK);
+      chSchRescheduleS();
+
+      return MSG_OK;
+    }
+
+    /* No message in the queue, waiting for a message to become available.*/
+    rdymsg = chThdEnqueueTimeoutS(&mbp->qr, timeout);
+  } while (rdymsg == MSG_OK);
+
+  return rdymsg;
+}
+
+/**
+ * @brief   Retrieves a message from a mailbox.
+ * @details This variant is non-blocking, the function returns a timeout
+ *          condition if the queue is empty.
+ *
+ * @param[in] mbp       the pointer to an initialized @p mailbox_t object
+ * @param[out] msgp     pointer to a message variable for the received message
+ * @return              The operation status.
+ * @retval MSG_OK       if a message has been correctly fetched.
+ * @retval MSG_RESET    if the mailbox has been reset.
+ * @retval MSG_TIMEOUT  if the mailbox is empty and a message cannot be
+ *                      fetched.
+ *
+ * @iclass
+ */
+msg_t chMBFetchI(mailbox_t *mbp, msg_t *msgp) {
+
+  chDbgCheckClassI();
+  chDbgCheck((mbp != NULL) && (msgp != NULL));
+
+  /* If the mailbox is in reset state then returns immediately.*/
+  if (mbp->reset) {
+    return MSG_RESET;
+  }
+
+  /* Is there a message in queue? if so then fetch.*/
+  if (chMBGetUsedCountI(mbp) > (size_t)0) {
+    *msgp = *mbp->rdptr++;
+    if (mbp->rdptr >= mbp->top) {
+      mbp->rdptr = mbp->buffer;
+    }
+    mbp->cnt--;
+
+    /* If there is a writer waiting then makes it ready.*/
+    chThdDequeueNextI(&mbp->qw, MSG_OK);
+
+    return MSG_OK;
+  }
+
+  /* No message, immediate timeout.*/
+  return MSG_TIMEOUT;
+}
+#endif /* CH_CFG_USE_MAILBOXES == TRUE */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/src/chmemcore.c b/lib/chibios/os/oslib/src/chmemcore.c
new file mode 100644
index 000000000..0c081aa48
--- /dev/null
+++ b/lib/chibios/os/oslib/src/chmemcore.c
@@ -0,0 +1,227 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/src/chmemcore.c
+ * @brief   Core memory manager code.
+ *
+ * @addtogroup oslib_memcore
+ * @details Core Memory Manager related APIs and services.
+ *          <h2>Operation mode</h2>
+ *          The core memory manager is a simplified allocator that only
+ *          allows to allocate memory blocks without the possibility to
+ *          free them.<br>
+ *          This allocator is meant as a memory blocks provider for the
+ *          other allocators such as:
+ *          - C-Runtime allocator (through a compiler specific adapter module).
+ *          - Heap allocator (see @ref oslib_memheaps).
+ *          - Memory pools allocator (see @ref oslib_mempools).
+ *          .
+ *          By having a centralized memory provider the various allocators
+ *          can coexist and share the main memory.<br>
+ *          This allocator, alone, is also useful for very simple
+ *          applications that just require a simple way to get memory
+ *          blocks.
+ * @pre     In order to use the core memory manager APIs the @p CH_CFG_USE_MEMCORE
+ *          option must be enabled in @p chconf.h.
+ * @note    Compatible with RT and NIL.
+ * @{
+ */
+
+#include "ch.h"
+
+#if (CH_CFG_USE_MEMCORE == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module exported variables.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Memory core descriptor.
+ */
+memcore_t ch_memcore;
+
+/*===========================================================================*/
+/* Module local types.                                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local variables.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Low level memory manager initialization.
+ *
+ * @notapi
+ */
+void _core_init(void) {
+#if CH_CFG_MEMCORE_SIZE == 0
+  extern uint8_t __heap_base__[];
+  extern uint8_t __heap_end__[];
+
+  /*lint -save -e9033 [10.8] Required cast operations.*/
+  ch_memcore.basemem = __heap_base__;
+  ch_memcore.topmem  = __heap_end__;
+  /*lint restore*/
+#else
+  static uint8_t static_heap[CH_CFG_MEMCORE_SIZE];
+
+  ch_memcore.basemem = &static_heap[0];
+  ch_memcore.topmem  = &static_heap[CH_CFG_MEMCORE_SIZE];
+#endif
+}
+
+/**
+ * @brief   Allocates a memory block starting from the lowest address upward.
+ * @details This function allocates a block of @p offset + @p size bytes. The
+ *          returned pointer has @p offset bytes before its address and
+ *          @p size bytes after.
+ *
+ * @param[in] size      the size of the block to be allocated.
+ * @param[in] align     desired memory alignment
+ * @param[in] offset    aligned pointer offset
+ * @return              A pointer to the allocated memory block.
+ * @retval NULL         allocation failed, core memory exhausted.
+ *
+ * @iclass
+ */
+void *chCoreAllocFromBaseI(size_t size, unsigned align, size_t offset) {
+  uint8_t *p, *next;
+
+  chDbgCheckClassI();
+  chDbgCheck(MEM_IS_VALID_ALIGNMENT(align));
+
+  p = (uint8_t *)MEM_ALIGN_NEXT(ch_memcore.basemem + offset, align);
+  next = p + size;
+
+  /* Considering also the case where there is numeric overflow.*/
+  if ((next > ch_memcore.topmem) || (next < ch_memcore.basemem)) {
+    return NULL;
+  }
+
+  ch_memcore.basemem = next;
+
+  return p;
+}
+
+/**
+ * @brief   Allocates a memory block starting from the top address downward.
+ * @details This function allocates a block of @p offset + @p size bytes. The
+ *          returned pointer has @p offset bytes before its address and
+ *          @p size bytes after.
+ *
+ * @param[in] size      the size of the block to be allocated.
+ * @param[in] align     desired memory alignment
+ * @param[in] offset    aligned pointer offset
+ * @return              A pointer to the allocated memory block.
+ * @retval NULL         allocation failed, core memory exhausted.
+ *
+ * @iclass
+ */
+void *chCoreAllocFromTopI(size_t size, unsigned align, size_t offset) {
+  uint8_t *p, *prev;
+
+  chDbgCheckClassI();
+  chDbgCheck(MEM_IS_VALID_ALIGNMENT(align));
+
+  p = (uint8_t *)MEM_ALIGN_PREV(ch_memcore.topmem - size, align);
+  prev = p - offset;
+
+  /* Considering also the case where there is numeric overflow.*/
+  if ((prev < ch_memcore.basemem) || (prev > ch_memcore.topmem)) {
+    return NULL;
+  }
+
+  ch_memcore.topmem = prev;
+
+  return p;
+}
+
+/**
+ * @brief   Allocates a memory block starting from the lowest address upward.
+ * @details This function allocates a block of @p offset + @p size bytes. The
+ *          returned pointer has @p offset bytes before its address and
+ *          @p size bytes after.
+ *
+ * @param[in] size      the size of the block to be allocated.
+ * @param[in] align     desired memory alignment
+ * @param[in] offset    aligned pointer offset
+ * @return              A pointer to the allocated memory block.
+ * @retval NULL         allocation failed, core memory exhausted.
+ *
+ * @api
+ */
+void *chCoreAllocFromBase(size_t size, unsigned align, size_t offset) {
+  void *p;
+
+  chSysLock();
+  p = chCoreAllocFromBaseI(size, align, offset);
+  chSysUnlock();
+
+  return p;
+}
+
+/**
+ * @brief   Allocates a memory block starting from the top address downward.
+ * @details This function allocates a block of @p offset + @p size bytes. The
+ *          returned pointer has @p offset bytes before its address and
+ *          @p size bytes after.
+ *
+ * @param[in] size      the size of the block to be allocated.
+ * @param[in] align     desired memory alignment
+ * @param[in] offset    aligned pointer offset
+ * @return              A pointer to the allocated memory block.
+ * @retval NULL         allocation failed, core memory exhausted.
+ *
+ * @api
+ */
+void *chCoreAllocFromTop(size_t size, unsigned align, size_t offset) {
+  void *p;
+
+  chSysLock();
+  p = chCoreAllocFromTopI(size, align, offset);
+  chSysUnlock();
+
+  return p;
+}
+
+/**
+ * @brief   Core memory status.
+ *
+ * @return              The size, in bytes, of the free core memory.
+ *
+ * @xclass
+ */
+size_t chCoreGetStatusX(void) {
+
+  /*lint -save -e9033 [10.8] The cast is safe.*/
+  return (size_t)(ch_memcore.topmem - ch_memcore.basemem);
+  /*lint -restore*/
+}
+#endif /* CH_CFG_USE_MEMCORE == TRUE */
+
+/** @} */
diff --git a/lib/chibios/os/oslib/src/chmemheaps.c b/lib/chibios/os/oslib/src/chmemheaps.c
new file mode 100644
index 000000000..d5d272317
--- /dev/null
+++ b/lib/chibios/os/oslib/src/chmemheaps.c
@@ -0,0 +1,399 @@
+/*
+    ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
+              2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
+
+    This file is part of ChibiOS.
+
+    ChibiOS is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation version 3 of the License.
+
+    ChibiOS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    oslib/src/chmemheaps.c
+ * @brief   Memory heaps code.
+ *
+ * @addtogroup oslib_memheaps
+ * @details Heap Allocator related APIs.
+ *          <h2>Operation mode</h2>
+ *          The heap allocator implements a first-fit strategy and its APIs
+ *          are functionally equivalent to the usual @p malloc() and @p free()
+ *          library functions. The main difference is that the OS heap APIs
+ *          are guaranteed to be thread safe and there is the ability to
+ *          return memory blocks aligned to arbitrary powers of two.<br>
+ * @pre     In order to use the heap APIs the @p CH_CFG_USE_HEAP option must
+ *          be enabled in @p chconf.h.
+ * @note    Compatible with RT and NIL.
+ * @{
+ */
+
+#include "ch.h"
+
+#if (CH_CFG_USE_HEAP == TRUE) || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Module local definitions.                                                 */
+/*===========================================================================*/
+
+/*
+ * Defaults on the best synchronization mechanism available.
+ */
+#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
+#define H_LOCK(h)       chMtxLock(&(h)->mtx)
+#define H_UNLOCK(h)     chMtxUnlock(&(h)->mtx)
+#else
+#define H_LOCK(h)       (void) chSemWait(&(h)->sem)
+#define H_UNLOCK(h)     chSemSignal(&(h)->sem)
+#endif
+
+#define H_BLOCK(hp)     ((hp) + 1U)
+
+#define H_LIMIT(hp)     (H_BLOCK(hp) + H_PAGES(hp))
+
+#define H_NEXT(hp)      ((hp)->free.next)
+
+#define H_PAGES(hp)     ((hp)->free.pages)
+
+#define H_HEAP(hp)      ((hp)->used.heap)
+
+#define H_SIZE(hp)      ((hp)->used.size)
+
+/*
+ * Number of pages between two pointers in a MISRA-compatible way.
+ */
+#define NPAGES(p1, p2)                                                      \
+  /*lint -save -e9033 [10.8] The cast is safe.*/                            \
+  ((size_t)((p1) - (p2)))                                                   \
+  /*lint -restore*/
+
+/*===========================================================================*/
+/* Module exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local types.                                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local variables.                                                   */
+/*===========================================================================*/
+
+/**
+ * @brief   Default heap descriptor.
+ */
+static memory_heap_t default_heap;
+
+/*===========================================================================*/
+/* Module local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Initializes the default heap.
+ *
+ * @notapi
+ */
+void _heap_init(void) {
+
+  default_heap.provider = chCoreAllocAlignedWithOffset;
+  H_NEXT(&default_heap.header) = NULL;
+  H_PAGES(&default_heap.header) = 0;
+#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
+  chMtxObjectInit(&default_heap.mtx);
+#else
+  chSemObjectInit(&default_heap.sem, (cnt_t)1);
+#endif
+}
+
+/**
+ * @brief   Initializes a memory heap from a static memory area.
+ * @note    The heap buffer base and size are adjusted if the passed buffer
+ *          is not aligned to @p CH_HEAP_ALIGNMENT. This mean that the
+ *          effective heap size can be less than @p size.
+ *
+ * @param[out] heapp    pointer to the memory heap descriptor to be initialized
+ * @param[in] buf       heap buffer base
+ * @param[in] size      heap size
+ *
+ * @init
+ */
+void chHeapObjectInit(memory_heap_t *heapp, void *buf, size_t size) {
+  heap_header_t *hp = (heap_header_t *)MEM_ALIGN_NEXT(buf, CH_HEAP_ALIGNMENT);
+
+  chDbgCheck((heapp != NULL) && (size > 0U));
+
+  /* Adjusting the size in case the initial block was not correctly
+     aligned.*/
+  /*lint -save -e9033 [10.8] Required cast operations.*/
+  size -= (size_t)((uint8_t *)hp - (uint8_t *)buf);
+  /*lint restore*/
+
+  /* Initializing the heap header.*/
+  heapp->provider = NULL;
+  H_NEXT(&heapp->header) = hp;
+  H_PAGES(&heapp->header) = 0;
+  H_NEXT(hp) = NULL;
+  H_PAGES(hp) = (size - sizeof (heap_header_t)) / CH_HEAP_ALIGNMENT;
+#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
+  chMtxObjectInit(&heapp->mtx);
+#else
+  chSemObjectInit(&heapp->sem, (cnt_t)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 to the
+ *          specified alignment.
+ *
+ * @param[in] heapp     pointer to a heap descriptor or @p NULL in order to
+ *                      access the default heap.
+ * @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.
+ * @param[in] align     desired memory alignment
+ * @return              A pointer to the aligned allocated block.
+ * @retval NULL         if the block cannot be allocated.
+ *
+ * @api
+ */
+void *chHeapAllocAligned(memory_heap_t *heapp, size_t size, unsigned align) {
+  heap_header_t *qp, *hp, *ahp;
+  size_t pages;
+
+  chDbgCheck((size > 0U) && MEM_IS_VALID_ALIGNMENT(align));
+
+  /* If an heap is not specified then the default system header is used.*/
+  if (heapp == NULL) {
+    heapp = &default_heap;
+  }
+
+  /* Minimum alignment is constrained by the heap header structure size.*/
+  if (align < CH_HEAP_ALIGNMENT) {
+    align = CH_HEAP_ALIGNMENT;
+  }
+
+  /* Size is converted in number of elementary allocation units.*/
+  pages = MEM_ALIGN_NEXT(size, CH_HEAP_ALIGNMENT) / CH_HEAP_ALIGNMENT;
+
+  /* Taking heap mutex/semaphore.*/
+  H_LOCK(heapp);
+
+  /* Start of the free blocks list.*/
+  qp = &heapp->header;
+  while (H_NEXT(qp) != NULL) {
+
+    /* Next free block.*/
+    hp = H_NEXT(qp);
+
+    /* Pointer aligned to the requested alignment.*/
+    ahp = (heap_header_t *)MEM_ALIGN_NEXT(H_BLOCK(hp), align) - 1U;
+
+    if ((ahp < H_LIMIT(hp)) && (pages <= NPAGES(H_LIMIT(hp), ahp + 1U))) {
+      /* The block is large enough to contain a correctly aligned area
+         of sufficient size.*/
+
+      if (ahp > hp) {
+        /* The block is not properly aligned, must split it.*/
+        size_t bpages;
+
+        bpages = NPAGES(H_LIMIT(hp), H_BLOCK(ahp));
+        H_PAGES(hp) = NPAGES(ahp, H_BLOCK(hp));
+        if (bpages > pages) {
+          /* The block is bigger than required, must split the excess.*/
+          heap_header_t *fp;
+
+          /* Creating the excess block.*/
+          fp = H_BLOCK(ahp) + pages;
+          H_PAGES(fp) = (bpages - pages) - 1U;
+
+          /* Linking the excess block.*/
+          H_NEXT(fp) = H_NEXT(hp);
+          H_NEXT(hp) = fp;
+        }
+
+        hp = ahp;
+      }
+      else {
+        /* The block is already properly aligned.*/
+
+        if (H_PAGES(hp) == pages) {
+          /* Exact size, getting the whole block.*/
+          H_NEXT(qp) = H_NEXT(hp);
+        }
+        else {
+          /* The block is bigger than required, must split the excess.*/
+          heap_header_t *fp;
+
+          fp = H_BLOCK(hp) + pages;
+          H_NEXT(fp) = H_NEXT(hp);
+          H_PAGES(fp) = NPAGES(H_LIMIT(hp), H_BLOCK(fp));
+          H_NEXT(qp) = fp;
+        }
+      }
+
+      /* Setting in the block owner heap and size.*/
+      H_SIZE(hp) = size;
+      H_HEAP(hp) = heapp;
+
+      /* Releasing heap mutex/semaphore.*/
+      H_UNLOCK(heapp);
+
+      /*lint -save -e9087 [11.3] Safe cast.*/
+      return (void *)H_BLOCK(hp);
+      /*lint -restore*/
+    }
+
+    /* Next in the free blocks list.*/
+    qp = hp;
+  }
+
+  /* Releasing heap mutex/semaphore.*/
+  H_UNLOCK(heapp);
+
+  /* More memory is required, tries to get it from the associated provider
+     else fails.*/
+  if (heapp->provider != NULL) {
+    ahp = heapp->provider(pages * CH_HEAP_ALIGNMENT,
+                          align,
+                          sizeof (heap_header_t));
+    if (ahp != NULL) {
+      hp = ahp - 1U;
+      H_HEAP(hp) = heapp;
+      H_SIZE(hp) = size;
+
+      /*lint -save -e9087 [11.3] Safe cast.*/
+      return (void *)ahp;
+      /*lint -restore*/
+    }
+  }
+
+  return NULL;
+}
+
+/**
+ * @brief   Frees a previously allocated memory block.
+ *
+ * @param[in] p         pointer to the memory block to be freed
+ *
+ * @api
+ */
+void chHeapFree(void *p) {
+  heap_header_t *qp, *hp;
+  memory_heap_t *heapp;
+
+  chDbgCheck((p != NULL) && MEM_IS_ALIGNED(p, CH_HEAP_ALIGNMENT));
+
+  /*lint -save -e9087 [11.3] Safe cast.*/
+  hp = (heap_header_t *)p - 1U;
+  /*lint -restore*/
+  heapp = H_HEAP(hp);
+  qp = &heapp->header;
+
+  /* Size is converted in number of elementary allocation units.*/
+  H_PAGES(hp) = MEM_ALIGN_NEXT(H_SIZE(hp),
+                               CH_HEAP_ALIGNMENT) / CH_HEAP_ALIGNMENT;
+
+  /* Taking heap mutex/semaphore.*/
+  H_LOCK(heapp);