C++ NG_FREE_M函数代码示例

/*
 * Receive data, and do something with it.
 * Actually we receive a queue item which holds the data.
 * If we free the item it will also free the data unless we have
 * previously disassociated it using the NGI_GET_M() macro.
 * Possibly send it out on another link after processing.
 * Possibly do something different if it comes from different
 * hooks. The caller will never free m, so if we use up this data or
 * abort we must free it.
 *
 * If we want, we may decide to force this data to be queued and reprocessed
 * at the netgraph NETISR time.
 * We would do that by setting the HK_QUEUE flag on our hook. We would do that
 * in the connect() method.
 */
static int
ng_xxx_rcvdata(hook_p hook, item_p item )
{
	const xxx_p xxxp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
	int chan = -2;
	int dlci = -2;
	int error;
	struct mbuf *m;

	NGI_GET_M(item, m);
	if (NG_HOOK_PRIVATE(hook)) {
		dlci = ((struct XXX_hookinfo *) NG_HOOK_PRIVATE(hook))->dlci;
		chan = ((struct XXX_hookinfo *) NG_HOOK_PRIVATE(hook))->channel;
		if (dlci != -1) {
			/* If received on a DLCI hook process for this
			 * channel and pass it to the downstream module.
			 * Normally one would add a multiplexing header at
			 * the front here */
			/* M_PREPEND(....)	; */
			/* mtod(m, xxxxxx)->dlci = dlci; */
			NG_FWD_NEW_DATA(error, item,
				xxxp->downstream_hook.hook, m);
			xxxp->packets_out++;
		} else {
			/* data came from the multiplexed link */
			dlci = 1;	/* get dlci from header */
			/* madjust(....) *//* chop off header */
			for (chan = 0; chan < XXX_NUM_DLCIS; chan++)
				if (xxxp->channel[chan].dlci == dlci)
					break;
			if (chan == XXX_NUM_DLCIS) {
				NG_FREE_ITEM(item);
				NG_FREE_M(m);
				return (ENETUNREACH);
			}
			/* If we were called at splnet, use the following:
			 * NG_SEND_DATA_ONLY(error, otherhook, m); if this
			 * node is running at some SPL other than SPLNET
			 * then you should use instead: error =
			 * ng_queueit(otherhook, m, NULL); m = NULL;
			 * This queues the data using the standard NETISR
			 * system and schedules the data to be picked
			 * up again once the system has moved to SPLNET and
			 * the processing of the data can continue. After
			 * these are run 'm' should be considered
			 * as invalid and NG_SEND_DATA actually zaps them. */
			NG_FWD_NEW_DATA(error, item,
				xxxp->channel[chan].hook, m);
			xxxp->packets_in++;
		}
	} else {
		/* It's the debug hook, throw it away.. */
		if (hook == xxxp->downstream_hook.hook) {
			NG_FREE_ITEM(item);
			NG_FREE_M(m);
		}
	}
	return 0;
}

static void
bt3c_forward(node_p node, hook_p hook, void *arg1, int arg2)
{
	bt3c_softc_p	 sc = (bt3c_softc_p) NG_NODE_PRIVATE(node);
	struct mbuf	*m = NULL;
	int		 error;

	if (sc == NULL)
		return;

	if (sc->hook != NULL && NG_HOOK_IS_VALID(sc->hook)) {
		for (;;) {
			IF_DEQUEUE(&sc->inq, m);
			if (m == NULL)
				break;

			NG_SEND_DATA_ONLY(error, sc->hook, m);
			if (error != 0)
				NG_BT3C_STAT_IERROR(sc->stat);
		}
	} else {
		IF_LOCK(&sc->inq);
		for (;;) {
			_IF_DEQUEUE(&sc->inq, m);
			if (m == NULL)
				break;

			NG_BT3C_STAT_IERROR(sc->stat);
			NG_FREE_M(m);
		}
		IF_UNLOCK(&sc->inq);
	}
} /* bt3c_forward */

static int
bt3c_pccard_detach(device_t dev)
{
	bt3c_softc_p	sc = (bt3c_softc_p) device_get_softc(dev);

	if (sc == NULL)
		return (0);

	unregister_swi(sc->ith, SWI_TTY, -1);
	sc->ith = NULL;

	bus_teardown_intr(dev, sc->irq, sc->irq_cookie);
	bus_release_resource(dev, SYS_RES_IRQ, sc->irq_rid, sc->irq);
	sc->irq_cookie = NULL;
	sc->irq = NULL;
	sc->irq_rid = 0;

	bus_release_resource(dev, SYS_RES_IOPORT, sc->iobase_rid, sc->iobase);
	sc->iobase = NULL;
	sc->iobase_rid = 0;

	if (sc->node != NULL) {
		NG_NODE_SET_PRIVATE(sc->node, NULL);
		ng_rmnode_self(sc->node);
		sc->node = NULL;
	}

	NG_FREE_M(sc->m);
	IF_DRAIN(&sc->inq);
	IF_DRAIN(&sc->outq);

	return (0);
} /* bt3c_pccacd_detach */

/*
 * Decode an incoming frame coming from the switch
 */
static int
ngfrm_decode(node_p node, item_p item)
{
	const sc_p  sc = NG_NODE_PRIVATE(node);
	char       *data;
	int         alen;
	u_int	    dlci = 0;
	int	    error = 0;
	int	    ctxnum;
	struct mbuf *m;

	NGI_GET_M(item, m);
	if (m->m_len < 4 && (m = m_pullup(m, 4)) == NULL) {
		error = ENOBUFS;
		goto out;
	}
	data = mtod(m, char *);
	if ((alen = sc->addrlen) == 0) {
		sc->addrlen = alen = ngfrm_addrlen(data);
	}
	switch (alen) {
	case 2:
		SHIFTIN(makeup + 0, data[0], dlci);
		SHIFTIN(makeup + 1, data[1], dlci);
		break;
	case 3:
		SHIFTIN(makeup + 0, data[0], dlci);
		SHIFTIN(makeup + 1, data[1], dlci);
		SHIFTIN(makeup + 3, data[2], dlci);	/* 3 and 2 is correct */
		break;
	case 4:
		SHIFTIN(makeup + 0, data[0], dlci);
		SHIFTIN(makeup + 1, data[1], dlci);
		SHIFTIN(makeup + 2, data[2], dlci);
		SHIFTIN(makeup + 3, data[3], dlci);
		break;
	default:
		error = EINVAL;
		goto out;
	}

	if (dlci > 1023) {
		error = EINVAL;
		goto out;
	}
	ctxnum = sc->ALT[dlci];
	if ((ctxnum & CTX_VALID) && sc->channel[ctxnum &= CTX_VALUE].hook) {
		/* Send it */
		m_adj(m, alen);
		NG_FWD_NEW_DATA(error, item, sc->channel[ctxnum].hook, m);
		return (error);
	} else {
		error = ENETDOWN;
	}
out:
	NG_FREE_ITEM(item);
	NG_FREE_M(m);
	return (error);
}

/*
 * Accept data from the hook and queue it for output.
 */
Static int
ng_udbp_rcvdata(hook_p hook, item_p item)
{
	const udbp_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
	int error;
	struct ifqueue	*xmitq_p;
	int	s;
	struct mbuf *m;
	meta_p meta;

	NGI_GET_M(item, m);
	NGI_GET_META(item, meta);
	NG_FREE_ITEM(item);
	/* 
	 * Now queue the data for when it can be sent
	 */
	if (meta && meta->priority > 0) {
		xmitq_p = (&sc->xmitq_hipri);
	} else {
		xmitq_p = (&sc->xmitq);
	}
	s = splusb();
	IF_LOCK(xmitq_p);
	if (_IF_QFULL(xmitq_p)) {
		_IF_DROP(xmitq_p);
		IF_UNLOCK(xmitq_p);
		splx(s);
		error = ENOBUFS;
		goto bad;
	}
	_IF_ENQUEUE(xmitq_p, m);
	IF_UNLOCK(xmitq_p);
	if (!(sc->flags & OUT_BUSY))
		udbp_setup_out_transfer(sc);
	splx(s);
	return (0);

bad:	/*
         * It was an error case.
	 * check if we need to free the mbuf, and then return the error
	 */
	NG_FREE_M(m);
	NG_FREE_META(meta);
	return (error);
}

static int
ubt_fwd_mbuf_up(ubt_softc_p sc, struct mbuf **m)
{
	hook_p	hook;
	int	error;

	/*
	 * Close the race with Netgraph hook newhook/disconnect methods.
	 * Save the hook pointer atomically. Two cases are possible:
	 *
	 * 1) The hook pointer is NULL. It means disconnect method got
	 *    there first. In this case we are done.
	 *
	 * 2) The hook pointer is not NULL. It means that hook pointer
	 *    could be either in valid or invalid (i.e. in the process
	 *    of disconnect) state. In any case grab an extra reference
	 *    to protect the hook pointer.
	 *
	 * It is ok to pass hook in invalid state to NG_SEND_DATA_ONLY() as
	 * it checks for it. Drop extra reference after NG_SEND_DATA_ONLY().
	 */

	UBT_NG_LOCK(sc);
	if ((hook = sc->sc_hook) != NULL)
		NG_HOOK_REF(hook);
	UBT_NG_UNLOCK(sc);

	if (hook == NULL) {
		NG_FREE_M(*m);
		return (ENETDOWN);
	}

	NG_SEND_DATA_ONLY(error, hook, *m);
	NG_HOOK_UNREF(hook);

	if (error != 0)
		UBT_STAT_IERROR(sc);

	return (error);
} /* ubt_fwd_mbuf_up */

/*
 * Receive data on a hook
 */
static int
ngs_rcvdata(hook_p hook, item_p item)
{
	struct ngsock *const priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
	struct ngpcb *const pcbp = priv->datasock;
	struct socket *so;
	struct sockaddr_ng *addr;
	char *addrbuf[NG_HOOKSIZ + 4];
	int addrlen;
	struct mbuf *m;

	NGI_GET_M(item, m);
	NG_FREE_ITEM(item);

	/* If there is no data socket, black-hole it. */
	if (pcbp == NULL) {
		NG_FREE_M(m);
		return (0);
	}
	so = pcbp->ng_socket;

	/* Get the return address into a sockaddr. */
	addrlen = strlen(NG_HOOK_NAME(hook));	/* <= NG_HOOKSIZ - 1 */
	addr = (struct sockaddr_ng *) addrbuf;
	addr->sg_len = addrlen + 3;
	addr->sg_family = AF_NETGRAPH;
	bcopy(NG_HOOK_NAME(hook), addr->sg_data, addrlen);
	addr->sg_data[addrlen] = '\0';

	/* Try to tell the socket which hook it came in on. */
	if (sbappendaddr((struct sockbuf *)&so->so_rcv, (struct sockaddr *)addr, m, NULL) == 0) {
		m_freem(m);
		TRAP_ERROR;
		return (ENOBUFS);
	}
	sorwakeup(so);
	return (0);
}

/*
 * Receive data packet
 */
static int
ngfrm_rcvdata(hook_p hook, item_p item)
{
	struct	ctxinfo *const ctxp = NG_HOOK_PRIVATE(hook);
	int     error = 0;
	int     dlci;
	sc_p    sc;
	int     alen;
	char   *data;
	struct mbuf *m;

	/* Data doesn't come in from just anywhere (e.g debug hook) */
	if (ctxp == NULL) {
		error = ENETDOWN;
		goto bad;
	}

	/* If coming from downstream, decode it to a channel */
	dlci = ctxp->dlci;
	if (dlci == -1)
		return (ngfrm_decode(NG_HOOK_NODE(hook), item));

	NGI_GET_M(item, m);
	/* Derive the softc we will need */
	sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));

	/* If there is no live channel, throw it away */
	if ((sc->downstream.hook == NULL)
	    || ((ctxp->flags & CHAN_ACTIVE) == 0)) {
		error = ENETDOWN;
		goto bad;
	}

	/* Store the DLCI on the front of the packet */
	alen = sc->addrlen;
	if (alen == 0)
		alen = 2;	/* default value for transmit */
	M_PREPEND(m, alen, M_DONTWAIT);
	if (m == NULL) {
		error = ENOBUFS;
		goto bad;
	}
	data = mtod(m, char *);

	/*
	 * Shift the lowest bits into the address field untill we are done.
	 * First byte is MSBits of addr so work backwards.
	 */
	switch (alen) {
	case 2:
		data[0] = data[1] = '\0';
		SHIFTOUT(makeup + 1, data[1], dlci);
		SHIFTOUT(makeup + 0, data[0], dlci);
		data[1] |= BYTEX_EA;
		break;
	case 3:
		data[0] = data[1] = data[2] = '\0';
		SHIFTOUT(makeup + 3, data[2], dlci);	/* 3 and 2 is correct */
		SHIFTOUT(makeup + 1, data[1], dlci);
		SHIFTOUT(makeup + 0, data[0], dlci);
		data[2] |= BYTEX_EA;
		break;
	case 4:
		data[0] = data[1] = data[2] = data[3] = '\0';
		SHIFTOUT(makeup + 3, data[3], dlci);
		SHIFTOUT(makeup + 2, data[2], dlci);
		SHIFTOUT(makeup + 1, data[1], dlci);
		SHIFTOUT(makeup + 0, data[0], dlci);
		data[3] |= BYTEX_EA;
		break;
	default:
		panic(__func__);
	}

	/* Send it */
	NG_FWD_NEW_DATA(error, item, sc->downstream.hook, m);
	return (error);

bad:
	NG_FREE_ITEM(item);
	NG_FREE_M(m);
	return (error);
}

static void
ubt_bulk_read_callback(struct usb_xfer *xfer, usb_error_t error)
{
	struct ubt_softc	*sc = usbd_xfer_softc(xfer);
	struct mbuf		*m;
	ng_hci_acldata_pkt_t	*hdr;
	struct usb_page_cache	*pc;
	int len;
	int actlen;

	usbd_xfer_status(xfer, &actlen, NULL, NULL, NULL);

	m = NULL;

	switch (USB_GET_STATE(xfer)) {
	case USB_ST_TRANSFERRED:
		/* Allocate new mbuf */
		MGETHDR(m, M_NOWAIT, MT_DATA);
		if (m == NULL) {
			UBT_STAT_IERROR(sc);
			goto submit_next;
		}

		if (!(MCLGET(m, M_NOWAIT))) {
			UBT_STAT_IERROR(sc);
			goto submit_next;
		}

		/* Add HCI packet type */
		*mtod(m, uint8_t *)= NG_HCI_ACL_DATA_PKT;
		m->m_pkthdr.len = m->m_len = 1;

		if (actlen > MCLBYTES - 1)
			actlen = MCLBYTES - 1;

		pc = usbd_xfer_get_frame(xfer, 0);
		usbd_copy_out(pc, 0, mtod(m, uint8_t *) + 1, actlen);
		m->m_pkthdr.len += actlen;
		m->m_len += actlen;

		UBT_INFO(sc, "got %d bytes from bulk-in pipe\n",
			actlen);

		/* Validate packet and send it up the stack */
		if (m->m_pkthdr.len < (int)sizeof(*hdr)) {
			UBT_INFO(sc, "HCI ACL packet is too short\n");

			UBT_STAT_IERROR(sc);
			goto submit_next;
		}

		hdr = mtod(m, ng_hci_acldata_pkt_t *);
		len = le16toh(hdr->length);
		if (len != (int)(m->m_pkthdr.len - sizeof(*hdr))) {
			UBT_ERR(sc, "Invalid ACL packet size, length=%d, " \
				"pktlen=%d\n", len, m->m_pkthdr.len);

			UBT_STAT_IERROR(sc);
			goto submit_next;
		}

		UBT_INFO(sc, "got complete ACL data packet, pktlen=%d, " \
			"length=%d\n", m->m_pkthdr.len, len);

		UBT_STAT_PCKTS_RECV(sc);
		UBT_STAT_BYTES_RECV(sc, m->m_pkthdr.len);

		ubt_fwd_mbuf_up(sc, &m);
		/* m == NULL at this point */
		/* FALLTHOUGH */

	case USB_ST_SETUP:
submit_next:
		NG_FREE_M(m); /* checks for m != NULL */

		usbd_xfer_set_frame_len(xfer, 0, usbd_xfer_max_len(xfer));
		usbd_transfer_submit(xfer);
		break;

	default: /* Error */
		if (error != USB_ERR_CANCELLED) {
			UBT_WARN(sc, "bulk-in transfer failed: %s\n",
				usbd_errstr(error));

			/* Try to clear stall first */
			usbd_xfer_set_stall(xfer);
			goto submit_next;
		}
			/* transfer cancelled */
		break;
	}
} /* ubt_bulk_read_callback */

static void
ubt_ctrl_write_callback(struct usb_xfer *xfer, usb_error_t error)
{
	struct ubt_softc		*sc = usbd_xfer_softc(xfer);
	struct usb_device_request	req;
	struct mbuf			*m;
	struct usb_page_cache		*pc;
	int				actlen;

	usbd_xfer_status(xfer, &actlen, NULL, NULL, NULL);

	switch (USB_GET_STATE(xfer)) {
	case USB_ST_TRANSFERRED:
		UBT_INFO(sc, "sent %d bytes to control pipe\n", actlen);
		UBT_STAT_BYTES_SENT(sc, actlen);
		UBT_STAT_PCKTS_SENT(sc);
		/* FALLTHROUGH */

	case USB_ST_SETUP:
send_next:
		/* Get next command mbuf, if any */
		UBT_NG_LOCK(sc);
		NG_BT_MBUFQ_DEQUEUE(&sc->sc_cmdq, m);
		UBT_NG_UNLOCK(sc);

		if (m == NULL) {
			UBT_INFO(sc, "HCI command queue is empty\n");
			break;	/* transfer complete */
		}

		/* Initialize a USB control request and then schedule it */
		bzero(&req, sizeof(req));
		req.bmRequestType = UBT_HCI_REQUEST;
		USETW(req.wLength, m->m_pkthdr.len);

		UBT_INFO(sc, "Sending control request, " \
			"bmRequestType=0x%02x, wLength=%d\n",
			req.bmRequestType, UGETW(req.wLength));

		pc = usbd_xfer_get_frame(xfer, 0);
		usbd_copy_in(pc, 0, &req, sizeof(req));
		pc = usbd_xfer_get_frame(xfer, 1);
		usbd_m_copy_in(pc, 0, m, 0, m->m_pkthdr.len);

		usbd_xfer_set_frame_len(xfer, 0, sizeof(req));
		usbd_xfer_set_frame_len(xfer, 1, m->m_pkthdr.len);
		usbd_xfer_set_frames(xfer, 2);

		NG_FREE_M(m);

		usbd_transfer_submit(xfer);
		break;

	default: /* Error */
		if (error != USB_ERR_CANCELLED) {
			UBT_WARN(sc, "control transfer failed: %s\n",
				usbd_errstr(error));

			UBT_STAT_OERROR(sc);
			goto send_next;
		}

		/* transfer cancelled */
		break;
	}
} /* ubt_ctrl_write_callback */

static int
ng_ubt_rcvdata(hook_p hook, item_p item)
{
	struct ubt_softc	*sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
	struct mbuf		*m;
	struct ng_bt_mbufq	*q;
	int			action, error = 0;

	if (hook != sc->sc_hook) {
		error = EINVAL;
		goto done;
	}

	/* Deatch mbuf and get HCI frame type */
	NGI_GET_M(item, m);

	/*
	 * Minimal size of the HCI frame is 4 bytes: 1 byte frame type,
	 * 2 bytes connection handle and at least 1 byte of length.
	 * Panic on data frame that has size smaller than 4 bytes (it
	 * should not happen)
	 */

	if (m->m_pkthdr.len < 4)
		panic("HCI frame size is too small! pktlen=%d\n",
			m->m_pkthdr.len);

	/* Process HCI frame */
	switch (*mtod(m, uint8_t *)) {	/* XXX call m_pullup ? */
	case NG_HCI_CMD_PKT:
		if (m->m_pkthdr.len - 1 > (int)UBT_CTRL_BUFFER_SIZE)
			panic("HCI command frame size is too big! " \
				"buffer size=%zd, packet len=%d\n",
				UBT_CTRL_BUFFER_SIZE, m->m_pkthdr.len);

		q = &sc->sc_cmdq;
		action = UBT_FLAG_T_START_CTRL;
		break;

	case NG_HCI_ACL_DATA_PKT:
		if (m->m_pkthdr.len - 1 > UBT_BULK_WRITE_BUFFER_SIZE)
			panic("ACL data frame size is too big! " \
				"buffer size=%d, packet len=%d\n",
				UBT_BULK_WRITE_BUFFER_SIZE, m->m_pkthdr.len);

		q = &sc->sc_aclq;
		action = UBT_FLAG_T_START_BULK;
		break;

	case NG_HCI_SCO_DATA_PKT:
		q = &sc->sc_scoq;
		action = 0;
		break;

	default:
		UBT_ERR(sc, "Dropping unsupported HCI frame, type=0x%02x, " \
			"pktlen=%d\n", *mtod(m, uint8_t *), m->m_pkthdr.len);

		NG_FREE_M(m);
		error = EINVAL;
		goto done;
		/* NOT REACHED */
	}

	UBT_NG_LOCK(sc);
	if (NG_BT_MBUFQ_FULL(q)) {
		NG_BT_MBUFQ_DROP(q);
		UBT_NG_UNLOCK(sc);
		
		UBT_ERR(sc, "Dropping HCI frame 0x%02x, len=%d. Queue full\n",
			*mtod(m, uint8_t *), m->m_pkthdr.len);

		NG_FREE_M(m);
	} else {

static void
ubt_isoc_write_callback(struct usb_xfer *xfer, usb_error_t error)
{
	struct ubt_softc	*sc = usbd_xfer_softc(xfer);
	struct usb_page_cache	*pc;
	struct mbuf		*m;
	int			n, space, offset;
	int			actlen, nframes;

	usbd_xfer_status(xfer, &actlen, NULL, NULL, &nframes);
	pc = usbd_xfer_get_frame(xfer, 0);

	switch (USB_GET_STATE(xfer)) {
	case USB_ST_TRANSFERRED:
		UBT_INFO(sc, "sent %d bytes to isoc-out pipe\n", actlen);
		UBT_STAT_BYTES_SENT(sc, actlen);
		UBT_STAT_PCKTS_SENT(sc);
		/* FALLTHROUGH */

	case USB_ST_SETUP:
send_next:
		offset = 0;
		space = usbd_xfer_max_framelen(xfer) * nframes;
		m = NULL;

		while (space > 0) {
			if (m == NULL) {
				UBT_NG_LOCK(sc);
				NG_BT_MBUFQ_DEQUEUE(&sc->sc_scoq, m);
				UBT_NG_UNLOCK(sc);

				if (m == NULL)
					break;
			}

			n = min(space, m->m_pkthdr.len);
			if (n > 0) {
				usbd_m_copy_in(pc, offset, m,0, n);
				m_adj(m, n);

				offset += n;
				space -= n;
			}

			if (m->m_pkthdr.len == 0)
				NG_FREE_M(m); /* sets m = NULL */
		}

		/* Put whatever is left from mbuf back on queue */
		if (m != NULL) {
			UBT_NG_LOCK(sc);
			NG_BT_MBUFQ_PREPEND(&sc->sc_scoq, m);
			UBT_NG_UNLOCK(sc);
		}

		/*
		 * Calculate sizes for isoc frames.
		 * Note that offset could be 0 at this point (i.e. we have
		 * nothing to send). That is fine, as we have isoc. transfers
		 * going in both directions all the time. In this case it
		 * would be just empty isoc. transfer.
		 */

		for (n = 0; n < nframes; n ++) {
			usbd_xfer_set_frame_len(xfer, n,
			    min(offset, usbd_xfer_max_framelen(xfer)));
			offset -= usbd_xfer_frame_len(xfer, n);
		}

		usbd_transfer_submit(xfer);
		break;

	default: /* Error */
		if (error != USB_ERR_CANCELLED) {
			UBT_STAT_OERROR(sc);
			goto send_next;
		}

		/* transfer cancelled */
		break;
	}
}

static int
ubt_isoc_read_one_frame(struct usb_xfer *xfer, int frame_no)
{
	struct ubt_softc	*sc = usbd_xfer_softc(xfer);
	struct usb_page_cache	*pc;
	struct mbuf		*m;
	int			len, want, got, total;

	/* Get existing SCO reassembly buffer */
	pc = usbd_xfer_get_frame(xfer, 0);
	m = sc->sc_isoc_in_buffer;
	total = usbd_xfer_frame_len(xfer, frame_no);

	/* While we have data in the frame */
	while (total > 0) {
		if (m == NULL) {
			/* Start new reassembly buffer */
			MGETHDR(m, M_NOWAIT, MT_DATA);
			if (m == NULL) {
				UBT_STAT_IERROR(sc);
				return (-1);	/* XXX out of sync! */
			}

			if (!(MCLGET(m, M_NOWAIT))) {
				UBT_STAT_IERROR(sc);
				NG_FREE_M(m);
				return (-1);	/* XXX out of sync! */
			}

			/* Expect SCO header */
			*mtod(m, uint8_t *) = NG_HCI_SCO_DATA_PKT;
			m->m_pkthdr.len = m->m_len = got = 1;
			want = sizeof(ng_hci_scodata_pkt_t);
		} else {
			/*
			 * Check if we have SCO header and if so 
			 * adjust amount of data we want
			 */
			got = m->m_pkthdr.len;
			want = sizeof(ng_hci_scodata_pkt_t);

			if (got >= want)
				want += mtod(m, ng_hci_scodata_pkt_t *)->length;
		}

		/* Append frame data to the SCO reassembly buffer */
		len = total;
		if (got + len > want)
			len = want - got;

		usbd_copy_out(pc, frame_no * usbd_xfer_max_framelen(xfer),
			mtod(m, uint8_t *) + m->m_pkthdr.len, len);

		m->m_pkthdr.len += len;
		m->m_len += len;
		total -= len;

		/* Check if we got everything we wanted, if not - continue */
		if (got != want)
			continue;

		/* If we got here then we got complete SCO frame */
		UBT_INFO(sc, "got complete SCO data frame, pktlen=%d, " \
			"length=%d\n", m->m_pkthdr.len,
			mtod(m, ng_hci_scodata_pkt_t *)->length);

		UBT_STAT_PCKTS_RECV(sc);
		UBT_STAT_BYTES_RECV(sc, m->m_pkthdr.len);

		ubt_fwd_mbuf_up(sc, &m);
		/* m == NULL at this point */
	}

	/* Put SCO reassembly buffer back */
	sc->sc_isoc_in_buffer = m;

	return (0);
} /* ubt_isoc_read_one_frame */

int
ng_l2cap_lp_send(ng_l2cap_con_p con, u_int16_t dcid, struct mbuf *m0)
{
	ng_l2cap_p		 l2cap = con->l2cap;
	ng_l2cap_hdr_t		*l2cap_hdr = NULL;
        ng_hci_acldata_pkt_t	*acl_hdr = NULL;
        struct mbuf		*m_last = NULL, *m = NULL;
        int			 len, flag = NG_HCI_PACKET_START;

	KASSERT((con->tx_pkt == NULL),
("%s: %s - another packet pending?!\n", __func__, NG_NODE_NAME(l2cap->node)));
	KASSERT((l2cap->pkt_size > 0),
("%s: %s - invalid l2cap->pkt_size?!\n", __func__, NG_NODE_NAME(l2cap->node)));

	/* Prepend mbuf with L2CAP header */
	m0 = ng_l2cap_prepend(m0, sizeof(*l2cap_hdr));
	if (m0 == NULL) {
		NG_L2CAP_ALERT(
"%s: %s - ng_l2cap_prepend(%zd) failed\n",
			__func__, NG_NODE_NAME(l2cap->node),
			sizeof(*l2cap_hdr));

		goto fail;
	}

	l2cap_hdr = mtod(m0, ng_l2cap_hdr_t *);
	l2cap_hdr->length = htole16(m0->m_pkthdr.len - sizeof(*l2cap_hdr));
	l2cap_hdr->dcid = htole16(dcid);

	/*
	 * Segment single L2CAP packet according to the HCI layer MTU. Convert 
	 * each segment into ACL data packet and prepend it with ACL data packet
	 * header. Link all segments together via m_nextpkt link. 
 	 *
	 * XXX BC (Broadcast flag) will always be 0 (zero).
	 */

	while (m0 != NULL) {
		/* Check length of the packet against HCI MTU */
		len = m0->m_pkthdr.len;
		if (len > l2cap->pkt_size) {
			m = m_split(m0, l2cap->pkt_size, M_DONTWAIT);
			if (m == NULL) {
				NG_L2CAP_ALERT(
"%s: %s - m_split(%d) failed\n",	__func__, NG_NODE_NAME(l2cap->node),
					l2cap->pkt_size);
				goto fail;
			}

			len = l2cap->pkt_size;
		}

		/* Convert packet fragment into ACL data packet */
		m0 = ng_l2cap_prepend(m0, sizeof(*acl_hdr));
		if (m0 == NULL) {
			NG_L2CAP_ALERT(
"%s: %s - ng_l2cap_prepend(%zd) failed\n",
				__func__, NG_NODE_NAME(l2cap->node),
				sizeof(*acl_hdr));
			goto fail;
		}

		acl_hdr = mtod(m0, ng_hci_acldata_pkt_t *);
		acl_hdr->type = NG_HCI_ACL_DATA_PKT;
		acl_hdr->length = htole16(len);
		acl_hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE(
					con->con_handle, flag, 0));

		/* Add fragment to the chain */
		m0->m_nextpkt = NULL;

		if (con->tx_pkt == NULL)
			con->tx_pkt = m_last = m0;
		else {
			m_last->m_nextpkt = m0;
			m_last = m0;
		}

		NG_L2CAP_INFO(
"%s: %s - attaching ACL packet, con_handle=%d, PB=%#x, length=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), con->con_handle,
			flag, len);

		m0 = m;
		m = NULL;
		flag = NG_HCI_PACKET_FRAGMENT;
	}

	return (0);
fail:
	NG_FREE_M(m0);
	NG_FREE_M(m);

	while (con->tx_pkt != NULL) {
		m = con->tx_pkt->m_nextpkt;
		m_freem(con->tx_pkt);
		con->tx_pkt = m;
	}

	return (ENOBUFS);
//.........这里部分代码省略.........

int
ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m)
{
	ng_hci_acldata_pkt_t	*acl_hdr = NULL;
	ng_l2cap_hdr_t		*l2cap_hdr = NULL;
	ng_l2cap_con_p		 con = NULL;
	u_int16_t		 con_handle, length, pb;
	int			 error = 0;

	/* Check ACL data packet */
	if (m->m_pkthdr.len < sizeof(*acl_hdr)) {
		NG_L2CAP_ERR(
"%s: %s - invalid ACL data packet. Packet too small, length=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len);
		error = EMSGSIZE;
		goto drop;
	}

	/* Strip ACL data packet header */
	NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr));
	if (m == NULL)
		return (ENOBUFS);

	acl_hdr = mtod(m, ng_hci_acldata_pkt_t *);
	m_adj(m, sizeof(*acl_hdr));

	/* Get ACL connection handle, PB flag and payload length */
	acl_hdr->con_handle = le16toh(acl_hdr->con_handle);
	con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle);
	pb = NG_HCI_PB_FLAG(acl_hdr->con_handle);
	length = le16toh(acl_hdr->length);

	NG_L2CAP_INFO(
"%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n",
		__func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length);

	/* Get connection descriptor */
	con = ng_l2cap_con_by_handle(l2cap, con_handle);
	if (con == NULL) {
		NG_L2CAP_ERR(
"%s: %s - unexpected ACL data packet. " \
"Connection does not exist, con_handle=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), con_handle);
		error = ENOENT;
		goto drop;
	}

	/* Verify connection state */
	if (con->state != NG_L2CAP_CON_OPEN) {
		NG_L2CAP_ERR(
"%s: %s - unexpected ACL data packet. Invalid connection state=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), con->state);
		error = EHOSTDOWN;
		goto drop;
	}

	/* Process packet */
	if (pb == NG_HCI_PACKET_START) {
		if (con->rx_pkt != NULL) {
			NG_L2CAP_ERR(
"%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %d bytes\n",
				__func__, NG_NODE_NAME(l2cap->node),
				con->rx_pkt->m_pkthdr.len, con->rx_pkt_len);
			NG_FREE_M(con->rx_pkt);
			con->rx_pkt_len = 0;
		}

		/* Get L2CAP header */
		if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) {
			NG_L2CAP_ERR(
"%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n",
				__func__, NG_NODE_NAME(l2cap->node),
				m->m_pkthdr.len);
			error = EMSGSIZE;
			goto drop;
		}

		NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr));
		if (m == NULL)
			return (ENOBUFS);

		l2cap_hdr = mtod(m, ng_l2cap_hdr_t *);

		NG_L2CAP_INFO(
"%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), con_handle,
			le16toh(l2cap_hdr->length));

		/* Start new L2CAP packet */
		con->rx_pkt = m;
		con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr);
	} else if (pb == NG_HCI_PACKET_FRAGMENT) {