subsys/zbus/zbus_runtime_observers.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;
}