From faf075a9d4d261591ac9a0d3338de5fdb26cb2cd Mon Sep 17 00:00:00 2001
From: Gerard Marull-Paretas <gerard@teslabs.com>
Date: Mon, 4 Nov 2024 10:14:36 +0100
Subject: [PATCH] soc: nrf54h: gpd: use callback to fetch nrfs async result

Busy-waiting for the result of the nrfs service calls can stall, so
let's use a callback that flags a semaphore instead. Since the API is
supposed to be callable in the context of pre-kernel, fallback to
busy-wait on that scenario.

Signed-off-by: Gerard Marull-Paretas <gerard@teslabs.com>
---
 soc/nordic/nrf54h/gpd/gpd.c | 55 +++++++++++++++++++++++++++++++++----
 1 file changed, 49 insertions(+), 6 deletions(-)

diff --git a/soc/nordic/nrf54h/gpd/gpd.c b/soc/nordic/nrf54h/gpd/gpd.c
index d100c893538021..a4d1889e53ffb0 100644
--- a/soc/nordic/nrf54h/gpd/gpd.c
+++ b/soc/nordic/nrf54h/gpd/gpd.c
@@ -28,6 +28,9 @@ struct gpd_onoff_manager {
 	struct onoff_manager mgr;
 	onoff_notify_fn notify;
 	uint8_t id;
+	struct k_mutex lock;
+	struct k_sem sem;
+	int res;
 };
 
 static void start(struct onoff_manager *mgr, onoff_notify_fn notify);
@@ -41,9 +44,21 @@ static void stop(struct onoff_manager *mgr, onoff_notify_fn notify);
 #define GPD_SERVICE_REQ_ERR BIT(3)
 static atomic_t gpd_service_status = ATOMIC_INIT(0);
 
-static struct gpd_onoff_manager fast_active1 = {.id = NRF_GPD_FAST_ACTIVE1};
-static struct gpd_onoff_manager slow_active = {.id = NRF_GPD_SLOW_ACTIVE};
-static struct gpd_onoff_manager slow_main = {.id = NRF_GPD_SLOW_MAIN};
+static struct gpd_onoff_manager fast_active1 = {
+	.id = NRF_GPD_FAST_ACTIVE1,
+	.lock = Z_MUTEX_INITIALIZER(fast_active1.lock),
+	.sem = Z_SEM_INITIALIZER(fast_active1.sem, 0, 1),
+};
+static struct gpd_onoff_manager slow_active = {
+	.id = NRF_GPD_SLOW_ACTIVE,
+	.lock = Z_MUTEX_INITIALIZER(slow_active.lock),
+	.sem = Z_SEM_INITIALIZER(slow_active.sem, 0, 1),
+};
+static struct gpd_onoff_manager slow_main = {
+	.id = NRF_GPD_SLOW_MAIN,
+	.lock = Z_MUTEX_INITIALIZER(slow_main.lock),
+	.sem = Z_SEM_INITIALIZER(slow_main.sem, 0, 1),
+};
 
 static const struct onoff_transitions transitions =
 	ONOFF_TRANSITIONS_INITIALIZER(start, stop, NULL);
@@ -62,6 +77,18 @@ static struct gpd_onoff_manager *get_mgr(uint8_t id)
 	}
 }
 
+static void request_cb(struct onoff_manager *mgr_, struct onoff_client *cli, uint32_t state,
+		       int res)
+{
+	ARG_UNUSED(cli);
+	ARG_UNUSED(state);
+
+	struct gpd_onoff_manager *gpd_mgr = CONTAINER_OF(mgr_, struct gpd_onoff_manager, mgr);
+
+	gpd_mgr->res = res;
+	k_sem_give(&gpd_mgr->sem);
+}
+
 static int nrf_gpd_sync(struct gpd_onoff_manager *gpd_mgr)
 {
 	int64_t start;
@@ -184,11 +211,27 @@ int nrf_gpd_request(uint8_t id)
 		return -EIO;
 	}
 
-	sys_notify_init_spinwait(&client.notify);
+	if (k_is_pre_kernel()) {
+		sys_notify_init_spinwait(&client.notify);
+
+		ret = onoff_request(&gpd_mgr->mgr, &client);
+		if (ret < 0) {
+			return ret;
+		}
 
-	onoff_request(&gpd_mgr->mgr, &client);
+		while (sys_notify_fetch_result(&client.notify, &ret) == -EAGAIN) {
+		}
+	} else {
+		sys_notify_init_callback(&client.notify, request_cb);
+		k_mutex_lock(&gpd_mgr->lock, K_FOREVER);
+
+		ret = onoff_request(&gpd_mgr->mgr, &client);
+		if (ret >= 0) {
+			(void)k_sem_take(&gpd_mgr->sem, K_FOREVER);
+			ret = gpd_mgr->res;
+		}
 
-	while (sys_notify_fetch_result(&client.notify, &ret) == -EAGAIN) {
+		k_mutex_unlock(&gpd_mgr->lock);
 	}
 
 	return ret;