marvell/services/piped/piped_uevent.c

#define _GNU_SOURCE
#include <libubox/uloop.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <sys/socket.h>

#include <netlink/msg.h>
#include <netlink/attr.h>
#include <netlink/socket.h>

#include "piped.h"
#include "piped_util.h"
#include "piped_uevent.h"

struct event_socket {
	struct uloop_fd uloop;
	struct nl_sock *sock;
};

static void parse_msg(char *data, int size)
{
	char *subsystem = NULL, *path = NULL, *dev = NULL;
	char *cur, *end, *sep;
	int skip;
	bool add;

	if (!strncmp(data, "add@", 4))
		add = true;
	else if (!strncmp(data, "remove@", 7))
		add = false;
	else
		return;

	skip = strlen(data) + 1;
	end = data + size;

	for (cur = data + skip; cur < end; cur += skip) {
		skip = strlen(cur) + 1;

		sep = strchr(cur, '=');
		if (!sep)
			continue;

		*sep = 0;

		if (!strcmp(cur, "DEVPATH")) {
			path = sep + 1;
			dev = strrchr(path, '/') + 1;
		} else if (!strcmp(cur, "SUBSYSTEM")) {
			subsystem = sep + 1;
			if (strcmp(subsystem, "mpipe") != 0)
				return;
		}

		if (subsystem && dev)
			goto found;
	}
	return;

found:
	pd_uevent_cb(dev, add);

	return;
}

static void handle_event(struct uloop_fd *u, unsigned int events)
{
    UNUSEDPARAM(events);
    
	struct event_socket *ev = container_of(u, struct event_socket, uloop);
	struct sockaddr_nl nla;
	unsigned char *buf = NULL;
	int size;

	while ((size = nl_recv(ev->sock, &nla, &buf, NULL)) > 0) {
		if (nla.nl_pid == 0)
			parse_msg((char *) buf, size);

		free(buf);
	}
}

static struct nl_sock *create_socket(int protocol, int groups)
{
	struct nl_sock *sock;

	sock = nl_socket_alloc();
	if (!sock)
		return NULL;

	if (groups)
		nl_join_groups(sock, groups);

	if (nl_connect(sock, protocol))
		return NULL;

	return sock;
}

static bool create_raw_event_socket(struct event_socket *es, int protocol,
				     int groups, uloop_fd_handler cb, int flags)
{
	es->sock = create_socket(protocol, groups);
	if (!es->sock)
		return false;

	es->uloop.fd = nl_socket_get_fd(es->sock);
	es->uloop.cb = cb;
	if (uloop_fd_add(&es->uloop, ULOOP_READ | flags))
		return false;

	return true;
}

int pd_uevent_init(void)
{
	static struct event_socket es;

	if (!create_raw_event_socket(&es, NETLINK_KOBJECT_UEVENT, 1,
				     handle_event, 0))
		return -1;

	return 0;
}