ARM Packages: Fixed missing braces (the warning was disabled by GCC)
[mirror_edk2.git] / Omap35xxPkg / LcdGraphicsOutputDxe / LcdGraphicsOutputDxe.c
index 6c20575..f7d2478 100644 (file)
-/** @file
-
- Copyright (c) 2011, ARM 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 "LcdGraphicsOutputDxe.h"
-
-BOOLEAN mDisplayInitialized = FALSE;
-
-LCD_MODE LcdModes[] = {
-  {
-    0, 640, 480,
-    9, 4,
-    96, 16, 48,
-    2, 10, 33
-  },
-  {
-    1, 800, 600,
-    11, 2,
-    120, 56, 64,
-    5, 37, 22
-  },
-  {
-    2, 1024, 768,
-    6, 2,
-    96, 16, 48,
-    2, 10, 33
-  },
-};
-
-LCD_INSTANCE mLcdTemplate = {
-  LCD_INSTANCE_SIGNATURE,
-  NULL, // Handle
-  { // ModeInfo
-    0, // Version
-    0, // HorizontalResolution
-    0, // VerticalResolution
-    PixelBltOnly, // PixelFormat
-    {
-      0xF800, //RedMask;
-      0x7E0, //GreenMask;
-      0x1F, //BlueMask;
-      0x0//ReservedMask
-    }, // PixelInformation
-    0, // PixelsPerScanLine
-  },
-  { // Mode
-    3, // MaxMode;
-    0, // Mode;
-    NULL, // Info;
-    0, // SizeOfInfo;
-    0, // FrameBufferBase;
-    0 // FrameBufferSize;
-  },
-  { // Gop
-    LcdGraphicsQueryMode,  // QueryMode
-    LcdGraphicsSetMode,    // SetMode
-    LcdGraphicsBlt,        // Blt
-    NULL                     // *Mode
-  },
-  { // DevicePath
-    {
-      {
-        HARDWARE_DEVICE_PATH, HW_VENDOR_DP,
-        (UINT8) (sizeof(VENDOR_DEVICE_PATH)),
-        (UINT8) ((sizeof(VENDOR_DEVICE_PATH)) >> 8),
-      },
-      // Hardware Device Path for Lcd
-      EFI_CALLER_ID_GUID // Use the driver's GUID
-    },
-
-    {
-      END_DEVICE_PATH_TYPE,
-      END_ENTIRE_DEVICE_PATH_SUBTYPE,
-      sizeof(EFI_DEVICE_PATH_PROTOCOL),
-      0
-    }
-  }
-};
-
-EFI_STATUS
-LcdInstanceContructor (
-  OUT LCD_INSTANCE** NewInstance
-  )
-{
-  LCD_INSTANCE* Instance;
-
-  Instance = AllocateCopyPool (sizeof(LCD_INSTANCE), &mLcdTemplate);
-  if (Instance == NULL) {
-    return EFI_OUT_OF_RESOURCES;
-  }
-
-  Instance->Gop.Mode          = &Instance->Mode;
-  Instance->Mode.Info         = &Instance->ModeInfo;
-
-  *NewInstance = Instance;
-  return EFI_SUCCESS;
-}
-
-EFI_STATUS
-LcdPlatformGetVram (
-  OUT EFI_PHYSICAL_ADDRESS*  VramBaseAddress,
-  OUT UINTN*                 VramSize
-  )
-{
-  EFI_STATUS             Status;
-  EFI_CPU_ARCH_PROTOCOL  *Cpu;
-  UINTN                  MaxSize;
-
-  MaxSize = 0x500000;
-  *VramSize = MaxSize;
-
-  // Allocate VRAM from DRAM
-  Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData, EFI_SIZE_TO_PAGES((MaxSize)), VramBaseAddress);
-  if (EFI_ERROR(Status)) {
-    return Status;
-  }
-
-  // Ensure the Cpu architectural protocol is already installed
-  Status = gBS->LocateProtocol (&gEfiCpuArchProtocolGuid, NULL, (VOID **)&Cpu);
-  ASSERT_EFI_ERROR(Status);
-
-  // Mark the VRAM as un-cacheable. The VRAM is inside the DRAM, which is cacheable.
-  Status = Cpu->SetMemoryAttributes (Cpu, *VramBaseAddress, *VramSize, EFI_MEMORY_UC);
-  if (EFI_ERROR(Status)) {
-    gBS->FreePool (VramBaseAddress);
-    return Status;
-  }
-
-  return EFI_SUCCESS;
-}
-
-EFI_STATUS
-DssSetMode (
-  UINT32 VramBaseAddress,
-  UINTN  ModeNumber
-  )
-{
-  // Make sure the interface clock is running
-  MmioWrite32 (CM_ICLKEN_DSS, EN_DSS);  
-
-  // Stop the functional clocks
-  MmioAnd32 (CM_FCLKEN_DSS, ~(EN_DSS1 | EN_DSS2 | EN_TV));
-
-  // Program the DSS clock divisor
-  MmioWrite32 (CM_CLKSEL_DSS, 0x1000 | (LcdModes[ModeNumber].DssDivisor));
-  
-  // Start the functional clocks
-  MmioOr32 (CM_FCLKEN_DSS, (EN_DSS1 | EN_DSS2 | EN_TV));
-
-  // Wait for DSS to stabilize
-  gBS->Stall(1);
-
-  // Reset the subsystem
-  MmioWrite32(DSS_SYSCONFIG, DSS_SOFTRESET);
-  while (!(MmioRead32 (DSS_SYSSTATUS) & DSS_RESETDONE));
-  
-  // Configure LCD parameters
-  MmioWrite32 (DISPC_SIZE_LCD,
-               ((LcdModes[ModeNumber].HorizontalResolution - 1) 
-               | ((LcdModes[ModeNumber].VerticalResolution - 1) << 16))
-              );
-  MmioWrite32 (DISPC_TIMING_H,
-               ( (LcdModes[ModeNumber].HSync - 1)
-               | ((LcdModes[ModeNumber].HFrontPorch - 1) << 8)
-               | ((LcdModes[ModeNumber].HBackPorch - 1) << 20))
-              );
-  MmioWrite32 (DISPC_TIMING_V,
-               ( (LcdModes[ModeNumber].VSync - 1)
-               | ((LcdModes[ModeNumber].VFrontPorch - 1) << 8)
-               | ((LcdModes[ModeNumber].VBackPorch - 1) << 20))
-              );
-
-  // Set the framebuffer to only load frames (no gamma tables)
-  MmioAnd32 (DISPC_CONFIG, CLEARLOADMODE);
-  MmioOr32  (DISPC_CONFIG, LOAD_FRAME_ONLY);
-
-  // Divisor for the pixel clock
-  MmioWrite32(DISPC_DIVISOR, ((1 << 16) | LcdModes[ModeNumber].DispcDivisor) );
-
-  // Set up the graphics layer
-  MmioWrite32 (DISPC_GFX_PRELD, 0x2D8);
-  MmioWrite32 (DISPC_GFX_BA0, VramBaseAddress);
-  MmioWrite32 (DISPC_GFX_SIZE,
-               ((LcdModes[ModeNumber].HorizontalResolution - 1) 
-               | ((LcdModes[ModeNumber].VerticalResolution - 1) << 16))
-              );
-
-  MmioWrite32(DISPC_GFX_ATTR, (GFXENABLE | RGB16 | BURSTSIZE16));
-
-  // Start it all
-  MmioOr32 (DISPC_CONTROL, (LCDENABLE | ACTIVEMATRIX | DATALINES24 | BYPASS_MODE | LCDENABLESIGNAL));
-  MmioOr32 (DISPC_CONTROL, GOLCD);
-
-  return EFI_SUCCESS;
-}
-
-EFI_STATUS
-HwInitializeDisplay (
-  UINTN VramBaseAddress,
-  UINTN VramSize
-  )
-{
-  EFI_STATUS    Status;
-  UINT8         Data;
-  EFI_TPL       OldTpl;
-  EMBEDDED_EXTERNAL_DEVICE   *gTPS65950;
-
-  // Enable power lines used by TFP410
-  Status = gBS->LocateProtocol (&gEmbeddedExternalDeviceProtocolGuid, NULL, (VOID **)&gTPS65950);
-  ASSERT_EFI_ERROR (Status);  
-
-  OldTpl = gBS->RaiseTPL(TPL_NOTIFY);
-  Data = VAUX_DEV_GRP_P1;
-  Status = gTPS65950->Write (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID4, VPLL2_DEV_GRP), 1, &Data);
-  ASSERT_EFI_ERROR(Status);
-
-  Data = VAUX_DEDICATED_18V;
-  Status = gTPS65950->Write (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID4, VPLL2_DEDICATED), 1, &Data);
-  ASSERT_EFI_ERROR (Status);  
-
-  // Power up TFP410 (set GPIO2 on TPS - for BeagleBoard-xM)
-  Status = gTPS65950->Read (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID2, GPIODATADIR1), 1, &Data);
-  ASSERT_EFI_ERROR (Status);
-  Data |= BIT2;
-  Status = gTPS65950->Write (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID2, GPIODATADIR1), 1, &Data);
-  ASSERT_EFI_ERROR (Status);
-
-  Data = BIT2;
-  Status = gTPS65950->Write (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID2, SETGPIODATAOUT1), 1, &Data);
-  ASSERT_EFI_ERROR (Status);
-
-  gBS->RestoreTPL(OldTpl);
-
-  // Power up TFP410 (set GPIO 170 - for older BeagleBoards)
-  MmioAnd32 (GPIO6_BASE + GPIO_OE, ~BIT10);
-  MmioOr32  (GPIO6_BASE + GPIO_SETDATAOUT, BIT10);  
-
-  return EFI_SUCCESS;
-}
-
-EFI_STATUS
-InitializeDisplay (
-  IN LCD_INSTANCE* Instance
-  )
-{
-  EFI_STATUS           Status; 
-  UINTN                VramSize;
-  EFI_PHYSICAL_ADDRESS VramBaseAddress;
-
-  Status = LcdPlatformGetVram (&VramBaseAddress, &VramSize);
-  if (EFI_ERROR (Status)) {
-    return Status;
-  }
-
-  Instance->Mode.FrameBufferBase = VramBaseAddress;
-  Instance->Mode.FrameBufferSize = VramSize;
-
-  Status = HwInitializeDisplay((UINTN)VramBaseAddress, VramSize);
-  if (!EFI_ERROR (Status)) {
-    mDisplayInitialized = TRUE;
-  }
-
-  return Status;
-}
-
-EFI_STATUS
-EFIAPI
-LcdGraphicsQueryMode (
-  IN EFI_GRAPHICS_OUTPUT_PROTOCOL            *This,
-       IN UINT32                                  ModeNumber,
-       OUT UINTN                                  *SizeOfInfo,
-       OUT EFI_GRAPHICS_OUTPUT_MODE_INFORMATION   **Info
-       )
-{
-       LCD_INSTANCE  *Instance;
-
-       Instance = LCD_INSTANCE_FROM_GOP_THIS(This);
-
-  if (!mDisplayInitialized) {
-    InitializeDisplay (Instance);
-  }
-
-  // Error checking
-       if ( (This == NULL) || (Info == NULL) || (SizeOfInfo == NULL) || (ModeNumber >= This->Mode->MaxMode) ) {
-         DEBUG((DEBUG_ERROR, "LcdGraphicsQueryMode: ERROR - For mode number %d : Invalid Parameter.\n", ModeNumber ));
-               return EFI_INVALID_PARAMETER;
-       }
-
-  *Info = AllocateCopyPool(sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION), &Instance->ModeInfo);
-  if (*Info == NULL) {
-    return EFI_OUT_OF_RESOURCES;
-  }
-
-  *SizeOfInfo = sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION);
-
-  (*Info)->Version = 0;
-  (*Info)->HorizontalResolution = LcdModes[ModeNumber].HorizontalResolution;
-  (*Info)->VerticalResolution = LcdModes[ModeNumber].VerticalResolution;
-  (*Info)->PixelFormat = PixelBltOnly;
-
-  return EFI_SUCCESS;
-}
-
-EFI_STATUS
-EFIAPI
-LcdGraphicsSetMode (
-  IN EFI_GRAPHICS_OUTPUT_PROTOCOL   *This,
-       IN UINT32                         ModeNumber
-       )
-{
-       LCD_INSTANCE  *Instance;
-
-       Instance = LCD_INSTANCE_FROM_GOP_THIS(This);
-
-  if (ModeNumber >= Instance->Mode.MaxMode) {
-    return EFI_UNSUPPORTED;
-  }
-
-  if (!mDisplayInitialized) {
-    InitializeDisplay (Instance);
-  }
-
-  DssSetMode((UINT32)Instance->Mode.FrameBufferBase, ModeNumber);
-
-  Instance->Mode.Mode = ModeNumber;
-  Instance->ModeInfo.HorizontalResolution = LcdModes[ModeNumber].HorizontalResolution;
-  Instance->ModeInfo.VerticalResolution = LcdModes[ModeNumber].VerticalResolution;
-
-  return EFI_SUCCESS;
-}
-
-EFI_STATUS
-EFIAPI
-LcdGraphicsOutputDxeInitialize (
-  IN EFI_HANDLE         ImageHandle,
-  IN EFI_SYSTEM_TABLE   *SystemTable
-  )
-{
-  EFI_STATUS Status = EFI_SUCCESS;
-  LCD_INSTANCE* Instance;
-
-  Status = LcdInstanceContructor (&Instance);
-  if (EFI_ERROR(Status)) {
-    goto EXIT;
-  }
-
-       // Install the Graphics Output Protocol and the Device Path
-       Status = gBS->InstallMultipleProtocolInterfaces(
-                       &Instance->Handle,
-                       &gEfiGraphicsOutputProtocolGuid, &Instance->Gop,
-                       &gEfiDevicePathProtocolGuid,     &Instance->DevicePath,
-                       NULL
-          );
-
-       if (EFI_ERROR(Status)) {
-         DEBUG((DEBUG_ERROR, "GraphicsOutputDxeInitialize: Can not install the protocol. Exit Status=%r\n", Status));
-               goto EXIT;
-       }
-
-  // Register for an ExitBootServicesEvent
-       // When ExitBootServices starts, this function here will make sure that the graphics driver will shut down properly,
-       // i.e. it will free up all allocated memory and perform any necessary hardware re-configuration.
-       /*Status = gBS->CreateEvent (
-           EVT_SIGNAL_EXIT_BOOT_SERVICES,
-           TPL_NOTIFY,
-           LcdGraphicsExitBootServicesEvent, NULL,
-                       &Instance->ExitBootServicesEvent
-                       );
-
-       if (EFI_ERROR(Status)) {
-         DEBUG((DEBUG_ERROR, "GraphicsOutputDxeInitialize: Can not install the ExitBootServicesEvent handler. Exit Status=%r\n", Status));
-               goto EXIT_ERROR_UNINSTALL_PROTOCOL;
-       }*/
-
-       // To get here, everything must be fine, so just exit
-       goto EXIT;
-
-//EXIT_ERROR_UNINSTALL_PROTOCOL:
-       /* The following function could return an error message,
-        * however, to get here something must have gone wrong already,
-        * so preserve the original error, i.e. don't change
-        * the Status variable, even it fails to uninstall the protocol.
-        */
-/*     gBS->UninstallMultipleProtocolInterfaces (
-           Instance->Handle,
-           &gEfiGraphicsOutputProtocolGuid, &Instance->Gop, // Uninstall Graphics Output protocol
-           &gEfiDevicePathProtocolGuid,     &Instance->DevicePath,     // Uninstall device path
-           NULL
-           );*/
-
-EXIT:
-       return Status;
-
-}
+/** @file\r
+\r
+ Copyright (c) 2011-2014, ARM Ltd. All rights reserved.<BR>\r
+ This program and the accompanying materials\r
+ are 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 "LcdGraphicsOutputDxe.h"\r
+\r
+BOOLEAN mDisplayInitialized = FALSE;\r
+\r
+LCD_MODE LcdModes[] = {\r
+  {\r
+    0, 640, 480,\r
+    9, 4,\r
+    96, 16, 48,\r
+    2, 10, 33\r
+  },\r
+  {\r
+    1, 800, 600,\r
+    11, 2,\r
+    120, 56, 64,\r
+    5, 37, 22\r
+  },\r
+  {\r
+    2, 1024, 768,\r
+    6, 2,\r
+    96, 16, 48,\r
+    2, 10, 33\r
+  },\r
+};\r
+\r
+LCD_INSTANCE mLcdTemplate = {\r
+  LCD_INSTANCE_SIGNATURE,\r
+  NULL, // Handle\r
+  { // ModeInfo\r
+    0, // Version\r
+    0, // HorizontalResolution\r
+    0, // VerticalResolution\r
+    PixelBltOnly, // PixelFormat\r
+    {\r
+      0xF800, //RedMask;\r
+      0x7E0, //GreenMask;\r
+      0x1F, //BlueMask;\r
+      0x0//ReservedMask\r
+    }, // PixelInformation\r
+    0, // PixelsPerScanLine\r
+  },\r
+  { // Mode\r
+    3, // MaxMode;\r
+    0, // Mode;\r
+    NULL, // Info;\r
+    0, // SizeOfInfo;\r
+    0, // FrameBufferBase;\r
+    0 // FrameBufferSize;\r
+  },\r
+  { // Gop\r
+    LcdGraphicsQueryMode,  // QueryMode\r
+    LcdGraphicsSetMode,    // SetMode\r
+    LcdGraphicsBlt,        // Blt\r
+    NULL                     // *Mode\r
+  },\r
+  { // DevicePath\r
+    {\r
+      {\r
+        HARDWARE_DEVICE_PATH, HW_VENDOR_DP,\r
+        { (UINT8) (sizeof(VENDOR_DEVICE_PATH)), (UINT8) ((sizeof(VENDOR_DEVICE_PATH)) >> 8) },\r
+      },\r
+      // Hardware Device Path for Lcd\r
+      EFI_CALLER_ID_GUID // Use the driver's GUID\r
+    },\r
+    {\r
+      END_DEVICE_PATH_TYPE,\r
+      END_ENTIRE_DEVICE_PATH_SUBTYPE,\r
+      { sizeof(EFI_DEVICE_PATH_PROTOCOL), 0}\r
+    }\r
+  }\r
+};\r
+\r
+EFI_STATUS\r
+LcdInstanceContructor (\r
+  OUT LCD_INSTANCE** NewInstance\r
+  )\r
+{\r
+  LCD_INSTANCE* Instance;\r
+\r
+  Instance = AllocateCopyPool (sizeof(LCD_INSTANCE), &mLcdTemplate);\r
+  if (Instance == NULL) {\r
+    return EFI_OUT_OF_RESOURCES;\r
+  }\r
+\r
+  Instance->Gop.Mode          = &Instance->Mode;\r
+  Instance->Mode.Info         = &Instance->ModeInfo;\r
+\r
+  *NewInstance = Instance;\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+EFI_STATUS\r
+LcdPlatformGetVram (\r
+  OUT EFI_PHYSICAL_ADDRESS*  VramBaseAddress,\r
+  OUT UINTN*                 VramSize\r
+  )\r
+{\r
+  EFI_STATUS             Status;\r
+  EFI_CPU_ARCH_PROTOCOL  *Cpu;\r
+  UINTN                  MaxSize;\r
+\r
+  MaxSize = 0x500000;\r
+  *VramSize = MaxSize;\r
+\r
+  // Allocate VRAM from DRAM\r
+  Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData, EFI_SIZE_TO_PAGES((MaxSize)), VramBaseAddress);\r
+  if (EFI_ERROR(Status)) {\r
+    return Status;\r
+  }\r
+\r
+  // Ensure the Cpu architectural protocol is already installed\r
+  Status = gBS->LocateProtocol (&gEfiCpuArchProtocolGuid, NULL, (VOID **)&Cpu);\r
+  ASSERT_EFI_ERROR(Status);\r
+\r
+  // Mark the VRAM as un-cacheable. The VRAM is inside the DRAM, which is cacheable.\r
+  Status = Cpu->SetMemoryAttributes (Cpu, *VramBaseAddress, *VramSize, EFI_MEMORY_UC);\r
+  if (EFI_ERROR(Status)) {\r
+    gBS->FreePool (VramBaseAddress);\r
+    return Status;\r
+  }\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+EFI_STATUS\r
+DssSetMode (\r
+  UINT32 VramBaseAddress,\r
+  UINTN  ModeNumber\r
+  )\r
+{\r
+  // Make sure the interface clock is running\r
+  MmioWrite32 (CM_ICLKEN_DSS, EN_DSS);  \r
+\r
+  // Stop the functional clocks\r
+  MmioAnd32 (CM_FCLKEN_DSS, ~(EN_DSS1 | EN_DSS2 | EN_TV));\r
+\r
+  // Program the DSS clock divisor\r
+  MmioWrite32 (CM_CLKSEL_DSS, 0x1000 | (LcdModes[ModeNumber].DssDivisor));\r
+  \r
+  // Start the functional clocks\r
+  MmioOr32 (CM_FCLKEN_DSS, (EN_DSS1 | EN_DSS2 | EN_TV));\r
+\r
+  // Wait for DSS to stabilize\r
+  gBS->Stall(1);\r
+\r
+  // Reset the subsystem\r
+  MmioWrite32(DSS_SYSCONFIG, DSS_SOFTRESET);\r
+  while (!(MmioRead32 (DSS_SYSSTATUS) & DSS_RESETDONE));\r
+  \r
+  // Configure LCD parameters\r
+  MmioWrite32 (DISPC_SIZE_LCD,\r
+               ((LcdModes[ModeNumber].HorizontalResolution - 1) \r
+               | ((LcdModes[ModeNumber].VerticalResolution - 1) << 16))\r
+              );\r
+  MmioWrite32 (DISPC_TIMING_H,\r
+               ( (LcdModes[ModeNumber].HSync - 1)\r
+               | ((LcdModes[ModeNumber].HFrontPorch - 1) << 8)\r
+               | ((LcdModes[ModeNumber].HBackPorch - 1) << 20))\r
+              );\r
+  MmioWrite32 (DISPC_TIMING_V,\r
+               ( (LcdModes[ModeNumber].VSync - 1)\r
+               | ((LcdModes[ModeNumber].VFrontPorch - 1) << 8)\r
+               | ((LcdModes[ModeNumber].VBackPorch - 1) << 20))\r
+              );\r
+\r
+  // Set the framebuffer to only load frames (no gamma tables)\r
+  MmioAnd32 (DISPC_CONFIG, CLEARLOADMODE);\r
+  MmioOr32  (DISPC_CONFIG, LOAD_FRAME_ONLY);\r
+\r
+  // Divisor for the pixel clock\r
+  MmioWrite32(DISPC_DIVISOR, ((1 << 16) | LcdModes[ModeNumber].DispcDivisor) );\r
+\r
+  // Set up the graphics layer\r
+  MmioWrite32 (DISPC_GFX_PRELD, 0x2D8);\r
+  MmioWrite32 (DISPC_GFX_BA0, VramBaseAddress);\r
+  MmioWrite32 (DISPC_GFX_SIZE,\r
+               ((LcdModes[ModeNumber].HorizontalResolution - 1) \r
+               | ((LcdModes[ModeNumber].VerticalResolution - 1) << 16))\r
+              );\r
+\r
+  MmioWrite32(DISPC_GFX_ATTR, (GFXENABLE | RGB16 | BURSTSIZE16));\r
+\r
+  // Start it all\r
+  MmioOr32 (DISPC_CONTROL, (LCDENABLE | ACTIVEMATRIX | DATALINES24 | BYPASS_MODE | LCDENABLESIGNAL));\r
+  MmioOr32 (DISPC_CONTROL, GOLCD);\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+EFI_STATUS\r
+HwInitializeDisplay (\r
+  UINTN VramBaseAddress,\r
+  UINTN VramSize\r
+  )\r
+{\r
+  EFI_STATUS    Status;\r
+  UINT8         Data;\r
+  EFI_TPL       OldTpl;\r
+  EMBEDDED_EXTERNAL_DEVICE   *gTPS65950;\r
+\r
+  // Enable power lines used by TFP410\r
+  Status = gBS->LocateProtocol (&gEmbeddedExternalDeviceProtocolGuid, NULL, (VOID **)&gTPS65950);\r
+  ASSERT_EFI_ERROR (Status);  \r
+\r
+  OldTpl = gBS->RaiseTPL(TPL_NOTIFY);\r
+  Data = VAUX_DEV_GRP_P1;\r
+  Status = gTPS65950->Write (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID4, VPLL2_DEV_GRP), 1, &Data);\r
+  ASSERT_EFI_ERROR(Status);\r
+\r
+  Data = VAUX_DEDICATED_18V;\r
+  Status = gTPS65950->Write (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID4, VPLL2_DEDICATED), 1, &Data);\r
+  ASSERT_EFI_ERROR (Status);  \r
+\r
+  // Power up TFP410 (set GPIO2 on TPS - for BeagleBoard-xM)\r
+  Status = gTPS65950->Read (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID2, GPIODATADIR1), 1, &Data);\r
+  ASSERT_EFI_ERROR (Status);\r
+  Data |= BIT2;\r
+  Status = gTPS65950->Write (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID2, GPIODATADIR1), 1, &Data);\r
+  ASSERT_EFI_ERROR (Status);\r
+\r
+  Data = BIT2;\r
+  Status = gTPS65950->Write (gTPS65950, EXTERNAL_DEVICE_REGISTER(I2C_ADDR_GRP_ID2, SETGPIODATAOUT1), 1, &Data);\r
+  ASSERT_EFI_ERROR (Status);\r
+\r
+  gBS->RestoreTPL(OldTpl);\r
+\r
+  // Power up TFP410 (set GPIO 170 - for older BeagleBoards)\r
+  MmioAnd32 (GPIO6_BASE + GPIO_OE, ~BIT10);\r
+  MmioOr32  (GPIO6_BASE + GPIO_SETDATAOUT, BIT10);  \r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+EFI_STATUS\r
+InitializeDisplay (\r
+  IN LCD_INSTANCE* Instance\r
+  )\r
+{\r
+  EFI_STATUS           Status; \r
+  UINTN                VramSize;\r
+  EFI_PHYSICAL_ADDRESS VramBaseAddress;\r
+\r
+  Status = LcdPlatformGetVram (&VramBaseAddress, &VramSize);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  Instance->Mode.FrameBufferBase = VramBaseAddress;\r
+  Instance->Mode.FrameBufferSize = VramSize;\r
+\r
+  Status = HwInitializeDisplay((UINTN)VramBaseAddress, VramSize);\r
+  if (!EFI_ERROR (Status)) {\r
+    mDisplayInitialized = TRUE;\r
+  }\r
+\r
+  return Status;\r
+}\r
+\r
+EFI_STATUS\r
+EFIAPI\r
+LcdGraphicsQueryMode (\r
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL            *This,\r
+       IN UINT32                                  ModeNumber,\r
+       OUT UINTN                                  *SizeOfInfo,\r
+       OUT EFI_GRAPHICS_OUTPUT_MODE_INFORMATION   **Info\r
+       )\r
+{\r
+       LCD_INSTANCE  *Instance;\r
+\r
+       Instance = LCD_INSTANCE_FROM_GOP_THIS(This);\r
+\r
+  if (!mDisplayInitialized) {\r
+    InitializeDisplay (Instance);\r
+  }\r
+\r
+  // Error checking\r
+       if ( (This == NULL) || (Info == NULL) || (SizeOfInfo == NULL) || (ModeNumber >= This->Mode->MaxMode) ) {\r
+         DEBUG((DEBUG_ERROR, "LcdGraphicsQueryMode: ERROR - For mode number %d : Invalid Parameter.\n", ModeNumber ));\r
+               return EFI_INVALID_PARAMETER;\r
+       }\r
+\r
+  *Info = AllocateCopyPool(sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION), &Instance->ModeInfo);\r
+  if (*Info == NULL) {\r
+    return EFI_OUT_OF_RESOURCES;\r
+  }\r
+\r
+  *SizeOfInfo = sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION);\r
+\r
+  (*Info)->Version = 0;\r
+  (*Info)->HorizontalResolution = LcdModes[ModeNumber].HorizontalResolution;\r
+  (*Info)->VerticalResolution = LcdModes[ModeNumber].VerticalResolution;\r
+  (*Info)->PixelFormat = PixelBltOnly;\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+EFI_STATUS\r
+EFIAPI\r
+LcdGraphicsSetMode (\r
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL   *This,\r
+       IN UINT32                         ModeNumber\r
+       )\r
+{\r
+       LCD_INSTANCE  *Instance;\r
+\r
+       Instance = LCD_INSTANCE_FROM_GOP_THIS(This);\r
+\r
+  if (ModeNumber >= Instance->Mode.MaxMode) {\r
+    return EFI_UNSUPPORTED;\r
+  }\r
+\r
+  if (!mDisplayInitialized) {\r
+    InitializeDisplay (Instance);\r
+  }\r
+\r
+  DssSetMode((UINT32)Instance->Mode.FrameBufferBase, ModeNumber);\r
+\r
+  Instance->Mode.Mode = ModeNumber;\r
+  Instance->ModeInfo.HorizontalResolution = LcdModes[ModeNumber].HorizontalResolution;\r
+  Instance->ModeInfo.VerticalResolution = LcdModes[ModeNumber].VerticalResolution;\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+EFI_STATUS\r
+EFIAPI\r
+LcdGraphicsOutputDxeInitialize (\r
+  IN EFI_HANDLE         ImageHandle,\r
+  IN EFI_SYSTEM_TABLE   *SystemTable\r
+  )\r
+{\r
+  EFI_STATUS Status = EFI_SUCCESS;\r
+  LCD_INSTANCE* Instance;\r
+\r
+  Status = LcdInstanceContructor (&Instance);\r
+  if (EFI_ERROR(Status)) {\r
+    goto EXIT;\r
+  }\r
+\r
+       // Install the Graphics Output Protocol and the Device Path\r
+       Status = gBS->InstallMultipleProtocolInterfaces(\r
+                       &Instance->Handle,\r
+                       &gEfiGraphicsOutputProtocolGuid, &Instance->Gop,\r
+                       &gEfiDevicePathProtocolGuid,     &Instance->DevicePath,\r
+                       NULL\r
+          );\r
+\r
+       if (EFI_ERROR(Status)) {\r
+         DEBUG((DEBUG_ERROR, "GraphicsOutputDxeInitialize: Can not install the protocol. Exit Status=%r\n", Status));\r
+               goto EXIT;\r
+       }\r
+\r
+  // Register for an ExitBootServicesEvent\r
+       // When ExitBootServices starts, this function here will make sure that the graphics driver will shut down properly,\r
+       // i.e. it will free up all allocated memory and perform any necessary hardware re-configuration.\r
+       /*Status = gBS->CreateEvent (\r
+           EVT_SIGNAL_EXIT_BOOT_SERVICES,\r
+           TPL_NOTIFY,\r
+           LcdGraphicsExitBootServicesEvent, NULL,\r
+                       &Instance->ExitBootServicesEvent\r
+                       );\r
+\r
+       if (EFI_ERROR(Status)) {\r
+         DEBUG((DEBUG_ERROR, "GraphicsOutputDxeInitialize: Can not install the ExitBootServicesEvent handler. Exit Status=%r\n", Status));\r
+               goto EXIT_ERROR_UNINSTALL_PROTOCOL;\r
+       }*/\r
+\r
+       // To get here, everything must be fine, so just exit\r
+       goto EXIT;\r
+\r
+//EXIT_ERROR_UNINSTALL_PROTOCOL:\r
+       /* The following function could return an error message,\r
+        * however, to get here something must have gone wrong already,\r
+        * so preserve the original error, i.e. don't change\r
+        * the Status variable, even it fails to uninstall the protocol.\r
+        */\r
+/*     gBS->UninstallMultipleProtocolInterfaces (\r
+           Instance->Handle,\r
+           &gEfiGraphicsOutputProtocolGuid, &Instance->Gop, // Uninstall Graphics Output protocol\r
+           &gEfiDevicePathProtocolGuid,     &Instance->DevicePath,     // Uninstall device path\r
+           NULL\r
+           );*/\r
+\r
+EXIT:\r
+       return Status;\r
+\r
+}\r