From 6d419676cde900b91f8de3cbfd8664ffa81232d7 Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Wed, 4 Oct 2017 07:54:36 +0000
Subject: Alignment capability for memory pools.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@10759 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/common/oslib/src/chfactory.c  |  9 ++++++---
 os/common/oslib/src/chmempools.c | 19 +++++++++++++++----
 2 files changed, 21 insertions(+), 7 deletions(-)

(limited to 'os/common/oslib/src')

diff --git a/os/common/oslib/src/chfactory.c b/os/common/oslib/src/chfactory.c
index 5e7604917..45a6a0681 100644
--- a/os/common/oslib/src/chfactory.c
+++ b/os/common/oslib/src/chfactory.c
@@ -581,7 +581,9 @@ void chFactoryReleaseMailbox(dyn_mailbox_t *dmp) {
  *
  * @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
@@ -592,7 +594,8 @@ void chFactoryReleaseMailbox(dyn_mailbox_t *dmp) {
  */
 dyn_objects_fifo_t *chFactoryCreateObjectsFIFO(const char *name,
                                                size_t objsize,
-                                               size_t objn) {
+                                               size_t objn,
+                                               unsigned objalign) {
   dyn_objects_fifo_t *dofp;
 
   chSysLock();
@@ -604,7 +607,7 @@ dyn_objects_fifo_t *chFactoryCreateObjectsFIFO(const char *name,
                                                       (objn * objsize));
   if (dofp != NULL) {
     /* Initializing mailbox object data.*/
-    chFifoObjectInit(&dofp->fifo, objsize, objn,
+    chFifoObjectInit(&dofp->fifo, objsize, objn, objalign,
                      dofp->msgbuf, (void *)&dofp->msgbuf[objn]);
   }
 
diff --git a/os/common/oslib/src/chmempools.c b/os/common/oslib/src/chmempools.c
index 384e2f7ee..aec47400c 100644
--- a/os/common/oslib/src/chmempools.c
+++ b/os/common/oslib/src/chmempools.c
@@ -67,18 +67,21 @@
  * @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] 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
  *
  * @init
  */
-void chPoolObjectInit(memory_pool_t *mp, size_t size, memgetfunc_t provider) {
+void chPoolObjectInitAligned(memory_pool_t *mp, size_t size,
+                             unsigned align, memgetfunc_t provider) {
 
   chDbgCheck((mp != NULL) && (size >= sizeof(void *)));
 
   mp->next = NULL;
   mp->object_size = size;
+  mp->align = align;
   mp->provider = provider;
 }
 
@@ -87,6 +90,8 @@ void chPoolObjectInit(memory_pool_t *mp, size_t size, memgetfunc_t provider) {
  * @pre     The memory pool must be already been initialized.
  * @pre     The array elements must be of the right size for the specified
  *          memory pool.
+ * @pre     The array elements size must be a multiple of the alignment
+ *          requirement for the pool.
  * @post    The memory pool contains the elements of the input array.
  *
  * @param[in] mp        pointer to a @p memory_pool_t structure
@@ -130,7 +135,7 @@ void *chPoolAllocI(memory_pool_t *mp) {
     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 */
+    objp = mp->provider(mp->object_size, mp->align);
   }
   /*lint -restore*/
 
@@ -175,6 +180,9 @@ void chPoolFreeI(memory_pool_t *mp, void *objp) {
   chDbgCheckClassI();
   chDbgCheck((mp != NULL) && (objp != NULL));
 
+  chDbgAssert(((size_t)objp & MEM_ALIGN_MASK(mp->align)) == 0U,
+              "unaligned object");
+
   php->next = mp->next;
   mp->next = php;
 }
@@ -206,12 +214,15 @@ void chPoolFree(memory_pool_t *mp, void *objp) {
  * @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.
+ * @param[in] align     required memory alignment
  *
  * @init
  */
-void chGuardedPoolObjectInit(guarded_memory_pool_t *gmp, size_t size) {
+void chGuardedPoolObjectInitAligned(guarded_memory_pool_t *gmp,
+                                    size_t size,
+                                    unsigned align) {
 
-  chPoolObjectInit(&gmp->pool, size, NULL);
+  chPoolObjectInitAligned(&gmp->pool, size, align, NULL);
   chSemObjectInit(&gmp->sem, (cnt_t)0);
 }
 
-- 
cgit v1.2.3