/*
* Insert IP options into preformed packet. Adjust IP destination as
* required for IP source routing, as indicated by a non-zero in_addr at the
* start of the options.
*
* XXX This routine assumes that the packet has no options in place.
*/
struct mbuf *
ip_insertoptions(struct mbuf *m, struct mbuf *opt, int *phlen)
{
struct ipoption *p = mtod(opt, struct ipoption *);
struct mbuf *n;
struct ip *ip = mtod(m, struct ip *);
unsigned optlen;
optlen = opt->m_len - sizeof(p->ipopt_dst);
if (optlen + ntohs(ip->ip_len) > IP_MAXPACKET) {
*phlen = 0;
return (m); /* XXX should fail */
}
if (p->ipopt_dst.s_addr)
ip->ip_dst = p->ipopt_dst;
if (!M_WRITABLE(m) || M_LEADINGSPACE(m) < optlen) {
n = m_gethdr(M_NOWAIT, MT_DATA);
if (n == NULL) {
*phlen = 0;
return (m);
}
m_move_pkthdr(n, m);
n->m_pkthdr.rcvif = NULL;
n->m_pkthdr.len += optlen;
m->m_len -= sizeof(struct ip);
m->m_data += sizeof(struct ip);
n->m_next = m;
m = n;
m->m_len = optlen + sizeof(struct ip);
m->m_data += max_linkhdr;
bcopy(ip, mtod(m, void *), sizeof(struct ip));
} else {
/*
* Re-align the payload in the mbuf. This is mainly used (right now)
* to handle IP header alignment requirements on certain architectures.
*/
struct mbuf *
ieee80211_realign(struct ieee80211vap *vap, struct mbuf *m, size_t align)
{
int pktlen, space;
struct mbuf *n;
pktlen = m->m_pkthdr.len;
space = pktlen + align;
if (space < MINCLSIZE)
n = m_gethdr(M_NOWAIT, MT_DATA);
else {
n = m_getjcl(M_NOWAIT, MT_DATA, M_PKTHDR,
space <= MCLBYTES ? MCLBYTES :
#if MJUMPAGESIZE != MCLBYTES
space <= MJUMPAGESIZE ? MJUMPAGESIZE :
#endif
space <= MJUM9BYTES ? MJUM9BYTES : MJUM16BYTES);
}
if (__predict_true(n != NULL)) {
m_move_pkthdr(n, m);
n->m_data = (caddr_t)(ALIGN(n->m_data + align) - align);
m_copydata(m, 0, pktlen, mtod(n, caddr_t));
n->m_len = pktlen;
} else {
IEEE80211_DISCARD(vap, IEEE80211_MSG_ANY,
mtod(m, const struct ieee80211_frame *), NULL,
"%s", "no mbuf to realign");
vap->iv_stats.is_rx_badalign++;
}
m_freem(m);
return n;
}
m_megapullup(PNATState pData, struct mbuf *m, int len)
#endif
{
struct mbuf *mcl;
if (len > m->m_pkthdr.len)
goto bad;
/* Do not reallocate packet if it is sequentional,
* writable and has some extra space for expansion.
* XXX: Constant 100bytes is completely empirical. */
#define RESERVE 100
if (m->m_next == NULL && M_WRITABLE(m) && M_TRAILINGSPACE(m) >= RESERVE)
return (m);
if (len <= MCLBYTES - RESERVE) {
#ifndef VBOX
mcl = m_getcl(M_DONTWAIT, MT_DATA, M_PKTHDR);
#else
mcl = m_getcl(pData, M_DONTWAIT, MT_DATA, M_PKTHDR);
#endif
} else if (len < MJUM16BYTES) {
int size;
if (len <= MJUMPAGESIZE - RESERVE) {
size = MJUMPAGESIZE;
} else if (len <= MJUM9BYTES - RESERVE) {
size = MJUM9BYTES;
} else {
size = MJUM16BYTES;
};
#ifndef VBOX
mcl = m_getjcl(M_DONTWAIT, MT_DATA, M_PKTHDR, size);
#else
mcl = m_getjcl(pData, M_DONTWAIT, MT_DATA, M_PKTHDR, size);
#endif
} else {
goto bad;
}
if (mcl == NULL)
goto bad;
m_move_pkthdr(mcl, m);
m_copydata(m, 0, len, mtod(mcl, caddr_t));
mcl->m_len = mcl->m_pkthdr.len = len;
#ifndef VBOX
m_freem(m);
#else
m_freem(pData, m);
#endif
return (mcl);
bad:
#ifndef VBOX
m_freem(m);
#else
m_freem(pData, m);
#endif
return (NULL);
}
/*
* Rearange an mbuf chain so that len bytes are contiguous
* and in the data area of an mbuf (so that mtod will work
* for a structure of size len). Returns the resulting
* mbuf chain on success, frees it and returns null on failure.
* If there is room, it will add up to max_protohdr-len extra bytes to the
* contiguous region in an attempt to avoid being called next time.
*/
struct mbuf *
m_pullup(struct mbuf *n, int len)
{
struct mbuf *m;
int count;
int space;
/*
* If first mbuf has no cluster, and has room for len bytes
* without shifting current data, pullup into it,
* otherwise allocate a new mbuf to prepend to the chain.
*/
if ((n->m_flags & M_EXT) == 0 &&
n->m_data + len < &n->m_dat[MLEN] && n->m_next) {
if (n->m_len >= len)
return (n);
m = n;
n = n->m_next;
len -= m->m_len;
} else {
if (len > MHLEN)
goto bad;
m = m_get(M_NOWAIT, n->m_type);
if (m == NULL)
goto bad;
if (n->m_flags & M_PKTHDR)
m_move_pkthdr(m, n);
}
space = &m->m_dat[MLEN] - (m->m_data + m->m_len);
do {
count = min(min(max(len, max_protohdr), space), n->m_len);
bcopy(mtod(n, caddr_t), mtod(m, caddr_t) + m->m_len,
(u_int)count);
len -= count;
m->m_len += count;
n->m_len -= count;
space -= count;
if (n->m_len)
n->m_data += count;
else
n = m_free(n);
} while (len > 0 && n);
if (len > 0) {
(void) m_free(m);
goto bad;
}
m->m_next = n;
return (m);
bad:
m_freem(n);
return (NULL);
}
/*
* Lesser-used path for M_PREPEND:
* allocate new mbuf to prepend to chain,
* copy junk along.
*/
struct mbuf *
m_prepend(struct mbuf *m, int len, int how)
{
struct mbuf *mn;
if (m->m_flags & M_PKTHDR)
mn = m_gethdr(how, m->m_type);
else
mn = m_get(how, m->m_type);
if (mn == NULL) {
m_freem(m);
return (NULL);
}
if (m->m_flags & M_PKTHDR)
m_move_pkthdr(mn, m);
mn->m_next = m;
m = mn;
if (len < M_SIZE(m))
M_ALIGN(m, len);
m->m_len = len;
return (m);
}
struct mbuf *
m_copyup(struct mbuf *n, int len, int dstoff)
{
struct mbuf *m;
int count, space;
if (len > (MHLEN - dstoff))
goto bad;
m = m_get(M_NOWAIT, n->m_type);
if (m == NULL)
goto bad;
if (n->m_flags & M_PKTHDR)
m_move_pkthdr(m, n);
m->m_data += dstoff;
space = &m->m_dat[MLEN] - (m->m_data + m->m_len);
do {
count = min(min(max(len, max_protohdr), space), n->m_len);
memcpy(mtod(m, caddr_t) + m->m_len, mtod(n, caddr_t),
(unsigned)count);
len -= count;
m->m_len += count;
n->m_len -= count;
space -= count;
if (n->m_len)
n->m_data += count;
else
n = m_free(n);
} while (len > 0 && n);
if (len > 0) {
(void) m_free(m);
goto bad;
}
m->m_next = n;
return (m);
bad:
m_freem(n);
MSFail++;
return (NULL);
}
/*
* m_megapullup() - this function is a big hack.
* Thankfully, it's only used in ng_nat and ipfw+nat.
*
* It allocates an mbuf with cluster and copies the specified part of the chain
* into cluster, so that it is all contiguous and can be accessed via a plain
* (char *) pointer. This is required, because libalias doesn't know how to
* handle mbuf chains.
*
* On success, m_megapullup returns an mbuf (possibly with cluster) containing
* the input packet, on failure NULL. The input packet is always consumed.
*/
struct mbuf *
m_megapullup(struct mbuf *m, int len) {
struct mbuf *mcl;
if (len > m->m_pkthdr.len)
goto bad;
if (m->m_next == NULL && M_WRITABLE(m))
return (m);
mcl = m_get2(len, M_NOWAIT, MT_DATA, M_PKTHDR);
if (mcl == NULL)
goto bad;
m_align(mcl, len);
m_move_pkthdr(mcl, m);
m_copydata(m, 0, len, mtod(mcl, caddr_t));
mcl->m_len = mcl->m_pkthdr.len = len;
m_freem(m);
return (mcl);
bad:
m_freem(m);
return (NULL);
}
