From 5da8f2b69ba7a3ed0d9d4880ec6ec41485ffd724 Mon Sep 17 00:00:00 2001
From: Oleksandr Tymoshenko <gonzo@FreeBSD.org>
Date: Thu, 29 Oct 2015 22:12:03 +0000
Subject: [PATCH] Fix LEAVE_HYP macro: spsr is not guaranteed to contain valid
 value at this point, e.g. on RaspberryPi 2 when control is passed from loader
 to kernel it contains garbage. So we use cpsr as a base for new cpsr value:
 if we have reached this point it means current value is OK

Reviewed by:	andrew
---
 sys/arm/arm/locore-v6.S | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/sys/arm/arm/locore-v6.S b/sys/arm/arm/locore-v6.S
index 5a5222877da9..6aa61f358af1 100644
--- a/sys/arm/arm/locore-v6.S
+++ b/sys/arm/arm/locore-v6.S
@@ -74,9 +74,11 @@ __FBSDID("$FreeBSD$");
 	teq	r0, #(PSR_HYP32_MODE) /* Hyp Mode? */			;\
 	bne	1f							;\
 	/* Ensure that IRQ, FIQ and Aborts will be disabled after eret */ ;\
-	mrs	r0, spsr						;\
+	mrs	r0, cpsr						;\
+	bic	r0, r0, #(PSR_MODE)					;\
+	orr	r0, r0, #(PSR_SVC32_MODE)				;\
 	orr	r0, r0, #(PSR_I | PSR_F | PSR_A)			;\
-	msr	spsr, r0						;\
+	msr	spsr_cxsf, r0						;\
 	/* Exit hypervisor mode */					;\
 	adr	lr, 1f							;\
 	MSR_ELR_HYP(14)							;\