[edk2,v2,02/24] ArmVirtPkg/FdtClientDxe: implement new driver

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