1aaea26efSSam Leffler /*- 2aaea26efSSam Leffler * Copyright (c) 2002, 2003 Sam Leffler, Errno Consulting 3aaea26efSSam Leffler * All rights reserved. 4aaea26efSSam Leffler * 5aaea26efSSam Leffler * Redistribution and use in source and binary forms, with or without 6aaea26efSSam Leffler * modification, are permitted provided that the following conditions 7aaea26efSSam Leffler * are met: 8aaea26efSSam Leffler * 1. Redistributions of source code must retain the above copyright 9aaea26efSSam Leffler * notice, this list of conditions and the following disclaimer. 10aaea26efSSam Leffler * 2. Redistributions in binary form must reproduce the above copyright 11aaea26efSSam Leffler * notice, this list of conditions and the following disclaimer in the 12aaea26efSSam Leffler * documentation and/or other materials provided with the distribution. 13aaea26efSSam Leffler * 14aaea26efSSam Leffler * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15aaea26efSSam Leffler * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16aaea26efSSam Leffler * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17aaea26efSSam Leffler * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18aaea26efSSam Leffler * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19aaea26efSSam Leffler * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20aaea26efSSam Leffler * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21aaea26efSSam Leffler * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22aaea26efSSam Leffler * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23aaea26efSSam Leffler * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24aaea26efSSam Leffler * SUCH DAMAGE. 25aaea26efSSam Leffler * 26aaea26efSSam Leffler * $FreeBSD$ 27aaea26efSSam Leffler */ 2888768458SSam Leffler 2988768458SSam Leffler /* 3088768458SSam Leffler * IPsec-specific mbuf routines. 3188768458SSam Leffler */ 3288768458SSam Leffler 3388768458SSam Leffler #include <sys/param.h> 3488768458SSam Leffler #include <sys/systm.h> 358ec07310SGleb Smirnoff #include <sys/malloc.h> 3688768458SSam Leffler #include <sys/mbuf.h> 3788768458SSam Leffler #include <sys/socket.h> 3888768458SSam Leffler 39eddfbb76SRobert Watson #include <net/vnet.h> 4088768458SSam Leffler #include <netinet/in.h> 4188768458SSam Leffler #include <netipsec/ipsec.h> 4288768458SSam Leffler 4388768458SSam Leffler /* 44972136faSSam Leffler * Make space for a new header of length hlen at skip bytes 45972136faSSam Leffler * into the packet. When doing this we allocate new mbufs only 4688768458SSam Leffler * when absolutely necessary. The mbuf where the new header 4788768458SSam Leffler * is to go is returned together with an offset into the mbuf. 4888768458SSam Leffler * If NULL is returned then the mbuf chain may have been modified; 4988768458SSam Leffler * the caller is assumed to always free the chain. 5088768458SSam Leffler */ 5188768458SSam Leffler struct mbuf * 5288768458SSam Leffler m_makespace(struct mbuf *m0, int skip, int hlen, int *off) 5388768458SSam Leffler { 5488768458SSam Leffler struct mbuf *m; 5588768458SSam Leffler unsigned remain; 5688768458SSam Leffler 579ffa9677SSam Leffler IPSEC_ASSERT(m0 != NULL, ("null mbuf")); 589ffa9677SSam Leffler IPSEC_ASSERT(hlen < MHLEN, ("hlen too big: %u", hlen)); 5988768458SSam Leffler 6088768458SSam Leffler for (m = m0; m && skip > m->m_len; m = m->m_next) 6188768458SSam Leffler skip -= m->m_len; 6288768458SSam Leffler if (m == NULL) 6388768458SSam Leffler return (NULL); 6488768458SSam Leffler /* 6588768458SSam Leffler * At this point skip is the offset into the mbuf m 6688768458SSam Leffler * where the new header should be placed. Figure out 6788768458SSam Leffler * if there's space to insert the new header. If so, 68023795f0SBjoern A. Zeeb * and copying the remainder makes sense then do so. 6988768458SSam Leffler * Otherwise insert a new mbuf in the chain, splitting 7088768458SSam Leffler * the contents of m as needed. 7188768458SSam Leffler */ 7288768458SSam Leffler remain = m->m_len - skip; /* data to move */ 73*fcf59617SAndrey V. Elsukov if (remain > skip && 74*fcf59617SAndrey V. Elsukov hlen + max_linkhdr < M_LEADINGSPACE(m)) { 75*fcf59617SAndrey V. Elsukov /* 76*fcf59617SAndrey V. Elsukov * mbuf has enough free space at the beginning. 77*fcf59617SAndrey V. Elsukov * XXX: which operation is the most heavy - copying of 78*fcf59617SAndrey V. Elsukov * possible several hundred of bytes or allocation 79*fcf59617SAndrey V. Elsukov * of new mbuf? We can remove max_linkhdr check 80*fcf59617SAndrey V. Elsukov * here, but it is possible that this will lead 81*fcf59617SAndrey V. Elsukov * to allocation of new mbuf in Layer 2 code. 82*fcf59617SAndrey V. Elsukov */ 83*fcf59617SAndrey V. Elsukov m->m_data -= hlen; 84*fcf59617SAndrey V. Elsukov bcopy(mtodo(m, hlen), mtod(m, caddr_t), skip); 85*fcf59617SAndrey V. Elsukov m->m_len += hlen; 86*fcf59617SAndrey V. Elsukov *off = skip; 87*fcf59617SAndrey V. Elsukov } else if (hlen > M_TRAILINGSPACE(m)) { 88357c11c9SVANHULLEBUS Yvan struct mbuf *n0, *n, **np; 89357c11c9SVANHULLEBUS Yvan int todo, len, done, alloc; 9088768458SSam Leffler 91357c11c9SVANHULLEBUS Yvan n0 = NULL; 92357c11c9SVANHULLEBUS Yvan np = &n0; 93357c11c9SVANHULLEBUS Yvan alloc = 0; 94357c11c9SVANHULLEBUS Yvan done = 0; 95357c11c9SVANHULLEBUS Yvan todo = remain; 96357c11c9SVANHULLEBUS Yvan while (todo > 0) { 97357c11c9SVANHULLEBUS Yvan if (todo > MHLEN) { 98eb1b1807SGleb Smirnoff n = m_getcl(M_NOWAIT, m->m_type, 0); 99357c11c9SVANHULLEBUS Yvan len = MCLBYTES; 100357c11c9SVANHULLEBUS Yvan } 101357c11c9SVANHULLEBUS Yvan else { 102eb1b1807SGleb Smirnoff n = m_get(M_NOWAIT, m->m_type); 103357c11c9SVANHULLEBUS Yvan len = MHLEN; 104357c11c9SVANHULLEBUS Yvan } 105357c11c9SVANHULLEBUS Yvan if (n == NULL) { 106357c11c9SVANHULLEBUS Yvan m_freem(n0); 107357c11c9SVANHULLEBUS Yvan return NULL; 108357c11c9SVANHULLEBUS Yvan } 109357c11c9SVANHULLEBUS Yvan *np = n; 110357c11c9SVANHULLEBUS Yvan np = &n->m_next; 111357c11c9SVANHULLEBUS Yvan alloc++; 112357c11c9SVANHULLEBUS Yvan len = min(todo, len); 113357c11c9SVANHULLEBUS Yvan memcpy(n->m_data, mtod(m, char *) + skip + done, len); 114357c11c9SVANHULLEBUS Yvan n->m_len = len; 115357c11c9SVANHULLEBUS Yvan done += len; 116357c11c9SVANHULLEBUS Yvan todo -= len; 117357c11c9SVANHULLEBUS Yvan } 118357c11c9SVANHULLEBUS Yvan 11988768458SSam Leffler if (hlen <= M_TRAILINGSPACE(m) + remain) { 12088768458SSam Leffler m->m_len = skip + hlen; 12188768458SSam Leffler *off = skip; 122357c11c9SVANHULLEBUS Yvan if (n0 != NULL) { 123357c11c9SVANHULLEBUS Yvan *np = m->m_next; 124357c11c9SVANHULLEBUS Yvan m->m_next = n0; 12588768458SSam Leffler } 126357c11c9SVANHULLEBUS Yvan } 127357c11c9SVANHULLEBUS Yvan else { 128eb1b1807SGleb Smirnoff n = m_get(M_NOWAIT, m->m_type); 129357c11c9SVANHULLEBUS Yvan if (n == NULL) { 130357c11c9SVANHULLEBUS Yvan m_freem(n0); 131357c11c9SVANHULLEBUS Yvan return NULL; 132357c11c9SVANHULLEBUS Yvan } 133357c11c9SVANHULLEBUS Yvan alloc++; 134357c11c9SVANHULLEBUS Yvan 135357c11c9SVANHULLEBUS Yvan if ((n->m_next = n0) == NULL) 136357c11c9SVANHULLEBUS Yvan np = &n->m_next; 137357c11c9SVANHULLEBUS Yvan n0 = n; 138357c11c9SVANHULLEBUS Yvan 139357c11c9SVANHULLEBUS Yvan *np = m->m_next; 140357c11c9SVANHULLEBUS Yvan m->m_next = n0; 141357c11c9SVANHULLEBUS Yvan 142357c11c9SVANHULLEBUS Yvan n->m_len = hlen; 143357c11c9SVANHULLEBUS Yvan m->m_len = skip; 144357c11c9SVANHULLEBUS Yvan 14588768458SSam Leffler m = n; /* header is at front ... */ 14688768458SSam Leffler *off = 0; /* ... of new mbuf */ 14788768458SSam Leffler } 1486659296cSAndrey V. Elsukov IPSECSTAT_INC(ips_mbinserted); 14988768458SSam Leffler } else { 15088768458SSam Leffler /* 15188768458SSam Leffler * Copy the remainder to the back of the mbuf 15288768458SSam Leffler * so there's space to write the new header. 15388768458SSam Leffler */ 1542aebee88SDag-Erling Smørgrav bcopy(mtod(m, caddr_t) + skip, 15588768458SSam Leffler mtod(m, caddr_t) + skip + hlen, remain); 15688768458SSam Leffler m->m_len += hlen; 15788768458SSam Leffler *off = skip; 15888768458SSam Leffler } 15988768458SSam Leffler m0->m_pkthdr.len += hlen; /* adjust packet length */ 16088768458SSam Leffler return m; 16188768458SSam Leffler } 16288768458SSam Leffler 16388768458SSam Leffler /* 16488768458SSam Leffler * m_pad(m, n) pads <m> with <n> bytes at the end. The packet header 16588768458SSam Leffler * length is updated, and a pointer to the first byte of the padding 16688768458SSam Leffler * (which is guaranteed to be all in one mbuf) is returned. 16788768458SSam Leffler */ 16888768458SSam Leffler caddr_t 16988768458SSam Leffler m_pad(struct mbuf *m, int n) 17088768458SSam Leffler { 17188768458SSam Leffler register struct mbuf *m0, *m1; 17288768458SSam Leffler register int len, pad; 17388768458SSam Leffler caddr_t retval; 17488768458SSam Leffler 17588768458SSam Leffler if (n <= 0) { /* No stupid arguments. */ 1769ffa9677SSam Leffler DPRINTF(("%s: pad length invalid (%d)\n", __func__, n)); 17788768458SSam Leffler m_freem(m); 17888768458SSam Leffler return NULL; 17988768458SSam Leffler } 18088768458SSam Leffler 18188768458SSam Leffler len = m->m_pkthdr.len; 18288768458SSam Leffler pad = n; 18388768458SSam Leffler m0 = m; 18488768458SSam Leffler 18588768458SSam Leffler while (m0->m_len < len) { 18688768458SSam Leffler len -= m0->m_len; 18788768458SSam Leffler m0 = m0->m_next; 18888768458SSam Leffler } 18988768458SSam Leffler 19088768458SSam Leffler if (m0->m_len != len) { 1919ffa9677SSam Leffler DPRINTF(("%s: length mismatch (should be %d instead of %d)\n", 1929ffa9677SSam Leffler __func__, m->m_pkthdr.len, 1939ffa9677SSam Leffler m->m_pkthdr.len + m0->m_len - len)); 19488768458SSam Leffler 19588768458SSam Leffler m_freem(m); 19688768458SSam Leffler return NULL; 19788768458SSam Leffler } 19888768458SSam Leffler 19988768458SSam Leffler /* Check for zero-length trailing mbufs, and find the last one. */ 20088768458SSam Leffler for (m1 = m0; m1->m_next; m1 = m1->m_next) { 20188768458SSam Leffler if (m1->m_next->m_len != 0) { 2029ffa9677SSam Leffler DPRINTF(("%s: length mismatch (should be %d instead " 2039ffa9677SSam Leffler "of %d)\n", __func__, 20488768458SSam Leffler m->m_pkthdr.len, 20588768458SSam Leffler m->m_pkthdr.len + m1->m_next->m_len)); 20688768458SSam Leffler 20788768458SSam Leffler m_freem(m); 20888768458SSam Leffler return NULL; 20988768458SSam Leffler } 21088768458SSam Leffler 21188768458SSam Leffler m0 = m1->m_next; 21288768458SSam Leffler } 21388768458SSam Leffler 21488768458SSam Leffler if (pad > M_TRAILINGSPACE(m0)) { 21588768458SSam Leffler /* Add an mbuf to the chain. */ 216eb1b1807SGleb Smirnoff MGET(m1, M_NOWAIT, MT_DATA); 217155d72c4SPedro F. Giffuni if (m1 == NULL) { 21888768458SSam Leffler m_freem(m0); 2199ffa9677SSam Leffler DPRINTF(("%s: unable to get extra mbuf\n", __func__)); 22088768458SSam Leffler return NULL; 22188768458SSam Leffler } 22288768458SSam Leffler 22388768458SSam Leffler m0->m_next = m1; 22488768458SSam Leffler m0 = m1; 22588768458SSam Leffler m0->m_len = 0; 22688768458SSam Leffler } 22788768458SSam Leffler 22888768458SSam Leffler retval = m0->m_data + m0->m_len; 22988768458SSam Leffler m0->m_len += pad; 23088768458SSam Leffler m->m_pkthdr.len += pad; 23188768458SSam Leffler 23288768458SSam Leffler return retval; 23388768458SSam Leffler } 23488768458SSam Leffler 23588768458SSam Leffler /* 23688768458SSam Leffler * Remove hlen data at offset skip in the packet. This is used by 23788768458SSam Leffler * the protocols strip protocol headers and associated data (e.g. IV, 23888768458SSam Leffler * authenticator) on input. 23988768458SSam Leffler */ 24088768458SSam Leffler int 24188768458SSam Leffler m_striphdr(struct mbuf *m, int skip, int hlen) 24288768458SSam Leffler { 24388768458SSam Leffler struct mbuf *m1; 24488768458SSam Leffler int roff; 24588768458SSam Leffler 24688768458SSam Leffler /* Find beginning of header */ 24788768458SSam Leffler m1 = m_getptr(m, skip, &roff); 24888768458SSam Leffler if (m1 == NULL) 24988768458SSam Leffler return (EINVAL); 25088768458SSam Leffler 25188768458SSam Leffler /* Remove the header and associated data from the mbuf. */ 25288768458SSam Leffler if (roff == 0) { 25388768458SSam Leffler /* The header was at the beginning of the mbuf */ 2546659296cSAndrey V. Elsukov IPSECSTAT_INC(ips_input_front); 25588768458SSam Leffler m_adj(m1, hlen); 25688768458SSam Leffler if ((m1->m_flags & M_PKTHDR) == 0) 25788768458SSam Leffler m->m_pkthdr.len -= hlen; 25888768458SSam Leffler } else if (roff + hlen >= m1->m_len) { 25988768458SSam Leffler struct mbuf *mo; 26088768458SSam Leffler 26188768458SSam Leffler /* 26288768458SSam Leffler * Part or all of the header is at the end of this mbuf, 26388768458SSam Leffler * so first let's remove the remainder of the header from 26488768458SSam Leffler * the beginning of the remainder of the mbuf chain, if any. 26588768458SSam Leffler */ 2666659296cSAndrey V. Elsukov IPSECSTAT_INC(ips_input_end); 26788768458SSam Leffler if (roff + hlen > m1->m_len) { 26888768458SSam Leffler /* Adjust the next mbuf by the remainder */ 26988768458SSam Leffler m_adj(m1->m_next, roff + hlen - m1->m_len); 27088768458SSam Leffler 27188768458SSam Leffler /* The second mbuf is guaranteed not to have a pkthdr... */ 27288768458SSam Leffler m->m_pkthdr.len -= (roff + hlen - m1->m_len); 27388768458SSam Leffler } 27488768458SSam Leffler 27588768458SSam Leffler /* Now, let's unlink the mbuf chain for a second...*/ 27688768458SSam Leffler mo = m1->m_next; 27788768458SSam Leffler m1->m_next = NULL; 27888768458SSam Leffler 27988768458SSam Leffler /* ...and trim the end of the first part of the chain...sick */ 28088768458SSam Leffler m_adj(m1, -(m1->m_len - roff)); 28188768458SSam Leffler if ((m1->m_flags & M_PKTHDR) == 0) 28288768458SSam Leffler m->m_pkthdr.len -= (m1->m_len - roff); 28388768458SSam Leffler 28488768458SSam Leffler /* Finally, let's relink */ 28588768458SSam Leffler m1->m_next = mo; 28688768458SSam Leffler } else { 28788768458SSam Leffler /* 28888768458SSam Leffler * The header lies in the "middle" of the mbuf; copy 28988768458SSam Leffler * the remainder of the mbuf down over the header. 29088768458SSam Leffler */ 2916659296cSAndrey V. Elsukov IPSECSTAT_INC(ips_input_middle); 29288768458SSam Leffler bcopy(mtod(m1, u_char *) + roff + hlen, 29388768458SSam Leffler mtod(m1, u_char *) + roff, 29488768458SSam Leffler m1->m_len - (roff + hlen)); 29588768458SSam Leffler m1->m_len -= hlen; 29688768458SSam Leffler m->m_pkthdr.len -= hlen; 29788768458SSam Leffler } 29888768458SSam Leffler return (0); 29988768458SSam Leffler } 30088768458SSam Leffler 30188768458SSam Leffler /* 30288768458SSam Leffler * Diagnostic routine to check mbuf alignment as required by the 30388768458SSam Leffler * crypto device drivers (that use DMA). 30488768458SSam Leffler */ 30588768458SSam Leffler void 30688768458SSam Leffler m_checkalignment(const char* where, struct mbuf *m0, int off, int len) 30788768458SSam Leffler { 30888768458SSam Leffler int roff; 30988768458SSam Leffler struct mbuf *m = m_getptr(m0, off, &roff); 31088768458SSam Leffler caddr_t addr; 31188768458SSam Leffler 31288768458SSam Leffler if (m == NULL) 31388768458SSam Leffler return; 31488768458SSam Leffler printf("%s (off %u len %u): ", where, off, len); 31588768458SSam Leffler addr = mtod(m, caddr_t) + roff; 31688768458SSam Leffler do { 31788768458SSam Leffler int mlen; 31888768458SSam Leffler 31988768458SSam Leffler if (((uintptr_t) addr) & 3) { 32088768458SSam Leffler printf("addr misaligned %p,", addr); 32188768458SSam Leffler break; 32288768458SSam Leffler } 32388768458SSam Leffler mlen = m->m_len; 32488768458SSam Leffler if (mlen > len) 32588768458SSam Leffler mlen = len; 32688768458SSam Leffler len -= mlen; 32788768458SSam Leffler if (len && (mlen & 3)) { 32888768458SSam Leffler printf("len mismatch %u,", mlen); 32988768458SSam Leffler break; 33088768458SSam Leffler } 33188768458SSam Leffler m = m->m_next; 33288768458SSam Leffler addr = m ? mtod(m, caddr_t) : NULL; 33388768458SSam Leffler } while (m && len > 0); 33488768458SSam Leffler for (m = m0; m; m = m->m_next) 33588768458SSam Leffler printf(" [%p:%u]", mtod(m, caddr_t), m->m_len); 33688768458SSam Leffler printf("\n"); 33788768458SSam Leffler } 338