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)
{
    return len >= (int)sizeof(struct nlmsghdr) &&
           nlh->nlmsg_len >= sizeof(struct nlmsghdr) &&
           (int)nlh->nlmsg_len <= len;
}
 
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;
}