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

@@ -159,9 +159,17 @@ 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 );
+             * structure in addition to the requested amount of bytes. */
+            if( heapADD_WILL_OVERFLOW( xWantedSize, xHeapStructSize ) == 0 )
+            {
+                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 )
                     {
@@ -176,6 +184,16 @@ void * pvPortMalloc( size_t xWantedSize )
                 {
                     mtCOVERAGE_TEST_MARKER();
                 }
+            }
+            else
+            {
+                xWantedSize = 0;
+            }
+        }
+        else
+        {
+            mtCOVERAGE_TEST_MARKER();
+        }
 
         /* Check the block size we are trying to allocate is not so large that the
          * top bit is set.  The top bit of the block size member of the BlockLink_t

portable/MemMang/heap_5.c

@@ -170,9 +170,17 @@ 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 );
+             * structure in addition to the requested amount of bytes. */
+            if( heapADD_WILL_OVERFLOW( xWantedSize, xHeapStructSize ) == 0 )
+            {
+                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 )
                     {
@@ -187,6 +195,16 @@ void * pvPortMalloc( size_t xWantedSize )
                 {
                     mtCOVERAGE_TEST_MARKER();
                 }
+            }
+            else
+            {
+                xWantedSize = 0;
+            }
+        }
+        else
+        {
+            mtCOVERAGE_TEST_MARKER();
+        }
 
         /* Check the block size we are trying to allocate is not so large that the
          * top bit is set.  The top bit of the block size member of the BlockLink_t