ipv6.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 <stdio.h>
#include <stddef.h>
#include <stdbool.h>
#include <arpa/inet.h>
#include <netinet/ip6.h>

#include <libnetfilter_queue/libnetfilter_queue.h>
#include <libnetfilter_queue/libnetfilter_queue_ipv6.h>
#include <libnetfilter_queue/pktbuff.h>

#include "internal.h"

EXPORT_SYMBOL
struct ip6_hdr *nfq_ip6_get_hdr(struct pkt_buff *pktb)
{
        struct ip6_hdr *ip6h;
        unsigned int pktlen = pktb_tail(pktb) - pktb->network_header;

        /* Not enough room for IPv6 header. */
        if (pktlen < sizeof(struct ip6_hdr))
                return NULL;

        ip6h = (struct ip6_hdr *)pktb->network_header;

        /* Not IPv6 packet. */
        if ((*(uint8_t *)ip6h & 0xf0) != 0x60)
                return NULL;

        return ip6h;
}

EXPORT_SYMBOL
int nfq_ip6_set_transport_header(struct pkt_buff *pktb, struct ip6_hdr *ip6h,
                                 uint8_t target)
{
        uint8_t nexthdr = ip6h->ip6_nxt;
        uint8_t *cur = (uint8_t *)ip6h + sizeof(struct ip6_hdr);

        while (nexthdr != target) {
                struct ip6_ext *ip6_ext;
                uint32_t hdrlen;

                /* No more extensions, we're done. */
                if (nexthdr == IPPROTO_NONE) {
                        cur = NULL;
                        break;
                }
                /* No room for extension, bad packet. */
                if (pktb_tail(pktb) - cur < sizeof(struct ip6_ext)) {
                        cur = NULL;
                        break;
                }
                ip6_ext = (struct ip6_ext *)cur;

                if (nexthdr == IPPROTO_FRAGMENT) {
                        uint16_t *frag_off;

                        /* No room for full fragment header, bad packet. */
                        if (pktb_tail(pktb) - cur < sizeof(struct ip6_frag)) {
                                cur = NULL;
                                break;
                        }

                        frag_off = (uint16_t *)cur +
                                        offsetof(struct ip6_frag, ip6f_offlg);

                        /* Fragment offset is only 13 bits long. */
                        if (htons(*frag_off & ~0x7)) {
                                /* Not the first fragment, it does not contain
                                 * any headers.
                                 */
                                cur = NULL;
                                break;
                        }
                        hdrlen = sizeof(struct ip6_frag);
                } else if (nexthdr == IPPROTO_AH)
                        hdrlen = (ip6_ext->ip6e_len + 2) << 2;
                else
                        hdrlen = ip6_ext->ip6e_len;

                nexthdr = ip6_ext->ip6e_nxt;
                cur += hdrlen;
        }
        pktb->transport_header = cur;
        return cur ? 1 : 0;
}

EXPORT_SYMBOL
int nfq_ip6_mangle(struct pkt_buff *pktb, unsigned int dataoff,
                   unsigned int match_offset, unsigned int match_len,
                   const char *rep_buffer, unsigned int rep_len)
{
        struct ip6_hdr *ip6h = (struct ip6_hdr *)pktb->network_header;

        if (!pktb_mangle(pktb, dataoff, match_offset, match_len, rep_buffer,
                         rep_len))
                return 0;

        /* Fix IPv6 hdr length information */
        ip6h->ip6_plen =
                htons(pktb_tail(pktb) - pktb->network_header - sizeof *ip6h);

        return 1;
}

EXPORT_SYMBOL
int nfq_ip6_snprintf(char *buf, size_t size, const struct ip6_hdr *ip6h)
{
        int ret;
        char src[INET6_ADDRSTRLEN];
        char dst[INET6_ADDRSTRLEN];

        inet_ntop(AF_INET6, &ip6h->ip6_src, src, INET6_ADDRSTRLEN);
        inet_ntop(AF_INET6, &ip6h->ip6_dst, dst, INET6_ADDRSTRLEN);

        ret = snprintf(buf, size, "SRC=%s DST=%s LEN=%zu TC=0x%X "
                                  "HOPLIMIT=%u FLOWLBL=%u ",
                        src, dst,
                        ntohs(ip6h->ip6_plen) + sizeof(struct ip6_hdr),
                        (ip6h->ip6_flow & 0x0ff00000) >> 20,
                        ip6h->ip6_hlim,
                        (ip6h->ip6_flow & 0x000fffff));

        return ret;
}