/** @file
*  FdtHelperLib.c
*
*  Copyright (c) 2021, NUVIA Inc. All rights reserved.
*  Copyright (c) 2020, Linaro Ltd. All rights reserved.
*
*  SPDX-License-Identifier: BSD-2-Clause-Patent
*
**/

#include <Uefi.h>
#include <Library/DebugLib.h>
#include <Library/FdtHelperLib.h>
#include <Library/PcdLib.h>
#include <libfdt.h>

STATIC INT32  mFdtFirstCpuOffset;
STATIC INT32  mFdtCpuNodeSize;

/**
  Get MPIDR for a given cpu from device tree passed by Qemu.

  @param [in]   CpuId    Index of cpu to retrieve MPIDR value for.

  @retval                MPIDR value of CPU at index <CpuId>
**/
UINT64
FdtHelperGetMpidr (
  IN UINTN  CpuId
  )
{
  VOID          *DeviceTreeBase;
  CONST UINT64  *RegVal;
  INT32         Len;

  DeviceTreeBase = (VOID *)(UINTN)PcdGet64 (PcdDeviceTreeInitialBaseAddress);
  ASSERT (DeviceTreeBase != NULL);

  RegVal = fdt_getprop (
             DeviceTreeBase,
             mFdtFirstCpuOffset + (CpuId * mFdtCpuNodeSize),
             "reg",
             &Len
             );
  if (!RegVal) {
    DEBUG ((DEBUG_ERROR, "Couldn't find reg property for CPU:%d\n", CpuId));
    return 0;
  }

  return (fdt64_to_cpu (ReadUnaligned64 (RegVal)));
}

/** Walks through the Device Tree created by Qemu and counts the number
    of CPUs present in it.

    @return The number of CPUs present.
**/
EFIAPI
UINT32
FdtHelperCountCpus (
  VOID
  )
{
  VOID    *DeviceTreeBase;
  INT32   Node;
  INT32   Prev;
  INT32   CpuNode;
  UINT32  CpuCount;

  DeviceTreeBase = (VOID *)(UINTN)PcdGet64 (PcdDeviceTreeInitialBaseAddress);
  ASSERT (DeviceTreeBase != NULL);

  // Make sure we have a valid device tree blob
  ASSERT (fdt_check_header (DeviceTreeBase) == 0);

  CpuNode = fdt_path_offset (DeviceTreeBase, "/cpus");
  if (CpuNode <= 0) {
    DEBUG ((DEBUG_ERROR, "Unable to locate /cpus in device tree\n"));
    return 0;
  }

  CpuCount = 0;

  // Walk through /cpus node and count the number of subnodes.
  // The count of these subnodes corresponds to the number of
  // CPUs created by Qemu.
  Prev               = fdt_first_subnode (DeviceTreeBase, CpuNode);
  mFdtFirstCpuOffset = Prev;
  while (1) {
    CpuCount++;
    Node = fdt_next_subnode (DeviceTreeBase, Prev);
    if (Node < 0) {
      break;
    }

    mFdtCpuNodeSize = Node - Prev;
    Prev            = Node;
  }

  return CpuCount;
}
