[mirror_edk2.git] / UefiCpuPkg / Library / MpInitLib / X64 / MpFuncs.nasm

;------------------------------------------------------------------------------ ;\r
; Copyright (c) 2015 - 2016, Intel Corporation. All rights reserved.<BR>\r
; This program and the accompanying materials\r
; are licensed and made available under the terms and conditions of the BSD License\r
; which accompanies this distribution.  The full text of the license may be found at\r
; http://opensource.org/licenses/bsd-license.php.\r
;\r
; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
;\r
; Module Name:\r
;\r
;   MpFuncs.nasm\r
;\r
; Abstract:\r
;\r
;   This is the assembly code for MP support\r
;\r
;-------------------------------------------------------------------------------\r
\r
%include "MpEqu.inc"\r
extern ASM_PFX(InitializeFloatingPointUnits)\r
\r
DEFAULT REL\r
\r
SECTION .text\r
\r
;-------------------------------------------------------------------------------------\r
;RendezvousFunnelProc  procedure follows. All APs execute their procedure. This\r
;procedure serializes all the AP processors through an Init sequence. It must be\r
;noted that APs arrive here very raw...ie: real mode, no stack.\r
;ALSO THIS PROCEDURE IS EXECUTED BY APs ONLY ON 16 BIT MODE. HENCE THIS PROC\r
;IS IN MACHINE CODE.\r
;-------------------------------------------------------------------------------------\r
global ASM_PFX(RendezvousFunnelProc)\r
ASM_PFX(RendezvousFunnelProc):\r
RendezvousFunnelProcStart:\r
; At this point CS = 0x(vv00) and ip= 0x0.\r
; Save BIST information to ebp firstly\r
\r
BITS 16\r
    mov        ebp, eax                        ; Save BIST information\r
\r
    mov        ax, cs\r
    mov        ds, ax\r
    mov        es, ax\r
    mov        ss, ax\r
    xor        ax, ax\r
    mov        fs, ax\r
    mov        gs, ax\r
\r
    mov        si,  BufferStartLocation\r
    mov        ebx, [si]\r
\r
    mov        di,  ModeOffsetLocation\r
    mov        eax, [di]\r
    mov        di,  CodeSegmentLocation\r
    mov        edx, [di]\r
    mov        di,  ax\r
    sub        di,  02h\r
    mov        [di],dx                         ; Patch long mode CS\r
    sub        di,  04h\r
    add        eax, ebx\r
    mov        [di],eax                        ; Patch address\r
\r
    mov        si, GdtrLocation\r
o32 lgdt       [cs:si]\r
\r
    mov        si, IdtrLocation\r
o32 lidt       [cs:si]\r
\r
\r
    mov        di,  DataSegmentLocation\r
    mov        edi, [di]                   ; Save long mode DS in edi\r
\r
    mov        si, Cr3Location             ; Save CR3 in ecx\r
    mov        ecx, [si]\r
\r
    xor        ax,  ax\r
    mov        ds,  ax                     ; Clear data segment\r
\r
    mov        eax, cr0                    ; Get control register 0\r
    or         eax, 000000003h             ; Set PE bit (bit #0) & MP\r
    mov        cr0, eax\r
\r
    mov        eax, cr4\r
    bts        eax, 5\r
    mov        cr4, eax\r
\r
    mov        cr3, ecx                    ; Load CR3\r
\r
    mov        ecx, 0c0000080h             ; EFER MSR number\r
    rdmsr                                  ; Read EFER\r
    bts        eax, 8                      ; Set LME=1\r
    wrmsr                                  ; Write EFER\r
\r
    mov        eax, cr0                    ; Read CR0\r
    bts        eax, 31                     ; Set PG=1\r
    mov        cr0, eax                    ; Write CR0\r
\r
    jmp        0:strict dword 0  ; far jump to long mode\r
BITS 64\r
LongModeStart:\r
    mov        eax, edi\r
    mov        ds,  ax\r
    mov        es,  ax\r
    mov        ss,  ax\r
\r
    mov        esi, ebx\r
    mov        edi, esi\r
    add        edi, LockLocation\r
    mov        rax, NotVacantFlag\r
\r
TestLock:\r
    xchg       qword [edi], rax\r
    cmp        rax, NotVacantFlag\r
    jz         TestLock\r
\r
    mov        edi, esi\r
    add        edi, NumApsExecutingLocation\r
    inc        dword [edi]\r
    mov        ebx, [edi]\r
\r
ProgramStack:\r
    mov        edi, esi\r
    add        edi, StackSizeLocation\r
    mov        rax, qword [edi]\r
    mov        edi, esi\r
    add        edi, StackStartAddressLocation\r
    add        rax, qword [edi]\r
    mov        rsp, rax\r
    mov        qword [edi], rax\r
\r
Releaselock:\r
    mov        rax, VacantFlag\r
    mov        edi, esi\r
    add        edi, LockLocation\r
    xchg       qword [edi], rax\r
\r
CProcedureInvoke:\r
    push       rbp               ; Push BIST data at top of AP stack\r
    xor        rbp, rbp          ; Clear ebp for call stack trace\r
    push       rbp\r
    mov        rbp, rsp\r
\r
    mov        rax, ASM_PFX(InitializeFloatingPointUnits)\r
    sub        rsp, 20h\r
    call       rax               ; Call assembly function to initialize FPU per UEFI spec\r
    add        rsp, 20h\r
\r
    mov        edx, ebx          ; edx is NumApsExecuting\r
    mov        ecx, esi\r
    add        ecx, LockLocation ; rcx is address of exchange info data buffer\r
\r
    mov        edi, esi\r
    add        edi, ApProcedureLocation\r
    mov        rax, qword [edi]\r
\r
    sub        rsp, 20h\r
    call       rax               ; Invoke C function\r
    add        rsp, 20h\r
    jmp        $                 ; Should never reach here\r
\r
RendezvousFunnelProcEnd:\r
\r
;-------------------------------------------------------------------------------------\r
;  AsmGetAddressMap (&AddressMap);\r
;-------------------------------------------------------------------------------------\r
global ASM_PFX(AsmGetAddressMap)\r
ASM_PFX(AsmGetAddressMap):\r
    mov        rax, ASM_PFX(RendezvousFunnelProc)\r
    mov        qword [rcx], rax\r
    mov        qword [rcx +  8h], LongModeStart - RendezvousFunnelProcStart\r
    mov        qword [rcx + 10h], RendezvousFunnelProcEnd - RendezvousFunnelProcStart\r
    ret\r
\r
;-------------------------------------------------------------------------------------\r
;AsmExchangeRole procedure follows. This procedure executed by current BSP, that is\r
;about to become an AP. It switches its stack with the current AP.\r
;AsmExchangeRole (IN   CPU_EXCHANGE_INFO    *MyInfo, IN   CPU_EXCHANGE_INFO    *OthersInfo);\r
;-------------------------------------------------------------------------------------\r
global ASM_PFX(AsmExchangeRole)\r
ASM_PFX(AsmExchangeRole):\r
    ; DO NOT call other functions in this function, since 2 CPU may use 1 stack\r
    ; at the same time. If 1 CPU try to call a function, stack will be corrupted.\r
\r
    push       rax\r
    push       rbx\r
    push       rcx\r
    push       rdx\r
    push       rsi\r
    push       rdi\r
    push       rbp\r
    push       r8\r
    push       r9\r
    push       r10\r
    push       r11\r
    push       r12\r
    push       r13\r
    push       r14\r
    push       r15\r
\r
    mov        rax, cr0\r
    push       rax\r
\r
    mov        rax, cr4\r
    push       rax\r
\r
    ; rsi contains MyInfo pointer\r
    mov        rsi, rcx\r
\r
    ; rdi contains OthersInfo pointer\r
    mov        rdi, rdx\r
\r
    ;Store EFLAGS, GDTR and IDTR regiter to stack\r
    pushfq\r
    sgdt       [rsi + 16]\r
    sidt       [rsi + 26]\r
\r
    ; Store the its StackPointer\r
    mov        [rsi + 8], rsp\r
\r
    ; update its switch state to STORED\r
    mov        byte [rsi], CPU_SWITCH_STATE_STORED\r
\r
WaitForOtherStored:\r
    ; wait until the other CPU finish storing its state\r
    cmp        byte [rdi], CPU_SWITCH_STATE_STORED\r
    jz         OtherStored\r
    pause\r
    jmp        WaitForOtherStored\r
\r
OtherStored:\r
    ; Since another CPU already stored its state, load them\r
    ; load GDTR value\r
    lgdt       [rdi + 16]\r
\r
    ; load IDTR value\r
    lidt       [rdi + 26]\r
\r
    ; load its future StackPointer\r
    mov        rsp, [rdi + 8]\r
\r
    ; update the other CPU's switch state to LOADED\r
    mov        byte [rdi], CPU_SWITCH_STATE_LOADED\r
\r
WaitForOtherLoaded:\r
    ; wait until the other CPU finish loading new state,\r
    ; otherwise the data in stack may corrupt\r
    cmp        byte [rsi], CPU_SWITCH_STATE_LOADED\r
    jz         OtherLoaded\r
    pause\r
    jmp        WaitForOtherLoaded\r
\r
OtherLoaded:\r
    ; since the other CPU already get the data it want, leave this procedure\r
    popfq\r
\r
    pop        rax\r
    mov        cr4, rax\r
\r
    pop        rax\r
    mov        cr0, rax\r
\r
    pop        r15\r
    pop        r14\r
    pop        r13\r
    pop        r12\r
    pop        r11\r
    pop        r10\r
    pop        r9\r
    pop        r8\r
    pop        rbp\r
    pop        rdi\r
    pop        rsi\r
    pop        rdx\r
    pop        rcx\r
    pop        rbx\r
    pop        rax\r
\r
    ret\r
Commit	Line	Data
	1	;------------------------------------------------------------------------------ ;\r
	2	; Copyright (c) 2015 - 2016, Intel Corporation. All rights reserved.<BR>\r
	3	; This program and the accompanying materials\r
	4	; are licensed and made available under the terms and conditions of the BSD License\r
	5	; which accompanies this distribution. The full text of the license may be found at\r
	6	; http://opensource.org/licenses/bsd-license.php.\r
	7	;\r
	8	; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
	9	; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
	10	;\r
	11	; Module Name:\r
	12	;\r
	13	; MpFuncs.nasm\r
	14	;\r
	15	; Abstract:\r
	16	;\r
	17	; This is the assembly code for MP support\r
	18	;\r
	19	;-------------------------------------------------------------------------------\r
	20	\r
	21	%include "MpEqu.inc"\r
	22	extern ASM_PFX(InitializeFloatingPointUnits)\r
	23	\r
	24	DEFAULT REL\r
	25	\r
	26	SECTION .text\r
	27	\r
	28	;-------------------------------------------------------------------------------------\r
	29	;RendezvousFunnelProc procedure follows. All APs execute their procedure. This\r
	30	;procedure serializes all the AP processors through an Init sequence. It must be\r
	31	;noted that APs arrive here very raw...ie: real mode, no stack.\r
	32	;ALSO THIS PROCEDURE IS EXECUTED BY APs ONLY ON 16 BIT MODE. HENCE THIS PROC\r
	33	;IS IN MACHINE CODE.\r
	34	;-------------------------------------------------------------------------------------\r
	35	global ASM_PFX(RendezvousFunnelProc)\r
	36	ASM_PFX(RendezvousFunnelProc):\r
	37	RendezvousFunnelProcStart:\r
	38	; At this point CS = 0x(vv00) and ip= 0x0.\r
	39	; Save BIST information to ebp firstly\r
	40	\r
	41	BITS 16\r
	42	mov ebp, eax ; Save BIST information\r
	43	\r
	44	mov ax, cs\r
	45	mov ds, ax\r
	46	mov es, ax\r
	47	mov ss, ax\r
	48	xor ax, ax\r
	49	mov fs, ax\r
	50	mov gs, ax\r
	51	\r
	52	mov si, BufferStartLocation\r
	53	mov ebx, [si]\r
	54	\r
	55	mov di, ModeOffsetLocation\r
	56	mov eax, [di]\r
	57	mov di, CodeSegmentLocation\r
	58	mov edx, [di]\r
	59	mov di, ax\r
	60	sub di, 02h\r
	61	mov [di],dx ; Patch long mode CS\r
	62	sub di, 04h\r
	63	add eax, ebx\r
	64	mov [di],eax ; Patch address\r
	65	\r
	66	mov si, GdtrLocation\r
	67	o32 lgdt [cs:si]\r
	68	\r
	69	mov si, IdtrLocation\r
	70	o32 lidt [cs:si]\r
	71	\r
	72	\r
	73	mov di, DataSegmentLocation\r
	74	mov edi, [di] ; Save long mode DS in edi\r
	75	\r
	76	mov si, Cr3Location ; Save CR3 in ecx\r
	77	mov ecx, [si]\r
	78	\r
	79	xor ax, ax\r
	80	mov ds, ax ; Clear data segment\r
	81	\r
	82	mov eax, cr0 ; Get control register 0\r
	83	or eax, 000000003h ; Set PE bit (bit #0) & MP\r
	84	mov cr0, eax\r
	85	\r
	86	mov eax, cr4\r
	87	bts eax, 5\r
	88	mov cr4, eax\r
	89	\r
	90	mov cr3, ecx ; Load CR3\r
	91	\r
	92	mov ecx, 0c0000080h ; EFER MSR number\r
	93	rdmsr ; Read EFER\r
	94	bts eax, 8 ; Set LME=1\r
	95	wrmsr ; Write EFER\r
	96	\r
	97	mov eax, cr0 ; Read CR0\r
	98	bts eax, 31 ; Set PG=1\r
	99	mov cr0, eax ; Write CR0\r
	100	\r
	101	jmp 0:strict dword 0 ; far jump to long mode\r
	102	BITS 64\r
	103	LongModeStart:\r
	104	mov eax, edi\r
	105	mov ds, ax\r
	106	mov es, ax\r
	107	mov ss, ax\r
	108	\r
	109	mov esi, ebx\r
	110	mov edi, esi\r
	111	add edi, LockLocation\r
	112	mov rax, NotVacantFlag\r
	113	\r
	114	TestLock:\r
	115	xchg qword [edi], rax\r
	116	cmp rax, NotVacantFlag\r
	117	jz TestLock\r
	118	\r
	119	mov edi, esi\r
	120	add edi, NumApsExecutingLocation\r
	121	inc dword [edi]\r
	122	mov ebx, [edi]\r
	123	\r
	124	ProgramStack:\r
	125	mov edi, esi\r
	126	add edi, StackSizeLocation\r
	127	mov rax, qword [edi]\r
	128	mov edi, esi\r
	129	add edi, StackStartAddressLocation\r
	130	add rax, qword [edi]\r
	131	mov rsp, rax\r
	132	mov qword [edi], rax\r
	133	\r
	134	Releaselock:\r
	135	mov rax, VacantFlag\r
	136	mov edi, esi\r
	137	add edi, LockLocation\r
	138	xchg qword [edi], rax\r
	139	\r
	140	CProcedureInvoke:\r
	141	push rbp ; Push BIST data at top of AP stack\r
	142	xor rbp, rbp ; Clear ebp for call stack trace\r
	143	push rbp\r
	144	mov rbp, rsp\r
	145	\r
	146	mov rax, ASM_PFX(InitializeFloatingPointUnits)\r
	147	sub rsp, 20h\r
	148	call rax ; Call assembly function to initialize FPU per UEFI spec\r
	149	add rsp, 20h\r
	150	\r
	151	mov edx, ebx ; edx is NumApsExecuting\r
	152	mov ecx, esi\r
	153	add ecx, LockLocation ; rcx is address of exchange info data buffer\r
	154	\r
	155	mov edi, esi\r
	156	add edi, ApProcedureLocation\r
	157	mov rax, qword [edi]\r
	158	\r
	159	sub rsp, 20h\r
	160	call rax ; Invoke C function\r
	161	add rsp, 20h\r
	162	jmp $ ; Should never reach here\r
	163	\r
	164	RendezvousFunnelProcEnd:\r
	165	\r
	166	;-------------------------------------------------------------------------------------\r
	167	; AsmGetAddressMap (&AddressMap);\r
	168	;-------------------------------------------------------------------------------------\r
	169	global ASM_PFX(AsmGetAddressMap)\r
	170	ASM_PFX(AsmGetAddressMap):\r
	171	mov rax, ASM_PFX(RendezvousFunnelProc)\r
	172	mov qword [rcx], rax\r
	173	mov qword [rcx + 8h], LongModeStart - RendezvousFunnelProcStart\r
	174	mov qword [rcx + 10h], RendezvousFunnelProcEnd - RendezvousFunnelProcStart\r
	175	ret\r
	176	\r
	177	;-------------------------------------------------------------------------------------\r
	178	;AsmExchangeRole procedure follows. This procedure executed by current BSP, that is\r
	179	;about to become an AP. It switches its stack with the current AP.\r
	180	;AsmExchangeRole (IN CPU_EXCHANGE_INFO MyInfo, IN CPU_EXCHANGE_INFO OthersInfo);\r
	181	;-------------------------------------------------------------------------------------\r
	182	global ASM_PFX(AsmExchangeRole)\r
	183	ASM_PFX(AsmExchangeRole):\r
	184	; DO NOT call other functions in this function, since 2 CPU may use 1 stack\r
	185	; at the same time. If 1 CPU try to call a function, stack will be corrupted.\r
	186	\r
	187	push rax\r
	188	push rbx\r
	189	push rcx\r
	190	push rdx\r
	191	push rsi\r
	192	push rdi\r
	193	push rbp\r
	194	push r8\r
	195	push r9\r
	196	push r10\r
	197	push r11\r
	198	push r12\r
	199	push r13\r
	200	push r14\r
	201	push r15\r
	202	\r
	203	mov rax, cr0\r
	204	push rax\r
	205	\r
	206	mov rax, cr4\r
	207	push rax\r
	208	\r
	209	; rsi contains MyInfo pointer\r
	210	mov rsi, rcx\r
	211	\r
	212	; rdi contains OthersInfo pointer\r
	213	mov rdi, rdx\r
	214	\r
	215	;Store EFLAGS, GDTR and IDTR regiter to stack\r
	216	pushfq\r
	217	sgdt [rsi + 16]\r
	218	sidt [rsi + 26]\r
	219	\r
	220	; Store the its StackPointer\r
	221	mov [rsi + 8], rsp\r
	222	\r
	223	; update its switch state to STORED\r
	224	mov byte [rsi], CPU_SWITCH_STATE_STORED\r
	225	\r
	226	WaitForOtherStored:\r
	227	; wait until the other CPU finish storing its state\r
	228	cmp byte [rdi], CPU_SWITCH_STATE_STORED\r
	229	jz OtherStored\r
	230	pause\r
	231	jmp WaitForOtherStored\r
	232	\r
	233	OtherStored:\r
	234	; Since another CPU already stored its state, load them\r
	235	; load GDTR value\r
	236	lgdt [rdi + 16]\r
	237	\r
	238	; load IDTR value\r
	239	lidt [rdi + 26]\r
	240	\r
	241	; load its future StackPointer\r
	242	mov rsp, [rdi + 8]\r
	243	\r
	244	; update the other CPU's switch state to LOADED\r
	245	mov byte [rdi], CPU_SWITCH_STATE_LOADED\r
	246	\r
	247	WaitForOtherLoaded:\r
	248	; wait until the other CPU finish loading new state,\r
	249	; otherwise the data in stack may corrupt\r
	250	cmp byte [rsi], CPU_SWITCH_STATE_LOADED\r
	251	jz OtherLoaded\r
	252	pause\r
	253	jmp WaitForOtherLoaded\r
	254	\r
	255	OtherLoaded:\r
	256	; since the other CPU already get the data it want, leave this procedure\r
	257	popfq\r
	258	\r
	259	pop rax\r
	260	mov cr4, rax\r
	261	\r
	262	pop rax\r
	263	mov cr0, rax\r
	264	\r
	265	pop r15\r
	266	pop r14\r
	267	pop r13\r
	268	pop r12\r
	269	pop r11\r
	270	pop r10\r
	271	pop r9\r
	272	pop r8\r
	273	pop rbp\r
	274	pop rdi\r
	275	pop rsi\r
	276	pop rdx\r
	277	pop rcx\r
	278	pop rbx\r
	279	pop rax\r
	280	\r
	281	ret\r