ArmVirtPkg/FdtClientDxe: implement new driver

[mirror_edk2.git] / ArmVirtPkg / FdtClientDxe / FdtClientDxe.c

/** @file\r
*  FDT client driver\r
*\r
*  Copyright (c) 2016, Linaro Ltd. All rights reserved.<BR>\r
*\r
*  This program and the accompanying materials are\r
*  licensed and made available under the terms and conditions of the BSD License\r
*  which accompanies this distribution.  The full text of the license may be found at\r
*  http://opensource.org/licenses/bsd-license.php\r
*\r
*  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
*  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
*\r
**/\r
\r
#include <Library/BaseLib.h>\r
#include <Library/DebugLib.h>\r
#include <Library/UefiDriverEntryPoint.h>\r
#include <Library/UefiBootServicesTableLib.h>\r
#include <Library/HobLib.h>\r
#include <libfdt.h>\r
\r
#include <Guid/FdtHob.h>\r
\r
#include <Protocol/FdtClient.h>\r
\r
STATIC VOID  *mDeviceTreeBase;\r
\r
STATIC\r
EFI_STATUS\r
GetNodeProperty (\r
  IN  FDT_CLIENT_PROTOCOL     *This,\r
  IN  INT32                   Node,\r
  IN  CONST CHAR8             *PropertyName,\r
  OUT CONST VOID              **Prop,\r
  OUT UINT32                  *PropSize OPTIONAL\r
  )\r
{\r
  INT32 Len;\r
\r
  ASSERT (mDeviceTreeBase != NULL);\r
  ASSERT (Prop != NULL);\r
\r
  *Prop = fdt_getprop (mDeviceTreeBase, Node, PropertyName, &Len);\r
  if (*Prop == NULL) {\r
    return EFI_NOT_FOUND;\r
  }\r
\r
  if (PropSize != NULL) {\r
    *PropSize = Len;\r
  }\r
  return EFI_SUCCESS;\r
}\r
\r
STATIC\r
EFI_STATUS\r
SetNodeProperty (\r
  IN  FDT_CLIENT_PROTOCOL     *This,\r
  IN  INT32                   Node,\r
  IN  CONST CHAR8             *PropertyName,\r
  IN  CONST VOID              *Prop,\r
  IN  UINT32                  PropSize\r
  )\r
{\r
  INT32 Ret;\r
\r
  ASSERT (mDeviceTreeBase != NULL);\r
\r
  Ret = fdt_setprop (mDeviceTreeBase, Node, PropertyName, Prop, PropSize);\r
  if (Ret != 0) {\r
    return EFI_DEVICE_ERROR;\r
  }\r
\r
  return EFI_SUCCESS;\r
}\r
\r
STATIC\r
EFI_STATUS\r
EFIAPI\r
FindNextCompatibleNode (\r
  IN  FDT_CLIENT_PROTOCOL     *This,\r
  IN  CONST CHAR8             *CompatibleString,\r
  IN  INT32                   PrevNode,\r
  OUT INT32                   *Node\r
  )\r
{\r
  INT32          Prev, Next;\r
  CONST CHAR8    *Type, *Compatible;\r
  INT32          Len;\r
\r
  ASSERT (mDeviceTreeBase != NULL);\r
  ASSERT (Node != NULL);\r
\r
  for (Prev = PrevNode;; Prev = Next) {\r
    Next = fdt_next_node (mDeviceTreeBase, Prev, NULL);\r
    if (Next < 0) {\r
      break;\r
    }\r
\r
    Type = fdt_getprop (mDeviceTreeBase, Next, "compatible", &Len);\r
    if (Type == NULL) {\r
      continue;\r
    }\r
\r
    //\r
    // A 'compatible' node may contain a sequence of NUL terminated\r
    // compatible strings so check each one\r
    //\r
    for (Compatible = Type; Compatible < Type + Len && *Compatible;\r
         Compatible += 1 + AsciiStrLen (Compatible)) {\r
      if (AsciiStrCmp (CompatibleString, Compatible) == 0) {\r
        *Node = Next;\r
        return EFI_SUCCESS;\r
      }\r
    }\r
  }\r
  return EFI_NOT_FOUND;\r
}\r
\r
STATIC\r
EFI_STATUS\r
EFIAPI\r
FindCompatibleNode (\r
  IN  FDT_CLIENT_PROTOCOL     *This,\r
  IN  CONST CHAR8             *CompatibleString,\r
  OUT INT32                   *Node\r
  )\r
{\r
  return FindNextCompatibleNode (This, CompatibleString, 0, Node);\r
}\r
\r
STATIC\r
EFI_STATUS\r
EFIAPI\r
FindCompatibleNodeProperty (\r
  IN  FDT_CLIENT_PROTOCOL     *This,\r
  IN  CONST CHAR8             *CompatibleString,\r
  IN  CONST CHAR8             *PropertyName,\r
  OUT CONST VOID              **Prop,\r
  OUT UINT32                  *PropSize OPTIONAL\r
  )\r
{\r
  EFI_STATUS        Status;\r
  INT32             Node;\r
\r
  Status = FindCompatibleNode (This, CompatibleString, &Node);\r
  if (EFI_ERROR (Status)) {\r
    return Status;\r
  }\r
\r
  return GetNodeProperty (This, Node, PropertyName, Prop, PropSize);\r
}\r
\r
STATIC\r
EFI_STATUS\r
EFIAPI\r
FindCompatibleNodeReg (\r
  IN  FDT_CLIENT_PROTOCOL     *This,\r
  IN  CONST CHAR8             *CompatibleString,\r
  OUT CONST VOID              **Reg,\r
  OUT UINT32                  *RegElemSize,\r
  OUT UINT32                  *RegSize\r
  )\r
{\r
  EFI_STATUS Status;\r
\r
  ASSERT (RegSize != NULL);\r
\r
  //\r
  // Get the 'reg' property of this node. For now, we will assume\r
  // 8 byte quantities for base and size, respectively.\r
  // TODO use #cells root properties instead\r
  //\r
  Status = FindCompatibleNodeProperty (This, CompatibleString, "reg", Reg,\r
             RegSize);\r
  if (EFI_ERROR (Status)) {\r
    return Status;\r
  }\r
\r
  if ((*RegSize % 8) != 0) {\r
    DEBUG ((EFI_D_ERROR,\r
      "%a: '%a' compatible node has invalid 'reg' property (size == 0x%x)\n",\r
      __FUNCTION__, CompatibleString, *RegSize));\r
    return EFI_NOT_FOUND;\r
  }\r
\r
  *RegElemSize = 8;\r
\r
  return EFI_SUCCESS;\r
}\r
\r
STATIC\r
EFI_STATUS\r
GetOrInsertChosenNode (\r
  IN  FDT_CLIENT_PROTOCOL     *This,\r
  OUT INT32                   *Node\r
  )\r
{\r
  INT32 NewNode;\r
\r
  ASSERT (mDeviceTreeBase != NULL);\r
  ASSERT (Node != NULL);\r
\r
  NewNode = fdt_path_offset (mDeviceTreeBase, "/chosen");\r
  if (NewNode < 0) {\r
    NewNode = fdt_add_subnode (mDeviceTreeBase, 0, "/chosen");\r
  }\r
\r
  if (NewNode < 0) {\r
    return EFI_OUT_OF_RESOURCES;\r
  }\r
\r
  *Node = NewNode;\r
\r
  return EFI_SUCCESS;\r
}\r
\r
STATIC FDT_CLIENT_PROTOCOL mFdtClientProtocol = {\r
  GetNodeProperty,\r
  SetNodeProperty,\r
  FindCompatibleNode,\r
  FindNextCompatibleNode,\r
  FindCompatibleNodeProperty,\r
  FindCompatibleNodeReg,\r
  GetOrInsertChosenNode,\r
};\r
\r
EFI_STATUS\r
EFIAPI\r
InitializeFdtClientDxe (\r
  IN EFI_HANDLE           ImageHandle,\r
  IN EFI_SYSTEM_TABLE     *SystemTable\r
  )\r
{\r
  VOID              *Hob;\r
  VOID              *DeviceTreeBase;\r
\r
  Hob = GetFirstGuidHob (&gFdtHobGuid);\r
  if (Hob == NULL || GET_GUID_HOB_DATA_SIZE (Hob) != sizeof (UINT64)) {\r
    return EFI_NOT_FOUND;\r
  }\r
  DeviceTreeBase = (VOID *)(UINTN)*(UINT64 *)GET_GUID_HOB_DATA (Hob);\r
\r
  if (fdt_check_header (DeviceTreeBase) != 0) {\r
    DEBUG ((EFI_D_ERROR, "%a: No DTB found @ 0x%p\n", __FUNCTION__,\r
      DeviceTreeBase));\r
    return EFI_NOT_FOUND;\r
  }\r
\r
  mDeviceTreeBase = DeviceTreeBase;\r
\r
  DEBUG ((EFI_D_INFO, "%a: DTB @ 0x%p\n", __FUNCTION__, mDeviceTreeBase));\r
\r
  return gBS->InstallProtocolInterface (&ImageHandle, &gFdtClientProtocolGuid,\r
                EFI_NATIVE_INTERFACE, &mFdtClientProtocol);\r
}\r