soc/amd/common/block/pi: Fix AGESA heap deallocator

The deallocation was always subtracting the header, even when it
shouldn't. This caused problems for the allocator where buffer
sizes were incorrect and freed and used buffers could collide.
Fix the deallocation size.

Clear deallocated concatinated buffer header memory.

Fix the initial calculation of the total buffer size
available to be allocated.

BUG=b:71764350
TEST= Boot grunt.
BRANCH=none

Change-Id: I2789ddf72d662f24709dc5d9873741169cc4ef36
Signed-off-by: Marc Jones <marcj303@gmail.com>
Reviewed-on: https://review.coreboot.org/23284
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Aaron Durbin <adurbin@chromium.org>
diff --git a/src/soc/amd/common/block/pi/heapmanager.c b/src/soc/amd/common/block/pi/heapmanager.c
index cea3171..862ba3d 100644
--- a/src/soc/amd/common/block/pi/heapmanager.c
+++ b/src/soc/amd/common/block/pi/heapmanager.c
@@ -77,8 +77,8 @@
 		FreedNodePtr = (BIOS_BUFFER_NODE *)(BiosHeapBaseAddr
 						+ FreedNodeOffset);
 		FreedNodePtr->BufferSize = AvailableHeapSize
-						- sizeof(BIOS_BUFFER_NODE)
-						- CurrNodePtr->BufferSize;
+					- (FreedNodeOffset - CurrNodeOffset)
+					- sizeof(BIOS_BUFFER_NODE);
 		FreedNodePtr->NextNodeOffset = 0;
 
 		/* Update the offsets for Allocated and Freed nodes */
@@ -250,13 +250,13 @@
 	memset((UINT8 *)AllocNodePtr + sizeof(BIOS_BUFFER_NODE), 0,
 		AllocNodePtr->BufferSize);
 	AllocNodePtr->BufferHandle = 0;
-	AllocNodePtr->BufferSize += sizeof(BIOS_BUFFER_NODE);
 
 	/* Add deallocated node in order to the list of freed nodes */
 	FreedNodeOffset = BiosHeapBasePtr->StartOfFreedNodes;
 	FreedNodePtr = (BIOS_BUFFER_NODE *)(BiosHeapBaseAddr + FreedNodeOffset);
 
-	EndNodeOffset = AllocNodeOffset + AllocNodePtr->BufferSize;
+	EndNodeOffset = AllocNodeOffset + AllocNodePtr->BufferSize +
+						sizeof(BIOS_BUFFER_NODE);
 
 	if (AllocNodeOffset < FreedNodeOffset) {
 		/* Add to the start of the freed list */
@@ -264,16 +264,14 @@
 			/* If the freed node is adjacent to the first node in
 			 * the list, concatenate both nodes
 			 */
-			AllocNodePtr->BufferSize += FreedNodePtr->BufferSize;
+			AllocNodePtr->BufferSize += FreedNodePtr->BufferSize +
+						sizeof(BIOS_BUFFER_NODE);
 			AllocNodePtr->NextNodeOffset =
 						FreedNodePtr->NextNodeOffset;
 
-			/* Clear the BufferSize and NextNodeOffset of the
-			 * previous first node
-			 */
-			FreedNodePtr->BufferSize = 0;
-			FreedNodePtr->NextNodeOffset = 0;
-
+			/* Zero out the FreedNode header */
+			memset((UINT8 *)FreedNodePtr, 0,
+						sizeof(BIOS_BUFFER_NODE));
 		} else {
 			/* Otherwise, add freed node to the start of the list
 			 * Update NextNodeOffset and BufferSize to include the
@@ -304,12 +302,14 @@
 		if (NextNodeOffset == EndNodeOffset) {
 			NextNodePtr = (BIOS_BUFFER_NODE *)(BiosHeapBaseAddr
 						+ NextNodeOffset);
-			AllocNodePtr->BufferSize += NextNodePtr->BufferSize;
+			AllocNodePtr->BufferSize += NextNodePtr->BufferSize +
+						sizeof(BIOS_BUFFER_NODE);
 			AllocNodePtr->NextNodeOffset =
 						NextNodePtr->NextNodeOffset;
 
-			NextNodePtr->BufferSize = 0;
-			NextNodePtr->NextNodeOffset = 0;
+			/* Zero out the NextNode header */
+			memset((UINT8 *)NextNodePtr, 0,
+						sizeof(BIOS_BUFFER_NODE));
 		} else {
 			/*AllocNodePtr->NextNodeOffset =
 			 * 			FreedNodePtr->NextNodeOffset; */
@@ -321,14 +321,18 @@
 		 */
 		PrevNodePtr = (BIOS_BUFFER_NODE *)(BiosHeapBaseAddr
 						+ PrevNodeOffset);
-		EndNodeOffset = PrevNodeOffset + PrevNodePtr->BufferSize;
+		EndNodeOffset = PrevNodeOffset + PrevNodePtr->BufferSize +
+						sizeof(BIOS_BUFFER_NODE);
+
 		if (AllocNodeOffset == EndNodeOffset) {
 			PrevNodePtr->NextNodeOffset =
 						AllocNodePtr->NextNodeOffset;
-			PrevNodePtr->BufferSize += AllocNodePtr->BufferSize;
+			PrevNodePtr->BufferSize += AllocNodePtr->BufferSize +
+						sizeof(BIOS_BUFFER_NODE);
 
-			AllocNodePtr->BufferSize = 0;
-			AllocNodePtr->NextNodeOffset = 0;
+			/* Zero out the AllocNode header */
+			memset((UINT8 *)AllocNodePtr, 0,
+						sizeof(BIOS_BUFFER_NODE));
 		} else {
 			PrevNodePtr->NextNodeOffset = AllocNodeOffset;
 		}