1 files changed, 19 insertions, 35 deletions
diff --git a/ArmPlatformPkg/Library/HdLcd/HdLcd.c b/ArmPlatformPkg/Library/HdLcd/HdLcd.c
index f5886848ce..96f2bf437f 100644
--- a/ArmPlatformPkg/Library/HdLcd/HdLcd.c
+++ b/ArmPlatformPkg/Library/HdLcd/HdLcd.c
@@ -22,31 +22,7 @@
 
 #include "HdLcd.h"
 
-STATIC
-UINTN
-GetBytesPerPixel (
-  IN  LCD_BPP       Bpp
-  )
-{
-  switch (Bpp) {
-  case LCD_BITS_PER_PIXEL_24:
-    return 4;
-
-  case LCD_BITS_PER_PIXEL_16_565:
-  case LCD_BITS_PER_PIXEL_16_555:
-  case LCD_BITS_PER_PIXEL_12_444:
-    return 2;
-
-  case LCD_BITS_PER_PIXEL_8:
-  case LCD_BITS_PER_PIXEL_4:
-  case LCD_BITS_PER_PIXEL_2:
-  case LCD_BITS_PER_PIXEL_1:
-    return 1;
-
-  default:
-    return 0;
-  }
-}
+#define BYTES_PER_PIXEL 4
 
 /** Initialize display.
 
@@ -78,10 +54,6 @@ LcdInitialize (
     HDLCD_LITTLE_ENDIAN | HDLCD_4BYTES_PER_PIXEL
     );
 
-  MmioWrite32 (HDLCD_REG_RED_SELECT,   (0 << 16 | 8 << 8 | 0));
-  MmioWrite32 (HDLCD_REG_GREEN_SELECT, (0 << 16 | 8 << 8 | 8));
-  MmioWrite32 (HDLCD_REG_BLUE_SELECT,  (0 << 16 | 8 << 8 | 16));
-
   return EFI_SUCCESS;
 }
 
@@ -100,8 +72,8 @@ LcdSetMode (
   EFI_STATUS        Status;
   SCAN_TIMINGS      *Horizontal;
   SCAN_TIMINGS      *Vertical;
-  UINT32            BytesPerPixel;
-  LCD_BPP           LcdBpp;
+
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  ModeInfo;
 
   // Set the video mode timings and other relevant information
   Status = LcdPlatformGetTimings (
@@ -117,13 +89,22 @@ LcdSetMode (
   ASSERT (Horizontal != NULL);
   ASSERT (Vertical != NULL);
 
-  Status = LcdPlatformGetBpp (ModeNumber, &LcdBpp);
+  // Get the pixel format information.
+  Status = LcdPlatformQueryMode (ModeNumber, &ModeInfo);
   if (EFI_ERROR (Status)) {
     ASSERT_EFI_ERROR (Status);
     return Status;
   }
 
-  BytesPerPixel = GetBytesPerPixel (LcdBpp);
+  if (ModeInfo.PixelFormat == PixelBlueGreenRedReserved8BitPerColor) {
+    MmioWrite32 (HDLCD_REG_RED_SELECT,  (8 << 8) | 16);
+    MmioWrite32 (HDLCD_REG_BLUE_SELECT, (8 << 8) | 0);
+  } else {
+    MmioWrite32 (HDLCD_REG_BLUE_SELECT, (8 << 8) | 16);
+    MmioWrite32 (HDLCD_REG_RED_SELECT,  (8 << 8) | 0);
+  }
+
+  MmioWrite32 (HDLCD_REG_GREEN_SELECT, (8 << 8) | 8);
 
   // Disable the controller
   MmioWrite32 (HDLCD_REG_COMMAND, HDLCD_DISABLE);
@@ -131,10 +112,13 @@ LcdSetMode (
   // Update the frame buffer information with the new settings
   MmioWrite32 (
     HDLCD_REG_FB_LINE_LENGTH,
-    Horizontal->Resolution * BytesPerPixel
+    Horizontal->Resolution * BYTES_PER_PIXEL
     );
 
-  MmioWrite32 (HDLCD_REG_FB_LINE_PITCH, Horizontal->Resolution * BytesPerPixel);
+  MmioWrite32 (
+    HDLCD_REG_FB_LINE_PITCH,
+    Horizontal->Resolution * BYTES_PER_PIXEL
+    );
 
   MmioWrite32 (HDLCD_REG_FB_LINE_COUNT, Vertical->Resolution - 1);