[PATCH v5 edk2-platforms 09/22] Platform/RaspberryPi/RPi3: Add display driver

[Pete Batard <pete@akeo.ie>]

Implements the EFI_GRAPHICS_OUTPUT_PROTOCOL for the Raspberry
Pi platform, through framebuffer, including resolution querying
and screenshot support.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Pete Batard <pete@akeo.ie>
---
 Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/ComponentName.c | 222 +++++++
 Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.c    | 606 ++++++++++++++++++++
 Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.h    |  42 ++
 Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.inf  |  71 +++
 Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/Screenshot.c    | 375 ++++++++++++
 5 files changed, 1316 insertions(+)

diff --git a/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/ComponentName.c b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/ComponentName.c
new file mode 100644
index 000000000000..9a84aea511f4
--- /dev/null
+++ b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/ComponentName.c
@@ -0,0 +1,222 @@
+/** @file
+ *
+ *  Copyright (c) 2018, Andrei Warkentin <andrey.warkentin@gmail.com>
+ *  Copyright (c) 2006-2016, Intel Corporation. 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 "DisplayDxe.h"
+
+STATIC
+EFI_STATUS
+EFIAPI
+ComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+STATIC
+EFI_STATUS
+EFIAPI
+ComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL *This,
+  IN  EFI_HANDLE                  ControllerHandle,
+  IN  EFI_HANDLE                  ChildHandle,
+  IN  CHAR8                       *Language,
+  OUT CHAR16                      **ControllerName
+  );
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL gComponentName = {
+  ComponentNameGetDriverName,
+  ComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME)ComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME)ComponentNameGetControllerName,
+  "en"
+};
+
+
+STATIC EFI_UNICODE_STRING_TABLE mDriverName[] = {
+  {
+    "eng;en",
+    (CHAR16*)L"Raspberry Pi Display Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+STATIC EFI_UNICODE_STRING_TABLE mDeviceName[] = {
+  {
+    "eng;en",
+    (CHAR16*)L"Raspberry Pi Framebuffer"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+ComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mDriverName,
+           DriverName,
+           (BOOLEAN)(This == &gComponentName)
+         );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+ComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL *This,
+  IN  EFI_HANDLE                  ControllerHandle,
+  IN  EFI_HANDLE                  ChildHandle,
+  IN  CHAR8                       *Language,
+  OUT CHAR16                      **ControllerName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mDeviceName,
+           ControllerName,
+           (BOOLEAN)(This == &gComponentName)
+         );
+}
diff --git a/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.c b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.c
new file mode 100644
index 000000000000..0e99194e8576
--- /dev/null
+++ b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.c
@@ -0,0 +1,606 @@
+/** @file
+ *
+ *  Copyright (c) 2017-2018, Andrei Warkentin <andrey.warkentin@gmail.com>
+ *  Copyright (c) Microsoft Corporation. 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 <Base.h>
+#include "DisplayDxe.h"
+
+#define POS_TO_FB(posX, posY) ((UINT8*)                                 \
+                               ((UINTN)This->Mode->FrameBufferBase +    \
+                                (posY) * This->Mode->Info->PixelsPerScanLine * \
+                                PI3_BYTES_PER_PIXEL +                   \
+                                (posX) * PI3_BYTES_PER_PIXEL))
+
+STATIC
+EFI_STATUS
+EFIAPI
+DriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+STATIC
+EFI_STATUS
+EFIAPI
+DriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+STATIC
+EFI_STATUS
+EFIAPI
+DriverStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  );
+
+STATIC
+EFI_STATUS
+EFIAPI
+DisplayQueryMode (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL          *This,
+  IN  UINT32                                ModeNumber,
+  OUT UINTN                                 *SizeOfInfo,
+  OUT EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  **Info
+  );
+
+STATIC
+EFI_STATUS
+EFIAPI
+DisplaySetMode (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL *This,
+  IN  UINT32                       ModeNumber
+  );
+
+STATIC
+EFI_STATUS
+EFIAPI
+DisplayBlt (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL            *This,
+  IN  EFI_GRAPHICS_OUTPUT_BLT_PIXEL           *BltBuffer, OPTIONAL
+  IN  EFI_GRAPHICS_OUTPUT_BLT_OPERATION       BltOperation,
+  IN  UINTN                                   SourceX,
+  IN  UINTN                                   SourceY,
+  IN  UINTN                                   DestinationX,
+  IN  UINTN                                   DestinationY,
+  IN  UINTN                                   Width,
+  IN  UINTN                                   Height,
+  IN  UINTN                                   Delta         OPTIONAL
+  );
+
+STATIC EFI_DRIVER_BINDING_PROTOCOL mDriverBinding = {
+  DriverSupported,
+  DriverStart,
+  DriverStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+typedef struct {
+  VENDOR_DEVICE_PATH DisplayDevicePath;
+  EFI_DEVICE_PATH EndDevicePath;
+} DISPLAY_DEVICE_PATH;
+
+typedef struct {
+  UINT32 Width;
+  UINT32 Height;
+} GOP_MODE_DATA;
+
+STATIC UINT32 mBootWidth;
+STATIC UINT32 mBootHeight;
+STATIC EFI_HANDLE mDevice;
+STATIC RASPBERRY_PI_FIRMWARE_PROTOCOL *mFwProtocol;
+STATIC EFI_CPU_ARCH_PROTOCOL *mCpu;
+
+STATIC UINTN mLastMode;
+STATIC GOP_MODE_DATA mGopModeData[] = {
+  { 800,  600  }, /* Legacy */
+  { 640,  480  }, /* Legacy */
+  { 1024, 768  }, /* Legacy */
+  { 1280, 720  }, /* 720p */
+  { 1920, 1080 }, /* 1080p */
+  { 0,    0    }, /* Physical */
+};
+
+STATIC DISPLAY_DEVICE_PATH mDisplayProtoDevicePath =
+{
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8)(sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8)((sizeof (VENDOR_DEVICE_PATH)) >> 8),
+      }
+    },
+    EFI_CALLER_ID_GUID,
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      sizeof (EFI_DEVICE_PATH_PROTOCOL),
+      0
+    }
+  }
+};
+
+#define PI3_BITS_PER_PIXEL              (32)
+#define PI3_BYTES_PER_PIXEL             (PI3_BITS_PER_PIXEL / 8)
+
+EFI_GRAPHICS_OUTPUT_PROTOCOL gDisplayProto = {
+  DisplayQueryMode,
+  DisplaySetMode,
+  DisplayBlt,
+  NULL
+};
+
+STATIC
+EFI_STATUS
+EFIAPI
+DisplayQueryMode (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL          *This,
+  IN  UINT32                                ModeNumber,
+  OUT UINTN                                 *SizeOfInfo,
+  OUT EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  **Info
+  )
+{
+  EFI_STATUS Status;
+  GOP_MODE_DATA *Mode;
+
+  if (ModeNumber > mLastMode) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = gBS->AllocatePool (
+                  EfiBootServicesData,
+                  sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION),
+                  (VOID**)Info
+                );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Mode = &mGopModeData[ModeNumber];
+
+  *SizeOfInfo = sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION);
+  (*Info)->Version = This->Mode->Info->Version;
+  (*Info)->HorizontalResolution = Mode->Width;
+  (*Info)->VerticalResolution = Mode->Height;
+  (*Info)->PixelFormat = This->Mode->Info->PixelFormat;
+  (*Info)->PixelsPerScanLine = Mode->Width;
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+VOID
+ClearScreen (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL *This
+  )
+{
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL Fill;
+
+  Fill.Red = 0x00;
+  Fill.Green = 0x00;
+  Fill.Blue = 0x00;
+  This->Blt (This, &Fill, EfiBltVideoFill,
+          0, 0, 0, 0, This->Mode->Info->HorizontalResolution,
+          This->Mode->Info->VerticalResolution,
+          This->Mode->Info->HorizontalResolution *
+          sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+}
+
+STATIC
+EFI_STATUS
+EFIAPI
+DisplaySetMode (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL *This,
+  IN  UINT32                       ModeNumber
+  )
+{
+  UINTN FbSize;
+  UINTN FbPitch;
+  EFI_STATUS Status;
+  EFI_PHYSICAL_ADDRESS FbBase;
+  GOP_MODE_DATA *Mode = &mGopModeData[ModeNumber];
+
+  if (ModeNumber > mLastMode) {
+    return EFI_UNSUPPORTED;
+  }
+
+  DEBUG ((DEBUG_INFO, "Setting mode %u from %u: %u x %u\n",
+    ModeNumber, This->Mode->Mode, Mode->Width, Mode->Height));
+  Status = mFwProtocol->GetFB (Mode->Width, Mode->Height,
+                          PI3_BITS_PER_PIXEL, &FbBase,
+                          &FbSize, &FbPitch);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "Could not set mode %u\n", ModeNumber));
+    return EFI_DEVICE_ERROR;
+  }
+
+  DEBUG ((DEBUG_INFO, "Mode %u: %u x %u framebuffer is %u bytes at %p\n",
+    ModeNumber, Mode->Width, Mode->Height, FbSize, FbBase));
+
+  if (FbPitch / PI3_BYTES_PER_PIXEL != Mode->Width) {
+    DEBUG ((DEBUG_ERROR, "Error: Expected width %u, got width %u\n",
+      Mode->Width, FbPitch / PI3_BYTES_PER_PIXEL));
+    return EFI_DEVICE_ERROR;
+  }
+
+  /*
+   * WT, because certain OS loaders access the frame buffer directly
+   * and we don't want to see corruption due to missing WB cache
+   * maintenance. Performance with WT is good.
+   */
+  Status = mCpu->SetMemoryAttributes (mCpu, FbBase,
+                   ALIGN_VALUE (FbSize, EFI_PAGE_SIZE),
+                   EFI_MEMORY_WT);
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((DEBUG_ERROR, "Couldn't set framebuffer attributes: %r\n", Status));
+    return Status;
+  }
+
+  This->Mode->Mode = ModeNumber;
+  This->Mode->Info->Version = 0;
+  This->Mode->Info->HorizontalResolution = Mode->Width;
+  This->Mode->Info->VerticalResolution = Mode->Height;
+  /*
+   * NOTE: Windows REQUIRES BGR in 32 or 24 bit format.
+   */
+  This->Mode->Info->PixelFormat = PixelBlueGreenRedReserved8BitPerColor;
+  This->Mode->Info->PixelsPerScanLine = Mode->Width;
+  This->Mode->SizeOfInfo = sizeof (*This->Mode->Info);
+  This->Mode->FrameBufferBase = FbBase;
+  This->Mode->FrameBufferSize = FbSize;
+
+  ClearScreen (This);
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+EFIAPI
+DisplayBlt (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL      *This,
+  IN  EFI_GRAPHICS_OUTPUT_BLT_PIXEL     *BltBuffer, OPTIONAL
+  IN  EFI_GRAPHICS_OUTPUT_BLT_OPERATION BltOperation,
+  IN  UINTN                             SourceX,
+  IN  UINTN                             SourceY,
+  IN  UINTN                             DestinationX,
+  IN  UINTN                             DestinationY,
+  IN  UINTN                             Width,
+  IN  UINTN                             Height,
+  IN  UINTN                             Delta         OPTIONAL
+  )
+{
+  UINT8 *VidBuf, *BltBuf, *VidBuf1;
+  UINTN i;
+
+  switch (BltOperation) {
+  case EfiBltVideoFill:
+    BltBuf = (UINT8*)BltBuffer;
+
+    for (i = 0; i < Height; i++) {
+      VidBuf = POS_TO_FB (DestinationX, DestinationY + i);
+
+      SetMem32 (VidBuf, Width * PI3_BYTES_PER_PIXEL, *(UINT32*)BltBuf);
+    }
+    break;
+
+  case EfiBltVideoToBltBuffer:
+    if (Delta == 0) {
+      Delta = Width * PI3_BYTES_PER_PIXEL;
+    }
+
+    for (i = 0; i < Height; i++) {
+      VidBuf = POS_TO_FB (SourceX, SourceY + i);
+
+      BltBuf = (UINT8*)((UINTN)BltBuffer + (DestinationY + i) * Delta +
+        DestinationX * PI3_BYTES_PER_PIXEL);
+
+      gBS->CopyMem ((VOID*)BltBuf, (VOID*)VidBuf, PI3_BYTES_PER_PIXEL * Width);
+    }
+    break;
+
+  case EfiBltBufferToVideo:
+    if (Delta == 0) {
+      Delta = Width * PI3_BYTES_PER_PIXEL;
+    }
+
+    for (i = 0; i < Height; i++) {
+      VidBuf = POS_TO_FB (DestinationX, DestinationY + i);
+      BltBuf = (UINT8*)((UINTN)BltBuffer + (SourceY + i) * Delta +
+        SourceX * PI3_BYTES_PER_PIXEL);
+
+      gBS->CopyMem ((VOID*)VidBuf, (VOID*)BltBuf, Width * PI3_BYTES_PER_PIXEL);
+    }
+    break;
+
+  case EfiBltVideoToVideo:
+    for (i = 0; i < Height; i++) {
+      VidBuf = POS_TO_FB (SourceX, SourceY + i);
+      VidBuf1 = POS_TO_FB (DestinationX, DestinationY + i);
+
+      gBS->CopyMem ((VOID*)VidBuf1, (VOID*)VidBuf, Width * PI3_BYTES_PER_PIXEL);
+    }
+    break;
+
+  default:
+    ASSERT_EFI_ERROR (EFI_SUCCESS);
+    break;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+   Initialize the state information for the Display Dxe
+
+   @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
+DisplayDxeInitialize (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS Status;
+
+  Status = gBS->LocateProtocol (&gRaspberryPiFirmwareProtocolGuid, NULL,
+                  (VOID**)&mFwProtocol);
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->LocateProtocol (&gEfiCpuArchProtocolGuid, NULL,
+                  (VOID**)&mCpu);
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  // Query the current display resolution from mailbox
+  Status = mFwProtocol->GetFBSize (&mBootWidth, &mBootHeight);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DEBUG ((DEBUG_INFO, "Display boot mode is %u x %u\n",
+    mBootWidth, mBootHeight));
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+    &mDevice, &gEfiDevicePathProtocolGuid,
+    &mDisplayProtoDevicePath, &gEfiCallerIdGuid,
+    NULL, NULL);
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &mDriverBinding,
+             ImageHandle,
+             &gComponentName,
+             &gComponentName2
+           );
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+EFIAPI
+DriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  VOID *Temp;
+
+  if (Controller != mDevice) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (gBS->HandleProtocol (Controller, &gEfiGraphicsOutputProtocolGuid,
+             (VOID**)&Temp) == EFI_SUCCESS) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+EFIAPI
+DriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  UINTN Index;
+  EFI_STATUS Status;
+  VOID *Dummy;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiCallerIdGuid,
+                  (VOID**)&Dummy,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gDisplayProto.Mode = AllocateZeroPool (sizeof (EFI_GRAPHICS_OUTPUT_PROTOCOL_MODE));
+  if (gDisplayProto.Mode == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  gDisplayProto.Mode->Info = AllocateZeroPool (sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
+  if (gDisplayProto.Mode->Info == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+
+  if (PcdGet32 (PcdDisplayEnableVModes)) {
+    mLastMode = ARRAY_SIZE (mGopModeData) - 1;
+  } else {
+    mLastMode = 0;
+    /*
+     * mBootWidth x mBootHeight may not be sensible,
+     * so clean it up, since we won't be adding
+     * any other extra vmodes.
+     */
+    if (mBootWidth < 640 || mBootHeight < 480) {
+      mBootWidth = 640;
+      mBootHeight = 480;
+    }
+  }
+
+  mGopModeData[mLastMode].Width = mBootWidth;
+  mGopModeData[mLastMode].Height = mBootHeight;
+
+  for (Index = 0; Index <= mLastMode; Index++) {
+    UINTN FbSize;
+    UINTN FbPitch;
+    EFI_PHYSICAL_ADDRESS FbBase;
+
+    GOP_MODE_DATA *Mode = &mGopModeData[Index];
+
+    Status = mFwProtocol->GetFB (Mode->Width, Mode->Height,
+                            PI3_BITS_PER_PIXEL, &FbBase,
+                            &FbSize, &FbPitch);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    //
+    // There is no way to communicate pitch back to OS. OS and even UEFI
+    // expect a fully linear frame buffer. So the width should
+    // be based on the frame buffer's pitch value. In some cases VC
+    // firmware would allocate ao frame buffer with some padding
+    // presumably to be 8 byte align.
+    //
+    Mode->Width = FbPitch / PI3_BYTES_PER_PIXEL;
+
+    DEBUG ((DEBUG_INFO, "Mode %u: %u x %u framebuffer is %u bytes at %p\n",
+      Index, Mode->Width, Mode->Height, FbSize, FbBase));
+
+    ASSERT (FbPitch != 0);
+    ASSERT (FbBase != 0);
+    ASSERT (FbSize != 0);
+  }
+
+  // Both set the mode and initialize current mode information.
+  gDisplayProto.Mode->MaxMode = mLastMode + 1;
+  DisplaySetMode (&gDisplayProto, 0);
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+    &Controller, &gEfiGraphicsOutputProtocolGuid,
+    &gDisplayProto, NULL);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  if (PcdGet32 (PcdDisplayEnableSShot)) {
+    RegisterScreenshotHandlers ();
+  } else {
+    DEBUG ((DEBUG_INFO, "Screenshot capture disabled\n"));
+  }
+
+Done:
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "Could not start DisplayDxe: %r\n", Status));
+    if (gDisplayProto.Mode->Info != NULL) {
+      FreePool (gDisplayProto.Mode->Info);
+      gDisplayProto.Mode->Info = NULL;
+    }
+
+    if (gDisplayProto.Mode != NULL) {
+      FreePool (gDisplayProto.Mode);
+      gDisplayProto.Mode = NULL;
+    }
+
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiCallerIdGuid,
+           This->DriverBindingHandle,
+           Controller
+         );
+  }
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+EFIAPI
+DriverStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  )
+{
+  EFI_STATUS Status;
+
+  ClearScreen (&gDisplayProto);
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+    Controller, &gEfiGraphicsOutputProtocolGuid,
+    &gDisplayProto, NULL);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  FreePool (gDisplayProto.Mode->Info);
+  gDisplayProto.Mode->Info = NULL;
+  FreePool (gDisplayProto.Mode);
+  gDisplayProto.Mode = NULL;
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiCallerIdGuid,
+         This->DriverBindingHandle,
+         Controller
+       );
+
+  return Status;
+}
diff --git a/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.h b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.h
new file mode 100644
index 000000000000..48a049de31d8
--- /dev/null
+++ b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.h
@@ -0,0 +1,42 @@
+/** @file
+ *
+ *  Copyright (c) 2017-2018, Andrei Warkentin <andrey.warkentin@gmail.com>
+ *  Copyright (c) Microsoft Corporation. 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.
+ *
+ **/
+
+#ifndef _DISPLAY_H_
+#define _DISPLAY_H_
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/PcdLib.h>
+#include <Library/IoLib.h>
+#include <Library/TimerLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Protocol/Cpu.h>
+#include <Protocol/GraphicsOutput.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/RpiFirmware.h>
+
+extern EFI_GRAPHICS_OUTPUT_PROTOCOL gDisplayProto;
+extern EFI_COMPONENT_NAME_PROTOCOL  gComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gComponentName2;
+
+VOID
+RegisterScreenshotHandlers (
+  VOID
+  );
+
+#endif /* _DISPLAY_H_ */
diff --git a/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.inf b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.inf
new file mode 100644
index 000000000000..2e716ff779b5
--- /dev/null
+++ b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/DisplayDxe.inf
@@ -0,0 +1,71 @@
+#/** @file
+#
+#  Component description file for Graphics Output module
+#
+#  Copyright (c) 2017, Andrei Warkentin <andrey.warkentin@gmail.com>
+#  Copyright (c) Microsoft Corporation. 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                    = 0x0001001A
+  BASE_NAME                      = DisplayDxe
+  FILE_GUID                      = c5deae31-fad2-4030-841b-cfc9644d2c5b
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = DisplayDxeInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  DRIVER_BINDING                =  gGraphicsConsoleDriverBinding
+#  COMPONENT_NAME                =  gGraphicsConsoleComponentName
+#  COMPONENT_NAME2               =  gGraphicsConsoleComponentName2
+#
+
+[Sources]
+  DisplayDxe.c
+  Screenshot.c
+  ComponentName.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  ArmPkg/ArmPkg.dec
+  Platform/RaspberryPi/RPi3/RPi3.dec
+
+[LibraryClasses]
+  BaseLib
+  UefiLib
+  MemoryAllocationLib
+  UefiDriverEntryPoint
+  IoLib
+  TimerLib
+  BmpSupportLib
+  UefiRuntimeServicesTableLib
+
+[Protocols]
+  gEfiLoadedImageProtocolGuid
+  gEfiDevicePathProtocolGuid
+  gEfiGraphicsOutputProtocolGuid
+  gEfiCpuArchProtocolGuid
+  gEfiSimpleFileSystemProtocolGuid
+  gEfiSimpleTextInputExProtocolGuid
+  gRaspberryPiFirmwareProtocolGuid
+
+[Pcd]
+  gRaspberryPiTokenSpaceGuid.PcdDisplayEnableVModes
+  gRaspberryPiTokenSpaceGuid.PcdDisplayEnableSShot
+
+[Guids]
+
+[Depex]
+  gEfiCpuArchProtocolGuid AND gRaspberryPiFirmwareProtocolGuid
diff --git a/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/Screenshot.c b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/Screenshot.c
new file mode 100644
index 000000000000..a5d32d59c38a
--- /dev/null
+++ b/Platform/RaspberryPi/RPi3/Drivers/DisplayDxe/Screenshot.c
@@ -0,0 +1,375 @@
+/** @file
+ *
+ *  Copyright (c) 2006 - 2016, Intel Corporation. All rights reserved.
+ *  Copyright (c) 2018, Andrei Warkentin <andrey.warkentin@gmail.com>
+ *
+ *  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.
+ *
+ **/
+
+/*
+ * Loosely based on CrScreenShotDxe (https://github.com/LongSoft/CrScreenshotDxe).
+ *
+ * Copyright (c) 2016, Nikolaj Schlej, All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * - Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "DisplayDxe.h"
+#include <Protocol/SimpleFileSystem.h>
+#include <Library/PrintLib.h>
+#include <Library/BmpSupportLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+
+/*
+ * ShowStatus defs.
+ */
+#define STATUS_SQUARE_SIDE 5
+#define STATUS_YELLOW 0xff, 0xff, 0x00
+#define STATUS_GREEN  0x00, 0xff, 0x00
+#define STATUS_BLUE   0x00, 0x00, 0xff
+#define STATUS_RED    0xff, 0x00, 0x00
+
+EFI_STATUS
+ShowStatus (
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL *GraphicsOutput,
+  IN UINT8 Red,
+  IN UINT8 Green,
+  IN UINT8 Blue
+  )
+{
+  UINTN Index;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL Square[STATUS_SQUARE_SIDE * STATUS_SQUARE_SIDE];
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL Backup[STATUS_SQUARE_SIDE * STATUS_SQUARE_SIDE];
+
+  for (Index = 0; Index < STATUS_SQUARE_SIDE * STATUS_SQUARE_SIDE; Index++) {
+    Square[Index].Blue = Blue;
+    Square[Index].Green = Green;
+    Square[Index].Red = Red;
+    Square[Index].Reserved = 0x00;
+  }
+
+  // Backup current image.
+  GraphicsOutput->Blt (GraphicsOutput, Backup,
+                    EfiBltVideoToBltBuffer, 0, 0, 0, 0,
+                    STATUS_SQUARE_SIDE, STATUS_SQUARE_SIDE, 0);
+
+  // Draw the status square.
+  GraphicsOutput->Blt (GraphicsOutput, Square,
+                    EfiBltBufferToVideo, 0, 0, 0, 0,
+                    STATUS_SQUARE_SIDE, STATUS_SQUARE_SIDE, 0);
+
+  // Wait 500ms.
+  gBS->Stall (500 * 1000);
+
+  // Restore the backup.
+  GraphicsOutput->Blt (GraphicsOutput, Backup,
+                    EfiBltBufferToVideo, 0, 0, 0, 0,
+                    STATUS_SQUARE_SIDE, STATUS_SQUARE_SIDE, 0);
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+FindWritableFs (
+  OUT EFI_FILE_PROTOCOL **WritableFs
+  )
+{
+  EFI_FILE_PROTOCOL *Fs = NULL;
+  EFI_HANDLE *HandleBuffer = NULL;
+  UINTN      HandleCount;
+  UINTN      Index;
+
+  EFI_STATUS Status = gBS->LocateHandleBuffer (ByProtocol,
+                             &gEfiSimpleFileSystemProtocolGuid,
+                             NULL, &HandleCount, &HandleBuffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  for (Index = 0; Index < HandleCount; Index++) {
+    EFI_SIMPLE_FILE_SYSTEM_PROTOCOL *SimpleFs = NULL;
+    EFI_FILE_PROTOCOL *File = NULL;
+
+    Status = gBS->HandleProtocol (HandleBuffer[Index],
+                    &gEfiSimpleFileSystemProtocolGuid,
+                    (VOID**)&SimpleFs);
+    if (EFI_ERROR (Status)) {
+      ASSERT_EFI_ERROR (Status);
+      /*
+       * Not supposed to happen.
+       */
+      continue;
+    }
+
+    Status = SimpleFs->OpenVolume (SimpleFs, &Fs);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "%a OpenVolume[%u] returned %r\n", __FUNCTION__, Index, Status));
+      continue;
+    }
+
+    Status = Fs->Open (Fs, &File, L"--------.---",
+                   EFI_FILE_MODE_CREATE | EFI_FILE_MODE_READ |
+                   EFI_FILE_MODE_WRITE, 0);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "%a Open[%u] returned %r\n", __FUNCTION__, Index, Status));
+      continue;
+    }
+
+    /*
+     * Okay, we have a writable filesystem!
+     */
+    Fs->Delete (File);
+    *WritableFs = Fs;
+    Status = EFI_SUCCESS;
+    break;
+  }
+
+  if (HandleBuffer) {
+    FreePool (HandleBuffer);
+  }
+
+  return Status;
+}
+
+STATIC
+VOID
+TakeScreenshot (
+  VOID
+  )
+{
+  VOID *BmpImage = NULL;
+  EFI_FILE_PROTOCOL *Fs = NULL;
+  EFI_FILE_PROTOCOL *File = NULL;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL *GraphicsOutput = &gDisplayProto;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL *Image = NULL;
+  EFI_STATUS Status;
+  CHAR16 FileName[8 + 1 + 3 + 1];
+  UINT32 ScreenWidth;
+  UINT32 ScreenHeight;
+  UINTN ImageSize;
+  UINTN BmpSize;
+  UINTN Index;
+  EFI_TIME Time;
+
+  Status = FindWritableFs (&Fs);
+  if (EFI_ERROR (Status)) {
+    ShowStatus (GraphicsOutput, STATUS_YELLOW);
+  }
+
+  ScreenWidth = GraphicsOutput->Mode->Info->HorizontalResolution;
+  ScreenHeight = GraphicsOutput->Mode->Info->VerticalResolution;
+  ImageSize = ScreenWidth * ScreenHeight;
+
+  Status = gRT->GetTime (&Time, NULL);
+  if (!EFI_ERROR (Status)) {
+    UnicodeSPrint (FileName, sizeof (FileName), L"%02d%02d%02d%02d.bmp",
+      Time.Day, Time.Hour, Time.Minute, Time.Second);
+  } else {
+    UnicodeSPrint (FileName, sizeof (FileName), L"scrnshot.bmp");
+  }
+
+  Image = AllocatePool (ImageSize * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+  if (Image == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    ShowStatus (GraphicsOutput, STATUS_RED);
+    goto Done;
+  }
+
+  Status = GraphicsOutput->Blt (GraphicsOutput, Image,
+                             EfiBltVideoToBltBuffer, 0, 0, 0, 0,
+                             ScreenWidth, ScreenHeight, 0);
+  if (EFI_ERROR (Status)) {
+    ShowStatus (GraphicsOutput, STATUS_RED);
+    goto Done;
+  }
+
+  for (Index = 0; Index < ImageSize; Index++) {
+    if (Image[Index].Red != 0x00 ||
+        Image[Index].Green != 0x00 ||
+        Image[Index].Blue != 0x00) {
+      break;
+    }
+  }
+
+  if (Index == ImageSize) {
+    ShowStatus (GraphicsOutput, STATUS_BLUE);
+    goto Done;
+  }
+
+  Status = TranslateGopBltToBmp (Image, ScreenHeight, ScreenWidth,
+             &BmpImage, (UINT32*)&BmpSize);
+  if (EFI_ERROR (Status)) {
+    ShowStatus (GraphicsOutput, STATUS_RED);
+    goto Done;
+  }
+
+  Status = Fs->Open (Fs, &File, FileName, EFI_FILE_MODE_CREATE |
+                 EFI_FILE_MODE_READ | EFI_FILE_MODE_WRITE, 0);
+  if (EFI_ERROR (Status)) {
+    ShowStatus (GraphicsOutput, STATUS_RED);
+    goto Done;
+  }
+
+  Status = File->Write (File, &BmpSize, BmpImage);
+  File->Close (File);
+  if (EFI_ERROR (Status)) {
+    ShowStatus (GraphicsOutput, STATUS_RED);
+    goto Done;
+  }
+
+  ShowStatus (GraphicsOutput, STATUS_GREEN);
+
+Done:
+  if (BmpImage != NULL) {
+    FreePool (BmpImage);
+  }
+
+  if (Image != NULL) {
+    FreePool (Image);
+  }
+}
+
+STATIC
+EFI_STATUS
+EFIAPI
+ScreenshotKeyHandler (
+  IN EFI_KEY_DATA *KeyData
+  )
+{
+  TakeScreenshot ();
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+ProcessScreenshotHandler (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *SimpleTextInEx
+  )
+{
+  EFI_STATUS Status;
+  EFI_HANDLE Handle;
+  EFI_KEY_DATA ScreenshotKey;
+
+  /*
+   * LCtrl+LAlt+F12
+   */
+  ScreenshotKey.Key.ScanCode = SCAN_F12;
+  ScreenshotKey.Key.UnicodeChar = 0;
+  ScreenshotKey.KeyState.KeyShiftState = EFI_SHIFT_STATE_VALID |
+    EFI_LEFT_CONTROL_PRESSED | EFI_LEFT_ALT_PRESSED;
+  ScreenshotKey.KeyState.KeyToggleState = 0;
+
+  Status = SimpleTextInEx->RegisterKeyNotify (
+                             SimpleTextInEx,
+                             &ScreenshotKey,
+                             ScreenshotKeyHandler,
+                             &Handle
+                           );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: couldn't register key notification: %r\n", __FUNCTION__, Status));
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+VOID
+ProcessScreenshotHandlers (
+  VOID
+  )
+{
+  UINTN Index;
+  EFI_STATUS Status;
+  UINTN HandleCount;
+  EFI_HANDLE *HandleBuffer;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *SimpleTextInEx;
+
+  Status = gBS->LocateHandleBuffer (ByProtocol, &gEfiSimpleTextInputExProtocolGuid,
+                  NULL, &HandleCount, &HandleBuffer);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = gBS->HandleProtocol (HandleBuffer[Index],
+                    &gEfiSimpleTextInputExProtocolGuid,
+                    (VOID**)&SimpleTextInEx);
+    if (EFI_ERROR (Status)) {
+      ASSERT_EFI_ERROR (Status);
+      /*
+       * Not supposed to happen.
+       */
+      continue;
+    }
+
+    Status = ProcessScreenshotHandler (SimpleTextInEx);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+  }
+}
+
+STATIC
+VOID
+EFIAPI
+OnTextInExInstall (
+  IN EFI_EVENT Event,
+  IN VOID *Context
+  )
+{
+  ProcessScreenshotHandlers ();
+}
+
+VOID
+RegisterScreenshotHandlers (
+  VOID
+)
+{
+  EFI_STATUS Status;
+  EFI_EVENT TextInExInstallEvent;
+  VOID *TextInExInstallRegistration;
+
+  ProcessScreenshotHandlers ();
+
+  Status = gBS->CreateEvent (EVT_NOTIFY_SIGNAL, TPL_CALLBACK,
+                  OnTextInExInstall, NULL,
+                  &TextInExInstallEvent);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: couldn't create protocol install event: %r\n", __FUNCTION__, Status));
+    return;
+  }
+
+  Status = gBS->RegisterProtocolNotify (&gEfiSimpleTextInputExProtocolGuid,
+                  TextInExInstallEvent,
+                  &TextInExInstallRegistration);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: couldn't register protocol install notify: %r\n", __FUNCTION__, Status));
+    return;
+  }
+}
-- 
2.17.0.windows.1