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 struct mbuf *m; 58 unsigned remain; 59 60 IPSEC_ASSERT(m0 != NULL, ("null mbuf")); 61 IPSEC_ASSERT(hlen < MHLEN, ("hlen too big: %u", hlen)); 62 63 for (m = m0; m && skip > m->m_len; m = m->m_next) 64 skip -= m->m_len; 65 if (m == NULL) 66 return (NULL); 67 /* 68 * At this point skip is the offset into the mbuf m 69 * where the new header should be placed. Figure out 70 * if there's space to insert the new header. If so, 71 * and copying the remainder makese sense then do so. 72 * Otherwise insert a new mbuf in the chain, splitting 73 * the contents of m as needed. 74 */ 75 remain = m->m_len - skip; /* data to move */ 76 if (hlen > M_TRAILINGSPACE(m)) { 77 struct mbuf *n; 78 79 /* XXX code doesn't handle clusters XXX */ 80 IPSEC_ASSERT(remain < MLEN, ("remainder too big: %u", remain)); 81 /* 82 * Not enough space in m, split the contents 83 * of m, inserting new mbufs as required. 84 * 85 * NB: this ignores mbuf types. 86 */ 87 MGET(n, M_DONTWAIT, MT_DATA); 88 if (n == NULL) 89 return (NULL); 90 n->m_next = m->m_next; /* splice new mbuf */ 91 m->m_next = n; 92 V_ipsec4stat.ips_mbinserted++; 93 if (hlen <= M_TRAILINGSPACE(m) + remain) { 94 /* 95 * New header fits in the old mbuf if we copy 96 * the remainder; just do the copy to the new 97 * mbuf and we're good to go. 98 */ 99 memcpy(mtod(n, caddr_t), 100 mtod(m, caddr_t) + skip, remain); 101 n->m_len = remain; 102 m->m_len = skip + hlen; 103 *off = skip; 104 } else { 105 /* 106 * No space in the old mbuf for the new header. 107 * Make space in the new mbuf and check the 108 * remainder'd data fits too. If not then we 109 * must allocate an additional mbuf (yech). 110 */ 111 n->m_len = 0; 112 if (remain + hlen > M_TRAILINGSPACE(n)) { 113 struct mbuf *n2; 114 115 MGET(n2, M_DONTWAIT, MT_DATA); 116 /* NB: new mbuf is on chain, let caller free */ 117 if (n2 == NULL) 118 return (NULL); 119 n2->m_len = 0; 120 memcpy(mtod(n2, caddr_t), 121 mtod(m, caddr_t) + skip, remain); 122 n2->m_len = remain; 123 /* splice in second mbuf */ 124 n2->m_next = n->m_next; 125 n->m_next = n2; 126 V_ipsec4stat.ips_mbinserted++; 127 } else { 128 memcpy(mtod(n, caddr_t) + hlen, 129 mtod(m, caddr_t) + skip, remain); 130 n->m_len += remain; 131 } 132 m->m_len -= remain; 133 n->m_len += hlen; 134 m = n; /* header is at front ... */ 135 *off = 0; /* ... of new mbuf */ 136 } 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 V_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 V_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 V_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 } 326