1 files changed, 107 insertions, 179 deletions

diff --git a/arch/x86_64/kernel/acpi/wakeup.S b/arch/x86_64/kernel/acpi/wakeup.S
index 185faa911db5..8550a6ffa275 100644
--- a/arch/x86_64/kernel/acpi/wakeup.S
+++ b/arch/x86_64/kernel/acpi/wakeup.S
@@ -1,6 +1,7 @@
 .text
 #include <linux/linkage.h>
 #include <asm/segment.h>
+#include <asm/pgtable.h>
 #include <asm/page.h>
 #include <asm/msr.h>
 
@@ -30,22 +31,28 @@ wakeup_code:
         cld
         # setup data segment
         movw    %cs, %ax
-        movw    %ax, %ds                                        # Make ds:0 point to wakeup_start
+        movw    %ax, %ds                # Make ds:0 point to wakeup_start
         movw    %ax, %ss
-        mov     $(wakeup_stack - wakeup_code), %sp              # Private stack is needed for ASUS board
+                                        # Private stack is needed for ASUS board
+        mov     $(wakeup_stack - wakeup_code), %sp
 
-        pushl   $0                                              # Kill any dangerous flags
+        pushl   $0                      # Kill any dangerous flags
         popfl
 
         movl    real_magic - wakeup_code, %eax
         cmpl    $0x12345678, %eax
         jne     bogus_real_magic
 
+        call    verify_cpu                      # Verify the cpu supports long
+                                                # mode
+        testl   %eax, %eax
+        jnz     no_longmode
+
         testl   $1, video_flags - wakeup_code
         jz      1f
         lcall   $0xc000,$3
         movw    %cs, %ax
-        movw    %ax, %ds                                        # Bios might have played with that
+        movw    %ax, %ds                # Bios might have played with that
         movw    %ax, %ss
 1:
 
@@ -61,12 +68,15 @@ wakeup_code:
 
         movb    $0xa2, %al      ;  outb %al, $0x80
         
-        lidt    %ds:idt_48a - wakeup_code
-        xorl    %eax, %eax
-        movw    %ds, %ax                        # (Convert %ds:gdt to a linear ptr)
-        shll    $4, %eax
-        addl    $(gdta - wakeup_code), %eax
-        movl    %eax, gdt_48a +2 - wakeup_code
+        mov     %ds, %ax                        # Find 32bit wakeup_code addr
+        movzx   %ax, %esi                       # (Convert %ds:gdt to a liner ptr)
+        shll    $4, %esi
+                                                # Fix up the vectors
+        addl    %esi, wakeup_32_vector - wakeup_code
+        addl    %esi, wakeup_long64_vector - wakeup_code
+        addl    %esi, gdt_48a + 2 - wakeup_code # Fixup the gdt pointer
+
+        lidtl   %ds:idt_48a - wakeup_code
         lgdtl   %ds:gdt_48a - wakeup_code       # load gdt with whatever is
                                                 # appropriate
 
@@ -75,86 +85,63 @@ wakeup_code:
         jmp     1f
 1:
 
-        .byte 0x66, 0xea                        # prefix + jmpi-opcode
-        .long   wakeup_32 - __START_KERNEL_map
-        .word   __KERNEL_CS
+        ljmpl   *(wakeup_32_vector - wakeup_code)
+
+        .balign 4
+wakeup_32_vector:
+        .long   wakeup_32 - wakeup_code
+        .word   __KERNEL32_CS, 0
 
         .code32
 wakeup_32:
 # Running in this code, but at low address; paging is not yet turned on.
         movb    $0xa5, %al      ;  outb %al, $0x80
 
-        /* Check if extended functions are implemented */               
-        movl    $0x80000000, %eax
-        cpuid
-        cmpl    $0x80000000, %eax
-        jbe     bogus_cpu
-        wbinvd
-        mov     $0x80000001, %eax
-        cpuid
-        btl     $29, %edx
-        jnc     bogus_cpu
-        movl    %edx,%edi
-        
-        movw    $__KERNEL_DS, %ax
-        movw    %ax, %ds
-        movw    %ax, %es
-        movw    %ax, %fs
-        movw    %ax, %gs
-
-        movw    $__KERNEL_DS, %ax       
-        movw    %ax, %ss
+        movl    $__KERNEL_DS, %eax
+        movl    %eax, %ds
 
-        mov     $(wakeup_stack - __START_KERNEL_map), %esp
-        movl    saved_magic - __START_KERNEL_map, %eax
-        cmpl    $0x9abcdef0, %eax
-        jne     bogus_32_magic
+        movw    $0x0e00 + 'i', %ds:(0xb8012)
+        movb    $0xa8, %al      ;  outb %al, $0x80;
 
         /*
          * Prepare for entering 64bits mode
          */
 
-        /* Enable PAE mode and PGE */
+        /* Enable PAE */
         xorl    %eax, %eax
         btsl    $5, %eax
-        btsl    $7, %eax
         movl    %eax, %cr4
 
         /* Setup early boot stage 4 level pagetables */
-        movl    $(wakeup_level4_pgt - __START_KERNEL_map), %eax
+        leal    (wakeup_level4_pgt - wakeup_code)(%esi), %eax
         movl    %eax, %cr3
 
-        /* Setup EFER (Extended Feature Enable Register) */
-        movl    $MSR_EFER, %ecx
-        rdmsr
-        /* Fool rdmsr and reset %eax to avoid dependences */
-        xorl    %eax, %eax
+        /* Check if nx is implemented */
+        movl    $0x80000001, %eax
+        cpuid
+        movl    %edx,%edi
+
         /* Enable Long Mode */
+        xorl    %eax, %eax
         btsl    $_EFER_LME, %eax
-        /* Enable System Call */
-        btsl    $_EFER_SCE, %eax
 
-        /* No Execute supported? */     
+        /* No Execute supported? */
         btl     $20,%edi
         jnc     1f
         btsl    $_EFER_NX, %eax
-1:      
                                 
         /* Make changes effective */
+1:      movl    $MSR_EFER, %ecx
+        xorl    %edx, %edx
         wrmsr
-        wbinvd
 
         xorl    %eax, %eax
         btsl    $31, %eax                       /* Enable paging and in turn activate Long Mode */
         btsl    $0, %eax                        /* Enable protected mode */
-        btsl    $1, %eax                        /* Enable MP */
-        btsl    $4, %eax                        /* Enable ET */
-        btsl    $5, %eax                        /* Enable NE */
-        btsl    $16, %eax                       /* Enable WP */
-        btsl    $18, %eax                       /* Enable AM */
 
         /* Make changes effective */
         movl    %eax, %cr0
+
         /* At this point:
                 CR4.PAE must be 1
                 CS.L must be 0
@@ -162,11 +149,6 @@ wakeup_32:
                 Next instruction must be a branch
                 This must be on identity-mapped page
         */
-        jmp     reach_compatibility_mode
-reach_compatibility_mode:
-        movw    $0x0e00 + 'i', %ds:(0xb8012)
-        movb    $0xa8, %al      ;  outb %al, $0x80;     
-                
         /*
          * At this point we're in long mode but in 32bit compatibility mode
          * with EFER.LME = 1, CS.L = 0, CS.D = 1 (and in turn
@@ -174,24 +156,19 @@ reach_compatibility_mode:
          * the new gdt/idt that has __KERNEL_CS with CS.L = 1.
          */
 
-        movw    $0x0e00 + 'n', %ds:(0xb8014)
-        movb    $0xa9, %al      ;  outb %al, $0x80
-        
-        /* Load new GDT with the 64bit segment using 32bit descriptor */
-        movl    $(pGDT32 - __START_KERNEL_map), %eax
-        lgdt    (%eax)
-
-        movl    $(wakeup_jumpvector - __START_KERNEL_map), %eax
         /* Finally jump in 64bit mode */
-        ljmp    *(%eax)
+        ljmp    *(wakeup_long64_vector - wakeup_code)(%esi)
 
-wakeup_jumpvector:
-        .long   wakeup_long64 - __START_KERNEL_map
-        .word   __KERNEL_CS
+        .balign 4
+wakeup_long64_vector:
+        .long   wakeup_long64 - wakeup_code
+        .word   __KERNEL_CS, 0
 
 .code64
 
-        /*      Hooray, we are in Long 64-bit mode (but still running in low memory) */
+        /* Hooray, we are in Long 64-bit mode (but still running in
+         * low memory)
+         */
 wakeup_long64:
         /*
          * We must switch to a new descriptor in kernel space for the GDT
@@ -199,7 +176,15 @@ wakeup_long64:
          * addresses where we're currently running on. We have to do that here
          * because in 32bit we couldn't load a 64bit linear address.
          */
-        lgdt    cpu_gdt_descr - __START_KERNEL_map
+        lgdt    cpu_gdt_descr
+
+        movw    $0x0e00 + 'n', %ds:(0xb8014)
+        movb    $0xa9, %al      ;  outb %al, $0x80
+
+        movq    saved_magic, %rax
+        movq    $0x123456789abcdef0, %rdx
+        cmpq    %rdx, %rax
+        jne     bogus_64_magic
 
         movw    $0x0e00 + 'u', %ds:(0xb8016)
         
@@ -211,75 +196,58 @@ wakeup_long64:
         movw    %ax, %es
         movw    %ax, %fs
         movw    %ax, %gs
-        movq    saved_esp, %rsp
+        movq    saved_rsp, %rsp
 
         movw    $0x0e00 + 'x', %ds:(0xb8018)
-        movq    saved_ebx, %rbx
-        movq    saved_edi, %rdi
-        movq    saved_esi, %rsi
-        movq    saved_ebp, %rbp
+        movq    saved_rbx, %rbx
+        movq    saved_rdi, %rdi
+        movq    saved_rsi, %rsi
+        movq    saved_rbp, %rbp
 
         movw    $0x0e00 + '!', %ds:(0xb801a)
-        movq    saved_eip, %rax
+        movq    saved_rip, %rax
         jmp     *%rax
 
 .code32
 
         .align  64      
 gdta:
+        /* Its good to keep gdt in sync with one in trampoline.S */
         .word   0, 0, 0, 0                      # dummy
-
-        .word   0, 0, 0, 0                      # unused
-
-        .word   0xFFFF                          # 4Gb - (0x100000*0x1000 = 4Gb)
-        .word   0                               # base address = 0
-        .word   0x9B00                          # code read/exec. ??? Why I need 0x9B00 (as opposed to 0x9A00 in order for this to work?)
-        .word   0x00CF                          # granularity = 4096, 386
-                                                #  (+5th nibble of limit)
-
-        .word   0xFFFF                          # 4Gb - (0x100000*0x1000 = 4Gb)
-        .word   0                               # base address = 0
-        .word   0x9200                          # data read/write
-        .word   0x00CF                          # granularity = 4096, 386
-                                                #  (+5th nibble of limit)
-# this is 64bit descriptor for code
-        .word   0xFFFF
-        .word   0
-        .word   0x9A00                          # code read/exec
-        .word   0x00AF                          # as above, but it is long mode and with D=0
+        /* ??? Why I need the accessed bit set in order for this to work? */
+        .quad   0x00cf9b000000ffff              # __KERNEL32_CS
+        .quad   0x00af9b000000ffff              # __KERNEL_CS
+        .quad   0x00cf93000000ffff              # __KERNEL_DS
 
 idt_48a:
         .word   0                               # idt limit = 0
         .word   0, 0                            # idt base = 0L
 
 gdt_48a:
-        .word   0x8000                          # gdt limit=2048,
+        .word   0x800                           # gdt limit=2048,
                                                 #  256 GDT entries
-        .word   0, 0                            # gdt base (filled in later)
-        
+        .long   gdta - wakeup_code              # gdt base (relocated in later)
         
-real_save_gdt:  .word 0
-                .quad 0
 real_magic:     .quad 0
 video_mode:     .quad 0
 video_flags:    .quad 0
 
+.code16
 bogus_real_magic:
-        movb    $0xba,%al       ;  outb %al,$0x80               
+        movb    $0xba,%al       ;  outb %al,$0x80
         jmp bogus_real_magic
 
-bogus_32_magic:
+.code64
+bogus_64_magic:
         movb    $0xb3,%al       ;  outb %al,$0x80
-        jmp bogus_32_magic
+        jmp bogus_64_magic
 
-bogus_31_magic:
-        movb    $0xb1,%al       ;  outb %al,$0x80
-        jmp bogus_31_magic
-
-bogus_cpu:
-        movb    $0xbc,%al       ;  outb %al,$0x80
-        jmp bogus_cpu
+.code16
+no_longmode:
+        movb    $0xbc,%al       ;  outb %al,$0x80
+        jmp no_longmode
 
+#include "../verify_cpu.S"
         
 /* This code uses an extended set of video mode numbers. These include:
  * Aliases for standard modes
@@ -301,6 +269,7 @@ bogus_cpu:
 #define VIDEO_FIRST_V7 0x0900
 
 # Setting of user mode (AX=mode ID) => CF=success
+.code16
 mode_seta:
         movw    %ax, %bx
 #if 0
@@ -346,21 +315,18 @@ check_vesaa:
 
 _setbada: jmp setbada
 
-        .code64
-bogus_magic:
-        movw    $0x0e00 + 'B', %ds:(0xb8018)
-        jmp bogus_magic
-
-bogus_magic2:
-        movw    $0x0e00 + '2', %ds:(0xb8018)
-        jmp bogus_magic2
-        
-
 wakeup_stack_begin:     # Stack grows down
 
 .org    0xff0
 wakeup_stack:           # Just below end of page
 
+.org   0x1000
+ENTRY(wakeup_level4_pgt)
+        .quad   level3_ident_pgt - __START_KERNEL_map + _KERNPG_TABLE
+        .fill   510,8,0
+        /* (2^48-(2*1024*1024*1024))/(2^39) = 511 */
+        .quad   level3_kernel_pgt - __START_KERNEL_map + _KERNPG_TABLE
+
 ENTRY(wakeup_end)
         
 ##
@@ -373,28 +339,11 @@ ENTRY(wakeup_end)
 #
 # Returned address is location of code in low memory (past data and stack)
 #
+        .code64
 ENTRY(acpi_copy_wakeup_routine)
         pushq   %rax
-        pushq   %rcx
         pushq   %rdx
 
-        sgdt    saved_gdt
-        sidt    saved_idt
-        sldt    saved_ldt
-        str     saved_tss
-
-        movq    %cr3, %rdx
-        movq    %rdx, saved_cr3
-        movq    %cr4, %rdx
-        movq    %rdx, saved_cr4
-        movq    %cr0, %rdx
-        movq    %rdx, saved_cr0
-        sgdt    real_save_gdt - wakeup_start (,%rdi)
-        movl    $MSR_EFER, %ecx
-        rdmsr
-        movl    %eax, saved_efer
-        movl    %edx, saved_efer2
-
         movl    saved_video_mode, %edx
         movl    %edx, video_mode - wakeup_start (,%rdi)
         movl    acpi_video_flags, %edx
@@ -403,21 +352,13 @@ ENTRY(acpi_copy_wakeup_routine)
         movq    $0x123456789abcdef0, %rdx
         movq    %rdx, saved_magic
 
-        movl    saved_magic - __START_KERNEL_map, %eax
-        cmpl    $0x9abcdef0, %eax
-        jne     bogus_32_magic
-
-        # make sure %cr4 is set correctly (features, etc)
-        movl    saved_cr4 - __START_KERNEL_map, %eax
-        movq    %rax, %cr4
+        movq    saved_magic, %rax
+        movq    $0x123456789abcdef0, %rdx
+        cmpq    %rdx, %rax
+        jne     bogus_64_magic
 
-        movl    saved_cr0 - __START_KERNEL_map, %eax
-        movq    %rax, %cr0
-        jmp     1f              # Flush pipelines
-1:
         # restore the regs we used
         popq    %rdx
-        popq    %rcx
         popq    %rax
 ENTRY(do_suspend_lowlevel_s4bios)
         ret
@@ -450,13 +391,13 @@ do_suspend_lowlevel:
         movq %r15, saved_context_r15(%rip)
         pushfq ; popq saved_context_eflags(%rip)
 
-        movq    $.L97, saved_eip(%rip)
+        movq    $.L97, saved_rip(%rip)
 
-        movq %rsp,saved_esp
-        movq %rbp,saved_ebp
-        movq %rbx,saved_ebx
-        movq %rdi,saved_edi
-        movq %rsi,saved_esi
+        movq %rsp,saved_rsp
+        movq %rbp,saved_rbp
+        movq %rbx,saved_rbx
+        movq %rdi,saved_rdi
+        movq %rsi,saved_rsi
 
         addq    $8, %rsp
         movl    $3, %edi
@@ -503,25 +444,12 @@ do_suspend_lowlevel:
         
 .data
 ALIGN
-ENTRY(saved_ebp)        .quad   0
-ENTRY(saved_esi)        .quad   0
-ENTRY(saved_edi)        .quad   0
-ENTRY(saved_ebx)        .quad   0
+ENTRY(saved_rbp)        .quad   0
+ENTRY(saved_rsi)        .quad   0
+ENTRY(saved_rdi)        .quad   0
+ENTRY(saved_rbx)        .quad   0
 
-ENTRY(saved_eip)        .quad   0
-ENTRY(saved_esp)        .quad   0
+ENTRY(saved_rip)        .quad   0
+ENTRY(saved_rsp)        .quad   0
 
 ENTRY(saved_magic)      .quad   0
-
-ALIGN
-# saved registers
-saved_gdt:      .quad   0,0
-saved_idt:      .quad   0,0
-saved_ldt:      .quad   0
-saved_tss:      .quad   0
-
-saved_cr0:      .quad 0
-saved_cr3:      .quad 0
-saved_cr4:      .quad 0
-saved_efer:     .quad 0
-saved_efer2:    .quad 0