diff --git a/subsys/pm/pm.c b/subsys/pm/pm.c index 65b1cdb9c61..a089e5de55d 100644 --- a/subsys/pm/pm.c +++ b/subsys/pm/pm.c @@ -35,15 +35,18 @@ static struct pm_state_info z_cpus_pm_state[] = { LISTIFY(CONFIG_MP_NUM_CPUS, CPU_PM_STATE_INIT, (,)) }; -/* bitmask to check if a power state was forced. */ -static ATOMIC_DEFINE(z_cpus_pm_state_forced, CONFIG_MP_NUM_CPUS); +static struct pm_state_info z_cpus_pm_forced_state[] = { + LISTIFY(CONFIG_MP_NUM_CPUS, CPU_PM_STATE_INIT, (,)) +}; + +static struct k_spinlock pm_forced_state_lock; + #ifdef CONFIG_PM_DEVICE static atomic_t z_cpus_active = ATOMIC_INIT(CONFIG_MP_NUM_CPUS); #endif static struct k_spinlock pm_notifier_lock; - #ifdef CONFIG_PM_DEVICE extern const struct device *__pm_device_slots_start[]; @@ -182,27 +185,30 @@ void pm_system_resume(void) bool pm_state_force(uint8_t cpu, const struct pm_state_info *info) { - bool ret = false; + k_spinlock_key_t key; __ASSERT(info->state < PM_STATE_COUNT, "Invalid power state %d!", info->state); + key = k_spin_lock(&pm_forced_state_lock); + z_cpus_pm_forced_state[cpu] = *info; + k_spin_unlock(&pm_forced_state_lock, key); - if (!atomic_test_and_set_bit(z_cpus_pm_state_forced, cpu)) { - z_cpus_pm_state[cpu] = *info; - ret = true; - } - - return ret; + return true; } bool pm_system_suspend(int32_t ticks) { uint8_t id = _current_cpu->id; + k_spinlock_key_t key; SYS_PORT_TRACING_FUNC_ENTER(pm, system_suspend, ticks); - if (!atomic_test_bit(z_cpus_pm_state_forced, id)) { + key = k_spin_lock(&pm_forced_state_lock); + if (z_cpus_pm_forced_state[id].state != PM_STATE_ACTIVE) { + z_cpus_pm_state[id] = z_cpus_pm_forced_state[id]; + z_cpus_pm_forced_state[id].state = PM_STATE_ACTIVE; + } else { const struct pm_state_info *info; info = pm_policy_next_state(id, ticks); @@ -210,12 +216,12 @@ bool pm_system_suspend(int32_t ticks) z_cpus_pm_state[id] = *info; } } + k_spin_unlock(&pm_forced_state_lock, key); if (z_cpus_pm_state[id].state == PM_STATE_ACTIVE) { LOG_DBG("No PM operations done."); SYS_PORT_TRACING_FUNC_EXIT(pm, system_suspend, ticks, z_cpus_pm_state[id].state); - atomic_clear_bit(z_cpus_pm_state_forced, id); return false; } @@ -239,7 +245,6 @@ bool pm_system_suspend(int32_t ticks) (void)atomic_add(&z_cpus_active, 1); SYS_PORT_TRACING_FUNC_EXIT(pm, system_suspend, ticks, z_cpus_pm_state[id].state); - atomic_clear_bit(z_cpus_pm_state_forced, id); return false; } } @@ -269,7 +274,6 @@ bool pm_system_suspend(int32_t ticks) #endif pm_stats_update(z_cpus_pm_state[id].state); pm_system_resume(); - atomic_clear_bit(z_cpus_pm_state_forced, id); k_sched_unlock(); SYS_PORT_TRACING_FUNC_EXIT(pm, system_suspend, ticks, z_cpus_pm_state[id].state);