global smp_stackarray_ptr
global smp_bootstrap_bsp
global smp_bootstrap_corecount
global final_gdt_descriptor

extern smp_kinit
extern _pmem_vaddr
extern temp_debug_2

[bits 16]
smp_trampoline:
cli

in al, 0x92
or al, 2
out 0x92, al

xor ax, ax
mov ds, ax
lgdt [.gdt_descriptor_p]
mov eax, cr0
or eax, 0x1
mov cr0, eax ; now in long mode

jmp 0x8:smp_protected


;________________________________________________________________________________________

.gdt_start_p: ; we need to start with a null gdt
dd 0
dd 0

.gdt_code_p:
dw 0xffff
dw 0x0000
db 0x00
db 10011010b 
db 11001111b
db 0x0000

.gdt_data_p:
dw 0xffff
dw 0x0000
db 0x00
db 10010010b 
db 11001111b
db 0x0000

.gdt_end_p:

.gdt_descriptor_p: 
  dw .gdt_end_p - .gdt_start_p - 1 
dq .gdt_start_p

SMP_PROTECTED_CODE_SEGMENT equ .gdt_code_p - .gdt_start_p
SMP_PROTECTED_DATA_SEGMENT equ .gdt_data_p - .gdt_start_p

;________________________________________________________________________________________

smp_protected:

bits 32
mov ax, SMP_PROTECTED_DATA_SEGMENT
mov es, ax
mov ds, ax
mov ss, ax
mov es, ax
mov fs, ax
mov gs, ax


mov eax, 0x10000
mov cr3, eax

;setting up misc features
mov eax, cr4         
; PAE, OSFXSR, OSXMMEXCPT
or eax, 1 << 5 | 1 << 9 | 1 << 10
mov cr4, eax         


;set NX and LME
mov eax, 0x80000001
cpuid
mov ebx, edx
and ebx, 1 << 20
shr ebx, 9
  
mov ecx, 0xc0000080
rdmsr
or eax, 1 << 8 
or eax, ebx
wrmsr


;enable paging 
mov eax, cr0
or eax, 1 << 31 | 1 << 0; 
and ax, ~(1 << 2)
mov cr0, eax

lgdt [final_gdt_descriptor]


jmp SMP_LONG_CODE_SEGMENT:smp_longsetup

;________________________________________________________________________________________

bits 64
align 8
final_gdt:
.start:
;and now we set up a temporary GDT creating a 1:1 mapping
dw 0xffff
dw 0
db 0 ;this is the invalid GDT
db 0
db 1
db 0

;now for the code GDT:
.code:
dw 0 ; segment limit 15:00 (I don't think this matters in 64 bit mode!)
dw 0 ; base address 15:00
db 0 ; base address 23:16
db 10011010b  
db 00100000b 
db 0

.data:
dw 0 
dw 0 
db 0 
db 10010010b  
db 00100000b  
db 0

.end: ; later calculates offset in defs below

final_gdt_descriptor: 
  dw final_gdt.end - final_gdt.start - 1 
.offset:
dq final_gdt.start

SMP_LONG_CODE_SEGMENT equ final_gdt.code - final_gdt.start
SMP_LONG_DATA_SEGMENT equ final_gdt.data - final_gdt.start

smp_longsetup:
mov eax, 1
cpuid
shr ebx, 24

mov rsi, [smp_stackarray_ptr]
.search_for_cpuinfo:
mov al, [rsi]
cmp al, bl
je smp_longsetup.cpuinfo_found
add rsi, 10
jmp .search_for_cpuinfo
.cpuinfo_found:
mov bl, [rsi + 1]
mov rsp, QWORD [rsi + 2] ;core_stack.stack


mov rax, [smp_bootstrap_corecount]
lock inc BYTE [rax]


smp_enter_kernel:
test bl, bl
jz .wait_for_gdt

; setting gdt
mov rax, 0xffff800000000000
or QWORD [final_gdt_descriptor.offset], rax
mov QWORD [smp_lock], 0


.wait_for_gdt:

mov al, [smp_lock]
test al, al
pause
jnz .wait_for_gdt
lgdt [final_gdt_descriptor]


mov rax, smp_kinit
jmp rax

cli
hlt
jmp $

align 8 
smp_stackarray_ptr: dq 0
smp_lock: dq 1
screen_debug_pos: dq 0
smp_bootstrap_bsp: db 0
smp_bootstrap_corecount: db 0

times 512 - ($ - $$) db 0