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 *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_DONTWAIT, m->m_type, 0);
  89                                 len = MCLBYTES;
  90                         }
  91                         else {
  92                                 n = m_get(M_DONTWAIT, 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_DONTWAIT, 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                 V_ipsec4stat.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         INIT_VNET_IPSEC(curvnet);
 162         register struct mbuf *m0, *m1;
 163         register int len, pad;
 164         caddr_t retval;
 165
 166         if (n <= 0) {  /* No stupid arguments. */
 167                 DPRINTF(("%s: pad length invalid (%d)\n", __func__, n));
 168                 m_freem(m);
 169                 return NULL;
 170         }
 171
 172         len = m->m_pkthdr.len;
 173         pad = n;
 174         m0 = m;
 175
 176         while (m0->m_len < len) {
 177                 len -= m0->m_len;
 178                 m0 = m0->m_next;
 179         }
 180
 181         if (m0->m_len != len) {
 182                 DPRINTF(("%s: length mismatch (should be %d instead of %d)\n",
 183                         __func__, m->m_pkthdr.len,
 184                         m->m_pkthdr.len + m0->m_len - len));
 185
 186                 m_freem(m);
 187                 return NULL;
 188         }
 189
 190         /* Check for zero-length trailing mbufs, and find the last one. */
 191         for (m1 = m0; m1->m_next; m1 = m1->m_next) {
 192                 if (m1->m_next->m_len != 0) {
 193                         DPRINTF(("%s: length mismatch (should be %d instead "
 194                                 "of %d)\n", __func__,
 195                                 m->m_pkthdr.len,
 196                                 m->m_pkthdr.len + m1->m_next->m_len));
 197
 198                         m_freem(m);
 199                         return NULL;
 200                 }
 201
 202                 m0 = m1->m_next;
 203         }
 204
 205         if (pad > M_TRAILINGSPACE(m0)) {
 206                 /* Add an mbuf to the chain. */
 207                 MGET(m1, M_DONTWAIT, MT_DATA);
 208                 if (m1 == 0) {
 209                         m_freem(m0);
 210                         DPRINTF(("%s: unable to get extra mbuf\n", __func__));
 211                         return NULL;
 212                 }
 213
 214                 m0->m_next = m1;
 215                 m0 = m1;
 216                 m0->m_len = 0;
 217         }
 218
 219         retval = m0->m_data + m0->m_len;
 220         m0->m_len += pad;
 221         m->m_pkthdr.len += pad;
 222
 223         return retval;
 224 }
 225
 226 /*
 227  * Remove hlen data at offset skip in the packet.  This is used by
 228  * the protocols strip protocol headers and associated data (e.g. IV,
 229  * authenticator) on input.
 230  */
 231 int
 232 m_striphdr(struct mbuf *m, int skip, int hlen)
 233 {
 234         INIT_VNET_IPSEC(curvnet);
 235         struct mbuf *m1;
 236         int roff;
 237
 238         /* Find beginning of header */
 239         m1 = m_getptr(m, skip, &roff);
 240         if (m1 == NULL)
 241                 return (EINVAL);
 242
 243         /* Remove the header and associated data from the mbuf. */
 244         if (roff == 0) {
 245                 /* The header was at the beginning of the mbuf */
 246                 V_ipsec4stat.ips_input_front++;
 247                 m_adj(m1, hlen);
 248                 if ((m1->m_flags & M_PKTHDR) == 0)
 249                         m->m_pkthdr.len -= hlen;
 250         } else if (roff + hlen >= m1->m_len) {
 251                 struct mbuf *mo;
 252
 253                 /*
 254                  * Part or all of the header is at the end of this mbuf,
 255                  * so first let's remove the remainder of the header from
 256                  * the beginning of the remainder of the mbuf chain, if any.
 257                  */
 258                 V_ipsec4stat.ips_input_end++;
 259                 if (roff + hlen > m1->m_len) {
 260                         /* Adjust the next mbuf by the remainder */
 261                         m_adj(m1->m_next, roff + hlen - m1->m_len);
 262
 263                         /* The second mbuf is guaranteed not to have a pkthdr... */
 264                         m->m_pkthdr.len -= (roff + hlen - m1->m_len);
 265                 }
 266
 267                 /* Now, let's unlink the mbuf chain for a second...*/
 268                 mo = m1->m_next;
 269                 m1->m_next = NULL;
 270
 271                 /* ...and trim the end of the first part of the chain...sick */
 272                 m_adj(m1, -(m1->m_len - roff));
 273                 if ((m1->m_flags & M_PKTHDR) == 0)
 274                         m->m_pkthdr.len -= (m1->m_len - roff);
 275
 276                 /* Finally, let's relink */
 277                 m1->m_next = mo;
 278         } else {
 279                 /*
 280                  * The header lies in the "middle" of the mbuf; copy
 281                  * the remainder of the mbuf down over the header.
 282                  */
 283                 V_ipsec4stat.ips_input_middle++;
 284                 bcopy(mtod(m1, u_char *) + roff + hlen,
 285                       mtod(m1, u_char *) + roff,
 286                       m1->m_len - (roff + hlen));
 287                 m1->m_len -= hlen;
 288                 m->m_pkthdr.len -= hlen;
 289         }
 290         return (0);
 291 }
 292
 293 /*
 294  * Diagnostic routine to check mbuf alignment as required by the
 295  * crypto device drivers (that use DMA).
 296  */
 297 void
 298 m_checkalignment(const char* where, struct mbuf *m0, int off, int len)
 299 {
 300         int roff;
 301         struct mbuf *m = m_getptr(m0, off, &roff);
 302         caddr_t addr;
 303
 304         if (m == NULL)
 305                 return;
 306         printf("%s (off %u len %u): ", where, off, len);
 307         addr = mtod(m, caddr_t) + roff;
 308         do {
 309                 int mlen;
 310
 311                 if (((uintptr_t) addr) & 3) {
 312                         printf("addr misaligned %p,", addr);
 313                         break;
 314                 }
 315                 mlen = m->m_len;
 316                 if (mlen > len)
 317                         mlen = len;
 318                 len -= mlen;
 319                 if (len && (mlen & 3)) {
 320                         printf("len mismatch %u,", mlen);
 321                         break;
 322                 }
 323                 m = m->m_next;
 324                 addr = m ? mtod(m, caddr_t) : NULL;
 325         } while (m && len > 0);
 326         for (m = m0; m; m = m->m_next)
 327                 printf(" [%p:%u]", mtod(m, caddr_t), m->m_len);
 328         printf("\n");
 329 }