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;