sys/netipsec/ipsec_mbuf.c

   1 /*-
   2  * Copyright (c) 2002, 2003 Sam Leffler, Errno Consulting
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  *
  26  * $FreeBSD$
  27  */
  28
  29 /*
  30  * IPsec-specific mbuf routines.
  31  */
  32
  33 #include "opt_param.h"
  34
  35 #include <sys/param.h>
  36 #include <sys/systm.h>
  37 #include <sys/mbuf.h>
  38 #include <sys/socket.h>
  39
  40 #include <net/route.h>
  41 #include <net/vnet.h>
  42
  43 #include <netinet/in.h>
  44
  45 #include <netipsec/ipsec.h>
  46
  47 /*
  48  * Make space for a new header of length hlen at skip bytes
  49  * into the packet.  When doing this we allocate new mbufs only
  50  * when absolutely necessary.  The mbuf where the new header
  51  * is to go is returned together with an offset into the mbuf.
  52  * If NULL is returned then the mbuf chain may have been modified;
  53  * the caller is assumed to always free the chain.
  54  */
  55 struct mbuf *
  56 m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
  57 {
  58         struct mbuf *m;
  59         unsigned remain;
  60
  61         IPSEC_ASSERT(m0 != NULL, ("null mbuf"));
  62         IPSEC_ASSERT(hlen < MHLEN, ("hlen too big: %u", hlen));
  63
  64         for (m = m0; m && skip > m->m_len; m = m->m_next)
  65                 skip -= m->m_len;
  66         if (m == NULL)
  67                 return (NULL);
  68         /*
  69          * At this point skip is the offset into the mbuf m
  70          * where the new header should be placed.  Figure out
  71          * if there's space to insert the new header.  If so,
  72          * and copying the remainder makes sense then do so.
  73          * Otherwise insert a new mbuf in the chain, splitting
  74          * the contents of m as needed.
  75          */
  76         remain = m->m_len - skip;               /* data to move */
  77         if (hlen > M_TRAILINGSPACE(m)) {
  78                 struct mbuf *n0, *n, **np;
  79                 int todo, len, done, alloc;
  80
  81                 n0 = NULL;
  82                 np = &n0;
  83                 alloc = 0;
  84                 done = 0;
  85                 todo = remain;
  86                 while (todo > 0) {
  87                         if (todo > MHLEN) {
  88                                 n = m_getcl(M_NOWAIT, m->m_type, 0);
  89                                 len = MCLBYTES;
  90                         }
  91                         else {
  92                                 n = m_get(M_NOWAIT, m->m_type);
  93                                 len = MHLEN;
  94                         }
  95                         if (n == NULL) {
  96                                 m_freem(n0);
  97                                 return NULL;
  98                         }
  99                         *np = n;
 100                         np = &n->m_next;
 101                         alloc++;
 102                         len = min(todo, len);
 103                         memcpy(n->m_data, mtod(m, char *) + skip + done, len);
 104                         n->m_len = len;
 105                         done += len;
 106                         todo -= len;
 107                 }
 108
 109                 if (hlen <= M_TRAILINGSPACE(m) + remain) {
 110                         m->m_len = skip + hlen;
 111                         *off = skip;
 112                         if (n0 != NULL) {
 113                                 *np = m->m_next;
 114                                 m->m_next = n0;
 115                         }
 116                 }
 117                 else {
 118                         n = m_get(M_NOWAIT, m->m_type);
 119                         if (n == NULL) {
 120                                 m_freem(n0);
 121                                 return NULL;
 122                         }
 123                         alloc++;
 124
 125                         if ((n->m_next = n0) == NULL)
 126                                 np = &n->m_next;
 127                         n0 = n;
 128
 129                         *np = m->m_next;
 130                         m->m_next = n0;
 131
 132                         n->m_len = hlen;
 133                         m->m_len = skip;
 134
 135                         m = n;                  /* header is at front ... */
 136                         *off = 0;               /* ... of new mbuf */
 137                 }
 138                 IPSECSTAT_INC(ips_mbinserted);
 139         } else {
 140                 /*
 141                  * Copy the remainder to the back of the mbuf
 142                  * so there's space to write the new header.
 143                  */
 144                 bcopy(mtod(m, caddr_t) + skip,
 145                       mtod(m, caddr_t) + skip + hlen, remain);
 146                 m->m_len += hlen;
 147                 *off = skip;
 148         }
 149         m0->m_pkthdr.len += hlen;               /* adjust packet length */
 150         return m;
 151 }
 152
 153 /*
 154  * m_pad(m, n) pads <m> with <n> bytes at the end. The packet header
 155  * length is updated, and a pointer to the first byte of the padding
 156  * (which is guaranteed to be all in one mbuf) is returned.
 157  */
 158 caddr_t
 159 m_pad(struct mbuf *m, int n)
 160 {
 161         register struct mbuf *m0, *m1;
 162         register int len, pad;
 163         caddr_t retval;
 164
 165         if (n <= 0) {  /* No stupid arguments. */
 166                 DPRINTF(("%s: pad length invalid (%d)\n", __func__, n));
 167                 m_freem(m);
 168                 return NULL;
 169         }
 170
 171         len = m->m_pkthdr.len;
 172         pad = n;
 173         m0 = m;
 174
 175         while (m0->m_len < len) {
 176                 len -= m0->m_len;
 177                 m0 = m0->m_next;
 178         }
 179
 180         if (m0->m_len != len) {
 181                 DPRINTF(("%s: length mismatch (should be %d instead of %d)\n",
 182                         __func__, m->m_pkthdr.len,
 183                         m->m_pkthdr.len + m0->m_len - len));
 184
 185                 m_freem(m);
 186                 return NULL;
 187         }
 188
 189         /* Check for zero-length trailing mbufs, and find the last one. */
 190         for (m1 = m0; m1->m_next; m1 = m1->m_next) {
 191                 if (m1->m_next->m_len != 0) {
 192                         DPRINTF(("%s: length mismatch (should be %d instead "
 193                                 "of %d)\n", __func__,
 194                                 m->m_pkthdr.len,
 195                                 m->m_pkthdr.len + m1->m_next->m_len));
 196
 197                         m_freem(m);
 198                         return NULL;
 199                 }
 200
 201                 m0 = m1->m_next;
 202         }
 203
 204         if (pad > M_TRAILINGSPACE(m0)) {
 205                 /* Add an mbuf to the chain. */
 206                 MGET(m1, M_NOWAIT, MT_DATA);
 207                 if (m1 == 0) {
 208                         m_freem(m0);
 209                         DPRINTF(("%s: unable to get extra mbuf\n", __func__));
 210                         return NULL;
 211                 }
 212
 213                 m0->m_next = m1;
 214                 m0 = m1;
 215                 m0->m_len = 0;
 216         }
 217
 218         retval = m0->m_data + m0->m_len;
 219         m0->m_len += pad;
 220         m->m_pkthdr.len += pad;
 221
 222         return retval;
 223 }
 224
 225 /*
 226  * Remove hlen data at offset skip in the packet.  This is used by
 227  * the protocols strip protocol headers and associated data (e.g. IV,
 228  * authenticator) on input.
 229  */
 230 int
 231 m_striphdr(struct mbuf *m, int skip, int hlen)
 232 {
 233         struct mbuf *m1;
 234         int roff;
 235
 236         /* Find beginning of header */
 237         m1 = m_getptr(m, skip, &roff);
 238         if (m1 == NULL)
 239                 return (EINVAL);
 240
 241         /* Remove the header and associated data from the mbuf. */
 242         if (roff == 0) {
 243                 /* The header was at the beginning of the mbuf */
 244                 IPSECSTAT_INC(ips_input_front);
 245                 m_adj(m1, hlen);
 246                 if ((m1->m_flags & M_PKTHDR) == 0)
 247                         m->m_pkthdr.len -= hlen;
 248         } else if (roff + hlen >= m1->m_len) {
 249                 struct mbuf *mo;
 250
 251                 /*
 252                  * Part or all of the header is at the end of this mbuf,
 253                  * so first let's remove the remainder of the header from
 254                  * the beginning of the remainder of the mbuf chain, if any.
 255                  */
 256                 IPSECSTAT_INC(ips_input_end);
 257                 if (roff + hlen > m1->m_len) {
 258                         /* Adjust the next mbuf by the remainder */
 259                         m_adj(m1->m_next, roff + hlen - m1->m_len);
 260
 261                         /* The second mbuf is guaranteed not to have a pkthdr... */
 262                         m->m_pkthdr.len -= (roff + hlen - m1->m_len);
 263                 }
 264
 265                 /* Now, let's unlink the mbuf chain for a second...*/
 266                 mo = m1->m_next;
 267                 m1->m_next = NULL;
 268
 269                 /* ...and trim the end of the first part of the chain...sick */
 270                 m_adj(m1, -(m1->m_len - roff));
 271                 if ((m1->m_flags & M_PKTHDR) == 0)
 272                         m->m_pkthdr.len -= (m1->m_len - roff);
 273
 274                 /* Finally, let's relink */
 275                 m1->m_next = mo;
 276         } else {
 277                 /*
 278                  * The header lies in the "middle" of the mbuf; copy
 279                  * the remainder of the mbuf down over the header.
 280                  */
 281                 IPSECSTAT_INC(ips_input_middle);
 282                 bcopy(mtod(m1, u_char *) + roff + hlen,
 283                       mtod(m1, u_char *) + roff,
 284                       m1->m_len - (roff + hlen));
 285                 m1->m_len -= hlen;
 286                 m->m_pkthdr.len -= hlen;
 287         }
 288         return (0);
 289 }
 290
 291 /*
 292  * Diagnostic routine to check mbuf alignment as required by the
 293  * crypto device drivers (that use DMA).
 294  */
 295 void
 296 m_checkalignment(const char* where, struct mbuf *m0, int off, int len)
 297 {
 298         int roff;
 299         struct mbuf *m = m_getptr(m0, off, &roff);
 300         caddr_t addr;
 301
 302         if (m == NULL)
 303                 return;
 304         printf("%s (off %u len %u): ", where, off, len);
 305         addr = mtod(m, caddr_t) + roff;
 306         do {
 307                 int mlen;
 308
 309                 if (((uintptr_t) addr) & 3) {
 310                         printf("addr misaligned %p,", addr);
 311                         break;
 312                 }
 313                 mlen = m->m_len;
 314                 if (mlen > len)
 315                         mlen = len;
 316                 len -= mlen;
 317                 if (len && (mlen & 3)) {
 318                         printf("len mismatch %u,", mlen);
 319                         break;
 320                 }
 321                 m = m->m_next;
 322                 addr = m ? mtod(m, caddr_t) : NULL;
 323         } while (m && len > 0);
 324         for (m = m0; m; m = m->m_next)
 325                 printf(" [%p:%u]", mtod(m, caddr_t), m->m_len);
 326         printf("\n");
 327 }