BeagleBoardPkg: Add Display Driver support to the BeagleBoard

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@12432 6f19259b-4bc3-4df7-8a09-765794883524

4 files changed, 1057 insertions, 0 deletions

diff --git a/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputBlt.c b/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputBlt.c
new file mode 100644
index 0000000000..38c84dd88c
--- /dev/null
+++ b/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputBlt.c
@@ -0,0 +1,445 @@
+/** @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 <PiDxe.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+#include <Guid/GlobalVariable.h>
+
+#include "LcdGraphicsOutputDxe.h"
+
+extern BOOLEAN mDisplayInitialized;
+
+//
+// Function Definitions
+//
+
+STATIC
+EFI_STATUS
+VideoCopyNoHorizontalOverlap (
+  IN UINTN          BitsPerPixel,
+  IN volatile VOID  *FrameBufferBase,
+  IN UINT32         HorizontalResolution,
+  IN UINTN          SourceX,
+  IN UINTN          SourceY,
+  IN UINTN          DestinationX,
+  IN UINTN          DestinationY,
+  IN UINTN          Width,
+  IN UINTN          Height
+  )
+{
+  EFI_STATUS    Status = EFI_SUCCESS;
+  UINTN         SourceLine;
+  UINTN         DestinationLine;
+  UINTN         WidthInBytes;
+  UINTN         LineCount;
+  INTN          Step;
+  VOID          *SourceAddr;
+  VOID          *DestinationAddr;
+
+  if( DestinationY <= SourceY ) {
+    // scrolling up (or horizontally but without overlap)
+    SourceLine       = SourceY;
+    DestinationLine  = DestinationY;
+    Step             = 1;
+  } else {
+    // scrolling down
+    SourceLine       = SourceY + Height;
+    DestinationLine  = DestinationY + Height;
+    Step             = -1;
+  }
+
+  WidthInBytes = Width * 2;
+
+  for( LineCount = 0; LineCount < Height; LineCount++ ) {
+    // Update the start addresses of source & destination using 16bit pointer arithmetic
+    SourceAddr      = (VOID *)((UINT16 *)FrameBufferBase + SourceLine      * HorizontalResolution + SourceX     );
+    DestinationAddr = (VOID *)((UINT16 *)FrameBufferBase + DestinationLine * HorizontalResolution + DestinationX);
+
+    // Copy the entire line Y from video ram to the temp buffer
+    CopyMem( DestinationAddr, SourceAddr, WidthInBytes);
+
+    // Update the line numbers
+    SourceLine      += Step;
+    DestinationLine += Step;
+  }
+
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+VideoCopyHorizontalOverlap (
+  IN UINTN          BitsPerPixel,
+  IN volatile VOID  *FrameBufferBase,
+  UINT32            HorizontalResolution,
+  IN UINTN          SourceX,
+  IN UINTN          SourceY,
+  IN UINTN          DestinationX,
+  IN UINTN          DestinationY,
+  IN UINTN          Width,
+  IN UINTN          Height
+  )
+{
+  EFI_STATUS      Status = EFI_SUCCESS;
+
+  UINT16 *PixelBuffer16bit;
+  UINT16 *SourcePixel16bit;
+  UINT16 *DestinationPixel16bit;
+
+  UINT32          SourcePixelY;
+  UINT32          DestinationPixelY;
+  UINTN           SizeIn16Bits;
+
+  // Allocate a temporary buffer
+  PixelBuffer16bit = (UINT16 *) AllocatePool((Height * Width) * sizeof(UINT16));
+
+  if (PixelBuffer16bit == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto EXIT;
+  }
+
+  // Access each pixel inside the source area of the Video Memory and copy it to the temp buffer
+
+  SizeIn16Bits = Width * 2;
+
+  for (SourcePixelY = SourceY, DestinationPixel16bit = PixelBuffer16bit;
+       SourcePixelY < SourceY + Height;
+       SourcePixelY++, DestinationPixel16bit += Width)
+  {
+    // Calculate the source address:
+    SourcePixel16bit = (UINT16 *)FrameBufferBase + SourcePixelY * HorizontalResolution + SourceX;
+
+    // Copy the entire line Y from Video to the temp buffer
+    CopyMem( (VOID *)DestinationPixel16bit, (CONST VOID *)SourcePixel16bit, SizeIn16Bits);
+  }
+
+  // Copy from the temp buffer into the destination area of the Video Memory
+
+  for (DestinationPixelY = DestinationY, SourcePixel16bit = PixelBuffer16bit;
+       DestinationPixelY < DestinationY + Height;
+       DestinationPixelY++, SourcePixel16bit += Width)
+  {
+    // Calculate the target address:
+    DestinationPixel16bit = (UINT16 *)FrameBufferBase + (DestinationPixelY * HorizontalResolution + DestinationX);
+
+    // Copy the entire line Y from the temp buffer to Video
+    CopyMem( (VOID *)DestinationPixel16bit, (CONST VOID *)SourcePixel16bit, SizeIn16Bits);
+  }
+
+  // Free the allocated memory
+  FreePool((VOID *) PixelBuffer16bit);
+
+
+EXIT:
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+BltVideoFill (
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL        *This,
+  IN OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL   *EfiSourcePixel,     OPTIONAL
+  IN UINTN                               SourceX,
+  IN UINTN                               SourceY,
+  IN UINTN                               DestinationX,
+  IN UINTN                               DestinationY,
+  IN UINTN                               Width,
+  IN UINTN                               Height,
+  IN UINTN                               Delta           OPTIONAL   // Number of BYTES in a row of the BltBuffer
+  )
+{
+  EFI_PIXEL_BITMASK*  PixelInformation;
+  EFI_STATUS          Status;
+  UINT32              HorizontalResolution;
+  VOID                *FrameBufferBase;
+  UINT16              *DestinationPixel16bit;
+  UINT16              Pixel16bit;
+  UINT32              DestinationPixelX;
+  UINT32              DestinationLine;
+
+  Status           = EFI_SUCCESS;
+  PixelInformation = &This->Mode->Info->PixelInformation;
+  FrameBufferBase = (UINTN *)((UINTN)(This->Mode->FrameBufferBase));
+  HorizontalResolution = This->Mode->Info->HorizontalResolution;
+
+  // Convert the EFI pixel at the start of the BltBuffer(0,0) into a video display pixel
+  Pixel16bit = (UINT16) (
+      ( (EfiSourcePixel->Red      <<  8) & PixelInformation->RedMask      )
+    | ( (EfiSourcePixel->Green    <<  3) & PixelInformation->GreenMask    )
+    | ( (EfiSourcePixel->Blue     >>  3) & PixelInformation->BlueMask     )
+   );
+
+  // Copy the SourcePixel into every pixel inside the target rectangle
+  for (DestinationLine = DestinationY;
+       DestinationLine < DestinationY + Height;
+       DestinationLine++)
+  {
+    for (DestinationPixelX = DestinationX;
+         DestinationPixelX < DestinationX + Width;
+         DestinationPixelX++)
+    {
+      // Calculate the target address:
+      DestinationPixel16bit =  (UINT16 *)FrameBufferBase + DestinationLine * HorizontalResolution  + DestinationPixelX;
+
+      // Copy the pixel into the new target
+      *DestinationPixel16bit = Pixel16bit;
+    }
+  }
+
+
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+BltVideoToBltBuffer (
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL        *This,
+  IN OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL   *BltBuffer,     OPTIONAL
+  IN UINTN                               SourceX,
+  IN UINTN                               SourceY,
+  IN UINTN                               DestinationX,
+  IN UINTN                               DestinationY,
+  IN UINTN                               Width,
+  IN UINTN                               Height,
+  IN UINTN                               Delta           OPTIONAL   // Number of BYTES in a row of the BltBuffer
+  )
+{
+  EFI_STATUS         Status;
+  UINT32             HorizontalResolution;
+  EFI_PIXEL_BITMASK  *PixelInformation;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL *EfiDestinationPixel;
+  VOID               *FrameBufferBase;
+  UINT16             *SourcePixel16bit;
+  UINT16             Pixel16bit;
+  UINT32             SourcePixelX;
+  UINT32             SourceLine;
+  UINT32             DestinationPixelX;
+  UINT32             DestinationLine;
+  UINT32             BltBufferHorizontalResolution;
+
+  Status = EFI_SUCCESS;
+  PixelInformation = &This->Mode->Info->PixelInformation;
+  HorizontalResolution = This->Mode->Info->HorizontalResolution;
+  FrameBufferBase = (UINTN *)((UINTN)(This->Mode->FrameBufferBase));
+
+  if(( Delta != 0 ) && ( Delta != Width * sizeof(EFI_GRAPHICS_OUTPUT_BLT_PIXEL))) {
+    // Delta is not zero and it is different from the width.
+    // Divide it by the size of a pixel to find out the buffer's horizontal resolution.
+    BltBufferHorizontalResolution = (UINT32) (Delta / sizeof(EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+  } else {
+    BltBufferHorizontalResolution = Width;
+  }
+
+  // Access each pixel inside the Video Memory
+  for (SourceLine = SourceY, DestinationLine = DestinationY;
+       SourceLine < SourceY + Height;
+       SourceLine++, DestinationLine++)
+  {
+    for (SourcePixelX = SourceX, DestinationPixelX = DestinationX;
+         SourcePixelX < SourceX + Width;
+         SourcePixelX++, DestinationPixelX++)
+    {
+      // Calculate the source and target addresses:
+      SourcePixel16bit = (UINT16 *)FrameBufferBase + SourceLine * HorizontalResolution + SourcePixelX;
+      EfiDestinationPixel = BltBuffer + DestinationLine * BltBufferHorizontalResolution + DestinationPixelX;
+
+      // Snapshot the pixel from the video buffer once, to speed up the operation.
+      // If we were dereferencing the pointer, as it is volatile, we would perform 3 memory read operations.
+      Pixel16bit = *SourcePixel16bit;
+
+      // Copy the pixel into the new target
+      EfiDestinationPixel->Red      = (UINT8) ( (Pixel16bit & PixelInformation->RedMask     ) >>  8 );
+      EfiDestinationPixel->Green    = (UINT8) ( (Pixel16bit & PixelInformation->GreenMask   ) >>  3 );
+      EfiDestinationPixel->Blue     = (UINT8) ( (Pixel16bit & PixelInformation->BlueMask    ) <<  3 );
+    }
+  }
+
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+BltBufferToVideo (
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL        *This,
+  IN OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL   *BltBuffer,     OPTIONAL
+  IN UINTN                               SourceX,
+  IN UINTN                               SourceY,
+  IN UINTN                               DestinationX,
+  IN UINTN                               DestinationY,
+  IN UINTN                               Width,
+  IN UINTN                               Height,
+  IN UINTN                               Delta           OPTIONAL   // Number of BYTES in a row of the BltBuffer
+  )
+{
+  EFI_STATUS         Status;
+  UINT32             HorizontalResolution;
+  EFI_PIXEL_BITMASK  *PixelInformation;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL *EfiSourcePixel;
+  VOID               *FrameBufferBase;
+  UINT16             *DestinationPixel16bit;
+  UINT32             SourcePixelX;
+  UINT32             SourceLine;
+  UINT32             DestinationPixelX;
+  UINT32             DestinationLine;
+  UINT32             BltBufferHorizontalResolution;
+
+  Status = EFI_SUCCESS;
+  PixelInformation = &This->Mode->Info->PixelInformation;
+  HorizontalResolution = This->Mode->Info->HorizontalResolution;
+  FrameBufferBase = (UINTN *)((UINTN)(This->Mode->FrameBufferBase));
+
+  if(( Delta != 0 ) && ( Delta != Width * sizeof(EFI_GRAPHICS_OUTPUT_BLT_PIXEL))) {
+    // Delta is not zero and it is different from the width.
+    // Divide it by the size of a pixel to find out the buffer's horizontal resolution.
+    BltBufferHorizontalResolution = (UINT32) (Delta / sizeof(EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+  } else {
+    BltBufferHorizontalResolution = Width;
+  }
+
+  // Access each pixel inside the BltBuffer Memory
+  for (SourceLine = SourceY, DestinationLine = DestinationY;
+       SourceLine < SourceY + Height;
+       SourceLine++, DestinationLine++) {
+
+    for (SourcePixelX = SourceX, DestinationPixelX = DestinationX;
+         SourcePixelX < SourceX + Width;
+         SourcePixelX++, DestinationPixelX++)
+    {
+      // Calculate the source and target addresses:
+      EfiSourcePixel  = BltBuffer + SourceLine * BltBufferHorizontalResolution + SourcePixelX;
+      DestinationPixel16bit = (UINT16 *)FrameBufferBase + DestinationLine * HorizontalResolution + DestinationPixelX;
+
+      // Copy the pixel into the new target
+      // Only the most significant bits will be copied across:
+      // To convert from 8 bits to 5 bits per pixel we throw away the 3 least significant bits
+        *DestinationPixel16bit = (UINT16) (
+              ( (EfiSourcePixel->Red      <<  8) & PixelInformation->RedMask      )
+            | ( (EfiSourcePixel->Green    <<  3) & PixelInformation->GreenMask    )
+            | ( (EfiSourcePixel->Blue     >>  3) & PixelInformation->BlueMask     )
+            );
+      }
+    }
+   
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+BltVideoToVideo (
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL        *This,
+  IN OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL   *BltBuffer,     OPTIONAL
+  IN UINTN                               SourceX,
+  IN UINTN                               SourceY,
+  IN UINTN                               DestinationX,
+  IN UINTN                               DestinationY,
+  IN UINTN                               Width,
+  IN UINTN                               Height,
+  IN UINTN                               Delta           OPTIONAL   // Number of BYTES in a row of the BltBuffer
+  )
+{
+  EFI_STATUS         Status;
+  UINT32             HorizontalResolution;
+  UINTN              BitsPerPixel;
+  VOID               *FrameBufferBase;
+
+  BitsPerPixel = 16;
+
+  HorizontalResolution = This->Mode->Info->HorizontalResolution;
+  FrameBufferBase = (UINTN *)((UINTN)(This->Mode->FrameBufferBase));
+
+  //
+  // BltVideo to BltVideo:
+  //
+  //  Source is the Video Memory,
+  //  Destination is the Video Memory
+
+  FrameBufferBase = (UINTN *)((UINTN)(This->Mode->FrameBufferBase));
+
+  // The UEFI spec currently states:
+  // "There is no limitation on the overlapping of the source and destination rectangles"
+  // Therefore, we must be careful to avoid overwriting the source data
+  if( SourceY == DestinationY ) {
+    // Copying within the same height, e.g. horizontal shift
+    if( SourceX == DestinationX ) {
+      // Nothing to do
+      Status = EFI_SUCCESS;
+    } else if( ((SourceX>DestinationX)?(SourceX - DestinationX):(DestinationX - SourceX)) < Width ) {
+      // There is overlap
+      Status = VideoCopyHorizontalOverlap (BitsPerPixel, FrameBufferBase, HorizontalResolution, SourceX, SourceY, DestinationX, DestinationY, Width, Height );
+    } else {
+      // No overlap
+      Status = VideoCopyNoHorizontalOverlap (BitsPerPixel, FrameBufferBase, HorizontalResolution, SourceX, SourceY, DestinationX, DestinationY, Width, Height );
+    }
+  } else {
+    // Copying from different heights
+    Status = VideoCopyNoHorizontalOverlap (BitsPerPixel, FrameBufferBase, HorizontalResolution, SourceX, SourceY, DestinationX, DestinationY, Width, Height );
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+EFIAPI
+LcdGraphicsBlt (
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL        *This,
+	IN OUT 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   // Number of BYTES in a row of the BltBuffer
+  )
+{
+  EFI_STATUS    Status;
+	LCD_INSTANCE  *Instance;
+
+	Instance = LCD_INSTANCE_FROM_GOP_THIS(This);
+
+  if (!mDisplayInitialized) {
+    InitializeDisplay (Instance);
+  }
+  
+  switch (BltOperation) {
+	  case EfiBltVideoFill:
+	    Status = BltVideoFill (This, BltBuffer, SourceX, SourceY, DestinationX, DestinationY, Width, Height, Delta);
+		  break;
+
+	  case EfiBltVideoToBltBuffer:
+	    Status = BltVideoToBltBuffer (This, BltBuffer, SourceX, SourceY, DestinationX, DestinationY, Width, Height, Delta);
+      break;
+
+    case EfiBltBufferToVideo:
+      Status = BltBufferToVideo (This, BltBuffer, SourceX, SourceY, DestinationX, DestinationY, Width, Height, Delta);
+		  break;
+
+	  case EfiBltVideoToVideo:
+	    Status = BltVideoToVideo (This, BltBuffer, SourceX, SourceY, DestinationX, DestinationY, Width, Height, Delta);
+		  break;
+
+	  case EfiGraphicsOutputBltOperationMax:
+	  default:
+		  DEBUG((DEBUG_ERROR, "LcdGraphicsBlt: Invalid Operation\n"));
+		  Status = EFI_INVALID_PARAMETER;
+		  break;
+	}
+
+  return Status;
+}
diff --git a/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputDxe.c b/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputDxe.c
new file mode 100644
index 0000000000..6c20575cfe
--- /dev/null
+++ b/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputDxe.c
@@ -0,0 +1,403 @@
+/** @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;
+
+}
diff --git a/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputDxe.h b/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputDxe.h
new file mode 100644
index 0000000000..39e69478a1
--- /dev/null
+++ b/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputDxe.h
@@ -0,0 +1,157 @@
+/** @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.
+
+**/
+
+#ifndef __OMAP3_DSS_GRAPHICS__
+#define __OMAP3_DSS_GRAPHICS__
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/IoLib.h>
+
+#include <Protocol/DevicePathToText.h>
+#include <Protocol/EmbeddedExternalDevice.h>
+#include <Protocol/Cpu.h>
+
+#include <Guid/GlobalVariable.h>
+
+#include <Omap3530/Omap3530.h>
+#include <TPS65950.h>
+
+typedef struct {
+  VENDOR_DEVICE_PATH            Guid;
+  EFI_DEVICE_PATH_PROTOCOL      End;
+} LCD_GRAPHICS_DEVICE_PATH;
+
+typedef struct {
+  UINTN                                 Signature;
+  EFI_HANDLE                            Handle;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  ModeInfo;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL_MODE     Mode;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL          Gop;
+  LCD_GRAPHICS_DEVICE_PATH              DevicePath;
+//  EFI_EVENT                             ExitBootServicesEvent;
+} LCD_INSTANCE;
+
+#define LCD_INSTANCE_SIGNATURE  SIGNATURE_32('l', 'c', 'd', '0')
+#define LCD_INSTANCE_FROM_GOP_THIS(a)     CR (a, LCD_INSTANCE, Gop, LCD_INSTANCE_SIGNATURE)
+
+typedef struct {
+  UINTN             Mode;
+  UINTN             HorizontalResolution;
+  UINTN             VerticalResolution;
+
+  UINT32            DssDivisor;
+  UINT32            DispcDivisor;
+
+  UINT32            HSync;
+  UINT32            HFrontPorch;
+  UINT32            HBackPorch;
+
+  UINT32            VSync;
+  UINT32            VFrontPorch;
+  UINT32            VBackPorch;
+} LCD_MODE;
+
+EFI_STATUS
+InitializeDisplay (
+  IN LCD_INSTANCE* Instance
+);
+
+EFI_STATUS
+EFIAPI
+LcdGraphicsQueryMode (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL          *This,
+  IN  UINT32                                ModeNumber,
+  OUT UINTN                                 *SizeOfInfo,
+  OUT EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  **Info
+);
+
+EFI_STATUS
+EFIAPI
+LcdGraphicsSetMode (
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL  *This,
+  IN UINT32                        ModeNumber
+);
+
+EFI_STATUS
+EFIAPI
+LcdGraphicsBlt (
+  IN EFI_GRAPHICS_OUTPUT_PROTOCOL        *This,
+	IN OUT 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   // Number of BYTES in a row of the BltBuffer
+);
+
+// HW registers
+#define CM_FCLKEN_DSS   0x48004E00
+#define CM_ICLKEN_DSS   0x48004E10
+
+#define DSS_CONTROL     0x48050040
+#define DSS_SYSCONFIG   0x48050010
+#define DSS_SYSSTATUS   0x48050014
+
+#define DISPC_CONTROL   0x48050440
+#define DISPC_CONFIG    0x48050444
+#define DISPC_SIZE_LCD  0x4805047C
+#define DISPC_TIMING_H  0x48050464
+#define DISPC_TIMING_V  0x48050468
+
+#define CM_CLKSEL_DSS   0x48004E40
+#define DISPC_DIVISOR   0x48050470
+#define DISPC_POL_FREQ  0x4805046C
+
+#define DISPC_GFX_TABLE_BA 0x480504B8
+#define DISPC_GFX_BA0   0x48050480
+#define DISPC_GFX_BA1   0x48050484
+#define DISPC_GFX_POS   0x48050488
+#define DISPC_GFX_SIZE  0x4805048C
+#define DISPC_GFX_ATTR  0x480504A0
+#define DISPC_GFX_PRELD 0x4805062C
+
+#define DISPC_DEFAULT_COLOR_0 0x4805044C
+
+//#define DISPC_IRQSTATUS
+
+// Bits
+#define EN_TV           0x4
+#define EN_DSS2         0x2
+#define EN_DSS1         0x1
+#define EN_DSS          0x1
+
+#define DSS_SOFTRESET   0x2
+#define DSS_RESETDONE   0x1
+
+#define BYPASS_MODE     (BIT15 | BIT16)
+
+#define LCDENABLE       BIT0
+#define ACTIVEMATRIX    BIT3
+#define GOLCD           BIT5
+#define DATALINES24     (BIT8 | BIT9)   
+#define LCDENABLESIGNAL BIT28
+
+#define GFXENABLE       BIT0
+#define RGB16           (0x6 << 1)
+#define BURSTSIZE16     (0x2 << 6)
+
+#define CLEARLOADMODE   ~(BIT2 | BIT1)
+#define LOAD_FRAME_ONLY BIT2
+
+#endif
diff --git a/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputDxe.inf b/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputDxe.inf
new file mode 100644
index 0000000000..f595bb39ed
--- /dev/null
+++ b/Omap35xxPkg/LcdGraphicsOutputDxe/LcdGraphicsOutputDxe.inf
@@ -0,0 +1,52 @@
+#/** @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.
+#  
+#**/
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = LcdGraphicsDxe
+  FILE_GUID                      = E68088EF-D1A4-4336-C1DB-4D3A204730A6
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = LcdGraphicsOutputDxeInitialize
+
+[Sources.common]
+  LcdGraphicsOutputDxe.c
+  LcdGraphicsOutputBlt.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
+  Omap35xxPkg/Omap35xxPkg.dec
+  EmbeddedPkg/EmbeddedPkg.dec
+
+[LibraryClasses]
+  ArmLib
+  UefiLib
+  BaseLib
+  DebugLib
+  TimerLib
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  IoLib
+  BaseMemoryLib
+
+[Protocols]
+  gEfiDevicePathProtocolGuid
+  gEfiGraphicsOutputProtocolGuid
+  gEfiDevicePathToTextProtocolGuid
+  gEmbeddedExternalDeviceProtocolGuid
+
+[Depex]
+  gEfiCpuArchProtocolGuid AND gEfiTimerArchProtocolGuid