Only restore FPCR if different from saved value.
Bug: 129131324
Change-Id: Id6db43e45d66d63a70a247d59ab355651420f897
diff --git a/src/arch/aarch64/hypervisor/exceptions.S b/src/arch/aarch64/hypervisor/exceptions.S
index bca781d..ea17ffa 100644
--- a/src/arch/aarch64/hypervisor/exceptions.S
+++ b/src/arch/aarch64/hypervisor/exceptions.S
@@ -386,9 +386,18 @@
ldp q30, q31, [x2, #32 * 15]!
ldp x3, x4, [x2, #32 * 1]
msr fpsr, x3
- /* TODO: Optimise by only performing expensive restore if changed. */
- msr fpcr, x4
+ /*
+ * Only restore FPCR if changed, to avoid expensive
+ * self-synchronising operation where possible.
+ */
+ mrs x5, fpcr
+ cmp x5, x4
+ b.eq vcpu_restore_lazy_and_run
+ msr fpcr, x4
+ /* Intentional fallthrough. */
+
+vcpu_restore_lazy_and_run:
/* Restore lazy registers. */
ldp x24, x25, [x0, #VCPU_LAZY + 16 * 0]
msr vmpidr_el2, x24
diff --git a/test/vmapi/primary_with_secondaries/floating_point.c b/test/vmapi/primary_with_secondaries/floating_point.c
index 9581a19..a480a8b 100644
--- a/test/vmapi/primary_with_secondaries/floating_point.c
+++ b/test/vmapi/primary_with_secondaries/floating_point.c
@@ -21,6 +21,7 @@
#include "vmapi/hf/call.h"
+#include "../msr.h"
#include "hftest.h"
#include "primary_with_secondary.h"
#include "util.h"
@@ -48,3 +49,25 @@
EXPECT_EQ(run_res.code, HF_VCPU_RUN_YIELD);
EXPECT_EQ(check_fp_register(second), true);
}
+
+/**
+ * Test that the floating point control register is restored correctly
+ * on full context switch when needed by changing it in the service.
+ */
+TEST(floating_point, fp_fpcr)
+{
+ uintreg_t value = 0;
+ struct hf_vcpu_run_return run_res;
+ struct mailbox_buffers mb = set_up_mailbox();
+
+ EXPECT_EQ(read_msr(fpcr), value);
+
+ SERVICE_SELECT(SERVICE_VM0, "fp_fpcr", mb.send);
+ run_res = hf_vcpu_run(SERVICE_VM0, 0);
+ EXPECT_EQ(run_res.code, HF_VCPU_RUN_YIELD);
+ EXPECT_EQ(read_msr(fpcr), value);
+
+ run_res = hf_vcpu_run(SERVICE_VM0, 0);
+ EXPECT_EQ(run_res.code, HF_VCPU_RUN_YIELD);
+ EXPECT_EQ(read_msr(fpcr), value);
+}
diff --git a/test/vmapi/primary_with_secondaries/services/floating_point.c b/test/vmapi/primary_with_secondaries/services/floating_point.c
index d98fdb9..e535a6a 100644
--- a/test/vmapi/primary_with_secondaries/services/floating_point.c
+++ b/test/vmapi/primary_with_secondaries/services/floating_point.c
@@ -21,6 +21,7 @@
#include "vmapi/hf/call.h"
+#include "../msr.h"
#include "hftest.h"
TEST_SERVICE(fp_fill)
@@ -32,3 +33,13 @@
ASSERT_TRUE(check_fp_register(value));
spci_yield();
}
+
+TEST_SERVICE(fp_fpcr)
+{
+ uintreg_t value = 3 << 22; /* Set RMode to RZ */
+ write_msr(fpcr, value);
+ EXPECT_EQ(spci_yield(), SPCI_SUCCESS);
+
+ ASSERT_EQ(read_msr(fpcr), value);
+ spci_yield();
+}