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  * pktb_alloc - allocate a new packet buffer
  31  * \param family Indicate what family, eg. AF_BRIDGE, AF_INET, AF_INET6, ...
  32  * \param data Pointer to packet data
  33  * \param len Packet length
  34  * \param extra Extra memory in the tail to be allocated (for mangling)
  35  *
  36  * This function returns a packet buffer that contains the packet data and
  37  * some extra memory room in the tail (in case of requested).
  38  *
  39  * \return a pointer to a new queue handle or NULL on failure.
  40  */
  41 struct pkt_buff *
  42 pktb_alloc(int family, void *data, size_t len, size_t extra)
  43 {
  44         struct pkt_buff *pktb;
  45         void *pkt_data;
  46
  47         pktb = calloc(1, sizeof(struct pkt_buff) + len + extra);
  48         if (pktb == NULL)
  49                 return NULL;
  50
  51         /* Better make sure alignment is correct. */
  52         pkt_data = (uint8_t *)pktb + sizeof(struct pkt_buff);
  53         memcpy(pkt_data, data, len);
  54
  55         pktb->len = len;
  56         pktb->data_len = len + extra;
  57
  58         pktb->head = pkt_data;
  59         pktb->data = pkt_data;
  60         pktb->tail = pktb->head + len;
  61
  62         switch(family) {
  63         case AF_INET:
  64                 pktb->network_header = pktb->data;
  65                 break;
  66         case AF_BRIDGE: {
  67                 struct ethhdr *ethhdr = (struct ethhdr *)pktb->data;
  68
  69                 pktb->mac_header = pktb->data;
  70
  71                 switch(ethhdr->h_proto) {
  72                 case ETH_P_IP:
  73                         pktb->network_header = pktb->data + ETH_HLEN;
  74                         break;
  75                 default:
  76                         /* This protocol is unsupported. */
  77                         free(pktb);
  78                         return NULL;
  79                 }
  80                 break;
  81         }
  82         }
  83         return pktb;
  84 }
  85
  86 uint8_t *pktb_data(struct pkt_buff *pktb)
  87 {
  88         return pktb->data;
  89 }
  90
  91 uint32_t pktb_len(struct pkt_buff *pktb)
  92 {
  93         return pktb->len;
  94 }
  95
  96 void pktb_free(struct pkt_buff *pktb)
  97 {
  98         free(pktb);
  99 }
 100
 101 void pktb_push(struct pkt_buff *pktb, unsigned int len)
 102 {
 103         pktb->data += len;
 104 }
 105
 106 void pktb_pull(struct pkt_buff *pktb, unsigned int len)
 107 {
 108         pktb->data -= len;
 109 }
 110
 111 void pktb_put(struct pkt_buff *pktb, unsigned int len)
 112 {
 113         pktb->tail += len;
 114 }
 115
 116 void pktb_trim(struct pkt_buff *pktb, unsigned int len)
 117 {
 118         pktb->len = len;
 119 }
 120
 121 unsigned int pktb_tailroom(struct pkt_buff *pktb)
 122 {
 123         return pktb->data_len - pktb->len;
 124 }
 125
 126 uint8_t *pktb_mac_header(struct pkt_buff *pktb)
 127 {
 128         return pktb->mac_header;
 129 }
 130
 131 uint8_t *pktb_network_header(struct pkt_buff *pktb)
 132 {
 133         return pktb->network_header;
 134 }
 135
 136 uint8_t *pktb_transport_header(struct pkt_buff *pktb)
 137 {
 138         return pktb->transport_header;
 139 }
 140
 141 static int pktb_expand_tail(struct pkt_buff *pkt, int extra)
 142 {
 143         /* No room in packet, cannot mangle it. We don't support dynamic
 144          * reallocation. Instead, increase the size of the extra room in
 145          * the tail in pktb_alloc.
 146          */
 147         if (pkt->len + extra > pkt->data_len)
 148                 return 0;
 149
 150         pkt->len += extra;
 151         pkt->tail = pkt->tail + extra;
 152         return 1;
 153 }
 154
 155 static int enlarge_pkt(struct pkt_buff *pkt, unsigned int extra)
 156 {
 157         if (pkt->len + extra > 65535)
 158                 return 0;
 159
 160         if (!pktb_expand_tail(pkt, extra - pktb_tailroom(pkt)))
 161                 return 0;
 162
 163         return 1;
 164 }
 165
 166 int pktb_mangle(struct pkt_buff *pkt,
 167                  unsigned int dataoff,
 168                  unsigned int match_offset,
 169                  unsigned int match_len,
 170                  const char *rep_buffer,
 171                  unsigned int rep_len)
 172 {
 173         unsigned char *data;
 174
 175         if (rep_len > match_len &&
 176             rep_len - match_len > pktb_tailroom(pkt) &&
 177             !enlarge_pkt(pkt, rep_len - match_len))
 178                 return 0;
 179
 180         data = pkt->network_header + dataoff;
 181
 182         /* move post-replacement */
 183         memmove(data + match_offset + rep_len,
 184                 data + match_offset + match_len,
 185                 pkt->tail - (pkt->network_header + dataoff +
 186                              match_offset + match_len));
 187
 188         /* insert data from buffer */
 189         memcpy(data + match_offset, rep_buffer, rep_len);
 190
 191         /* update pkt info */
 192         if (rep_len > match_len)
 193                 pktb_put(pkt, rep_len - match_len);
 194         else
 195                 pktb_trim(pkt, pkt->len + rep_len - match_len);
 196
 197         pkt->mangled = true;
 198         return 1;
 199 }
 200 EXPORT_SYMBOL(pktb_mangle);
 201
 202 bool pktb_mangled(const struct pkt_buff *pkt)
 203 {
 204         return pkt->mangled;
 205 }
 206 EXPORT_SYMBOL(pktb_mangled);
 207
 208 /**
 209  * @}
 210  */