errno_t
mbuf_dup(const mbuf_t src, mbuf_how_t how, mbuf_t *new_mbuf)
{
/* Must set *new_mbuf to NULL in failure case */
*new_mbuf = m_dup(src, how);
return (*new_mbuf == NULL ? ENOMEM : 0);
}
static int
nicvf_if_transmit(struct ifnet *ifp, struct mbuf *mbuf)
{
struct nicvf *nic = if_getsoftc(ifp);
struct queue_set *qs = nic->qs;
struct snd_queue *sq;
struct mbuf *mtmp;
int qidx;
int err = 0;
if (__predict_false(qs == NULL)) {
panic("%s: missing queue set for %s", __func__,
device_get_nameunit(nic->dev));
}
/* Select queue */
if (M_HASHTYPE_GET(mbuf) != M_HASHTYPE_NONE)
qidx = mbuf->m_pkthdr.flowid % qs->sq_cnt;
else
qidx = curcpu % qs->sq_cnt;
sq = &qs->sq[qidx];
if (mbuf->m_next != NULL &&
(mbuf->m_pkthdr.csum_flags &
(CSUM_IP | CSUM_TCP | CSUM_UDP | CSUM_SCTP)) != 0) {
if (M_WRITABLE(mbuf) == 0) {
mtmp = m_dup(mbuf, M_NOWAIT);
m_freem(mbuf);
if (mtmp == NULL)
return (ENOBUFS);
mbuf = mtmp;
}
}
err = drbr_enqueue(ifp, sq->br, mbuf);
if (((if_getdrvflags(ifp) & (IFF_DRV_RUNNING | IFF_DRV_OACTIVE)) !=
IFF_DRV_RUNNING) || !nic->link_up || (err != 0)) {
/*
* Try to enqueue packet to the ring buffer.
* If the driver is not active, link down or enqueue operation
* failed, return with the appropriate error code.
*/
return (err);
}
if (NICVF_TX_TRYLOCK(sq) != 0) {
err = nicvf_xmit_locked(sq);
NICVF_TX_UNLOCK(sq);
return (err);
} else
taskqueue_enqueue(sq->snd_taskq, &sq->snd_task);
return (0);
}
struct pager *
pager_spawn(const char *desc)
{
struct sigaction sa;
struct pager *pager;
const char *exec;
pager = m_calloc(1, sizeof(*pager));
pager->used = isatty(0) && isatty(1);
pager->desc = desc;
exec = pager_get_exec();
if (strcmp(exec, CAT) == 0)
pager->used = false;
if (!pager_enabled)
pager->used = false;
if (!pager->used)
return pager;
m_pipe(pager->pipe);
memset(&sa, 0, sizeof(sa));
sigemptyset(&sa.sa_mask);
sa.sa_handler = SIG_IGN;
sa.sa_flags = 0;
sigaction(SIGPIPE, &sa, &pager->sigpipe);
pager->pid = subproc_fork();
if (pager->pid == 0) {
/* Set better defaults for less if not already set. */
setenv("LESS", "-FRSXMQ", 0);
m_dup2(pager->pipe[0], 0);
close(pager->pipe[0]);
close(pager->pipe[1]);
command_shell(exec, desc);
}
pager->stdout_old = m_dup(1);
m_dup2(pager->pipe[1], 1);
close(pager->pipe[0]);
close(pager->pipe[1]);
/* Force the output to fully buffered, because originally stdout was
* a tty, so it was set as line buffered. This way we send as much as
* possible to the pager, which will handle the output by itself. */
setvbuf(stdout, NULL, _IOFBF, 0);
return pager;
}
static int
ipfw_divert(struct mbuf **m, int incoming, int tee)
{
/*
* ipfw_chk() has already tagged the packet with the divert tag.
* If tee is set, copy packet and return original.
* If not tee, consume packet and send it to divert socket.
*/
#ifdef IPDIVERT
struct mbuf *clone, *reass;
struct ip *ip;
int hlen;
reass = NULL;
/* Cloning needed for tee? */
if (tee)
clone = m_dup(*m, M_DONTWAIT);
else
clone = *m;
/* In case m_dup was unable to allocate mbufs. */
if (clone == NULL)
goto teeout;
/*
* Divert listeners can only handle non-fragmented packets.
* However when tee is set we will *not* de-fragment the packets;
* Doing do would put the reassembly into double-jeopardy. On top
* of that someone doing a tee will probably want to get the packet
* in its original form.
*/
ip = mtod(clone, struct ip *);
if (!tee && ip->ip_off & (IP_MF | IP_OFFMASK)) {
/* Reassemble packet. */
reass = ip_reass(clone);
/*
* IP header checksum fixup after reassembly and leave header
* in network byte order.
*/
if (reass != NULL) {
ip = mtod(reass, struct ip *);
hlen = ip->ip_hl << 2;
ip->ip_len = htons(ip->ip_len);
ip->ip_off = htons(ip->ip_off);
ip->ip_sum = 0;
if (hlen == sizeof(struct ip))
ip->ip_sum = in_cksum_hdr(ip);
else
ip->ip_sum = in_cksum(reass, hlen);
clone = reass;
} else
void
ng_hci_mtap(ng_hci_unit_p unit, struct mbuf *m0)
{
struct mbuf *m = NULL;
int error = 0;
if (unit->raw != NULL && NG_HOOK_IS_VALID(unit->raw)) {
m = m_dup(m0, M_DONTWAIT);
if (m != NULL)
NG_SEND_DATA_ONLY(error, unit->raw, m);
if (error != 0)
NG_HCI_INFO(
"%s: %s - Could not forward packet, error=%d\n",
__func__, NG_NODE_NAME(unit->node), error);
}
} /* ng_hci_mtap */
/*
* Ethernet output routine.
* Encapsulate a packet of type family for the local net.
* Use trailer local net encapsulation if enough data in first
* packet leaves a multiple of 512 bytes of data in remainder.
*/
int
ether_output(struct ifnet *ifp, struct mbuf *m,
const struct sockaddr *dst, struct route *ro)
{
short type;
int error = 0, hdrcmplt = 0;
u_char esrc[ETHER_ADDR_LEN], edst[ETHER_ADDR_LEN];
struct llentry *lle = NULL;
struct rtentry *rt0 = NULL;
struct ether_header *eh;
struct pf_mtag *t;
int loop_copy = 1;
int hlen; /* link layer header length */
if (ro != NULL) {
if (!(m->m_flags & (M_BCAST | M_MCAST)))
lle = ro->ro_lle;
rt0 = ro->ro_rt;
}
#ifdef MAC
error = mac_ifnet_check_transmit(ifp, m);
if (error)
senderr(error);
#endif
M_PROFILE(m);
if (ifp->if_flags & IFF_MONITOR)
senderr(ENETDOWN);
if (!((ifp->if_flags & IFF_UP) &&
(ifp->if_drv_flags & IFF_DRV_RUNNING)))
senderr(ENETDOWN);
hlen = ETHER_HDR_LEN;
switch (dst->sa_family) {
#ifdef INET
case AF_INET:
if (lle != NULL && (lle->la_flags & LLE_VALID))
memcpy(edst, &lle->ll_addr.mac16, sizeof(edst));
else
error = arpresolve(ifp, rt0, m, dst, edst, &lle);
if (error)
return (error == EWOULDBLOCK ? 0 : error);
type = htons(ETHERTYPE_IP);
break;
case AF_ARP:
{
struct arphdr *ah;
ah = mtod(m, struct arphdr *);
ah->ar_hrd = htons(ARPHRD_ETHER);
loop_copy = 0; /* if this is for us, don't do it */
switch(ntohs(ah->ar_op)) {
case ARPOP_REVREQUEST:
case ARPOP_REVREPLY:
type = htons(ETHERTYPE_REVARP);
break;
case ARPOP_REQUEST:
case ARPOP_REPLY:
default:
type = htons(ETHERTYPE_ARP);
break;
}
if (m->m_flags & M_BCAST)
bcopy(ifp->if_broadcastaddr, edst, ETHER_ADDR_LEN);
else
bcopy(ar_tha(ah), edst, ETHER_ADDR_LEN);
}
break;
#endif
#ifdef INET6
case AF_INET6:
if (lle != NULL && (lle->la_flags & LLE_VALID))
memcpy(edst, &lle->ll_addr.mac16, sizeof(edst));
else
error = nd6_storelladdr(ifp, m, dst, (u_char *)edst, &lle);
if (error)
return error;
type = htons(ETHERTYPE_IPV6);
break;
#endif
#ifdef IPX
case AF_IPX:
if (ef_outputp) {
error = ef_outputp(ifp, &m, dst, &type, &hlen);
if (error)
goto bad;
} else
type = htons(ETHERTYPE_IPX);
bcopy(&((const struct sockaddr_ipx *)dst)->sipx_addr.x_host,
edst, sizeof (edst));
break;
#endif
#ifdef NETATALK
case AF_APPLETALK:
{
struct at_ifaddr *aa;
if ((aa = at_ifawithnet((const struct sockaddr_at *)dst)) == NULL)
senderr(EHOSTUNREACH); /* XXX */
if (!aarpresolve(ifp, m, (const struct sockaddr_at *)dst, edst)) {
ifa_free(&aa->aa_ifa);
return (0);
}
/*
* In the phase 2 case, need to prepend an mbuf for the llc header.
*/
if ( aa->aa_flags & AFA_PHASE2 ) {
struct llc llc;
ifa_free(&aa->aa_ifa);
M_PREPEND(m, LLC_SNAPFRAMELEN, M_NOWAIT);
if (m == NULL)
senderr(ENOBUFS);
llc.llc_dsap = llc.llc_ssap = LLC_SNAP_LSAP;
llc.llc_control = LLC_UI;
bcopy(at_org_code, llc.llc_snap_org_code, sizeof(at_org_code));
llc.llc_snap_ether_type = htons( ETHERTYPE_AT );
bcopy(&llc, mtod(m, caddr_t), LLC_SNAPFRAMELEN);
type = htons(m->m_pkthdr.len);
hlen = LLC_SNAPFRAMELEN + ETHER_HDR_LEN;
} else {
ifa_free(&aa->aa_ifa);
type = htons(ETHERTYPE_AT);
}
break;
}
#endif /* NETATALK */
case pseudo_AF_HDRCMPLT:
{
const struct ether_header *eh;
hdrcmplt = 1;
eh = (const struct ether_header *)dst->sa_data;
(void)memcpy(esrc, eh->ether_shost, sizeof (esrc));
/* FALLTHROUGH */
case AF_UNSPEC:
loop_copy = 0; /* if this is for us, don't do it */
eh = (const struct ether_header *)dst->sa_data;
(void)memcpy(edst, eh->ether_dhost, sizeof (edst));
type = eh->ether_type;
break;
}
#ifdef MPLS
case AF_MPLS:
if (lle != NULL && (lle->la_flags & LLE_VALID))
bcopy(&lle->ll_addr.mac16 , edst, sizeof(edst));
else
error = mpls_arpresolve(ifp, rt0, m, dst, edst, &lle);
if (error)
return (error == EWOULDBLOCK ? 0 : error);
if (m->m_flags & (M_BCAST | M_MCAST))
type = htons(ETHERTYPE_MPLS_MCAST);
else
type = htons(ETHERTYPE_MPLS);
break;
#endif /* MPLS */
default:
if_printf(ifp, "can't handle af%d\n", dst->sa_family);
senderr(EAFNOSUPPORT);
}
if (lle != NULL && (lle->la_flags & LLE_IFADDR)) {
update_mbuf_csumflags(m, m);
return (if_simloop(ifp, m, dst->sa_family, 0));
}
/*
* Add local net header. If no space in first mbuf,
* allocate another.
*/
M_PREPEND(m, ETHER_HDR_LEN, M_NOWAIT);
if (m == NULL)
senderr(ENOBUFS);
eh = mtod(m, struct ether_header *);
(void)memcpy(&eh->ether_type, &type,
sizeof(eh->ether_type));
(void)memcpy(eh->ether_dhost, edst, sizeof (edst));
if (hdrcmplt)
(void)memcpy(eh->ether_shost, esrc,
sizeof(eh->ether_shost));
else
(void)memcpy(eh->ether_shost, IF_LLADDR(ifp),
sizeof(eh->ether_shost));
/*
* If a simplex interface, and the packet is being sent to our
* Ethernet address or a broadcast address, loopback a copy.
* XXX To make a simplex device behave exactly like a duplex
* device, we should copy in the case of sending to our own
* ethernet address (thus letting the original actually appear
* on the wire). However, we don't do that here for security
* reasons and compatibility with the original behavior.
*/
if ((ifp->if_flags & IFF_SIMPLEX) && loop_copy &&
((t = pf_find_mtag(m)) == NULL || !t->routed)) {
if (m->m_flags & M_BCAST) {
struct mbuf *n;
/*
* Because if_simloop() modifies the packet, we need a
* writable copy through m_dup() instead of a readonly
* one as m_copy[m] would give us. The alternative would
* be to modify if_simloop() to handle the readonly mbuf,
* but performancewise it is mostly equivalent (trading
* extra data copying vs. extra locking).
*
* XXX This is a local workaround. A number of less
* often used kernel parts suffer from the same bug.
* See PR kern/105943 for a proposed general solution.
*/
if ((n = m_dup(m, M_NOWAIT)) != NULL) {
update_mbuf_csumflags(m, n);
(void)if_simloop(ifp, n, dst->sa_family, hlen);
} else
ifp->if_iqdrops++;
} else if (bcmp(eh->ether_dhost, eh->ether_shost,
ETHER_ADDR_LEN) == 0) {
update_mbuf_csumflags(m, m);
(void) if_simloop(ifp, m, dst->sa_family, hlen);
return (0); /* XXX */
}
}
/*
* Bridges require special output handling.
*/
if (ifp->if_bridge) {
BRIDGE_OUTPUT(ifp, m, error);
return (error);
}
#if defined(INET) || defined(INET6)
if (ifp->if_carp &&
(error = (*carp_output_p)(ifp, m, dst)))
goto bad;
#endif
/* Handle ng_ether(4) processing, if any */
if (IFP2AC(ifp)->ac_netgraph != NULL) {
KASSERT(ng_ether_output_p != NULL,
("ng_ether_output_p is NULL"));
if ((error = (*ng_ether_output_p)(ifp, &m)) != 0) {
bad: if (m != NULL)
m_freem(m);
return (error);
}
if (m == NULL)
return (0);
}
/* Continue with link-layer output */
return ether_output_frame(ifp, m);
}
/*
* Ethernet output routine.
* Encapsulate a packet of type family for the local net.
* Use trailer local net encapsulation if enough data in first
* packet leaves a multiple of 512 bytes of data in remainder.
*/
int
ether_output(struct ifnet *ifp, struct mbuf *m,
const struct sockaddr *dst, struct route *ro)
{
short type;
int error = 0, hdrcmplt = 0;
u_char edst[ETHER_ADDR_LEN];
struct llentry *lle = NULL;
struct rtentry *rt0 = NULL;
struct ether_header *eh;
struct pf_mtag *t;
int loop_copy = 1;
int hlen; /* link layer header length */
int is_gw = 0;
uint32_t pflags = 0;
if (ro != NULL) {
if (!(m->m_flags & (M_BCAST | M_MCAST))) {
lle = ro->ro_lle;
if (lle != NULL)
pflags = lle->la_flags;
}
rt0 = ro->ro_rt;
if (rt0 != NULL && (rt0->rt_flags & RTF_GATEWAY) != 0)
is_gw = 1;
}
#ifdef MAC
error = mac_ifnet_check_transmit(ifp, m);
if (error)
senderr(error);
#endif
M_PROFILE(m);
if (ifp->if_flags & IFF_MONITOR)
senderr(ENETDOWN);
if (!((ifp->if_flags & IFF_UP) &&
(ifp->if_drv_flags & IFF_DRV_RUNNING)))
senderr(ENETDOWN);
hlen = ETHER_HDR_LEN;
switch (dst->sa_family) {
#ifdef INET
case AF_INET:
if (lle != NULL && (pflags & LLE_VALID) != 0)
memcpy(edst, &lle->ll_addr.mac16, sizeof(edst));
else
error = arpresolve(ifp, is_gw, m, dst, edst, &pflags);
if (error)
return (error == EWOULDBLOCK ? 0 : error);
type = htons(ETHERTYPE_IP);
break;
case AF_ARP:
{
struct arphdr *ah;
ah = mtod(m, struct arphdr *);
ah->ar_hrd = htons(ARPHRD_ETHER);
loop_copy = 0; /* if this is for us, don't do it */
switch(ntohs(ah->ar_op)) {
case ARPOP_REVREQUEST:
case ARPOP_REVREPLY:
type = htons(ETHERTYPE_REVARP);
break;
case ARPOP_REQUEST:
case ARPOP_REPLY:
default:
type = htons(ETHERTYPE_ARP);
break;
}
if (m->m_flags & M_BCAST)
bcopy(ifp->if_broadcastaddr, edst, ETHER_ADDR_LEN);
else
bcopy(ar_tha(ah), edst, ETHER_ADDR_LEN);
}
break;
#endif
#ifdef INET6
case AF_INET6:
if (lle != NULL && (pflags & LLE_VALID))
memcpy(edst, &lle->ll_addr.mac16, sizeof(edst));
else
error = nd6_resolve(ifp, is_gw, m, dst, (u_char *)edst,
&pflags);
if (error)
return (error == EWOULDBLOCK ? 0 : error);
type = htons(ETHERTYPE_IPV6);
break;
#endif
case pseudo_AF_HDRCMPLT:
{
const struct ether_header *eh;
hdrcmplt = 1;
/* FALLTHROUGH */
case AF_UNSPEC:
loop_copy = 0; /* if this is for us, don't do it */
eh = (const struct ether_header *)dst->sa_data;
(void)memcpy(edst, eh->ether_dhost, sizeof (edst));
type = eh->ether_type;
break;
}
default:
if_printf(ifp, "can't handle af%d\n", dst->sa_family);
senderr(EAFNOSUPPORT);
}
if ((pflags & LLE_IFADDR) != 0) {
update_mbuf_csumflags(m, m);
return (if_simloop(ifp, m, dst->sa_family, 0));
}
/*
* Add local net header. If no space in first mbuf,
* allocate another.
*/
M_PREPEND(m, ETHER_HDR_LEN, M_NOWAIT);
if (m == NULL)
senderr(ENOBUFS);
eh = mtod(m, struct ether_header *);
if (hdrcmplt == 0) {
memcpy(&eh->ether_type, &type, sizeof(eh->ether_type));
memcpy(eh->ether_dhost, edst, sizeof (edst));
memcpy(eh->ether_shost, IF_LLADDR(ifp),sizeof(eh->ether_shost));
}
/*
* If a simplex interface, and the packet is being sent to our
* Ethernet address or a broadcast address, loopback a copy.
* XXX To make a simplex device behave exactly like a duplex
* device, we should copy in the case of sending to our own
* ethernet address (thus letting the original actually appear
* on the wire). However, we don't do that here for security
* reasons and compatibility with the original behavior.
*/
if ((ifp->if_flags & IFF_SIMPLEX) && loop_copy &&
((t = pf_find_mtag(m)) == NULL || !t->routed)) {
if (m->m_flags & M_BCAST) {
struct mbuf *n;
/*
* Because if_simloop() modifies the packet, we need a
* writable copy through m_dup() instead of a readonly
* one as m_copy[m] would give us. The alternative would
* be to modify if_simloop() to handle the readonly mbuf,
* but performancewise it is mostly equivalent (trading
* extra data copying vs. extra locking).
*
* XXX This is a local workaround. A number of less
* often used kernel parts suffer from the same bug.
* See PR kern/105943 for a proposed general solution.
*/
if ((n = m_dup(m, M_NOWAIT)) != NULL) {
update_mbuf_csumflags(m, n);
(void)if_simloop(ifp, n, dst->sa_family, hlen);
} else
if_inc_counter(ifp, IFCOUNTER_IQDROPS, 1);
} else if (bcmp(eh->ether_dhost, eh->ether_shost,
ETHER_ADDR_LEN) == 0) {
update_mbuf_csumflags(m, m);
(void) if_simloop(ifp, m, dst->sa_family, hlen);
return (0); /* XXX */
}
}
/*
* Bridges require special output handling.
*/
if (ifp->if_bridge) {
BRIDGE_OUTPUT(ifp, m, error);
return (error);
}
#if defined(INET) || defined(INET6)
if (ifp->if_carp &&
(error = (*carp_output_p)(ifp, m, dst)))
goto bad;
#endif
/* Handle ng_ether(4) processing, if any */
if (ifp->if_l2com != NULL) {
KASSERT(ng_ether_output_p != NULL,
("ng_ether_output_p is NULL"));
if ((error = (*ng_ether_output_p)(ifp, &m)) != 0) {
bad: if (m != NULL)
m_freem(m);
return (error);
}
if (m == NULL)
return (0);
}
/* Continue with link-layer output */
return ether_output_frame(ifp, m);
}
/*
* Receive data on a hook
*
* If data comes in the right link send a copy out right2left, and then
* send the original onwards out through the left link.
* Do the opposite for data coming in from the left link.
* Data coming in right2left or left2right is forwarded
* on through the appropriate destination hook as if it had come
* from the other side.
*/
static int
ngt_rcvdata(hook_p hook, item_p item)
{
const sc_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct hookinfo *const hinfo = NG_HOOK_PRIVATE(hook);
struct hookinfo *dest;
struct hookinfo *dup;
int error = 0;
struct mbuf *m;
meta_p meta;
m = NGI_M(item);
meta = NGI_META(item); /* leave these owned by the item */
/* Which hook? */
if (hinfo == &sc->left) {
dup = &sc->left2right;
dest = &sc->right;
} else if (hinfo == &sc->right) {
dup = &sc->right2left;
dest = &sc->left;
} else if (hinfo == &sc->right2left) {
dup = NULL;
dest = &sc->right;
} else if (hinfo == &sc->left2right) {
dup = NULL;
dest = &sc->left;
} else {
panic("%s: no hook!", __func__);
#ifdef RESTARTABLE_PANICS
return(EINVAL);
#endif
}
/* Update stats on incoming hook */
hinfo->stats.inOctets += m->m_pkthdr.len;
hinfo->stats.inFrames++;
/*
* Don't make a copy if only the dup hook exists.
*/
if ((dup && dup->hook) && (dest->hook == NULL)) {
dest = dup;
dup = NULL;
}
/* Duplicate packet and meta info if requried */
if (dup != NULL) {
struct mbuf *m2;
meta_p meta2;
/* Copy packet (failure will not stop the original)*/
m2 = m_dup(m, M_NOWAIT);
if (m2) {
/* Copy meta info */
/* If we can't get a copy, tough.. */
if (meta != NULL) {
meta2 = ng_copy_meta(meta);
} else
meta2 = NULL;
/* Deliver duplicate */
dup->stats.outOctets += m->m_pkthdr.len;
dup->stats.outFrames++;
NG_SEND_DATA(error, dup->hook, m2, meta2);
}
}
/* Deliver frame out destination hook */
dest->stats.outOctets += m->m_pkthdr.len;
dest->stats.outFrames++;
if (dest->hook)
NG_FWD_ITEM_HOOK(error, item, dest->hook);
else
NG_FREE_ITEM(item);
return (0);
}
/*
* ensure that [off, off + len) is contiguous on the mbuf chain "m".
* packet chain before "off" is kept untouched.
* if offp == NULL, the target will start at <retval, 0> on resulting chain.
* if offp != NULL, the target will start at <retval, *offp> on resulting chain.
*
* on error return (NULL return value), original "m" will be freed.
*
* XXX M_TRAILINGSPACE/M_LEADINGSPACE on shared cluster (sharedcluster)
*/
struct mbuf *
m_pulldown(struct mbuf *m, int off, int len, int *offp)
{
struct mbuf *n, *o;
int hlen, tlen, olen;
int sharedcluster;
/* check invalid arguments. */
if (m == NULL)
panic("m == NULL in m_pulldown()");
if (len > MCLBYTES) {
m_freem(m);
return NULL; /* impossible */
}
n = m;
while (n != NULL && off > 0) {
if (n->m_len > off)
break;
off -= n->m_len;
n = n->m_next;
}
/* be sure to point non-empty mbuf */
while (n != NULL && n->m_len == 0)
n = n->m_next;
if (!n) {
m_freem(m);
return NULL; /* mbuf chain too short */
}
sharedcluster = M_READONLY(n);
/*
* the target data is on <n, off>.
* if we got enough data on the mbuf "n", we're done.
*/
if ((off == 0 || offp) && len <= n->m_len - off && !sharedcluster)
goto ok;
/*
* when len <= n->m_len - off and off != 0, it is a special case.
* len bytes from <n, off> sits in single mbuf, but the caller does
* not like the starting position (off).
* chop the current mbuf into two pieces, set off to 0.
*/
if (len <= n->m_len - off) {
struct mbuf *mlast;
o = m_dup(n, off, n->m_len - off, M_DONTWAIT);
if (o == NULL) {
m_freem(m);
return NULL; /* ENOBUFS */
}
KASSERT(o->m_len >= len);
for (mlast = o; mlast->m_next != NULL; mlast = mlast->m_next)
;
n->m_len = off;
mlast->m_next = n->m_next;
n->m_next = o;
n = o;
off = 0;
goto ok;
}
/*
* we need to take hlen from <n, off> and tlen from <n->m_next, 0>,
* and construct contiguous mbuf with m_len == len.
* note that hlen + tlen == len, and tlen > 0.
*/
hlen = n->m_len - off;
tlen = len - hlen;
/*
* ensure that we have enough trailing data on mbuf chain.
* if not, we can do nothing about the chain.
*/
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; /* mbuf chain too short */
}
/*
* easy cases first.
* we need to use m_copydata() to get data from <n->m_next, 0>.
*/
if ((off == 0 || offp) && M_TRAILINGSPACE(n) >= tlen &&
!sharedcluster) {
m_copydata(n->m_next, 0, tlen, mtod(n, char *) + n->m_len);
n->m_len += tlen;
m_adj(n->m_next, tlen);
goto ok;
}
int
ng_hci_send_command(ng_hci_unit_p unit)
{
struct mbuf *m0 = NULL, *m = NULL;
int free, error = 0;
/* Check if other command is pending */
if (unit->state & NG_HCI_UNIT_COMMAND_PENDING)
return (0);
/* Check if unit can accept our command */
NG_HCI_BUFF_CMD_GET(unit->buffer, free);
if (free == 0)
return (0);
/* Check if driver hook is still ok */
if (unit->drv == NULL || NG_HOOK_NOT_VALID(unit->drv)) {
NG_HCI_WARN(
"%s: %s - hook \"%s\" is not connected or valid\n",
__func__, NG_NODE_NAME(unit->node), NG_HCI_HOOK_DRV);
NG_BT_MBUFQ_DRAIN(&unit->cmdq);
return (ENOTCONN);
}
/*
* Get first command from queue, give it to RAW hook then
* make copy of it and send it to the driver
*/
m0 = NG_BT_MBUFQ_FIRST(&unit->cmdq);
if (m0 == NULL)
return (0);
ng_hci_mtap(unit, m0);
m = m_dup(m0, MB_DONTWAIT);
if (m != NULL)
NG_SEND_DATA_ONLY(error, unit->drv, m);
else
error = ENOBUFS;
if (error != 0)
NG_HCI_ERR(
"%s: %s - could not send HCI command, error=%d\n",
__func__, NG_NODE_NAME(unit->node), error);
/*
* Even if we were not able to send command we still pretend
* that everything is OK and let timeout handle that.
*/
NG_HCI_BUFF_CMD_USE(unit->buffer, 1);
NG_HCI_STAT_CMD_SENT(unit->stat);
NG_HCI_STAT_BYTES_SENT(unit->stat, m0->m_pkthdr.len);
/*
* Note: ng_hci_command_timeout() will set
* NG_HCI_UNIT_COMMAND_PENDING flag
*/
ng_hci_command_timeout(unit);
return (0);
} /* ng_hci_send_command */