2 files changed, 36 insertions, 7 deletions

diff --git a/arch/x86/include/asm/efi.h b/arch/x86/include/asm/efi.h
index c9dcc181d4d1..ae3bf3b8da71 100644
--- a/arch/x86/include/asm/efi.h
+++ b/arch/x86/include/asm/efi.h
@@ -69,23 +69,37 @@ extern u64 efi_call6(void *fp, u64 arg1, u64 arg2, u64 arg3,
         efi_call6((void *)(f), (u64)(a1), (u64)(a2), (u64)(a3),         \
                   (u64)(a4), (u64)(a5), (u64)(a6))
 
+extern unsigned long efi_call_virt_prelog(void);
+extern void efi_call_virt_epilog(unsigned long);
+
+#define efi_callx(x, func, ...)                                 \
+        ({                                                      \
+                efi_status_t __status;                          \
+                unsigned long __pgd;                            \
+                                                                \
+                __pgd = efi_call_virt_prelog();                 \
+                __status = efi_call##x(func, __VA_ARGS__);      \
+                efi_call_virt_epilog(__pgd);                    \
+                __status;                                       \
+        })
+
 #define efi_call_virt0(f)                               \
-        efi_call0((void *)(efi.systab->runtime->f))
+        efi_callx(0, (void *)(efi.systab->runtime->f))
 #define efi_call_virt1(f, a1)                                   \
-        efi_call1((void *)(efi.systab->runtime->f), (u64)(a1))
+        efi_callx(1, (void *)(efi.systab->runtime->f), (u64)(a1))
 #define efi_call_virt2(f, a1, a2)                                       \
-        efi_call2((void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2))
+        efi_callx(2, (void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2))
 #define efi_call_virt3(f, a1, a2, a3)                                   \
-        efi_call3((void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2), \
+        efi_callx(3, (void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2), \
                   (u64)(a3))
 #define efi_call_virt4(f, a1, a2, a3, a4)                               \
-        efi_call4((void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2), \
+        efi_callx(4, (void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2), \
                   (u64)(a3), (u64)(a4))
 #define efi_call_virt5(f, a1, a2, a3, a4, a5)                           \
-        efi_call5((void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2), \
+        efi_callx(5, (void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2), \
                   (u64)(a3), (u64)(a4), (u64)(a5))
 #define efi_call_virt6(f, a1, a2, a3, a4, a5, a6)                       \
-        efi_call6((void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2), \
+        efi_callx(6, (void *)(efi.systab->runtime->f), (u64)(a1), (u64)(a2), \
                   (u64)(a3), (u64)(a4), (u64)(a5), (u64)(a6))
 
 extern void __iomem *efi_ioremap(unsigned long addr, unsigned long size,
diff --git a/arch/x86/platform/efi/efi_64.c b/arch/x86/platform/efi/efi_64.c
index ac3aa54e2654..ddb0174cf093 100644
--- a/arch/x86/platform/efi/efi_64.c
+++ b/arch/x86/platform/efi/efi_64.c
@@ -58,6 +58,21 @@ static void __init early_code_mapping_set_exec(int executable)
         }
 }
 
+unsigned long efi_call_virt_prelog(void)
+{
+        unsigned long saved;
+
+        saved = read_cr3();
+        write_cr3(real_mode_header->trampoline_pgd);
+
+        return saved;
+}
+
+void efi_call_virt_epilog(unsigned long saved)
+{
+        write_cr3(saved);
+}
+
 void __init efi_call_phys_prelog(void)
 {
         unsigned long vaddress;