WWW.LAB-Z.COM

/**
Output current RIP to debug port (0x402 for QEMU)

**/
VOID
EFIAPI
AsmCurrentRIP(
	VOID
);

;------------------------------------------------------------------------------
;
; Copyright (c) 2006 - 2008, Intel Corporation. All rights reserved.<BR>
; SPDX-License-Identifier: BSD-2-Clause-Patent
;
; Module Name:
;
;   CurrentRIP.Asm
;
; Abstract:
;
;  Output current RIP to debug port (0x402 for QEMU)
;  
;
; Notes:
;
;------------------------------------------------------------------------------

    DEFAULT REL
    SECTION .text

;------------------------------------------------------------------------------
; VOID
; EFIAPI
; AsmCurrentRIP (
;   VOID
;   );
;------------------------------------------------------------------------------
global ASM_PFX(AsmCurrentRIP)
ASM_PFX(AsmCurrentRIP):
    lea     rax,[$]
    mov     dx,0x402
	
	; 7-0 Bits
    out     dx,al       

    ; 15-8 Bits
	shr     rax,8
    out     dx,al       
	
	; 23-16 Bits
	shr     rax,8
    out     dx,al       

	; 31-24 Bits
	shr     rax,8
    out     dx,al       

	; 39-32 Bits
	shr     rax,8
    out     dx,al       

	; 47-40 Bits
	shr     rax,8
    out     dx,al       

	; 55-48 Bits
	shr     rax,8
    out     dx,al       

	; 63-56 Bits
	shr     rax,8
    out     dx,al     

ret

[Sources.X64]
  X64/Thunk16.nasm
  X64/CpuIdEx.nasm
  X64/CpuId.nasm
  X64/LongJump.nasm
  X64/SetJump.nasm
  X64/SwitchStack.nasm
  X64/EnableCache.nasm
  X64/DisableCache.nasm
  X64/WriteTr.nasm
  X64/Lfence.nasm
  X64/CurrentRIP.nasm
  X64/CpuBreakpoint.c | MSFT
  X64/WriteMsr64.c | MSFT
  X64/ReadMsr64.c | MSFT
  X64/CpuPause.nasm| MSFT
  X64/DisableInterrupts.nasm| MSFT

  if (!SevEsIsEnabled ()) {
    //
    // For non SEV-ES guests, just load the IDTR.
    //
    AsmWriteIdtr (&IdtDescriptor);
  } else {
    //
    // Under SEV-ES, the hypervisor can't modify CR0 and so can't enable
    // caching in order to speed up the boot. Enable caching early for
    // an SEV-ES guest.
    //
    AsmEnableCache ();
  }
AsmCurrentRIP();
  DEBUG ((DEBUG_INFO,
    "SecCoreStartupWithStack(0x%x, 0x%x)\n",
    (UINT32)(UINTN)BootFv,
    (UINT32)(UINTN)TopOfCurrentStack
));

[BuildOptions]
  MSFT:*_*_X64_CC_FLAGS  = /FAsc /Od

在 SecMain.cod 中我们看到的结果如下：
; 908  :     //
; 909  :     // Under SEV-ES, the hypervisor can't modify CR0 and so can't enable
; 910  :     // caching in order to speed up the boot. Enable caching early for
; 911  :     // an SEV-ES guest.
; 912  :     //
; 913  :     AsmEnableCache ();

  00131	e8 00 00 00 00	 call	 AsmEnableCache
$LN25@SecCoreSta:

; 914  :   }
; 915  : AsmCurrentRIP();

  00136	e8 00 00 00 00	 call	 AsmCurrentRIP
$LN13@SecCoreSta:

; 916  :   DEBUG ((DEBUG_INFO,

  0013b	e8 00 00 00 00	 call	 DebugPrintEnabled
  00140	0f b6 c0	 movzx	 eax, al
  00143	85 c0		 test	 eax, eax
  00145	74 38		 je	 SHORT $LN26@SecCoreSta
$LN16@SecCoreSta:

; 917  :     "SecCoreStartupWithStack(0x%x, 0x%x)\n",
; 918  :     (UINT32)(UINTN)BootFv,
; 919  :     (UINT32)(UINTN)TopOfCurrentStack
; 920  :     ));

;------------------------------------------------------------------------------
;
; Copyright (c) 2006 - 2008, Intel Corporation. All rights reserved.<BR>
; SPDX-License-Identifier: BSD-2-Clause-Patent
;
; Module Name:
;
;   CurrentRIP.Asm
;
; Abstract:
;
;  Output current RIP to debug port (0x402 for QEMU)
;  
;
; Notes:
;
;------------------------------------------------------------------------------

    DEFAULT REL
    SECTION .text

;------------------------------------------------------------------------------
; VOID
; EFIAPI
; AsmCurrentRIP (
;   VOID
;   );
;------------------------------------------------------------------------------
global ASM_PFX(AsmCurrentRIP)
ASM_PFX(AsmCurrentRIP):
    mov     rax,[rsp]
    mov     dx,0x402
	
	; 7-0 Bits
    out     dx,al       

    ; 15-8 Bits
	shr     rax,8
    out     dx,al       
	
	; 23-16 Bits
	shr     rax,8
    out     dx,al       

	; 31-24 Bits
	shr     rax,8
    out     dx,al       

	; 39-32 Bits
	shr     rax,8
    out     dx,al       

	; 47-40 Bits
	shr     rax,8
    out     dx,al       

	; 55-48 Bits
	shr     rax,8
    out     dx,al       

	; 63-56 Bits
	shr     rax,8
    out     dx,al     

ret

//
  // Find PEI Core entry point. It will report SEC and Pei Core debug information if remote debug
  // is enabled.
  //
  BootFv = (EFI_FIRMWARE_VOLUME_HEADER *)SecCoreData->BootFirmwareVolumeBase;
  FindAndReportEntryPoints (&BootFv, &PeiCoreEntryPoint);
  SecCoreData->BootFirmwareVolumeBase = BootFv;
  SecCoreData->BootFirmwareVolumeSize = (UINTN) BootFv->FvLength;
  AsmCurrentRIP(); //ZivDebug
  DEBUG ((DEBUG_INFO,
    "Check [BootFirmwareVolumeBase]-0x%X [PeiCoreEntryPoint]=0x%X BootFirmwareVolumeBase=0x%X \n",
	(UINT64)BootFv,
    (UINT64)(PeiCoreEntryPoint),
	(UINT64)(*PeiCoreEntryPoint)
    ));
  AsmCurrentRIP(); //ZivDebug	
  //
  // Transfer the control to the PEI core
  //
  (*PeiCoreEntryPoint) (SecCoreData, (EFI_PEI_PPI_DESCRIPTOR *)&mPrivateDispatchTable);

; 1016 :   AsmCurrentRIP(); //ZivDebug

  00050	e8 00 00 00 00	 call	 AsmCurrentRIP
$LN4@SecStartup:

; 1017 :   DEBUG ((DEBUG_INFO,

  00055	e8 00 00 00 00	 call	 DebugPrintEnabled
  0005a	0f b6 c0	 movzx	 eax, al
  0005d	85 c0		 test	 eax, eax
  0005f	74 3c		 je	 SHORT $LN11@SecStartup
$LN7@SecStartup:

; 1018 :     "Check [BootFirmwareVolumeBase]-0x%X [PeiCoreEntryPoint]=0x%X BootFirmwareVolumeBase=0x%X \n",
; 1019 : 	(UINT64)BootFv,
; 1020 :     (UINT64)(PeiCoreEntryPoint),
; 1021 : 	(UINT64)(*PeiCoreEntryPoint)
; 1022 :     ));

  00061	b9 40 00 00 00	 mov	 ecx, 64			; 00000040H
  00066	e8 00 00 00 00	 call	 DebugPrintLevelEnabled
  0006b	0f b6 c0	 movzx	 eax, al
  0006e	85 c0		 test	 eax, eax
  00070	74 25		 je	 SHORT $LN12@SecStartup
  00072	48 8b 44 24 40	 mov	 rax, QWORD PTR PeiCoreEntryPoint$[rsp]
  00077	48 89 44 24 20	 mov	 QWORD PTR [rsp+32], rax
  0007c	4c 8b 4c 24 40	 mov	 r9, QWORD PTR PeiCoreEntryPoint$[rsp]
  00081	4c 8b 44 24 30	 mov	 r8, QWORD PTR BootFv$[rsp]
  00086	48 8d 15 00 00
	00 00		 lea	 rdx, OFFSET FLAT:??_C@_0FL@FMODEEHL@Check?5?$FLBootFirmwareVolumeBase?$FN?90@
  0008d	b9 40 00 00 00	 mov	 ecx, 64			; 00000040H
  00092	e8 00 00 00 00	 call	 DebugPrint
$LN12@SecStartup:
  00097	33 c0		 xor	 eax, eax
  00099	85 c0		 test	 eax, eax
  0009b	75 c4		 jne	 SHORT $LN7@SecStartup
$LN11@SecStartup:

; 1017 :   DEBUG ((DEBUG_INFO,

  0009d	33 c0		 xor	 eax, eax
  0009f	85 c0		 test	 eax, eax
  000a1	75 b2		 jne	 SHORT $LN4@SecStartup

; 1023 :   AsmCurrentRIP(); //ZivDebug	

  000a3	e8 00 00 00 00	 call	 AsmCurrentRIP

ProcessLibraryConstructorList (NULL, NULL);

VOID
EFIAPI
ProcessLibraryConstructorList (
  VOID
  )
{
  RETURN_STATUS  Status;

  Status = PlatformRomDebugLibIoPortConstructor ();
  ASSERT_RETURN_ERROR (Status);

  Status = AcpiTimerLibConstructor ();
  ASSERT_RETURN_ERROR (Status);

  Status = LzmaDecompressLibConstructor ();
  ASSERT_RETURN_ERROR (Status);
}

/**
  The constructor function enables ACPI IO space.

  If ACPI I/O space not enabled, this function will enable it.
  It will always return RETURN_SUCCESS.

  @retval EFI_SUCCESS   The constructor always returns RETURN_SUCCESS.

**/
RETURN_STATUS
EFIAPI
AcpiTimerLibConstructor (
  VOID
  )
{
  UINT16 HostBridgeDevId;
  UINTN Pmba;
  UINT32 PmbaAndVal;
  UINT32 PmbaOrVal;
  UINTN AcpiCtlReg;
  UINT8 AcpiEnBit;

  //
  // Query Host Bridge DID to determine platform type
  //
  HostBridgeDevId = PciRead16 (OVMF_HOSTBRIDGE_DID);
  switch (HostBridgeDevId) {
    case INTEL_82441_DEVICE_ID:
      Pmba       = POWER_MGMT_REGISTER_PIIX4 (PIIX4_PMBA);
      PmbaAndVal = ~(UINT32)PIIX4_PMBA_MASK;
      PmbaOrVal  = PIIX4_PMBA_VALUE;
      AcpiCtlReg = POWER_MGMT_REGISTER_PIIX4 (PIIX4_PMREGMISC);
      AcpiEnBit  = PIIX4_PMREGMISC_PMIOSE;
      break;
    case INTEL_Q35_MCH_DEVICE_ID:
      Pmba       = POWER_MGMT_REGISTER_Q35 (ICH9_PMBASE);
      PmbaAndVal = ~(UINT32)ICH9_PMBASE_MASK;
      PmbaOrVal  = ICH9_PMBASE_VALUE;
      AcpiCtlReg = POWER_MGMT_REGISTER_Q35 (ICH9_ACPI_CNTL);
      AcpiEnBit  = ICH9_ACPI_CNTL_ACPI_EN;
      break;
    default:
      DEBUG ((DEBUG_ERROR, "%a: Unknown Host Bridge Device ID: 0x%04x\n",
        __FUNCTION__, HostBridgeDevId));
      ASSERT (FALSE);
      return RETURN_UNSUPPORTED;
  }

  //
  // Check to see if the Power Management Base Address is already enabled
  //
  if ((PciRead8 (AcpiCtlReg) & AcpiEnBit) == 0) {
    //
    // If the Power Management Base Address is not programmed,
    // then program it now.
    //
    PciAndThenOr32 (Pmba, PmbaAndVal, PmbaOrVal);

    //
    // Enable PMBA I/O port decodes
    //
    PciOr8 (AcpiCtlReg, AcpiEnBit);
  }

  return RETURN_SUCCESS;
}

/**
  Register LzmaDecompress and LzmaDecompressGetInfo handlers with LzmaCustomerDecompressGuid.

  @retval  RETURN_SUCCESS            Register successfully.
  @retval  RETURN_OUT_OF_RESOURCES   No enough memory to store this handler.
**/
EFI_STATUS
EFIAPI
LzmaDecompressLibConstructor (
  VOID
  )
{
  return ExtractGuidedSectionRegisterHandlers (
          &gLzmaCustomDecompressGuid,
          LzmaGuidedSectionGetInfo,
          LzmaGuidedSectionExtraction
          );
}

CONSTRUCTOR                    = LzmaDecompressLibConstructor

## Create code for library constructor
#
#   @param      Info        The ModuleAutoGen object
#   @param      AutoGenC    The TemplateString object for C code
#   @param      AutoGenH    The TemplateString object for header file
#
def CreateLibraryConstructorCode(Info, AutoGenC, AutoGenH):

    for Lib in DependentLibraryList:
        if len(Lib.ConstructorList) <= 0:
            continue
        Dict = {'Function':Lib.ConstructorList}
        if Lib.ModuleType in [SUP_MODULE_BASE, SUP_MODULE_SEC]:
            ConstructorPrototypeString.Append("//www.lab-z.com testing");
            ConstructorPrototypeString.Append(gLibraryStructorPrototype[SUP_MODULE_BASE].Replace(Dict))
            ConstructorCallingString.Append(gLibraryStructorCall[SUP_MODULE_BASE].Replace(Dict))

GLOBAL_REMOVE_IF_UNREFERENCED const BOOLEAN _gPcd_FixedAtBuild_PcdCpuSmmStackGuard = _PCD_VALUE_PcdCpuSmmStackGuard;
extern const  BOOLEAN  _gPcd_FixedAtBuild_PcdCpuSmmStackGuard;
#define _PCD_GET_MODE_BOOL_PcdCpuSmmStackGuard  _gPcd_FixedAtBuild_PcdCpuSmmStackGuard
//#define _PCD_SET_MODE_BOOL_PcdCpuSmmStackGuard  ASSERT(FALSE)  // It is not allowed to set value for a FIXED_AT_BUILD PCD

//www.lab-z.com testing
RETURN_STATUS
EFIAPI
PlatformRomDebugLibIoPortConstructor (
  VOID
  );
//www.lab-z.com testing
RETURN_STATUS
EFIAPI
AcpiTimerLibConstructor (
  VOID
  );
//www.lab-z.com testing
RETURN_STATUS
EFIAPI
LzmaDecompressLibConstructor (
  VOID
  );


VOID
EFIAPI
ProcessLibraryConstructorList (
  VOID
  )
{
  RETURN_STATUS  Status;

  Status = PlatformRomDebugLibIoPortConstructor ();
  ASSERT_RETURN_ERROR (Status);

  Status = AcpiTimerLibConstructor ();
  ASSERT_RETURN_ERROR (Status);

  Status = LzmaDecompressLibConstructor ();
  ASSERT_RETURN_ERROR (Status);

}

  DEBUG ((DEBUG_INFO,
    "SecCoreStartupWithStack(0x%x, 0x%x)\n",
    (UINT32)(UINTN)BootFv,
    (UINT32)(UINTN)TopOfCurrentStack
));

/**
  Macro that calls DebugPrint().

  If MDEPKG_NDEBUG is not defined and the DEBUG_PROPERTY_DEBUG_PRINT_ENABLED
  bit of PcdDebugProperyMask is set, then this macro passes Expression to
  DebugPrint().

  @param  Expression  Expression containing an error level, a format string,
                      and a variable argument list based on the format string.


**/
#if !defined(MDEPKG_NDEBUG)
  #define DEBUG(Expression)        \
    do {                           \
      if (DebugPrintEnabled ()) {  \
        _DEBUG (Expression);       \
      }                            \
    } while (FALSE)
#else
  #define DEBUG(Expression)
#endif

/**
  Returns TRUE if DEBUG() macros are enabled.

  This function returns TRUE if the DEBUG_PROPERTY_DEBUG_PRINT_ENABLED bit of
  PcdDebugProperyMask is set.  Otherwise FALSE is returned.

  @retval  TRUE    The DEBUG_PROPERTY_DEBUG_PRINT_ENABLED bit of PcdDebugProperyMask is set.
  @retval  FALSE   The DEBUG_PROPERTY_DEBUG_PRINT_ENABLED bit of PcdDebugProperyMask is clear.

**/
BOOLEAN
EFIAPI
DebugPrintEnabled (
  VOID
  )
{
  return (BOOLEAN) ((PcdGet8(PcdDebugPropertyMask) & DEBUG_PROPERTY_DEBUG_PRINT_ENABLED) != 0);
}

/**
  Internal worker macro that calls DebugPrint().

  This macro calls DebugPrint() passing in the debug error level, a format
  string, and a variable argument list.
  __VA_ARGS__ is not supported by EBC compiler, Microsoft Visual Studio .NET 2003
  and Microsoft Windows Server 2003 Driver Development Kit (Microsoft WINDDK) version 3790.1830.

  @param  Expression  Expression containing an error level, a format string,
                      and a variable argument list based on the format string.

**/

#if !defined(MDE_CPU_EBC) && (!defined (_MSC_VER) || _MSC_VER > 1400)
  #define _DEBUG_PRINT(PrintLevel, ...)              \
    do {                                             \
      if (DebugPrintLevelEnabled (PrintLevel)) {     \
        DebugPrint (PrintLevel, ##__VA_ARGS__);      \
      }                                              \
    } while (FALSE)
  #define _DEBUG(Expression)   _DEBUG_PRINT Expression
#else
#define _DEBUG(Expression)   DebugPrint Expression
#endif

/**
  Prints a debug message to the debug output device if the specified error level is enabled.

  If any bit in ErrorLevel is also set in DebugPrintErrorLevelLib function
  GetDebugPrintErrorLevel (), then print the message specified by Format and the
  associated variable argument list to the debug output device.

  If Format is NULL, then ASSERT().

  @param  ErrorLevel  The error level of the debug message.
  @param  Format      Format string for the debug message to print.
  @param  ...         Variable argument list whose contents are accessed
                      based on the format string specified by Format.

**/
VOID
EFIAPI
DebugPrint (
  IN  UINTN        ErrorLevel,
  IN  CONST CHAR8  *Format,
  ...
  )
{
  VA_LIST         Marker;

  VA_START (Marker, Format);
  DebugVPrint (ErrorLevel, Format, Marker);
  VA_END (Marker);
}

/**
  Prints a debug message to the debug output device if the specified
  error level is enabled.

  If any bit in ErrorLevel is also set in DebugPrintErrorLevelLib function
  GetDebugPrintErrorLevel (), then print the message specified by Format and
  the associated variable argument list to the debug output device.

  If Format is NULL, then ASSERT().

  @param  ErrorLevel    The error level of the debug message.
  @param  Format        Format string for the debug message to print.
  @param  VaListMarker  VA_LIST marker for the variable argument list.

**/
VOID
EFIAPI
DebugVPrint (
  IN  UINTN         ErrorLevel,
  IN  CONST CHAR8   *Format,
  IN  VA_LIST       VaListMarker
  )
{
  DebugPrintMarker (ErrorLevel, Format, VaListMarker, NULL);
}

/**
  Prints a debug message to the debug output device if the specified
  error level is enabled base on Null-terminated format string and a
  VA_LIST argument list or a BASE_LIST argument list.

  If any bit in ErrorLevel is also set in DebugPrintErrorLevelLib function
  GetDebugPrintErrorLevel (), then print the message specified by Format and
  the associated variable argument list to the debug output device.

  If Format is NULL, then ASSERT().

  @param  ErrorLevel      The error level of the debug message.
  @param  Format          Format string for the debug message to print.
  @param  VaListMarker    VA_LIST marker for the variable argument list.
  @param  BaseListMarker  BASE_LIST marker for the variable argument list.

**/
VOID
DebugPrintMarker (
  IN  UINTN         ErrorLevel,
  IN  CONST CHAR8   *Format,
  IN  VA_LIST       VaListMarker,
  IN  BASE_LIST     BaseListMarker
  )
{
  CHAR8    Buffer[MAX_DEBUG_MESSAGE_LENGTH];
  UINTN    Length;

  //
  // If Format is NULL, then ASSERT().
  //
  ASSERT (Format != NULL);

  //
  // Check if the global mask disables this message or the device is inactive
  //
  if ((ErrorLevel & GetDebugPrintErrorLevel ()) == 0 ||
      !PlatformDebugLibIoPortFound ()) {
    return;
  }

  //
  // Convert the DEBUG() message to an ASCII String
  //
  if (BaseListMarker == NULL) {
    Length = AsciiVSPrint (Buffer, sizeof (Buffer), Format, VaListMarker);
  } else {
    Length = AsciiBSPrint (Buffer, sizeof (Buffer), Format, BaseListMarker);
  }

  //
  // Send the print string to the debug I/O port
  //
  IoWriteFifo8 (PcdGet16 (PcdDebugIoPort), Length, Buffer);
}

  ## This flag is used to control the destination port for PlatformDebugLibIoPort
  gUefiOvmfPkgTokenSpaceGuid.PcdDebugIoPort|0x402|UINT16|4

BITS    64

    ;
    ; Some values were calculated in 32-bit mode.  Make sure the upper
    ; 32-bits of 64-bit registers are zero for these values.
    ;
    mov     rax, 0x00000000ffffffff
    and     rsi, rax
    and     rbp, rax
    and     rsp, rax

    ;
    ; RSI - SEC Core entry point
    ; RBP - Start of BFV
    ;

    ;
    ; Restore initial EAX value into the RAX register
    ;
    mov     rax, rsp

    ;
    ; Jump to the 64-bit SEC entry point
    ;
    jmp     rsi

    ;
    ; Restore initial EAX value into the RAX register
    ;
    mov     rax, rsp

    mov     rax,rsi
    mov     dx,0x402
	
	; 7-0 Bits
    out     dx,al       

    ; 15-8 Bits
	shr     rax,8
    out     dx,al       
	
	; 23-16 Bits
	shr     rax,8
    out     dx,al       

	; 31-24 Bits
	shr     rax,8
    out     dx,al       

	; 39-32 Bits
	shr     rax,8
    out     dx,al       

	; 47-40 Bits
	shr     rax,8
    out     dx,al       

	; 55-48 Bits
	shr     rax,8
    out     dx,al       

	; 63-56 Bits
	shr     rax,8
    out     dx,al       

;
; SecCore Entry Point
;
; Processor is in flat protected mode
;
; @param[in]  RAX   Initial value of the EAX register (BIST: Built-in Self Test)
; @param[in]  DI    'BP': boot-strap processor, or 'AP': application processor
; @param[in]  RBP   Pointer to the start of the Boot Firmware Volume
; @param[in]  DS    Selector allowing flat access to all addresses
; @param[in]  ES    Selector allowing flat access to all addresses
; @param[in]  FS    Selector allowing flat access to all addresses
; @param[in]  GS    Selector allowing flat access to all addresses
; @param[in]  SS    Selector allowing flat access to all addresses
;
; @return     None  This routine does not return
;
global ASM_PFX(_ModuleEntryPoint)
ASM_PFX(_ModuleEntryPoint):
    lea     rax,[$]
    mov     dx,0x402
	
	; 7-0 Bits
    out     dx,al       

    ; 15-8 Bits
	shr     rax,8
    out     dx,al       
	
	; 23-16 Bits
	shr     rax,8
    out     dx,al       

	; 31-24 Bits
	shr     rax,8
    out     dx,al       

	; 39-32 Bits
	shr     rax,8
    out     dx,al       

	; 47-40 Bits
	shr     rax,8
    out     dx,al       

	; 55-48 Bits
	shr     rax,8
    out     dx,al       

	; 63-56 Bits
	shr     rax,8
    out     dx,al     

    ;
    ; Fill the temporary RAM with the initial stack value.
    ; The loop below will seed the heap as well, but that's harmless.
    ;
    mov     rax, (FixedPcdGet32 (PcdInitValueInTempStack) << 32) | FixedPcdGet32 (PcdInitValueInTempStack)
                                                              ; qword to store
    mov     rdi, FixedPcdGet32 (PcdOvmfSecPeiTempRamBase)     ; base address,
                                                              ;   relative to
                                                              ;   ES
    mov     rcx, FixedPcdGet32 (PcdOvmfSecPeiTempRamSize) / 8 ; qword count
    cld                                                       ; store from base
                                                              ;   up
    rep stosq

int GetPressed() {
  int result = 0;
  
  digitalWrite(KEY_ROW1, HIGH);
  digitalWrite(KEY_ROW2, LOW);
  digitalWrite(KEY_ROW3, LOW);

  // Upper line
  result = (digitalRead(KEY_COL1) << 3) +
           (digitalRead(KEY_COL2) << 2) +
           (digitalRead(KEY_COL3) << 1) +
           digitalRead(KEY_COL4);

  // Middle line
  digitalWrite(KEY_ROW1, LOW);
  digitalWrite(KEY_ROW2, HIGH);
  digitalWrite(KEY_ROW3, LOW);

  result = ((digitalRead(KEY_COL1) << 3) +
            (digitalRead(KEY_COL2) << 2) +
            (digitalRead(KEY_COL3) << 1) +
            (digitalRead(KEY_COL4))) +
           result * 10;
  
  
  // Buttom line
  digitalWrite(KEY_ROW1, LOW);
  digitalWrite(KEY_ROW2, LOW);
  digitalWrite(KEY_ROW3, HIGH);
  result = (digitalRead(KEY_COL1) << 3) +
           (digitalRead(KEY_COL2) << 2) +
           (digitalRead(KEY_COL3) << 1) +
           (digitalRead(KEY_COL4) << 0) +
           result * 10;
  return result;
}

  digitalWrite(KEY_ROW1, HIGH);
  digitalWrite(KEY_ROW2, LOW);
  digitalWrite(KEY_ROW3,  LOW);

c1=(digitalRead(KEY_COL1) << 3) +
           (digitalRead(KEY_COL2) << 2) +
           (digitalRead(KEY_COL3) << 1) +
           digitalRead(KEY_COL4);

  digitalWrite(KEY_ROW1, LOW);
  digitalWrite(KEY_ROW2, LOW);
  digitalWrite(KEY_ROW3,  HIGH);

c2=(digitalRead(KEY_COL1) << 3) +
           (digitalRead(KEY_COL2) << 2) +
           (digitalRead(KEY_COL3) << 1) +
           digitalRead(KEY_COL4);