Tree reorganization.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@8899 35acf78f-673a-0410-8e92-d51de3d6d3f4

5 files changed, 1337 insertions, 0 deletions

diff --git a/os/common/oslib/src/chdynamic.c b/os/common/oslib/src/chdynamic.c
new file mode 100644
index 000000000..7d8b609be
--- /dev/null
+++ b/os/common/oslib/src/chdynamic.c
@@ -0,0 +1,150 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio.

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+*/

+

+/**

+ * @file    chdynamic.c

+ * @brief   Dynamic threads code.

+ *

+ * @addtogroup dynamic_threads

+ * @details Dynamic threads related APIs and services.

+ * @note    Compatible with RT only.

+ * @{

+ */

+

+#include "ch.h"

+

+#if (CH_CFG_USE_DYNAMIC == TRUE) || defined(__DOXYGEN__)

+

+/*===========================================================================*/

+/* Module local definitions.                                                 */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module local types.                                                       */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module local variables.                                                   */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module local functions.                                                   */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module exported functions.                                                */

+/*===========================================================================*/

+

+#if (CH_CFG_USE_HEAP == TRUE) || defined(__DOXYGEN__)

+/**

+ * @brief   Creates a new thread allocating the memory from the heap.

+ * @pre     The configuration options @p CH_CFG_USE_DYNAMIC and

+ *          @p CH_CFG_USE_HEAP must be enabled in order to use this function.

+ * @note    A thread can terminate by calling @p chThdExit() or by simply

+ *          returning from its main function.

+ * @note    The memory allocated for the thread is not released automatically,

+ *          it is responsibility of the creator thread to call @p chThdWait()

+ *          and then release the allocated memory.

+ *

+ * @param[in] heapp     heap from which allocate the memory or @p NULL for the

+ *                      default heap

+ * @param[in] size      size of the working area to be allocated

+ * @param[in] prio      the priority level for the new thread

+ * @param[in] pf        the thread function

+ * @param[in] arg       an argument passed to the thread function. It can be

+ *                      @p NULL.

+ * @return              The pointer to the @p thread_t structure allocated for

+ *                      the thread into the working space area.

+ * @retval NULL         if the memory cannot be allocated.

+ *

+ * @api

+ */

+thread_t *chThdCreateFromHeap(memory_heap_t *heapp, size_t size,

+                              tprio_t prio, tfunc_t pf, void *arg) {

+  void *wsp;

+

+  wsp = chHeapAllocAligned(heapp, size, PORT_WORKING_AREA_ALIGN);

+  if (wsp == NULL) {

+    return NULL;

+  }

+

+#if CH_DBG_FILL_THREADS == TRUE

+  _thread_memfill((uint8_t *)wsp,

+                  (uint8_t *)wsp + size,

+                  CH_DBG_STACK_FILL_VALUE);

+#endif

+

+  return chThdCreateStatic(wsp, size, prio, pf, arg);

+}

+#endif /* CH_CFG_USE_HEAP == TRUE */

+

+#if (CH_CFG_USE_MEMPOOLS == TRUE) || defined(__DOXYGEN__)

+/**

+ * @brief   Creates a new thread allocating the memory from the specified

+ *          memory pool.

+ * @pre     The configuration options @p CH_CFG_USE_DYNAMIC and

+ *          @p CH_CFG_USE_MEMPOOLS must be enabled in order to use this

+ *          function.

+ * @pre     The pool must be initialized to contain only objects with

+ *          alignment @p PORT_WORKING_AREA_ALIGN.

+ * @note    A thread can terminate by calling @p chThdExit() or by simply

+ *          returning from its main function.

+ * @note    The memory allocated for the thread is not released automatically,

+ *          it is responsibility of the creator thread to call @p chThdWait()

+ *          and then release the allocated memory.

+ *

+ * @param[in] mp        pointer to the memory pool object

+ * @param[in] prio      the priority level for the new thread

+ * @param[in] pf        the thread function

+ * @param[in] arg       an argument passed to the thread function. It can be

+ *                      @p NULL.

+ * @return              The pointer to the @p thread_t structure allocated for

+ *                      the thread into the working space area.

+ * @retval  NULL        if the memory pool is empty.

+ *

+ * @api

+ */

+thread_t *chThdCreateFromMemoryPool(memory_pool_t *mp, tprio_t prio,

+                                    tfunc_t pf, void *arg) {

+  void *wsp;

+

+  chDbgCheck(mp != NULL);

+

+  wsp = chPoolAlloc(mp);

+  if (wsp == NULL) {

+    return NULL;

+  }

+

+#if CH_DBG_FILL_THREADS == TRUE

+  _thread_memfill((uint8_t *)wsp,

+                  (uint8_t *)wsp + mp->object_size,

+                  CH_DBG_STACK_FILL_VALUE);

+#endif

+

+  return chThdCreateStatic(wsp, mp->object_size, prio, pf, arg);

+}

+#endif /* CH_CFG_USE_MEMPOOLS == TRUE */

+

+#endif /* CH_CFG_USE_DYNAMIC == TRUE */

+

+/** @} */

diff --git a/os/common/oslib/src/chheap.c b/os/common/oslib/src/chheap.c
new file mode 100644
index 000000000..a35b646ba
--- /dev/null
+++ b/os/common/oslib/src/chheap.c
@@ -0,0 +1,383 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio.

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+*/

+

+/**

+ * @file    chheap.c

+ * @brief   Heaps code.

+ *

+ * @addtogroup heaps

+ * @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)

+

+/*===========================================================================*/

+/* 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 = chCoreAllocAligned;

+  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.

+ * @pre     Both the heap buffer base and the heap size must be aligned to

+ *          the @p heap_header_t type 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 = buf;

+

+  chDbgCheck((heapp != NULL) && (size > 0U) &&

+             MEM_IS_ALIGNED(buf, CH_HEAP_ALIGNMENT) &&

+             MEM_IS_ALIGNED(size, CH_HEAP_ALIGNMENT));

+

+  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;

+  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) {

+    heap_header_t *ahp;

+

+    /* 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 <= (size_t)(H_LIMIT(hp) - 1U - ahp))) {

+      /* 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 = H_LIMIT(hp) - H_BLOCK(ahp);

+        H_PAGES(hp) = 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) = 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) {

+    hp = heapp->provider((pages + 1U) * CH_HEAP_ALIGNMENT, align);

+    if (hp != NULL) {

+      H_HEAP(hp) = heapp;

+      H_SIZE(hp) = size;

+

+      /*lint -save -e9087 [11.3] Safe cast.*/

+      return (void *)H_BLOCK(hp);

+      /*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);

+

+  while (true) {

+    chDbgAssert((hp < qp) || (hp >= H_LIMIT(qp)), "within free block");

+

+    if (((qp == &heapp->header) || (hp > qp)) &&

+        ((H_NEXT(qp) == NULL) || (hp < H_NEXT(qp)))) {

+      /* Insertion after qp.*/

+      H_NEXT(hp) = H_NEXT(qp);

+      H_NEXT(qp) = hp;

+      /* Verifies if the newly inserted block should be merged.*/

+      if (H_LIMIT(hp) == H_NEXT(hp)) {

+        /* Merge with the next block.*/

+        H_PAGES(hp) += H_PAGES(H_NEXT(hp)) + 1U;

+        H_NEXT(hp) = H_NEXT(H_NEXT(hp));

+      }

+      if ((H_LIMIT(qp) == hp)) {

+        /* Merge with the previous block.*/

+        H_PAGES(qp) += H_PAGES(hp) + 1U;

+        H_NEXT(qp) = H_NEXT(hp);

+      }

+      break;

+    }

+    qp = H_NEXT(qp);

+  }

+

+  /* Releasing heap mutex/semaphore.*/

+  H_UNLOCK(heapp);

+

+  return;

+}

+

+/**

+ * @brief   Reports the heap status.

+ * @note    This function is meant to be used in the test suite, it should

+ *          not be really useful for the application code.

+ *

+ * @param[in] heapp     pointer to a heap descriptor or @p NULL in order to

+ *                      access the default heap.

+ * @param[in] totalp    pointer to a variable that will receive the total

+ *                      fragmented free space or @ NULL

+ * @param[in] largestp  pointer to a variable that will receive the largest

+ *                      free free block found space or @ NULL

+ * @return              The number of fragments in the heap.

+ *

+ * @api

+ */

+size_t chHeapStatus(memory_heap_t *heapp, size_t *totalp, size_t *largestp) {

+  heap_header_t *qp;

+  size_t n, tpages, lpages;

+

+  if (heapp == NULL) {

+    heapp = &default_heap;

+  }

+

+  H_LOCK(heapp);

+  tpages = 0U;

+  lpages = 0U;

+  n = 0U;

+  qp = &heapp->header;

+  while (H_NEXT(qp) != NULL) {

+    size_t pages = H_PAGES(H_NEXT(qp));

+

+    /* Updating counters.*/

+    n++;

+    tpages += pages;

+    if (pages > lpages) {

+      lpages = pages;

+    }

+

+    qp = H_NEXT(qp);

+  }

+

+  /* Writing out fragmented free memory.*/

+  if (totalp != NULL) {

+    *totalp = tpages * CH_HEAP_ALIGNMENT;

+  }

+

+  /* Writing out unfragmented free memory.*/

+  if (largestp != NULL) {

+    *largestp = lpages * CH_HEAP_ALIGNMENT;

+  }

+  H_UNLOCK(heapp);

+

+  return n;

+}

+

+#endif /* CH_CFG_USE_HEAP == TRUE */

+

+/** @} */

diff --git a/os/common/oslib/src/chmboxes.c b/os/common/oslib/src/chmboxes.c
new file mode 100644
index 000000000..bb75fee6c
--- /dev/null
+++ b/os/common/oslib/src/chmboxes.c
@@ -0,0 +1,435 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio.

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+*/

+

+/**

+ * @file    chmboxes.c

+ * @brief   Mailboxes code.

+ *

+ * @addtogroup 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, cnt_t n) {

+

+  chDbgCheck((mbp != NULL) && (buf != NULL) && (n > (cnt_t)0));

+

+  mbp->buffer = buf;

+  mbp->rdptr = buf;

+  mbp->wrptr = buf;

+  mbp->top = &buf[n];

+  chSemObjectInit(&mbp->emptysem, n);

+  chSemObjectInit(&mbp->fullsem, (cnt_t)0);

+}

+

+/**

+ * @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.

+ *

+ * @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.

+ *

+ * @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;

+  chSemResetI(&mbp->emptysem, (cnt_t)(mbp->top - mbp->buffer));

+  chSemResetI(&mbp->fullsem, (cnt_t)0);

+}

+

+/**

+ * @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 while waiting.

+ * @retval MSG_TIMEOUT  if the operation has timed out.

+ *

+ * @api

+ */

+msg_t chMBPost(mailbox_t *mbp, msg_t msg, systime_t timeout) {

+  msg_t rdymsg;

+

+  chSysLock();

+  rdymsg = chMBPostS(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 while waiting.

+ * @retval MSG_TIMEOUT  if the operation has timed out.

+ *

+ * @sclass

+ */

+msg_t chMBPostS(mailbox_t *mbp, msg_t msg, systime_t timeout) {

+  msg_t rdymsg;

+

+  chDbgCheckClassS();

+  chDbgCheck(mbp != NULL);

+

+  rdymsg = chSemWaitTimeoutS(&mbp->emptysem, timeout);

+  if (rdymsg == MSG_OK) {

+    *mbp->wrptr++ = msg;

+    if (mbp->wrptr >= mbp->top) {

+      mbp->wrptr = mbp->buffer;

+    }

+    chSemSignalI(&mbp->fullsem);

+    chSchRescheduleS();

+  }

+

+  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_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 (chSemGetCounterI(&mbp->emptysem) <= (cnt_t)0) {

+    return MSG_TIMEOUT;

+  }

+

+  chSemFastWaitI(&mbp->emptysem);

+  *mbp->wrptr++ = msg;

+  if (mbp->wrptr >= mbp->top) {

+     mbp->wrptr = mbp->buffer;

+  }

+  chSemSignalI(&mbp->fullsem);

+

+  return MSG_OK;

+}

+

+/**

+ * @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 while waiting.

+ * @retval MSG_TIMEOUT  if the operation has timed out.

+ *

+ * @api

+ */

+msg_t chMBPostAhead(mailbox_t *mbp, msg_t msg, systime_t timeout) {

+  msg_t rdymsg;

+

+  chSysLock();

+  rdymsg = chMBPostAheadS(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 while waiting.

+ * @retval MSG_TIMEOUT  if the operation has timed out.

+ *

+ * @sclass

+ */

+msg_t chMBPostAheadS(mailbox_t *mbp, msg_t msg, systime_t timeout) {

+  msg_t rdymsg;

+

+  chDbgCheckClassS();

+  chDbgCheck(mbp != NULL);

+

+  rdymsg = chSemWaitTimeoutS(&mbp->emptysem, timeout);

+  if (rdymsg == MSG_OK) {

+    if (--mbp->rdptr < mbp->buffer) {

+      mbp->rdptr = mbp->top - 1;

+    }

+    *mbp->rdptr = msg;

+    chSemSignalI(&mbp->fullsem);

+    chSchRescheduleS();

+  }

+

+  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_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 (chSemGetCounterI(&mbp->emptysem) <= (cnt_t)0) {

+    return MSG_TIMEOUT;

+  }

+  chSemFastWaitI(&mbp->emptysem);

+  if (--mbp->rdptr < mbp->buffer) {

+    mbp->rdptr = mbp->top - 1;

+  }

+  *mbp->rdptr = msg;

+  chSemSignalI(&mbp->fullsem);

+

+  return MSG_OK;

+}

+

+/**

+ * @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 while waiting.

+ * @retval MSG_TIMEOUT  if the operation has timed out.

+ *

+ * @api

+ */

+msg_t chMBFetch(mailbox_t *mbp, msg_t *msgp, systime_t timeout) {

+  msg_t rdymsg;

+

+  chSysLock();

+  rdymsg = chMBFetchS(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 while waiting.

+ * @retval MSG_TIMEOUT  if the operation has timed out.

+ *

+ * @sclass

+ */

+msg_t chMBFetchS(mailbox_t *mbp, msg_t *msgp, systime_t timeout) {

+  msg_t rdymsg;

+

+  chDbgCheckClassS();

+  chDbgCheck((mbp != NULL) && (msgp != NULL));

+

+  rdymsg = chSemWaitTimeoutS(&mbp->fullsem, timeout);

+  if (rdymsg == MSG_OK) {

+    *msgp = *mbp->rdptr++;

+    if (mbp->rdptr >= mbp->top) {

+      mbp->rdptr = mbp->buffer;

+    }

+    chSemSignalI(&mbp->emptysem);

+    chSchRescheduleS();

+  }

+

+  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_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 (chSemGetCounterI(&mbp->fullsem) <= (cnt_t)0) {

+    return MSG_TIMEOUT;

+  }

+  chSemFastWaitI(&mbp->fullsem);

+  *msgp = *mbp->rdptr++;

+  if (mbp->rdptr >= mbp->top) {

+    mbp->rdptr = mbp->buffer;

+  }

+  chSemSignalI(&mbp->emptysem);

+

+  return MSG_OK;

+}

+#endif /* CH_CFG_USE_MAILBOXES == TRUE */

+

+/** @} */

diff --git a/os/common/oslib/src/chmemcore.c b/os/common/oslib/src/chmemcore.c
new file mode 100644
index 000000000..a665b07b2
--- /dev/null
+++ b/os/common/oslib/src/chmemcore.c
@@ -0,0 +1,166 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio.

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+*/

+

+/**

+ * @file    chmemcore.c

+ * @brief   Core memory manager code.

+ *

+ * @addtogroup 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 heaps).

+ *          - Memory pools allocator (see @ref pools).

+ *          .

+ *          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.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module local types.                                                       */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module local variables.                                                   */

+/*===========================================================================*/

+

+static uint8_t *nextmem;

+static uint8_t *endmem;

+

+/*===========================================================================*/

+/* 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.*/

+  nextmem = (uint8_t *)MEM_ALIGN_NEXT(__heap_base__, PORT_NATURAL_ALIGN);

+  endmem = (uint8_t *)MEM_ALIGN_PREV(__heap_end__, PORT_NATURAL_ALIGN);

+  /*lint restore*/

+#else

+  static stkalign_t buffer[MEM_ALIGN_NEXT(CH_CFG_MEMCORE_SIZE) /

+                           PORT_NATURAL_ALIGN];

+

+  nextmem = (uint8_t *)&buffer[0];

+  endmem = (uint8_t *)&buffer[MEM_ALIGN_NEXT(CH_CFG_MEMCORE_SIZE) /

+                              PORT_NATURAL_ALIGN];

+#endif

+}

+

+/**

+ * @brief   Allocates a memory block.

+ * @details The allocated block is guaranteed to be properly aligned to the

+ *          specified alignment.

+ *

+ * @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

+ */

+void *chCoreAllocAlignedI(size_t size, unsigned align) {

+  uint8_t *p;

+

+  chDbgCheckClassI();

+  chDbgCheck(MEM_IS_VALID_ALIGNMENT(align));

+

+  size = MEM_ALIGN_NEXT(size, align);

+  p = (uint8_t *)MEM_ALIGN_NEXT(nextmem, align);

+

+  /* ---????? lint -save -e9033 [10.8] The cast is safe.*/

+  if ((size_t)(endmem - p) < size) {

+  /* ---????? lint -restore*/

+    return NULL;

+  }

+  nextmem = p + size;

+

+  return p;

+}

+

+/**

+ * @brief   Allocates a memory block.

+ * @details The allocated block is guaranteed to be properly aligned to the

+ *          specified alignment.

+ *

+ * @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

+ */

+void *chCoreAllocAligned(size_t size, unsigned align) {

+  void *p;

+

+  chSysLock();

+  p = chCoreAllocAlignedI(size, align);

+  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)(endmem - nextmem);

+  /*lint -restore*/

+}

+#endif /* CH_CFG_USE_MEMCORE == TRUE */

+

+/** @} */

diff --git a/os/common/oslib/src/chmempools.c b/os/common/oslib/src/chmempools.c
new file mode 100644
index 000000000..c29fde8f4
--- /dev/null
+++ b/os/common/oslib/src/chmempools.c
@@ -0,0 +1,203 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio.

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+*/

+

+/**

+ * @file    chmempools.c

+ * @brief   Memory Pools code.

+ *

+ * @addtogroup pools

+ * @details Memory Pools related APIs and services.

+ *          <h2>Operation mode</h2>

+ *          The Memory Pools APIs allow to allocate/free fixed size objects in

+ *          <b>constant time</b> and reliably without memory fragmentation

+ *          problems.<br>

+ *          Memory Pools do not enforce any alignment constraint on the

+ *          contained object however the objects must be properly aligned

+ *          to contain a pointer to void.

+ * @pre     In order to use the memory pools APIs the @p CH_CFG_USE_MEMPOOLS option

+ *          must be enabled in @p chconf.h.

+ * @note    Compatible with RT and NIL.

+ * @{

+ */

+

+#include "ch.h"

+

+#if (CH_CFG_USE_MEMPOOLS == TRUE) || defined(__DOXYGEN__)

+

+/*===========================================================================*/

+/* Module exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module local types.                                                       */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module local variables.                                                   */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module local functions.                                                   */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Module exported 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

+ */

+void chPoolObjectInit(memory_pool_t *mp, size_t size, memgetfunc_t provider) {

+

+  chDbgCheck((mp != NULL) && (size >= sizeof(void *)));

+

+  mp->next = NULL;

+  mp->object_size = size;

+  mp->provider = provider;

+}

+

+/**

+ * @brief   Loads a memory pool with an array of static objects.

+ * @pre     The memory pool must be already been initialized.

+ * @pre     The array elements must be of the right size for the specified

+ *          memory pool.

+ * @post    The memory pool contains the elements of the input array.

+ *

+ * @param[in] mp        pointer to a @p memory_pool_t structure

+ * @param[in] p         pointer to the array first element

+ * @param[in] n         number of elements in the array

+ *

+ * @api

+ */

+void chPoolLoadArray(memory_pool_t *mp, void *p, size_t n) {

+

+  chDbgCheck((mp != NULL) && (n != 0U));

+

+  while (n != 0U) {

+    chPoolAdd(mp, p);

+    /*lint -save -e9087 [11.3] Safe cast.*/

+    p = (void *)(((uint8_t *)p) + mp->object_size);

+    /*lint -restore*/

+    n--;

+  }

+}

+

+/**

+ * @brief   Allocates an object from a memory pool.

+ * @pre     The memory pool must be already been initialized.

+ *

+ * @param[in] mp        pointer to a @p memory_pool_t structure

+ * @return              The pointer to the allocated object.

+ * @retval NULL         if pool is empty.

+ *

+ * @iclass

+ */

+void *chPoolAllocI(memory_pool_t *mp) {

+  void *objp;

+

+  chDbgCheckClassI();

+  chDbgCheck(mp != NULL);

+

+  objp = mp->next;

+  /*lint -save -e9013 [15.7] There is no else because it is not needed.*/

+  if (objp != NULL) {

+    mp->next = mp->next->next;

+  }

+  else if (mp->provider != NULL) {

+    objp = mp->provider(mp->object_size, PORT_NATURAL_ALIGN); /* TODO: Alignment is not properly handled */

+  }

+  /*lint -restore*/

+

+  return objp;

+}

+

+/**

+ * @brief   Allocates an object from a memory pool.

+ * @pre     The memory pool must be already been initialized.

+ *

+ * @param[in] mp        pointer to a @p memory_pool_t structure

+ * @return              The pointer to the allocated object.

+ * @retval NULL         if pool is empty.

+ *

+ * @api

+ */

+void *chPoolAlloc(memory_pool_t *mp) {

+  void *objp;

+

+  chSysLock();

+  objp = chPoolAllocI(mp);

+  chSysUnlock();

+

+  return objp;

+}

+

+/**

+ * @brief   Releases an object into a memory pool.

+ * @pre     The memory pool must be already been initialized.

+ * @pre     The freed object must be of the right size for the specified

+ *          memory pool.

+ * @pre     The object must be properly aligned to contain a pointer to void.

+ *

+ * @param[in] mp        pointer to a @p memory_pool_t structure

+ * @param[in] objp      the pointer to the object to be released

+ *

+ * @iclass

+ */

+void chPoolFreeI(memory_pool_t *mp, void *objp) {

+  struct pool_header *php = objp;

+

+  chDbgCheckClassI();

+  chDbgCheck((mp != NULL) && (objp != NULL));

+

+  php->next = mp->next;

+  mp->next = php;

+}

+

+/**

+ * @brief   Releases an object into a memory pool.

+ * @pre     The memory pool must be already been initialized.

+ * @pre     The freed object must be of the right size for the specified

+ *          memory pool.

+ * @pre     The object must be properly aligned to contain a pointer to void.

+ *

+ * @param[in] mp        pointer to a @p memory_pool_t structure

+ * @param[in] objp      the pointer to the object to be released

+ *

+ * @api

+ */

+void chPoolFree(memory_pool_t *mp, void *objp) {

+

+  chSysLock();

+  chPoolFreeI(mp, objp);

+  chSysUnlock();

+}

+

+#endif /* CH_CFG_USE_MEMPOOLS == TRUE */

+

+/** @} */