root/luci2/libunl/io.c @ 6262

/**
 *   unl - Minimalistic netlink library
 *   Copyright (C) 2010 Steven Barth <steven@midlink.org>
 *   Copyright (C) 2010 John Crispin <blogic@openwrt.org>
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation; either version 2 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
 *
 */

#include <stdint.h>
#include <asm/types.h>
#include <sys/socket.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>

#include "unl/io.h"
#include "unl/msg.h"


int unl_open(struct unl *hndl, int proto, const struct sockaddr_nl *peer) {
    const struct sockaddr_nl kernel = {.nl_family = AF_NETLINK};
    if (!peer) peer = &kernel;

    memset(hndl, 0, sizeof(*hndl));
    if ((hndl->fd = socket(AF_NETLINK, SOCK_RAW, proto)) < 0) return -1;
    fcntl(hndl->fd, F_SETFD, fcntl(hndl->fd, F_GETFD) | FD_CLOEXEC);

    struct sockaddr_nl local = { .nl_family = AF_NETLINK };

    if (bind(hndl->fd, (struct sockaddr*)&local, sizeof(hndl->addr))
    || connect(hndl->fd, (struct sockaddr*)peer, sizeof(*peer))) {
        close(hndl->fd);
        return -1;
    }

    return 0;
}

int unl_subscribe(struct unl *hndl, uint32_t group) {
    return setsockopt(hndl->fd, SOL_NETLINK,
        NETLINK_ADD_MEMBERSHIP, &group, sizeof(group));
}

int unl_unsubscribe(struct unl *hndl, uint32_t group) {
    return setsockopt(hndl->fd, SOL_NETLINK,
        NETLINK_DROP_MEMBERSHIP, &group, sizeof(group));
}

int unl_close(struct unl *hndl) {
    int stat = close(hndl->fd);
    if (!stat)
        hndl->fd = -1;
    return stat;
}



/* IO functions */

// Flush it
void unl_flush(struct unl *hndl) {
    uint8_t buf[32];
    struct iovec iov = { .iov_base = buf, .iov_len = sizeof(buf) };
    struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1 };
    while (recvmsg(hndl->fd, &msg, MSG_DONTWAIT) > 0);
}

// Sends a request (return a message sequence id > 0 on success, -1 on fail)
int unl_request(struct unl *hndl, struct nlmsghdr *nlm) {
    struct iovec iov = { .iov_base = nlm, .iov_len = nlm->nlmsg_len };
    struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1 };
    nlm->nlmsg_flags |= NLM_F_REQUEST;
    nlm->nlmsg_pid = 0;
    nlm->nlmsg_seq = ++hndl->seq;
    ssize_t txed = sendmsg(hndl->fd, &msg, 0);
    if (txed == nlm->nlmsg_len) {
        hndl->status = UNL_STATUS_MORE;
        hndl->buffered = 0;
        return 0;
    } else if (txed > 0) {
        errno = EPIPE;
    }
    hndl->status = UNL_STATUS_READY;
    return -1;
}

// Simple receive
ssize_t unl_receive(struct unl *hndl, void *buf, size_t buflen) {
    struct iovec iov = { .iov_base = buf, .iov_len = buflen };
    struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1 };
    ssize_t len = recvmsg(hndl->fd, &msg, 0);
    if (msg.msg_flags & MSG_TRUNC) {
        errno = ENOBUFS;
        len = -1;
    }
    return len;
}


struct nlmsghdr* unl_response
(struct unl *hndl, void *buf, size_t len, int *errcode) {
    if (hndl->status != UNL_STATUS_MORE) return 0;
    for (;; hndl->current = NLMSG_NEXT(hndl->current, hndl->buffered)) {
        while (!NLMSG_OK(hndl->current, hndl->buffered)) {
            ssize_t rxed = unl_receive(hndl, buf, len);
            if (rxed <= 0) {
                if (errcode) *errcode = -errno;
                return NULL; // IO error
            }
            hndl->buffered = rxed;
            hndl->current = (struct nlmsghdr*)buf;
        }
        struct nlmsghdr *c = hndl->current;
        if (c->nlmsg_seq != hndl->seq) continue; // not for us...
        if (c->nlmsg_type == NLMSG_ERROR) {
            if (NLMSG_PAYLOAD(c, sizeof(struct nlmsgerr)) < 0) {
                errno = EPROTO;
            } else { // regular error message
                errno = -((struct nlmsgerr*)NLMSG_DATA(c))->error;
            }
            if (errno) { // ACK packages are error packages with error 0
                if (errcode) *errcode = -errno;
                return NULL;
            }
        }
        if (c->nlmsg_type == NLMSG_DONE) {
            hndl->status = UNL_STATUS_READY;
            if (errcode) *errcode = 0;
            return NULL;
        } else {
            if (!(c->nlmsg_flags & NLM_F_MULTI))
                hndl->status = UNL_STATUS_READY;
            hndl->current = NLMSG_NEXT(c, hndl->buffered);
            if (errcode) *errcode = 0;
            return c;
        }
    }
}


int unl_call(struct unl *hndl, struct nlmsghdr *nlm) {
    uint8_t buffer[UNL_BUFLEN];
    nlm->nlmsg_flags |= NLM_F_ACK; // We need some reply
    int status = unl_request(hndl, nlm);
    if (!status)
        unl_response(hndl, buffer, sizeof(buffer), &status);
    else
        status = -errno;
    return status;
}