107 lines
2.5 KiB
C
107 lines
2.5 KiB
C
/*
|
|
* Copyright (c) 2022 Rodrigo Peixoto <rodrigopex@gmail.com>
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/logging/log.h>
|
|
#include <zephyr/zbus/zbus.h>
|
|
|
|
LOG_MODULE_DECLARE(zbus, CONFIG_ZBUS_LOG_LEVEL);
|
|
|
|
K_MEM_SLAB_DEFINE_STATIC(_zbus_runtime_obs_pool, sizeof(struct zbus_observer_node),
|
|
CONFIG_ZBUS_RUNTIME_OBSERVERS_POOL_SIZE, 4);
|
|
|
|
struct k_mem_slab *zbus_runtime_obs_pool(void)
|
|
{
|
|
return &_zbus_runtime_obs_pool;
|
|
}
|
|
|
|
int zbus_chan_add_obs(const struct zbus_channel *chan, const struct zbus_observer *obs,
|
|
k_timeout_t timeout)
|
|
{
|
|
int err;
|
|
struct zbus_observer_node *obs_nd, *tmp;
|
|
uint64_t end_ticks = sys_clock_timeout_end_calc(timeout);
|
|
|
|
_ZBUS_ASSERT(!k_is_in_isr(), "ISR blocked");
|
|
_ZBUS_ASSERT(chan != NULL, "chan is required");
|
|
_ZBUS_ASSERT(obs != NULL, "obs is required");
|
|
|
|
/* Check if the observer is already a static observer */
|
|
for (const struct zbus_observer *const *static_obs = chan->observers; *static_obs != NULL;
|
|
++static_obs) {
|
|
if (*static_obs == obs) {
|
|
return -EEXIST;
|
|
}
|
|
}
|
|
|
|
err = k_mutex_lock(chan->mutex, timeout);
|
|
if (err) {
|
|
return err;
|
|
}
|
|
|
|
/* Check if the observer is already a runtime observer */
|
|
SYS_SLIST_FOR_EACH_CONTAINER_SAFE(chan->runtime_observers, obs_nd, tmp, node) {
|
|
if (obs_nd->obs == obs) {
|
|
k_mutex_unlock(chan->mutex);
|
|
|
|
return -EALREADY;
|
|
}
|
|
}
|
|
|
|
err = k_mem_slab_alloc(&_zbus_runtime_obs_pool, (void **)&obs_nd,
|
|
_zbus_timeout_remainder(end_ticks));
|
|
|
|
if (err) {
|
|
LOG_ERR("Could not allocate memory on runtime observers pool\n");
|
|
|
|
k_mutex_unlock(chan->mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
obs_nd->obs = obs;
|
|
|
|
sys_slist_append(chan->runtime_observers, &obs_nd->node);
|
|
|
|
k_mutex_unlock(chan->mutex);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int zbus_chan_rm_obs(const struct zbus_channel *chan, const struct zbus_observer *obs,
|
|
k_timeout_t timeout)
|
|
{
|
|
int err;
|
|
struct zbus_observer_node *obs_nd, *tmp;
|
|
struct zbus_observer_node *prev_obs_nd = NULL;
|
|
|
|
_ZBUS_ASSERT(!k_is_in_isr(), "ISR blocked");
|
|
_ZBUS_ASSERT(chan != NULL, "chan is required");
|
|
_ZBUS_ASSERT(obs != NULL, "obs is required");
|
|
|
|
err = k_mutex_lock(chan->mutex, timeout);
|
|
if (err) {
|
|
return err;
|
|
}
|
|
|
|
SYS_SLIST_FOR_EACH_CONTAINER_SAFE(chan->runtime_observers, obs_nd, tmp, node) {
|
|
if (obs_nd->obs == obs) {
|
|
sys_slist_remove(chan->runtime_observers, &prev_obs_nd->node,
|
|
&obs_nd->node);
|
|
|
|
k_mem_slab_free(&_zbus_runtime_obs_pool, (void **)&obs_nd);
|
|
|
|
k_mutex_unlock(chan->mutex);
|
|
|
|
return 0;
|
|
}
|
|
|
|
prev_obs_nd = obs_nd;
|
|
}
|
|
|
|
k_mutex_unlock(chan->mutex);
|
|
|
|
return -ENODATA;
|
|
}
|