#include <sys/param.h>
#include <sys/systm.h>
#include <sys/proc_internal.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>
#if defined(PULLDOWN_STAT) && defined(INET6)
#include <netinet/in.h>
#include <netinet/ip6.h>
#include <netinet6/ip6_var.h>
#endif
struct mbuf *
m_pulldown(m, off, len, offp)
struct mbuf *m;
int off, len;
int *offp;
{
struct mbuf *n, *o;
int hlen, tlen, olen;
int sharedcluster;
#if defined(PULLDOWN_STAT) && defined(INET6)
static struct mbuf *prev = NULL;
int prevlen = 0, prevmlen = 0;
#endif
if (m == NULL)
panic("m == NULL in m_pulldown()");
if (len > MCLBYTES) {
m_freem(m);
return NULL;
}
#if defined(PULLDOWN_STAT) && defined(INET6)
ip6stat.ip6s_pulldown++;
#endif
#if defined(PULLDOWN_STAT) && defined(INET6)
ip6stat.ip6s_pullup++;
if (off + len > MHLEN)
ip6stat.ip6s_pullup_fail++;
else {
int dlen, mlen;
dlen = (prev == m) ? prevlen : m->m_len;
mlen = (prev == m) ? prevmlen : m->m_len + M_TRAILINGSPACE(m);
if (dlen >= off + len)
ip6stat.ip6s_pullup--;
else if ((m->m_flags & M_EXT) != 0) {
ip6stat.ip6s_pullup_alloc++;
ip6stat.ip6s_pullup_copy++;
} else {
if (mlen >= off + len)
ip6stat.ip6s_pullup_copy++;
else {
ip6stat.ip6s_pullup_alloc++;
ip6stat.ip6s_pullup_copy++;
}
}
prevlen = off + len;
prevmlen = MHLEN;
}
ip6stat.ip6s_pullup2++;
if (off + len > MCLBYTES)
ip6stat.ip6s_pullup2_fail++;
else {
int dlen, mlen;
dlen = (prev == m) ? prevlen : m->m_len;
mlen = (prev == m) ? prevmlen : m->m_len + M_TRAILINGSPACE(m);
prevlen = off + len;
prevmlen = mlen;
if (dlen >= off + len)
ip6stat.ip6s_pullup2--;
else if ((m->m_flags & M_EXT) != 0) {
ip6stat.ip6s_pullup2_alloc++;
ip6stat.ip6s_pullup2_copy++;
prevmlen = (off + len > MHLEN) ? MCLBYTES : MHLEN;
} else {
if (mlen >= off + len)
ip6stat.ip6s_pullup2_copy++;
else {
ip6stat.ip6s_pullup2_alloc++;
ip6stat.ip6s_pullup2_copy++;
prevmlen = (off + len > MHLEN) ? MCLBYTES
: MHLEN;
}
}
}
prev = m;
#endif
#ifdef PULLDOWN_DEBUG
{
struct mbuf *t;
printf("before:");
for (t = m; t; t = t->m_next)
printf(" %d", t->m_len);
printf("\n");
}
#endif
n = m;
while (n != NULL && off > 0) {
if (n->m_len > off)
break;
off -= n->m_len;
n = n->m_next;
}
while (n != NULL && n->m_len == 0)
n = n->m_next;
if (!n) {
m_freem(m);
return NULL;
}
if ((off == 0 || offp) && len <= n->m_len - off)
goto ok;
#if defined(PULLDOWN_STAT) && defined(INET6)
ip6stat.ip6s_pulldown_copy++;
#endif
if (len < n->m_len - off) {
o = m_copym(n, off, n->m_len - off, M_DONTWAIT);
if (o == NULL) {
m_freem(m);
return NULL;
}
n->m_len = off;
o->m_next = n->m_next;
n->m_next = o;
n = n->m_next;
off = 0;
goto ok;
}
hlen = n->m_len - off;
tlen = len - hlen;
olen = 0;
for (o = n->m_next; o != NULL; o = o->m_next)
olen += o->m_len;
if (hlen + olen < len) {
m_freem(m);
return NULL;
}
if ((n->m_flags & M_EXT) == 0)
sharedcluster = 0;
else {
if (n->m_ext.ext_free)
sharedcluster = 1;
else if (m_mclhasreference(n))
sharedcluster = 1;
else
sharedcluster = 0;
}
if ((off == 0 || offp) && M_TRAILINGSPACE(n) >= tlen
&& !sharedcluster) {
m_copydata(n->m_next, 0, tlen, mtod(n, caddr_t) + n->m_len);
n->m_len += tlen;
m_adj(n->m_next, tlen);
goto ok;
}
if ((off == 0 || offp) && M_LEADINGSPACE(n->m_next) >= hlen
&& !sharedcluster) {
n->m_next->m_data -= hlen;
n->m_next->m_len += hlen;
bcopy(mtod(n, caddr_t) + off, mtod(n->m_next, caddr_t), hlen);
n->m_len -= hlen;
n = n->m_next;
off = 0;
goto ok;
}
#if defined(PULLDOWN_STAT) && defined(INET6)
ip6stat.ip6s_pulldown_alloc++;
#endif
MGET(o, M_DONTWAIT, m->m_type);
if (o == NULL) {
m_freem(m);
return NULL;
}
if (len > MHLEN) {
MCLGET(o, M_DONTWAIT);
if ((o->m_flags & M_EXT) == 0) {
m_freem(m);
m_free(o);
return NULL;
}
}
o->m_len = hlen;
bcopy(mtod(n, caddr_t) + off, mtod(o, caddr_t), hlen);
n->m_len -= hlen;
m_copydata(n->m_next, 0, tlen, mtod(o, caddr_t) + o->m_len);
o->m_len += tlen;
m_adj(n->m_next, tlen);
o->m_next = n->m_next;
n->m_next = o;
n = o;
off = 0;
ok:
#ifdef PULLDOWN_DEBUG
{
struct mbuf *t;
printf("after:");
for (t = m; t; t = t->m_next)
printf("%c%d", t == n ? '*' : ' ', t->m_len);
printf(" (off=%d)\n", off);
}
#endif
if (offp)
*offp = off;
return n;
}
struct mbuf *
m_aux_add(m, af, type)
struct mbuf *m;
int af, type;
{
struct mbuf *n;
struct mauxtag *t;
if ((m->m_flags & M_PKTHDR) == 0)
return NULL;
n = m_aux_find(m, af, type);
if (n)
return n;
MGET(n, M_DONTWAIT, m->m_type);
if (n == NULL)
return NULL;
t = mtod(n, struct mauxtag *);
t->af = af;
t->type = type;
n->m_data += sizeof(struct mauxtag);
n->m_len = 0;
n->m_next = m->m_pkthdr.aux;
m->m_pkthdr.aux = n;
return n;
}
struct mbuf *
m_aux_find(m, af, type)
struct mbuf *m;
int af, type;
{
struct mbuf *n;
struct mauxtag *t;
if ((m->m_flags & M_PKTHDR) == 0)
return NULL;
for (n = m->m_pkthdr.aux; n; n = n->m_next) {
t = (struct mauxtag *)n->m_dat;
if (t->af == af && t->type == type)
return n;
}
return NULL;
}
void
m_aux_delete(m, victim)
struct mbuf *m;
struct mbuf *victim;
{
struct mbuf *n, *prev, *next;
struct mauxtag *t;
if ((m->m_flags & M_PKTHDR) == 0)
return;
prev = NULL;
n = m->m_pkthdr.aux;
while (n) {
t = (struct mauxtag *)n->m_dat;
next = n->m_next;
if (n == victim) {
if (prev)
prev->m_next = n->m_next;
else
m->m_pkthdr.aux = n->m_next;
n->m_next = NULL;
m_free(n);
} else
prev = n;
n = next;
}
}
struct m_tag *
m_tag_alloc(u_int32_t id, u_int16_t type, int len, int wait)
{
struct m_tag *t;
if (len < 0)
return NULL;
#ifndef __APPLE__
t = malloc(len + sizeof(struct m_tag), M_PACKET_TAGS, wait);
#else
if (len + sizeof(struct m_tag) <= MLEN) {
struct mbuf *m = m_get(wait, MT_TAG);
if (m == NULL)
return NULL;
t = (struct m_tag *) m->m_dat;
} else if (len + sizeof(struct m_tag) <= MCLBYTES) {
MCLALLOC((caddr_t)t, wait);
} else
t = NULL;
#endif
if (t == NULL)
return NULL;
t->m_tag_type = type;
t->m_tag_len = len;
t->m_tag_id = id;
return t;
}
void
m_tag_free(struct m_tag *t)
{
#ifndef __APPLE__
free(t, M_PACKET_TAGS);
#else
if (t == NULL)
return;
if (t->m_tag_len <= MLEN) {
struct mbuf * m = m_dtom(t);
m_free(m);
} else {
MCLFREE((caddr_t)t);
}
#endif
}
void
m_tag_prepend(struct mbuf *m, struct m_tag *t)
{
KASSERT(m && t, ("m_tag_prepend: null argument, m %p t %p", m, t));
SLIST_INSERT_HEAD(&m->m_pkthdr.tags, t, m_tag_link);
}
void
m_tag_unlink(struct mbuf *m, struct m_tag *t)
{
KASSERT(m && t, ("m_tag_unlink: null argument, m %p t %p", m, t));
SLIST_REMOVE(&m->m_pkthdr.tags, t, m_tag, m_tag_link);
}
void
m_tag_delete(struct mbuf *m, struct m_tag *t)
{
KASSERT(m && t, ("m_tag_delete: null argument, m %p t %p", m, t));
m_tag_unlink(m, t);
m_tag_free(t);
}
void
m_tag_delete_chain(struct mbuf *m, struct m_tag *t)
{
struct m_tag *p, *q;
KASSERT(m, ("m_tag_delete_chain: null mbuf"));
if (t != NULL)
p = t;
else
p = SLIST_FIRST(&m->m_pkthdr.tags);
if (p == NULL)
return;
while ((q = SLIST_NEXT(p, m_tag_link)) != NULL)
m_tag_delete(m, q);
m_tag_delete(m, p);
}
struct m_tag *
m_tag_locate(struct mbuf *m, u_int32_t id, u_int16_t type, struct m_tag *t)
{
struct m_tag *p;
KASSERT(m, ("m_tag_find: null mbuf"));
if (t == NULL)
p = SLIST_FIRST(&m->m_pkthdr.tags);
else
p = SLIST_NEXT(t, m_tag_link);
while (p != NULL) {
if (p->m_tag_id == id && p->m_tag_type == type)
return p;
p = SLIST_NEXT(p, m_tag_link);
}
return NULL;
}
struct m_tag *
m_tag_copy(struct m_tag *t, int how)
{
struct m_tag *p;
KASSERT(t, ("m_tag_copy: null tag"));
p = m_tag_alloc(t->m_tag_id, t->m_tag_type, t->m_tag_len, how);
if (p == NULL)
return (NULL);
bcopy(t + 1, p + 1, t->m_tag_len);
return p;
}
int
m_tag_copy_chain(struct mbuf *to, struct mbuf *from, int how)
{
struct m_tag *p, *t, *tprev = NULL;
KASSERT(to && from,
("m_tag_copy: null argument, to %p from %p", to, from));
m_tag_delete_chain(to, NULL);
SLIST_FOREACH(p, &from->m_pkthdr.tags, m_tag_link) {
t = m_tag_copy(p, how);
if (t == NULL) {
m_tag_delete_chain(to, NULL);
return 0;
}
if (tprev == NULL)
SLIST_INSERT_HEAD(&to->m_pkthdr.tags, t, m_tag_link);
else {
SLIST_INSERT_AFTER(tprev, t, m_tag_link);
tprev = t;
}
}
return 1;
}
void
m_tag_init(struct mbuf *m)
{
SLIST_INIT(&m->m_pkthdr.tags);
}
struct m_tag *
m_tag_first(struct mbuf *m)
{
return SLIST_FIRST(&m->m_pkthdr.tags);
}
struct m_tag *
m_tag_next(__unused struct mbuf *m, struct m_tag *t)
{
return SLIST_NEXT(t, m_tag_link);
}