bits 16

section .rodata

global smp_trampoline_start
smp_trampoline_start:
    cli
    cld

    mov ebx, cs
    shl ebx, 4

    o32 lidt [cs:(invalid_idt - smp_trampoline_start)]
    o32 lgdt [cs:(passed_info.gdtr - smp_trampoline_start)]

    lea eax, [ebx + (.mode32 - smp_trampoline_start)]
    mov [cs:(.farjmp_off - smp_trampoline_start)], eax

    mov eax, 0x00000011
    mov cr0, eax
    o32 jmp far [cs:(.farjmp - smp_trampoline_start)]

  .farjmp:
    .farjmp_off: dd 0
    .farjmp_seg: dd 0x18

    bits 32
  .mode32:
    mov ax, 0x20
    mov ds, ax
    mov es, ax
    mov fs, ax
    mov gs, ax
    mov ss, ax

    xor eax, eax
    lldt ax

    xor eax, eax
    mov cr4, eax

    mov ecx, 0x277
    mov eax, 0x00070406
    mov edx, 0x00000105
    wrmsr

    test dword [ebx + (passed_info.target_mode - smp_trampoline_start)], (1 << 2)
    jz .nox2apic

    mov ecx, 0x1b
    rdmsr
    bts eax, 10
    bts eax, 11
    wrmsr

  .nox2apic:
    lea esp, [ebx + (temp_stack.top - smp_trampoline_start)]

    test dword [ebx + (passed_info.target_mode - smp_trampoline_start)], (1 << 0)
    jz parking32

    mov eax, cr4
    bts eax, 5
    mov cr4, eax

    mov ecx, 0xc0000080
    mov eax, 0x100
    xor edx, edx
    wrmsr

    test dword [ebx + (passed_info.target_mode - smp_trampoline_start)], (1 << 1)
    jz .no5lv

    mov eax, cr4
    bts eax, 12
    mov cr4, eax

  .no5lv:
    mov eax, dword [ebx + (passed_info.pagemap - smp_trampoline_start)]
    mov cr3, eax

    mov eax, cr0
    bts eax, 31
    mov cr0, eax

    lea eax, [ebx + (.mode64 - smp_trampoline_start)]
    push 0x28
    push eax
    retf

    bits 64
  .mode64:
    mov ax, 0x30
    mov ds, ax
    mov es, ax
    mov fs, ax
    mov gs, ax
    mov ss, ax

    mov ebx, ebx
    test dword [rbx + (passed_info.target_mode - smp_trampoline_start)], (1 << 3)
    jz .nonx

    mov ecx, 0xc0000080
    rdmsr
    bts eax, 11
    wrmsr

  .nonx:
    test dword [rbx + (passed_info.target_mode - smp_trampoline_start)], (1 << 4)
    jz .nowp

    mov rax, cr0
    bts rax, 16
    mov cr0, rax

  .nowp:
    mov rax, qword [rbx + (passed_info.hhdm - smp_trampoline_start)]
    add qword [rbx + (passed_info.gdtr - smp_trampoline_start) + 2], rax
    lgdt [rbx + (passed_info.gdtr - smp_trampoline_start)]

    lea rax, [rax + rbx + (parking64 - smp_trampoline_start)]

    jmp rax

bits 32
parking32:
    mov edi, dword [ebx + (passed_info.smp_info_struct - smp_trampoline_start)]
    mov eax, 1
    lock xchg dword [ebx + (passed_info.booted_flag - smp_trampoline_start)], eax

    xor eax, eax
  .loop:
    lock xadd dword [edi + 16], eax
    test eax, eax
    jnz .out
    pause
    jmp .loop

  .out:
    mov esp, dword [edi + 8]
    push 0
    push edi
    push eax
    xor eax, eax
    xor ebx, ebx
    xor ecx, ecx
    xor edx, edx
    xor esi, esi
    xor edi, edi
    xor ebp, ebp
    ret

bits 64
parking64:
    mov ebx, ebx
    mov edi, dword [rbx + (passed_info.smp_info_struct - smp_trampoline_start)]
    add rdi, qword [rbx + (passed_info.hhdm - smp_trampoline_start)]
    mov eax, 1
    lock xchg dword [rbx + (passed_info.booted_flag - smp_trampoline_start)], eax

    xor eax, eax
  .loop:
    lock xadd qword [rdi + 16], rax
    test rax, rax
    jnz .out
    pause
    jmp .loop

  .out:
    mov rsp, qword [rdi + 8]
    push 0
    push rax
    xor rax, rax
    xor rbx, rbx
    xor rcx, rcx
    xor rdx, rdx
    xor rsi, rsi
    xor rbp, rbp
    xor r8,  r8
    xor r9,  r9
    xor r10, r10
    xor r11, r11
    xor r12, r12
    xor r13, r13
    xor r14, r14
    xor r15, r15
    ret

align 16
temp_stack:
    times 128 db 0
  .top:

invalid_idt:
    times 2 dq 0

align 16
passed_info:
    .booted_flag db 0
    .target_mode db 0
    .pagemap dd 0
    .smp_info_struct dd 0
    .gdtr:
        dw 0
        dq 0
    .hhdm:
        dq 0

smp_trampoline_end:

global smp_trampoline_size
smp_trampoline_size dq smp_trampoline_end - smp_trampoline_start