zephyr/lib/os/onoff.c

/*
 * Copyright (c) 2019 Peter Bigot Consulting, LLC
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <kernel.h>
#include <sys/onoff.h>
#include <syscall_handler.h>

#define CLIENT_NOTIFY_METHOD_MASK 0x03
#define CLIENT_VALID_FLAGS_MASK 0x07

#define SERVICE_CONFIG_FLAGS	     \
	(ONOFF_SERVICE_START_SLEEPS  \
	 | ONOFF_SERVICE_STOP_SLEEPS \
	 | ONOFF_SERVICE_RESET_SLEEPS)

#define SERVICE_REFS_MAX UINT16_MAX

#define SERVICE_STATE_OFF 0
#define SERVICE_STATE_ON ONOFF_SERVICE_INTERNAL_BASE
#define SERVICE_STATE_TRANSITION (ONOFF_SERVICE_INTERNAL_BASE << 1)
#define SERVICE_STATE_TO_ON (SERVICE_STATE_TRANSITION | SERVICE_STATE_ON)
#define SERVICE_STATE_TO_OFF (SERVICE_STATE_TRANSITION | SERVICE_STATE_OFF)

#define SERVICE_STATE_MASK (SERVICE_STATE_ON | SERVICE_STATE_TRANSITION)

static void set_service_state(struct onoff_service *srv,
			      u32_t state)
{
	srv->flags &= ~SERVICE_STATE_MASK;
	srv->flags |= (state & SERVICE_STATE_MASK);
}

static int validate_args(const struct onoff_service *srv,
			 struct onoff_client *cli)
{
	if ((srv == NULL) || (cli == NULL)) {
		return -EINVAL;
	}

	int rv = 0;
	u32_t mode = cli->flags;

	/* Reject unexpected flags. */
	if (mode != (cli->flags & CLIENT_VALID_FLAGS_MASK)) {
		return -EINVAL;
	}

	/* Validate configuration based on mode */
	switch (mode & CLIENT_NOTIFY_METHOD_MASK) {
	case ONOFF_CLIENT_NOTIFY_SPINWAIT:
		break;
	case ONOFF_CLIENT_NOTIFY_CALLBACK:
		if (cli->async.callback.handler == NULL) {
			rv = -EINVAL;
		}
		break;
	case ONOFF_CLIENT_NOTIFY_SIGNAL:
		if (cli->async.signal == NULL) {
			rv = -EINVAL;
		}
		break;
	default:
		rv = -EINVAL;
		break;
	}

	/* Clear the result here instead of in all callers. */
	if (rv == 0) {
		cli->result = 0;
	}

	return rv;
}

int onoff_service_init(struct onoff_service *srv,
		       onoff_service_transition_fn start,
		       onoff_service_transition_fn stop,
		       onoff_service_transition_fn reset,
		       u32_t flags)
{
	if ((flags & SERVICE_CONFIG_FLAGS) != flags) {
		return -EINVAL;
	}

	if ((start == NULL) || (stop == NULL)) {
		return -EINVAL;
	}

	*srv = (struct onoff_service)ONOFF_SERVICE_INITIALIZER(start, stop,
							       reset, flags);

	return 0;
}

static void notify_one(struct onoff_service *srv,
		       struct onoff_client *cli,
		       int res)
{
	unsigned int flags = cli->flags;

	/* Store the result, and notify if requested. */
	cli->result = res;
	cli->flags = 0;
	switch (flags & CLIENT_NOTIFY_METHOD_MASK) {
	case ONOFF_CLIENT_NOTIFY_SPINWAIT:
		break;
	case ONOFF_CLIENT_NOTIFY_CALLBACK:
		cli->async.callback.handler(srv, cli,
					    cli->async.callback.user_data, res);
		break;
#ifdef CONFIG_POLL
	case ONOFF_CLIENT_NOTIFY_SIGNAL:
		k_poll_signal_raise(cli->async.signal, res);
		break;
#endif /* CONFIG_POLL */
	default:
		__ASSERT_NO_MSG(false);
	}
}

static void notify_all(struct onoff_service *srv,
		       sys_slist_t *list,
		       int res)
{
	while (!sys_slist_is_empty(list)) {
		sys_snode_t *node = sys_slist_get_not_empty(list);
		struct onoff_client *cli =
			CONTAINER_OF(node,
				     struct onoff_client,
				     node);

		notify_one(srv, cli, res);
	}
}

static void onoff_start_notify(struct onoff_service *srv,
			       int res)
{
	k_spinlock_key_t key = k_spin_lock(&srv->lock);
	sys_slist_t clients = srv->clients;

	/* Can't have a queued releaser during start */
	__ASSERT_NO_MSG(srv->releaser == NULL);

	/* If the start failed log an error and leave the rest of the
	 * state in place for diagnostics.
	 *
	 * If the start succeeded record a reference for all clients
	 * and set the state to ON.  There must be at least one client
	 * left to receive the result.
	 *
	 * In either case reset the client queue and notify all
	 * clients of operation completion.
	 */
	if (res < 0) {
		srv->flags &= ~SERVICE_STATE_TRANSITION;
		srv->flags |= ONOFF_SERVICE_HAS_ERROR;
	} else {
		sys_snode_t *node;
		unsigned int refs = 0U;

		set_service_state(srv, SERVICE_STATE_ON);

		SYS_SLIST_FOR_EACH_NODE(&clients, node) {
			refs += 1U;
		}

		/* Update the reference count, or fail if the count
		 * would overflow.
		 */
		if (srv->refs > (SERVICE_REFS_MAX - refs)) {
			srv->flags |= ONOFF_SERVICE_HAS_ERROR;
		} else {
			srv->refs += refs;
		}
		__ASSERT_NO_MSG(srv->refs > 0U);
	}

	sys_slist_init(&srv->clients);

	k_spin_unlock(&srv->lock, key);

	notify_all(srv, &clients, res);
}

int onoff_request(struct onoff_service *srv,
		  struct onoff_client *cli)
{
	bool add_client = false;        /* add client to pending list */
	bool start = false;             /* invoke start transition */
	bool notify = false;            /* do client notification */
	int rv = validate_args(srv, cli);

	if (rv < 0) {
		return rv;
	}

	k_spinlock_key_t key = k_spin_lock(&srv->lock);

	if ((srv->flags & ONOFF_SERVICE_HAS_ERROR) != 0) {
		rv = -EIO;
		goto out;
	}

	/* Reject if this would overflow the reference count. */
	if (srv->refs == SERVICE_REFS_MAX) {
		rv = -EAGAIN;
		goto out;
	}

	u32_t state = srv->flags & SERVICE_STATE_MASK;

	switch (state) {
	case SERVICE_STATE_TO_OFF:
		/* Queue to start after release */
		__ASSERT_NO_MSG(srv->releaser != NULL);
		add_client = true;
		rv = 3;
		break;
	case SERVICE_STATE_OFF:
		/* Reject if in a non-thread context and start could
		 * wait.
		 */
		if ((k_is_in_isr() || k_is_pre_kernel())
		    && ((srv->flags & ONOFF_SERVICE_START_SLEEPS) != 0U)) {
			rv = -EWOULDBLOCK;
			break;
		}

		/* Start with first request while off */
		__ASSERT_NO_MSG(srv->refs == 0);
		set_service_state(srv, SERVICE_STATE_TO_ON);
		start = true;
		add_client = true;
		rv = 2;
		break;
	case SERVICE_STATE_TO_ON:
		/* Already starting, just queue it */
		add_client = true;
		rv = 1;
		break;
	case SERVICE_STATE_ON:
		/* Just increment the reference count */
		notify = true;
		break;
	default:
		rv = -EINVAL;
		break;
	}

out:
	if (add_client) {
		sys_slist_append(&srv->clients, &cli->node);
	} else if (notify) {
		srv->refs += 1;
	}

	k_spin_unlock(&srv->lock, key);

	if (start) {
		__ASSERT_NO_MSG(srv->start != NULL);
		srv->start(srv, onoff_start_notify);
	} else if (notify) {
		notify_one(srv, cli, 0);
	}

	return rv;
}

static void onoff_stop_notify(struct onoff_service *srv,
			      int res)
{
	bool notify_clients = false;
	int client_res = res;
	bool start = false;
	k_spinlock_key_t key = k_spin_lock(&srv->lock);
	sys_slist_t clients = srv->clients;
	struct onoff_client *releaser = srv->releaser;

	/* If the stop operation failed log an error and leave the
	 * rest of the state in place.
	 *
	 * If it succeeded remove the last reference and transition to
	 * off.
	 *
	 * In either case remove the last reference, and notify all
	 * waiting clients of operation completion.
	 */
	if (res < 0) {
		srv->flags &= ~SERVICE_STATE_TRANSITION;
		srv->flags |= ONOFF_SERVICE_HAS_ERROR;
		notify_clients = true;
	} else if (sys_slist_is_empty(&clients)) {
		set_service_state(srv, SERVICE_STATE_OFF);
	} else if ((k_is_in_isr() || k_is_pre_kernel())
		   && ((srv->flags & ONOFF_SERVICE_START_SLEEPS) != 0U)) {
		set_service_state(srv, SERVICE_STATE_OFF);
		notify_clients = true;
		client_res = -EWOULDBLOCK;
	} else {
		set_service_state(srv, SERVICE_STATE_TO_ON);
		start = true;
	}

	__ASSERT_NO_MSG(releaser);
	srv->refs -= 1U;
	srv->releaser = NULL;
	__ASSERT_NO_MSG(srv->refs == 0);

	/* Remove the clients if there was an error or a delayed start
	 * couldn't be initiated, because we're resolving their
	 * operation with an error.
	 */
	if (notify_clients) {
		sys_slist_init(&srv->clients);
	}

	k_spin_unlock(&srv->lock, key);

	/* Notify the releaser.  If there was an error, notify any
	 * pending requests; otherwise if there are pending requests
	 * start the transition to ON.
	 */
	notify_one(srv, releaser, res);
	if (notify_clients) {
		notify_all(srv, &clients, client_res);
	} else if (start) {
		srv->start(srv, onoff_start_notify);
	}
}

int onoff_release(struct onoff_service *srv,
		  struct onoff_client *cli)
{
	bool stop = false;      /* invoke stop transition */
	bool notify = false;    /* do client notification */
	int rv = validate_args(srv, cli);

	if (rv < 0) {
		return rv;
	}

	k_spinlock_key_t key = k_spin_lock(&srv->lock);

	if ((srv->flags & ONOFF_SERVICE_HAS_ERROR) != 0) {
		rv = -EIO;
		goto out;
	}

	u32_t state = srv->flags & SERVICE_STATE_MASK;

	switch (state) {
	case SERVICE_STATE_ON:
		/* Stay on if release leaves a client. */
		if (srv->refs > 1U) {
			notify = true;
			rv = 1;
			break;
		}

		/* Reject if in non-thread context but stop could
		 * wait
		 */
		if ((k_is_in_isr() || k_is_pre_kernel())
		    && ((srv->flags & ONOFF_SERVICE_STOP_SLEEPS) != 0)) {
			rv = -EWOULDBLOCK;
			break;
		}

		stop = true;

		set_service_state(srv, SERVICE_STATE_TO_OFF);
		srv->releaser = cli;
		rv = 2;

		break;
	case SERVICE_STATE_TO_ON:
		rv = -EBUSY;
		break;
	case SERVICE_STATE_OFF:
	case SERVICE_STATE_TO_OFF:
		rv = -EALREADY;
		break;
	default:
		rv = -EINVAL;
	}

out:
	if (notify) {
		srv->refs -= 1U;
	}

	k_spin_unlock(&srv->lock, key);

	if (stop) {
		__ASSERT_NO_MSG(srv->stop != NULL);
		srv->stop(srv, onoff_stop_notify);
	} else if (notify) {
		notify_one(srv, cli, 0);
	}

	return rv;
}

static void onoff_reset_notify(struct onoff_service *srv,
			       int res)
{
	k_spinlock_key_t key = k_spin_lock(&srv->lock);
	sys_slist_t clients = srv->clients;

	/* If the reset failed clear the transition flag but otherwise
	 * leave the state unchanged.
	 *
	 * If it was successful clear the reference count and all
	 * flags except capability flags (sets to SERVICE_STATE_OFF).
	 */
	if (res < 0) {
		srv->flags &= ~SERVICE_STATE_TRANSITION;
	} else {
		__ASSERT_NO_MSG(srv->refs == 0U);
		srv->refs = 0U;
		srv->flags &= SERVICE_CONFIG_FLAGS;
	}

	sys_slist_init(&srv->clients);

	k_spin_unlock(&srv->lock, key);

	notify_all(srv, &clients, res);
}

int onoff_service_reset(struct onoff_service *srv,
			struct onoff_client *cli)
{
	if (srv->reset == NULL) {
		return -ENOTSUP;
	}

	bool reset = false;
	int rv = validate_args(srv, cli);

	if (rv < 0) {
		return rv;
	}

	/* Reject if in a non-thread context and reset could wait. */
	if ((k_is_in_isr() || k_is_pre_kernel())
	    && ((srv->flags & ONOFF_SERVICE_RESET_SLEEPS) != 0U)) {
		return -EWOULDBLOCK;
	}

	k_spinlock_key_t key = k_spin_lock(&srv->lock);

	if ((srv->flags & ONOFF_SERVICE_HAS_ERROR) == 0) {
		rv = -EALREADY;
		goto out;
	}

	if ((srv->flags & SERVICE_STATE_TRANSITION) == 0) {
		reset = true;
		srv->flags |= SERVICE_STATE_TRANSITION;
	}

out:
	if (rv >= 0) {
		sys_slist_append(&srv->clients, &cli->node);
	}

	k_spin_unlock(&srv->lock, key);

	if (reset) {
		srv->reset(srv, onoff_reset_notify);
	}

	return rv;
}

int onoff_cancel(struct onoff_service *srv,
		 struct onoff_client *cli)
{
	int rv = validate_args(srv, cli);

	if (rv < 0) {
		return rv;
	}

	rv = -EALREADY;
	k_spinlock_key_t key = k_spin_lock(&srv->lock);
	u32_t state = srv->flags & SERVICE_STATE_MASK;

	/* Can't remove the last client waiting for the in-progress
	 * transition, as there would be nobody to receive the
	 * completion notification, which might indicate a service
	 * error.
	 */
	if (sys_slist_find_and_remove(&srv->clients, &cli->node)) {
		rv = 0;
		if (sys_slist_is_empty(&srv->clients)
		    && (state != SERVICE_STATE_TO_OFF)) {
			rv = -EWOULDBLOCK;
			sys_slist_append(&srv->clients, &cli->node);
		}
	} else if (srv->releaser == cli) {
		/* must be waiting for TO_OFF to complete */
		rv = -EWOULDBLOCK;
	}

	k_spin_unlock(&srv->lock, key);

	if (rv == 0) {
		notify_one(srv, cli, -ECANCELED);
	}

	return rv;
}