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 #include <sys/vimage.h>
  40
  41 #include <net/route.h>
  42 #include <netinet/in.h>
  43
  44 #include <netipsec/ipsec.h>
  45
  46 /*
  47  * Make space for a new header of length hlen at skip bytes
  48  * into the packet.  When doing this we allocate new mbufs only
  49  * when absolutely necessary.  The mbuf where the new header
  50  * is to go is returned together with an offset into the mbuf.
  51  * If NULL is returned then the mbuf chain may have been modified;
  52  * the caller is assumed to always free the chain.
  53  */
  54 struct mbuf *
  55 m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
  56 {
  57         INIT_VNET_IPSEC(curvnet);
  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 makese 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 *n;
  79
  80                 /* XXX code doesn't handle clusters XXX */
  81                 IPSEC_ASSERT(remain < MLEN, ("remainder too big: %u", remain));
  82                 /*
  83                  * Not enough space in m, split the contents
  84                  * of m, inserting new mbufs as required.
  85                  *
  86                  * NB: this ignores mbuf types.
  87                  */
  88                 MGET(n, M_DONTWAIT, MT_DATA);
  89                 if (n == NULL)
  90                         return (NULL);
  91                 n->m_next = m->m_next;          /* splice new mbuf */
  92                 m->m_next = n;
  93                 V_ipsec4stat.ips_mbinserted++;
  94                 if (hlen <= M_TRAILINGSPACE(m) + remain) {
  95                         /*
  96                          * New header fits in the old mbuf if we copy
  97                          * the remainder; just do the copy to the new
  98                          * mbuf and we're good to go.
  99                          */
 100                         memcpy(mtod(n, caddr_t),
 101                                mtod(m, caddr_t) + skip, remain);
 102                         n->m_len = remain;
 103                         m->m_len = skip + hlen;
 104                         *off = skip;
 105                 } else {
 106                         /*
 107                          * No space in the old mbuf for the new header.
 108                          * Make space in the new mbuf and check the
 109                          * remainder'd data fits too.  If not then we
 110                          * must allocate an additional mbuf (yech).
 111                          */
 112                         n->m_len = 0;
 113                         if (remain + hlen > M_TRAILINGSPACE(n)) {
 114                                 struct mbuf *n2;
 115
 116                                 MGET(n2, M_DONTWAIT, MT_DATA);
 117                                 /* NB: new mbuf is on chain, let caller free */
 118                                 if (n2 == NULL)
 119                                         return (NULL);
 120                                 n2->m_len = 0;
 121                                 memcpy(mtod(n2, caddr_t),
 122                                        mtod(m, caddr_t) + skip, remain);
 123                                 n2->m_len = remain;
 124                                 /* splice in second mbuf */
 125                                 n2->m_next = n->m_next;
 126                                 n->m_next = n2;
 127                                 V_ipsec4stat.ips_mbinserted++;
 128                         } else {
 129                                 memcpy(mtod(n, caddr_t) + hlen,
 130                                        mtod(m, caddr_t) + skip, remain);
 131                                 n->m_len += remain;
 132                         }
 133                         m->m_len -= remain;
 134                         n->m_len += hlen;
 135                         m = n;                  /* header is at front ... */
 136                         *off = 0;               /* ... of new mbuf */
 137                 }
 138         } else {
 139                 /*
 140                  * Copy the remainder to the back of the mbuf
 141                  * so there's space to write the new header.
 142                  */
 143                 bcopy(mtod(m, caddr_t) + skip,
 144                       mtod(m, caddr_t) + skip + hlen, remain);
 145                 m->m_len += hlen;
 146                 *off = skip;
 147         }
 148         m0->m_pkthdr.len += hlen;               /* adjust packet length */
 149         return m;
 150 }
 151
 152 /*
 153  * m_pad(m, n) pads <m> with <n> bytes at the end. The packet header
 154  * length is updated, and a pointer to the first byte of the padding
 155  * (which is guaranteed to be all in one mbuf) is returned.
 156  */
 157 caddr_t
 158 m_pad(struct mbuf *m, int n)
 159 {
 160         INIT_VNET_IPSEC(curvnet);
 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_DONTWAIT, 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         INIT_VNET_IPSEC(curvnet);
 234         struct mbuf *m1;
 235         int roff;
 236
 237         /* Find beginning of header */
 238         m1 = m_getptr(m, skip, &roff);
 239         if (m1 == NULL)
 240                 return (EINVAL);
 241
 242         /* Remove the header and associated data from the mbuf. */
 243         if (roff == 0) {
 244                 /* The header was at the beginning of the mbuf */
 245                 V_ipsec4stat.ips_input_front++;
 246                 m_adj(m1, hlen);
 247                 if ((m1->m_flags & M_PKTHDR) == 0)
 248                         m->m_pkthdr.len -= hlen;
 249         } else if (roff + hlen >= m1->m_len) {
 250                 struct mbuf *mo;
 251
 252                 /*
 253                  * Part or all of the header is at the end of this mbuf,
 254                  * so first let's remove the remainder of the header from
 255                  * the beginning of the remainder of the mbuf chain, if any.
 256                  */
 257                 V_ipsec4stat.ips_input_end++;
 258                 if (roff + hlen > m1->m_len) {
 259                         /* Adjust the next mbuf by the remainder */
 260                         m_adj(m1->m_next, roff + hlen - m1->m_len);
 261
 262                         /* The second mbuf is guaranteed not to have a pkthdr... */
 263                         m->m_pkthdr.len -= (roff + hlen - m1->m_len);
 264                 }
 265
 266                 /* Now, let's unlink the mbuf chain for a second...*/
 267                 mo = m1->m_next;
 268                 m1->m_next = NULL;
 269
 270                 /* ...and trim the end of the first part of the chain...sick */
 271                 m_adj(m1, -(m1->m_len - roff));
 272                 if ((m1->m_flags & M_PKTHDR) == 0)
 273                         m->m_pkthdr.len -= (m1->m_len - roff);
 274
 275                 /* Finally, let's relink */
 276                 m1->m_next = mo;
 277         } else {
 278                 /*
 279                  * The header lies in the "middle" of the mbuf; copy
 280                  * the remainder of the mbuf down over the header.
 281                  */
 282                 V_ipsec4stat.ips_input_middle++;
 283                 bcopy(mtod(m1, u_char *) + roff + hlen,
 284                       mtod(m1, u_char *) + roff,
 285                       m1->m_len - (roff + hlen));
 286                 m1->m_len -= hlen;
 287                 m->m_pkthdr.len -= hlen;
 288         }
 289         return (0);
 290 }
 291
 292 /*
 293  * Diagnostic routine to check mbuf alignment as required by the
 294  * crypto device drivers (that use DMA).
 295  */
 296 void
 297 m_checkalignment(const char* where, struct mbuf *m0, int off, int len)
 298 {
 299         int roff;
 300         struct mbuf *m = m_getptr(m0, off, &roff);
 301         caddr_t addr;
 302
 303         if (m == NULL)
 304                 return;
 305         printf("%s (off %u len %u): ", where, off, len);
 306         addr = mtod(m, caddr_t) + roff;
 307         do {
 308                 int mlen;
 309
 310                 if (((uintptr_t) addr) & 3) {
 311                         printf("addr misaligned %p,", addr);
 312                         break;
 313                 }
 314                 mlen = m->m_len;
 315                 if (mlen > len)
 316                         mlen = len;
 317                 len -= mlen;
 318                 if (len && (mlen & 3)) {
 319                         printf("len mismatch %u,", mlen);
 320                         break;
 321                 }
 322                 m = m->m_next;
 323                 addr = m ? mtod(m, caddr_t) : NULL;
 324         } while (m && len > 0);
 325         for (m = m0; m; m = m->m_next)
 326                 printf(" [%p:%u]", mtod(m, caddr_t), m->m_len);
 327         printf("\n");
 328 }