/**
 * GraphicsConsole.c
 *
 * AMI GraphicsConsole DXE Driver
 * Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL on top of
 * EFI_GRAPHICS_OUTPUT_PROTOCOL.
 *
 * Source: MdeModulePkg/Universal/Console/GraphicsConsoleDxe
 * PDB:    GraphicsConsole.pdb
 * Arch:   X64
 *
 * Pipeline:
 *   TextOutOutputString -> accumulate chars in gOutputBuffer ->
 *   FlushCurrentLine when newline/wrap -> HII_FONT StringToImage ->
 *   TextOutSetModeCallback -> GOP Blt
 */

#include "GraphicsConsole.h"

//
// Global state variables for text output line buffer
//
UINT16  *gOutputBuffer           = NULL;
UINT32   gOutputBufferCount      = 0;
UINT32   gOutputBufferPixelWidth = 0;
UINT32   gOutputBufferMaxChars   = 0;

UINT64   gHiiFontInterface       = 0;   // cached HII_FONT protocol pointer
UINT64   gHiiFontHandle          = 0;   // cached HII_FONT protocol handle

//
// Standard UEFI globals (set by ModuleEntryPoint)
//
EFI_HANDLE            gImageHandle       = NULL;
EFI_SYSTEM_TABLE     *gSystemTable       = NULL;
EFI_BOOT_SERVICES    *gBootServices      = NULL;
EFI_RUNTIME_SERVICES *gRuntimeServices   = NULL;

//
// DXE-phase copies
//
EFI_HANDLE            gImageHandle_0     = NULL;
EFI_SYSTEM_TABLE     *gSystemTable_0     = NULL;
EFI_BOOT_SERVICES    *gBootServices_0    = NULL;
EFI_RUNTIME_SERVICES *gRuntimeServices_0 = NULL;

//=============================================================================
// Standard UEFI Module Entry Point
//=============================================================================

EFI_STATUS
EFIAPI
ModuleEntryPoint(
    IN EFI_HANDLE        ImageHandle,
    IN EFI_SYSTEM_TABLE *SystemTable
    )
{
    gImageHandle = ImageHandle;
    ASSERT (gImageHandle != NULL);

    gSystemTable = SystemTable;
    ASSERT (gSystemTable != NULL);

    gBootServices = SystemTable->BootServices;
    ASSERT (gBootServices != NULL);

    gRuntimeServices = SystemTable->RuntimeServices;
    ASSERT (gRuntimeServices != NULL);

    GetHobList ();

    return GraphicsConsoleDriverEntry (ImageHandle, SystemTable);
}

//=============================================================================
// Driver Entry Point
//=============================================================================

EFI_STATUS
EFIAPI
GraphicsConsoleDriverEntry(
    IN EFI_HANDLE        ImageHandle,
    IN EFI_SYSTEM_TABLE *SystemTable
    )
{
    EFI_STATUS  Status;

    if (gSystemTable_0 == NULL) {
        gSystemTable_0     = SystemTable;
        gBootServices_0    = SystemTable->BootServices;
        gRuntimeServices_0 = SystemTable->RuntimeServices;
    }

    //
    // Create GOP notification event (TPL_NOTIFY)
    //
    Status = gBootServices_0->CreateEvent (
                                EVT_NOTIFY_SIGNAL,
                                TPL_NOTIFY,
                                GopNotificationEvent,
                                NULL,
                                &Handle
                                );
    if (EFI_ERROR (Status)) {
        DEBUG ((EFI_D_ERROR, "ASSERT_EFI_ERROR (Status = %r)\n", Status));
        ASSERT (!EFI_ERROR (Status));
    }

    //
    // Install protocol notification for SimpleTextOut on GOP
    //
    Status = gBootServices_0->InstallProtocolInterface (
                                &Handle,
                                &gEfiSimpleTextOutProtocolGuid,
                                EFI_NATIVE_INTERFACE,
                                &Context
                                );
    if (EFI_ERROR (Status)) {
        DEBUG ((EFI_D_ERROR, "ASSERT_EFI_ERROR (Status = %r)\n", Status));
        ASSERT (!EFI_ERROR (Status));
    }

    gBootServices_0->CloseEvent (Handle);

    gImageHandle_0 = ImageHandle;

    //
    // Install driver binding + component name protocols
    //
    return gBootServices_0->InstallMultipleProtocolInterfaces (
                              &gImageHandle_0,
                              &gEfiDriverBindingProtocolGuid,
                              mDriverBinding,
                              &gEfiComponentNameProtocolGuid,
                              mComponentName,
                              NULL
                              );
}

//=============================================================================
// GOP Notification Event Handler
//=============================================================================

EFI_STATUS
EFIAPI
GopNotificationEvent(
    IN EFI_EVENT  Event
    )
{
    EFI_STATUS                   Status;
    UINTN                        HandleCount;
    EFI_HANDLE                  *HandleBuffer;
    UINTN                        Index;
    EFI_GRAPHICS_OUTPUT_PROTOCOL *Gop;
    EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *GopModeInfo;
    UINTN                        GopModeInfoSize;
    GRAPHICS_CONSOLE_MODE        *ModeEntry;
    UINT32                      *Pool;
    UINT32                       AllocationSize;
    UINT8                        FontInfo[16];

    ModeEntry = &gModuleModeTable[0];

    //
    // Locate all handles with GOP protocol
    //
    Status = gBootServices_0->LocateHandleBuffer (
                                ByProtocol,
                                &gEfiGraphicsOutputProtocolGuid,
                                NULL,
                                &HandleCount,
                                &HandleBuffer
                                );
    if (EFI_ERROR (Status)) {
        return Status;
    }

    for (Index = 0; Index < HandleCount; Index++) {
        Status = gBootServices_0->HandleProtocol (
                                    HandleBuffer[Index],
                                    &gEfiGraphicsOutputProtocolGuid,
                                    (VOID **)&Gop
                                    );
        if (EFI_ERROR (Status)) {
            continue;
        }

        GopModeInfo     = NULL;
        GopModeInfoSize = 0;

        Status = Gop->QueryMode (
                        Gop,
                        Gop->Mode->Mode,
                        &GopModeInfoSize,
                        &GopModeInfo,
                        FontInfo
                        );
        if (EFI_ERROR (Status)) {
            if (GopModeInfo != NULL) {
                gBootServices_0->FreePool (GopModeInfo);
            }
            continue;
        }

        //
        // Match GOP handle by comparing first 16 bytes of mode info
        //
        if (CompareMem (GopModeInfo, &mGopModeInfoSource, 16) != 0) {
            gBootServices_0->FreePool (GopModeInfo);
            continue;
        }

        ModeEntry = (GRAPHICS_CONSOLE_MODE *)((UINT8 *)GopModeInfo + 16);
    }

    //
    // Cache HII_FONT protocol
    //
    if (gHiiFontInterface == NULL) {
        Status = gBootServices_0->LocateProtocol (
                                    &gEfiHiiFontProtocolGuid,
                                    NULL,
                                    (VOID **)&gHiiFontInterface
                                    );
        if (EFI_ERROR (Status)) {
            gHiiFontInterface = NULL;
        }
    }

    if (gHiiFontInterface != NULL) {
        //
        // Validate HII font with notification buffer
        //
        AllocationSize = (ModeEntry->ModeNumber & 0xFFFFFF) + 24;
        Pool = (UINT32 *)AllocatePool (AllocationSize);
        if (Pool != NULL) {
            Pool[0] = *(UINT32 *)&gEfiHiiFontProtocolGuid;
            Pool[4] = AllocationSize;

            gBootServices_0->CopyMem (Pool + 5, ModeEntry, ModeEntry->ModeNumber & 0xFFFFFF);

            UINT8 *End = (UINT8 *)Pool + 20 + (ModeEntry->ModeNumber & 0xFFFFFF);
            *(UINT32 *)End &= 0xFF000004;
            *(UINT32 *)End |= 4;
            End[3] = 0xDF;

            ((EFI_HII_FONT_PROTOCOL *)gHiiFontInterface)->StringToImage (
                                                             (EFI_HII_FONT_PROTOCOL *)gHiiFontInterface,
                                                             (EFI_HII_OUT_FLAGS *)Pool,
                                                             NULL,
                                                             FontInfo
                                                             );
            gBootServices_0->FreePool (Pool);
        }
    }

    if (GopModeInfo != NULL) {
        gBootServices_0->FreePool (GopModeInfo);
    }
    gBootServices_0->FreePool (HandleBuffer);

    gBootServices_0->SignalEvent (Event);
    return EFI_SUCCESS;
}

//=============================================================================
// TextOutConstructor (Driver Binding Start)
//=============================================================================

EFI_STATUS
TextOutConstructor(
    IN EFI_HANDLE  ControllerHandle,
    IN VOID       *DriverBindingHandle
    )
{
    EFI_STATUS                    Status;
    GRAPHICS_CONSOLE_PRIVATE_DATA *Private;
    SIMPLE_TEXT_OUTPUT_MODE       *Mode;

    //
    // Allocate private context (266 bytes)
    //
    Status = gBootServices_0->AllocatePool (
                                EfiBootServicesData,
                                sizeof (GRAPHICS_CONSOLE_PRIVATE_DATA),
                                (VOID **)&Private
                                );
    if (EFI_ERROR (Status)) {
        return Status;
    }
    ZeroMem (Private, sizeof (GRAPHICS_CONSOLE_PRIVATE_DATA));

    //
    // Copy function table template (80 bytes at off_2990)
    //
    gBootServices_0->CopyMem (Private, &mTextOutFunctionTable, sizeof (mTextOutFunctionTable));

    //
    // Allocate and init SimpleTextOutMode (24 bytes)
    //
    Status = gBootServices_0->AllocatePool (
                                EfiBootServicesData,
                                sizeof (SIMPLE_TEXT_OUTPUT_MODE),
                                (VOID **)&Mode
                                );
    if (EFI_ERROR (Status)) {
        gBootServices_0->FreePool (Private);
        return Status;
    }
    Mode->Attribute = (UINT32)-1;
    Private->TextOutMode = Mode;

    //
    // Allocate mode table (4 modes x 25 bytes = 100 bytes)
    //
    Status = gBootServices_0->AllocatePool (
                                EfiBootServicesData,
                                GRAPHICS_CONSOLE_MODE_MAX * GRAPHICS_CONSOLE_MODE_ENTRY_SIZE,
                                (VOID **)&Private->Modes
                                );
    if (EFI_ERROR (Status)) {
        gBootServices_0->FreePool (Private->TextOutMode);
        gBootServices_0->FreePool (Private);
        return Status;
    }

    //
    // Open GOP protocol BY_DRIVER
    //
    Status = gBootServices_0->OpenProtocol (
                                ControllerHandle,
                                &gEfiGraphicsOutputProtocolGuid,
                                (VOID **)&Private->Gop,
                                gImageHandle_0,
                                ControllerHandle,
                                EFI_OPEN_PROTOCOL_BY_DRIVER
                                );
    if (EFI_ERROR (Status)) {
        goto FreeAndReturn;
    }

    //
    // Open HII Font protocol BY_DRIVER (optional)
    //
    if (EFI_ERROR (gBootServices_0->OpenProtocol (
                                      ControllerHandle,
                                      &gEfiHiiFontProtocolGuid,
                                      (VOID **)&Private->HiiFont,
                                      gImageHandle_0,
                                      ControllerHandle,
                                      EFI_OPEN_PROTOCOL_BY_DRIVER
                                      ))) {
        //
        // Locate HII Font on any handle
        //
        gBootServices_0->LocateProtocol (
                          &gEfiHiiFontProtocolGuid,
                          NULL,
                          (VOID **)&Private->HiiFont
                          );
    }

    //
    // Initialize graphics mode table
    //
    Status = InitializeGraphicsMode (Private);
    if (EFI_ERROR (Status)) {
        gBootServices_0->CloseProtocol (
                          ControllerHandle,
                          &gEfiGraphicsOutputProtocolGuid,
                          gImageHandle_0,
                          ControllerHandle
                          );
        goto FreeAndReturn;
    }

    //
    // Initialize private fields
    //
    Private->Signature       = GRAPHICS_CONSOLE_PRIVATE_SIGNATURE;
    Private->FlushStub       = TextOutFlushStub;
    Private->FlushFunc       = TextOutFlush;
    Private->SetModeCallback = TextOutSetModeCallback;
    Private->CursorEnabled   = TRUE;
    Private->GopResetRequired = FALSE;

    Private->TextOutMode->CursorVisible = FALSE;
    Private->TextOutMode->Attribute     = EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK);

    //
    // Set default mode 0 and clear screen
    //
    if (TextOutSetMode (Private, 0) >= 0) {
        ClearScreen (Private, TRUE);
    }

    //
    // Install protocols on new handle
    //
    Status = gBootServices_0->InstallMultipleProtocolInterfaces (
                                &ControllerHandle,
                                &gEfiSimpleTextOutProtocolGuid,
                                Private,
                                &gEfiGraphicsOutputProtocolGuid,
                                Private->SetModeCallback,
                                NULL
                                );
    if (EFI_ERROR (Status)) {
        gBootServices_0->CloseProtocol (
                          ControllerHandle,
                          &gEfiGraphicsOutputProtocolGuid,
                          gImageHandle_0,
                          ControllerHandle
                          );
        goto FreeAndReturn;
    }

    return EFI_SUCCESS;

FreeAndReturn:
    gBootServices_0->FreePool (Private->Modes);
    gBootServices_0->FreePool (Private->TextOutMode);
    gBootServices_0->FreePool (Private);
    return Status;
}

//=============================================================================
// TextOutDestructor (Driver Binding Stop)
//=============================================================================

EFI_STATUS
TextOutDestructor(
    IN EFI_HANDLE  ControllerHandle,
    IN VOID       *DriverBindingHandle
    )
{
    EFI_STATUS                    Status;
    GRAPHICS_CONSOLE_PRIVATE_DATA *Private;

    //
    // Retrieve private data from the protocol interface
    //
    Status = gBootServices_0->OpenProtocol (
                                ControllerHandle,
                                &gEfiSimpleTextOutProtocolGuid,
                                (VOID **)&Private,
                                gImageHandle_0,
                                ControllerHandle,
                                EFI_OPEN_PROTOCOL_GET_PROTOCOL
                                );
    if (EFI_ERROR (Status)) {
        return EFI_UNSUPPORTED;
    }

    //
    // Close GOP protocol (opened BY_DRIVER)
    //
    gBootServices_0->CloseProtocol (
                      ControllerHandle,
                      &gEfiGraphicsOutputProtocolGuid,
                      gImageHandle_0,
                      ControllerHandle
                      );

    //
    // Uninstall protocols
    //
    Status = gBootServices_0->UninstallMultipleProtocolInterfaces (
                                ControllerHandle,
                                &gEfiSimpleTextOutProtocolGuid,
                                Private,
                                &gEfiGraphicsOutputProtocolGuid,
                                Private->SetModeCallback,
                                NULL
                                );
    if (EFI_ERROR (Status)) {
        return Status;
    }

    //
    // Free resources
    //
    gBootServices_0->FreePool (Private->Modes);
    gBootServices_0->FreePool (Private->TextOutMode);
    gBootServices_0->FreePool (Private);
    return EFI_SUCCESS;
}

//=============================================================================
// TextOutResetGop - Reset GOP state for this instance
//=============================================================================

EFI_STATUS
TextOutResetGop(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private
    )
{
    TextOutSetAttribute (Private, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
    TextOutClearScreen (Private);
    SetCursorPosition (Private, 0, 0);
    EnableCursor (Private);
    return EFI_SUCCESS;
}

//=============================================================================
// EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.Reset
//=============================================================================

EFI_STATUS
EFIAPI
TextOutReset(
    IN EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL *This,
    IN BOOLEAN                         ExtendedVerification
    )
{
    GRAPHICS_CONSOLE_PRIVATE_DATA *Private;
    EFI_STATUS                     Status;
    EFI_HANDLE                     Handle;

    Private = TEXT_OUT_PRIVATE_DATA_FROM_THIS (This);

    //
    // Locate the controller handle via GOP protocol
    //
    Status = gBootServices_0->LocateProtocol (
                                &gEfiSimpleTextOutProtocolGuid,
                                NULL,
                                (VOID **)&Handle
                                );
    if (EFI_ERROR (Status)) {
        return Status;
    }

    //
    // Reopen GOP protocol
    //
    gBootServices_0->CloseProtocol (
                      Handle,
                      &gEfiGraphicsOutputProtocolGuid,
                      gImageHandle_0,
                      Handle
                      );

    Status = gBootServices_0->OpenProtocol (
                                Handle,
                                &gEfiGraphicsOutputProtocolGuid,
                                (VOID **)&Private->Gop,
                                gImageHandle_0,
                                Handle,
                                EFI_OPEN_PROTOCOL_BY_DRIVER
                                );
    return Status;
}

//=============================================================================
// EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.OutputString
//=============================================================================

EFI_STATUS
EFIAPI
TextOutOutputString(
    IN EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL *This,
    IN CHAR16                          *String
    )
{
    GRAPHICS_CONSOLE_PRIVATE_DATA *Private;
    BOOLEAN                        WideCharFound;
    CHAR16                         Char;
    UINTN                          Column;
    UINTN                          Row;
    BOOLEAN                        CursorWasEnabled;
    VOID                          *GlyphBuffer;
    EFI_STATUS                     Status;
    UINTN                          CharPixelWidth;

    WideCharFound    = FALSE;
    GlyphBuffer      = NULL;
    Private = TEXT_OUT_PRIVATE_DATA_FROM_THIS (This);

    CursorWasEnabled = Private->TextOutMode->CursorVisible;
    if (CursorWasEnabled) {
        Private->FlushStub (Private);
        Private->TextOutMode->CursorVisible = FALSE;
    }

    while ((Char = *String) != 0) {
        switch (Char) {

        case CHAR_BACKSPACE:
            FlushCurrentLine (Private);
            Column = Private->TextOutMode->CursorColumn;
            Row    = Private->TextOutMode->CursorRow;
            if (Column > 0) {
                SetCursorPosition (Private, Column - 1, Row);
            }
            break;

        case CHAR_LF:
            FlushCurrentLine (Private);
            Column = Private->TextOutMode->CursorColumn;
            Row    = Private->TextOutMode->CursorRow;
            if (Row == (UINTN)Private->MaxTextRows - 1) {
                ScrollUp (Private);
                ScrollDown (Private);
            } else {
                SetCursorPosition (Private, Column, Row + 1);
            }
            break;

        case CHAR_CR:
            FlushCurrentLine (Private);
            SetCursorPosition (Private, 0, Private->TextOutMode->CursorRow);
            break;

        default:
            //
            // Convert char to glyph via HII Font
            //
            Status = Private->HiiFont->StringToImage (
                                         Private->HiiFont,
                                         Char,
                                         EFI_HII_OUT_FLAG_CLIP |
                                         EFI_HII_OUT_FLAG_WRAP,
                                         &GlyphBuffer,
                                         EFI_HII_IGNORE_IF_NO_GLYPH
                                         );
            if (EFI_ERROR (Status)) {
                return Status;
            }

            if (Status == EFI_WARN_UNKNOWN_GLYPH) {
                WideCharFound = TRUE;
                //
                // Force width to 8 for wide chars
                //
                if (((*(UINT16 *)GlyphBuffer - 8) & 0xFFF7) != 0) {
                    *(UINT16 *)GlyphBuffer = 8;
                }
            }

            //
            // Accumulate character in output buffer
            //
            CharPixelWidth = *(UINT16 *)GlyphBuffer >> 3;
            if (gOutputBufferPixelWidth + CharPixelWidth +
                Private->TextOutMode->CursorColumn > Private->MaxTextColumns) {
                FlushCurrentLine (Private);
                if (CharPixelWidth == 2) {
                    ScrollRow (Private, 1);
                }
            }

            gOutputBufferPixelWidth += CharPixelWidth;
            gOutputBuffer[gOutputBufferCount++] = Char;
            String++;

            //
            // Free glyph buffer
            //
            if (GlyphBuffer != NULL) {
                gBootServices_0->FreePool (*(VOID **)((UINT8 *)GlyphBuffer + 8));
                gBootServices_0->FreePool (GlyphBuffer);
                GlyphBuffer = NULL;
            }
            continue;
        }
        String++;
    }

    //
    // Flush remaining buffered characters
    //
    FlushCurrentLine (Private);

    if (CursorWasEnabled) {
        Private->TextOutMode->CursorVisible = TRUE;
        EnableCursor (Private);
    }

    gBootServices_0->CloseEvent (Private->TimerEvent);
    return (EFI_STATUS)WideCharFound;
}

//=============================================================================
// EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.TestString
//=============================================================================

EFI_STATUS
EFIAPI
TextOutTestString(
    IN EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL *This,
    IN CHAR16                          *String
    )
{
    CHAR16     *Str;
    EFI_STATUS  Status;
    VOID       *GlyphBuffer;

    GlyphBuffer = NULL;
    Str = String;

    while (*Str != 0) {
        if (*Str != CHAR_BACKSPACE && *Str != CHAR_LF && *Str != CHAR_CR) {
            Status = Private->HiiFont->StringToImage (
                                         Private->HiiFont,
                                         *Str,
                                         0,
                                         &GlyphBuffer,
                                         0
                                         );
            if (EFI_ERROR (Status)) {
                return Status;
            }

            if (GlyphBuffer != NULL) {
                gBootServices_0->FreePool (*(VOID **)((UINT8 *)GlyphBuffer + 8));
                gBootServices_0->FreePool (GlyphBuffer);
            }

            Status = (EFI_STATUS)Private->HiiFont->StringToImage (
                                                     Private->HiiFont,
                                                     *Str,
                                                     0,
                                                     NULL,
                                                     0
                                                     );
            if (Status == EFI_WARN_UNKNOWN_GLYPH) {
                return EFI_UNSUPPORTED;
            }
        }
        Str++;
    }
    return EFI_SUCCESS;
}

//=============================================================================
// EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.QueryMode
//=============================================================================

EFI_STATUS
EFIAPI
TextOutQueryMode(
    IN EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL *This,
    IN UINTN                           ModeNumber,
    OUT UINTN                          *Columns,
    OUT UINTN                          *Rows
    )
{
    GRAPHICS_CONSOLE_PRIVATE_DATA *Private;
    UINTN                          Index;

    Private = TEXT_OUT_PRIVATE_DATA_FROM_THIS (This);

    if (ModeNumber >= (UINTN)Private->TextOutMode->MaxMode) {
        return EFI_UNSUPPORTED;
    }

    for (Index = 0; Index < GRAPHICS_CONSOLE_MODE_MAX; Index++) {
        if (Private->Modes[Index].ModeNumber == (UINT32)ModeNumber) {
            break;
        }
    }
    if (Index >= GRAPHICS_CONSOLE_MODE_MAX) {
        return EFI_UNSUPPORTED;
    }
    if (!Private->Modes[Index].Supported) {
        return EFI_UNSUPPORTED;
    }

    *Columns = Private->Modes[Index].TextColumns;
    *Rows    = Private->Modes[Index].TextRows;
    return EFI_SUCCESS;
}

//=============================================================================
// EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.SetMode
//=============================================================================

EFI_STATUS
EFIAPI
TextOutSetMode(
    IN EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL *This,
    IN UINTN                           ModeNumber
    )
{
    GRAPHICS_CONSOLE_PRIVATE_DATA *Private;
    UINTN                          Index;
    GRAPHICS_CONSOLE_MODE         *Mode;
    UINT32                         GopCurrentMode;
    UINT32                         OldHorizRes;
    UINT32                         OldVertRes;
    UINT32                         TextCols;
    UINT32                         TextRows;
    BOOLEAN                        CursorWasEnabled;

    Private = TEXT_OUT_PRIVATE_DATA_FROM_THIS (This);

    if (ModeNumber >= (UINTN)Private->TextOutMode->MaxMode) {
        return EFI_UNSUPPORTED;
    }

    for (Index = 0; Index < GRAPHICS_CONSOLE_MODE_MAX; Index++) {
        if (Private->Modes[Index].ModeNumber == (UINT32)ModeNumber) {
            break;
        }
    }
    if (Index >= GRAPHICS_CONSOLE_MODE_MAX) {
        return EFI_UNSUPPORTED;
    }
    if (!Private->Modes[Index].Supported) {
        return EFI_UNSUPPORTED;
    }

    Mode = &Private->Modes[Index];

    CursorWasEnabled = Private->TextOutMode->CursorVisible;
    OldHorizRes = Private->GopHorizontalResolution;
    OldVertRes  = Private->GopVerticalResolution;

    TextCols = (Mode->HorizontalResolution - 8 * Mode->TextColumns) >> 1;
    TextRows = (Mode->VerticalResolution - 19 * Mode->TextRows) >> 1;

    if (CursorWasEnabled) {
        Private->FlushStub (Private);
    }

    //
    // Set GOP mode if needed
    //
    GopCurrentMode = Private->Gop->Mode->Mode;
    if (GopCurrentMode != Mode->GopModeNumber) {
        if (GopCurrentMode < Private->Gop->Mode->MaxMode) {
            Private->Gop->SetMode (Private->Gop, GopCurrentMode);
        }
        Private->Gop->SetMode (Private->Gop, Mode->GopModeNumber);
        if (Private->FlushFunc != TextOutFlush) {
            // fallback: direct set mode
        } else {
            Private->FlushFunc (Private);
        }
    }

    //
    * Update dimensions
    *
    if (Private->GopHorizontalResolution == OldHorizRes &&
        Private->GopVerticalResolution == OldVertRes) {
        Private->GopHorizontalResolution = TextCols;
        Private->GopVerticalResolution   = TextRows;
    }

    Private->MaxTextColumns = Mode->TextColumns;
    Private->MaxTextRows    = Mode->TextRows;
    Private->TextOutMode->ModeNumber = Mode->ModeNumber;

    SetCursorPosition (Private, 0, 0);

    if (CursorWasEnabled) {
        EnableCursor (Private);
    }

    //
    // (Re)allocate output line buffer
    //
    if (gOutputBuffer != NULL) {
        gBootServices_0->FreePool (gOutputBuffer);
    }
    gBootServices_0->AllocatePool (
                      EfiBootServicesData,
                      2 * (Private->MaxTextColumns + 1),
                      (VOID **)&gOutputBuffer
                      );
    gOutputBufferCount      = 0;
    gOutputBufferPixelWidth = 0;
    gOutputBufferMaxChars   = Private->MaxTextColumns + 1;

    return EFI_SUCCESS;
}

//=============================================================================
// EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.SetAttribute
//=============================================================================

EFI_STATUS
EFIAPI
TextOutSetAttribute(
    IN EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL *This,
    IN UINTN                           Attribute
    )
{
    return EFI_UNSUPPORTED;
}

//=============================================================================
// Internal Helper Functions
//=============================================================================

EFI_STATUS
FlushCursor(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private,
    IN UINT8                          CursorType
    )
{
    if ((CursorType | 0xFF) != 0xFF) {
        return EFI_UNSUPPORTED;
    }
    Private->TextOutMode->CursorVisible = (CursorType & 0x01) != 0;
    return EFI_SUCCESS;
}

EFI_STATUS
EnableCursor(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private
    )
{
    if (Private->GopResetRequired) {
        Private->Gop->SetMode (
                        Private->Gop,
                        Private->Modes[Private->TextOutMode->ModeNumber].GopModeNumber
                        );
    }
    Private->FlushFunc (Private);
    return EFI_SUCCESS;
}

EFI_STATUS
SetCursorPosition(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private,
    IN UINTN                          Column,
    IN UINTN                          Row
    )
{
    BOOLEAN CursorWasEnabled;

    if (Column >= Private->MaxTextColumns || Row >= Private->MaxTextRows) {
        return EFI_UNSUPPORTED;
    }

    CursorWasEnabled = Private->TextOutMode->CursorVisible;
    if (CursorWasEnabled) {
        Private->FlushStub (Private);
    }

    Private->TextOutMode->CursorColumn = (INT32)Column;
    Private->TextOutMode->CursorRow    = (INT32)Row;

    if (CursorWasEnabled) {
        EnableCursor (Private);
    }
    return EFI_SUCCESS;
}

EFI_STATUS
ClearScreen(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private,
    IN BOOLEAN                        ClearWholeScreen
    )
{
    BOOLEAN CursorWasVisible;

    CursorWasVisible = Private->TextOutMode->CursorVisible;
    if (CursorWasVisible != ClearWholeScreen) {
        Private->TextOutMode->CursorVisible = ClearWholeScreen;
        if (ClearWholeScreen) {
            ScrollDown (Private);
        }
        TextOutClearLine (Private, ClearWholeScreen);
    }
    return EFI_SUCCESS;
}

EFI_STATUS
GraphicsOutputFindMode(
    IN  EFI_GRAPHICS_OUTPUT_PROTOCOL *Gop,
    IN  UINT32                        HorizontalResolution,
    IN  UINT32                        VerticalResolution,
    OUT UINT32                       *GopModeNumber,
    IN  BOOLEAN                       ExactMatch,
    OUT UINT32                       *OutHorizontal,
    OUT UINT32                       *OutVertical
    )
{
    EFI_STATUS  Status;
    UINTN       ModeIndex;
    UINT32      BestWidth;
    UINT32      BestHeight;
    UINT32      BestMode;
    UINT64      BestArea;
    UINT64      ModeArea;
    UINT32      ModeWidth;
    UINT32      ModeHeight;
    EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *ModeInfo;
    UINTN                                 ModeInfoSize;

    if (Gop->Mode->MaxMode == 0) {
        return EFI_NOT_FOUND;
    }

    BestWidth  = 0;
    BestHeight = 0;
    BestMode   = 0;
    BestArea   = 0;

    for (ModeIndex = 0; ModeIndex < Gop->Mode->MaxMode; ModeIndex++) {
        ModeInfo = NULL;
        Status = Gop->QueryMode (Gop, ModeIndex, &ModeInfoSize, &ModeInfo);
        if (EFI_ERROR (Status)) {
            if (ModeInfo != NULL) {
                gBootServices_0->FreePool (ModeInfo);
            }
            continue;
        }

        ModeWidth  = ModeInfo->HorizontalResolution;
        ModeHeight = ModeInfo->VerticalResolution;

        if (HorizontalResolution && VerticalResolution) {
            if (ExactMatch) {
                if (ModeWidth == HorizontalResolution &&
                    ModeHeight == VerticalResolution) {
                    BestWidth  = ModeWidth;
                    BestHeight = ModeHeight;
                    BestMode   = (UINT32)ModeIndex;
                    gBootServices_0->FreePool (ModeInfo);
                    break;
                }
            } else if (ModeWidth >= HorizontalResolution &&
                       ModeHeight >= VerticalResolution) {
                ModeArea = (UINT64)ModeWidth * ModeHeight;
                if (ModeArea > BestArea) {
                    BestWidth  = ModeWidth;
                    BestHeight = ModeHeight;
                    BestMode   = (UINT32)ModeIndex;
                    BestArea   = ModeArea;
                }
            }
        } else {
            ModeArea = (UINT64)ModeWidth * ModeHeight;
            if (ModeArea > BestArea) {
                BestWidth  = ModeWidth;
                BestHeight = ModeHeight;
                BestMode   = (UINT32)ModeIndex;
                BestArea   = ModeArea;
            }
        }

        gBootServices_0->FreePool (ModeInfo);
    }

    if (BestWidth == 0 || BestHeight == 0) {
        return EFI_NOT_FOUND;
    }

    if (OutHorizontal != NULL) *OutHorizontal = BestWidth;
    if (OutVertical   != NULL) *OutVertical   = BestHeight;
    *GopModeNumber = BestMode;
    return EFI_SUCCESS;
}

EFI_STATUS
InitializeGraphicsMode(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private
    )
{
    UINTN           ModeIndex;
    UINT32          BestModeNum;
    UINT32          BestHoriz;
    UINT32          BestVert;
    UINT32          CharCols;
    UINT32          CharRows;
    BOOLEAN         NeedFallback;
    UINTN           FallbackIndex;
    EFI_STATUS      Status;

    NeedFallback   = FALSE;
    FallbackIndex  = 0;

    for (ModeIndex = 0; ModeIndex < GRAPHICS_CONSOLE_MODE_MAX; ModeIndex++) {
        GRAPHICS_CONSOLE_MODE_SOURCE *Src = &mModeSourceTable[ModeIndex];
        GRAPHICS_CONSOLE_MODE        *Dst = &Private->Modes[ModeIndex];
        UINT32 HRes = Src->HorizontalResolution;
        UINT32 VRes = Src->VerticalResolution;

        if (HRes == (UINT32)-1 || VRes == (UINT32)-1) {
            Status = GraphicsOutputFindMode (
                       Private->Gop, 0x280, 0x1DB,
                       &BestModeNum, FALSE, &BestHoriz, &BestVert
                       );
        } else {
            Status = GraphicsOutputFindMode (
                       Private->Gop, HRes, VRes,
                       &BestModeNum, FALSE, &BestHoriz, &BestVert
                       );
        }

        if (EFI_ERROR (Status)) {
            Dst->Supported  = FALSE;
            Dst->ModeNumber = Src->ModeNumber;
            if (Src->ModeNumber == 0) {
                NeedFallback  = TRUE;
                FallbackIndex = ModeIndex;
            }
            continue;
        }

        Dst->Supported    = TRUE;
        Dst->GopModeNumber = BestModeNum;
        Dst->ModeNumber   = Src->ModeNumber;

        CharCols = Src->TextColumns;
        CharRows = Src->TextRows;

        if (CharCols <= 1) CharCols = BestHoriz >> 3;
        if (CharRows <= 1) CharRows = BestVert / 19;

        Dst->TextColumns           = CharCols;
        Dst->TextRows              = CharRows;
        Dst->HorizontalResolution  = BestHoriz;
        Dst->VerticalResolution    = BestVert;

        if ((BestHoriz >> 3) < CharCols || (BestVert / 19) < CharRows) {
            Dst->Supported = FALSE;
        }

        if (Src->ModeNumber >= (UINT32)ModeIndex + 1) {
            Private->TextOutMode->MaxMode = Src->ModeNumber + 1;
        }
    }

    if (NeedFallback) {
        Status = GraphicsOutputFindMode (
                   Private->Gop, 0x280, 0x1DB,
                   &BestModeNum, FALSE, &BestHoriz, &BestVert
                   );
        if (EFI_ERROR (Status)) {
            Private->TextOutMode->MaxMode = 0;
            return EFI_UNSUPPORTED;
        }
        Private->Modes[FallbackIndex].Supported     = TRUE;
        Private->Modes[FallbackIndex].GopModeNumber = BestModeNum;
        Private->Modes[FallbackIndex].TextRows      = 25;
        Private->Modes[FallbackIndex].TextColumns   = 80;
        Private->Modes[FallbackIndex].ModeNumber    = mModeSourceTable[FallbackIndex].ModeNumber;
    }

    //
    // Dump mode table (debug output)
    //
    DEBUG ((EFI_D_INFO, " Mode#  Text Col  Text Row  Vid Col  Vid Row  Supported   GraphicsMode\n\r"));
    for (ModeIndex = 0; ModeIndex < (UINTN)Private->TextOutMode->MaxMode; ModeIndex++) {
        GRAPHICS_CONSOLE_MODE *Dst = &Private->Modes[ModeIndex];
        DEBUG ((EFI_D_INFO,
                " %5d %9d %9d %7d %7d %10d %15d\n\r",
                Dst->ModeNumber, Dst->TextColumns, Dst->TextRows,
                Dst->HorizontalResolution, Dst->VerticalResolution,
                Dst->Supported, Dst->GopModeNumber
                ));
    }

    return EFI_SUCCESS;
}

EFI_STATUS
ScrollUp(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private
    )
{
    UINT32 BgColor;
    UINT64 LinePixels;

    BgColor = mColorTable[(Private->TextOutMode->Attribute >> 4) & 0x0F];
    LinePixels = 8 * Private->MaxTextColumns;

    return Private->Gop->Blt (
                           Private->Gop, &BgColor,
                           EfiBltVideoFill, 0, 0, 0, 0,
                           0, 0,
                           Private->GopHorizontalResolution,
                           19 * Private->MaxTextRows - 19 + Private->GopVerticalResolution,
                           0, 0
                           );
}

EFI_STATUS
ScrollDown(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private
    )
{
    return Private->Gop->Blt (
                           Private->Gop, Private->LineBuffer,
                           EfiBltBufferToVideo,
                           Private->GopHorizontalResolution + 8 * Private->TextOutMode->CursorColumn,
                           19 * Private->TextOutMode->CursorRow + 15 + Private->GopVerticalResolution,
                           0, 0, 8, 3, 32
                           );
}

EFI_STATUS
ScrollRow(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private,
    IN UINT16                         PixelOffset
    )
{
    UINTN NewCol;
    UINTN NewRow;

    NewCol = PixelOffset + Private->TextOutMode->CursorColumn;
    if (NewCol < Private->MaxTextColumns) {
        return SetCursorPosition (Private, NewCol, Private->TextOutMode->CursorRow);
    }
    if (Private->TextOutMode->CursorRow == (INT32)Private->MaxTextRows - 1) {
        ScrollUp (Private);
        return SetCursorPosition (Private, 0, Private->TextOutMode->CursorRow);
    }
    NewRow = Private->TextOutMode->CursorRow + 1;
    return SetCursorPosition (Private, 0, NewRow);
}

EFI_STATUS
TextOutClearLine(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private,
    IN BOOLEAN                        ClearToEndOfLine
    )
{
    UINT32 AttrColor;

    if (!ClearToEndOfLine) {
        return Private->Gop->Blt (
                               Private->Gop, Private->LineBuffer,
                               EfiBltBufferToVideo,
                               Private->GopHorizontalResolution + 8 * Private->TextOutMode->CursorColumn,
                               19 * Private->TextOutMode->CursorRow + 15 + Private->GopVerticalResolution,
                               0, 0, 8, 3, 32
                               );
    }

    AttrColor = mColorTable[Private->TextOutMode->Attribute & 0x0F];
    return Private->Gop->Blt (
                           Private->Gop, &AttrColor,
                           EfiBltVideoFill, 0, 0, 0, 0,
                           0, 0,
                           Private->GopHorizontalResolution + 8 * Private->TextOutMode->CursorColumn,
                           15 + 19 * Private->TextOutMode->CursorRow + Private->GopVerticalResolution,
                           0, 0
                           );
}

EFI_STATUS
FlushCurrentLine(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private
    )
{
    EFI_STATUS       Status;
    UINT32           AttrColors[3];
    UINT8            BgAttr;
    UINT8            FgAttr;
    EFI_IMAGE_OUTPUT *Image;

    if (gOutputBufferCount == 0) {
        return EFI_SUCCESS;
    }

    BgAttr = (Private->TextOutMode->Attribute >> 4) & 0x0F;
    FgAttr = Private->TextOutMode->Attribute & 0x0F;

    AttrColors[0] = mColorTable[BgAttr];
    AttrColors[1] = mColorTable[FgAttr];
    AttrColors[2] = 7;

    gOutputBuffer[gOutputBufferCount] = 0;
    Image = NULL;

    Status = Private->HiiFont->StringToImage (
                                 Private->HiiFont,
                                 EFI_HII_OUT_FLAG_CLIP |
                                 EFI_HII_OUT_FLAG_WRAP,
                                 gOutputBuffer,
                                 AttrColors,
                                 &Image,
                                 0, 0, 0, 0, 0
                                 );
    //
    // Flush cursor stub with width info
    //
    Private->FlushStub (Private, (UINT16)Image->Width, (UINT16)Image->Height,
                        Image->Image);

    //
    // Blit to GOP framebuffer
    //
    Private->Gop->Blt (
                    Private->Gop, Image->Image,
                    EfiBltBufferToVideo,
                    0, 0,
                    Private->GopHorizontalResolution + 8 * Private->TextOutMode->CursorColumn,
                    Private->GopVerticalResolution + 19 * Private->TextOutMode->CursorRow,
                    Image->Width, Image->Height,
                    Image->Width * 4
                    );

    ScrollRow (Private, Image->Width >> 3);

    gBootServices_0->FreePool (Image->Image);
    gBootServices_0->FreePool (Image);

    gOutputBufferCount      = 0;
    gOutputBufferPixelWidth = 0;
    return EFI_SUCCESS;
}

EFI_STATUS
TextOutSetModeCallback(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private,
    IN UINT8                         *InputBuffer,
    IN UINT8                          Attribute,
    IN VOID                          *GopBltBuffer,
    IN UINTN                          Alignment,
    IN BOOLEAN                        DeltaUpdate
    )
{
    EFI_IMAGE_OUTPUT *Image;
    UINT32            TotalPixelWidth;
    UINT32            CharPixelWidth;
    UINT32           *RenderBuf;
    UINTN             Row;
    UINTN             Col;
    UINTN             BufIdx;
    UINTN             SrcIdx;
    UINT32            AttrColors[2];
    UINT32            Start;
    UINT32            Mid;
    UINT32            End;
    EFI_STATUS        Status;

    Private = (GRAPHICS_CONSOLE_PRIVATE_DATA *)((UINT8 *)Private - 257);

    if (Attribute == 0) {
        Attribute = Private->TextOutMode->Attribute;
    }
    AttrColors[0] = mColorTable[Attribute & 0x0F];
    AttrColors[1] = mColorTable[(Attribute >> 4) & 0x0F];

    Image = NULL;
    Status = Private->HiiFont->StringToImage (
                                 Private->HiiFont, 0, InputBuffer,
                                 AttrColors, &Image,
                                 0, 0, 0, 0, 0
                                 );
    if (EFI_ERROR (Status)) {
        return EFI_INVALID_PARAMETER;
    }

    TotalPixelWidth = 8 * Private->MaxTextColumns;
    CharPixelWidth  = Image->Width;

    if (CharPixelWidth > TotalPixelWidth || Image->Height != 19) {
        gBootServices_0->FreePool (Image->Image);
        gBootServices_0->FreePool (Image);
        return EFI_INVALID_PARAMETER;
    }

    switch (Alignment) {
    case 0: Start = 0;               break;
    case 1: Start = (TotalPixelWidth - CharPixelWidth) >> 1; break;
    case 2: Start = TotalPixelWidth - CharPixelWidth;        break;
    default:
        gBootServices_0->FreePool (Image->Image);
        gBootServices_0->FreePool (Image);
        return EFI_INVALID_PARAMETER;
    }
    Mid = Start + CharPixelWidth;
    End = Start + CharPixelWidth;

    Status = gBootServices_0->AllocatePool (
                                EfiBootServicesData,
                                4 * TotalPixelWidth * 19,
                                (VOID **)&RenderBuf
                                );
    if (EFI_ERROR (Status)) {
        gBootServices_0->FreePool (Image->Image);
        gBootServices_0->FreePool (Image);
        return Status;
    }

    for (Row = 0; Row < 19; Row++) {
        BufIdx = Row * TotalPixelWidth;
        for (Col = 0; Col < Start; Col++) {
            RenderBuf[BufIdx + Col] = AttrColors[1];
        }
        for (Col = Start; Col < Mid; Col++) {
            SrcIdx = (Col - Start) + Row * CharPixelWidth;
            RenderBuf[BufIdx + Col] = Image->Image[SrcIdx];
        }
        for (Col = Mid; Col < End; Col++) {
            RenderBuf[BufIdx + Col] = AttrColors[1];
        }
    }

    gBootServices_0->FreePool (Image->Image);
    gBootServices_0->FreePool (Image);

    if (DeltaUpdate) {
        Private->FlushStub (Private, TotalPixelWidth, 19, RenderBuf);
    }

    Private->Gop->Blt (Private->Gop, RenderBuf,
                       EfiBltBufferToVideo, 0, 0, 0, 0,
                       Private->GopHorizontalResolution,
                       Private->GopVerticalResolution, 2);

    gBootServices_0->FreePool (RenderBuf);
    return EFI_SUCCESS;
}

VOID
EFIAPI
TextOutFlushStub(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private
    )
{
    return;
}

EFI_STATUS
TextOutFlush(
    IN GRAPHICS_CONSOLE_PRIVATE_DATA *Private
    )
{
    UINT32 BgColor;
    UINT32 HorizRes;
    UINT32 VertRes;

    if (Private->MaxTextColumns != 0 && Private->MaxTextRows != 0) {
        HorizRes = Private->GopHorizontalResolution;
        VertRes  = Private->GopVerticalResolution;
    } else {
        HorizRes = Private->Gop->Mode->Info->HorizontalResolution;
        VertRes  = Private->Gop->Mode->Info->VerticalResolution;
    }

    BgColor = mColorTable[(Private->TextOutMode->Attribute >> 4) & 0x0F];

    Private->Gop->Blt (Private->Gop, &BgColor,
                       EfiBltVideoFill, 0, 0, 0, 0,
                       0, 0, HorizRes, VertRes, 0, 0);

    SetCursorPosition (Private, 0, 0);
    return EFI_SUCCESS;
}

BOOLEAN
IsValidChar (
    VOID
    )
{
    if (gHiiFontInterface != NULL) {
        return gHiiFontInterface != 0;
    }

    UINTN PoolSize;
    PoolSize = gBootServices->CalculateTimerValue (31);  // TPL check
    if (PoolSize <= 16) {
        EFI_STATUS Status;
        Status = gBootServices_0->LocateProtocol (
                                    &gEfiHiiFontProtocolGuid,
                                    NULL,
                                    (VOID **)&gHiiFontInterface
                                    );
        if (EFI_ERROR (Status)) {
            gHiiFontInterface = 0;
        }
        return gHiiFontInterface != 0;
    }
    return FALSE;
}