pktbuff.c

/*
 * (C) 2012 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 General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This code has been sponsored by Vyatta Inc. <http://www.vyatta.com>
 */

#include <errno.h>
#include <stdlib.h>
#include <string.h> /* for memcpy */
#include <stdbool.h>

#include <netinet/if_ether.h>
#include <netinet/ip.h>
#include <netinet/tcp.h>

#include "internal.h"

EXPORT_SYMBOL
struct pkt_buff *pktb_alloc(int family, void *data, size_t len, size_t extra)
{
        struct pkt_buff *pktb;
        struct ethhdr *ethhdr;
        void *pkt_data;

        pktb = calloc(1, sizeof(struct pkt_buff) + len + extra);
        if (pktb == NULL)
                return NULL;

        /* Better make sure alignment is correct. */
        pkt_data = (uint8_t *)pktb + sizeof(struct pkt_buff);
        memcpy(pkt_data, data, len);

        pktb->len = len;
        pktb->data_len = len + extra;

        pktb->data = pkt_data;

        switch(family) {
        case AF_INET:
        case AF_INET6:
                pktb->network_header = pktb->data;
                break;
        case AF_BRIDGE:
                ethhdr = (struct ethhdr *)pktb->data;
                pktb->mac_header = pktb->data;

                switch(ethhdr->h_proto) {
                case ETH_P_IP:
                case ETH_P_IPV6:
                        pktb->network_header = pktb->data + ETH_HLEN;
                        break;
                default:
                        /* This protocol is unsupported. */
                        errno = EPROTONOSUPPORT;
                        free(pktb);
                        return NULL;
                }
                break;
        }
        return pktb;
}

EXPORT_SYMBOL
uint8_t *pktb_data(struct pkt_buff *pktb)
{
        return pktb->data;
}

EXPORT_SYMBOL
uint32_t pktb_len(struct pkt_buff *pktb)
{
        return pktb->len;
}

EXPORT_SYMBOL
void pktb_free(struct pkt_buff *pktb)
{
        free(pktb);
}

EXPORT_SYMBOL
void pktb_push(struct pkt_buff *pktb, unsigned int len)
{
        pktb->data -= len;
        pktb->len += len;
}

EXPORT_SYMBOL
void pktb_pull(struct pkt_buff *pktb, unsigned int len)
{
        pktb->data += len;
        pktb->len -= len;
}

EXPORT_SYMBOL
void pktb_put(struct pkt_buff *pktb, unsigned int len)
{
        pktb->len += len;
}

EXPORT_SYMBOL
void pktb_trim(struct pkt_buff *pktb, unsigned int len)
{
        pktb->len = len;
}

EXPORT_SYMBOL
unsigned int pktb_tailroom(struct pkt_buff *pktb)
{
        return pktb->data_len - pktb->len;
}

EXPORT_SYMBOL
uint8_t *pktb_mac_header(struct pkt_buff *pktb)
{
        return pktb->mac_header;
}

EXPORT_SYMBOL
uint8_t *pktb_network_header(struct pkt_buff *pktb)
{
        return pktb->network_header;
}

EXPORT_SYMBOL
uint8_t *pktb_transport_header(struct pkt_buff *pktb)
{
        return pktb->transport_header;
}

static int pktb_expand_tail(struct pkt_buff *pktb, int extra)
{
        /* No room in packet, cannot mangle it. We don't support dynamic
         * reallocation. Instead, increase the size of the extra room in
         * the tail in pktb_alloc.
         */
        if (pktb->len + extra > pktb->data_len)
                return 0;

        pktb->len += extra;
        return 1;
}

static int enlarge_pkt(struct pkt_buff *pktb, unsigned int extra)
{
        if (pktb->len + extra > 65535)
                return 0;

        if (!pktb_expand_tail(pktb, extra - pktb_tailroom(pktb)))
                return 0;

        return 1;
}

EXPORT_SYMBOL
int pktb_mangle(struct pkt_buff *pktb,
                int dataoff,
                unsigned int match_offset,
                unsigned int match_len,
                const char *rep_buffer,
                unsigned int rep_len)
{
        unsigned char *data;

        if (rep_len > match_len &&
            rep_len - match_len > pktb_tailroom(pktb) &&
            !enlarge_pkt(pktb, rep_len - match_len))
                return 0;

        data = pktb->network_header + dataoff;

        /* move post-replacement */
        memmove(data + match_offset + rep_len,
                data + match_offset + match_len,
                pktb_tail(pktb) - (pktb->network_header + dataoff +
                             match_offset + match_len));

        /* insert data from buffer */
        memcpy(data + match_offset, rep_buffer, rep_len);

        /* update packet info */
        if (rep_len > match_len)
                pktb_put(pktb, rep_len - match_len);
        else
                pktb_trim(pktb, pktb->len + rep_len - match_len);

        pktb->mangled = true;
        return 1;
}

EXPORT_SYMBOL
bool pktb_mangled(const struct pkt_buff *pktb)
{
        return pktb->mangled;
}