/** @file
  Helper implementations recovered from copied BaseLib/PeiServices helpers.
**/

#include "AmiCpuPeiPreMem.h"

#include <stdarg.h>

INTN EFIAPI DebugAssertWithFunc (
  IN CONST CHAR8 *FileName,
  IN INTN        LineNumber,
  IN CONST CHAR8 *Description
  );
UINT64 EFIAPI ReadUnaligned64 (IN CONST VOID *Buffer);

void *
Internal_memmove (
  VOID  *Destination,
  VOID  *Source,
  UINTN  Count
  )
{
  UINT8 *Dst;
  UINT8 *Src;
  UINTN  Remaining;

  if (Count == 0) {
    return Destination;
  }

  Dst = Destination;
  Src = Source;
  Remaining = Count;

  if (Src < Dst && Src + Count - 1 >= Dst) {
    Src += Count - 1;
    Dst += Count - 1;
    while (Count-- > 0) {
      *Dst-- = *Src--;
    }
    return Destination;
  }

  while (Remaining > 3) {
    *Dst++ = *Src++;
    *Dst++ = *Src++;
    *Dst++ = *Src++;
    *Dst++ = *Src++;
    Remaining -= 4;
  }

  while (Remaining-- > 0) {
    *Dst++ = *Src++;
  }

  return Destination;
}

VOID *
memset (
  VOID *Buffer,
  UINTN Count,
  CHAR8 Value
  )
{
  UINT8 *Dst = Buffer;

  while (Count-- != 0) {
    *Dst++ = (UINT8)Value;
  }

  return Buffer;
}

UINT32 *
Internal_memset64 (
  UINT32 *Buffer,
  UINT32  Count,
  UINT32  ValueLo,
  UINT32  ValueHi
  )
{
  UINT32 *Result = Buffer;
  while (Count-- != 0) {
    *Result++ = ValueLo;
    *Result++ = ValueHi;
  }
  return Buffer;
}

VOID *
memset32 (
  VOID *Buffer,
  UINTN Count,
  INT32 Value
  )
{
  UINT32 *Dst = Buffer;
  while (Count-- != 0) {
    *Dst++ = (UINT32)Value;
  }
  return Buffer;
}

BOOLEAN
CompareGuid (
  IN UINTN Guid1,
  IN UINTN Guid2
  )
{
  return (ReadUnaligned64 ((CONST VOID *)Guid1) == ReadUnaligned64 ((CONST VOID *)Guid2)) &&
         (ReadUnaligned64 ((CONST VOID *)(Guid1 + 8)) == ReadUnaligned64 ((CONST VOID *)(Guid2 + 8)));
}

INTN EFIAPI
CpuId (
  IN INTN     FunctionId,
  OUT UINT32 *CpuidEax
  )
{
  UINT32 EaxOut;

  (VOID)FunctionId;
  EaxOut = 0;
  __asm {
    mov eax, 1
    cpuid
    mov EaxOut, eax
  }

  if (CpuidEax != NULL) {
    *CpuidEax = EaxOut;
  }
  return 1;
}

UINT64 EFIAPI
ReadUnaligned64 (
  IN CONST VOID *Buffer
  )
{
  if (Buffer == NULL) {
    DebugAssertWithFunc (0x80000000, (CONST CHAR8 *)"AmiCpuPeiPreMem", 0);
  }
  return *(UINT64 *)Buffer;
}

UINT64 EFIAPI
GetMask23 (
  VOID
  )
{
  return 0xFFE0000000000000ULL;
}

UINT64 EFIAPI
RShift18 (
  IN UINTN  LowBits,
  IN UINT32 HighBits
  )
{
  (VOID)LowBits;
  return (UINT64)HighBits >> 18;
}

VOID *EFIAPI
GetPeiServicesTablePointer (
  VOID
  )
{
  UINT8 IdtrData[10];
  UINTN IdtrBase;

  ReadIdtr (IdtrData);
  IdtrBase = *(UINTN *)(IdtrData + 2);
  if (IdtrBase == 0) {
    DebugAssertWithFunc (
      0x80000000,
      (CONST CHAR8 *)"e:\\hs\\MdePkg\\Library\\PeiServicesTablePointerLibIdt\\PeiServicesTablePointer.c",
      48
      );
  }

  return (VOID *)(UINTN)IdtrBase;
}

INTN EFIAPI
GetBootModeFromCmos (
  VOID
  )
{
  UINT8 CmosReg;
  UINT8 RawBootMode;
  UINT8 BootMode;
  volatile UINT8 *BootModeReg;

  BootModeReg = (UINT8 *)(UINTN)0xFDAF0490;
  CmosReg = __inbyte (0x70u);
  __outbyte (0x70u, (UINT8)(CmosReg & 0x80u | 0x4Au));
  RawBootMode = __inbyte (0x71u);
  BootMode = RawBootMode;

  if (RawBootMode > 3u) {
    BootMode = RawBootMode;
    if (RawBootMode == 0u) {
      BootMode = (UINT8)(*BootModeReg & 2U | 1U);
    }
    if (BootMode != 0xFFu) {
      return (BootMode != 1) ? (INTN)0x80000006 : (INTN)0x80000054;
    }
  }

  return 0;
}

VOID *EFIAPI
ReadIdtr (
  OUT VOID *Idtr
  )
{
  if (Idtr == NULL) {
    DebugAssertWithFunc (
      0x80000000,
      (CONST CHAR8 *)"e:\\hs\\MdePkg\\Library\\BaseLib\\X86ReadIdtr.c",
      37
      );
  }
  __sidt (Idtr);
  return Idtr;
}

INTN EFIAPI
DebugPrint (
  VOID
  )
{
  EFI_STATUS (*LocatePpi)(
    IN VOID *PeiServices,
    IN VOID *Guid,
    IN UINTN Instance,
    OUT VOID **Descriptor,
    OUT VOID **Protocol
    );
  VOID    *PeiServices;
  VOID    *PpiDescriptor;
  VOID    *DebugProtocol;
  EFI_STATUS Status;

  PeiServices = GetPeiServicesTablePointer ();
  LocatePpi = (EFI_STATUS (*)(VOID *, VOID *, UINTN, VOID **, VOID **))((VOID **)PeiServices)[8];
  Status = LocatePpi (PeiServices, NULL, 0, &PpiDescriptor, &DebugProtocol);
  if (Status >= 0) {
    return (INTN)(UINTN)DebugProtocol;
  }
  return 0;
}

INTN EFIAPI
DebugAssert (
  IN INTN   ErrorMask,
  IN CONST CHAR8 *Format,
  ...
  )
{
  INTN   Result;
  va_list Args;
  INTN (*DebugPrintFunc) (INTN, CONST CHAR8 *, char *);

  va_start (Args, Format);
  Result = DebugPrint ();
  DebugPrintFunc = (INTN (*)(INTN, CONST CHAR8 *, char *))(UINTN)Result;
  if (Result != 0 && ((INTN)(GetBootModeFromCmos () & ErrorMask) != 0)) {
    Result = DebugPrintFunc (ErrorMask, Format, (char *)Args);
  }
  va_end (Args);
  return Result;
}

INTN EFIAPI
DebugAssertWithFunc (
  IN CONST CHAR8 *FileName,
  IN INTN        LineNumber,
  IN CONST CHAR8 *Description
  )
{
  INTN (*DebugPrintFunc)(INTN, CONST CHAR8 *, INTN);
  INTN  Result;

  Result = DebugPrint ();
  DebugPrintFunc = (INTN (*)(INTN, CONST CHAR8 *, INTN))(UINTN)Result;
  if (Result != 0) {
    return DebugPrintFunc (0x80000000, FileName, LineNumber);
  }

  return Result;
}