udev-monitor.c

/*
 * Copyright (c) 2015, 2021 Vladimir Kondratyev <vladimir@kondratyev.su>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include "udev-global.h"

#include <sys/types.h>
#include <sys/queue.h>
#include <sys/socket.h>
#include <sys/un.h>

#include <errno.h>
#include <fcntl.h>
#include <poll.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#define	DEVD_SOCK_PATH		"/var/run/devd.seqpacket.pipe"
#define	DEVD_RECONNECT_INTERVAL	1000	/* reconnect after 1 second */

STAILQ_HEAD(udev_monitor_queue_head, udev_monitor_queue_entry);
struct udev_monitor_queue_entry {
	struct udev_device *ud;
	STAILQ_ENTRY(udev_monitor_queue_entry) next;
};

struct udev_monitor {
	int refcount;
	int fds[2];
	struct udev_filter_head filters;
	struct udev *udev;
	struct udev_monitor_queue_head queue;
	pthread_mutex_t mtx;
	pthread_t thread;
};

LIBUDEV_EXPORT struct udev_device *
udev_monitor_receive_device(struct udev_monitor *um)
{
	struct udev_monitor_queue_entry *umqe;
	struct udev_device *ud;
	char buf[1];

	TRC("(%p)", um);
	if (read(um->fds[0], buf, 1) < 0)
		return (NULL);

	if (STAILQ_EMPTY(&um->queue))
		return (NULL);

	pthread_mutex_lock(&um->mtx);
	umqe = STAILQ_FIRST(&um->queue);
	STAILQ_REMOVE_HEAD(&um->queue, next);
	pthread_mutex_unlock(&um->mtx);
	ud = umqe->ud;
	free(umqe);

	return (ud);
}

static int
udev_monitor_send_device(struct udev_monitor *um, const char *syspath,
    int action)
{
	struct udev_monitor_queue_entry *umqe;

	umqe = calloc(1, sizeof(struct udev_monitor_queue_entry));
	if (umqe == NULL)
		return (-1);

	umqe->ud = udev_device_new_common(um->udev, syspath, action);
	if (umqe->ud == NULL) {
		free(umqe);
		return (-1);
	}

	pthread_mutex_lock(&um->mtx);
	STAILQ_INSERT_TAIL(&um->queue, umqe, next);
	pthread_mutex_unlock(&um->mtx);

	if (write(um->fds[1], "*", 1) != 1) {
		pthread_mutex_lock(&um->mtx);
		STAILQ_REMOVE(&um->queue, umqe, udev_monitor_queue_entry, next);
		pthread_mutex_unlock(&um->mtx);
		udev_device_unref(umqe->ud);
		free(umqe);
		return (-1);
	}

	return (0);
}

static int
parse_devd_message(char *msg, char *syspath, size_t syspathlen)
{
	int action;

	action = udev_dev_monitor(msg, syspath, syspathlen);
	if (action == UD_ACTION_NONE)
		action = udev_sys_monitor(msg, syspath, syspathlen);
	if (action == UD_ACTION_NONE)
		action = udev_pci_monitor(msg, syspath, syspathlen);
	if (action == UD_ACTION_NONE)
		action = udev_net_monitor(msg, syspath, syspathlen);

	return (action);
}

static void *
udev_monitor_thread(void *args)
{
	struct udev_monitor *um = args;
	char ev[1024], syspath[DEV_PATH_MAX];
	struct pollfd fds[2];
	nfds_t nfds;
	ssize_t len;
	int devd_fd = -1, ret, action, timeout;
	sigset_t set;
	const static struct sockaddr_un sa = {
		.sun_family = AF_UNIX,
		.sun_path = DEVD_SOCK_PATH,
	};

	sigfillset(&set);
	pthread_sigmask(SIG_BLOCK, &set, NULL);

	fds[0].fd = um->fds[1];
	fds[0].events = 0;
	fds[1].events = POLLIN;

	for (;;) {
		if (devd_fd < 0 &&
		    (devd_fd = socket(AF_UNIX, SOCK_SEQPACKET | SOCK_CLOEXEC, 0)) >= 0 &&
		    connect(devd_fd, (struct sockaddr *) &sa, sizeof(sa)) < 0) {
			close(devd_fd);
			devd_fd = -1;
		}

		if (devd_fd < 0) {
			nfds = 1;
			timeout = DEVD_RECONNECT_INTERVAL;
		} else {
			fds[1].fd = devd_fd;
			nfds = 2;
			timeout = -1;
		}

		ret = poll(fds, nfds, timeout);
		if (ret == -1 && errno == EINTR)
			continue;
		if (ret == -1)
			break;

		/* edev_monitor is finishing */
		if (fds[0].revents & POLLHUP)
			break;

		/* connection respawn timer expired */
		if (ret == 0 || devd_fd < 1)
			continue;

		if (fds[1].revents & POLLIN) {
			if ((len = recv(devd_fd, ev, sizeof(ev), MSG_WAITALL))
			    <= 0) {
				close(devd_fd);
				devd_fd = -1;
				continue;
			}
			/* Replace terminating LF with 0 to make C-string */
			ev[len - 1] = '\0';
			action = parse_devd_message(ev, syspath, sizeof(syspath));
			if (action != UD_ACTION_NONE &&
			    udev_filter_match(um->udev, &um->filters, syspath))
				udev_monitor_send_device(um, syspath, action);
		}

		if (fds[1].revents & POLLHUP) {
			close(devd_fd);
			devd_fd = -1;
		}
	}

	if (devd_fd >= 0)
		close(devd_fd);

	return (NULL);
}

LIBUDEV_EXPORT struct udev_monitor *
udev_monitor_new_from_netlink(struct udev *udev, const char *name)
{
	struct udev_monitor *um;
	
	TRC("(%p, %s)", udev, name);
	um = calloc(1, sizeof(struct udev_monitor));
	if (!um)
		return (NULL);

	if (pipe2(um->fds, O_CLOEXEC) == -1) {
		ERR("pipe2 failed");
		free(um);
		return (NULL);
	}

	um->udev = udev;
	_udev_ref(udev);
	um->refcount = 1;
	udev_filter_init(&um->filters);
	STAILQ_INIT(&um->queue);
	pthread_mutex_init(&um->mtx, NULL);

	return (um);
}

LIBUDEV_EXPORT int
udev_monitor_filter_add_match_subsystem_devtype(struct udev_monitor *um,
    const char *subsystem, const char *devtype)
{
	TRC("(%p, %s, %s)", um, subsystem, devtype);
	return (udev_filter_add(&um->filters, UDEV_FILTER_TYPE_SUBSYSTEM, 0,
	    subsystem, devtype));
}

LIBUDEV_EXPORT int
udev_monitor_filter_add_match_tag(struct udev_monitor *um, const char *tag)
{
	TRC("(%p, %s)", um, tag);
	return (udev_filter_add(&um->filters, UDEV_FILTER_TYPE_TAG, 0, tag, 0));
}

LIBUDEV_EXPORT int
udev_monitor_enable_receiving(struct udev_monitor *um)
{

	TRC("(%p)", um);

	if (pthread_create(&um->thread, NULL, udev_monitor_thread, um) != 0) {
		ERR("thread_create failed");
		return (-1);
	}

	return (0);
}

LIBUDEV_EXPORT int
udev_monitor_get_fd(struct udev_monitor *um)
{

	/* TRC("(%p)", um); */
	return (um->fds[0]);
}

LIBUDEV_EXPORT struct udev_monitor *
udev_monitor_ref(struct udev_monitor *um)
{

	TRC("(%p) refcount=%d", um, um->refcount);
	++um->refcount;
	return (um);
}

static void
udev_monitor_queue_drop(struct udev_monitor_queue_head *umqh)
{
	struct udev_monitor_queue_entry *umqe;

	while (!STAILQ_EMPTY(umqh)) {
		umqe = STAILQ_FIRST(umqh);
		STAILQ_REMOVE_HEAD(umqh, next);
		udev_device_unref(umqe->ud);
		free(umqe);
	}
}

LIBUDEV_EXPORT void
udev_monitor_unref(struct udev_monitor *um)
{
	TRC("(%p) refcount=%d", um, um->refcount);
	if (--um->refcount == 0) {
		close(um->fds[0]);
		pthread_join(um->thread, NULL);
		close(um->fds[1]);
		udev_filter_free(&um->filters);
		udev_monitor_queue_drop(&um->queue);
		pthread_mutex_destroy(&um->mtx);
		_udev_unref(um->udev);
		free(um);
	}
}

LIBUDEV_EXPORT
struct udev *udev_monitor_get_udev(struct udev_monitor *um)
{

	TRC();
	return (um->udev);
}

LIBUDEV_EXPORT int
udev_monitor_set_receive_buffer_size(struct udev_monitor *um, int size)
{
	TRC("(%d)", size);
	UNIMPL();
	return (0);
}

LIBUDEV_EXPORT int
udev_monitor_filter_update(struct udev_monitor *um)
{
	TRC();
	UNIMPL();
	return (0);
}

LIBUDEV_EXPORT int
udev_monitor_filter_remove(struct udev_monitor *um)
{
	TRC();
	UNIMPL();
	return (0);
}