/*
* Help break down an mbuf chain by setting the first siz bytes contiguous
* pointed to by returned val.
* This is used by the macro NFSM_DISSECT for tough
* cases.
*/
APPLESTATIC void *
nfsm_dissct(struct nfsrv_descript *nd, int siz, int how)
{
mbuf_t mp2;
int siz2, xfer;
caddr_t p;
int left;
caddr_t retp;
retp = NULL;
left = NFSMTOD(nd->nd_md, caddr_t) + mbuf_len(nd->nd_md) - nd->nd_dpos;
while (left == 0) {
nd->nd_md = mbuf_next(nd->nd_md);
if (nd->nd_md == NULL)
return (retp);
left = mbuf_len(nd->nd_md);
nd->nd_dpos = NFSMTOD(nd->nd_md, caddr_t);
}
if (left >= siz) {
retp = nd->nd_dpos;
nd->nd_dpos += siz;
} else if (mbuf_next(nd->nd_md) == NULL) {
return (retp);
} else if (siz > ncl_mbuf_mhlen) {
panic("nfs S too big");
} else {
MGET(mp2, MT_DATA, how);
if (mp2 == NULL)
return (NULL);
mbuf_setnext(mp2, mbuf_next(nd->nd_md));
mbuf_setnext(nd->nd_md, mp2);
mbuf_setlen(nd->nd_md, mbuf_len(nd->nd_md) - left);
nd->nd_md = mp2;
retp = p = NFSMTOD(mp2, caddr_t);
NFSBCOPY(nd->nd_dpos, p, left); /* Copy what was left */
siz2 = siz - left;
p += left;
mp2 = mbuf_next(mp2);
/* Loop around copying up the siz2 bytes */
while (siz2 > 0) {
if (mp2 == NULL)
return (NULL);
xfer = (siz2 > mbuf_len(mp2)) ? mbuf_len(mp2) : siz2;
if (xfer > 0) {
NFSBCOPY(NFSMTOD(mp2, caddr_t), p, xfer);
NFSM_DATAP(mp2, xfer);
mbuf_setlen(mp2, mbuf_len(mp2) - xfer);
p += xfer;
siz2 -= xfer;
}
if (siz2 > 0)
mp2 = mbuf_next(mp2);
}
mbuf_setlen(nd->nd_md, siz);
nd->nd_md = mp2;
nd->nd_dpos = NFSMTOD(mp2, caddr_t);
}
return (retp);
}
/*
* Copy a string into mbuf(s).
* Return the number of bytes output, including XDR overheads.
*/
APPLESTATIC int
nfsm_strtom(struct nfsrv_descript *nd, const char *cp, int siz)
{
mbuf_t m2;
int xfer, left;
mbuf_t m1;
int rem, bytesize;
u_int32_t *tl;
char *cp2;
NFSM_BUILD(tl, u_int32_t *, NFSX_UNSIGNED);
*tl = txdr_unsigned(siz);
rem = NFSM_RNDUP(siz) - siz;
bytesize = NFSX_UNSIGNED + siz + rem;
m2 = nd->nd_mb;
cp2 = nd->nd_bpos;
left = M_TRAILINGSPACE(m2);
/*
* Loop around copying the string to mbuf(s).
*/
while (siz > 0) {
if (left == 0) {
if (siz > ncl_mbuf_mlen)
NFSMCLGET(m1, M_WAITOK);
else
NFSMGET(m1);
mbuf_setlen(m1, 0);
mbuf_setnext(m2, m1);
m2 = m1;
cp2 = NFSMTOD(m2, caddr_t);
left = M_TRAILINGSPACE(m2);
}
if (left >= siz)
xfer = siz;
else
xfer = left;
NFSBCOPY(cp, cp2, xfer);
cp += xfer;
mbuf_setlen(m2, mbuf_len(m2) + xfer);
siz -= xfer;
left -= xfer;
if (siz == 0 && rem) {
if (left < rem)
panic("nfsm_strtom");
NFSBZERO(cp2 + xfer, rem);
mbuf_setlen(m2, mbuf_len(m2) + rem);
}
}
nd->nd_mb = m2;
nd->nd_bpos = NFSMTOD(m2, caddr_t) + mbuf_len(m2);
return (bytesize);
}
/*
* copies a uio scatter/gather list to an mbuf chain.
* NOTE: can ony handle iovcnt == 1
*/
APPLESTATIC void
nfsm_uiombuf(struct nfsrv_descript *nd, struct uio *uiop, int siz)
{
char *uiocp;
struct mbuf *mp, *mp2;
int xfer, left, mlen;
int uiosiz, clflg, rem;
char *cp, *tcp;
KASSERT(uiop->uio_iovcnt == 1, ("nfsm_uiotombuf: iovcnt != 1"));
if (siz > ncl_mbuf_mlen) /* or should it >= MCLBYTES ?? */
clflg = 1;
else
clflg = 0;
rem = NFSM_RNDUP(siz) - siz;
mp = mp2 = nd->nd_mb;
while (siz > 0) {
left = uiop->uio_iov->iov_len;
uiocp = uiop->uio_iov->iov_base;
if (left > siz)
left = siz;
uiosiz = left;
while (left > 0) {
mlen = M_TRAILINGSPACE(mp);
if (mlen == 0) {
if (clflg)
NFSMCLGET(mp, M_WAITOK);
else
NFSMGET(mp);
mbuf_setlen(mp, 0);
mbuf_setnext(mp2, mp);
mp2 = mp;
mlen = M_TRAILINGSPACE(mp);
}
xfer = (left > mlen) ? mlen : left;
#ifdef notdef
/* Not Yet.. */
if (uiop->uio_iov->iov_op != NULL)
(*(uiop->uio_iov->iov_op))
(uiocp, NFSMTOD(mp, caddr_t) + mbuf_len(mp),
xfer);
else
#endif
if (uiop->uio_segflg == UIO_SYSSPACE)
NFSBCOPY(uiocp, NFSMTOD(mp, caddr_t) + mbuf_len(mp),
xfer);
else
copyin(CAST_USER_ADDR_T(uiocp), NFSMTOD(mp, caddr_t)
+ mbuf_len(mp), xfer);
mbuf_setlen(mp, mbuf_len(mp) + xfer);
left -= xfer;
uiocp += xfer;
uiop->uio_offset += xfer;
uiop->uio_resid -= xfer;
}
tcp = (char *)uiop->uio_iov->iov_base;
tcp += uiosiz;
uiop->uio_iov->iov_base = (void *)tcp;
uiop->uio_iov->iov_len -= uiosiz;
siz -= uiosiz;
}
if (rem > 0) {
if (rem > M_TRAILINGSPACE(mp)) {
NFSMGET(mp);
mbuf_setlen(mp, 0);
mbuf_setnext(mp2, mp);
}
cp = NFSMTOD(mp, caddr_t) + mbuf_len(mp);
for (left = 0; left < rem; left++)
*cp++ = '\0';
mbuf_setlen(mp, mbuf_len(mp) + rem);
nd->nd_bpos = cp;
} else
nd->nd_bpos = NFSMTOD(mp, caddr_t) + mbuf_len(mp);
nd->nd_mb = mp;
}
/*
* copies a uio scatter/gather list to an mbuf chain.
* This version returns the mbuf list and does not use "nd".
* NOTE: can ony handle iovcnt == 1
*/
struct mbuf *
nfsm_uiombuflist(struct uio *uiop, int siz, struct mbuf **mbp, char **cpp)
{
char *uiocp;
struct mbuf *mp, *mp2, *firstmp;
int xfer, left, mlen;
int uiosiz, clflg;
char *tcp;
KASSERT(uiop->uio_iovcnt == 1, ("nfsm_uiotombuf: iovcnt != 1"));
if (siz > ncl_mbuf_mlen) /* or should it >= MCLBYTES ?? */
clflg = 1;
else
clflg = 0;
if (clflg != 0)
NFSMCLGET(mp, M_WAITOK);
else
NFSMGET(mp);
mbuf_setlen(mp, 0);
firstmp = mp2 = mp;
while (siz > 0) {
left = uiop->uio_iov->iov_len;
uiocp = uiop->uio_iov->iov_base;
if (left > siz)
left = siz;
uiosiz = left;
while (left > 0) {
mlen = M_TRAILINGSPACE(mp);
if (mlen == 0) {
if (clflg)
NFSMCLGET(mp, M_WAITOK);
else
NFSMGET(mp);
mbuf_setlen(mp, 0);
mbuf_setnext(mp2, mp);
mp2 = mp;
mlen = M_TRAILINGSPACE(mp);
}
xfer = (left > mlen) ? mlen : left;
if (uiop->uio_segflg == UIO_SYSSPACE)
NFSBCOPY(uiocp, NFSMTOD(mp, caddr_t) +
mbuf_len(mp), xfer);
else
copyin(uiocp, NFSMTOD(mp, caddr_t) +
mbuf_len(mp), xfer);
mbuf_setlen(mp, mbuf_len(mp) + xfer);
left -= xfer;
uiocp += xfer;
uiop->uio_offset += xfer;
uiop->uio_resid -= xfer;
}
tcp = (char *)uiop->uio_iov->iov_base;
tcp += uiosiz;
uiop->uio_iov->iov_base = (void *)tcp;
uiop->uio_iov->iov_len -= uiosiz;
siz -= uiosiz;
}
if (cpp != NULL)
*cpp = NFSMTOD(mp, caddr_t) + mbuf_len(mp);
if (mbp != NULL)
*mbp = mp;
return (firstmp);
}
// Temporarily stuff a vlan tag back into a packet so that tag shows up to bpf.
// We do it by creating a temp header mbuf with the enet/vlan header in it and
// then point its next field to the proper place (after the dest+src addresses) in the original
// mbuf.
void IOEthernetInterface::_fixupVlanPacket(mbuf_t mt, u_int16_t vlan_tag, int inputPacket)
{
mbuf_t newmb;
mbuf_t chain;
size_t remainingBytes;
size_t copyBytes = 0; //initialize to prevent annoying, incorrect warning that it's used uninitialized
char * destptr;
if( mbuf_gethdr(MBUF_DONTWAIT, MT_DATA, &newmb) )
return;
//init enough of the mbuf to keep bpf happy
mbuf_setlen(newmb, ETHER_ADDR_LEN*2 + VLAN_HEADER_LEN);
mbuf_pkthdr_setlen(newmb, mbuf_pkthdr_len( mt ) + VLAN_HEADER_LEN);
mbuf_pkthdr_setrcvif(newmb, mbuf_pkthdr_rcvif( mt ) );
//now walk the incoming mbuf to copy out its dst & src address and
//locate the type/len field in the packet.
chain = mt;
remainingBytes = ETHER_ADDR_LEN*2;
destptr = (char *)mbuf_data( newmb );
while(chain && remainingBytes)
{
copyBytes = remainingBytes > mbuf_len( chain ) ? mbuf_len( chain ): remainingBytes;
remainingBytes -= copyBytes;
bcopy( mbuf_data( chain ), destptr, copyBytes);
destptr += copyBytes;
if (mbuf_len( chain ) == copyBytes) //we've completely drained this mbuf
{
chain = mbuf_next( chain ); //advance to next
copyBytes = 0; //if we break out of loop now, make sure the offset is correct
}
}
// chain points to the mbuf that contains the packet data with type/len field
// and copyBytes indicates the offset it's at.
if(chain==0 || remainingBytes)
{
mbuf_freem( newmb );
return; //if we can't munge the packet, just return
}
//patch mbuf so its data points after the dst+src address
mbuf_setdata(chain, (char *)mbuf_data( chain ) + copyBytes, mbuf_len( chain ) - copyBytes );
//finish setting up our head mbuf
*(short *)(destptr) = htons(ETHERTYPE_VLAN); //vlan magic number
*(short *)(destptr + 2) = htons( vlan_tag ); // and the tag's value
mbuf_setnext( newmb, chain ); //stick it infront of the rest of the packet
// feed the tap
if(inputPacket)
super::feedPacketInputTap( newmb );
else
super::feedPacketOutputTap( newmb );
//release the fake header
mbuf_setnext( newmb, NULL );
mbuf_freem( newmb );
//and repair our old mbuf
mbuf_setdata( chain, (char *)mbuf_data( chain ) - copyBytes, mbuf_len( chain ) + copyBytes );
}
UInt32 IOMbufMemoryCursor::genPhysicalSegments(mbuf_t packet, void *vector,
UInt32 maxSegs, bool doCoalesce)
{
bool doneCoalesce = false;
if (!packet || !(mbuf_flags(packet) & MBUF_PKTHDR))
return 0;
if (!maxSegs)
{
maxSegs = maxNumSegments;
if (!maxSegs) return 0;
}
if ( mbuf_next(packet) == 0 )
{
uintptr_t src;
struct IOPhysicalSegment physSeg;
/*
* the packet consists of only 1 mbuf
* so if the data buffer doesn't span a page boundary
* we can take the simple way out
*/
src = (uintptr_t)mbuf_data(packet);
if ( trunc_page(src) == trunc_page(src + mbuf_len(packet) - 1) )
{
physSeg.location = (IOPhysicalAddress) mbuf_data_to_physical((char *)src);
if ( physSeg.location )
{
physSeg.length = mbuf_len(packet);
(*outSeg)(physSeg, vector, 0);
return 1;
}
maxSegs = 1;
if ( doCoalesce == false ) return 0;
}
}
if ( doCoalesce == true && maxSegs == 1 )
{
uintptr_t src;
uintptr_t dst;
mbuf_t m;
mbuf_t mnext;
mbuf_t out;
UInt32 len = 0;
struct IOPhysicalSegment physSeg;
if ( mbuf_pkthdr_len(packet) > MCLBYTES ) return 0;
m = packet;
// Allocate a non-header mbuf + cluster.
if (mbuf_getpacket( MBUF_DONTWAIT, &out ))
return 0;
mbuf_setflags( out, mbuf_flags( out ) & ~MBUF_PKTHDR );
dst = (uintptr_t)mbuf_data(out);
do
{
src = (uintptr_t)mbuf_data(m);
BCOPY( src, dst, mbuf_len(m) );
dst += mbuf_len(m);
len += mbuf_len(m);
} while ( (m = mbuf_next(m)) != 0 );
mbuf_setlen(out , len);
dst = (uintptr_t)mbuf_data(out);
physSeg.location = (IOPhysicalAddress) mbuf_data_to_physical((char *)dst);
if (!physSeg.location)
{
mbuf_free(out);
return 0;
}
physSeg.length = mbuf_len(out);
(*outSeg)(physSeg, vector, 0);
m = mbuf_next(packet);
while (m != 0)
{
mnext = mbuf_next(m);
mbuf_free(m);
m = mnext;
}
// The initial header mbuf is preserved, its length set to zero,
// and linked to the new packet chain.
mbuf_setlen(packet , 0);
mbuf_setnext(packet , out);
mbuf_setnext(out , 0);
return 1;
}
//
// Iterate over the mbuf, translating segments were allowed. When we
// are not allowed to translate segments then accumulate segment
// statistics up to kMBufDataCacheSize of mbufs. Finally
// if we overflow our cache just count how many segments this
// packet represents.
//
UInt32 segsPerMBuf[kMBufDataCacheSize];
tryAgain:
UInt32 curMBufIndex = 0;
UInt32 curSegIndex = 0;
UInt32 lastSegCount = 0;
mbuf_t m = packet;
// For each mbuf in incoming packet.
do {
vm_size_t mbufLen, thisLen = 0;
uintptr_t src;
// Step through each segment in the current mbuf
for (mbufLen = mbuf_len(m), src = (uintptr_t)mbuf_data(m);
mbufLen;
src += thisLen, mbufLen -= thisLen)
{
// If maxSegmentSize is atleast PAGE_SIZE, then
// thisLen = MIN(next_page(src), src + mbufLen) - src;
thisLen = MIN(mbufLen, maxSegmentSize);
thisLen = MIN(next_page(src), src + thisLen) - src;
// If room left then find the current segment addr and output
if (curSegIndex < maxSegs) {
struct IOPhysicalSegment physSeg;
physSeg.location = (IOPhysicalAddress) mbuf_data_to_physical((char *)src);
if ( physSeg.location == 0 )
{
return doCoalesce ?
genPhysicalSegments(packet, vector, 1, true) : 0;
}
physSeg.length = thisLen;
(*outSeg)(physSeg, vector, curSegIndex);
}
// Count segments if we are coalescing.
curSegIndex++;
}
// Cache the segment count data if room is available.
if (curMBufIndex < kMBufDataCacheSize) {
segsPerMBuf[curMBufIndex] = curSegIndex - lastSegCount;
lastSegCount = curSegIndex;
}
// Move on to next imcoming mbuf
curMBufIndex++;
m = mbuf_next(m);
} while (m);
// If we finished cleanly return number of segments found
if (curSegIndex <= maxSegs)
return curSegIndex;
if (!doCoalesce)
return 0; // if !coalescing we've got a problem.
// If we are coalescing and it is possible then attempt coalesce,
if (!doneCoalesce
&& (UInt) mbuf_pkthdr_len(packet) <= maxSegs * maxSegmentSize) {
// Hmm, we have to do some coalescing.
bool analysisRet;
analysisRet = analyseSegments(packet,
MIN(curMBufIndex, kMBufDataCacheSize),
segsPerMBuf,
curSegIndex, maxSegs);
if (analysisRet) {
doneCoalesce = true;
coalesceCount++;
goto tryAgain;
}
}
assert(!doneCoalesce); // Problem in Coalesce code.
packetTooBigErrors++;
return 0;
}
static inline bool analyseSegments(
mbuf_t packet, /* input packet mbuf */
const UInt32 mbufsInCache, /* number of entries in segsPerMBuf[] */
const UInt32 segsPerMBuf[], /* segments required per mbuf */
SInt32 numSegs, /* total number of segments */
const UInt32 maxSegs) /* max controller segments per mbuf */
{
mbuf_t newPacket; // output mbuf chain.
mbuf_t out; // current output mbuf link.
SInt32 outSize; // size of current output mbuf link.
SInt32 outSegs; // segments for current output mbuf link.
SInt32 doneSegs; // segments for output mbuf chain.
SInt32 outLen; // remaining length of input buffer.
mbuf_t in = packet; // save the original input packet pointer.
UInt32 inIndex = 0;
const uint32_t c_mlen = mbuf_get_mlen();
// Allocate a mbuf (non header mbuf) to begin the output mbuf chain.
if(mbuf_get(MBUF_DONTWAIT, MT_DATA, &newPacket))
{
ERROR_LOG("analyseSegments: MGET() 1 error\n");
return false;
}
/* Initialise outgoing packet controls */
out = newPacket;
outSize = c_mlen;
doneSegs = outSegs = outLen = 0;
// numSegs stores the delta between the total and the max. For each
// input mbuf consumed, we decrement numSegs.
//
numSegs -= maxSegs;
// Loop through the input packet mbuf 'in' and construct a new mbuf chain
// large enough to make (numSegs + doneSegs + outSegs) less than or
// equal to zero.
//
do {
uintptr_t vmo;
outLen += mbuf_len(in);
while (outLen > outSize) {
// Oh dear the current outgoing length is too big.
if (outSize != MCLBYTES) {
// Current mbuf is not yet a cluster so promote, then
// check for error.
if(mbuf_mclget(MBUF_DONTWAIT, MT_DATA, &out) || !(mbuf_flags(out) & MBUF_EXT) )
{
ERROR_LOG("analyseSegments: MCLGET() error\n");
goto bombAnalysis;
}
outSize = MCLBYTES;
continue;
}
vmo = (uintptr_t)mbuf_data(out);
mbuf_setlen(out, MCLBYTES); /* Fill in target copy size */
doneSegs += (round_page(vmo + MCLBYTES) - trunc_page(vmo))
/ PAGE_SIZE;
// If the number of segments of the output chain, plus
// the segment for the mbuf we are about to allocate is greater
// than maxSegs, then abort.
//
if (doneSegs + 1 > (int) maxSegs) {
ERROR_LOG("analyseSegments: maxSegs limit 1 reached! %ld %ld\n",
doneSegs, maxSegs);
goto bombAnalysis;
}
mbuf_t tempmbuf;
if(mbuf_get(MBUF_DONTWAIT, MT_DATA, &tempmbuf))
{
ERROR_LOG("analyseSegments: MGET() error\n");
goto bombAnalysis;
}
mbuf_setnext(out, tempmbuf);
out = tempmbuf;
outSize = c_mlen;
outLen -= MCLBYTES;
}
// Compute number of segment in current outgoing mbuf.
vmo = (uintptr_t)mbuf_data(out);
outSegs = ((SInt32)round_page(vmo + outLen) - (SInt32)trunc_page(vmo)) / (SInt32)PAGE_SIZE;
if (doneSegs + outSegs > (int) maxSegs) {
ERROR_LOG("analyseSegments: maxSegs limit 2 reached! %ld %ld %ld\n",
doneSegs, outSegs, maxSegs);
goto bombAnalysis;
}
// Get the number of segments in the current inbuf
if (inIndex < mbufsInCache)
numSegs -= segsPerMBuf[inIndex]; // Yeah, in cache
else {
// Hmm, we have to recompute from scratch. Copy code from genPhys.
int thisLen = 0, mbufLen;
vmo = (uintptr_t)mbuf_data(in);
for (mbufLen = (SInt32)mbuf_len(in); mbufLen; mbufLen -= thisLen) {
thisLen = MIN((SInt32)next_page(vmo), (SInt32)(vmo + mbufLen)) - (SInt32)vmo;
vmo += thisLen;
numSegs--;
}
}
// Walk the incoming buffer on one.
in = mbuf_next(in);
inIndex++;
// continue looping until the total number of segments has dropped
// to an acceptable level, or if we ran out of mbuf links.
} while (in && ((numSegs + doneSegs + outSegs) > 0));
if ( (int) (numSegs + doneSegs + outSegs) <= 0) { // success
mbuf_setlen(out, outLen); // Set last mbuf with the remaining length.
// The amount to copy is determine by the segment length in each
// mbuf linked to newPacket. The sum can be smaller than
// packet->pkthdr.len;
//
coalesceSegments(packet, newPacket);
// The initial header mbuf is preserved, its length set to zero, and
// linked to the new packet chain.
// coalesceSegments() has already freed the mbufs that it coalesced into the newPacket chain.
// It also hooked the remaining chain pointed to by "in" to the end of the newPacket chain.
// All that remains is to set packet's len to 0 (to "free" the contents that coalesceSegments copied out)
// and make it the head of the new chain.
mbuf_setlen(packet , 0 );
mbuf_setnext(packet, newPacket);
return true;
}
bombAnalysis:
mbuf_freem(newPacket);
return false;
}
static inline void coalesceSegments(mbuf_t srcm, mbuf_t dstm)
{
uintptr_t src, dst;
SInt32 srcLen, dstLen;
mbuf_t temp;
mbuf_t head = srcm;
srcLen = (SInt32)mbuf_len( srcm );
src = (uintptr_t) mbuf_data(srcm);
dstLen = (SInt32)mbuf_len( dstm );
dst = (uintptr_t) mbuf_data( dstm );
for (;;) {
if (srcLen < dstLen) {
// Copy remainder of src mbuf to current dst.
BCOPY(src, dst, srcLen);
dst += srcLen;
dstLen -= srcLen;
// Move on to the next source mbuf.
temp = mbuf_next( srcm ); assert(temp);
if(srcm != head)
mbuf_free(srcm);
srcm = temp;
srcLen = (SInt32)mbuf_len( srcm );
src = (uintptr_t)mbuf_data(srcm);
}
else if (srcLen > dstLen) {
// Copy some of src mbuf to remaining space in dst mbuf.
BCOPY(src, dst, dstLen);
src += dstLen;
srcLen -= dstLen;
// Move on to the next destination mbuf.
temp = mbuf_next( dstm ); assert(temp);
dstm = temp;
dstLen = (SInt32)mbuf_len( dstm );
dst = (uintptr_t)mbuf_data( dstm );
}
else { /* (srcLen == dstLen) */
// copy remainder of src into remaining space of current dst
BCOPY(src, dst, srcLen);
// Free current mbuf and move the current onto the next
temp = mbuf_next( srcm );
if(srcm != head)
mbuf_free(srcm);
srcm = temp;
// Do we have any more dest buffers to copy to?
if (! mbuf_next ( dstm ))
{
// nope- hook the remainder of source chain to end of dest chain
mbuf_setnext(dstm, srcm);
break;
}
dstm = mbuf_next ( dstm );
assert(srcm);
dstLen = (SInt32)mbuf_len ( dstm );
dst = (uintptr_t)mbuf_data( dstm );
srcLen = (SInt32)mbuf_len( srcm );
src = (uintptr_t)mbuf_data( srcm );
}
}
}
