errno_t mbuf_align_32(mbuf_t mbuf, size_t len) { if ((mbuf->m_flags & M_EXT) != 0 && m_mclhasreference(mbuf)) return (ENOTSUP); mbuf->m_data = mbuf_datastart(mbuf); mbuf->m_data += ((mbuf_trailingspace(mbuf) - len) &~ (sizeof(u_int32_t) - 1)); return (0); }
static bool mbuf_buffer(IOMemoryDescriptor *buffer, int skip_buffer, mbuf_t m, int skip_mbuf, int copy) { int offset = 0; bool isWrite = (buffer->getDirection() == kIODirectionOut); if (buffer->prepare() != kIOReturnSuccess) { KINFO("buffer prepare failed"); return false; } if (isWrite && mbuf_pkthdr_len(m) < skip_mbuf + copy) mbuf_pkthdr_setlen(m, skip_mbuf + copy); for (; m; m = mbuf_next(m)) { if (isWrite && mbuf_len(m) < skip_mbuf + copy && mbuf_trailingspace(m)) mbuf_setlen(m, min(mbuf_maxlen(m), skip_mbuf + copy)); UInt32 available = mbuf_len(m); //KDEBUG("available=%d, skip_mbuf=%d", available, skip_mbuf); if (skip_mbuf >= available) { skip_mbuf -= available; continue; } UInt8 *buf = (UInt8 *)mbuf_data(m) + skip_mbuf; IOByteCount len = copy; // remaining requested len = min(len, available - skip_mbuf); // available in mbuf len = min(len, buffer->getLength() - offset); // available in iomd IOByteCount wrote = 0; if (!len) { KDEBUG("no space, %d-%d, %d-%d", available, skip_mbuf, buffer->getLength(), offset); break; } //KDEBUG("COPY: skip_buffer=%d, offset=%d, len=%d (remaining=%d)", skip_buffer, offset, len, copy); if (isWrite) wrote = buffer->readBytes(skip_buffer + offset, buf, len); else wrote = buffer->writeBytes(skip_buffer + offset, buf, len); if (wrote != len) { KINFO("short IO"); break; } offset += len; copy -= len; skip_mbuf = 0; } if (buffer->complete() != kIOReturnSuccess) { KINFO("buffer complete failed"); return false; } if (copy > 0) { KINFO("failed to copy requested data: %d remaining", copy); return false; } return true; }
errno_t firewire_inet_arp( ifnet_t ifp, u_short arpop, const struct sockaddr_dl* sender_hw, const struct sockaddr* sender_proto, const struct sockaddr_dl* target_hw, const struct sockaddr* target_proto) { mbuf_t m; errno_t result; register struct firewire_header *fwh; register IP1394_ARP *fwa; const struct sockaddr_in* sender_ip = (const struct sockaddr_in*)sender_proto; const struct sockaddr_in* target_ip = (const struct sockaddr_in*)target_proto; char *datap; IOFWInterface *fwIf = (IOFWInterface*)ifnet_softc(ifp); if(fwIf == NULL) return EINVAL; IOFireWireIP *fwIpObj = (IOFireWireIP*)fwIf->getController(); if(fwIpObj == NULL) return EINVAL; LCB *lcb = fwIpObj->getLcb(); if (target_ip == NULL) return EINVAL; if ((sender_ip && sender_ip->sin_family != AF_INET) || (target_ip && target_ip->sin_family != AF_INET)) return EAFNOSUPPORT; result = mbuf_gethdr(MBUF_DONTWAIT, MBUF_TYPE_DATA, &m); if (result != 0) return result; mbuf_setlen(m, sizeof(*fwa)); mbuf_pkthdr_setlen(m, sizeof(*fwa)); /* Move the data pointer in the mbuf to the end, aligned to 4 bytes */ datap = (char*)mbuf_datastart(m); datap += mbuf_trailingspace(m); datap -= (((u_long)datap) & 0x3); mbuf_setdata(m, datap, sizeof(*fwa)); fwa = (IP1394_ARP*)mbuf_data(m); bzero((caddr_t)fwa, sizeof(*fwa)); /* Prepend the ethernet header, we will send the raw frame */ result = mbuf_prepend(&m, sizeof(*fwh), MBUF_DONTWAIT); if(result != 0) return result; fwh = (struct firewire_header*)mbuf_data(m); fwh->fw_type = htons(FWTYPE_ARP); /* Fill out the arp packet */ fwa->hardwareType = htons(ARP_HDW_TYPE); fwa->protocolType = htons(FWTYPE_IP); fwa->hwAddrLen = sizeof(IP1394_HDW_ADDR); fwa->ipAddrLen = IPV4_ADDR_SIZE; fwa->opcode = htons(arpop); fwa->senderMaxRec = lcb->ownHardwareAddress.maxRec; fwa->sspd = lcb->ownHardwareAddress.spd; fwa->senderUnicastFifoHi = htons(lcb->ownHardwareAddress.unicastFifoHi); fwa->senderUnicastFifoLo = htonl(lcb->ownHardwareAddress.unicastFifoLo); /* Sender Hardware */ if (sender_hw != NULL) bcopy(CONST_LLADDR(sender_hw), &fwa->senderUniqueID, sizeof(fwa->senderUniqueID)); else ifnet_lladdr_copy_bytes(ifp, &fwa->senderUniqueID, FIREWIRE_ADDR_LEN); ifnet_lladdr_copy_bytes(ifp, fwh->fw_shost, sizeof(fwh->fw_shost)); /* Sender IP */ if (sender_ip != NULL) fwa->senderIpAddress = sender_ip->sin_addr.s_addr; else { ifaddr_t *addresses; struct sockaddr sa; if (ifnet_get_address_list_family(ifp, &addresses, AF_INET) == 0) { ifaddr_address( addresses[0], &sa, 16 ); fwa->senderIpAddress = ((UInt32)(sa.sa_data[5] & 0xFF)) << 24; fwa->senderIpAddress |= ((UInt32)(sa.sa_data[4] & 0xFF)) << 16; fwa->senderIpAddress |= ((UInt32)(sa.sa_data[3] & 0xFF)) << 8; fwa->senderIpAddress |= ((UInt32)(sa.sa_data[2] & 0xFF)); ifnet_free_address_list(addresses); } else { mbuf_free(m); return ENXIO; } } /* Target Hardware */ if (target_hw == 0) bcopy(fwbroadcastaddr, fwh->fw_dhost, sizeof(fwh->fw_dhost)); else bcopy(CONST_LLADDR(target_hw), fwh->fw_dhost, sizeof(fwh->fw_dhost)); /* Target IP */ fwa->targetIpAddress = target_ip->sin_addr.s_addr; ifnet_output_raw(ifp, PF_INET, m); return 0; }
/* * mbuf_copyback differs from m_copyback in a few ways: * 1) mbuf_copyback will allocate clusters for new mbufs we append * 2) mbuf_copyback will grow the last mbuf in the chain if possible * 3) mbuf_copyback reports whether or not the operation succeeded * 4) mbuf_copyback allows the caller to specify M_WAITOK or M_NOWAIT */ errno_t mbuf_copyback( mbuf_t m, size_t off, size_t len, const void *data, mbuf_how_t how) { size_t mlen; mbuf_t m_start = m; mbuf_t n; int totlen = 0; errno_t result = 0; const char *cp = data; if (m == NULL || len == 0 || data == NULL) return (EINVAL); while (off > (mlen = m->m_len)) { off -= mlen; totlen += mlen; if (m->m_next == 0) { n = m_getclr(how, m->m_type); if (n == 0) { result = ENOBUFS; goto out; } n->m_len = MIN(MLEN, len + off); m->m_next = n; } m = m->m_next; } while (len > 0) { mlen = MIN(m->m_len - off, len); if (mlen < len && m->m_next == NULL && mbuf_trailingspace(m) > 0) { size_t grow = MIN(mbuf_trailingspace(m), len - mlen); mlen += grow; m->m_len += grow; } bcopy(cp, off + (char *)mbuf_data(m), (unsigned)mlen); cp += mlen; len -= mlen; mlen += off; off = 0; totlen += mlen; if (len == 0) break; if (m->m_next == 0) { n = m_get(how, m->m_type); if (n == NULL) { result = ENOBUFS; goto out; } if (len > MINCLSIZE) { /* * cluster allocation failure is okay, * we can grow chain */ mbuf_mclget(how, m->m_type, &n); } n->m_len = MIN(mbuf_maxlen(n), len); m->m_next = n; } m = m->m_next; } out: if ((m_start->m_flags & M_PKTHDR) && (m_start->m_pkthdr.len < totlen)) m_start->m_pkthdr.len = totlen; return (result); }