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 40195699Srwatson#include <net/vnet.h> 41105197Ssam#include <netinet/in.h> 42105197Ssam#include <netipsec/ipsec.h> 43105197Ssam 44105197Ssam/* 45108989Ssam * Make space for a new header of length hlen at skip bytes 46108989Ssam * into the packet. When doing this we allocate new mbufs only 47105197Ssam * when absolutely necessary. The mbuf where the new header 48105197Ssam * is to go is returned together with an offset into the mbuf. 49105197Ssam * If NULL is returned then the mbuf chain may have been modified; 50105197Ssam * the caller is assumed to always free the chain. 51105197Ssam */ 52105197Ssamstruct mbuf * 53105197Ssamm_makespace(struct mbuf *m0, int skip, int hlen, int *off) 54105197Ssam{ 55105197Ssam struct mbuf *m; 56105197Ssam unsigned remain; 57105197Ssam 58120585Ssam IPSEC_ASSERT(m0 != NULL, ("null mbuf")); 59120585Ssam IPSEC_ASSERT(hlen < MHLEN, ("hlen too big: %u", hlen)); 60105197Ssam 61105197Ssam for (m = m0; m && skip > m->m_len; m = m->m_next) 62105197Ssam skip -= m->m_len; 63105197Ssam if (m == NULL) 64105197Ssam return (NULL); 65105197Ssam /* 66105197Ssam * At this point skip is the offset into the mbuf m 67105197Ssam * where the new header should be placed. Figure out 68105197Ssam * if there's space to insert the new header. If so, 69199894Sbz * and copying the remainder makes sense then do so. 70105197Ssam * Otherwise insert a new mbuf in the chain, splitting 71105197Ssam * the contents of m as needed. 72105197Ssam */ 73105197Ssam remain = m->m_len - skip; /* data to move */ 74105197Ssam if (hlen > M_TRAILINGSPACE(m)) { 75187815Svanhu struct mbuf *n0, *n, **np; 76187815Svanhu int todo, len, done, alloc; 77105197Ssam 78187815Svanhu n0 = NULL; 79187815Svanhu np = &n0; 80187815Svanhu alloc = 0; 81187815Svanhu done = 0; 82187815Svanhu todo = remain; 83187815Svanhu while (todo > 0) { 84187815Svanhu if (todo > MHLEN) { 85243882Sglebius n = m_getcl(M_NOWAIT, m->m_type, 0); 86187815Svanhu len = MCLBYTES; 87187815Svanhu } 88187815Svanhu else { 89243882Sglebius n = m_get(M_NOWAIT, m->m_type); 90187815Svanhu len = MHLEN; 91187815Svanhu } 92187815Svanhu if (n == NULL) { 93187815Svanhu m_freem(n0); 94187815Svanhu return NULL; 95187815Svanhu } 96187815Svanhu *np = n; 97187815Svanhu np = &n->m_next; 98187815Svanhu alloc++; 99187815Svanhu len = min(todo, len); 100187815Svanhu memcpy(n->m_data, mtod(m, char *) + skip + done, len); 101187815Svanhu n->m_len = len; 102187815Svanhu done += len; 103187815Svanhu todo -= len; 104187815Svanhu } 105187815Svanhu 106105197Ssam if (hlen <= M_TRAILINGSPACE(m) + remain) { 107105197Ssam m->m_len = skip + hlen; 108105197Ssam *off = skip; 109187815Svanhu if (n0 != NULL) { 110187815Svanhu *np = m->m_next; 111187815Svanhu m->m_next = n0; 112187815Svanhu } 113187815Svanhu } 114187815Svanhu else { 115243882Sglebius n = m_get(M_NOWAIT, m->m_type); 116187815Svanhu if (n == NULL) { 117187815Svanhu m_freem(n0); 118187815Svanhu return NULL; 119187815Svanhu } 120187815Svanhu alloc++; 121105197Ssam 122187815Svanhu if ((n->m_next = n0) == NULL) 123187815Svanhu np = &n->m_next; 124187815Svanhu n0 = n; 125187815Svanhu 126187815Svanhu *np = m->m_next; 127187815Svanhu m->m_next = n0; 128187815Svanhu 129187815Svanhu n->m_len = hlen; 130187815Svanhu m->m_len = skip; 131187815Svanhu 132105197Ssam m = n; /* header is at front ... */ 133105197Ssam *off = 0; /* ... of new mbuf */ 134105197Ssam } 135252026Sae IPSECSTAT_INC(ips_mbinserted); 136105197Ssam } else { 137105197Ssam /* 138105197Ssam * Copy the remainder to the back of the mbuf 139105197Ssam * so there's space to write the new header. 140105197Ssam */ 141113075Sdes bcopy(mtod(m, caddr_t) + skip, 142113075Sdes mtod(m, caddr_t) + skip + hlen, remain); 143105197Ssam m->m_len += hlen; 144105197Ssam *off = skip; 145105197Ssam } 146105197Ssam m0->m_pkthdr.len += hlen; /* adjust packet length */ 147105197Ssam return m; 148105197Ssam} 149105197Ssam 150105197Ssam/* 151105197Ssam * m_pad(m, n) pads <m> with <n> bytes at the end. The packet header 152105197Ssam * length is updated, and a pointer to the first byte of the padding 153105197Ssam * (which is guaranteed to be all in one mbuf) is returned. 154105197Ssam */ 155105197Ssamcaddr_t 156105197Ssamm_pad(struct mbuf *m, int n) 157105197Ssam{ 158105197Ssam register struct mbuf *m0, *m1; 159105197Ssam register int len, pad; 160105197Ssam caddr_t retval; 161105197Ssam 162105197Ssam if (n <= 0) { /* No stupid arguments. */ 163120585Ssam DPRINTF(("%s: pad length invalid (%d)\n", __func__, n)); 164105197Ssam m_freem(m); 165105197Ssam return NULL; 166105197Ssam } 167105197Ssam 168105197Ssam len = m->m_pkthdr.len; 169105197Ssam pad = n; 170105197Ssam m0 = m; 171105197Ssam 172105197Ssam while (m0->m_len < len) { 173105197Ssam len -= m0->m_len; 174105197Ssam m0 = m0->m_next; 175105197Ssam } 176105197Ssam 177105197Ssam if (m0->m_len != len) { 178120585Ssam DPRINTF(("%s: length mismatch (should be %d instead of %d)\n", 179120585Ssam __func__, m->m_pkthdr.len, 180120585Ssam m->m_pkthdr.len + m0->m_len - len)); 181105197Ssam 182105197Ssam m_freem(m); 183105197Ssam return NULL; 184105197Ssam } 185105197Ssam 186105197Ssam /* Check for zero-length trailing mbufs, and find the last one. */ 187105197Ssam for (m1 = m0; m1->m_next; m1 = m1->m_next) { 188105197Ssam if (m1->m_next->m_len != 0) { 189120585Ssam DPRINTF(("%s: length mismatch (should be %d instead " 190120585Ssam "of %d)\n", __func__, 191120585Ssam m->m_pkthdr.len, 192120585Ssam m->m_pkthdr.len + m1->m_next->m_len)); 193105197Ssam 194105197Ssam m_freem(m); 195105197Ssam return NULL; 196105197Ssam } 197105197Ssam 198105197Ssam m0 = m1->m_next; 199105197Ssam } 200105197Ssam 201105197Ssam if (pad > M_TRAILINGSPACE(m0)) { 202105197Ssam /* Add an mbuf to the chain. */ 203243882Sglebius MGET(m1, M_NOWAIT, MT_DATA); 204105197Ssam if (m1 == 0) { 205105197Ssam m_freem(m0); 206120585Ssam DPRINTF(("%s: unable to get extra mbuf\n", __func__)); 207105197Ssam return NULL; 208105197Ssam } 209105197Ssam 210105197Ssam m0->m_next = m1; 211105197Ssam m0 = m1; 212105197Ssam m0->m_len = 0; 213105197Ssam } 214105197Ssam 215105197Ssam retval = m0->m_data + m0->m_len; 216105197Ssam m0->m_len += pad; 217105197Ssam m->m_pkthdr.len += pad; 218105197Ssam 219105197Ssam return retval; 220105197Ssam} 221105197Ssam 222105197Ssam/* 223105197Ssam * Remove hlen data at offset skip in the packet. This is used by 224105197Ssam * the protocols strip protocol headers and associated data (e.g. IV, 225105197Ssam * authenticator) on input. 226105197Ssam */ 227105197Ssamint 228105197Ssamm_striphdr(struct mbuf *m, int skip, int hlen) 229105197Ssam{ 230105197Ssam struct mbuf *m1; 231105197Ssam int roff; 232105197Ssam 233105197Ssam /* Find beginning of header */ 234105197Ssam m1 = m_getptr(m, skip, &roff); 235105197Ssam if (m1 == NULL) 236105197Ssam return (EINVAL); 237105197Ssam 238105197Ssam /* Remove the header and associated data from the mbuf. */ 239105197Ssam if (roff == 0) { 240105197Ssam /* The header was at the beginning of the mbuf */ 241252026Sae IPSECSTAT_INC(ips_input_front); 242105197Ssam m_adj(m1, hlen); 243105197Ssam if ((m1->m_flags & M_PKTHDR) == 0) 244105197Ssam m->m_pkthdr.len -= hlen; 245105197Ssam } else if (roff + hlen >= m1->m_len) { 246105197Ssam struct mbuf *mo; 247105197Ssam 248105197Ssam /* 249105197Ssam * Part or all of the header is at the end of this mbuf, 250105197Ssam * so first let's remove the remainder of the header from 251105197Ssam * the beginning of the remainder of the mbuf chain, if any. 252105197Ssam */ 253252026Sae IPSECSTAT_INC(ips_input_end); 254105197Ssam if (roff + hlen > m1->m_len) { 255105197Ssam /* Adjust the next mbuf by the remainder */ 256105197Ssam m_adj(m1->m_next, roff + hlen - m1->m_len); 257105197Ssam 258105197Ssam /* The second mbuf is guaranteed not to have a pkthdr... */ 259105197Ssam m->m_pkthdr.len -= (roff + hlen - m1->m_len); 260105197Ssam } 261105197Ssam 262105197Ssam /* Now, let's unlink the mbuf chain for a second...*/ 263105197Ssam mo = m1->m_next; 264105197Ssam m1->m_next = NULL; 265105197Ssam 266105197Ssam /* ...and trim the end of the first part of the chain...sick */ 267105197Ssam m_adj(m1, -(m1->m_len - roff)); 268105197Ssam if ((m1->m_flags & M_PKTHDR) == 0) 269105197Ssam m->m_pkthdr.len -= (m1->m_len - roff); 270105197Ssam 271105197Ssam /* Finally, let's relink */ 272105197Ssam m1->m_next = mo; 273105197Ssam } else { 274105197Ssam /* 275105197Ssam * The header lies in the "middle" of the mbuf; copy 276105197Ssam * the remainder of the mbuf down over the header. 277105197Ssam */ 278252026Sae IPSECSTAT_INC(ips_input_middle); 279105197Ssam bcopy(mtod(m1, u_char *) + roff + hlen, 280105197Ssam mtod(m1, u_char *) + roff, 281105197Ssam m1->m_len - (roff + hlen)); 282105197Ssam m1->m_len -= hlen; 283105197Ssam m->m_pkthdr.len -= hlen; 284105197Ssam } 285105197Ssam return (0); 286105197Ssam} 287105197Ssam 288105197Ssam/* 289105197Ssam * Diagnostic routine to check mbuf alignment as required by the 290105197Ssam * crypto device drivers (that use DMA). 291105197Ssam */ 292105197Ssamvoid 293105197Ssamm_checkalignment(const char* where, struct mbuf *m0, int off, int len) 294105197Ssam{ 295105197Ssam int roff; 296105197Ssam struct mbuf *m = m_getptr(m0, off, &roff); 297105197Ssam caddr_t addr; 298105197Ssam 299105197Ssam if (m == NULL) 300105197Ssam return; 301105197Ssam printf("%s (off %u len %u): ", where, off, len); 302105197Ssam addr = mtod(m, caddr_t) + roff; 303105197Ssam do { 304105197Ssam int mlen; 305105197Ssam 306105197Ssam if (((uintptr_t) addr) & 3) { 307105197Ssam printf("addr misaligned %p,", addr); 308105197Ssam break; 309105197Ssam } 310105197Ssam mlen = m->m_len; 311105197Ssam if (mlen > len) 312105197Ssam mlen = len; 313105197Ssam len -= mlen; 314105197Ssam if (len && (mlen & 3)) { 315105197Ssam printf("len mismatch %u,", mlen); 316105197Ssam break; 317105197Ssam } 318105197Ssam m = m->m_next; 319105197Ssam addr = m ? mtod(m, caddr_t) : NULL; 320105197Ssam } while (m && len > 0); 321105197Ssam for (m = m0; m; m = m->m_next) 322105197Ssam printf(" [%p:%u]", mtod(m, caddr_t), m->m_len); 323105197Ssam printf("\n"); 324105197Ssam} 325