C++ MGETHDR函数代码示例

unsigned char* Ecos_MemPool_Alloc (
	unsigned long Length,
	int Type)
{
    struct mbuf *pMBuf = NULL;

	switch (Type)
	{        
                case MemPool_TYPE_Header:
                        MGETHDR(pMBuf, M_DONTWAIT, MT_DATA);
                        break;
		case MemPool_TYPE_CLUSTER:
			MGETHDR(pMBuf, M_DONTWAIT, MT_DATA);
			if (pMBuf== NULL)
				return NULL;
			MCLGET(pMBuf, M_DONTWAIT);
			if ((pMBuf->m_flags & M_EXT) == 0)
                        {
				m_freem(pMBuf);
                                return NULL;
                        }
                        break;
                default:
                        DBGPRINT(RT_DEBUG_ERROR, ("%s: Unknown Type %d\n", __FUNCTION__, Type));
                        break;
	}
    
    return pMBuf;
}

static void
sscfu_send_lower(struct sscfu *sscf, void *p, enum sscop_aasig sig,
    struct mbuf *m, u_int arg)
{
	node_p node = (node_p)p;
	struct priv *priv = NG_NODE_PRIVATE(node);
	int error;
	struct sscop_arg *a;

	if (priv->lower == NULL) {
		if (m != NULL)
			m_freem(m);
		return;
	}
	if (m == NULL) {
		MGETHDR(m, M_NOWAIT, MT_DATA);
		if (m == NULL)
			return;
		m->m_len = sizeof(struct sscop_arg);
		m->m_pkthdr.len = m->m_len;
	} else {
		M_PREPEND(m, sizeof(struct sscop_arg), M_NOWAIT);
		if (m == NULL)
			return;
	}
	a = mtod(m, struct sscop_arg *);
	a->sig = sig;
	a->arg = arg;

	NG_SEND_DATA_ONLY(error, priv->lower, m);
}

/*
 * Partition an mbuf chain in two pieces, returning the tail --
 * all but the first len0 bytes.  In case of failure, it returns NULL and
 * attempts to restore the chain to its original state.
 */
struct mbuf *
m_split(struct mbuf *m0, int len0, int wait)
{
	struct mbuf *m, *n;
	unsigned len = len0, remain, olen;

	for (m = m0; m && len > m->m_len; m = m->m_next)
		len -= m->m_len;
	if (m == NULL)
		return (NULL);
	remain = m->m_len - len;
	if (m0->m_flags & M_PKTHDR) {
		MGETHDR(n, wait, m0->m_type);
		if (n == NULL)
			return (NULL);
		if (m_dup_pkthdr(n, m0, wait)) {
			m_freem(n);
			return (NULL);
		}
		n->m_pkthdr.len -= len0;
		olen = m0->m_pkthdr.len;
		m0->m_pkthdr.len = len0;
		if (m->m_flags & M_EXT)
			goto extpacket;
		if (remain > MHLEN) {
			/* m can't be the lead packet */
			MH_ALIGN(n, 0);
			n->m_next = m_split(m, len, wait);
			if (n->m_next == NULL) {
				(void) m_free(n);
				m0->m_pkthdr.len = olen;
				return (NULL);
			} else
				return (n);
		} else
			MH_ALIGN(n, remain);
	} else if (remain == 0) {
		n = m->m_next;
		m->m_next = NULL;
		return (n);
	} else {
		MGET(n, wait, m->m_type);
		if (n == NULL)
			return (NULL);
		M_ALIGN(n, remain);
	}
extpacket:
	if (m->m_flags & M_EXT) {
		n->m_ext = m->m_ext;
		MCLADDREFERENCE(m, n);
		n->m_data = m->m_data + len;
	} else {
		memcpy(mtod(n, caddr_t), mtod(m, caddr_t) + len, remain);
	}
	n->m_len = remain;
	m->m_len = len;
	n->m_next = m->m_next;
	m->m_next = NULL;
	return (n);
}

static void virtio_net_tx_lazy(struct vmm_netport *port, void *arg, int budget)
{
	u16 head = 0;
	u32 iov_cnt = 0, pkt_len = 0, total_len = 0;
	struct virtio_net_dev *ndev = arg;
	struct virtio_device *dev = ndev->vdev;
	struct virtio_queue *vq = &ndev->vqs[VIRTIO_NET_TX_QUEUE];
	struct virtio_iovec *iov = ndev->tx_iov;
	struct vmm_mbuf *mb;

	while ((budget > 0) && virtio_queue_available(vq)) {
		head = virtio_queue_get_iovec(vq, iov, &iov_cnt, &total_len);

		/* iov[0] is offload info */
		pkt_len = total_len - iov[0].len;

		if (pkt_len <= VIRTIO_NET_MTU) {
			MGETHDR(mb, 0, 0);
			MEXTMALLOC(mb, pkt_len, M_WAIT);
			virtio_iovec_to_buf_read(dev, 
						 &iov[1], iov_cnt - 1,
						 M_BUFADDR(mb), pkt_len);
			mb->m_len = mb->m_pktlen = pkt_len;
			vmm_port2switch_xfer_mbuf(ndev->port, mb);
		}

		virtio_queue_set_used_elem(vq, head, total_len);

		budget--;
	}

	if (virtio_queue_should_signal(vq)) {
		dev->tra->notify(dev, VIRTIO_NET_TX_QUEUE);
	}
}

static void
sscop_send_manage(struct sscop *sscop, void *p, enum sscop_maasig sig,
    struct SSCOP_MBUF_T *m, u_int err, u_int cnt)
{
	node_p node = (node_p)p;
	struct priv *priv = NG_NODE_PRIVATE(node);
	int error;
	struct sscop_merr *e;
	struct sscop_marg *a;

	if (priv->manage == NULL) {
		if (m != NULL)
			m_freem(m);
		priv->stats.maa_dropped++;
		return;
	}

	if (sig == SSCOP_MERROR_indication) {
		MGETHDR(m, M_NOWAIT, MT_DATA);
		if (m == NULL)
			return;
		m->m_len = sizeof(*e);
		m->m_pkthdr.len = m->m_len;
		e = mtod(m, struct sscop_merr *);
		e->sig = sig;
		e->err = err;
		e->cnt = cnt;
		priv->stats.errors++;
	} else if (m == NULL) {

/*
 * Helper for sbappendchainaddr: prepend a struct sockaddr* to
 * an mbuf chain.
 */
static inline struct mbuf *
m_prepend_sockaddr(struct sockbuf *sb, struct mbuf *m0,
		   const struct sockaddr *asa)
{
	struct mbuf *m;
	const int salen = asa->sa_len;

	KASSERT(solocked(sb->sb_so));

	/* only the first in each chain need be a pkthdr */
	MGETHDR(m, M_DONTWAIT, MT_SONAME);
	if (m == 0)
		return (0);
	MCLAIM(m, sb->sb_mowner);
#ifdef notyet
	if (salen > MHLEN) {
		MEXTMALLOC(m, salen, M_NOWAIT);
		if ((m->m_flags & M_EXT) == 0) {
			m_free(m);
			return (0);
		}
	}
#else
	KASSERT(salen <= MHLEN);
#endif
	m->m_len = salen;
	memcpy(mtod(m, void *), asa, salen);
	m->m_next = m0;
	m->m_pkthdr.len = salen + m0->m_pkthdr.len;

	return m;
}

/*
 * DON'T use free_sent_buffers to drop the queue!
 */
static void
alloc_rx_buffers(struct sbsh_softc *sc)
{
	unsigned	cur_rbd = sc->regs->LRDR & 0x7f;
	struct mbuf	*m;

	while (sc->tail_rq != ((sc->head_rq - 1) & (RQLEN - 1))) {
		MGETHDR(m, M_NOWAIT, MT_DATA);
		if (!m) {
			if_printf (&sc->arpcom.ac_if,
				   "unable to get mbuf.\n");
			return;
		}

		if (SBNI16_MAX_FRAME > MHLEN) {
			MCLGET(m, M_NOWAIT);
			if (!(m->m_flags & M_EXT)) {
				m_freem(m);
				if_printf (&sc->arpcom.ac_if,
					   "unable to get mbuf cluster.\n");
				return;
			}
			m->m_pkthdr.len = m->m_len = MCLBYTES;
		}

		m_adj(m, 2);	/* align ip on longword boundaries */

		sc->rq[sc->tail_rq++] = m;
		sc->tail_rq &= (RQLEN - 1);

		sc->rbd[cur_rbd].address = vtophys(mtod(m, vm_offset_t));
		sc->rbd[cur_rbd].length  = 0;
		sc->regs->LRDR = cur_rbd = (cur_rbd + 1) & 0x7f;
	}
}

/*
 * Function:	mcf548x_fec_rx_bd_init
 *
 * Description:	Initialize the receive buffer descriptor ring.
 *
 * Returns:		void
 *
 * Notes:       Space for the buffers of rx BDs is allocated by the rx deamon
 *
 */
static void mcf548x_fec_rx_bd_init(struct mcf548x_enet_struct *sc) {
  int rxBdIndex;
  struct mbuf *m;
  struct ifnet *ifp = &sc->arpcom.ac_if;

  /*
   * Fill RX buffer descriptor ring.
   */
  for( rxBdIndex = 0; rxBdIndex < sc->rxBdCount; rxBdIndex++ ) {
    MGETHDR (m, M_WAIT, MT_DATA);
    MCLGET (m, M_WAIT);

    m->m_pkthdr.rcvif = ifp;
    sc->rxMbuf[rxBdIndex] = m;
    rtems_cache_invalidate_multiple_data_lines(mtod(m,const void *),
					       ETHER_MAX_LEN);
    SET_BD_BUFFER(sc->rxBd+rxBdIndex,mtod(m, void *));
    SET_BD_LENGTH(sc->rxBd+rxBdIndex,ETHER_MAX_LEN);
    SET_BD_STATUS(sc->rxBd+rxBdIndex,
		  MCF548X_FEC_RBD_EMPTY
		  | MCF548X_FEC_RBD_INT
		  | ((rxBdIndex == sc->rxBdCount-1)
		     ? MCF548X_FEC_RBD_WRAP
		     : 0));
  }
}

/*
 * Add a receive buffer to the indicated descriptor.
 */
int
ni_add_rxbuf(struct ni_softc *sc, struct ni_dg *data, int idx)
{
	struct ni_bbd *bd = &bbd[idx];
	struct mbuf *m;

	MGETHDR(m, M_DONTWAIT, MT_DATA);
	if (m == NULL)
		return (ENOBUFS);

	MCLGET(m, M_DONTWAIT);
	if ((m->m_flags & M_EXT) == 0) {
		m_freem(m);
		return (ENOBUFS);
	}

	m->m_data += 2;
	bd->nb_len = (m->m_ext.ext_size - 2);
	bd->nb_pte = (long)kvtopte(m->m_ext.ext_buf);
	bd->nb_status = 2 | NIBD_VALID;
	bd->nb_key = 1;

	data->bufs[0]._offset = 0;
	data->bufs[0]._len = bd->nb_len;
	data->bufs[0]._index = idx;
	data->nd_cmdref = (long)m;

	return (0);
}

static void receive_packet(uint8_t *buffer, int length)
{
  struct ifnet *ifp = &arpcom.ac_if;
  struct mbuf *m;
  struct ether_header *eh;
  uint32_t computed_crc, net_crc;
  
  if(length < 64) {
    printk("Warning: Ethernet packet too short\n");
    return;
  }

  length -= 4; /* strip CRC */
  net_crc = ((uint32_t)buffer[length])
    | ((uint32_t)buffer[length+1] << 8)
    | ((uint32_t)buffer[length+2] << 16)
    | ((uint32_t)buffer[length+3] << 24);
  length -= 8; /* strip preamble */
  computed_crc = ether_crc32_le(&buffer[8], length) ^ 0xffffffff;
  if(computed_crc == net_crc) {
    MGETHDR(m, M_WAIT, MT_DATA);
    MCLGET(m, M_WAIT);
    length -= sizeof(struct ether_header); /* strip Ethernet header */
    memcpy(m->m_data, &buffer[8+sizeof(struct ether_header)], length);
    m->m_len = m->m_pkthdr.len = length;
    m->m_pkthdr.rcvif = ifp;
    eh = (struct ether_header *)&buffer[8];
    ether_input(ifp, eh, m);
  } else
    printk("Ethernet CRC error: got %08x expected %08x (len=%d)\n",
      net_crc, computed_crc, length);
}

/* Add a receive buffer to the indiciated descriptor. */
int
bce_add_rxbuf(struct bce_softc *sc, int idx)
{
	struct mbuf    *m;
	int             error;

	MGETHDR(m, M_DONTWAIT, MT_DATA);
	if (m == NULL)
		return (ENOBUFS);

	MCLGET(m, M_DONTWAIT);
	if ((m->m_flags & M_EXT) == 0) {
		m_freem(m);
		return (ENOBUFS);
	}
	if (sc->bce_cdata.bce_rx_chain[idx] != NULL)
		bus_dmamap_unload(sc->bce_dmatag,
		    sc->bce_cdata.bce_rx_map[idx]);

	sc->bce_cdata.bce_rx_chain[idx] = m;

	error = bus_dmamap_load(sc->bce_dmatag, sc->bce_cdata.bce_rx_map[idx],
	    m->m_ext.ext_buf, m->m_ext.ext_size, NULL,
	    BUS_DMA_READ | BUS_DMA_NOWAIT);
	if (error)
		return (error);

	bus_dmamap_sync(sc->bce_dmatag, sc->bce_cdata.bce_rx_map[idx], 0,
	    sc->bce_cdata.bce_rx_map[idx]->dm_mapsize, BUS_DMASYNC_PREREAD);

	BCE_INIT_RXDESC(sc, idx);

	return (0);

}

static struct mbuf *
repack(struct sbsh_softc *sc, struct mbuf *m)
{
	struct mbuf  *m_new;

	MGETHDR(m_new, M_NOWAIT, MT_DATA);
	if (!m_new) {
		if_printf (&sc->arpcom.ac_if,
			   "unable to get mbuf.\n");
		return (NULL);
	}

	if (m->m_pkthdr.len > MHLEN) {
		MCLGET(m_new, M_NOWAIT);
		if (!(m_new->m_flags & M_EXT)) {
			m_freem(m_new);
			if_printf (&sc->arpcom.ac_if,
				   "unable to get mbuf cluster.\n");
			return (NULL);
		}
	}

	m_copydata(m, 0, m->m_pkthdr.len, mtod(m_new, caddr_t));
	m_new->m_pkthdr.len = m_new->m_len = m->m_pkthdr.len;
	m_freem(m);
	return (m_new);
}

/*
 * send data by simply allocating an MBUF packet
 * header and pointing it to our data region.
 *
 * Optionally, the caller may supply 'reference'
 * and 'free' procs. (The latter may call the
 * user back once the networking stack has
 * released the buffer).
 *
 * The callbacks are provided with the 'closure'
 * pointer and the 'buflen' argument.
 */
ssize_t
sendto_nocpy (
		int s,
		const void *buf, size_t buflen,
		int flags,
		const struct sockaddr *toaddr, int tolen,
		void *closure,
		void (*freeproc)(caddr_t, u_int),
		void (*refproc)(caddr_t, u_int)
)
{
	int           error;
	struct socket *so;
	struct mbuf   *to, *m;
	int           ret = -1;

	rtems_bsdnet_semaphore_obtain ();
	if ((so = rtems_bsdnet_fdToSocket (s)) == NULL) {
		rtems_bsdnet_semaphore_release ();
		return -1;
	}

	error = sockaddrtombuf (&to, toaddr, tolen);
	if (error) {
		errno = error;
		rtems_bsdnet_semaphore_release ();
		return -1;
	}

	MGETHDR(m, M_WAIT, MT_DATA);
	m->m_pkthdr.len   = 0;
	m->m_pkthdr.rcvif =  (struct ifnet *) 0;

	m->m_flags       |= M_EXT;
	m->m_ext.ext_buf  = closure ? closure : (void*)buf;
	m->m_ext.ext_size = buflen;
	/* we _must_ supply non-null procs; otherwise,
	 * the kernel code assumes it's a mbuf cluster
	 */
	m->m_ext.ext_free = freeproc ? freeproc : dummyproc;
	m->m_ext.ext_ref  = refproc  ? refproc  : dummyproc;
	m->m_pkthdr.len  += buflen;
	m->m_len          = buflen;
	m->m_data		  = (void*)buf;

	error = sosend (so, to, NULL, m, NULL, flags);
	if (error) {
		if (/*auio.uio_resid != len &&*/ (error == EINTR || error == EWOULDBLOCK))
			error = 0;
	}
	if (error)
		errno = error;
	else
		ret = buflen;
	if (to)
		m_freem(to);
	rtems_bsdnet_semaphore_release ();
	return (ret);
}

int
octeon_eth_recv_mbuf(struct octeon_eth_softc *sc, uint64_t *work,
    struct mbuf **rm)
{
	struct mbuf *m;
	void (*ext_free)(caddr_t, u_int, void *);
	void *ext_buf;
	size_t ext_size;
	void *data;
	uint64_t word1 = work[1];
	uint64_t word2 = work[2];
	uint64_t word3 = work[3];

	MGETHDR(m, M_NOWAIT, MT_DATA);
	if (m == NULL)
		return 1;
	OCTEON_ETH_KASSERT(m != NULL);

	if ((word2 & PIP_WQE_WORD2_IP_BUFS) == 0) {
		/* Dynamic short */
		ext_free = octeon_eth_buf_ext_free_m;
		ext_buf = &work[4];
		ext_size = 96;

		data = &work[4 + sc->sc_ip_offset / sizeof(uint64_t)];
	} else {
		vaddr_t addr;
		vaddr_t start_buffer;

		addr = PHYS_TO_XKPHYS(word3 & PIP_WQE_WORD3_ADDR, CCA_CACHED);
		start_buffer = addr & ~(2048 - 1);

		ext_free = octeon_eth_buf_ext_free_ext;
		ext_buf = (void *)start_buffer;
		ext_size = 2048;

		data = (void *)addr;
	}

	MEXTADD(m, ext_buf, ext_size, 0, ext_free, work);
	OCTEON_ETH_KASSERT(ISSET(m->m_flags, M_EXT));

	m->m_data = data;
	m->m_len = m->m_pkthdr.len = (word1 & PIP_WQE_WORD1_LEN) >> 48;
#if 0
	/*
	 * not readonly buffer
	 */
	m->m_flags |= M_EXT_RW;
#endif

	*rm = m;

	OCTEON_ETH_KASSERT(*rm != NULL);

	return 0;
}

int
cpsw_new_rxbuf(struct cpsw_softc * const sc, const u_int i)
{
	struct cpsw_ring_data * const rdp = sc->sc_rdp;
	const u_int h = RXDESC_PREV(i);
	struct cpsw_cpdma_bd bd;
	struct mbuf *m;
	int error = ENOBUFS;

	MGETHDR(m, M_DONTWAIT, MT_DATA);
	if (m == NULL) {
		goto reuse;
	}

	MCLGET(m, M_DONTWAIT);
	if ((m->m_flags & M_EXT) == 0) {
		m_freem(m);
		goto reuse;
	}

	/* We have a new buffer, prepare it for the ring. */

	if (rdp->rx_mb[i] != NULL)
		bus_dmamap_unload(sc->sc_bdt, rdp->rx_dm[i]);

	m->m_len = m->m_pkthdr.len = MCLBYTES;

	rdp->rx_mb[i] = m;

	error = bus_dmamap_load_mbuf(sc->sc_bdt, rdp->rx_dm[i], rdp->rx_mb[i],
	    BUS_DMA_READ|BUS_DMA_NOWAIT);
	if (error) {
		printf("can't load rx DMA map %d: %d\n", i, error);
	}

	bus_dmamap_sync(sc->sc_bdt, rdp->rx_dm[i],
	    0, rdp->rx_dm[i]->dm_mapsize, BUS_DMASYNC_PREREAD);

	error = 0;

reuse:
	/* (re-)setup the descriptor */
	bd.next = 0;
	bd.bufptr = rdp->rx_dm[i]->dm_segs[0].ds_addr;
	bd.bufoff = 0;
	bd.buflen = MIN(0x7ff, rdp->rx_dm[i]->dm_segs[0].ds_len);
	bd.pktlen = 0;
	bd.flags = CPDMA_BD_OWNER;

	cpsw_set_rxdesc(sc, i, &bd);
	/* and link onto ring */
	cpsw_set_rxdesc_next(sc, h, cpsw_rxdesc_paddr(sc, i));

	return error;
}