1112758Ssam/*- 2112758Ssam * Copyright (c) 2002, 2003 Sam Leffler, Errno Consulting 3112758Ssam * All rights reserved. 4112758Ssam * 5112758Ssam * Redistribution and use in source and binary forms, with or without 6112758Ssam * modification, are permitted provided that the following conditions 7112758Ssam * are met: 8112758Ssam * 1. Redistributions of source code must retain the above copyright 9112758Ssam * notice, this list of conditions and the following disclaimer. 10112758Ssam * 2. Redistributions in binary form must reproduce the above copyright 11112758Ssam * notice, this list of conditions and the following disclaimer in the 12112758Ssam * documentation and/or other materials provided with the distribution. 13112758Ssam * 14112758Ssam * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15112758Ssam * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16112758Ssam * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17112758Ssam * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18112758Ssam * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19112758Ssam * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20112758Ssam * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21112758Ssam * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22112758Ssam * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23112758Ssam * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24112758Ssam * SUCH DAMAGE. 25112758Ssam * 26112758Ssam * $FreeBSD$ 27112758Ssam */ 28105197Ssam 29105197Ssam/* 30105197Ssam * IPsec-specific mbuf routines. 31105197Ssam */ 32105197Ssam 33105197Ssam#include "opt_param.h" 34105197Ssam 35105197Ssam#include <sys/param.h> 36105197Ssam#include <sys/systm.h> 37105197Ssam#include <sys/mbuf.h> 38105197Ssam#include <sys/socket.h> 39105197Ssam 40105197Ssam#include <net/route.h> 41195699Srwatson#include <net/vnet.h> 42195699Srwatson 43105197Ssam#include <netinet/in.h> 44105197Ssam 45105197Ssam#include <netipsec/ipsec.h> 46105197Ssam 47105197Ssam/* 48108989Ssam * Make space for a new header of length hlen at skip bytes 49108989Ssam * into the packet. When doing this we allocate new mbufs only 50105197Ssam * when absolutely necessary. The mbuf where the new header 51105197Ssam * is to go is returned together with an offset into the mbuf. 52105197Ssam * If NULL is returned then the mbuf chain may have been modified; 53105197Ssam * the caller is assumed to always free the chain. 54105197Ssam */ 55105197Ssamstruct mbuf * 56105197Ssamm_makespace(struct mbuf *m0, int skip, int hlen, int *off) 57105197Ssam{ 58105197Ssam struct mbuf *m; 59105197Ssam unsigned remain; 60105197Ssam 61120585Ssam IPSEC_ASSERT(m0 != NULL, ("null mbuf")); 62120585Ssam IPSEC_ASSERT(hlen < MHLEN, ("hlen too big: %u", hlen)); 63105197Ssam 64105197Ssam for (m = m0; m && skip > m->m_len; m = m->m_next) 65105197Ssam skip -= m->m_len; 66105197Ssam if (m == NULL) 67105197Ssam return (NULL); 68105197Ssam /* 69105197Ssam * At this point skip is the offset into the mbuf m 70105197Ssam * where the new header should be placed. Figure out 71105197Ssam * if there's space to insert the new header. If so, 72199894Sbz * and copying the remainder makes sense then do so. 73105197Ssam * Otherwise insert a new mbuf in the chain, splitting 74105197Ssam * the contents of m as needed. 75105197Ssam */ 76105197Ssam remain = m->m_len - skip; /* data to move */ 77105197Ssam if (hlen > M_TRAILINGSPACE(m)) { 78187815Svanhu struct mbuf *n0, *n, **np; 79187815Svanhu int todo, len, done, alloc; 80105197Ssam 81187815Svanhu n0 = NULL; 82187815Svanhu np = &n0; 83187815Svanhu alloc = 0; 84187815Svanhu done = 0; 85187815Svanhu todo = remain; 86187815Svanhu while (todo > 0) { 87187815Svanhu if (todo > MHLEN) { 88187815Svanhu n = m_getcl(M_DONTWAIT, m->m_type, 0); 89187815Svanhu len = MCLBYTES; 90187815Svanhu } 91187815Svanhu else { 92187815Svanhu n = m_get(M_DONTWAIT, m->m_type); 93187815Svanhu len = MHLEN; 94187815Svanhu } 95187815Svanhu if (n == NULL) { 96187815Svanhu m_freem(n0); 97187815Svanhu return NULL; 98187815Svanhu } 99187815Svanhu *np = n; 100187815Svanhu np = &n->m_next; 101187815Svanhu alloc++; 102187815Svanhu len = min(todo, len); 103187815Svanhu memcpy(n->m_data, mtod(m, char *) + skip + done, len); 104187815Svanhu n->m_len = len; 105187815Svanhu done += len; 106187815Svanhu todo -= len; 107187815Svanhu } 108187815Svanhu 109105197Ssam if (hlen <= M_TRAILINGSPACE(m) + remain) { 110105197Ssam m->m_len = skip + hlen; 111105197Ssam *off = skip; 112187815Svanhu if (n0 != NULL) { 113187815Svanhu *np = m->m_next; 114187815Svanhu m->m_next = n0; 115187815Svanhu } 116187815Svanhu } 117187815Svanhu else { 118187815Svanhu n = m_get(M_DONTWAIT, m->m_type); 119187815Svanhu if (n == NULL) { 120187815Svanhu m_freem(n0); 121187815Svanhu return NULL; 122187815Svanhu } 123187815Svanhu alloc++; 124105197Ssam 125187815Svanhu if ((n->m_next = n0) == NULL) 126187815Svanhu np = &n->m_next; 127187815Svanhu n0 = n; 128187815Svanhu 129187815Svanhu *np = m->m_next; 130187815Svanhu m->m_next = n0; 131187815Svanhu 132187815Svanhu n->m_len = hlen; 133187815Svanhu m->m_len = skip; 134187815Svanhu 135105197Ssam m = n; /* header is at front ... */ 136105197Ssam *off = 0; /* ... of new mbuf */ 137105197Ssam } 138252692Sae IPSECSTAT_INC(ips_mbinserted); 139105197Ssam } else { 140105197Ssam /* 141105197Ssam * Copy the remainder to the back of the mbuf 142105197Ssam * so there's space to write the new header. 143105197Ssam */ 144113075Sdes bcopy(mtod(m, caddr_t) + skip, 145113075Sdes mtod(m, caddr_t) + skip + hlen, remain); 146105197Ssam m->m_len += hlen; 147105197Ssam *off = skip; 148105197Ssam } 149105197Ssam m0->m_pkthdr.len += hlen; /* adjust packet length */ 150105197Ssam return m; 151105197Ssam} 152105197Ssam 153105197Ssam/* 154105197Ssam * m_pad(m, n) pads <m> with <n> bytes at the end. The packet header 155105197Ssam * length is updated, and a pointer to the first byte of the padding 156105197Ssam * (which is guaranteed to be all in one mbuf) is returned. 157105197Ssam */ 158105197Ssamcaddr_t 159105197Ssamm_pad(struct mbuf *m, int n) 160105197Ssam{ 161105197Ssam register struct mbuf *m0, *m1; 162105197Ssam register int len, pad; 163105197Ssam caddr_t retval; 164105197Ssam 165105197Ssam if (n <= 0) { /* No stupid arguments. */ 166120585Ssam DPRINTF(("%s: pad length invalid (%d)\n", __func__, n)); 167105197Ssam m_freem(m); 168105197Ssam return NULL; 169105197Ssam } 170105197Ssam 171105197Ssam len = m->m_pkthdr.len; 172105197Ssam pad = n; 173105197Ssam m0 = m; 174105197Ssam 175105197Ssam while (m0->m_len < len) { 176105197Ssam len -= m0->m_len; 177105197Ssam m0 = m0->m_next; 178105197Ssam } 179105197Ssam 180105197Ssam if (m0->m_len != len) { 181120585Ssam DPRINTF(("%s: length mismatch (should be %d instead of %d)\n", 182120585Ssam __func__, m->m_pkthdr.len, 183120585Ssam m->m_pkthdr.len + m0->m_len - len)); 184105197Ssam 185105197Ssam m_freem(m); 186105197Ssam return NULL; 187105197Ssam } 188105197Ssam 189105197Ssam /* Check for zero-length trailing mbufs, and find the last one. */ 190105197Ssam for (m1 = m0; m1->m_next; m1 = m1->m_next) { 191105197Ssam if (m1->m_next->m_len != 0) { 192120585Ssam DPRINTF(("%s: length mismatch (should be %d instead " 193120585Ssam "of %d)\n", __func__, 194120585Ssam m->m_pkthdr.len, 195120585Ssam m->m_pkthdr.len + m1->m_next->m_len)); 196105197Ssam 197105197Ssam m_freem(m); 198105197Ssam return NULL; 199105197Ssam } 200105197Ssam 201105197Ssam m0 = m1->m_next; 202105197Ssam } 203105197Ssam 204105197Ssam if (pad > M_TRAILINGSPACE(m0)) { 205105197Ssam /* Add an mbuf to the chain. */ 206111119Simp MGET(m1, M_DONTWAIT, MT_DATA); 207105197Ssam if (m1 == 0) { 208105197Ssam m_freem(m0); 209120585Ssam DPRINTF(("%s: unable to get extra mbuf\n", __func__)); 210105197Ssam return NULL; 211105197Ssam } 212105197Ssam 213105197Ssam m0->m_next = m1; 214105197Ssam m0 = m1; 215105197Ssam m0->m_len = 0; 216105197Ssam } 217105197Ssam 218105197Ssam retval = m0->m_data + m0->m_len; 219105197Ssam m0->m_len += pad; 220105197Ssam m->m_pkthdr.len += pad; 221105197Ssam 222105197Ssam return retval; 223105197Ssam} 224105197Ssam 225105197Ssam/* 226105197Ssam * Remove hlen data at offset skip in the packet. This is used by 227105197Ssam * the protocols strip protocol headers and associated data (e.g. IV, 228105197Ssam * authenticator) on input. 229105197Ssam */ 230105197Ssamint 231105197Ssamm_striphdr(struct mbuf *m, int skip, int hlen) 232105197Ssam{ 233105197Ssam struct mbuf *m1; 234105197Ssam int roff; 235105197Ssam 236105197Ssam /* Find beginning of header */ 237105197Ssam m1 = m_getptr(m, skip, &roff); 238105197Ssam if (m1 == NULL) 239105197Ssam return (EINVAL); 240105197Ssam 241105197Ssam /* Remove the header and associated data from the mbuf. */ 242105197Ssam if (roff == 0) { 243105197Ssam /* The header was at the beginning of the mbuf */ 244252692Sae IPSECSTAT_INC(ips_input_front); 245105197Ssam m_adj(m1, hlen); 246105197Ssam if ((m1->m_flags & M_PKTHDR) == 0) 247105197Ssam m->m_pkthdr.len -= hlen; 248105197Ssam } else if (roff + hlen >= m1->m_len) { 249105197Ssam struct mbuf *mo; 250105197Ssam 251105197Ssam /* 252105197Ssam * Part or all of the header is at the end of this mbuf, 253105197Ssam * so first let's remove the remainder of the header from 254105197Ssam * the beginning of the remainder of the mbuf chain, if any. 255105197Ssam */ 256252692Sae IPSECSTAT_INC(ips_input_end); 257105197Ssam if (roff + hlen > m1->m_len) { 258105197Ssam /* Adjust the next mbuf by the remainder */ 259105197Ssam m_adj(m1->m_next, roff + hlen - m1->m_len); 260105197Ssam 261105197Ssam /* The second mbuf is guaranteed not to have a pkthdr... */ 262105197Ssam m->m_pkthdr.len -= (roff + hlen - m1->m_len); 263105197Ssam } 264105197Ssam 265105197Ssam /* Now, let's unlink the mbuf chain for a second...*/ 266105197Ssam mo = m1->m_next; 267105197Ssam m1->m_next = NULL; 268105197Ssam 269105197Ssam /* ...and trim the end of the first part of the chain...sick */ 270105197Ssam m_adj(m1, -(m1->m_len - roff)); 271105197Ssam if ((m1->m_flags & M_PKTHDR) == 0) 272105197Ssam m->m_pkthdr.len -= (m1->m_len - roff); 273105197Ssam 274105197Ssam /* Finally, let's relink */ 275105197Ssam m1->m_next = mo; 276105197Ssam } else { 277105197Ssam /* 278105197Ssam * The header lies in the "middle" of the mbuf; copy 279105197Ssam * the remainder of the mbuf down over the header. 280105197Ssam */ 281252692Sae IPSECSTAT_INC(ips_input_middle); 282105197Ssam bcopy(mtod(m1, u_char *) + roff + hlen, 283105197Ssam mtod(m1, u_char *) + roff, 284105197Ssam m1->m_len - (roff + hlen)); 285105197Ssam m1->m_len -= hlen; 286105197Ssam m->m_pkthdr.len -= hlen; 287105197Ssam } 288105197Ssam return (0); 289105197Ssam} 290105197Ssam 291105197Ssam/* 292105197Ssam * Diagnostic routine to check mbuf alignment as required by the 293105197Ssam * crypto device drivers (that use DMA). 294105197Ssam */ 295105197Ssamvoid 296105197Ssamm_checkalignment(const char* where, struct mbuf *m0, int off, int len) 297105197Ssam{ 298105197Ssam int roff; 299105197Ssam struct mbuf *m = m_getptr(m0, off, &roff); 300105197Ssam caddr_t addr; 301105197Ssam 302105197Ssam if (m == NULL) 303105197Ssam return; 304105197Ssam printf("%s (off %u len %u): ", where, off, len); 305105197Ssam addr = mtod(m, caddr_t) + roff; 306105197Ssam do { 307105197Ssam int mlen; 308105197Ssam 309105197Ssam if (((uintptr_t) addr) & 3) { 310105197Ssam printf("addr misaligned %p,", addr); 311105197Ssam break; 312105197Ssam } 313105197Ssam mlen = m->m_len; 314105197Ssam if (mlen > len) 315105197Ssam mlen = len; 316105197Ssam len -= mlen; 317105197Ssam if (len && (mlen & 3)) { 318105197Ssam printf("len mismatch %u,", mlen); 319105197Ssam break; 320105197Ssam } 321105197Ssam m = m->m_next; 322105197Ssam addr = m ? mtod(m, caddr_t) : NULL; 323105197Ssam } while (m && len > 0); 324105197Ssam for (m = m0; m; m = m->m_next) 325105197Ssam printf(" [%p:%u]", mtod(m, caddr_t), m->m_len); 326105197Ssam printf("\n"); 327105197Ssam} 328