3 files changed, 57 insertions, 2 deletions

diff --git a/arch/x86/kernel/acpi/realmode/wakeup.S b/arch/x86/kernel/acpi/realmode/wakeup.S
index f9b77fb37e5b..3355973b12ac 100644
--- a/arch/x86/kernel/acpi/realmode/wakeup.S
+++ b/arch/x86/kernel/acpi/realmode/wakeup.S
@@ -5,6 +5,7 @@
 #include <asm/msr-index.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
+#include <asm/processor-flags.h>
 
         .code16
         .section ".header", "a"
@@ -24,6 +25,11 @@ pmode_gdt:	.quad	0
 realmode_flags: .long   0
 real_magic:     .long   0
 trampoline_segment:     .word 0
+_pad1:          .byte   0
+wakeup_jmp:     .byte   0xea    /* ljmpw */
+wakeup_jmp_off: .word   3f
+wakeup_jmp_seg: .word   0
+wakeup_gdt:     .quad   0, 0, 0
 signature:      .long   0x51ee1111
 
         .text
@@ -34,11 +40,34 @@ _start:
         cli
         cld
 
+        /* Apparently some dimwit BIOS programmers don't know how to
+           program a PM to RM transition, and we might end up here with
+           junk in the data segment descriptor registers.  The only way
+           to repair that is to go into PM and fix it ourselves... */
+        movw    $16, %cx
+        lgdtl   %cs:wakeup_gdt
+        movl    %cr0, %eax
+        orb     $X86_CR0_PE, %al
+        movl    %eax, %cr0
+        jmp     1f
+1:      ljmpw   $8, $2f
+2:
+        movw    %cx, %ds
+        movw    %cx, %es
+        movw    %cx, %ss
+        movw    %cx, %fs
+        movw    %cx, %gs
+
+        andb    $~X86_CR0_PE, %al
+        movl    %eax, %cr0
+        jmp     wakeup_jmp
+3:
         /* Set up segments */
         movw    %cs, %ax
         movw    %ax, %ds
         movw    %ax, %es
         movw    %ax, %ss
+        lidtl   wakeup_idt
 
         movl    $wakeup_stack_end, %esp
 
@@ -98,7 +127,14 @@ bogus_real_magic:
         jmp     1b
 
         .data
-        .balign 4
+        .balign 8
+
+        /* This is the standard real-mode IDT */
+wakeup_idt:
+        .word   0xffff          /* limit */
+        .long   0               /* address */
+        .word   0
+
         .globl  HEAP, heap_end
 HEAP:
         .long   wakeup_heap
diff --git a/arch/x86/kernel/acpi/realmode/wakeup.h b/arch/x86/kernel/acpi/realmode/wakeup.h
index ef8166fe8020..69d38d0b2b64 100644
--- a/arch/x86/kernel/acpi/realmode/wakeup.h
+++ b/arch/x86/kernel/acpi/realmode/wakeup.h
@@ -24,6 +24,11 @@ struct wakeup_header {
         u32 realmode_flags;
         u32 real_magic;
         u16 trampoline_segment; /* segment with trampoline code, 64-bit only */
+        u8  _pad1;
+        u8  wakeup_jmp;
+        u16 wakeup_jmp_off;
+        u16 wakeup_jmp_seg;
+        u64 wakeup_gdt[3];
         u32 signature;          /* To check we have correct structure */
 } __attribute__((__packed__));
 
diff --git a/arch/x86/kernel/acpi/sleep.c b/arch/x86/kernel/acpi/sleep.c
index afc25ee9964b..36af01f029ed 100644
--- a/arch/x86/kernel/acpi/sleep.c
+++ b/arch/x86/kernel/acpi/sleep.c
@@ -50,6 +50,20 @@ int acpi_save_state_mem(void)
 
         header->video_mode = saved_video_mode;
 
+        header->wakeup_jmp_seg = acpi_wakeup_address >> 4;
+        /* GDT[0]: GDT self-pointer */
+        header->wakeup_gdt[0] =
+                (u64)(sizeof(header->wakeup_gdt) - 1) +
+                ((u64)(acpi_wakeup_address +
+                        ((char *)&header->wakeup_gdt - (char *)acpi_realmode))
+                                << 16);
+        /* GDT[1]: real-mode-like code segment */
+        header->wakeup_gdt[1] = (0x009bULL << 40) +
+                ((u64)acpi_wakeup_address << 16) + 0xffff;
+        /* GDT[2]: real-mode-like data segment */
+        header->wakeup_gdt[2] = (0x0093ULL << 40) +
+                ((u64)acpi_wakeup_address << 16) + 0xffff;
+
 #ifndef CONFIG_64BIT
         store_gdt((struct desc_ptr *)&header->pmode_gdt);
 
@@ -111,7 +125,7 @@ void __init acpi_reserve_bootmem(void)
                 return;
         }
 
-        acpi_wakeup_address = acpi_realmode;
+        acpi_wakeup_address = virt_to_phys((void *)acpi_realmode);
 }