From 3e69c4113c57948aa6931cd5871f6852580cb94b Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Tue, 26 Jan 2021 10:58:38 +0000
Subject: [PATCH] Fixed bug #1141.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/branches/stable_20.3.x@14027 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 os/oslib/src/chfactory.c | 30 +++++++++++++++++++++---------
 readme.txt               |  3 ++-
 2 files changed, 23 insertions(+), 10 deletions(-)

diff --git a/os/oslib/src/chfactory.c b/os/oslib/src/chfactory.c
index 1a80e8ecf..ed09131f1 100644
--- a/os/oslib/src/chfactory.c
+++ b/os/oslib/src/chfactory.c
@@ -135,7 +135,8 @@ static dyn_element_t *dyn_list_unlink(dyn_element_t *element,
 #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) {
+                                             size_t size,
+                                             unsigned align) {
   dyn_element_t *dep;
 
   chDbgCheck(name != NULL);
@@ -147,7 +148,7 @@ static dyn_element_t *dyn_create_object_heap(const char *name,
   }
 
   /* Allocating space for the new buffer object.*/
-  dep = (dyn_element_t *)chHeapAlloc(NULL, size);
+  dep = (dyn_element_t *)chHeapAllocAligned(NULL, size, align);
   if (dep == NULL) {
     return NULL;
   }
@@ -421,7 +422,8 @@ dyn_buffer_t *chFactoryCreateBuffer(const char *name, size_t size) {
 
   dbp = (dyn_buffer_t *)dyn_create_object_heap(name,
                                                &ch_factory.buf_list,
-                                               size);
+                                               sizeof (dyn_buffer_t) + size,
+                                               CH_HEAP_ALIGNMENT);
   if (dbp != NULL) {
     /* Initializing buffer object data.*/
     memset((void *)(dbp + 1), 0, size);
@@ -582,7 +584,8 @@ dyn_mailbox_t *chFactoryCreateMailbox(const char *name, size_t n) {
   dmp = (dyn_mailbox_t *)dyn_create_object_heap(name,
                                                 &ch_factory.mbx_list,
                                                 sizeof (dyn_mailbox_t) +
-                                                (n * sizeof (msg_t)));
+                                                (n * sizeof (msg_t)),
+                                                CH_HEAP_ALIGNMENT);
   if (dmp != NULL) {
     /* Initializing mailbox object data.*/
     chMBObjectInit(&dmp->mbx, (msg_t *)(dmp + 1), n);
@@ -662,21 +665,29 @@ 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,
-                                                      sizeof (dyn_objects_fifo_t) +
-                                                      (objn * sizeof (msg_t)) +
-                                                      (objn * objsize));
+                                                      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 *)&msgbuf[objn], msgbuf);
+                            (void *)objbuf, msgbuf);
   }
 
   F_UNLOCK();
@@ -755,7 +766,8 @@ dyn_pipe_t *chFactoryCreatePipe(const char *name, size_t size) {
 
   dpp = (dyn_pipe_t *)dyn_create_object_heap(name,
                                              &ch_factory.pipe_list,
-                                             sizeof (dyn_pipe_t) + size);
+                                             sizeof (dyn_pipe_t) + size,
+                                             CH_HEAP_ALIGNMENT);
   if (dpp != NULL) {
     /* Initializing mailbox object data.*/
     chPipeObjectInit(&dpp->pipe, (uint8_t *)(dpp + 1), size);
diff --git a/readme.txt b/readme.txt
index f93df008d..ccaf24c79 100644
--- a/readme.txt
+++ b/readme.txt
@@ -74,9 +74,10 @@
 *****************************************************************************
 
 *** 20.3.3 ***
-- FIX: Fixed chsnprintf() sign mode/filler mode conflict (bug #1140)
 - NEW: RT, NIL: Renamed _THREADS_QUEUE_DECL to THREADS_QUEUE_DECL for
        consistency.
+- FIX: Fixed Heap allocation of aligned FIFO objects in chFactory (bug #1141).
+- FIX: Fixed chsnprintf() sign mode/filler mode conflict (bug #1140).
 - FIX: Fixed GCC 10 causes warning in factory module (bug #1139).
 - FIX: Fixed C strict aliasing rules violation causes test cases to fail
        (bug #1138).