sbin/udevd/test_udevd.c

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

#include <err.h>
#include <errno.h>
#include <fcntl.h>
#include <libgen.h>
#include <signal.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <syslog.h>
#include <unistd.h>

#include <libprop/proplib.h>
#include <sys/udev.h>

#define   LISTEN_SOCKET_FILE  "/tmp/udevd.socket"
#define SOCKFILE_NAMELEN      strlen(LISTEN_SOCKET_FILE)+1

int conn_local_server(const char *sockfile, int socktype, int nonblock,
                      int *retsock);
prop_dictionary_t udevd_get_command_dict(char *command);
void udevd_request_devs(int s);

struct udev {
          int       gp_fd;
          int       monitor_fd;
          int       refs;

          void      *userdata;
};

struct udev_enumerate {
          struct udev         *udev_ctx;
          prop_array_t        pa;
          int       refs;
          TAILQ_HEAD(, udev_list_entry) list_entries;
};

struct udev_list_entry {
          prop_dictionary_t   dict;
          TAILQ_ENTRY(udev_list_entry)  link;
};

struct udev_monitor {
          struct udev         *udev_ctx;
          prop_array_t        ev_filt;
          int       socket;
          int       user_socket; /* maybe... one day... */
          int       refs;
};

struct udev_device {
          struct udev         *udev_ctx;
          prop_dictionary_t   dict;
          int       ev_type;
          int       refs;
};

struct udev *
udev_ref(struct udev *udev_ctx)
{
          atomic_add_int(&udev_ctx->refs, 1);

          return udev_ctx;
}

void
udev_unref(struct udev *udev_ctx)
{
          int refcount;

          refcount = atomic_fetchadd_int(&udev_ctx->refs, -1);

          if (refcount == 1) {
                    atomic_subtract_int(&udev_ctx->refs, 0x400); /* in destruction */
                    if (udev_ctx->gp_fd != -1)
                              close (udev_ctx->gp_fd);
                    if (udev_ctx->monitor_fd != -1)
                              close (udev_ctx->monitor_fd);

                    free(udev_ctx);
          }
}

struct udev *
udev_new()
{
          struct udev *udev_ctx;

          udev_ctx = malloc(sizeof(struct udev));

          udev_ctx->refs = 1;
          udev_ctx->gp_fd = -1;
          udev_ctx->monitor_fd = -1;
          udev_ctx->userdata = NULL;

          return udev_ctx;
}

const char *udev_get_dev_path(struct udev *udev_ctx __unused)
{
          return "/dev";
}

void *
udev_get_userdata(struct udev *udev_ctx)
{
          return udev_ctx->userdata;
}

void
udev_set_userdata(struct udev *udev_ctx, void *userdata)
{
          udev_ctx->userdata = userdata;
}

struct udev_enumerate *
udev_enumerate_new(struct udev *udev_ctx)
{
          struct udev_enumerate *udev_enum;

          udev_enum = malloc(sizeof(struct udev_enumerate));

          udev_enum->refs = 1;
          udev_enum->pa = NULL;
          TAILQ_INIT(&udev_enum->list_entries);
          udev_enum->udev_ctx = udev_ref(udev_ctx);
}

struct udev_enumerate *
udev_enumerate_ref(struct udev_enumerate *udev_enum)
{
          atomic_add_int(&udev_enum->refs, 1);

          return udev_enum;
}

void
udev_enumerate_unref(struct udev_enumerate *udev_enum)
{
          struct udev_list_entry        *le;
          int refcount;

          refcount = atomic_fetchadd_int(&udev_enum->refs, -1);

          if (refcount == 1) {
                    atomic_subtract_int(&udev_enum->refs, 0x400); /* in destruction */
                    if (udev_enum->pa != NULL)
                              prop_object_release(udev_enum->pa);

                    while (!TAILQ_EMPTY(&udev_enum->list_entries)) {
                              le = TAILQ_FIRST(&udev_enum->list_entries);
                              TAILQ_REMOVE(&udev_enum->list_entries, le, link);
                              prop_object_release(le->dict);
                              free(le);
                    }
                    udev_unref(udev_enum->udev_ctx);
                    free(udev_enum);
          }
}

struct udev *
udev_enumerate_get_udev(struct udev_enumerate *udev_enum)
{
          return udev_enum->udev_ctx;
}

int
udev_enumerate_scan_devices(struct udev_enumerate *udev_enum)
{
          prop_array_t        pa;

          if (udev_enum->udev_ctx->gp_fd == -1)
                    return -1;

          pa = udevd_request_devs(udev_enum->udev_ctx->gp_fd);
          if (pa == NULL)
                    return -1;

          prop_object_retain(pa);

          if (udev_enum->pa != NULL)
                    prop_object_release(udev_enum->pa);

          udev_enum->iter = NULL;
          udev_enum->pa = pa;

          return 0;
}

struct udev_list_entry *
udev_enumerate_get_list_entry(struct udev_enumerate *udev_enum)
{
          struct udev_list_entry *le;
          prop_object_iterator_t        iter;

          /* If the list is not empty, assume it was populated in an earlier call */
          if (!TAILQ_EMPTY(&udev_enum->list_entries))
                    return TAILQ_FIRST(&udev_enum->list_entries);

          iter = prop_array_iterator(udev_enum->pa);
          if (iter == NULL)
                    return NULL;

          while ((dict = prop_object_iterator_next(iter)) != NULL) {
                    le = malloc(sizeof(struct udev_list_entry));
                    if (le == NULL)
                              goto out;

                    prop_object_retain(dict);
                    le->dict = dict;
                    TAILQ_INSERT_TAIL(&udev_enum->list_entries, le, link);
          }

          le = TAILQ_FIRST(&udev_enum->list_entries);

out:
          prop_object_iterator_release(iter);
          return le;
}

prop_array_t
udev_enumerate_get_array(struct udev_enumerate *udev_enum)
{
          return udev_enum->pa;
}

struct udev_list_entry *
udev_list_entry_get_next(struct udev_list_entry *list_entry)
{
          return TAILQ_NEXT(list_entry, link);
}

prop_dictionary_t
udev_list_entry_get_dictionary(struct udev_list_entry *list_entry)
{
          return list_entry->dict;
}

#define   udev_list_entry_foreach(list_entry, first_entry) \
          for(list_entry = first_entry; \
              list_entry != NULL; \
              list_entry = udev_list_entry_get_next(list_entry))


struct udev_monitor *
udev_monitor_new(struct udev *udev_ctx)
{
          struct udev_monitor *udev_monitor;
          int ret, s;

          ret = conn_local_server(LISTEN_SOCKET_FILE, SOCK_STREAM, 0, &s);
          if (ret < 0)
                    return NULL;

          udev_monitor = malloc(sizeof(struct udev_monitor));
          if (udev_monitor == NULL)
                    return NULL;

          udev_monitor->refs = 1;
          udev_monitor->ev_filt = NULL;
          udev_monitor->socket = s;
          udev_monitor->user_socket = 1;
          udev_monitor->udev_ctx = udev_ref(udev_ctx);

          return udev_monitor;
}


struct udev_monitor *
udev_monitor_ref(struct udev_monitor *udev_monitor)
{
          atomic_add_int(&udev_monitor->refs, 1);

          return udev_monitor;
}

void
udev_monitor_unref(struct udev_monitor *udev_monitor)
{
          int refcount;

          refcount = atomic_fetchadd_int(&udev_monitor->refs, -1);

          if (refcount == 1) {
                    atomic_subtract_int(&udev_monitor->refs, 0x400); /* in destruction */
                    if (udev_monitor->ev_filt != NULL)
                              prop_object_release(udev_monitor->ev_filt);

                    if (udev_monitor->socket != -1)
                              close(udev_monitor->socket);
                    if (udev_monitor->user_socket != -1)
                              close(udev_monitor->user_socket);

                    udev_unref(udev_monitor->udev_ctx);
                    free(udev_monitor);
          }
}

struct udev *
udev_monitor_get_udev(struct udev_monitor *udev_monitor)
{
          return udev_monitor->udev_ctx;
}

int
udev_monitor_get_fd(struct udev_monitor *udev_monitor)
{
          return udev_monitor->socket;
}

struct udev_device *
udev_monitor_receive_device(struct udev_monitor *udev_monitor)
{
          struct udev_device *udev_dev;
          prop_dictionary_t dict;
          prop_number_t       pn;
          char *xml;
          int n, evtype;

          xml = malloc(12*1024*1024);
          if (xml == NULL)
                    return NULL;

          if ((n = read_xml(udev_monitor->socket, xml, 12*1024*1024)) <= 0) {
                    free(xml);
                    return NULL;
          }

          xml[n+1] = '\0';
          dict = prop_dictionary_internalize(xml);
          free(xml);
          if (dict == NULL)
                    return NULL;

          pn = prop_dictionary_get(dict, "evtype");
          if (pn == NULL) {
                    prop_object_release(dict);
                    return NULL;
          }

          udev_dev = malloc(sizeof(struct udev_dev));
          if (udev_dev == NULL) {
                    prop_object_release(dict);
                    return NULL;
          }

          udev_dev->refs = 1;
          udev_dev->ev_type = prop_number_integer_value(pn);
          udev_dev->dict = prop_dictionary_get(dict, "evdict");
          if (udev_dev->dict == NULL) {
                    free(udev_dev);
                    return NULL;
          }
          udev_dev->udev_ctx = udev_ref(udev_monitor->udev_ctx);

out:
          return udev_dev;
}

int
udev_monitor_enable_receiving(struct udev_monitor *udev_monitor)
{
          prop_dictionary_t   dict;
          char *xml;
          /* ->socket, ->user_socket, ->ev_filt */

          dict = udevd_get_command_dict(__DECONST(char *, "monitor"));
          if (dict == NULL)
                    return -1;

          /* Add event filters to message, if available */
          if (udev_monitor->ev_filt != NULL) {
                    if (prop_dictionary_set(dict, "filters",
                        udev_monitor->ev_filt) == false) {
                              prop_object_release(dict);
                              return -1;
                    }
          }

          xml = prop_dictionary_externalize(dict);
          prop_object_release(dict);
          if (xml == NULL)
                    return -1;

          n = send_xml(udev_monitor->socket, xml);
          free(xml);
          if (n <= 0)
                    return NULL;

          return 0;
}

int
udev_monitor_filter_add_match_subsystem_devtype(struct udev_monitor *udev_monitor,
                                                            const char *subsystem,
                                                            const char *devtype __unused)
{
          int ret;

          ret = _udev_monitor_filter_add_match_gen(udev_monitor,
                                                             EVENT_FILTER_TYPE_WILDCARD,
                                                             0,
                                                             "subsystem",
                                                             subsystem);

          return ret;
}

int
udev_monitor_filter_add_match_expr(struct udev_monitor *udev_monitor,
                                           const char *key,
                                           char *expr)
{
          int ret;

          ret = _udev_monitor_filter_add_match_gen(udev_monitor,
                                                             EVENT_FILTER_TYPE_WILDCARD,
                                                             0,
                                                             key,
                                                             expr);

          return ret;
}

int
udev_monitor_filter_add_nomatch_expr(struct udev_monitor *udev_monitor,
                                             const char *key,
                                             char *expr)
{
          int ret;

          ret = _udev_monitor_filter_add_match_gen(udev_monitor,
                                                             EVENT_FILTER_TYPE_WILDCARD,
                                                             1,
                                                             key,
                                                             expr);

          return ret;
}

int
udev_monitor_filter_add_match_regex(struct udev_monitor *udev_monitor,
                                           const char *key,
                                           char *expr)
{
          int ret;

          ret = _udev_monitor_filter_add_match_gen(udev_monitor,
                                                             EVENT_FILTER_TYPE_REGEX,
                                                             0,
                                                             key,
                                                             expr);

          return ret;
}

int
udev_monitor_filter_add_nomatch_regex(struct udev_monitor *udev_monitor,
                                             const char *key,
                                             char *expr)
{
          int ret;

          ret = _udev_monitor_filter_add_match_gen(udev_monitor,
                                                             EVENT_FILTER_TYPE_REGEX,
                                                             1,
                                                             key,
                                                             expr);

          return ret;
}

int
_udev_monitor_filter_add_match_gen(struct udev_monitor *udev_monitor,
                                           int type,
                                           int neg,
                                           const char *key,
                                           char *expr)
{
          prop_array_t                  pa;
          prop_dictionary_t   dict;
          int error;

          if (subsystem == NULL)
                    return NULL;

          dict = prop_dictionary_create();
          if (dict == NULL)
                    return -1;

          error = _udev_dict_set_cstr(dict, "key", key);
          if (error != 0)
                    goto error_out;
          error = _udev_dict_set_int(dict, "type", type);
          if (error != 0)
                    goto error_out;
          error = _udev_dict_set_int(dict, "expr", expr);
          if (error != 0)
                    goto error_out;

          if (neg) {
                    error = _udev_dict_set_int(dict, "negative", 1);
                    if (error != 0)
                              goto error_out;
          }

          if (udev_monitor->ev_filt == NULL) {
                    pa = prop_array_create();
                    if (pa == NULL)
                              goto error_out;

                    udev_monitor->ev_filt = pa;
          }

          if (prop_array_add(udev_monitor->ev_filt, dict) == false)
                    goto error_out;

          return 0;

error_out:
          prop_object_release(dict);
          return -1;
}

struct udev_device *
udev_device_ref(struct udev_device *udev_device)
{
          atomic_add_int(&udev_device->refs, 1);

          return udev_device;
}

void
udev_device_unref(struct udev_device *udev_device)
{
          int refcount;

          refcount = atomic_fetchadd_int(&udev_device->refs, -1);

          if (refcount == 1) {
                    atomic_subtract_int(&udev_device->refs, 0x400); /* in destruction */
                    if (udev_device->dict != NULL)
                              prop_object_release(udev_device->dict);

                    udev_unref(udev_device->udev_ctx);
                    free(udev_device);
          }
}

prop_dictionary_t
udev_device_get_dictionary(struct udev_device *udev_device)
{
          return udev_device->dict;
}

struct udev *
udev_device_get_udev(struct udev_device *udev_device)
{
          return udev_device->udev_ctx;
}

int
send_xml(int s, char *xml)
{
          ssize_t r,n;
          size_t sz;

          sz = strlen(xml) + 1;

          r = send(s, &sz, sizeof(sz), 0);
          if (r <= 0)
                    return r;

          r = 0;
          while (r < (ssize_t)sz) {
                    n = send(s, xml+r, sz-r, 0);
                    if (n <= 0)
                              return n;
                    r += n;
          }

          return r;
}

int
read_xml(int s, char *buf, size_t buf_sz)
{
          size_t sz;
          int n, r;

          n = recv(s, &sz, sizeof(sz), MSG_WAITALL);
          if (n <= 0)
                    return n;

          r = 0;
          while ((r < (ssize_t)sz) && (r < (ssize_t)buf_sz)) {
                    n = recv(s, buf+r, sz-r, MSG_WAITALL);
                    if (n <= 0)
                              return n;
                    r += n;
          }

          return r;
}


static int
_udev_dict_set_cstr(prop_dictionary_t dict, const char *key, char *str)
{
          prop_string_t       ps;

          ps = prop_string_create_cstring(str);
          if (ps == NULL)
                    return ENOMEM;

          if (prop_dictionary_set(dict, key, ps) == false) {
                    prop_object_release(ps);
                    return ENOMEM;
          }

          prop_object_release(ps);
          return 0;
}

static int
_udev_dict_set_int(prop_dictionary_t dict, const char *key, int64_t val)
{
          prop_number_t       pn;

          pn = prop_number_create_integer(val);
          if (pn == NULL)
                    return ENOMEM;

          if (prop_dictionary_set(dict, key, pn) == false) {
                    prop_object_release(pn);
                    return ENOMEM;
          }

          prop_object_release(pn);
          return 0;
}

static int
_udev_dict_set_uint(prop_dictionary_t dict, const char *key, uint64_t val)
{
          prop_number_t       pn;

          pn = prop_number_create_unsigned_integer(val);
          if (pn == NULL)
                    return ENOMEM;

          if (prop_dictionary_set(dict, key, pn) == false) {
                    prop_object_release(pn);
                    return ENOMEM;
          }

          prop_object_release(pn);
          return 0;
}

int
conn_local_server(const char *sockfile, int socktype, int nonblock,
                      int *retsock)
{
          int s;
          struct sockaddr_un serv_addr;

          *retsock = -1;
          if ((s = socket(AF_UNIX, socktype, 0)) < 0)
                    return -1;

          memset(&serv_addr, 0, sizeof(serv_addr));
          serv_addr.sun_family = AF_UNIX;
          strncpy(serv_addr.sun_path, sockfile, SOCKFILE_NAMELEN);
          serv_addr.sun_path[SOCKFILE_NAMELEN - 1] = '\0';

          if (nonblock && unblock_descriptor(s) < 0) {
                    close(s);
                    return -1;
          }

          *retsock = s;
          return connect(s, (struct sockaddr *)&serv_addr, sizeof(serv_addr));
}

prop_dictionary_t
udevd_get_command_dict(char *command)
{
          prop_dictionary_t   dict;
          int       error;

          dict = prop_dictionary_create();
          if (dict == NULL)
                    return NULL;

          if ((error = _udev_dict_set_cstr(dict, "command", command)))
                    goto error_out;

          return dict;

error_out:
          prop_object_release(dict);
          return NULL;
}

prop_array_t
udevd_request_devs(int s)
{
          prop_array_t        pa;
          prop_dictionary_t   dict;
          char *xml;

          int n, t;

          dict = udevd_get_command_dict(__DECONST(char *, "getdevs"));
          if (dict == NULL)
                    return NULL;

          xml = prop_dictionary_externalize(dict);
          prop_object_release(dict);
          if (xml == NULL)
                    return NULL;

          n = send_xml(s, xml);
          free(xml);

          if (n <= 0)
                    return NULL;

          xml = malloc(12*1024*1024); /* generous 12 MB */
          if ((n = read_xml(s, xml, 12*1024*1024)) <= 0) {
                    free(xml);
                    return NULL;
          }

          xml[n+1] = '\0';
          pa = prop_array_internalize(xml);
          free(xml);
          return (pa);
}


int
main(void)
{
          int ret, s;

          ret = conn_local_server(LISTEN_SOCKET_FILE, SOCK_STREAM, 0, &s);
          if (ret < 0)
                    err(1, "conn_local_server");

          udevd_request_devs(s);

          return 0;
}