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 <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"

struct pkt_buff *
pktb_alloc(int family, void *data, size_t len, size_t extra)
{
        struct pkt_buff *pktb;
        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->head = pkt_data;
        pktb->data = pkt_data;
        pktb->tail = pktb->head + len;

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

                pktb->mac_header = pktb->data;

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

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

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

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

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

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

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

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

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

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

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

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

static int pktb_expand_tail(struct pkt_buff *pkt, 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 (pkt->len + extra > pkt->data_len)
                return 0;

        pkt->len += extra;
        pkt->tail = pkt->tail + extra;
        return 1;
}

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

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

        return 1;
}

int pktb_mangle(struct pkt_buff *pkt,
                 unsigned 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(pkt) &&
            !enlarge_pkt(pkt, rep_len - match_len))
                return 0;

        data = pkt->network_header + dataoff;

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

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

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

        pkt->mangled = true;
        return 1;
}
EXPORT_SYMBOL(pktb_mangle);

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