sys/netipsec/ipsec_mbuf.c

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