From ee7bbf55e0d4e1aeb17ac9d27d498669f4ee0091 Mon Sep 17 00:00:00 2001
From: Peter Mitsis <peter.mitsis@intel.com>
Date: Mon, 12 Aug 2024 22:12:14 +0000
Subject: [PATCH] tests: latency_measure: Add busy threads for SMP

The latency_measure benchmark is designed for systems with a single
CPU. When the system allows for multiple CPUs, instead of forcing
a single CPU to be used via 'prj.conf', spawn a non-preemptible
thread to keep the other CPUs busy.

Signed-off-by: Peter Mitsis <peter.mitsis@intel.com>
---
 tests/benchmarks/latency_measure/prj.conf   |  2 --
 tests/benchmarks/latency_measure/src/main.c | 27 +++++++++++++++++++++
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/tests/benchmarks/latency_measure/prj.conf b/tests/benchmarks/latency_measure/prj.conf
index aa8be9843d0..be2e8f0d99a 100644
--- a/tests/benchmarks/latency_measure/prj.conf
+++ b/tests/benchmarks/latency_measure/prj.conf
@@ -20,8 +20,6 @@ CONFIG_COVERAGE=n
 # Disable system power management
 CONFIG_PM=n
 
-# Can only run under 1 CPU
-CONFIG_MP_MAX_NUM_CPUS=1
 CONFIG_TIMING_FUNCTIONS=y
 
 CONFIG_HEAP_MEM_POOL_SIZE=2048
diff --git a/tests/benchmarks/latency_measure/src/main.c b/tests/benchmarks/latency_measure/src/main.c
index ba709d54512..be345c1dddf 100644
--- a/tests/benchmarks/latency_measure/src/main.c
+++ b/tests/benchmarks/latency_measure/src/main.c
@@ -31,6 +31,14 @@ K_THREAD_STACK_DEFINE(alt_stack, ALT_STACK_SIZE);
 
 K_SEM_DEFINE(pause_sem, 0, 1);
 
+#if (CONFIG_MP_MAX_NUM_CPUS > 1)
+struct k_thread  busy_thread[CONFIG_MP_MAX_NUM_CPUS - 1];
+
+#define BUSY_THREAD_STACK_SIZE  (1024 + CONFIG_TEST_EXTRA_STACK_SIZE)
+
+K_THREAD_STACK_DEFINE(busy_thread_stack, BUSY_THREAD_STACK_SIZE);
+#endif
+
 struct k_thread  start_thread;
 struct k_thread  alt_thread;
 
@@ -60,6 +68,14 @@ extern int stack_blocking_ops(uint32_t num_iterations, uint32_t start_options,
 			       uint32_t alt_options);
 extern void heap_malloc_free(void);
 
+#if (CONFIG_MP_MAX_NUM_CPUS > 1)
+static void busy_thread_entry(void *arg1, void *arg2, void *arg3)
+{
+	while (1) {
+	}
+}
+#endif
+
 static void test_thread(void *arg1, void *arg2, void *arg3)
 {
 	uint32_t freq;
@@ -68,6 +84,17 @@ static void test_thread(void *arg1, void *arg2, void *arg3)
 	ARG_UNUSED(arg2);
 	ARG_UNUSED(arg3);
 
+#if (CONFIG_MP_MAX_NUM_CPUS > 1)
+	/* Spawn busy threads that will execute on the other cores */
+
+	for (uint32_t i = 0; i < CONFIG_MP_MAX_NUM_CPUS - 1; i++) {
+		k_thread_create(&busy_thread[i], &busy_thread_stack[i],
+				BUSY_THREAD_STACK_SIZE, busy_thread_entry,
+				NULL, NULL, NULL,
+				K_HIGHEST_THREAD_PRIO, 0, K_NO_WAIT);
+	}
+#endif
+
 #ifdef CONFIG_USERSPACE
 	k_mem_domain_add_partition(&k_mem_domain_default,
 				   &bench_mem_partition);