/**
 * @file 794E15D9-BF1B-4568-99AC-DCE207C022E4.c
 * @brief ASPEED Graphics DXE Driver - Decompiled Source
 *
 * Source: c:\edk2\OptionRomPkg\ASPEEDGraphicsDxe\
 * Files:  ASPEEDGraphics.c, ASPEEDGraphicsOutput.c, ASPEEDGraphicsModes.c
 *
 * This is the decompiled and manually renamed source for the ASPEED AST2xxx
 * UEFI Graphics Output Protocol (GOP) driver.
 *
 * NOTE: IDA Pro was not available at write time. Function names and structs
 * have been reconstructed from decompilation data. Apply renames to IDA
 * using the rename table in the accompanying .md file.
 */

#include <Uefi.h>
#include <Protocol/GraphicsOutput.h>
#include <Protocol/DriverBinding.h>
#include <Protocol/ComponentName2.h>
#include <Protocol/DevicePath.h>
#include <Library/UefiBootServicesTableLib.h>
#include <Library/UefiLib.h>

//
// GUID Definitions
//
EFI_GUID gAspGraphicsOutputProtocolGuid =
    { 0x9042A9DE, 0x23DC, 0x4A38, { 0x96, 0xFB, 0x7A, 0xDE, 0xD0, 0x80, 0x51, 0x6A } };

EFI_GUID gAspVideoProtocolGuid =
    { 0x4CF5B200, 0x68B8, 0x4CA5, { 0x9E, 0xEC, 0xB2, 0x3E, 0x3F, 0x50, 0x02, 0x9A } };

//
// Module Globals
//
EFI_HANDLE  gImageHandle = NULL;
EFI_SYSTEM_TABLE *gSystemTable = NULL;
EFI_BOOT_SERVICES *gBootServices = NULL;
EFI_RUNTIME_SERVICES *gRuntimeServices = NULL;

//
// Forward declarations
//
EFI_STATUS
EFIAPI
AspGraphicsDriverBindingSupported (
    IN EFI_DRIVER_BINDING_PROTOCOL  *This,
    IN EFI_HANDLE                   ControllerHandle,
    IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
    );

EFI_STATUS
EFIAPI
AspGraphicsDriverBindingStart (
    IN EFI_DRIVER_BINDING_PROTOCOL  *This,
    IN EFI_HANDLE                   ControllerHandle,
    IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
    );

EFI_STATUS
EFIAPI
AspGraphicsDriverBindingStop (
    IN EFI_DRIVER_BINDING_PROTOCOL  *This,
    IN EFI_HANDLE                   ControllerHandle,
    IN UINTN                        NumberOfChildren,
    IN EFI_HANDLE                   *ChildBuffer
    );

//
// Component Name prototype
//
EFI_STATUS
EFIAPI
AspGraphicsComponentNameGetDriverName (
    IN  EFI_COMPONENT_NAME2_PROTOCOL  *This,
    IN  CHAR8                         *Language,
    OUT CHAR16                        **DriverName
    );

EFI_STATUS
EFIAPI
AspGraphicsComponentNameGetControllerName (
    IN  EFI_COMPONENT_NAME2_PROTOCOL  *This,
    IN  EFI_HANDLE                    ControllerHandle,
    IN  EFI_HANDLE                    ChildHandle,
    IN  CHAR8                         *Language,
    OUT CHAR16                        **ControllerName
    );

//
// Driver Binding Protocol instance
//
EFI_DRIVER_BINDING_PROTOCOL gAspGraphicsDriverBinding = {
    AspGraphicsDriverBindingSupported,
    AspGraphicsDriverBindingStart,
    AspGraphicsDriverBindingStop,
    0x10,           // Version
    NULL,           // ImageHandle (set at entry)
    NULL            // DriverBindingHandle (set at entry)
};

//
// Component Name Protocol instance
//
EFI_COMPONENT_NAME2_PROTOCOL gAspGraphicsComponentName2 = {
    AspGraphicsComponentNameGetDriverName,
    AspGraphicsComponentNameGetControllerName,
    "en"            // Supported Languages
};

//=============================================================================
// Module Entry Point
//=============================================================================

EFI_STATUS
EFIAPI
_ModuleEntryPoint (
    IN EFI_HANDLE        ImageHandle,
    IN EFI_SYSTEM_TABLE *SystemTable
    )
{
    gImageHandle = ImageHandle;
    ASSERT (ImageHandle != NULL);
    gSystemTable = SystemTable;
    ASSERT (SystemTable != NULL);
    gBootServices = SystemTable->BootServices;
    ASSERT (gBootServices != NULL);
    gRuntimeServices = SystemTable->RuntimeServices;
    ASSERT (gRuntimeServices != NULL);

    return AspGraphicsDriverEntry (ImageHandle);
}

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

EFI_STATUS
EFIAPI
AspGraphicsDriverEntry (
    IN EFI_HANDLE ImageHandle
    )
{
    EFI_STATUS  Status;
    EFI_HANDLE  DriverImageHandle = ImageHandle;
    UINTN       N131082 = 131082;   // Driver binding version

    //
    // Install Driver Binding Protocol + Component Name + ASPEED Video Protocol
    //
    Status = gBootServices->InstallMultipleProtocolInterfaces (
                &DriverImageHandle,
                &gAspGraphicsDriverBinding,
                gAspGraphicsDriverBinding.Supported,  // ComponentName2
                &gAspVideoProtocolGuid,
                &gAspGraphicsComponentName2,
                NULL,               // DevicePath
                NULL
                );

    if (EFI_ERROR (Status)) {
        DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
        ASSERT (!EFI_ERROR (Status));
    }

    //
    // Install Component Name protocol
    //
    Status = gBootServices->InstallMultipleProtocolInterfaces (
                &DriverImageHandle,
                &gAspVideoProtocolGuid,
                NULL
                );

    if (EFI_ERROR (Status)) {
        DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
        ASSERT (!EFI_ERROR (Status));
    }

    return Status;
}

//=============================================================================
// Driver Binding Protocol: Supported
//=============================================================================

EFI_STATUS
EFIAPI
AspGraphicsDriverBindingSupported (
    IN EFI_DRIVER_BINDING_PROTOCOL  *This,
    IN EFI_HANDLE                   ControllerHandle,
    IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
    )
{
    EFI_STATUS  Status;

    if (RemainingDevicePath != NULL) {
        //
        // Open the ASPEED video protocol to check if this controller
        // is already managed by us
        //
        Status = gBootServices->OpenProtocol (
                    *RemainingDevicePath,       // RemainingDevicePath treated as child
                    &gAspVideoProtocolGuid,
                    NULL,                       // Interface
                    This->DriverBindingHandle,
                    ControllerHandle,
                    EFI_OPEN_PROTOCOL_BY_DRIVER
                    );

        if (!EFI_ERROR (Status)) {
            //
            // Already have the protocol - check signature
            //
            ASPEED_PRIVATE_DATA *Private = CR (Interface, ASPEED_PRIVATE_DATA, ProtocolHandle, ASPEED_PRIVATE_DATA_SIGNATURE);
            if (Private->ProtocolHandle != NULL) {
                gBootServices->FreePool (Private->ProtocolHandle);
            }
            gBootServices->CloseProtocol (
                ControllerHandle,
                &gAspVideoProtocolGuid,
                This->DriverBindingHandle,
                ControllerHandle
                );
        }
        return EFI_UNSUPPORTED;
    }

    //
    // Check if the controller supports the ASPEED video protocol
    // by trying to open it
    //
    Status = gBootServices->OpenProtocol (
                ControllerHandle,
                &gAspVideoProtocolGuid,
                NULL,
                This->DriverBindingHandle,
                ControllerHandle,
                EFI_OPEN_PROTOCOL_TEST_PROTOCOL
                );

    if (Status == EFI_ALREADY_STARTED) {
        return EFI_UNSUPPORTED;
    }

    if (Status != EFI_UNSUPPORTED) {
        return EFI_UNSUPPORTED;
    }

    return AspGraphicsCheckPciDevice (ControllerHandle);
}

//=============================================================================
// Driver Binding Protocol: Start
//=============================================================================

EFI_STATUS
EFIAPI
AspGraphicsDriverBindingStart (
    IN EFI_DRIVER_BINDING_PROTOCOL  *This,
    IN EFI_HANDLE                   ControllerHandle,
    IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
    )
{
    ASPEED_PRIVATE_DATA *Private;
    EFI_STATUS           Status;
    UINT8                DisplayDetect;
    UINT16               ModeOverride[8];

    //
    // Allocate private context structure (752 bytes)
    //
    Private = (ASPEED_PRIVATE_DATA *)AspAllocateZeroPool (sizeof (ASPEED_PRIVATE_DATA));
    if (Private == NULL) {
        return EFI_OUT_OF_RESOURCES;
    }

    //
    // Initialize signature
    //
    Private->Signature = ASPEED_PRIVATE_DATA_SIGNATURE;

    //
    // Open PCI IO protocol on the controller
    //
    Status = gBootServices->OpenProtocol (
                ControllerHandle,
                &gEfiPciIoProtocolGuid,
                (VOID **)&Private->PciIo,
                This->ImageHandle,
                ControllerHandle,
                EFI_OPEN_PROTOCOL_BY_DRIVER | EFI_OPEN_PROTOCOL_EXCLUSIVE
                );

    if (EFI_ERROR (Status)) {
        goto ErrorExit;
    }

    //
    // Initialize ASPEED hardware (enable memory space, detect display type)
    //
    AspInitDisplayHardware (Private);

    //
    // Detect display type using scratch registers
    // First check if display override provided
    //
    if (RemainingDevicePath == NULL) {
        //
        // Read scratch register for display type detection
        //
        DisplayDetect = AspCrtcRead (Private, ASPEED_REG_SEQUENCER);
        if ((DisplayDetect & ASPEED_SCRATCH_DISPLAY_VGA) == 0) {
            //
            // No display detected - check for DPMS override
            //
        }
    }

    //
    // Build mode table from EDID
    //
    Status = AspInitModeTable (Private);
    if (EFI_ERROR (Status)) {
        goto ErrorExit;
    }

    //
    // Install GOP protocol (QueryMode, SetMode, Blt)
    //
    Status = AspInstallGopProtocol (Private);
    if (EFI_ERROR (Status)) {
        DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
        ASSERT (!EFI_ERROR (Status));
    }

    //
    // Install protocols on the controller
    //
    Status = gBootServices->InstallMultipleProtocolInterfaces (
                &ControllerHandle,
                &gAspVideoProtocolGuid,
                Private + 2,                  // ASPEED protocol instance
                &gEfiDevicePathProtocolGuid,
                Private[15],                  // Device path
                NULL
                );

    if (!EFI_ERROR (Status)) {
        Status = gBootServices->InstallMultipleProtocolInterfaces (
                    &ControllerHandle,
                    &gAspGraphicsOutputProtocolGuid,
                    Private + 7,              // GOP protocol
                    &gAspVideoProtocolGuid,
                    Private + 11,
                    &gAspVideoProtocolGuid,
                    Private + 13,
                    NULL
                    );
    }

    //
    // Install the protocol on the child handle (RemainingDevicePath)
    //
    Status = gBootServices->InstallProtocolInterface (
                &ControllerHandle,
                &gAspVideoProtocolGuid,
                EFI_NATIVE_INTERFACE,
                Private + 2
                );

    return Status;

ErrorExit:
    //
    // Cleanup on failure
    //
    if (Private->PciIo != NULL) {
        if (Private[3] != 0) {
            AspMmioWrite32 (Private, 2, Private[3]);  // restore PCI config
        }
        gBootServices->CloseProtocol (
            ControllerHandle,
            &gAspVideoProtocolGuid,
            This->DriverBindingHandle,
            ControllerHandle
            );
    }
    AspSafeFreePool (Private);

    return Status;
}

//=============================================================================
// Driver Binding Protocol: Stop
//=============================================================================

EFI_STATUS
EFIAPI
AspGraphicsDriverBindingStop (
    IN EFI_DRIVER_BINDING_PROTOCOL  *This,
    IN EFI_HANDLE                   ControllerHandle,
    IN UINTN                        NumberOfChildren,
    IN EFI_HANDLE                   *ChildBuffer
    )
{
    EFI_STATUS  Status;
    UINT64      *Private;
    UINT16      Config[32];   // 32 bytes for PCI config read
    UINT64      v7;

    //
    // Open the ASPEED video protocol on the controller
    //
    Status = gBootServices->OpenProtocol (
                ControllerHandle,
                &gAspVideoProtocolGuid,
                (VOID **)&v7,
                This->DriverBindingHandle,
                ControllerHandle,
                EFI_OPEN_PROTOCOL_GET_PROTOCOL
                );

    if (EFI_ERROR (Status)) {
        return Status;
    }

    Private = (UINT64 *)(v7 - 56);  // CR: back to private data

    //
    // Restore PCI configuration
    //
    if (Private[10] != 0) {   // saved PCI config
        if (*(UINT64 *)(Private[10] + 8) != 0) {
            AspSafeFreePool ((VOID *)Private[10]);
        }
        AspSafeFreePool ((VOID *)Private[10]);
    }

    //
    // Check if we need to restore PCI config
    //
    Status = AspGraphicsCheckAndVerify (ControllerHandle, v7);
    if (EFI_ERROR (Status)) {
        return Status;
    }

    //
    // Close and restore
    //
    AspSafeFreePool ((VOID *)Private[2]);    // close PCI IO
    gBootServices->CloseProtocol (
        ControllerHandle,
        &gAspVideoProtocolGuid,
        This->DriverBindingHandle,
        ControllerHandle
        );
    AspSafeFreePool (Private);

    return EFI_SUCCESS;
}

//=============================================================================
// Graphics Output Protocol: QueryMode
//=============================================================================

EFI_STATUS
EFIAPI
AspGraphicsQueryMode (
    IN  EFI_GRAPHICS_OUTPUT_PROTOCOL        *This,
    IN  UINT32                              ModeNumber,
    OUT UINTN                               *SizeOfInfo,
    OUT EFI_GRAPHICS_OUTPUT_MODE_INFORMATION **Info
    )
{
    ASPEED_PRIVATE_DATA *Private;
    EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *ModeInfo;

    //
    // CR: Get private data from GOP protocol (GOP is at Private+56)
    //
    Private = CR (This, ASPEED_PRIVATE_DATA, GopMode, ASPEED_PRIVATE_DATA_SIGNATURE);

    if (Private->ModeInvalid) {
        return EFI_NOT_STARTED;
    }

    if ((Info == NULL) || (SizeOfInfo == NULL) || (ModeNumber >= *This->Mode->MaxMode)) {
        return EFI_INVALID_PARAMETER;
    }

    //
    // Allocate mode info structure
    //
    ModeInfo = AspAllocatePool (sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
    if (ModeInfo == NULL) {
        return EFI_OUT_OF_RESOURCES;
    }

    *Info = ModeInfo;
    *SizeOfInfo = sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION);

    //
    // Fill in mode info from the mode configuration table
    //
    ModeInfo->HorizontalResolution = Private->ModeConfigTable[ModeNumber].HorizontalResolution;
    ModeInfo->VerticalResolution   = Private->ModeConfigTable[ModeNumber].VerticalResolution;

    //
    // Set pixel format and bitmasks based on color depth
    //
    AspFillModeInfo (Private, ModeNumber, ModeInfo);

    return EFI_SUCCESS;
}

//=============================================================================
// Graphics Output Protocol: SetMode
//=============================================================================

EFI_STATUS
EFIAPI
AspGraphicsSetMode (
    IN EFI_GRAPHICS_OUTPUT_PROTOCOL  *This,
    IN UINT32                        ModeNumber
    )
{
    ASPEED_PRIVATE_DATA *Private;
    EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *ModeInfo;
    UINT32  HorizontalResolution;
    UINT32  VerticalResolution;
    UINT32  ColorDepth;
    UINTN   FrameBufferSize;
    VOID    *FreeBuffer;

    //
    // CR: Get private data
    //
    Private = CR (This, ASPEED_PRIVATE_DATA, GopMode, ASPEED_PRIVATE_DATA_SIGNATURE);

    if (ModeNumber >= *This->Mode->MaxMode) {
        return EFI_UNSUPPORTED;
    }

    //
    // Free existing frame buffer if any
    //
    FreeBuffer = (VOID *)(UINTN)Private->FrameBufferBase;
    if (FreeBuffer != NULL) {
        gBootServices->FreePages (Private->FrameBufferBase, 0);
    }
    Private->FrameBufferBase = 0;

    //
    // Calculate frame buffer size
    //
    HorizontalResolution = Private->ModeConfigTable[ModeNumber].HorizontalResolution;
    VerticalResolution   = Private->ModeConfigTable[ModeNumber].VerticalResolution;
    ColorDepth           = Private->ModeConfigTable[ModeNumber].ColorDepth;

    FrameBufferSize = HorizontalResolution * VerticalResolution * (ColorDepth / 8);

    //
    // Allocate frame buffer
    //
    Status = gBootServices->AllocatePages (
                AllocateAnyPages,
                EfiBootServicesData,
                EFI_SIZE_TO_PAGES (FrameBufferSize),
                &Private->FrameBufferBase
                );

    if (EFI_ERROR (Status)) {
        return EFI_OUT_OF_RESOURCES;
    }

    //
    // Program the CRTC registers for the requested mode
    //
    AspProgramCrtcRegisters (Private, &gAspModeTable[ModeNumber]);

    //
    // Update mode structure
    //
    This->Mode->Mode = ModeNumber;
    This->Mode->Info->HorizontalResolution = HorizontalResolution;
    This->Mode->Info->VerticalResolution   = VerticalResolution;
    This->Mode->Info->PixelsPerScanLine    = HorizontalResolution;
    This->Mode->SizeOfInfo = sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION);

    //
    // Fill pixel format
    //
    ModeInfo = This->Mode->Info;
    AspFillModeInfo (Private, ModeNumber, ModeInfo);

    //
    // Configure the frame buffer address in hardware
    //
    This->Mode->FrameBufferSize = FrameBufferSize;
    This->Mode->FrameBufferBase = Private->FrameBufferBase;

    //
    // Program hardware registers
    //
    AspGopConfigureFbAddress (Private, This->Mode);

    Private->ModeInvalid = 0;

    return EFI_SUCCESS;
}

//=============================================================================
// Graphics Output Protocol: Blt (BitBLT)
//=============================================================================

EFI_STATUS
EFIAPI
AspGraphicsBlt (
    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
    )
{
    ASPEED_PRIVATE_DATA *Private;
    UINT8               *FbBase;
    UINT32              FbWidth;
    UINT32              FbHeight;
    UINT32              FbBpp;
    UINT32              FbStride;
    UINT8               *FbAddress;
    UINTN               SrcY;
    UINTN               DstY;
    EFI_GRAPHICS_OUTPUT_BLT_PIXEL  *Blt;

    //
    // CR: Get private data
    //
    Private = CR (This, ASPEED_PRIVATE_DATA, GopMode, ASPEED_PRIVATE_DATA_SIGNATURE);

    if (BltOperation > EFI_GRAPHICS_OUTPUT_BLT_OPERATION_MAX) {
        return EFI_INVALID_PARAMETER;
    }

    if ((Width == 0) || (Height == 0)) {
        return EFI_INVALID_PARAMETER;
    }

    //
    // Check bounds
    //
    FbWidth  = This->Mode->Info->HorizontalResolution;
    FbHeight = This->Mode->Info->VerticalResolution;
    FbBpp    = This->Mode->Info->PixelsPerScanLine;

    if (Delta == 0) {
        Delta = Width * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL);
    }

    switch (BltOperation) {
    case EfiBltVideoFill:
        //
        // Video Fill: Fill a rectangle with a solid color
        //
        if ((BltBuffer == NULL) || (DestinationX + Width  > FbWidth) ||
                                   (DestinationY + Height > FbHeight)) {
            return EFI_INVALID_PARAMETER;
        }

        // Fill via VideoToVideo or direct draw -- implemented with
        // BltToVideo of a 1-pixel wide brush

    case EfiBltVideoToBlt:
        //
        // Video to BLT Buffer: Read from frame buffer
        //
        if ((BltBuffer == NULL) || (SourceX + Width > FbWidth) ||
                                   (SourceY + Height > FbHeight)) {
            return EFI_INVALID_PARAMETER;
        }

        FbAddress = (UINT8 *)(UINTN)This->Mode->FrameBufferBase;
        FbAddress += SourceY * FbWidth * (FbBpp / 8) + SourceX * (FbBpp / 8);

        // Copy line by line (bottom-up in VGA)
        Blt = BltBuffer;
        for (SrcY = 0; SrcY < Height; SrcY++) {
            gBS->CopyMem (Blt, FbAddress + (SrcY * FbWidth * (FbBpp / 8)), Width * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
            Blt += Delta / sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL);
        }
        break;

    case EfiBltVideoToVideo:
        //
        // Video to Video: Copy within frame buffer
        //
        // Hardware accelerated via ASPEED FIFO registers
        AspMmioWrite32 (Private, ASPEED_MMIO_OFFSET0, 0);
        AspMmioWrite32 (Private, ASPEED_MMIO_OFFSET1, FbStride << 16);
        AspMmioWrite32 (Private, ASPEED_MMIO_OFFSET2, 0);
        AspMmioWrite32 (Private, ASPEED_MMIO_OFFSET3, (Width & FbWidth) | (FbStride << 16));
        // ... hardware BLT dispatch
        break;

    // ... additional Blt operations
    }

    return EFI_SUCCESS;
}

//=============================================================================
// Hardware initialization sequence
//=============================================================================

UINT64
AspInitDisplayHardware (
    IN ASPEED_PRIVATE_DATA  *Private
    )
{
    UINT8   n35;
    UINT16  RegPair[2];
    UINT32  RegVal;
    UINT32  Status;
    UINT8   Index;
    UINT8   *PaletteData;

    //
    // Step 1: Mask sequencer interrupts
    //
    n35 = 1;
    AspCrtcWrite16 (Private, ASPEED_REG_SEQUENCER, 67, &n35);

    //
    // Step 2: Program CRTC clocking (register 0x42 = 35, CRTC index)
    //
    n35 = 35;
    AspCrtcWrite16 (Private, ASPEED_REG_CRTC, 66, &n35);

    //
    // Step 3: Set horizontal total (register 0x42 = offset 84 = HTOTAL)
    //
    RegPair[0] = 640 - 1;        // HTOTAL
    AspCrtcWrite16 (Private, ASPEED_REG_CRTC, 84, RegPair);

    //
    // Step 4: Set horizontal display enable end
    //
    RegPair[0] = 640 - 1 + 640 + 1185;  // typical ASPEED timing
    AspCrtcWrite16 (Private, ASPEED_REG_CRTC, 84, RegPair);

    //
    // Step 5: Clear vertical sync interrupt
    //
    AspCrtcWrite (Private, ASPEED_REG_SEQUENCER, 964, 33);
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 980, 1);

    //
    // Step 6: Program gamma/palette for index values 0x81-0x9E
    //
    for (Index = 0x81; Index <= 0x9E; Index++) {
        AspCrtcWrite (Private, ASPEED_REG_CRTC, 980, Index);
        AspCrtcWrite (Private, ASPEED_REG_CRTC, 981, 0);
    }
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 980, 0x99);
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 981, 0);

    //
    // Step 7: Palette table (VGA DAC data at 0x6C70)
    //
    PaletteData = (UINT8 *)&unk_6C70;
    for (Index = 0xA0; *PaletteData != 0xFF; Index++, PaletteData++) {
        AspCrtcWrite (Private, ASPEED_REG_CRTC, 980, Index);
        AspCrtcWrite (Private, ASPEED_REG_CRTC, 981, *PaletteData);
    }

    //
    // Step 8: Enable CRTC display
    //
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 980, 0x8C);
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 981, 1);

    //
    // Step 9: Set display mode flags
    //
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 980, 0x90);
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 981, 0x08);

    AspCrtcWrite (Private, ASPEED_REG_CRTC, 980, 0xB7);
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 981, AspCrtcRead (Private, 980, 0xB7) | 0x24);

    //
    // Step 10: Check if display mode register needs PLL programming
    //
    if ((AspCrtcRead (Private, ASPEED_REG_CRTC, 980, 0xD0) & 0xC0) == 0) {
        //
        // Program PLL registers (MMIO at 0x1E600000 + offset)
        //
        AspMmioWriteMask (Private, ASPEED_HW_BASE + 0x1E60000, 0xAE000000);
        AspMmioWriteMask (Private, ASPEED_HW_BASE + 0x1E60044, 0x10000);

        // ... PLL configuration continues
    }

    //
    // Step 11: Poll display status ready
    //
    AspPollVgaStatus (Private);

    //
    // Step 12: Program misc CRTC register 0xB3
    //
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 980, 0xCF);
    // ... additional register init

    //
    // Step 13: Set GPIO/strap register for display
    //
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 980, 0xA4);
    AspCrtcWrite (Private, ASPEED_REG_CRTC, 981, AspCrtcRead (Private, ASPEED_REG_CRTC, 980, 0xA4) | 1);

    //
    // Step 14: Enable MMIO access
    //
    AspMmioWrite32 (Private, ASPEED_MMIO_CTRL, 0xF2000000);

    //
    // Step 15: Restore DPMS
    //
    AspDpmsRestore (Private);

    return EFI_SUCCESS;
}

//=============================================================================
// Mode table initialization
//=============================================================================

EFI_STATUS
AspInitModeTable (
    IN ASPEED_PRIVATE_DATA  *Private
    )
{
    EFI_STATUS  Status;
    UINTN       NumModes;
    UINTN       ModeIndex;
    UINT8       *ModeEntry;
    UINT8       DacStatus;
    UINT8       DisplayFlags;
    UINT8       MonitorType;
    UINT8       MonitorTypeAnalog;
    UINT8       MonitorTypeDigital;

    //
    // Get EDID (Extended Display Identification Data)
    //
    Private->NumModes     = 0;
    Private->ModeConfig   = NULL;
    Private->EdidModeCount = 0;
    Private->EdidModes    = NULL;

    //
    // Attempt EDID retrieval
    //
    Status = AspGetEdidInfo (Private, &MonitorType, &NumModes);

    if (EFI_ERROR (Status)) {
        //
        // EDID unavailable, use built-in mode table
        //
        NumModes = 0;
        MonitorType = 0;
    }

    //
    // Detect display type from scratch register
    //
    DacStatus = AspCrtcRead (Private, ASPEED_REG_CRTC, 980, 0xD0);

    //
    // Set display flags based on available monitors
    //
    MonitorTypeAnalog  = 0;
    MonitorTypeDigital = 0;

    if ((DacStatus & 0x81) == 0x80) {
        // Analog detected
        MonitorTypeAnalog = 1;
    }

    //
    // Filter modes: each mode entry has flags indicating compatibility
    //
    ModeEntry = (UINT8 *)&gAspModeTable[0];
    ModeIndex = 0;

    for (UINTN Mode = 0; Mode < ASPEED_MAX_MODES; Mode++) {
        UINT16  Width        = *(UINT16 *)(ModeEntry + 4);
        UINT16  Height       = *(UINT16 *)(ModeEntry + 6);
        UINT32  Refresh      = *(UINT32 *)(ModeEntry + 12);
        UINT8   ModeFlag     = *(ModeEntry + 20);

        //
        // Filter mode by capabilities:
        // - Skip modes exceeding monitor limits
        // - Check analog/digital compatibility
        // - Check color depth
        //
        if (((ModeFlag & ASPEED_MODE_FLAG_ANALOG) && !MonitorTypeAnalog) ||
            ((ModeFlag & ASPEED_MODE_FLAG_DIGITAL) && !MonitorTypeDigital)) {
            // Skip this mode
        } else if (Width > 1920 || Height > 1200) {
            // Exceeds known hardware limits
        } else {
            //
            // Add mode to the private mode table
            //
            Private->ModeConfigTable[ModeIndex].ModeIndex            = Mode;
            Private->ModeConfigTable[ModeIndex].HorizontalResolution = Width;
            Private->ModeConfigTable[ModeIndex].VerticalResolution   = Height;
            Private->ModeConfigTable[ModeIndex].ColorDepth           = 32;
            Private->ModeConfigTable[ModeIndex].RefreshRate          = Refresh;
            ModeIndex++;
        }
    }

    Private->NumModes = ModeIndex;

    return EFI_SUCCESS;
}

//=============================================================================
// Install the Graphics Output Protocol
//=============================================================================

EFI_STATUS
AspInstallGopProtocol (
    IN ASPEED_PRIVATE_DATA  *Private
    )
{
    EFI_STATUS  Status;
    VOID        *GopModeInfo;

    //
    // Set GOP function table
    //
    Private->GopMode.GraphicsOutput.QueryMode = AspGraphicsQueryMode;
    Private->GopMode.GraphicsOutput.SetMode   = AspGraphicsSetMode;
    Private->GopMode.GraphicsOutput.Blt       = AspGraphicsBlt;

    //
    // Allocate mode structure
    //
    Status = gBootServices->AllocatePool (
                EfiBootServicesData,
                sizeof (EFI_GRAPHICS_OUTPUT_PROTOCOL_MODE),
                &GopModeInfo
                );

    if (EFI_ERROR (Status)) {
        return Status;
    }

    //
    // Allocate mode info structure within the GOP mode
    //
    Status = gBootServices->AllocatePool (
                EfiBootServicesData,
                sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION),
                &((EFI_GRAPHICS_OUTPUT_PROTOCOL_MODE *)GopModeInfo)->Info
                );

    if (EFI_ERROR (Status)) {
        return Status;
    }

    //
    // Initialize GOP mode structure
    //
    *((UINT32 *)GopModeInfo) = Private->NumModes;         // MaxMode
    *((UINT32 *)((EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *)((EFI_GRAPHICS_OUTPUT_PROTOCOL_MODE *)GopModeInfo)->Info) + 1) = 0xFFFF;  // Mode (invalid)

    Private->FrameBufferBase = 0;
    Private->ModeInvalid     = 1;                         // No mode set yet

    //
    // Set mode to mode 0 (default)
    //
    AspGraphicsSetMode (
        &Private->GopMode.GraphicsOutput,
        0
        );

    return EFI_SUCCESS;
}