nlmsg.c

/*
 * (C) 2008-2010 by Pablo Neira Ayuso <pablo@netfilter.org>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published
 * by the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 */
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <errno.h>
#include <string.h>
#include <libmnl/libmnl.h>
#include "internal.h"

EXPORT_SYMBOL size_t mnl_nlmsg_size(size_t len)
{
        return len + MNL_NLMSG_HDRLEN;
}

EXPORT_SYMBOL size_t mnl_nlmsg_get_payload_len(const struct nlmsghdr *nlh)
{
        return nlh->nlmsg_len - MNL_NLMSG_HDRLEN;
}

EXPORT_SYMBOL struct nlmsghdr *mnl_nlmsg_put_header(void *buf)
{
        int len = MNL_ALIGN(sizeof(struct nlmsghdr));
        struct nlmsghdr *nlh = buf;

        memset(buf, 0, len);
        nlh->nlmsg_len = len;
        return nlh;
}

EXPORT_SYMBOL void *mnl_nlmsg_put_extra_header(struct nlmsghdr *nlh,
                                               size_t size)
{
        char *ptr = (char *)nlh + nlh->nlmsg_len;
        size_t len = MNL_ALIGN(size);
        nlh->nlmsg_len += len;
        memset(ptr, 0, len);
        return ptr;
}

EXPORT_SYMBOL void *mnl_nlmsg_get_payload(const struct nlmsghdr *nlh)
{
        return (void *)nlh + MNL_NLMSG_HDRLEN;
}

EXPORT_SYMBOL void *mnl_nlmsg_get_payload_offset(const struct nlmsghdr *nlh,
                                                 size_t offset)
{
        return (void *)nlh + MNL_NLMSG_HDRLEN + MNL_ALIGN(offset);
}

EXPORT_SYMBOL bool mnl_nlmsg_ok(const struct nlmsghdr *nlh, int len)
{
        size_t ulen = len;

        if (len < 0)
                return false;

        return ulen           >= sizeof(struct nlmsghdr) &&
               nlh->nlmsg_len >= sizeof(struct nlmsghdr) &&
               nlh->nlmsg_len <= ulen;
}

EXPORT_SYMBOL struct nlmsghdr *mnl_nlmsg_next(const struct nlmsghdr *nlh,
                                              int *len)
{
        *len -= MNL_ALIGN(nlh->nlmsg_len);
        return (struct nlmsghdr *)((void *)nlh + MNL_ALIGN(nlh->nlmsg_len));
}

EXPORT_SYMBOL void *mnl_nlmsg_get_payload_tail(const struct nlmsghdr *nlh)
{
        return (void *)nlh + MNL_ALIGN(nlh->nlmsg_len);
}

EXPORT_SYMBOL bool mnl_nlmsg_seq_ok(const struct nlmsghdr *nlh,
                                    unsigned int seq)
{
        return nlh->nlmsg_seq && seq ? nlh->nlmsg_seq == seq : true;
}

EXPORT_SYMBOL bool mnl_nlmsg_portid_ok(const struct nlmsghdr *nlh,
                                       unsigned int portid)
{
        return nlh->nlmsg_pid && portid ? nlh->nlmsg_pid == portid : true;
}

static void mnl_nlmsg_fprintf_header(FILE *fd, const struct nlmsghdr *nlh)
{
        fprintf(fd, "----------------\t------------------\n");
        fprintf(fd, "|  %.010u  |\t| message length |\n", nlh->nlmsg_len);
        fprintf(fd, "| %.05u | %c%c%c%c |\t|  type | flags  |\n",
                nlh->nlmsg_type,
                nlh->nlmsg_flags & NLM_F_REQUEST ? 'R' : '-',
                nlh->nlmsg_flags & NLM_F_MULTI ? 'M' : '-',
                nlh->nlmsg_flags & NLM_F_ACK ? 'A' : '-',
                nlh->nlmsg_flags & NLM_F_ECHO ? 'E' : '-');
        fprintf(fd, "|  %.010u  |\t| sequence number|\n", nlh->nlmsg_seq);
        fprintf(fd, "|  %.010u  |\t|     port ID    |\n", nlh->nlmsg_pid);
        fprintf(fd, "----------------\t------------------\n");
}

static void mnl_nlmsg_fprintf_payload(FILE *fd, const struct nlmsghdr *nlh,
                                      size_t extra_header_size)
{
        int rem = 0;
        unsigned int i;

        for (i=sizeof(struct nlmsghdr); i<nlh->nlmsg_len; i+=4) {
                char *b = (char *) nlh;
                struct nlattr *attr = (struct nlattr *) (b+i);

                /* netlink control message. */
                if (nlh->nlmsg_type < NLMSG_MIN_TYPE) {
                        fprintf(fd, "| %.2x %.2x %.2x %.2x  |\t",
                                0xff & b[i],    0xff & b[i+1],
                                0xff & b[i+2],  0xff & b[i+3]);
                        fprintf(fd, "|                |\n");
                /* special handling for the extra header. */
                } else if (extra_header_size > 0) {
                        extra_header_size -= 4;
                        fprintf(fd, "| %.2x %.2x %.2x %.2x  |\t",
                                0xff & b[i],    0xff & b[i+1],
                                0xff & b[i+2],  0xff & b[i+3]);
                        fprintf(fd, "|  extra header  |\n");
                /* this seems like an attribute header. */
                } else if (rem == 0 && (attr->nla_type & NLA_TYPE_MASK) != 0) {
                        fprintf(fd, "|%c[%d;%dm"
                                    "%.5u"
                                    "%c[%dm"
                                    "|"
                                    "%c[%d;%dm"
                                    "%c%c"
                                    "%c[%dm"
                                    "|"
                                    "%c[%d;%dm"
                                    "%.5u"
                                    "%c[%dm|\t",
                                27, 1, 31,
                                attr->nla_len,
                                27, 0,
                                27, 1, 32,
                                attr->nla_type & NLA_F_NESTED ? 'N' : '-',
                                attr->nla_type &
                                        NLA_F_NET_BYTEORDER ? 'B' : '-',
                                27, 0,
                                27, 1, 34,
                                attr->nla_type & NLA_TYPE_MASK,
                                27, 0);
                        fprintf(fd, "|len |flags| type|\n");

                        if (!(attr->nla_type & NLA_F_NESTED)) {
                                rem = NLA_ALIGN(attr->nla_len) -
                                        sizeof(struct nlattr);
                        }
                /* this is the attribute payload. */
                } else if (rem > 0) {
                        rem -= 4;
                        fprintf(fd, "| %.2x %.2x %.2x %.2x  |\t",
                                0xff & b[i],    0xff & b[i+1],
                                0xff & b[i+2],  0xff & b[i+3]);
                        fprintf(fd, "|      data      |");
                        fprintf(fd, "\t %c %c %c %c\n",
                                isprint(b[i]) ? b[i] : ' ',
                                isprint(b[i+1]) ? b[i+1] : ' ',
                                isprint(b[i+2]) ? b[i+2] : ' ',
                                isprint(b[i+3]) ? b[i+3] : ' ');
                }
        }
        fprintf(fd, "----------------\t------------------\n");
}

EXPORT_SYMBOL void mnl_nlmsg_fprintf(FILE *fd, const void *data, size_t datalen,
                                     size_t extra_header_size)
{
        const struct nlmsghdr *nlh = data;
        int len = datalen;

        while (mnl_nlmsg_ok(nlh, len)) {
                mnl_nlmsg_fprintf_header(fd, nlh);
                mnl_nlmsg_fprintf_payload(fd, nlh, extra_header_size);
                nlh = mnl_nlmsg_next(nlh, &len);
        }
}

struct mnl_nlmsg_batch {
        /* the buffer that is used to store the batch. */
        void *buf;
        size_t limit;
        size_t buflen;
        /* the current netlink message in the batch. */
        void *cur;
        bool overflow;
};

EXPORT_SYMBOL struct mnl_nlmsg_batch *mnl_nlmsg_batch_start(void *buf,
                                                            size_t limit)
{
        struct mnl_nlmsg_batch *b;

        b = malloc(sizeof(struct mnl_nlmsg_batch));
        if (b == NULL)
                return NULL;

        b->buf = buf;
        b->limit = limit;
        b->buflen = 0;
        b->cur = buf;
        b->overflow = false;

        return b;
}

EXPORT_SYMBOL void mnl_nlmsg_batch_stop(struct mnl_nlmsg_batch *b)
{
        free(b);
}

EXPORT_SYMBOL bool mnl_nlmsg_batch_next(struct mnl_nlmsg_batch *b)
{
        struct nlmsghdr *nlh = b->cur;

        if (b->buflen + nlh->nlmsg_len > b->limit) {
                b->overflow = true;
                return false;
        }
        b->cur = b->buf + b->buflen + nlh->nlmsg_len;
        b->buflen += nlh->nlmsg_len;
        return true;
}

EXPORT_SYMBOL void mnl_nlmsg_batch_reset(struct mnl_nlmsg_batch *b)
{
        if (b->overflow) {
                struct nlmsghdr *nlh = b->cur;
                memcpy(b->buf, b->cur, nlh->nlmsg_len);
                b->buflen = nlh->nlmsg_len;
                b->cur = b->buf + b->buflen;
                b->overflow = false;
        } else {
                b->buflen = 0;
                b->cur = b->buf;
        }
}

EXPORT_SYMBOL size_t mnl_nlmsg_batch_size(struct mnl_nlmsg_batch *b)
{
        return b->buflen;
}

EXPORT_SYMBOL void *mnl_nlmsg_batch_head(struct mnl_nlmsg_batch *b)
{
        return b->buf;
}

EXPORT_SYMBOL void *mnl_nlmsg_batch_current(struct mnl_nlmsg_batch *b)
{
        return b->cur;
}

EXPORT_SYMBOL bool mnl_nlmsg_batch_is_empty(struct mnl_nlmsg_batch *b)
{
        return b->buflen == 0;
}