From 68f105375f65ffd347e3e322d46200cb389d8886 Mon Sep 17 00:00:00 2001
From: Gaurav-Aggarwal-AWS <33462878+aggarg@users.noreply.github.com>
Date: Tue, 28 Mar 2023 17:01:37 +0530
Subject: [PATCH] Only add alignment padding when needed (#650)

Heap 4 and Heap 5 add some padding to ensure that the allocated blocks
are always aligned to portBYTE_ALIGNMENT bytes. The code until now was
adding padding always even if the resulting block was already aligned.
This commits updates the code to only add padding if the resulting block
is not aligned.

Signed-off-by: Gaurav Aggarwal <aggarg@amazon.com>
---
 portable/MemMang/heap_4.c | 30 ++++++++++++++++++++++++------
 portable/MemMang/heap_5.c | 30 ++++++++++++++++++++++++------
 2 files changed, 48 insertions(+), 12 deletions(-)

diff --git a/portable/MemMang/heap_4.c b/portable/MemMang/heap_4.c
index 8ca7c81e4..c7a8209ed 100644
--- a/portable/MemMang/heap_4.c
+++ b/portable/MemMang/heap_4.c
@@ -159,13 +159,31 @@ void * pvPortMalloc( size_t xWantedSize )
         if( xWantedSize > 0 )
         {
             /* The wanted size must be increased so it can contain a BlockLink_t
-             * structure in addition to the requested amount of bytes. Some
-             * additional increment may also be needed for alignment. */
-            xAdditionalRequiredSize = xHeapStructSize + portBYTE_ALIGNMENT - ( xWantedSize & portBYTE_ALIGNMENT_MASK );
-
-            if( heapADD_WILL_OVERFLOW( xWantedSize, xAdditionalRequiredSize ) == 0 )
+             * structure in addition to the requested amount of bytes. */
+            if( heapADD_WILL_OVERFLOW( xWantedSize, xHeapStructSize ) == 0 )
             {
-                xWantedSize += xAdditionalRequiredSize;
+                xWantedSize += xHeapStructSize;
+
+                /* Ensure that blocks are always aligned to the required number
+                 * of bytes. */
+                if( ( xWantedSize & portBYTE_ALIGNMENT_MASK ) != 0x00 )
+                {
+                    /* Byte alignment required. */
+                    xAdditionalRequiredSize = portBYTE_ALIGNMENT - ( xWantedSize & portBYTE_ALIGNMENT_MASK );
+
+                    if( heapADD_WILL_OVERFLOW( xWantedSize, xAdditionalRequiredSize ) == 0 )
+                    {
+                        xWantedSize += xAdditionalRequiredSize;
+                    }
+                    else
+                    {
+                        xWantedSize = 0;
+                    }
+                }
+                else
+                {
+                    mtCOVERAGE_TEST_MARKER();
+                }
             }
             else
             {
diff --git a/portable/MemMang/heap_5.c b/portable/MemMang/heap_5.c
index 3a1df9b64..db9e1eb37 100644
--- a/portable/MemMang/heap_5.c
+++ b/portable/MemMang/heap_5.c
@@ -170,13 +170,31 @@ void * pvPortMalloc( size_t xWantedSize )
         if( xWantedSize > 0 )
         {
             /* The wanted size must be increased so it can contain a BlockLink_t
-             * structure in addition to the requested amount of bytes. Some
-             * additional increment may also be needed for alignment. */
-            xAdditionalRequiredSize = xHeapStructSize + portBYTE_ALIGNMENT - ( xWantedSize & portBYTE_ALIGNMENT_MASK );
-
-            if( heapADD_WILL_OVERFLOW( xWantedSize, xAdditionalRequiredSize ) == 0 )
+             * structure in addition to the requested amount of bytes. */
+            if( heapADD_WILL_OVERFLOW( xWantedSize, xHeapStructSize ) == 0 )
             {
-                xWantedSize += xAdditionalRequiredSize;
+                xWantedSize += xHeapStructSize;
+
+                /* Ensure that blocks are always aligned to the required number
+                 * of bytes. */
+                if( ( xWantedSize & portBYTE_ALIGNMENT_MASK ) != 0x00 )
+                {
+                    /* Byte alignment required. */
+                    xAdditionalRequiredSize = portBYTE_ALIGNMENT - ( xWantedSize & portBYTE_ALIGNMENT_MASK );
+
+                    if( heapADD_WILL_OVERFLOW( xWantedSize, xAdditionalRequiredSize ) == 0 )
+                    {
+                        xWantedSize += xAdditionalRequiredSize;
+                    }
+                    else
+                    {
+                        xWantedSize = 0;
+                    }
+                }
+                else
+                {
+                    mtCOVERAGE_TEST_MARKER();
+                }
             }
             else
             {