UefiCpuPkg: Move AsmRelocateApLoopStart from Mpfuncs.nasm to AmdSev.nasm

[mirror_edk2.git] / MdeModulePkg / Bus / Sd / SdBlockIoPei / SdHcMem.c

/** @file\r
\r
Copyright (c) 2015 - 2017, Intel Corporation. All rights reserved.<BR>\r
\r
SPDX-License-Identifier: BSD-2-Clause-Patent\r
\r
**/\r
\r
#include "SdBlockIoPei.h"\r
\r
/**\r
  Allocate a block of memory to be used by the buffer pool.\r
\r
  @param  Pages          How many pages to allocate.\r
\r
  @return The allocated memory block or NULL if failed.\r
\r
**/\r
SD_PEIM_MEM_BLOCK *\r
SdPeimAllocMemBlock (\r
  IN  UINTN  Pages\r
  )\r
{\r
  SD_PEIM_MEM_BLOCK     *Block;\r
  VOID                  *BufHost;\r
  VOID                  *Mapping;\r
  EFI_PHYSICAL_ADDRESS  MappedAddr;\r
  EFI_STATUS            Status;\r
  VOID                  *TempPtr;\r
\r
  TempPtr = NULL;\r
  Block   = NULL;\r
\r
  Status = PeiServicesAllocatePool (sizeof (SD_PEIM_MEM_BLOCK), &TempPtr);\r
  if (EFI_ERROR (Status)) {\r
    return NULL;\r
  }\r
\r
  ZeroMem ((VOID *)(UINTN)TempPtr, sizeof (SD_PEIM_MEM_BLOCK));\r
\r
  //\r
  // each bit in the bit array represents SD_PEIM_MEM_UNIT\r
  // bytes of memory in the memory block.\r
  //\r
  ASSERT (SD_PEIM_MEM_UNIT * 8 <= EFI_PAGE_SIZE);\r
\r
  Block          = (SD_PEIM_MEM_BLOCK *)(UINTN)TempPtr;\r
  Block->BufLen  = EFI_PAGES_TO_SIZE (Pages);\r
  Block->BitsLen = Block->BufLen / (SD_PEIM_MEM_UNIT * 8);\r
\r
  Status = PeiServicesAllocatePool (Block->BitsLen, &TempPtr);\r
  if (EFI_ERROR (Status)) {\r
    return NULL;\r
  }\r
\r
  ZeroMem ((VOID *)(UINTN)TempPtr, Block->BitsLen);\r
\r
  Block->Bits = (UINT8 *)(UINTN)TempPtr;\r
\r
  Status = IoMmuAllocateBuffer (\r
             Pages,\r
             &BufHost,\r
             &MappedAddr,\r
             &Mapping\r
             );\r
  if (EFI_ERROR (Status)) {\r
    return NULL;\r
  }\r
\r
  ZeroMem ((VOID *)(UINTN)BufHost, EFI_PAGES_TO_SIZE (Pages));\r
\r
  Block->BufHost = (UINT8 *)(UINTN)BufHost;\r
  Block->Buf     = (UINT8 *)(UINTN)MappedAddr;\r
  Block->Mapping = Mapping;\r
  Block->Next    = NULL;\r
\r
  return Block;\r
}\r
\r
/**\r
  Free the memory block from the memory pool.\r
\r
  @param  Pool           The memory pool to free the block from.\r
  @param  Block          The memory block to free.\r
\r
**/\r
VOID\r
SdPeimFreeMemBlock (\r
  IN SD_PEIM_MEM_POOL   *Pool,\r
  IN SD_PEIM_MEM_BLOCK  *Block\r
  )\r
{\r
  ASSERT ((Pool != NULL) && (Block != NULL));\r
\r
  IoMmuFreeBuffer (EFI_SIZE_TO_PAGES (Block->BufLen), Block->BufHost, Block->Mapping);\r
}\r
\r
/**\r
  Alloc some memory from the block.\r
\r
  @param  Block          The memory block to allocate memory from.\r
  @param  Units          Number of memory units to allocate.\r
\r
  @return The pointer to the allocated memory. If couldn't allocate the needed memory,\r
          the return value is NULL.\r
\r
**/\r
VOID *\r
SdPeimAllocMemFromBlock (\r
  IN  SD_PEIM_MEM_BLOCK  *Block,\r
  IN  UINTN              Units\r
  )\r
{\r
  UINTN  Byte;\r
  UINT8  Bit;\r
  UINTN  StartByte;\r
  UINT8  StartBit;\r
  UINTN  Available;\r
  UINTN  Count;\r
\r
  ASSERT ((Block != 0) && (Units != 0));\r
\r
  StartByte = 0;\r
  StartBit  = 0;\r
  Available = 0;\r
\r
  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {\r
    //\r
    // If current bit is zero, the corresponding memory unit is\r
    // available, otherwise we need to restart our searching.\r
    // Available counts the consecutive number of zero bit.\r
    //\r
    if (!SD_PEIM_MEM_BIT_IS_SET (Block->Bits[Byte], Bit)) {\r
      Available++;\r
\r
      if (Available >= Units) {\r
        break;\r
      }\r
\r
      SD_PEIM_NEXT_BIT (Byte, Bit);\r
    } else {\r
      SD_PEIM_NEXT_BIT (Byte, Bit);\r
\r
      Available = 0;\r
      StartByte = Byte;\r
      StartBit  = Bit;\r
    }\r
  }\r
\r
  if (Available < Units) {\r
    return NULL;\r
  }\r
\r
  //\r
  // Mark the memory as allocated\r
  //\r
  Byte = StartByte;\r
  Bit  = StartBit;\r
\r
  for (Count = 0; Count < Units; Count++) {\r
    ASSERT (!SD_PEIM_MEM_BIT_IS_SET (Block->Bits[Byte], Bit));\r
\r
    Block->Bits[Byte] = (UINT8)(Block->Bits[Byte] | (UINT8)SD_PEIM_MEM_BIT (Bit));\r
    SD_PEIM_NEXT_BIT (Byte, Bit);\r
  }\r
\r
  return Block->Buf + (StartByte * 8 + StartBit) * SD_PEIM_MEM_UNIT;\r
}\r
\r
/**\r
  Insert the memory block to the pool's list of the blocks.\r
\r
  @param  Head           The head of the memory pool's block list.\r
  @param  Block          The memory block to insert.\r
\r
**/\r
VOID\r
SdPeimInsertMemBlockToPool (\r
  IN SD_PEIM_MEM_BLOCK  *Head,\r
  IN SD_PEIM_MEM_BLOCK  *Block\r
  )\r
{\r
  ASSERT ((Head != NULL) && (Block != NULL));\r
  Block->Next = Head->Next;\r
  Head->Next  = Block;\r
}\r
\r
/**\r
  Is the memory block empty?\r
\r
  @param  Block   The memory block to check.\r
\r
  @retval TRUE    The memory block is empty.\r
  @retval FALSE   The memory block isn't empty.\r
\r
**/\r
BOOLEAN\r
SdPeimIsMemBlockEmpty (\r
  IN SD_PEIM_MEM_BLOCK  *Block\r
  )\r
{\r
  UINTN  Index;\r
\r
  for (Index = 0; Index < Block->BitsLen; Index++) {\r
    if (Block->Bits[Index] != 0) {\r
      return FALSE;\r
    }\r
  }\r
\r
  return TRUE;\r
}\r
\r
/**\r
  Initialize the memory management pool for the host controller.\r
\r
  @param  Private               The Sd Peim driver private data.\r
\r
  @retval EFI_SUCCESS           The memory pool is initialized.\r
  @retval Others                Fail to init the memory pool.\r
\r
**/\r
EFI_STATUS\r
SdPeimInitMemPool (\r
  IN  SD_PEIM_HC_PRIVATE_DATA  *Private\r
  )\r
{\r
  SD_PEIM_MEM_POOL  *Pool;\r
  EFI_STATUS        Status;\r
  VOID              *TempPtr;\r
\r
  TempPtr = NULL;\r
  Pool    = NULL;\r
\r
  Status = PeiServicesAllocatePool (sizeof (SD_PEIM_MEM_POOL), &TempPtr);\r
  if (EFI_ERROR (Status)) {\r
    return EFI_OUT_OF_RESOURCES;\r
  }\r
\r
  ZeroMem ((VOID *)(UINTN)TempPtr, sizeof (SD_PEIM_MEM_POOL));\r
\r
  Pool = (SD_PEIM_MEM_POOL *)((UINTN)TempPtr);\r
\r
  Pool->Head = SdPeimAllocMemBlock (SD_PEIM_MEM_DEFAULT_PAGES);\r
\r
  if (Pool->Head == NULL) {\r
    return EFI_OUT_OF_RESOURCES;\r
  }\r
\r
  Private->Pool = Pool;\r
  return EFI_SUCCESS;\r
}\r
\r
/**\r
  Release the memory management pool.\r
\r
  @param  Pool                  The memory pool to free.\r
\r
  @retval EFI_DEVICE_ERROR      Fail to free the memory pool.\r
  @retval EFI_SUCCESS           The memory pool is freed.\r
\r
**/\r
EFI_STATUS\r
SdPeimFreeMemPool (\r
  IN SD_PEIM_MEM_POOL  *Pool\r
  )\r
{\r
  SD_PEIM_MEM_BLOCK  *Block;\r
\r
  ASSERT (Pool->Head != NULL);\r
\r
  //\r
  // Unlink all the memory blocks from the pool, then free them.\r
  //\r
  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {\r
    SdPeimFreeMemBlock (Pool, Block);\r
  }\r
\r
  SdPeimFreeMemBlock (Pool, Pool->Head);\r
\r
  return EFI_SUCCESS;\r
}\r
\r
/**\r
  Allocate some memory from the host controller's memory pool\r
  which can be used to communicate with host controller.\r
\r
  @param  Pool      The host controller's memory pool.\r
  @param  Size      Size of the memory to allocate.\r
\r
  @return The allocated memory or NULL.\r
\r
**/\r
VOID *\r
SdPeimAllocateMem (\r
  IN  SD_PEIM_MEM_POOL  *Pool,\r
  IN  UINTN             Size\r
  )\r
{\r
  SD_PEIM_MEM_BLOCK  *Head;\r
  SD_PEIM_MEM_BLOCK  *Block;\r
  SD_PEIM_MEM_BLOCK  *NewBlock;\r
  VOID               *Mem;\r
  UINTN              AllocSize;\r
  UINTN              Pages;\r
\r
  Mem       = NULL;\r
  AllocSize = SD_PEIM_MEM_ROUND (Size);\r
  Head      = Pool->Head;\r
  ASSERT (Head != NULL);\r
\r
  //\r
  // First check whether current memory blocks can satisfy the allocation.\r
  //\r
  for (Block = Head; Block != NULL; Block = Block->Next) {\r
    Mem = SdPeimAllocMemFromBlock (Block, AllocSize / SD_PEIM_MEM_UNIT);\r
\r
    if (Mem != NULL) {\r
      ZeroMem (Mem, Size);\r
      break;\r
    }\r
  }\r
\r
  if (Mem != NULL) {\r
    return Mem;\r
  }\r
\r
  //\r
  // Create a new memory block if there is not enough memory\r
  // in the pool. If the allocation size is larger than the\r
  // default page number, just allocate a large enough memory\r
  // block. Otherwise allocate default pages.\r
  //\r
  if (AllocSize > EFI_PAGES_TO_SIZE (SD_PEIM_MEM_DEFAULT_PAGES)) {\r
    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;\r
  } else {\r
    Pages = SD_PEIM_MEM_DEFAULT_PAGES;\r
  }\r
\r
  NewBlock = SdPeimAllocMemBlock (Pages);\r
  if (NewBlock == NULL) {\r
    return NULL;\r
  }\r
\r
  //\r
  // Add the new memory block to the pool, then allocate memory from it\r
  //\r
  SdPeimInsertMemBlockToPool (Head, NewBlock);\r
  Mem = SdPeimAllocMemFromBlock (NewBlock, AllocSize / SD_PEIM_MEM_UNIT);\r
\r
  if (Mem != NULL) {\r
    ZeroMem (Mem, Size);\r
  }\r
\r
  return Mem;\r
}\r
\r
/**\r
  Free the allocated memory back to the memory pool.\r
\r
  @param  Pool           The memory pool of the host controller.\r
  @param  Mem            The memory to free.\r
  @param  Size           The size of the memory to free.\r
\r
**/\r
VOID\r
SdPeimFreeMem (\r
  IN SD_PEIM_MEM_POOL  *Pool,\r
  IN VOID              *Mem,\r
  IN UINTN             Size\r
  )\r
{\r
  SD_PEIM_MEM_BLOCK  *Head;\r
  SD_PEIM_MEM_BLOCK  *Block;\r
  UINT8              *ToFree;\r
  UINTN              AllocSize;\r
  UINTN              Byte;\r
  UINTN              Bit;\r
  UINTN              Count;\r
\r
  Head      = Pool->Head;\r
  AllocSize = SD_PEIM_MEM_ROUND (Size);\r
  ToFree    = (UINT8 *)Mem;\r
\r
  for (Block = Head; Block != NULL; Block = Block->Next) {\r
    //\r
    // scan the memory block list for the memory block that\r
    // completely contains the memory to free.\r
    //\r
    if ((Block->Buf <= ToFree) && ((ToFree + AllocSize) <= (Block->Buf + Block->BufLen))) {\r
      //\r
      // compute the start byte and bit in the bit array\r
      //\r
      Byte = ((ToFree - Block->Buf) / SD_PEIM_MEM_UNIT) / 8;\r
      Bit  = ((ToFree - Block->Buf) / SD_PEIM_MEM_UNIT) % 8;\r
\r
      //\r
      // reset associated bits in bit array\r
      //\r
      for (Count = 0; Count < (AllocSize / SD_PEIM_MEM_UNIT); Count++) {\r
        ASSERT (SD_PEIM_MEM_BIT_IS_SET (Block->Bits[Byte], Bit));\r
\r
        Block->Bits[Byte] = (UINT8)(Block->Bits[Byte] ^ SD_PEIM_MEM_BIT (Bit));\r
        SD_PEIM_NEXT_BIT (Byte, Bit);\r
      }\r
\r
      break;\r
    }\r
  }\r
\r
  //\r
  // If Block == NULL, it means that the current memory isn't\r
  // in the host controller's pool. This is critical because\r
  // the caller has passed in a wrong memory point\r
  //\r
  ASSERT (Block != NULL);\r
\r
  //\r
  // Release the current memory block if it is empty and not the head\r
  //\r
  if ((Block != Head) && SdPeimIsMemBlockEmpty (Block)) {\r
    SdPeimFreeMemBlock (Pool, Block);\r
  }\r
\r
  return;\r
}\r