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         pktb->len += len;
 105 }
 106
 107 void pktb_pull(struct pkt_buff *pktb, unsigned int len)
 108 {
 109         pktb->data += len;
 110         pktb->len -= len;
 111 }
 112
 113 void pktb_put(struct pkt_buff *pktb, unsigned int len)
 114 {
 115         pktb->tail += len;
 116         pktb->len += len;
 117 }
 118
 119 void pktb_trim(struct pkt_buff *pktb, unsigned int len)
 120 {
 121         pktb->len = len;
 122 }
 123
 124 unsigned int pktb_tailroom(struct pkt_buff *pktb)
 125 {
 126         return pktb->data_len - pktb->len;
 127 }
 128
 129 uint8_t *pktb_mac_header(struct pkt_buff *pktb)
 130 {
 131         return pktb->mac_header;
 132 }
 133
 134 uint8_t *pktb_network_header(struct pkt_buff *pktb)
 135 {
 136         return pktb->network_header;
 137 }
 138
 139 uint8_t *pktb_transport_header(struct pkt_buff *pktb)
 140 {
 141         return pktb->transport_header;
 142 }
 143
 144 static int pktb_expand_tail(struct pkt_buff *pkt, int extra)
 145 {
 146         /* No room in packet, cannot mangle it. We don't support dynamic
 147          * reallocation. Instead, increase the size of the extra room in
 148          * the tail in pktb_alloc.
 149          */
 150         if (pkt->len + extra > pkt->data_len)
 151                 return 0;
 152
 153         pkt->len += extra;
 154         pkt->tail = pkt->tail + extra;
 155         return 1;
 156 }
 157
 158 static int enlarge_pkt(struct pkt_buff *pkt, unsigned int extra)
 159 {
 160         if (pkt->len + extra > 65535)
 161                 return 0;
 162
 163         if (!pktb_expand_tail(pkt, extra - pktb_tailroom(pkt)))
 164                 return 0;
 165
 166         return 1;
 167 }
 168
 169 int pktb_mangle(struct pkt_buff *pkt,
 170                  unsigned int dataoff,
 171                  unsigned int match_offset,
 172                  unsigned int match_len,
 173                  const char *rep_buffer,
 174                  unsigned int rep_len)
 175 {
 176         unsigned char *data;
 177
 178         if (rep_len > match_len &&
 179             rep_len - match_len > pktb_tailroom(pkt) &&
 180             !enlarge_pkt(pkt, rep_len - match_len))
 181                 return 0;
 182
 183         data = pkt->network_header + dataoff;
 184
 185         /* move post-replacement */
 186         memmove(data + match_offset + rep_len,
 187                 data + match_offset + match_len,
 188                 pkt->tail - (pkt->network_header + dataoff +
 189                              match_offset + match_len));
 190
 191         /* insert data from buffer */
 192         memcpy(data + match_offset, rep_buffer, rep_len);
 193
 194         /* update pkt info */
 195         if (rep_len > match_len)
 196                 pktb_put(pkt, rep_len - match_len);
 197         else
 198                 pktb_trim(pkt, pkt->len + rep_len - match_len);
 199
 200         pkt->mangled = true;
 201         return 1;
 202 }
 203 EXPORT_SYMBOL(pktb_mangle);
 204
 205 bool pktb_mangled(const struct pkt_buff *pkt)
 206 {
 207         return pkt->mangled;
 208 }
 209 EXPORT_SYMBOL(pktb_mangled);
 210
 211 /**
 212  * @}
 213  */