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/malloc.h> 38 #include <sys/mbuf.h> 39 #include <sys/socket.h> 40 41 #include <net/vnet.h> 42 #include <netinet/in.h> 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 makes 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_NOWAIT, m->m_type, 0); 87 len = MCLBYTES; 88 } 89 else { 90 n = m_get(M_NOWAIT, 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_NOWAIT, 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 IPSECSTAT_INC(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_NOWAIT, MT_DATA); 205 if (m1 == NULL) { 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 IPSECSTAT_INC(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 IPSECSTAT_INC(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 IPSECSTAT_INC(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 } 326