KVM: arm64: Extract __do_hyp_init into a helper function

	mov	x0, #SMCCC_RET_NOT_SUPPORTED

	eret

1:	ldr	x0, [x1, #NVHE_INIT_TPIDR_EL2]

	msr	tpidr_el2, x0

1:	mov	x0, x1

	mov	x4, lr

	bl	___kvm_hyp_init

	mov	lr, x4

	ldr	x0, [x1, #NVHE_INIT_MAIR_EL2]

	msr	mair_el2, x0

	/* Hello, World! */

	mov	x0, #SMCCC_RET_SUCCESS

	eret

SYM_CODE_END(__kvm_hyp_init)

/*

 * Initialize the hypervisor in EL2.

 *

 * Only uses x0..x3 so as to not clobber callee-saved SMCCC registers

 * and leave x4 for the caller.

 *

 * x0: struct kvm_nvhe_init_params PA

 */

SYM_CODE_START_LOCAL(___kvm_hyp_init)

	ldr	x1, [x0, #NVHE_INIT_TPIDR_EL2]

	msr	tpidr_el2, x1

	ldr	x1, [x0, #NVHE_INIT_STACK_HYP_VA]

	mov	sp, x1

	ldr	x0, [x1, #NVHE_INIT_STACK_HYP_VA]

	mov	sp, x0

	ldr	x1, [x0, #NVHE_INIT_MAIR_EL2]

	msr	mair_el2, x1

	ldr	x0, [x1, #NVHE_INIT_PGD_PA]

	phys_to_ttbr x2, x0

	ldr	x1, [x0, #NVHE_INIT_PGD_PA]

	phys_to_ttbr x2, x1

alternative_if ARM64_HAS_CNP

	orr	x2, x2, #TTBR_CNP_BIT

alternative_else_nop_endif

	/*

	 * Set the PS bits in TCR_EL2.

	 */

	ldr	x0, [x1, #NVHE_INIT_TCR_EL2]

	tcr_compute_pa_size x0, #TCR_EL2_PS_SHIFT, x1, x2

	msr	tcr_el2, x0

	ldr	x1, [x0, #NVHE_INIT_TCR_EL2]

	tcr_compute_pa_size x1, #TCR_EL2_PS_SHIFT, x2, x3

	msr	tcr_el2, x1

	isb

	kimg_hyp_va x0, x1

	msr	vbar_el2, x0

	/* Hello, World! */

	mov	x0, #SMCCC_RET_SUCCESS

	eret

SYM_CODE_END(__kvm_hyp_init)

	ret

SYM_CODE_END(___kvm_hyp_init)

SYM_CODE_START(__kvm_handle_stub_hvc)

	cmp	x0, #HVC_SOFT_RESTART