Add the AMD Family10 Agesa code
[coreboot.git] / src / vendorcode / amd / agesa / f10 / Proc / IDS / Debug / IdsDebug.c
diff --git a/src/vendorcode/amd/agesa/f10/Proc/IDS/Debug/IdsDebug.c b/src/vendorcode/amd/agesa/f10/Proc/IDS/Debug/IdsDebug.c
new file mode 100755 (executable)
index 0000000..95bb509
--- /dev/null
@@ -0,0 +1,1323 @@
+/**
+ * @file
+ *
+ * AMD Integrated Debug Debug_library Routines
+ *
+ * Contains AMD AGESA debug macros and library functions
+ *
+ * @xrefitem bom "File Content Label" "Release Content"
+ * @e project:      AGESA
+ * @e sub-project:  IDS
+ * @e \$Revision: 44323 $   @e \$Date: 2010-12-22 01:24:58 -0700 (Wed, 22 Dec 2010) $
+ */
+/*****************************************************************************
+ * Copyright (c) 2011, Advanced Micro Devices, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Advanced Micro Devices, Inc. nor the names of 
+ *       its contributors may be used to endorse or promote products derived 
+ *       from this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL ADVANCED MICRO DEVICES, INC. BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * 
+ ******************************************************************************
+ */
+
+ /*----------------------------------------------------------------------------------------
+ *                             M O D U L E S    U S E D
+ *----------------------------------------------------------------------------------------
+ */
+#include "AGESA.h"
+#include "Ids.h"
+#include "IdsLib.h"
+#include "amdlib.h"
+#include "AMD.h"
+#include "heapManager.h"
+#include "cpuRegisters.h"
+#include "Filecode.h"
+#define FILECODE PROC_IDS_DEBUG_IDSDEBUG_FILECODE
+
+extern BUILD_OPT_CFG UserOptions;
+typedef struct _IDS_CONSOLE IDS_CONSOLE;
+
+/*--------------------------------------------------------------------------------------*/
+/**
+ *  IDS back-end code for AGESA_TESTPOINT
+ *
+ *  @param[in] TestPoint  Progress indicator value, see @ref AGESA_TP
+ *  @param[in,out] StdHeader    The Pointer of AGESA Header
+ *
+ **/
+/*--------------------------------------------------------------------------------------*/
+VOID
+IdsAgesaTestPoint (
+  IN       AGESA_TP      TestPoint,
+  IN OUT   AMD_CONFIG_PARAMS *StdHeader
+  )
+{
+  LibAmdIoWrite (AccessWidth8, IDS_DEBUG_PORT, &TestPoint, StdHeader);
+  IDS_PERF_TIMESTAMP (StdHeader, TestPoint);
+  if (TestPoint == EndAgesaTps) {
+    IDS_PERF_ANALYSE (StdHeader);
+  }
+}
+
+#define DEBUG_PRINT_INIT        0x10BF0000
+#define DEBUG_PRINT_EXIT        0xE0BF0000
+#define DEBUG_PRINT_COMMAND     0xC0BF0000
+#define DEBUG_PRINT_BREAKPOINT  0xB0BF0000
+#define DEBUG_PRINT_EVENT       0x1EBF0000
+
+#define IDS_HDTOUT_BPFLAG_FORMAT_STR   0
+#define IDS_HDTOUT_BPFLAG_STATUS_STR   1
+
+#define HDTOUT_BP_ACTION_HALT          1
+#define HDTOUT_BP_ACTION_PRINTON       2
+#define HDTOUT_BP_ACTION_PRINTONE      3
+#define HDTOUT_BP_ACTION_PRINTOFF      4
+
+typedef struct _BREAKPOINT_UNIT {
+  UINT8 AndFlag : 1;        ///< Next string is ANDed to current string
+  UINT8 BpFlag : 1;         ///< Format string or Status string
+  UINT8 Action : 4;         ///< Halt, start HDTOUT, or stop HDT,...
+  UINT8 BpStrOffset;        ///< Offset from BreakpointList to the breakpoint string
+} BREAKPOINT_UNIT;
+
+typedef enum {
+  IDS_STATE_OFF = 0xF0,
+  IDS_STATE_ON
+} IDS_STATE_TYPE;
+
+typedef enum {
+  NON_CONSOLE = 0xD0,
+  HDT_CONSOLE,
+  IDS_CONSOLE_END
+} IDS_CONSOLE_TYPE;
+
+// IDS HdtOut Event Level
+typedef enum {
+  EVENT_OFF = 0x30,              ///< Default,no event triggered.
+  EVENT_WARNING,                 ///< Event warning.
+  EVENT_ERROR,                   ///< Event error.
+  EVENT_FAIL_BUFFER_ALLOCATION,  ///< Reserved, fail buffer allocation
+  EVENT_END                   ///< Event end sentinel.
+} CONSOLE_EVENT_TYPE;
+
+#define _INT_SIZE_OF(n) ((sizeof (n) + sizeof (UINTN) - 1) &~(sizeof (UINTN) - 1))
+
+//
+// Also support coding convention rules for var arg macros
+//
+typedef CHAR8 *VA_LIST;
+#define VA_START(ap, v) (ap = (VA_LIST) & (v) + _INT_SIZE_OF (v))
+#define VA_ARG(ap, t)   (*(t *) ((ap += _INT_SIZE_OF (t)) - _INT_SIZE_OF (t)))
+#define VA_END(ap)      (ap = (VA_LIST) 0)
+
+#define LEFT_JUSTIFY    0x01
+#define PREFIX_SIGN     0x02
+#define PREFIX_BLANK    0x04
+#define COMMA_TYPE      0x08
+#define LONG_TYPE       0x10
+#define PREFIX_ZERO     0x20
+
+/**
+ *  Create console context
+ *
+ *  Do hardware settings related with specific console context
+ *
+ *  @param[in,out] IdsConsole    The Pointer of IDS console
+ *
+ **/
+typedef VOID F_CREATE_CONSOLE_CONTEXT (
+  IN OUT   IDS_CONSOLE *IdsConsole
+  );
+/// Reference to a method.
+typedef F_CREATE_CONSOLE_CONTEXT *PF_CREATE_CONSOLE_CONTEXT;
+
+/**
+ *  Initialize console context
+ *
+ *  Initilize preference settings related with specific console context
+ *
+ *  @param[in,out] IdsConsole    The Pointer of IDS console
+ *
+ **/
+typedef VOID F_INIT_CONSOLE_CONTEXT (
+  IN OUT   IDS_CONSOLE *IdsConsole
+  );
+/// Reference to a method.
+typedef F_INIT_CONSOLE_CONTEXT *PF_INIT_CONSOLE_CONTEXT;
+
+/**
+ *  Update console context
+ *
+ *  Update preference settings related with specific console context
+ *
+ *  @param[in,out] IdsConsole    The Pointer of IDS console
+ *
+ **/
+typedef VOID F_UPDATE_CONSOLE_CONTEXT (
+  IN OUT   IDS_CONSOLE *IdsConsole
+  );
+/// Reference to a method.
+typedef F_UPDATE_CONSOLE_CONTEXT *PF_UPDATE_CONSOLE_CONTEXT;
+
+/**
+ *  Save console context
+ *
+ *  Save console context snapshot
+ *
+ *  @param[in,out] IdsConsole    The Pointer of IDS console
+ *
+ **/
+typedef VOID F_SAVE_CONSOLE_CONTEXT (
+  IN OUT   IDS_CONSOLE *IdsConsole
+  );
+/// Reference to a method.
+typedef F_SAVE_CONSOLE_CONTEXT *PF_SAVE_CONSOLE_CONTEXT;
+
+/**
+ *  Destroy console context
+ *
+ *  Destroy console context snapshot
+ *
+ *  @param[in,out] IdsConsole    The Pointer of IDS console
+ *
+ **/
+typedef VOID F_DESTROY_CONSOLE_CONTEXT (
+  IN OUT   IDS_CONSOLE *IdsConsole
+  );
+/// Reference to a method.
+typedef F_DESTROY_CONSOLE_CONTEXT *PF_DESTROY_CONSOLE_CONTEXT;
+
+/**
+ *  Print function
+ *
+ *  Print function related with specific console
+ *
+ *  @param[in] PrintType             Print Type
+ *  @param[in] ConsoleBufferAddress  The address of console buffer
+ *  @param[in] ConsoleBufferSize     The size of console buffer
+ *  @param[in,out] IdsConsole        The Pointer of IDS console
+ *
+ **/
+typedef VOID F_PRINT (
+  IN       UINT32 PrintType,
+  IN       UINT32 ConsoleBufferAddress,
+  IN       UINT32 ConsoleBufferSize,
+  IN OUT   IDS_CONSOLE *IdsConsole
+  );
+/// Reference to a method.
+typedef F_PRINT *PF_PRINT;
+
+/// IDS Console Operations
+typedef struct _IDS_CONSOLE_OPERATIONS {
+  PF_CREATE_CONSOLE_CONTEXT CreateConsoleContext;
+  PF_INIT_CONSOLE_CONTEXT InitConsoleContext;
+  PF_UPDATE_CONSOLE_CONTEXT UpdateConsoleContext;
+  PF_DESTROY_CONSOLE_CONTEXT DestroyConsoleContext;
+  PF_PRINT Print;
+} IDS_CONSOLE_OPERATIONS;
+
+/// IDS Console Header
+typedef struct _IDS_CONSOLE_HEADER {
+  UINT32 Signature;               ///< Signature information.
+  UINT32 Version;                 ///< Version.
+  UINT8  ConsoleType;             ///< Console type
+  UINT8  Event;                   ///< Event type.
+  UINT8  PrintState;              ///< On or Off
+  UINT8  OutBufferMode;           ///< Off:stack mode, On: heap mode
+  UINT16 OutBufferSize;           ///< Buffer size
+  UINT16 OutBufferIndex;          ///< Buffer index
+  UINT32 NumBreakpointUnit;       ///< default 0 no bp unit others number of bp unit
+} IDS_CONSOLE_HEADER;
+
+/// IDS Console
+struct _IDS_CONSOLE
+{
+  IDS_CONSOLE_HEADER Header;      /**< IDS console header - 5 dwords */
+  UINT32 FuncListAddr;            /**< 32 bit address to the list of functions that script can execute */
+  UINT8  Reserved[56 - 24];       /**< ----------------- New fields must be added here. */
+  CHAR8  BreakpointList[300];     /**< Breakpoint list */
+  CHAR8  StatusStr[156];          /**< Shows current node, DCT, CS,... */
+  CHAR8  OutBuffer[2];            /**< Console Out content. Its size will be determined by BufferSize. */
+};
+
+/**
+ *  Create hdt console context
+ *
+ *  Do hardware settings for hdt console context
+ *
+ *  @param[in,out] IdsConsole    The Pointer of IDS console
+ *
+ **/
+STATIC VOID
+CreateHdtConsoleContext (
+  IN OUT   IDS_CONSOLE *IdsConsole
+  )
+{
+  UINT64 SMsr;
+  UINT32 CR4reg;
+
+  LibAmdMsrRead (0xC001100A, (UINT64*)&SMsr, NULL);
+  SMsr |= 1;
+  LibAmdMsrWrite (0xC001100A, (UINT64*)&SMsr, NULL);
+
+  LibAmdWriteCpuReg (DR2_REG, 0x99CC);
+  LibAmdWriteCpuReg (DR7_REG, 0x02000420);
+
+  LibAmdReadCpuReg (CR4_REG, &CR4reg);
+  LibAmdWriteCpuReg (CR4_REG, CR4reg | ((UINT32)1 << 3));
+}
+
+/**
+ *  Initialize hdt console context
+ *
+ *  Initilize preference settings for hdt console context
+ *
+ *  @param[in,out] IdsConsole    The Pointer of IDS console
+ *
+ **/
+STATIC VOID
+InitHdtConsoleContext (
+  IN OUT   IDS_CONSOLE *IdsConsole
+  )
+{
+  IDS_FUNCLIST_EXTERN ();
+
+  IdsConsole->FuncListAddr = (UINT32) IDS_FUNCLIST_ADDR;
+  IdsConsole->StatusStr[0] = 0;
+
+}
+
+/**
+ *  Update hdt console context
+ *
+ *  Update preference settings for hdt console context
+ *
+ *  @param[in,out] IdsConsole    The Pointer of IDS console
+ *
+ **/
+STATIC VOID
+UpdateHdtConsoleContext (
+  IN OUT   IDS_CONSOLE *IdsConsole
+  )
+{
+  if (IdsConsole->Header.OutBufferMode == IDS_STATE_OFF) {
+    IdsConsole->Header.OutBufferSize = 0;
+  }
+
+}
+
+/**
+ *  Destroy hdt console context
+ *
+ *  Save hdt console context snapshot
+ *
+ *  @param[in,out] IdsConsole    The Pointer of IDS console
+ *
+ **/
+STATIC VOID
+DestroyHdtConsoleContext (
+  IN OUT   IDS_CONSOLE *IdsConsole
+  )
+{
+  UINT64 SMsr;
+
+  LibAmdMsrRead (0xC001100A, (UINT64 *)&SMsr, NULL);
+  SMsr &= ~BIT0;
+  LibAmdMsrWrite (0xC001100A, (UINT64 *)&SMsr, NULL);
+
+  LibAmdWriteCpuReg (DR2_REG, 0);
+  LibAmdWriteCpuReg (DR3_REG, 0);
+  LibAmdWriteCpuReg (DR7_REG, 0);
+}
+
+/**
+ *  Hdt console print function
+ *
+ *  Print function related with hdt console
+ *
+ *  @param[in] PrintType             Print Type
+ *  @param[in] ConsoleBufferAddress  The address of console buffer
+ *  @param[in] ConsoleBufferSize     The size of console buffer
+ *  @param[in,out] IdsConsole        The Pointer of IDS console
+ *
+ **/
+STATIC VOID
+HdtConsolePrint (
+  IN       UINT32 PrintType,
+  IN       UINT32 ConsoleBufferAddress,
+  IN       UINT32 ConsoleBufferSize,
+  IN OUT   IDS_CONSOLE *IdsConsole
+  )
+{
+  IdsOutPort (PrintType | 0x99CC, ConsoleBufferAddress, ConsoleBufferSize);
+}
+
+/// Initial construction data for HDT console header.
+CONST IDS_CONSOLE_HEADER ROMDATA HdtConsoleHeader =
+{
+ 0xDB1099CC,
+ 0x0100,
+ IDS_STATE_ON,
+ EVENT_OFF,
+ IDS_STATE_ON,
+ IDS_STATE_ON,
+ 0x1000,
+ 0x0,
+ 0,
+};
+
+#define OPTION_HDT_CONSOLE_HEADER &HdtConsoleHeader
+
+/// Initial construction data for HDT console operations.
+CONST IDS_CONSOLE_OPERATIONS ROMDATA HdtConsoleOperations =
+{
+ CreateHdtConsoleContext,
+ InitHdtConsoleContext,
+ UpdateHdtConsoleContext,
+ DestroyHdtConsoleContext,
+ HdtConsolePrint
+};
+
+#define OPTION_HDT_CONSOLE_OPERATIONS &HdtConsoleOperations
+
+/**
+ *  Parses flag and width information from theFormat string and returns the next index
+ *  into the Format string that needs to be parsed. See file headed for details of Flag and Width.
+ *
+ *  @param[in]  Format  Current location in the AvSPrint format string.
+ *  @param[out] Flags  Returns flags
+ *  @param[out] Width  Returns width of element
+ *  @param[out] Marker Vararg list that may be partially consumed and returned.
+ *
+ *  @retval Pointer indexed into the Format string for all the information parsed by this routine.
+ *
+ **/
+STATIC CHAR8 *
+GetFlagsAndWidth (
+  IN       CHAR8 *Format,
+     OUT   UINTN *Flags,
+     OUT   UINTN *Width,
+  IN OUT   VA_LIST *Marker
+  )
+{
+  UINTN   Count;
+  BOOLEAN Done;
+
+  *Flags = 0;
+  *Width = 0;
+  for (Done = FALSE; !Done; ) {
+    Format++;
+
+    switch (*Format) {
+
+    case '-':
+      *Flags |= LEFT_JUSTIFY;
+      break;
+    case '+':
+      *Flags |= PREFIX_SIGN;
+      break;
+    case ' ':
+      *Flags |= PREFIX_BLANK;
+      break;
+    case ',':
+      *Flags |= COMMA_TYPE;
+      break;
+    case 'L':
+    case 'l':
+      *Flags |= LONG_TYPE;
+      break;
+
+    case '*':
+      *Width = VA_ARG (*Marker, UINTN);
+      break;
+
+    case '0':
+      *Flags |= PREFIX_ZERO;
+      break;
+
+    case '1':
+    case '2':
+    case '3':
+    case '4':
+    case '5':
+    case '6':
+    case '7':
+    case '8':
+    case '9':
+      Count = 0;
+      do {
+        Count = (Count * 10) + *Format - '0';
+        Format++;
+      } while ((*Format >= '0')  &&  (*Format <= '9'));
+      Format--;
+      *Width = Count;
+      break;
+
+    default:
+      Done = TRUE;
+    }
+  }
+  return Format;
+}
+
+CHAR8 STATIC HexStr[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
+/**
+ *
+ *  @param[in] Buffer Location to place ascii hex string of Value.
+ *  @param[in] Value  - Hex value to convert to a string in Buffer.
+ *  @param[in] Flags  - Flags to use in printing Hex string, see file header for details.
+ *  @param[in] Width  - Width of hex value.
+ *
+ *  @retval Number of characters printed.
+ **/
+
+STATIC UINTN
+ValueTomHexStr (
+  IN OUT   CHAR8   *Buffer,
+  IN       UINT64      Value,
+  IN       UINTN       Flags,
+  IN       UINTN       Width
+  )
+{
+  CHAR8   TempBuffer[30];
+  CHAR8   *TempStr;
+  CHAR8   Prefix;
+  CHAR8   *BufferPtr;
+  UINTN   Count;
+  UINTN   Index;
+
+  TempStr = TempBuffer;
+  BufferPtr = Buffer;
+  //
+  // Count starts at one since we will null terminate. Each iteration of the
+  // loop picks off one nibble. Oh yea TempStr ends up backwards
+  //
+  Count = 0;
+  do {
+    *(TempStr++) = HexStr[Value & 0x0f];
+    Value >>= 4;
+    Count++;
+  } while (Value != 0);
+
+  if (Flags & PREFIX_ZERO) {
+    Prefix = '0';
+  } else if (!(Flags & LEFT_JUSTIFY)) {
+    Prefix = ' ';
+  } else {
+    Prefix = 0x00;
+  }
+  for (Index = Count; Index < Width; Index++) {
+    *(TempStr++) = Prefix;
+  }
+
+  //
+  // Reverse temp string into Buffer.
+  //
+  while (TempStr != TempBuffer) {
+    *(BufferPtr++) = *(--TempStr);
+  }
+
+  *BufferPtr = 0;
+  return Index;
+}
+
+/**
+ *  Prints a Value as a decimal number in Buffer
+ *
+ *  @param[in] Buffer  Location to place ascii decimal number string of Value.
+ *  @param[in] Value  Decimal value to convert to a string in Buffer.
+ *  @param[in] Flags  Flags to use in printing decimal string, see file header for details.
+ *
+ *  @retval Number of characters printed.
+ *
+**/
+
+STATIC UINTN
+ValueToString (
+  IN OUT   CHAR8   *Buffer,
+  IN       INT32       Value,
+  IN       UINTN       Flags
+  )
+{
+  CHAR8   TempBuffer[30];
+  CHAR8   *TempStr;
+  CHAR8   *BufferPtr;
+  UINTN   Count;
+  UINTN   Remainder;
+
+  TempStr = TempBuffer;
+  BufferPtr = Buffer;
+  Count = 0;
+
+  if (Value < 0) {
+    *(BufferPtr++) = '-';
+    Value = - Value;
+    Count++;
+  }
+
+  do {
+    Remainder = Value % 10;
+    Value /= 10;
+    *(TempStr++) = (CHAR8)(Remainder + '0');
+    Count++;
+    if ((Flags & COMMA_TYPE) == COMMA_TYPE) {
+      if (Count % 3 == 0) {
+        *(TempStr++) = ',';
+      }
+    }
+  } while (Value != 0);
+
+  //
+  // Reverse temp string into Buffer.
+  //
+  while (TempStr != TempBuffer) {
+    *(BufferPtr++) = *(--TempStr);
+  }
+
+  *BufferPtr = 0;
+  return Count;
+}
+
+/**
+ *  Check if String contain the substring
+ *
+ *  @param[in] String  Pointer of string.
+ *  @param[in] Substr  Pointer of sub string.
+ *
+ *  @retval TRUE S2 is substring of S1
+ *  @retval FALSE S2 isn't substring of S1
+ *
+**/
+STATIC BOOLEAN
+AmdIdsSubStr (
+  IN      CHAR8 *String,
+  IN      CHAR8 *Substr
+  )
+{
+  UINT16 i;
+  UINT16 j;
+
+  for (i = 0; String[i] != 0 ; i++) {
+    for (j = 0; (Substr[j] != 0) && (Substr[j] == String[i + j]); j++) {
+    }
+    if (Substr[j] == 0) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+ *  IDS Backend Function for Memory timeout control
+ *
+ *  This function is used to override Memory timeout control.
+ *
+ *  @param[in,out]   DataPtr      The Pointer of UINT8.
+ *
+ **/
+VOID
+IdsMemTimeOut (
+  IN OUT   VOID *DataPtr
+  )
+{
+  UINT32 DR2reg;
+
+  LibAmdReadCpuReg (DR2_REG, &DR2reg);
+  if (DR2reg == 0x99CC) {
+    // Turn timeout off if HDTout is on
+    *((UINT8 *)DataPtr) = (UINT8)0;
+  }
+}
+
+/**
+ *
+ *  IDS Debug Function to check the sentinels are intact
+ *
+ * This function complete heap walk and check to be performed at any time.
+ *
+ * @param[in]      StdHeader         Config handle for library and services.
+ *
+ * @retval         TRUE     No error
+ *
+ **/
+BOOLEAN
+AmdHeapIntactCheck (
+  IN       AMD_CONFIG_PARAMS *StdHeader
+  )
+{
+  UINT8       *HeapBufferPtr;
+  BUFFER_NODE       *StartOfBufferPtr;
+  BUFFER_NODE       *EndOfBufferPtr;
+  HEAP_MANAGER *HeapManagerPtr;
+  BUFFER_NODE *HeadNodePtr;
+  BUFFER_NODE *CurrentNodePtr;
+  UINT32  AmdHeapRamAddress;
+  UINT32 SentinelBefore;
+  UINT32 SentinelAfter;
+
+  ASSERT (StdHeader != NULL);
+
+  AmdHeapRamAddress = (UINT32)UserOptions.CfgHeapDramAddress;
+
+  if (StdHeader->HeapStatus == HEAP_LOCAL_CACHE) {
+    HeapBufferPtr = (UINT8 *) HeapGetCurrentBase (StdHeader);
+  } else if (StdHeader->HeapStatus == HEAP_TEMP_MEM) {
+    HeapBufferPtr = (UINT8 *) AmdHeapRamAddress;
+  } else {
+    return TRUE;
+  }
+
+  HeapManagerPtr = (HEAP_MANAGER *) HeapBufferPtr;
+  HeadNodePtr = (BUFFER_NODE *) (HeapBufferPtr + sizeof (HEAP_MANAGER));
+  CurrentNodePtr = HeadNodePtr;
+
+  if (HeapManagerPtr->AvailableSize != AMD_HEAP_SIZE_PER_CORE - sizeof (HEAP_MANAGER)) {
+    while (CurrentNodePtr != NULL) {
+      StartOfBufferPtr = (BUFFER_NODE *) ((UINT8 *) CurrentNodePtr + sizeof (BUFFER_NODE) + CurrentNodePtr->BufferSize + 2 * SIZE_OF_SENTINEL);
+      EndOfBufferPtr = (BUFFER_NODE *) ((UINT8 *) HeadNodePtr + AMD_HEAP_SIZE_PER_CORE);
+      if (CurrentNodePtr->NextNodePtr != NULL) {
+        ASSERT ((CurrentNodePtr->NextNodePtr >= StartOfBufferPtr) && (CurrentNodePtr->NextNodePtr < EndOfBufferPtr));
+        SentinelBefore = *(UINT32 *) ((UINT8 *) CurrentNodePtr + sizeof (BUFFER_NODE));
+        SentinelAfter = *(UINT32 *) ((UINT8 *) CurrentNodePtr + sizeof (BUFFER_NODE) + SIZE_OF_SENTINEL + CurrentNodePtr->BufferSize);
+
+        ASSERT ((SentinelBefore == SENTINEL_BEFORE_VALUE) && (SentinelAfter == SENTINEL_AFTER_VALUE));
+      } else {
+        ASSERT ((UINT8 *) StartOfBufferPtr == HeapBufferPtr + AMD_HEAP_SIZE_PER_CORE - HeapManagerPtr->AvailableSize);
+        SentinelBefore = *(UINT32 *) ((UINT8 *) CurrentNodePtr + sizeof (BUFFER_NODE));
+        SentinelAfter = *(UINT32 *) ((UINT8 *) CurrentNodePtr + sizeof (BUFFER_NODE) + SIZE_OF_SENTINEL + CurrentNodePtr->BufferSize);
+        ASSERT ((SentinelBefore == SENTINEL_BEFORE_VALUE) && (SentinelAfter == SENTINEL_AFTER_VALUE));
+      }
+      CurrentNodePtr = CurrentNodePtr->NextNodePtr;
+    }
+  }
+  return TRUE;
+}
+
+/**
+ *  Determine whether IDS console is enabled.
+ *
+ *  @param[in,out] IdsConsole    The Pointer of Ids console data
+ *
+ *  @retval       TRUE    Ids console is enabled.
+ *  @retval       FALSE   Ids console is disabled.
+ *
+ **/
+BOOLEAN
+STATIC
+IsIdsConsoleEnabled (
+  IN OUT   UINTN *IdsConsole
+  )
+{
+  BOOLEAN Result;
+  UINT32  DR2reg;
+
+  Result = FALSE;
+
+  LibAmdReadCpuReg (DR2_REG, &DR2reg);
+  if (DR2reg == 0x99CC) {
+    Result = TRUE;
+  }
+
+  return Result;
+}
+
+/**
+ *  Get IDS console.
+ *
+ *  @param[in,out] IdsConsolePtr  The Pointer of Ids console data
+ *
+ **/
+STATIC VOID
+GetIdsConsole (
+  IN OUT   UINTN *IdsConsolePtr
+  )
+{
+  UINT32 DR3Reg;
+
+  LibAmdReadCpuReg (DR3_REG, &DR3Reg);
+  *IdsConsolePtr = (UINTN) DR3Reg;
+}
+
+/**
+ *  Get IDS console operations.
+ *
+ *  @param[in,out] IdsConsoleOperations  The Pointer of Ids console operations
+ *
+ **/
+STATIC VOID
+GetIdsConsoleHeader (
+  IN OUT   UINTN *IdsConsoleHeader
+  )
+{
+  IDS_CONSOLE_TYPE IdsConsoleType;
+
+  IdsConsoleType = (IDS_CONSOLE_TYPE) OPTION_IDS_CONSOLE;
+
+  if (IdsConsoleType == HDT_CONSOLE) {
+    *IdsConsoleHeader = (UINTN) OPTION_HDT_CONSOLE_HEADER;
+  } else {
+    ASSERT (FALSE);
+  }
+}
+
+/**
+ *  Get IDS console operations.
+ *
+ *  @param[in,out] IdsConsoleOperations  The Pointer of Ids console operations
+ *
+ **/
+STATIC VOID
+GetIdsConsoleOperations (
+  IN OUT   UINTN *IdsConsoleOperations
+  )
+{
+  IDS_CONSOLE_TYPE IdsConsoleType;
+
+  IdsConsoleType = (IDS_CONSOLE_TYPE) OPTION_IDS_CONSOLE;
+
+  if (IdsConsoleType == HDT_CONSOLE) {
+    *IdsConsoleOperations = (UINTN) OPTION_HDT_CONSOLE_OPERATIONS;
+  } else {
+    ASSERT (FALSE);
+  }
+}
+
+/**
+ *  Create IDS console.
+ *
+ *  @param[in,out] IdsConsole    The Pointer of Ids console data
+ *  @param[in,out] StdHeader     The Pointer of AGESA Header
+ **/
+STATIC VOID
+NewIdsConsole (
+  IN OUT   IDS_CONSOLE *IdsConsole,
+  IN OUT   AMD_CONFIG_PARAMS *StdHeader
+  )
+{
+  IDS_CONSOLE_HEADER *IdsConsoleHeader;
+  UINTN IdsConsoleHeaderPtr;
+
+  GetIdsConsoleHeader (&IdsConsoleHeaderPtr);
+  IdsConsoleHeader = &(IdsConsole->Header);
+
+  LibAmdMemCopy ((VOID *)IdsConsoleHeader, (VOID *) (IdsConsoleHeaderPtr), (UINT32) sizeof (IDS_CONSOLE_HEADER), StdHeader);
+}
+
+/**
+ *  Destroy IDS console.
+ *
+ *  @param[in,out] IdsConsole    The Pointer of Ids console data
+ *  @param[in,out] StdHeader     The Pointer of AGESA Header
+ **/
+STATIC VOID
+DestroyIdsConsole (
+  IN OUT   IDS_CONSOLE *IdsConsole,
+  IN OUT   AMD_CONFIG_PARAMS *StdHeader
+  )
+{
+  HeapDeallocateBuffer (IDS_HDT_OUT_BUFFER_HANDLE, StdHeader);
+}
+
+/**
+ *  Save IDS console Snapshot.
+ *
+ *  @param[in,out] IdsConsole    The Pointer of Ids console data
+ *  @param[in,out] StdHeader     The Pointer of AGESA Header
+ **/
+STATIC VOID
+SaveIdsConsole (
+  IN OUT   IDS_CONSOLE *IdsConsole,
+  IN OUT   IDS_CONSOLE_OPERATIONS *IdsConsoleOps,
+  IN OUT   AMD_CONFIG_PARAMS *StdHeader
+  )
+{
+  ALLOCATE_HEAP_PARAMS AllocHeapParams;
+
+  do {
+    AllocHeapParams.RequestedBufferSize = IdsConsole->Header.OutBufferSize + sizeof (IDS_CONSOLE) - 2;
+    AllocHeapParams.BufferHandle = IDS_HDT_OUT_BUFFER_HANDLE;
+    AllocHeapParams.Persist = HEAP_SYSTEM_MEM;
+    if (HeapAllocateBuffer (&AllocHeapParams, StdHeader) == AGESA_SUCCESS) {
+      break;
+    } else {
+      IdsConsole->Header.OutBufferSize -= 256;
+      IdsConsole->Header.Event = EVENT_FAIL_BUFFER_ALLOCATION;
+      IdsConsoleOps->Print (DEBUG_PRINT_EVENT, (UINT32)IdsConsole, 0, IdsConsole);
+    }
+  } while ((IdsConsole->Header.OutBufferSize & 0x8000) == 0);
+
+  if ((IdsConsole->Header.OutBufferSize & 0x8000) == 0) {
+    LibAmdWriteCpuReg (DR3_REG, (UINT32)AllocHeapParams.BufferPtr);
+    LibAmdMemCopy (AllocHeapParams.BufferPtr, (VOID *) IdsConsole, (UINT32) sizeof (IDS_CONSOLE) - 2, StdHeader);
+  }
+}
+
+/**
+ *
+ *  Initial function for IDS console.
+ *
+ *  Create IDS console context, let Ids console function to be ready.
+ *
+ *  @param[in,out] StdHeader    The Pointer of AGESA Header
+ *
+ **/
+VOID
+AmdIdsConsoleInit (
+  IN OUT   AMD_CONFIG_PARAMS *StdHeader
+  )
+{
+  IDS_CONSOLE IdsConsole;
+  IDS_CONSOLE_OPERATIONS *IdsConsoleOps;
+  UINTN IdsConsolePtr;
+  UINTN IdsConsoleOpsPtr;
+
+  if (IsIdsConsoleEnabled (&IdsConsolePtr)) {
+    GetIdsConsoleOperations (&IdsConsoleOpsPtr);
+    IdsConsoleOps = (IDS_CONSOLE_OPERATIONS *) IdsConsoleOpsPtr;
+    NewIdsConsole (&IdsConsole, StdHeader);
+
+    IdsConsoleOps->CreateConsoleContext (&IdsConsole);
+    IdsConsoleOps->InitConsoleContext (&IdsConsole);
+    IdsConsoleOps->Print (DEBUG_PRINT_INIT, (UINT32)&IdsConsole, 0, &IdsConsole);
+    IdsConsoleOps->UpdateConsoleContext (&IdsConsole);
+
+    SaveIdsConsole (&IdsConsole, IdsConsoleOps, StdHeader);
+  }
+}
+
+/**
+ *  Prints string to debug host like printf in C
+ *
+ *  @param[in] Format of string
+ *  @param[in] ... Variable parameter
+ *
+**/
+VOID
+AmdIdsConsolePrint (
+  IN      CHAR8 *Format,
+  IN      ...
+  )
+{
+  IDS_CONSOLE *IdsConsole;
+  UINTN IdsConsolePtr;
+  IDS_CONSOLE_OPERATIONS *IdsConsoleOps;
+  UINTN  IdsConsoleOpsPtr;
+  VA_LIST Marker;
+  CHAR8 LocalBuffer[256];
+  CHAR8 *ConsoleBuffer;
+  CHAR8 Null_Str[] = " < null string > ";
+  CHAR8 *AsciiStr;
+  UINTN Index;
+  UINTN Flags;
+  UINTN Width;
+  UINTN ConsoleBufferSize;
+  UINT64 Value;
+  UINT16 *Array;
+  UINT32 ArrayLength;
+  UINT32 ArrayIndex;
+  BOOLEAN SaveStatus;
+  UINT32 LastIndex;
+  UINT32 i;
+  UINT32 j;
+  UINT32 NumBpUnit;
+  BREAKPOINT_UNIT *Pbpunit;
+  CHAR8 *Pbpstr;
+  CHAR8 *PCmpStr;
+  CHAR8 *EventLevelStr;
+  BOOLEAN LastBpmatched;
+  BOOLEAN Bpmatched;
+  IDS_STATE_TYPE PrintCtrl;
+
+  //Init the default Value
+  IdsConsole = NULL;
+  IdsConsolePtr = 0;
+  ConsoleBuffer = LocalBuffer;
+  Index = 0;
+  LastIndex = 0;
+  ConsoleBufferSize = 256;
+  NumBpUnit = 0;
+  PrintCtrl = IDS_STATE_ON;
+
+  if (IsIdsConsoleEnabled (&IdsConsolePtr)) {
+    GetIdsConsole (&IdsConsolePtr);
+    IdsConsole = (IDS_CONSOLE *) IdsConsolePtr;
+    GetIdsConsoleOperations (&IdsConsoleOpsPtr);
+    IdsConsoleOps = (IDS_CONSOLE_OPERATIONS *) IdsConsoleOpsPtr;
+
+    if (IdsConsole->Header.OutBufferMode == IDS_STATE_ON) {
+      ConsoleBuffer = IdsConsole->OutBuffer;
+      Index = IdsConsole->Header.OutBufferIndex;
+      ConsoleBufferSize = (UINTN) (IdsConsole->Header.OutBufferSize);
+      NumBpUnit = IdsConsole->Header.NumBreakpointUnit;
+      PrintCtrl = IdsConsole->Header.PrintState;
+    }
+
+    if ((PrintCtrl != 0) || (NumBpUnit > 0)) {
+      VA_START(Marker,Format); //init marker to 1st dynamic parameters.
+
+      LastIndex = (UINT32) Index;
+
+      if (*Format != '@') {
+        if (*Format == '!') {
+          SaveStatus = TRUE;
+          Format++;
+        } else {
+          SaveStatus = FALSE;
+        }
+
+        for (; *Format != '\0'; Format++) {
+          if (*Format != '%') {
+            ConsoleBuffer[Index++] = *Format;
+          } else {
+            Format = GetFlagsAndWidth (Format, &Flags, &Width, &Marker);
+            switch (*Format) {
+            // Using %[EventType] style to catch predefined event.
+            case '[':
+              EventLevelStr = Format;
+              EventLevelStr++;
+              if ((*EventLevelStr > EVENT_OFF) && (*EventLevelStr < EVENT_END)) {
+                EventLevelStr++;
+                if (*EventLevelStr == ']') {
+                  EventLevelStr--;
+                  IdsConsole->Header.Event = (UINT8) (CONSOLE_EVENT_TYPE) (*EventLevelStr);
+                }
+              }
+              break;
+            case 'X':
+              Flags |= PREFIX_ZERO;
+              Width = sizeof (UINT64) * 2;
+              //
+              // break skipped on purpose
+              //
+            case 'x':
+              if ((Flags & LONG_TYPE) == LONG_TYPE) {
+                Value = VA_ARG (Marker, UINT64);
+              } else {
+                Value = VA_ARG (Marker, UINTN);
+              }
+              Index += ValueTomHexStr (&ConsoleBuffer[Index], Value, Flags, Width);
+              break;
+
+            case 'd':
+              Value = (UINTN)VA_ARG (Marker, UINT32);
+              Index += ValueToString (&ConsoleBuffer[Index], (UINT32)Value, Flags);
+              break;
+
+            case 's':
+            case 'S':
+              AsciiStr = (CHAR8 *)VA_ARG (Marker, CHAR8 *);
+              if (AsciiStr == NULL) {
+                AsciiStr = Null_Str; //" < null string > ";
+              }
+              while (*AsciiStr != '\0') {
+                ConsoleBuffer[Index++] = *AsciiStr++;
+              }
+              break;
+
+            case 'c':
+              ConsoleBuffer[Index++] = (CHAR8)VA_ARG (Marker, UINTN);
+              break;
+
+            case 'a':
+              Array = (UINT16 *) VA_ARG (Marker, VOID *);
+              ArrayLength = (UINT32) VA_ARG (Marker, UINT32);
+              for (ArrayIndex = 0; ArrayIndex < ArrayLength; ArrayIndex++) {
+                // Only support hex format of an array of UINT16 for now.
+                Width = 2;
+                Flags = PREFIX_ZERO;
+                ConsoleBuffer[Index++] = ' ';
+                Index += ValueTomHexStr (&ConsoleBuffer[Index], Array[ArrayIndex] & 0xFF, Flags, Width);
+
+                // If buffer is full
+                if (Index > (ConsoleBufferSize - 8)) {
+                  if (IdsConsole != NULL) {
+                    if (LastIndex != 0) {
+                      // Stream all out except current string
+                      if (PrintCtrl != 0) {
+                        IdsConsoleOps->Print (DEBUG_PRINT_COMMAND, (UINT32)ConsoleBuffer, LastIndex, IdsConsole);
+                      }
+                      Index = Index - LastIndex;
+                      // Move current string too top
+                      for (i = 0; i < Index; i++) {
+                        ConsoleBuffer[i] = ConsoleBuffer[LastIndex + i];
+                      }
+                      LastIndex = 0;
+                    } else {
+                      // Buffer size is too small
+                      ASSERT (FALSE);
+                    }
+                  } else {
+                    break;
+                  }
+                }
+              }
+              break;
+
+            case 'v':
+              ConsoleBuffer[Index++] = '%';
+              ConsoleBuffer[Index++] = 'v';
+              Format++;
+              ConsoleBuffer[Index++] = *Format;
+              if (*Format == 'h') {
+                Format++;
+                ConsoleBuffer[Index++] = *Format;
+              }
+              break;
+
+            case '%':
+              ConsoleBuffer[Index++] = *Format;
+              break;
+
+            default:
+              //
+              // if the type is unknown print it to the screen
+              //
+              ConsoleBuffer[Index++] = '%';
+              ConsoleBuffer[Index++] = *Format;
+            }
+          }
+
+          // If buffer is full
+          if (Index > (ConsoleBufferSize - 32)) {
+            if (IdsConsole != NULL) {
+              if (LastIndex != 0) {
+                // Stream all out except current string
+                if (PrintCtrl != IDS_STATE_OFF) {
+                  IdsConsoleOps->Print (DEBUG_PRINT_COMMAND, (UINT32)ConsoleBuffer, LastIndex, IdsConsole);
+                }
+                Index = Index - LastIndex;
+                // Move current string too top
+                for (i = 0; i < Index; i++) {
+                  ConsoleBuffer[i] = ConsoleBuffer[LastIndex + i];
+                }
+                LastIndex = 0;
+              } else {
+                // Buffer size is too small
+                ASSERT (FALSE);
+              }
+            } else {
+              break;
+            }
+          }
+        }
+
+        if (IdsConsole->Header.OutBufferMode == IDS_STATE_OFF) {
+          IdsConsoleOps->Print (DEBUG_PRINT_COMMAND, (UINT32)ConsoleBuffer, (UINT32) Index, IdsConsole);
+          Index = 0;
+        }
+
+        if ((IdsConsole->Header.Event > EVENT_OFF) && (IdsConsole->Header.Event < EVENT_END) ) {
+          // if HdtOut Buffer is on, flush hdtout buffer contents out.
+          if (IdsConsole->Header.OutBufferMode == IDS_STATE_ON) {
+            IdsConsoleOps->Print (DEBUG_PRINT_COMMAND, (UINT32)ConsoleBuffer, (UINT32) Index, IdsConsole);
+            Index = 0;
+          }
+
+          IdsConsoleOps->Print (DEBUG_PRINT_EVENT, (UINT32)IdsConsole, 0, IdsConsole);
+          IdsConsole->Header.Event = EVENT_OFF;
+        }
+
+        //
+        //  Check breakpoint
+        //
+        if (NumBpUnit) {
+          Pbpunit = (BREAKPOINT_UNIT *) IdsConsole->BreakpointList;
+          LastBpmatched = TRUE;
+          Bpmatched = TRUE;
+          for (i = 0; i < NumBpUnit; i++) {
+            Pbpstr = IdsConsole->BreakpointList + Pbpunit[i].BpStrOffset;
+            if (Pbpunit[i].BpFlag == IDS_HDTOUT_BPFLAG_FORMAT_STR) {
+              PCmpStr = &ConsoleBuffer[LastIndex];
+              ConsoleBuffer[Index] = 0;
+            } else {
+              PCmpStr = IdsConsole->StatusStr;
+            }
+
+            if (LastBpmatched) {
+              Bpmatched = AmdIdsSubStr (PCmpStr, Pbpstr);
+              if (Bpmatched) {
+                if (Pbpunit[i].AndFlag == 0) {
+                  // This is the last of matching string of an AND block, apply action here
+                  switch (Pbpunit[i].Action) {
+                  case HDTOUT_BP_ACTION_HALT:
+                    IdsConsoleOps->Print (DEBUG_PRINT_BREAKPOINT, (UINT32)(ConsoleBuffer + LastIndex), ( i << 16) | (UINT32) Index, IdsConsole);
+                    break;
+                  case HDTOUT_BP_ACTION_PRINTON:
+                    PrintCtrl = IDS_STATE_ON;
+                    IdsConsole->Header.PrintState = IDS_STATE_ON;
+                    break;
+                  case HDTOUT_BP_ACTION_PRINTOFF:
+                    if (IdsConsole->Header.PrintState != IDS_STATE_OFF) {
+                      IdsConsole->Header.PrintState = IDS_STATE_OFF;
+                      IdsConsoleOps->Print (DEBUG_PRINT_COMMAND, (UINT32)ConsoleBuffer, (UINT32)Index, IdsConsole);
+                    }
+                    break;
+                  default:
+                    ASSERT (FALSE);
+                  }
+                  break;
+                }
+              }
+            }
+            if (Pbpunit[i].AndFlag == 1) {
+              LastBpmatched = Bpmatched;
+            } else {
+              LastBpmatched = TRUE;
+            }
+          }
+        }
+
+        //
+        //  Store status fields
+        //
+        if (SaveStatus) {
+          // Look for the start of the first word
+          for (; LastIndex < Index; LastIndex++) {
+            if ((ConsoleBuffer[LastIndex] > 32) && (ConsoleBuffer[LastIndex] < 127)) {
+              break;
+            }
+          }
+          if (LastIndex < Index) {
+            // Match the first word in StatusStr
+            SaveStatus = FALSE;
+            j = LastIndex;
+            for (i = 0; !SaveStatus && (IdsConsole->StatusStr[i] != 0); i++) {
+              ArrayLength = 1;
+              for (j = LastIndex; ConsoleBuffer[j] == IdsConsole->StatusStr[i]; j++) {
+                i++;
+                ArrayLength++;
+                if ((j == (Index - 1)) || (ConsoleBuffer[j] == ' ')) {
+                  // Find the length of this entry
+                  ArrayIndex = i;
+                  for (; IdsConsole->StatusStr[i] != '\n'; i++) {
+                    ArrayLength++;
+                  }
+
+                  // Remove old entry if it does not fit
+                  if (ArrayLength != ((UINT32) Index - LastIndex)) {
+                    for (i++; IdsConsole->StatusStr[i] != 0; i++) {
+                      IdsConsole->StatusStr[i - ArrayLength] = IdsConsole->StatusStr[i];
+                    }
+                    j = LastIndex;
+                    i = i - ArrayLength - 1;
+                    // Mark the end of string
+                    IdsConsole->StatusStr[i + ((UINT32) Index - LastIndex) + 1] = 0;
+                  } else {
+                    i = ArrayIndex - 2;
+                  }
+
+                  // Word match, exit for saving
+                  SaveStatus = TRUE;
+                  break;
+                }
+              }
+            }
+
+            // Copy string to StatusStr
+            for (; j < Index; j++, i++) {
+              IdsConsole->StatusStr[i] = ConsoleBuffer[j];
+            }
+            if (!SaveStatus) {
+              // Mark the end of string if not done so
+              IdsConsole->StatusStr[i] = 0;
+            }
+          }
+        }
+
+      }
+    }
+
+    if (IdsConsole != NULL) {
+      if (IdsConsole->Header.PrintState == IDS_STATE_OFF) {
+        Index = 0;    // Clear buffer if the data will not be printed out.
+      }
+      IdsConsole->Header.OutBufferIndex = (UINT16) Index;
+    } else {
+      IdsConsoleOps->Print (DEBUG_PRINT_COMMAND, (UINT32)ConsoleBuffer, (UINT32)Index, IdsConsole);
+    }
+  }
+}
+
+/**
+ *
+ *  Exit function for IDS console Function.
+ *
+ *  Restore debug register and Deallocate heap.
+ *
+ *  @param[in,out] StdHeader    The Pointer of AGESA Header
+ *
+ **/
+VOID
+AmdIdsConsoleExit (
+  IN OUT   AMD_CONFIG_PARAMS *StdHeader
+  )
+{
+  IDS_CONSOLE *IdsConsole;
+  UINTN IdsConsolePtr;
+  IDS_CONSOLE_OPERATIONS *IdsConsoleOps;
+  UINTN  IdsConsoleOpsPtr;
+
+  if (IsIdsConsoleEnabled (&IdsConsolePtr)) {
+    GetIdsConsole (&IdsConsolePtr);
+    IdsConsole = (IDS_CONSOLE *) IdsConsolePtr;
+    GetIdsConsoleOperations (&IdsConsoleOpsPtr);
+    IdsConsoleOps = (IDS_CONSOLE_OPERATIONS *) IdsConsoleOpsPtr;
+
+    if (IdsConsole != NULL) {
+      if (IdsConsole->Header.PrintState != IDS_STATE_OFF) {
+        IdsConsoleOps->Print (DEBUG_PRINT_COMMAND, (UINT32)(IdsConsole->OutBuffer), IdsConsole->Header.OutBufferIndex, IdsConsole);
+      }
+
+      IdsConsoleOps->Print (DEBUG_PRINT_EXIT, (UINT32)IdsConsole, 0, IdsConsole);
+
+      IdsConsoleOps->DestroyConsoleContext (IdsConsole);
+      DestroyIdsConsole (IdsConsole, StdHeader);
+
+    }
+  }
+}
+/**
+ * Check for CAR Corruption, the performance monitor number three programed to log the CAR Corruption.
+ * Check to see if control register is enabled and then check the preformance counter and stop the system by executing
+ * IDS_ERROR_TRAP if counter has any value other than zero.
+ *
+ *  @param[in,out] StdHeader    The Pointer of Standard Header.
+ *
+ *
+ **/
+VOID
+IdsCarCorruptionCheck (
+  IN OUT   AMD_CONFIG_PARAMS *StdHeader
+  )
+{
+
+  UINT64 Perf_Msr;
+
+  LibAmdMsrRead (MSR_PERF_CONTROL3, (UINT64*)&Perf_Msr, StdHeader);
+  if ((Perf_Msr & PERF_RESERVE_BIT_MASK) == PERF_CAR_CORRUPTION_EVENT) {
+    LibAmdMsrRead (MSR_PERF_COUNTER3, (UINT64*)&Perf_Msr, StdHeader);
+    if ((Perf_Msr != 0)) {
+      IDS_ERROR_TRAP;
+    }
+  }
+}