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 <netinet/in.h>
  42
  43 #include <netipsec/ipsec.h>
  44
  45 /*
  46  * Make space for a new header of length hlen at skip bytes
  47  * into the packet.  When doing this we allocate new mbufs only
  48  * when absolutely necessary.  The mbuf where the new header
  49  * is to go is returned together with an offset into the mbuf.
  50  * If NULL is returned then the mbuf chain may have been modified;
  51  * the caller is assumed to always free the chain.
  52  */
  53 struct mbuf *
  54 m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
  55 {
  56         struct mbuf *m;
  57         unsigned remain;
  58
  59         IPSEC_ASSERT(m0 != NULL, ("null mbuf"));
  60         IPSEC_ASSERT(hlen < MHLEN, ("hlen too big: %u", hlen));
  61
  62         for (m = m0; m && skip > m->m_len; m = m->m_next)
  63                 skip -= m->m_len;
  64         if (m == NULL)
  65                 return (NULL);
  66         /*
  67          * At this point skip is the offset into the mbuf m
  68          * where the new header should be placed.  Figure out
  69          * if there's space to insert the new header.  If so,
  70          * and copying the remainder makese sense then do so.
  71          * Otherwise insert a new mbuf in the chain, splitting
  72          * the contents of m as needed.
  73          */
  74         remain = m->m_len - skip;               /* data to move */
  75         if (hlen > M_TRAILINGSPACE(m)) {
  76                 struct mbuf *n0, *n, **np;
  77                 int todo, len, done, alloc;
  78
  79                 n0 = NULL;
  80                 np = &n0;
  81                 alloc = 0;
  82                 done = 0;
  83                 todo = remain;
  84                 while (todo > 0) {
  85                         if (todo > MHLEN) {
  86                                 n = m_getcl(M_DONTWAIT, m->m_type, 0);
  87                                 len = MCLBYTES;
  88                         }
  89                         else {
  90                                 n = m_get(M_DONTWAIT, m->m_type);
  91                                 len = MHLEN;
  92                         }
  93                         if (n == NULL) {
  94                                 m_freem(n0);
  95                                 return NULL;
  96                         }
  97                         *np = n;
  98                         np = &n->m_next;
  99                         alloc++;
 100                         len = min(todo, len);
 101                         memcpy(n->m_data, mtod(m, char *) + skip + done, len);
 102                         n->m_len = len;
 103                         done += len;
 104                         todo -= len;
 105                 }
 106
 107                 if (hlen <= M_TRAILINGSPACE(m) + remain) {
 108                         m->m_len = skip + hlen;
 109                         *off = skip;
 110                         if (n0 != NULL) {
 111                                 *np = m->m_next;
 112                                 m->m_next = n0;
 113                         }
 114                 }
 115                 else {
 116                         n = m_get(M_DONTWAIT, m->m_type);
 117                         if (n == NULL) {
 118                                 m_freem(n0);
 119                                 return NULL;
 120                         }
 121                         alloc++;
 122
 123                         if ((n->m_next = n0) == NULL)
 124                                 np = &n->m_next;
 125                         n0 = n;
 126
 127                         *np = m->m_next;
 128                         m->m_next = n0;
 129
 130                         n->m_len = hlen;
 131                         m->m_len = skip;
 132
 133                         m = n;                  /* header is at front ... */
 134                         *off = 0;               /* ... of new mbuf */
 135                 }
 136                 ipsec4stat.ips_mbinserted++;
 137         } else {
 138                 /*
 139                  * Copy the remainder to the back of the mbuf
 140                  * so there's space to write the new header.
 141                  */
 142                 bcopy(mtod(m, caddr_t) + skip,
 143                       mtod(m, caddr_t) + skip + hlen, remain);
 144                 m->m_len += hlen;
 145                 *off = skip;
 146         }
 147         m0->m_pkthdr.len += hlen;               /* adjust packet length */
 148         return m;
 149 }
 150
 151 /*
 152  * m_pad(m, n) pads <m> with <n> bytes at the end. The packet header
 153  * length is updated, and a pointer to the first byte of the padding
 154  * (which is guaranteed to be all in one mbuf) is returned.
 155  */
 156 caddr_t
 157 m_pad(struct mbuf *m, int n)
 158 {
 159         register struct mbuf *m0, *m1;
 160         register int len, pad;
 161         caddr_t retval;
 162
 163         if (n <= 0) {  /* No stupid arguments. */
 164                 DPRINTF(("%s: pad length invalid (%d)\n", __func__, n));
 165                 m_freem(m);
 166                 return NULL;
 167         }
 168
 169         len = m->m_pkthdr.len;
 170         pad = n;
 171         m0 = m;
 172
 173         while (m0->m_len < len) {
 174                 len -= m0->m_len;
 175                 m0 = m0->m_next;
 176         }
 177
 178         if (m0->m_len != len) {
 179                 DPRINTF(("%s: length mismatch (should be %d instead of %d)\n",
 180                         __func__, m->m_pkthdr.len,
 181                         m->m_pkthdr.len + m0->m_len - len));
 182
 183                 m_freem(m);
 184                 return NULL;
 185         }
 186
 187         /* Check for zero-length trailing mbufs, and find the last one. */
 188         for (m1 = m0; m1->m_next; m1 = m1->m_next) {
 189                 if (m1->m_next->m_len != 0) {
 190                         DPRINTF(("%s: length mismatch (should be %d instead "
 191                                 "of %d)\n", __func__,
 192                                 m->m_pkthdr.len,
 193                                 m->m_pkthdr.len + m1->m_next->m_len));
 194
 195                         m_freem(m);
 196                         return NULL;
 197                 }
 198
 199                 m0 = m1->m_next;
 200         }
 201
 202         if (pad > M_TRAILINGSPACE(m0)) {
 203                 /* Add an mbuf to the chain. */
 204                 MGET(m1, M_DONTWAIT, MT_DATA);
 205                 if (m1 == 0) {
 206                         m_freem(m0);
 207                         DPRINTF(("%s: unable to get extra mbuf\n", __func__));
 208                         return NULL;
 209                 }
 210
 211                 m0->m_next = m1;
 212                 m0 = m1;
 213                 m0->m_len = 0;
 214         }
 215
 216         retval = m0->m_data + m0->m_len;
 217         m0->m_len += pad;
 218         m->m_pkthdr.len += pad;
 219
 220         return retval;
 221 }
 222
 223 /*
 224  * Remove hlen data at offset skip in the packet.  This is used by
 225  * the protocols strip protocol headers and associated data (e.g. IV,
 226  * authenticator) on input.
 227  */
 228 int
 229 m_striphdr(struct mbuf *m, int skip, int hlen)
 230 {
 231         struct mbuf *m1;
 232         int roff;
 233
 234         /* Find beginning of header */
 235         m1 = m_getptr(m, skip, &roff);
 236         if (m1 == NULL)
 237                 return (EINVAL);
 238
 239         /* Remove the header and associated data from the mbuf. */
 240         if (roff == 0) {
 241                 /* The header was at the beginning of the mbuf */
 242                 ipsec4stat.ips_input_front++;
 243                 m_adj(m1, hlen);
 244                 if ((m1->m_flags & M_PKTHDR) == 0)
 245                         m->m_pkthdr.len -= hlen;
 246         } else if (roff + hlen >= m1->m_len) {
 247                 struct mbuf *mo;
 248
 249                 /*
 250                  * Part or all of the header is at the end of this mbuf,
 251                  * so first let's remove the remainder of the header from
 252                  * the beginning of the remainder of the mbuf chain, if any.
 253                  */
 254                 ipsec4stat.ips_input_end++;
 255                 if (roff + hlen > m1->m_len) {
 256                         /* Adjust the next mbuf by the remainder */
 257                         m_adj(m1->m_next, roff + hlen - m1->m_len);
 258
 259                         /* The second mbuf is guaranteed not to have a pkthdr... */
 260                         m->m_pkthdr.len -= (roff + hlen - m1->m_len);
 261                 }
 262
 263                 /* Now, let's unlink the mbuf chain for a second...*/
 264                 mo = m1->m_next;
 265                 m1->m_next = NULL;
 266
 267                 /* ...and trim the end of the first part of the chain...sick */
 268                 m_adj(m1, -(m1->m_len - roff));
 269                 if ((m1->m_flags & M_PKTHDR) == 0)
 270                         m->m_pkthdr.len -= (m1->m_len - roff);
 271
 272                 /* Finally, let's relink */
 273                 m1->m_next = mo;
 274         } else {
 275                 /*
 276                  * The header lies in the "middle" of the mbuf; copy
 277                  * the remainder of the mbuf down over the header.
 278                  */
 279                 ipsec4stat.ips_input_middle++;
 280                 bcopy(mtod(m1, u_char *) + roff + hlen,
 281                       mtod(m1, u_char *) + roff,
 282                       m1->m_len - (roff + hlen));
 283                 m1->m_len -= hlen;
 284                 m->m_pkthdr.len -= hlen;
 285         }
 286         return (0);
 287 }
 288
 289 /*
 290  * Diagnostic routine to check mbuf alignment as required by the
 291  * crypto device drivers (that use DMA).
 292  */
 293 void
 294 m_checkalignment(const char* where, struct mbuf *m0, int off, int len)
 295 {
 296         int roff;
 297         struct mbuf *m = m_getptr(m0, off, &roff);
 298         caddr_t addr;
 299
 300         if (m == NULL)
 301                 return;
 302         printf("%s (off %u len %u): ", where, off, len);
 303         addr = mtod(m, caddr_t) + roff;
 304         do {
 305                 int mlen;
 306
 307                 if (((uintptr_t) addr) & 3) {
 308                         printf("addr misaligned %p,", addr);
 309                         break;
 310                 }
 311                 mlen = m->m_len;
 312                 if (mlen > len)
 313                         mlen = len;
 314                 len -= mlen;
 315                 if (len && (mlen & 3)) {
 316                         printf("len mismatch %u,", mlen);
 317                         break;
 318                 }
 319                 m = m->m_next;
 320                 addr = m ? mtod(m, caddr_t) : NULL;
 321         } while (m && len > 0);
 322         for (m = m0; m; m = m->m_next)
 323                 printf(" [%p:%u]", mtod(m, caddr_t), m->m_len);
 324         printf("\n");
 325 }