feat: add helper functions for SP sleep

This patch introduces new helper utilities: virtualcounter_read() and
sp_sleep_elapsed_time() which are further used in sp_sleep() utility.

This patch also adds a needed isb when reading the virtual counter,
to prevent reading stale values, as well as reducing the number of
reads of the virtual counter required. Refer Arm ARM D11.2.2

Signed-off-by: Madhukar Pappireddy <madhukar.pappireddy@arm.com>
Change-Id: Icaf1777e85ee2efc043bfa83d630edf54bf5a099

4 files changed, 34 insertions, 12 deletions

diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 696cd01..8287422 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -562,4 +562,16 @@ static inline uint64_t syscounter_read(void)
 	return read_cntpct_el0();
 }
 
+/* Read the value of the Counter-timer virtual count. */
+static inline uint64_t virtualcounter_read(void)
+{
+	/*
+	 * The instruction barrier is needed to guarantee that we read an
+	 * accurate value. Otherwise, the CPU might speculatively read it and
+	 * return a stale value.
+	 */
+	isb();
+	return read_cntvct_el0();
+}
+
 #endif /* ARCH_HELPERS_H */
diff --git a/spm/cactus/cactus_tests/cactus_test_interrupts.c b/spm/cactus/cactus_tests/cactus_test_interrupts.c
index b675dfc..aa39ef2 100644
--- a/spm/cactus/cactus_tests/cactus_test_interrupts.c
+++ b/spm/cactus/cactus_tests/cactus_test_interrupts.c
@@ -13,18 +13,16 @@
 
 CACTUS_CMD_HANDLER(sleep_cmd, CACTUS_SLEEP_CMD)
 {
-	uint64_t timer_freq = read_cntfrq_el0();
-	uint64_t time1, time2, time_lapsed;
+	uint64_t time_lapsed;
 	uint32_t sleep_time = cactus_get_sleep_time(*args);
 
-	VERBOSE("Request to sleep %x for %ums.\n", ffa_dir_msg_dest(*args), sleep_time);
+	VERBOSE("Request to sleep %x for %ums.\n", ffa_dir_msg_dest(*args),
+		sleep_time);
 
-	time1 = read_cntvct_el0();
-	sp_sleep(sleep_time);
-	time2 = read_cntvct_el0();
+	time_lapsed = sp_sleep_elapsed_time(sleep_time);
 
-	/* Lapsed time should be at least equal to sleep time */
-	time_lapsed = ((time2 - time1) * 1000) / timer_freq;
+	/* Lapsed time should be at least equal to sleep time. */
+	VERBOSE("Sleep complete: %llu\n", time_lapsed);
 
 	return cactus_response(ffa_dir_msg_dest(*args),
 			       ffa_dir_msg_source(*args),
diff --git a/spm/common/sp_helpers.c b/spm/common/sp_helpers.c
index a6b6bc5..77031f8 100644
--- a/spm/common/sp_helpers.c
+++ b/spm/common/sp_helpers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -58,16 +58,25 @@ void announce_test_end(const char *test_desc)
 	INFO("Test \"%s\" end.\n", test_desc);
 }
 
-void sp_sleep(uint32_t ms)
+uint64_t sp_sleep_elapsed_time(uint32_t ms)
 {
 	uint64_t timer_freq = read_cntfrq_el0();
 
 	VERBOSE("%s: Timer frequency = %llu\n", __func__, timer_freq);
 
 	VERBOSE("%s: Sleeping for %u milliseconds...\n", __func__, ms);
-	uint64_t time1 = read_cntvct_el0();
+
+	uint64_t time1 = virtualcounter_read();
 	volatile uint64_t time2 = time1;
+
 	while ((time2 - time1) < ((ms * timer_freq) / 1000U)) {
-		time2 = read_cntvct_el0();
+		time2 = virtualcounter_read();
 	}
+
+	return ((time2 - time1) * 1000) / timer_freq;
+}
+
+void sp_sleep(uint32_t ms)
+{
+	(void)sp_sleep_elapsed_time(ms);
 }
diff --git a/spm/common/sp_helpers.h b/spm/common/sp_helpers.h
index 9f5400c..6fe8ec0 100644
--- a/spm/common/sp_helpers.h
+++ b/spm/common/sp_helpers.h
@@ -56,6 +56,9 @@ void announce_test_section_end(const char *test_sect_desc);
 void announce_test_start(const char *test_desc);
 void announce_test_end(const char *test_desc);
 
+/* Sleep for at least 'ms' milliseconds and return the elapsed time(ms). */
+uint64_t sp_sleep_elapsed_time(uint32_t ms);
+
 /* Sleep for at least 'ms' milliseconds. */
 void sp_sleep(uint32_t ms);