//;******************************************************************************
//;* Copyright (c) 1983-2011, Insyde Software Corporation. All Rights Reserved.
//;*
//;* You may not reproduce, distribute, publish, display, perform, modify, adapt,
//;* transmit, broadcast, present, recite, release, license or otherwise exploit
//;* any part of this publication in any form, by any means, without the prior
//;* written permission of Insyde Software Corporation.
//;*
//;******************************************************************************

#include "L05ModifyKeyCodeTable.h"

VOID
L05ModifyKeyCodeTableNotificationFunction (
  IN  EFI_EVENT                         Event,
  IN  VOID                              *Context
  )
{
  EFI_STATUS                            Status;
  EFI_LEGACY_REGION2_PROTOCOL           *LegacyRegion;
  L05_RAW_KEY                           *L05KeyTable;
  UINTN                                 Index;
  UINTN                                 Address;
  UINT8                                 IdentifyEntry[] = L05_KEY_CODE_TABLE_IDENTIFY_ENTRY;
  UINT8                                 ModifyCtrlCodeTable[] = L05_KEY_MODIFY_CTRL_CODE_TABLE;
  UINT32                                Granularity;

  Status       = EFI_SUCCESS;
  LegacyRegion = NULL;
  L05KeyTable  = NULL;
  Index        = 0;
  Address      = 0;

  //
  //  Unlock F segment to modify the key code table.
  //
  Status = gBS->LocateProtocol (&gEfiLegacyRegion2ProtocolGuid, NULL, &LegacyRegion);

  if (EFI_ERROR (Status)) {
    return;
  }

  LegacyRegion->UnLock (LegacyRegion, 0xF0000, 0x10000, &Granularity);

  for (Address = L05_KEY_CODE_TABLE_ADDRESS_MIN; Address < L05_KEY_CODE_TABLE_ADDRESS_MAX; Address++) {

    for (Index = 0; Index < sizeof (IdentifyEntry); Index++) {
      if (((UINT8 *) Address)[Index] != IdentifyEntry[Index]) {
        break;
      }
    }

    //
    //  The key code table is found, we need to patch
    //
    if (Index == sizeof (IdentifyEntry)) {
      L05KeyTable = (L05_RAW_KEY *) Address;

      for (Index = 0; Index < sizeof (ModifyCtrlCodeTable); Index++) {
        L05KeyTable[ModifyCtrlCodeTable[Index]].CtrlCode = (UINT16) (ModifyCtrlCodeTable[Index] * 256);
      }

      break;
    }
  }

  LegacyRegion->Lock (LegacyRegion, 0xF0000, 0x10000, &Granularity);

  return;
}

EFI_STATUS
InstallModifyKeyCodeTableNotification (
  IN EFI_HANDLE                         ImageHandle,
  IN EFI_SYSTEM_TABLE                   *SystemTable
  )
{
  EFI_STATUS                            Status;
  VOID                                  *StartOfBdsDiagnosticsEventRegistration;
  EFI_EVENT                             StartOfBdsDiagnosticsEvent;

  Status                                 = EFI_SUCCESS;
  StartOfBdsDiagnosticsEventRegistration = NULL;
  StartOfBdsDiagnosticsEvent             = NULL;

  Status = gBS->CreateEvent (
                  EVT_NOTIFY_SIGNAL,
                  TPL_CALLBACK,
                  L05ModifyKeyCodeTableNotificationFunction,
                  NULL,
                  &StartOfBdsDiagnosticsEvent
                  );

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

  Status = gBS->RegisterProtocolNotify (
                  &gEfiStartOfBdsDiagnosticsProtocolGuid,
                  StartOfBdsDiagnosticsEvent,
                  &StartOfBdsDiagnosticsEventRegistration
                  );

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

  return Status;
}