src/extra/pktbuff.c

   1 /*
   2  * (C) 2012 by Pablo Neira Ayuso <pablo@netfilter.org>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This code has been sponsored by Vyatta Inc. <http://www.vyatta.com>
  10  */
  11
  12 #include <stdlib.h>
  13 #include <string.h> /* for memcpy */
  14 #include <stdbool.h>
  15
  16 #include <netinet/if_ether.h>
  17 #include <netinet/ip.h>
  18 #include <netinet/tcp.h>
  19
  20 #include "internal.h"
  21
  22 /**
  23  * \defgroup pktbuff User-space network packet buffer
  24  *
  25  * This library provides the user-space network packet buffer. This abstraction
  26  * is strongly inspired by Linux kernel network buffer, the so-called sk_buff.
  27  *
  28  * @{
  29  */
  30
  31 /**
  32  * pktb_alloc - allocate a new packet buffer
  33  * \param family Indicate what family, eg. AF_BRIDGE, AF_INET, AF_INET6, ...
  34  * \param data Pointer to packet data
  35  * \param len Packet length
  36  * \param extra Extra memory in the tail to be allocated (for mangling)
  37  *
  38  * This function returns a packet buffer that contains the packet data and
  39  * some extra memory room in the tail (in case of requested).
  40  *
  41  * \return a pointer to a new queue handle or NULL on failure.
  42  */
  43 struct pkt_buff *
  44 pktb_alloc(int family, void *data, size_t len, size_t extra)
  45 {
  46         struct pkt_buff *pktb;
  47         void *pkt_data;
  48
  49         pktb = calloc(1, sizeof(struct pkt_buff) + len + extra);
  50         if (pktb == NULL)
  51                 return NULL;
  52
  53         /* Better make sure alignment is correct. */
  54         pkt_data = (uint8_t *)pktb + sizeof(struct pkt_buff);
  55         memcpy(pkt_data, data, len);
  56
  57         pktb->len = len;
  58         pktb->data_len = len + extra;
  59
  60         pktb->head = pkt_data;
  61         pktb->data = pkt_data;
  62         pktb->tail = pktb->head + len;
  63
  64         switch(family) {
  65         case AF_INET:
  66                 pktb->network_header = pktb->data;
  67                 break;
  68         case AF_BRIDGE: {
  69                 struct ethhdr *ethhdr = (struct ethhdr *)pktb->data;
  70
  71                 pktb->mac_header = pktb->data;
  72
  73                 switch(ethhdr->h_proto) {
  74                 case ETH_P_IP:
  75                         pktb->network_header = pktb->data + ETH_HLEN;
  76                         break;
  77                 default:
  78                         /* This protocol is unsupported. */
  79                         free(pktb);
  80                         return NULL;
  81                 }
  82                 break;
  83         }
  84         }
  85         return pktb;
  86 }
  87
  88 /**
  89  * pktb_data - return pointer to the beginning of the packet buffer
  90  * \param pktb Pointer to packet buffer
  91  */
  92 uint8_t *pktb_data(struct pkt_buff *pktb)
  93 {
  94         return pktb->data;
  95 }
  96
  97 /**
  98  * pktb_len - return length of the packet buffer
  99  * \param pktb Pointer to packet buffer
 100  */
 101 uint32_t pktb_len(struct pkt_buff *pktb)
 102 {
 103         return pktb->len;
 104 }
 105
 106 /**
 107  * pktb_free - release packet buffer
 108  * \param pktb Pointer to packet buffer
 109  */
 110 void pktb_free(struct pkt_buff *pktb)
 111 {
 112         free(pktb);
 113 }
 114
 115 /**
 116  * pktb_push - update pointer to the beginning of the packet buffer
 117  * \param pktb Pointer to packet buffer
 118  */
 119 void pktb_push(struct pkt_buff *pktb, unsigned int len)
 120 {
 121         pktb->data -= len;
 122         pktb->len += len;
 123 }
 124
 125 /**
 126  * pktb_pull - update pointer to the beginning of the packet buffer
 127  * \param pktb Pointer to packet buffer
 128  */
 129 void pktb_pull(struct pkt_buff *pktb, unsigned int len)
 130 {
 131         pktb->data += len;
 132         pktb->len -= len;
 133 }
 134
 135 /**
 136  * pktb_put - add extra bytes to the tail of the packet buffer
 137  * \param pktb Pointer to packet buffer
 138  */
 139 void pktb_put(struct pkt_buff *pktb, unsigned int len)
 140 {
 141         pktb->tail += len;
 142         pktb->len += len;
 143 }
 144
 145 /**
 146  * pktb_trim - set new length for this packet buffer
 147  * \param pktb Pointer to packet buffer
 148  */
 149 void pktb_trim(struct pkt_buff *pktb, unsigned int len)
 150 {
 151         pktb->len = len;
 152 }
 153
 154 /**
 155  * pktb_tailroom - get room in bytes in the tail of the packet buffer
 156  * \param pktb Pointer to packet buffer
 157  */
 158 unsigned int pktb_tailroom(struct pkt_buff *pktb)
 159 {
 160         return pktb->data_len - pktb->len;
 161 }
 162
 163 /**
 164  * pktb_mac_header - return pointer to layer 2 header (if any)
 165  * \param pktb Pointer to packet buffer
 166  */
 167 uint8_t *pktb_mac_header(struct pkt_buff *pktb)
 168 {
 169         return pktb->mac_header;
 170 }
 171
 172 /**
 173  * pktb_network_header - return pointer to layer 3 header
 174  * \param pktb Pointer to packet buffer
 175  */
 176 uint8_t *pktb_network_header(struct pkt_buff *pktb)
 177 {
 178         return pktb->network_header;
 179 }
 180
 181 /**
 182  * pktb_transport_header - return pointer to layer 4 header (if any)
 183  * \param pktb Pointer to packet buffer
 184  */
 185 uint8_t *pktb_transport_header(struct pkt_buff *pktb)
 186 {
 187         return pktb->transport_header;
 188 }
 189
 190 static int pktb_expand_tail(struct pkt_buff *pkt, int extra)
 191 {
 192         /* No room in packet, cannot mangle it. We don't support dynamic
 193          * reallocation. Instead, increase the size of the extra room in
 194          * the tail in pktb_alloc.
 195          */
 196         if (pkt->len + extra > pkt->data_len)
 197                 return 0;
 198
 199         pkt->len += extra;
 200         pkt->tail = pkt->tail + extra;
 201         return 1;
 202 }
 203
 204 static int enlarge_pkt(struct pkt_buff *pkt, unsigned int extra)
 205 {
 206         if (pkt->len + extra > 65535)
 207                 return 0;
 208
 209         if (!pktb_expand_tail(pkt, extra - pktb_tailroom(pkt)))
 210                 return 0;
 211
 212         return 1;
 213 }
 214
 215 int pktb_mangle(struct pkt_buff *pkt,
 216                  unsigned int dataoff,
 217                  unsigned int match_offset,
 218                  unsigned int match_len,
 219                  const char *rep_buffer,
 220                  unsigned int rep_len)
 221 {
 222         unsigned char *data;
 223
 224         if (rep_len > match_len &&
 225             rep_len - match_len > pktb_tailroom(pkt) &&
 226             !enlarge_pkt(pkt, rep_len - match_len))
 227                 return 0;
 228
 229         data = pkt->network_header + dataoff;
 230
 231         /* move post-replacement */
 232         memmove(data + match_offset + rep_len,
 233                 data + match_offset + match_len,
 234                 pkt->tail - (pkt->network_header + dataoff +
 235                              match_offset + match_len));
 236
 237         /* insert data from buffer */
 238         memcpy(data + match_offset, rep_buffer, rep_len);
 239
 240         /* update pkt info */
 241         if (rep_len > match_len)
 242                 pktb_put(pkt, rep_len - match_len);
 243         else
 244                 pktb_trim(pkt, pkt->len + rep_len - match_len);
 245
 246         pkt->mangled = true;
 247         return 1;
 248 }
 249 EXPORT_SYMBOL(pktb_mangle);
 250
 251 /**
 252  * pktb_mangled - return true if packet has been mangled
 253  * \param pktb Pointer to packet buffer
 254  */
 255 bool pktb_mangled(const struct pkt_buff *pkt)
 256 {
 257         return pkt->mangled;
 258 }
 259 EXPORT_SYMBOL(pktb_mangled);
 260
 261 /**
 262  * @}
 263  */