[PATCH v2 edk2-platforms 06/20] Platform/Broadcom/RPi3: Add Interrupt and Device Tree drivers

[Pete Batard <pete@akeo.ie>]

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Pete Batard <pete@akeo.ie>
---
 Platform/Broadcom/Bcm283x/Drivers/Bcm2836InterruptDxe/Bcm2836InterruptDxe.c   | 367 +++++++++++++++++++
 Platform/Broadcom/Bcm283x/Drivers/Bcm2836InterruptDxe/Bcm2836InterruptDxe.inf |  48 +++
 Platform/Broadcom/Bcm283x/Drivers/RpiFdtDxe/RpiFdtDxe.c                       | 370 ++++++++++++++++++++
 Platform/Broadcom/Bcm283x/Drivers/RpiFdtDxe/RpiFdtDxe.inf                     |  53 +++
 4 files changed, 838 insertions(+)

diff --git a/Platform/Broadcom/Bcm283x/Drivers/Bcm2836InterruptDxe/Bcm2836InterruptDxe.c b/Platform/Broadcom/Bcm283x/Drivers/Bcm2836InterruptDxe/Bcm2836InterruptDxe.c
new file mode 100644
index 000000000000..dda61665031d
--- /dev/null
+++ b/Platform/Broadcom/Bcm283x/Drivers/Bcm2836InterruptDxe/Bcm2836InterruptDxe.c
@@ -0,0 +1,367 @@
+/** @file
+ *
+ *  Copyright (c) 2016, Linaro, Ltd. All rights reserved.
+ *
+ *  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 <PiDxe.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/IoLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+
+#include <IndustryStandard/Bcm2836.h>
+
+#include <Protocol/Cpu.h>
+#include <Protocol/HardwareInterrupt.h>
+
+//
+// This currently only implements support for the architected timer interrupts
+// on the per-CPU interrupt controllers.
+//
+#define NUM_IRQS                    (4)
+
+#ifdef MDE_CPU_AARCH64
+#define ARM_ARCH_EXCEPTION_IRQ      EXCEPT_AARCH64_IRQ
+#else
+#define ARM_ARCH_EXCEPTION_IRQ      EXCEPT_ARM_IRQ
+#endif
+
+STATIC CONST
+EFI_PHYSICAL_ADDRESS RegBase = FixedPcdGet32 (PcdInterruptBaseAddress);
+
+//
+// Notifications
+//
+STATIC EFI_EVENT                    mExitBootServicesEvent;
+STATIC HARDWARE_INTERRUPT_HANDLER   mRegisteredInterruptHandlers[NUM_IRQS];
+
+/**
+  Shutdown our hardware
+
+  DXE Core will disable interrupts and turn off the timer and disable interrupts
+  after all the event handlers have run.
+
+  @param[in]  Event   The Event that is being processed
+  @param[in]  Context Event Context
+**/
+STATIC
+VOID
+EFIAPI
+ExitBootServicesEvent (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  // Disable all interrupts
+  MmioWrite32 (RegBase + BCM2836_INTC_TIMER_CONTROL_OFFSET, 0);
+}
+
+/**
+  Enable interrupt source Source.
+
+  @param This     Instance pointer for this protocol
+  @param Source   Hardware source of the interrupt
+
+  @retval EFI_SUCCESS       Source interrupt enabled.
+  @retval EFI_DEVICE_ERROR  Hardware could not be programmed.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+EnableInterruptSource (
+  IN EFI_HARDWARE_INTERRUPT_PROTOCOL    *This,
+  IN HARDWARE_INTERRUPT_SOURCE          Source
+  )
+{
+  if (Source >= NUM_IRQS) {
+    ASSERT(FALSE);
+    return EFI_UNSUPPORTED;
+  }
+
+  MmioOr32 (RegBase + BCM2836_INTC_TIMER_CONTROL_OFFSET, 1 << Source);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Disable interrupt source Source.
+
+  @param This     Instance pointer for this protocol
+  @param Source   Hardware source of the interrupt
+
+  @retval EFI_SUCCESS       Source interrupt disabled.
+  @retval EFI_DEVICE_ERROR  Hardware could not be programmed.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+DisableInterruptSource (
+  IN EFI_HARDWARE_INTERRUPT_PROTOCOL    *This,
+  IN HARDWARE_INTERRUPT_SOURCE          Source
+  )
+{
+  if (Source >= NUM_IRQS) {
+    ASSERT(FALSE);
+    return EFI_UNSUPPORTED;
+  }
+
+  MmioAnd32 (RegBase + BCM2836_INTC_TIMER_CONTROL_OFFSET, ~(1 << Source));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register Handler for the specified interrupt source.
+
+  @param This     Instance pointer for this protocol
+  @param Source   Hardware source of the interrupt
+  @param Handler  Callback for interrupt. NULL to unregister
+
+  @retval EFI_SUCCESS Source was updated to support Handler.
+  @retval EFI_DEVICE_ERROR  Hardware could not be programmed.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+RegisterInterruptSource (
+  IN EFI_HARDWARE_INTERRUPT_PROTOCOL    *This,
+  IN HARDWARE_INTERRUPT_SOURCE          Source,
+  IN HARDWARE_INTERRUPT_HANDLER         Handler
+  )
+{
+  if (Source >= NUM_IRQS) {
+    ASSERT (FALSE);
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Handler == NULL && mRegisteredInterruptHandlers[Source] == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Handler != NULL && mRegisteredInterruptHandlers[Source] != NULL) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  mRegisteredInterruptHandlers[Source] = Handler;
+  return EnableInterruptSource(This, Source);
+}
+
+
+/**
+  Return current state of interrupt source Source.
+
+  @param This     Instance pointer for this protocol
+  @param Source   Hardware source of the interrupt
+  @param InterruptState  TRUE: source enabled, FALSE: source disabled.
+
+  @retval EFI_SUCCESS       InterruptState is valid
+  @retval EFI_DEVICE_ERROR  InterruptState is not valid
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+GetInterruptSourceState (
+  IN EFI_HARDWARE_INTERRUPT_PROTOCOL    *This,
+  IN HARDWARE_INTERRUPT_SOURCE          Source,
+  IN BOOLEAN                            *InterruptState
+  )
+{
+  if (InterruptState == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Source >= NUM_IRQS) {
+    ASSERT(FALSE);
+    return EFI_UNSUPPORTED;
+  }
+
+  *InterruptState = (MmioRead32 (RegBase + BCM2836_INTC_TIMER_CONTROL_OFFSET) &
+                     (1 << Source)) != 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Signal to the hardware that the End Of Intrrupt state
+  has been reached.
+
+  @param This     Instance pointer for this protocol
+  @param Source   Hardware source of the interrupt
+
+  @retval EFI_SUCCESS       Source interrupt EOI'ed.
+  @retval EFI_DEVICE_ERROR  Hardware could not be programmed.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+EndOfInterrupt (
+  IN EFI_HARDWARE_INTERRUPT_PROTOCOL    *This,
+  IN HARDWARE_INTERRUPT_SOURCE          Source
+  )
+{
+  return EFI_SUCCESS;
+}
+
+
+/**
+  EFI_CPU_INTERRUPT_HANDLER that is called when a processor interrupt occurs.
+
+  @param  InterruptType    Defines the type of interrupt or exception that
+                           occurred on the processor.This parameter is processor
+                           architecture specific.
+  @param  SystemContext    A pointer to the processor context when
+                           the interrupt occurred on the processor.
+
+  @return None
+
+**/
+STATIC
+VOID
+EFIAPI
+IrqInterruptHandler (
+  IN EFI_EXCEPTION_TYPE           InterruptType,
+  IN EFI_SYSTEM_CONTEXT           SystemContext
+  )
+{
+  HARDWARE_INTERRUPT_HANDLER  InterruptHandler;
+  HARDWARE_INTERRUPT_SOURCE   Source;
+  UINT32                      RegVal;
+
+  RegVal = MmioRead32 (RegBase + BCM2836_INTC_TIMER_PENDING_OFFSET) &
+           ((1 << NUM_IRQS) - 1);
+  Source = HighBitSet32 (RegVal);
+  if (Source < 0) {
+    return;
+  }
+
+  InterruptHandler = mRegisteredInterruptHandlers [Source];
+  if (InterruptHandler != NULL) {
+    // Call the registered interrupt handler.
+    InterruptHandler (Source, SystemContext);
+  }
+}
+
+//
+// The protocol instance produced by this driver
+//
+STATIC EFI_HARDWARE_INTERRUPT_PROTOCOL gHardwareInterruptProtocol = {
+  RegisterInterruptSource,
+  EnableInterruptSource,
+  DisableInterruptSource,
+  GetInterruptSourceState,
+  EndOfInterrupt
+};
+
+STATIC VOID *mCpuArchProtocolNotifyEventRegistration;
+
+STATIC
+VOID
+EFIAPI
+CpuArchEventProtocolNotify (
+  IN  EFI_EVENT       Event,
+  IN  VOID            *Context
+  )
+{
+  EFI_CPU_ARCH_PROTOCOL   *Cpu;
+  EFI_STATUS              Status;
+
+  //
+  // Get the CPU protocol that this driver requires.
+  //
+  Status = gBS->LocateProtocol(&gEfiCpuArchProtocolGuid, NULL, (VOID **)&Cpu);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // Unregister the default exception handler.
+  //
+  Status = Cpu->RegisterInterruptHandler(Cpu, ARM_ARCH_EXCEPTION_IRQ, NULL);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: Cpu->RegisterInterruptHandler() - %r\n",
+      __FUNCTION__, Status));
+    ASSERT (FALSE);
+    return;
+  }
+
+  //
+  // Register to receive interrupts
+  //
+  Status = Cpu->RegisterInterruptHandler(Cpu, ARM_ARCH_EXCEPTION_IRQ,
+                                         IrqInterruptHandler);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: Cpu->RegisterInterruptHandler() - %r\n",
+      __FUNCTION__, Status));
+    ASSERT (FALSE);
+    return;
+  }
+}
+
+
+/**
+  Initialize the state information for the CPU Architectural Protocol
+
+  @param  ImageHandle   of the loaded driver
+  @param  SystemTable   Pointer to the System Table
+
+  @retval EFI_SUCCESS           Protocol registered
+  @retval EFI_OUT_OF_RESOURCES  Cannot allocate protocol data structure
+  @retval EFI_DEVICE_ERROR      Hardware problems
+
+**/
+EFI_STATUS
+InterruptDxeInitialize (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS Status;
+  EFI_EVENT  CpuArchEvent;
+
+  // Make sure the Interrupt Controller Protocol is not already installed in the system.
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gHardwareInterruptProtocolGuid);
+
+  Status = gBS->InstallMultipleProtocolInterfaces(
+                  &ImageHandle,
+                  &gHardwareInterruptProtocolGuid,
+                  &gHardwareInterruptProtocol,
+                  NULL);
+  ASSERT_EFI_ERROR(Status);
+
+  //
+  // Install the interrupt handler as soon as the CPU arch protocol appears.
+  //
+  CpuArchEvent = EfiCreateProtocolNotifyEvent (
+                   &gEfiCpuArchProtocolGuid,
+                   TPL_CALLBACK,
+                   CpuArchEventProtocolNotify,
+                   NULL,
+                   &mCpuArchProtocolNotifyEventRegistration
+                   );
+  ASSERT (CpuArchEvent != NULL);
+
+  // Register for an ExitBootServicesEvent
+  Status = gBS->CreateEvent(EVT_SIGNAL_EXIT_BOOT_SERVICES, TPL_NOTIFY,
+                  ExitBootServicesEvent, NULL, &mExitBootServicesEvent);
+
+  ASSERT_EFI_ERROR(Status);
+
+  return Status;
+}
diff --git a/Platform/Broadcom/Bcm283x/Drivers/Bcm2836InterruptDxe/Bcm2836InterruptDxe.inf b/Platform/Broadcom/Bcm283x/Drivers/Bcm2836InterruptDxe/Bcm2836InterruptDxe.inf
new file mode 100644
index 000000000000..831ae1bfeeab
--- /dev/null
+++ b/Platform/Broadcom/Bcm283x/Drivers/Bcm2836InterruptDxe/Bcm2836InterruptDxe.inf
@@ -0,0 +1,48 @@
+#/** @file
+#
+#  Copyright (c) 2017, Andrei Warkentin <andrey.warkentin@gmail.com>
+#  Copyright (c) 2016 Linaro, Ltd. All rights reserved.
+#
+#  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                    = 0x00010019
+  BASE_NAME                      = Bcm2836InterruptDxe
+  FILE_GUID                      = 3944f2d7-2e09-4fc0-9e98-008375641453
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InterruptDxeInitialize
+
+[Sources]
+  Bcm2836InterruptDxe.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  EmbeddedPkg/EmbeddedPkg.dec
+  Platform/Broadcom/Bcm283x/RaspberryPiPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  IoLib
+  UefiBootServicesTableLib
+  UefiLib
+  UefiDriverEntryPoint
+
+[Protocols]
+  gHardwareInterruptProtocolGuid  ## PRODUCES
+  gEfiCpuArchProtocolGuid         ## CONSUMES ## NOTIFY
+
+[FixedPcd]
+  gEmbeddedTokenSpaceGuid.PcdInterruptBaseAddress
+
+[Depex]
+  TRUE
diff --git a/Platform/Broadcom/Bcm283x/Drivers/RpiFdtDxe/RpiFdtDxe.c b/Platform/Broadcom/Bcm283x/Drivers/RpiFdtDxe/RpiFdtDxe.c
new file mode 100644
index 000000000000..f2b0117629fd
--- /dev/null
+++ b/Platform/Broadcom/Bcm283x/Drivers/RpiFdtDxe/RpiFdtDxe.c
@@ -0,0 +1,370 @@
+/** @file
+ *
+ *  Copyright (c) 2017, Andrey Warkentin <andrey.warkentin@gmail.com>
+ *  Copyright (c) 2016, Linaro, Ltd. All rights reserved.
+ *
+ *  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 <PiDxe.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/DxeServicesLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <libfdt.h>
+
+#include <Protocol/RaspberryPiFirmware.h>
+
+#include <Guid/Fdt.h>
+
+STATIC VOID                             *mFdtImage;
+
+STATIC RASPBERRY_PI_FIRMWARE_PROTOCOL   *mFwProtocol;
+
+STATIC
+VOID
+UpdateMacAddress (
+  VOID
+  )
+{
+  INTN          Node;
+  INTN          Retval;
+  EFI_STATUS    Status;
+  UINT8         MacAddress[6];
+
+  //
+  // Locate the node that the 'ethernet' alias refers to
+  //
+  Node = fdt_path_offset(mFdtImage, "ethernet");
+  if (Node < 0) {
+    DEBUG ((DEBUG_ERROR, "%a: failed to locate 'ethernet' alias\n",
+      __FUNCTION__));
+    return;
+  }
+
+  //
+  // Get the MAC address from the firmware
+  //
+  Status = mFwProtocol->GetMacAddress (MacAddress);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: failed to retrieve MAC address\n", __FUNCTION__));
+    return;
+  }
+
+  Retval = fdt_setprop (mFdtImage, Node, "mac-address", MacAddress,
+    sizeof MacAddress);
+  if (Retval != 0) {
+    DEBUG ((DEBUG_ERROR, "%a: failed to create 'mac-address' property (%d)\n",
+      __FUNCTION__, Retval));
+      return;
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "%a: setting MAC address to %02x:%02x:%02x:%02x:%02x:%02x\n",
+    __FUNCTION__, MacAddress[0], MacAddress[1], MacAddress[2], MacAddress[3],
+    MacAddress[4], MacAddress[5]));
+}
+
+STATIC
+VOID
+CleanMemoryNodes (
+  VOID
+  )
+{
+  INTN Node;
+  INT32 Retval;
+
+  Node = fdt_path_offset(mFdtImage, "/memory");
+  if (Node < 0) {
+    return;
+  }
+
+  /*
+   * Remove bogus memory nodes which can make the booted
+   * OS go crazy and ignore the UEFI map.
+   */
+  DEBUG ((DEBUG_INFO, "Removing bogus /memory\n"));
+  Retval = fdt_del_node(mFdtImage, Node);
+  if (Retval != 0) {
+    DEBUG ((DEBUG_ERROR, "Failed to remove /memory\n"));
+  }
+}
+
+STATIC
+VOID
+SanitizePSCI (
+  VOID
+  )
+{
+  INTN Node;
+  INTN Root;
+  INT32 Retval;
+
+  Root = fdt_path_offset(mFdtImage, "/");
+  ASSERT (Root >= 0);
+  if (Root < 0) {
+    return;
+  }
+
+  Node = fdt_path_offset(mFdtImage, "/psci");
+  if (Node < 0) {
+    Node = fdt_add_subnode(mFdtImage, Root, "psci");
+  }
+
+  ASSERT (Node >= 0);
+  if (Node < 0) {
+    DEBUG ((DEBUG_ERROR, "Couldn't find/create /psci\n"));
+    return;
+  }
+
+  Retval = fdt_setprop_string(mFdtImage, Node, "compatible",
+                               "arm,psci-1.0");
+  if (Retval != 0) {
+    DEBUG ((DEBUG_ERROR, "Couldn't set /psci compatible property\n"));
+    return;
+  }
+
+  Retval = fdt_setprop_string(mFdtImage, Node, "method", "smc");
+  if (Retval != 0) {
+    DEBUG ((DEBUG_ERROR, "Couldn't set /psci method property\n"));
+    return;
+  }
+
+  Root = fdt_path_offset(mFdtImage, "/cpus");
+  if (Root < 0) {
+    DEBUG ((DEBUG_ERROR, "No CPUs to update with PSCI enable-method?\n"));
+    return;
+  }
+
+  Node = fdt_first_subnode(mFdtImage, Root);
+  while (Node >= 0) {
+    if (fdt_setprop_string(mFdtImage, Node, "enable-method", "psci") != 0) {
+      DEBUG ((DEBUG_ERROR, "Failed to update enable-method for a CPU\n"));
+      return;
+    }
+
+    fdt_delprop(mFdtImage, Node, "cpu-release-addr");
+    Node = fdt_next_subnode(mFdtImage, Node);
+  }
+}
+
+STATIC
+VOID
+CleanSimpleFramebuffer (
+  VOID
+  )
+{
+  INTN Node;
+  INT32 Retval;
+
+  /*
+   * Should look for nodes by kind and remove aliases
+   * by matching against device.
+   */
+  Node = fdt_path_offset(mFdtImage, "display0");
+  if (Node < 0) {
+    return;
+  }
+
+  /*
+   * Remove bogus GPU-injected simple-framebuffer, which
+   * doesn't reflect the framebuffer built by UEFI.
+   */
+  DEBUG ((DEBUG_INFO, "Removing bogus display0\n"));
+  Retval = fdt_del_node(mFdtImage, Node);
+  if (Retval != 0) {
+    DEBUG ((DEBUG_ERROR, "Failed to remove display0\n"));
+    return;
+  }
+
+  Node =  fdt_path_offset(mFdtImage, "/aliases");
+  if (Node < 0) {
+    DEBUG ((DEBUG_ERROR, "Couldn't find /aliases to remove display0\n"));
+    return;
+  }
+
+  Retval = fdt_delprop(mFdtImage, Node, "display0");
+  if (Retval != 0) {
+    DEBUG ((DEBUG_ERROR, "Failed to remove display0 alias\n"));
+  }
+}
+
+#define MAX_CMDLINE_SIZE    512
+
+STATIC
+VOID
+UpdateBootArgs (
+  VOID
+  )
+{
+  INTN          Node;
+  INTN          Retval;
+  EFI_STATUS    Status;
+  CHAR8         *CommandLine;
+
+  //
+  // Locate the /chosen node
+  //
+  Node = fdt_path_offset(mFdtImage, "/chosen");
+  if (Node < 0) {
+    DEBUG ((DEBUG_ERROR, "%a: failed to locate /chosen node\n",
+      __FUNCTION__));
+    return;
+  }
+
+  //
+  // If /chosen/bootargs already exists, we want to add a space character
+  // before adding the firmware supplied arguments. However, the RpiFirmware
+  // protocol expects a 32-bit aligned buffer. So let's allocate 4 bytes of
+  // slack, and skip the first 3 when passing this buffer into libfdt.
+  //
+  CommandLine = AllocatePool (MAX_CMDLINE_SIZE) + 4;
+  if (!CommandLine) {
+    DEBUG ((DEBUG_ERROR, "%a: failed to allocate memory\n", __FUNCTION__));
+    return;
+  }
+
+  //
+  // Get the command line from the firmware
+  //
+  Status = mFwProtocol->GetCommandLine (MAX_CMDLINE_SIZE, CommandLine + 4);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: failed to retrieve command line\n",
+      __FUNCTION__));
+    return;
+  }
+
+  if (AsciiStrLen (CommandLine + 4) == 0) {
+    DEBUG ((DEBUG_INFO, "%a: empty command line received\n", __FUNCTION__));
+    return;
+  }
+
+  CommandLine[3] = ' ';
+
+  Retval = fdt_appendprop_string (mFdtImage, Node, "bootargs", &CommandLine[3]);
+  if (Retval != 0) {
+    DEBUG ((DEBUG_ERROR, "%a: failed to set /chosen/bootargs property (%d)\n",
+      __FUNCTION__, Retval));
+  }
+
+  DEBUG_CODE_BEGIN ();
+    CONST CHAR8    *Prop;
+    INT32         Length;
+    INT32         Index;
+
+    Node = fdt_path_offset (mFdtImage, "/chosen");
+    ASSERT (Node >= 0);
+
+    Prop = fdt_getprop (mFdtImage, Node, "bootargs", &Length);
+    ASSERT (Prop != NULL);
+
+    DEBUG ((DEBUG_INFO, "Command line set from firmware (length %d):\n'", Length));
+
+    for (Index = 0; Index < Length; Index++, Prop++) {
+      if (*Prop == '\0') {
+        continue;
+      }
+      DEBUG ((DEBUG_INFO, "%c", *Prop));
+    }
+
+    DEBUG ((DEBUG_INFO, "'\n"));
+  DEBUG_CODE_END ();
+
+  FreePool (CommandLine - 4);
+}
+
+
+/**
+  @param  ImageHandle   of the loaded driver
+  @param  SystemTable   Pointer to the System Table
+
+  @retval EFI_SUCCESS           Protocol registered
+  @retval EFI_OUT_OF_RESOURCES  Cannot allocate protocol data structure
+  @retval EFI_DEVICE_ERROR      Hardware problems
+
+**/
+EFI_STATUS
+EFIAPI
+RpiFdtDxeInitialize (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS Status;
+  VOID       *FdtImage;
+  UINTN      FdtSize;
+  INT32      Retval;
+  BOOLEAN    Internal;
+
+  Status = gBS->LocateProtocol (&gRaspberryPiFirmwareProtocolGuid, NULL,
+                  (VOID **)&mFwProtocol);
+  ASSERT_EFI_ERROR (Status);
+
+  Internal = FALSE;
+  FdtImage =  (VOID *) (UINTN) PcdGet32(PcdFdtBaseAddress);
+  Retval = fdt_check_header (FdtImage);
+  if (Retval == 0) {
+    /*
+     * Have FDT passed via config.txt.
+     */
+    FdtSize = fdt_totalsize (FdtImage);
+    DEBUG ((DEBUG_INFO, "DTB passed via config.txt of 0x%lx bytes\n", FdtSize));
+    Status = EFI_SUCCESS;
+  } else {
+    Internal = TRUE;
+    DEBUG ((DEBUG_INFO, "No/bad FDT at %p (%a), trying internal DTB...\n",
+            FdtImage, fdt_strerror (Retval)));
+    Status = GetSectionFromAnyFv (&gRaspberryPiFdtFileGuid, EFI_SECTION_RAW, 0,
+                                  &FdtImage, &FdtSize);
+    if (Status == EFI_SUCCESS) {
+      if (fdt_check_header (FdtImage) != 0) {
+        Status = EFI_INCOMPATIBLE_VERSION;
+      }
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "Failed to locate device tree: %r\n", Status));
+    return Status;
+  }
+
+  /*
+   * Probably overkill.
+   */
+  FdtSize += EFI_PAGE_SIZE * 2;
+  Status = gBS->AllocatePages (AllocateAnyPages, EfiRuntimeServicesData,
+                               EFI_SIZE_TO_PAGES(FdtSize),
+                               (EFI_PHYSICAL_ADDRESS *) &mFdtImage);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "Failed to allocate new device tree: %r\n", Status));
+    return Status;
+  }
+
+  Retval = fdt_open_into (FdtImage, mFdtImage, FdtSize);
+  ASSERT (Retval == 0);
+
+  SanitizePSCI ();
+  CleanMemoryNodes ();
+  CleanSimpleFramebuffer ();
+  UpdateMacAddress ();
+  if (Internal) {
+    /*
+     * A GPU-provided DTB already has the full command line.
+     */
+    UpdateBootArgs ();
+  }
+
+  DEBUG ((DEBUG_INFO, "Installed FDT is at %p\n", mFdtImage));
+  Status = gBS->InstallConfigurationTable (&gFdtTableGuid, mFdtImage);
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Platform/Broadcom/Bcm283x/Drivers/RpiFdtDxe/RpiFdtDxe.inf b/Platform/Broadcom/Bcm283x/Drivers/RpiFdtDxe/RpiFdtDxe.inf
new file mode 100644
index 000000000000..a79e9ddcdc8a
--- /dev/null
+++ b/Platform/Broadcom/Bcm283x/Drivers/RpiFdtDxe/RpiFdtDxe.inf
@@ -0,0 +1,53 @@
+#/** @file
+#
+#  Copyright (c) 2017, Andrei Warkentin <andrey.warkentin@gmail.com>
+#  Copyright (c) 2016, Linaro, Ltd. All rights reserved.
+#
+#  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                    = 0x00010019
+  BASE_NAME                      = RpiFdtDxe
+  FILE_GUID                      = 8505280f-109e-437e-9fe4-1aa09c7074d9
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = RpiFdtDxeInitialize
+
+[Sources]
+  RpiFdtDxe.c
+
+[Packages]
+  EmbeddedPkg/EmbeddedPkg.dec
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  Platform/Broadcom/Bcm283x/RaspberryPiPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  DxeServicesLib
+  FdtLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+
+[Guids]
+  gFdtTableGuid
+  gRaspberryPiFdtFileGuid
+
+[Protocols]
+  gRaspberryPiFirmwareProtocolGuid              ## CONSUMES
+
+[Depex]
+  gRaspberryPiFirmwareProtocolGuid
+
+[FixedPcd]
+  gRaspberryPiTokenSpaceGuid.PcdFdtBaseAddress
-- 
2.17.0.windows.1