/*
 * ng_btsocket_rfcomm.c
 */

/*-
 * Copyright (c) 2001-2003 Maksim Yevmenkin <m_evmenkin@yahoo.com>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $Id: ng_btsocket_rfcomm.c,v 1.28 2003/09/14 23:29:06 max Exp $
 * $FreeBSD: src/sys/netgraph/bluetooth/socket/ng_btsocket_rfcomm.c,v 1.27 2007/10/29 19:06:47 emax Exp $
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/bitstring.h>
#include <sys/domain.h>
#include <sys/endian.h>
#include <sys/errno.h>
#include <sys/filedesc.h>
#include <sys/kernel.h>
#include <sys/lock.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>
#include <sys/proc.h>
#include <sys/protosw.h>
#include <sys/queue.h>
#include <sys/socket.h>
#include <sys/socketvar.h>
#include <sys/sysctl.h>
#include <sys/taskqueue.h>
#include <sys/uio.h>
#include <sys/msgport2.h>
#include <sys/fcntl.h>
#include <sys/socketvar2.h>
#include <net/netmsg2.h>
#include <netgraph7/ng_message.h>
#include <netgraph7/netgraph.h>
#include <netgraph7/bluetooth/include/ng_bluetooth.h>
#include <netgraph7/bluetooth/include/ng_hci.h>
#include <netgraph7/bluetooth/include/ng_l2cap.h>
#include <netgraph7/bluetooth/include/ng_btsocket.h>
#include <netgraph7/bluetooth/include/ng_btsocket_l2cap.h>
#include <netgraph7/bluetooth/include/ng_btsocket_rfcomm.h>

/* MALLOC define */
#ifdef NG_SEPARATE_MALLOC
MALLOC_DEFINE(M_NETGRAPH_BTSOCKET_RFCOMM, "netgraph_btsocks_rfcomm",
                "Netgraph Bluetooth RFCOMM sockets");
#else
#define M_NETGRAPH_BTSOCKET_RFCOMM M_NETGRAPH
#endif /* NG_SEPARATE_MALLOC */

/* Debug */
#define NG_BTSOCKET_RFCOMM_INFO \
        if (ng_btsocket_rfcomm_debug_level >= NG_BTSOCKET_INFO_LEVEL) \
                kprintf

#define NG_BTSOCKET_RFCOMM_WARN \
        if (ng_btsocket_rfcomm_debug_level >= NG_BTSOCKET_WARN_LEVEL) \
                kprintf

#define NG_BTSOCKET_RFCOMM_ERR \
        if (ng_btsocket_rfcomm_debug_level >= NG_BTSOCKET_ERR_LEVEL) \
                kprintf

#define NG_BTSOCKET_RFCOMM_ALERT \
        if (ng_btsocket_rfcomm_debug_level >= NG_BTSOCKET_ALERT_LEVEL) \
                kprintf

#define ALOT    0x7fff

/* Local prototypes */
static void ng_btsocket_rfcomm_upcall
        (struct socket *so, void *arg, int waitflag);
static void ng_btsocket_rfcomm_sessions_task
        (void *ctx, int pending);
static void ng_btsocket_rfcomm_session_task
        (ng_btsocket_rfcomm_session_p s);
#define ng_btsocket_rfcomm_task_wakeup() \
        taskqueue_enqueue(taskqueue_swi, &ng_btsocket_rfcomm_task)

static ng_btsocket_rfcomm_pcb_p ng_btsocket_rfcomm_connect_ind
        (ng_btsocket_rfcomm_session_p s, int channel);
static void ng_btsocket_rfcomm_connect_cfm
        (ng_btsocket_rfcomm_session_p s);

static int ng_btsocket_rfcomm_session_create
        (ng_btsocket_rfcomm_session_p *sp, struct socket *l2so,
         bdaddr_p src, bdaddr_p dst, struct thread *td);
static int ng_btsocket_rfcomm_session_accept
        (ng_btsocket_rfcomm_session_p s0);
static int ng_btsocket_rfcomm_session_connect
        (ng_btsocket_rfcomm_session_p s);
static int ng_btsocket_rfcomm_session_receive
        (ng_btsocket_rfcomm_session_p s);
static int ng_btsocket_rfcomm_session_send
        (ng_btsocket_rfcomm_session_p s);
static void ng_btsocket_rfcomm_session_clean
        (ng_btsocket_rfcomm_session_p s);
static void ng_btsocket_rfcomm_session_process_pcb
        (ng_btsocket_rfcomm_session_p s);
static ng_btsocket_rfcomm_session_p ng_btsocket_rfcomm_session_by_addr
        (bdaddr_p src, bdaddr_p dst);

static int ng_btsocket_rfcomm_receive_frame
        (ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
static int ng_btsocket_rfcomm_receive_sabm
        (ng_btsocket_rfcomm_session_p s, int dlci);
static int ng_btsocket_rfcomm_receive_disc
        (ng_btsocket_rfcomm_session_p s, int dlci);
static int ng_btsocket_rfcomm_receive_ua
        (ng_btsocket_rfcomm_session_p s, int dlci);
static int ng_btsocket_rfcomm_receive_dm
        (ng_btsocket_rfcomm_session_p s, int dlci);
static int ng_btsocket_rfcomm_receive_uih
        (ng_btsocket_rfcomm_session_p s, int dlci, int pf, struct mbuf *m0);
static int ng_btsocket_rfcomm_receive_mcc
        (ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
static int ng_btsocket_rfcomm_receive_test
        (ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
static int ng_btsocket_rfcomm_receive_fc
        (ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
static int ng_btsocket_rfcomm_receive_msc
        (ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
static int ng_btsocket_rfcomm_receive_rpn
        (ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
static int ng_btsocket_rfcomm_receive_rls
        (ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
static int ng_btsocket_rfcomm_receive_pn
        (ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
static void ng_btsocket_rfcomm_set_pn
        (ng_btsocket_rfcomm_pcb_p pcb, u_int8_t cr, u_int8_t flow_control, 
         u_int8_t credits, u_int16_t mtu);

static int ng_btsocket_rfcomm_send_command
        (ng_btsocket_rfcomm_session_p s, u_int8_t type, u_int8_t dlci);
static int ng_btsocket_rfcomm_send_uih
        (ng_btsocket_rfcomm_session_p s, u_int8_t address, u_int8_t pf, 
         u_int8_t credits, struct mbuf *data);
static int ng_btsocket_rfcomm_send_msc
        (ng_btsocket_rfcomm_pcb_p pcb);
static int ng_btsocket_rfcomm_send_pn
        (ng_btsocket_rfcomm_pcb_p pcb);
static int ng_btsocket_rfcomm_send_credits
        (ng_btsocket_rfcomm_pcb_p pcb);

static int ng_btsocket_rfcomm_pcb_send
        (ng_btsocket_rfcomm_pcb_p pcb, int limit);
static void ng_btsocket_rfcomm_pcb_kill
        (ng_btsocket_rfcomm_pcb_p pcb, int error);
static ng_btsocket_rfcomm_pcb_p ng_btsocket_rfcomm_pcb_by_dlci
        (ng_btsocket_rfcomm_session_p s, int dlci);
static ng_btsocket_rfcomm_pcb_p ng_btsocket_rfcomm_pcb_listener
        (bdaddr_p src, int channel);

static void ng_btsocket_rfcomm_timeout
        (ng_btsocket_rfcomm_pcb_p pcb);
static void ng_btsocket_rfcomm_untimeout
        (ng_btsocket_rfcomm_pcb_p pcb);
static void ng_btsocket_rfcomm_process_timeout
        (void *xpcb);

static struct mbuf * ng_btsocket_rfcomm_prepare_packet
        (struct sockbuf *sb, int length);

/* Globals */
extern int                                      ifqmaxlen;
static u_int32_t                                ng_btsocket_rfcomm_debug_level;
static u_int32_t                                ng_btsocket_rfcomm_timo;
struct task                                     ng_btsocket_rfcomm_task;
static LIST_HEAD(, ng_btsocket_rfcomm_session)  ng_btsocket_rfcomm_sessions;
static struct lock                              ng_btsocket_rfcomm_sessions_lock;
static LIST_HEAD(, ng_btsocket_rfcomm_pcb)      ng_btsocket_rfcomm_sockets;
static struct lock                              ng_btsocket_rfcomm_sockets_lock;

/* Sysctl tree */
SYSCTL_DECL(_net_bluetooth_rfcomm_sockets);
SYSCTL_NODE(_net_bluetooth_rfcomm_sockets, OID_AUTO, stream, CTLFLAG_RW,
        0, "Bluetooth STREAM RFCOMM sockets family");
SYSCTL_INT(_net_bluetooth_rfcomm_sockets_stream, OID_AUTO, debug_level,
        CTLFLAG_RW,
        &ng_btsocket_rfcomm_debug_level, NG_BTSOCKET_INFO_LEVEL,
        "Bluetooth STREAM RFCOMM sockets debug level");
SYSCTL_INT(_net_bluetooth_rfcomm_sockets_stream, OID_AUTO, timeout,
        CTLFLAG_RW,
        &ng_btsocket_rfcomm_timo, 60,
        "Bluetooth STREAM RFCOMM sockets timeout");

/*****************************************************************************
 *****************************************************************************
 **                              RFCOMM CRC
 *****************************************************************************
 *****************************************************************************/

static u_int8_t ng_btsocket_rfcomm_crc_table[256] = {
        0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
        0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
        0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
        0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,

        0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
        0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
        0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
        0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,

        0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
        0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
        0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
        0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,

        0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
        0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
        0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
        0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,

        0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
        0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
        0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
        0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,

        0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
        0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
        0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
        0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,

        0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
        0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
        0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
        0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,

        0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
        0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
        0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
        0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf
};

/* CRC */
static u_int8_t
ng_btsocket_rfcomm_crc(u_int8_t *data, int length)
{
        u_int8_t        crc = 0xff;

        while (length --)
                crc = ng_btsocket_rfcomm_crc_table[crc ^ *data++];

        return (crc);
} /* ng_btsocket_rfcomm_crc */

/* FCS on 2 bytes */
static u_int8_t
ng_btsocket_rfcomm_fcs2(u_int8_t *data)
{
        return (0xff - ng_btsocket_rfcomm_crc(data, 2));
} /* ng_btsocket_rfcomm_fcs2 */
  
/* FCS on 3 bytes */
static u_int8_t
ng_btsocket_rfcomm_fcs3(u_int8_t *data)
{
        return (0xff - ng_btsocket_rfcomm_crc(data, 3));
} /* ng_btsocket_rfcomm_fcs3 */

/* 
 * Check FCS
 *
 * From Bluetooth spec
 *
 * "... In 07.10, the frame check sequence (FCS) is calculated on different 
 * sets of fields for different frame types. These are the fields that the 
 * FCS are calculated on:
 *
 * For SABM, DISC, UA, DM frames: on Address, Control and length field.
 * For UIH frames: on Address and Control field.
 *
 * (This is stated here for clarification, and to set the standard for RFCOMM;
 * the fields included in FCS calculation have actually changed in version
 * 7.0.0 of TS 07.10, but RFCOMM will not change the FCS calculation scheme
 * from the one above.) ..."
 */

static int
ng_btsocket_rfcomm_check_fcs(u_int8_t *data, int type, u_int8_t fcs)
{
        if (type != RFCOMM_FRAME_UIH)
                return (ng_btsocket_rfcomm_fcs3(data) != fcs);

        return (ng_btsocket_rfcomm_fcs2(data) != fcs);
} /* ng_btsocket_rfcomm_check_fcs */

/*****************************************************************************
 *****************************************************************************
 **                              Socket interface
 *****************************************************************************
 *****************************************************************************/

/* 
 * Initialize everything
 */

void
ng_btsocket_rfcomm_init(void)
{
        ng_btsocket_rfcomm_debug_level = NG_BTSOCKET_WARN_LEVEL;
        ng_btsocket_rfcomm_timo = 60;

        /* RFCOMM task */
        TASK_INIT(&ng_btsocket_rfcomm_task, 0,
                ng_btsocket_rfcomm_sessions_task, NULL);

        /* RFCOMM sessions list */
        LIST_INIT(&ng_btsocket_rfcomm_sessions);
        lockinit(&ng_btsocket_rfcomm_sessions_lock,
                "btsocks_rfcomm_sessions_lock", 0, 0);

        /* RFCOMM sockets list */
        LIST_INIT(&ng_btsocket_rfcomm_sockets);
        lockinit(&ng_btsocket_rfcomm_sockets_lock,
                "btsocks_rfcomm_sockets_lock", 0, 0);
} /* ng_btsocket_rfcomm_init */

/*
 * Abort connection on socket
 */

void
ng_btsocket_rfcomm_abort(netmsg_t msg)
{
        struct socket                   *so = msg->abort.base.nm_so;

        so->so_error = ECONNABORTED;
        ng_btsocket_rfcomm_disconnect(msg);
} /* ng_btsocket_rfcomm_abort */

#if 0 /* XXX */
void
ng_btsocket_rfcomm_close(struct socket *so)
{

        (void)ng_btsocket_rfcomm_disconnect(so);
} /* ng_btsocket_rfcomm_close */
#endif

/*
 * Accept connection on socket. Nothing to do here, socket must be connected
 * and ready, so just return peer address and be done with it.
 */

void
ng_btsocket_rfcomm_accept(netmsg_t msg)
{
        ng_btsocket_rfcomm_peeraddr(msg);
} /* ng_btsocket_rfcomm_accept */

/*
 * Create and attach new socket
 */

void
ng_btsocket_rfcomm_attach(netmsg_t msg)
{
        struct socket                   *so = msg->attach.base.nm_so;
        ng_btsocket_rfcomm_pcb_p         pcb = so2rfcomm_pcb(so);
        int                              error = 0;

        /* Check socket and protocol */
        if (so->so_type != SOCK_STREAM) {
                error = ESOCKTNOSUPPORT;
                goto out;
        }

#if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */
        if (proto != 0) 
                if (proto != BLUETOOTH_PROTO_RFCOMM) {
                        error = EPROTONOSUPPORT;
                        goto out;
                }
#endif /* XXX */

        if (pcb != NULL) {
                error = EISCONN;
                goto out;
        }

        /* Reserve send and receive space if it is not reserved yet */
        if ((so->so_snd.ssb_hiwat == 0) || (so->so_rcv.ssb_hiwat == 0)) {
                error = soreserve(so, NG_BTSOCKET_RFCOMM_SENDSPACE,
                                        NG_BTSOCKET_RFCOMM_RECVSPACE, NULL);
                if (error != 0)
                        goto out;
        }

        /* Allocate the PCB */
        pcb = kmalloc(sizeof(*pcb), M_NETGRAPH_BTSOCKET_RFCOMM,
                      M_WAITOK | M_NULLOK | M_ZERO);
        if (pcb == NULL) {
                error = ENOMEM;
                goto out;
        }

        /* Link the PCB and the socket */
        so->so_pcb = (caddr_t) pcb;
        pcb->so = so;

        /* Initialize PCB */
        pcb->state = NG_BTSOCKET_RFCOMM_DLC_CLOSED;
        pcb->flags = NG_BTSOCKET_RFCOMM_DLC_CFC;

        pcb->lmodem =
        pcb->rmodem = (RFCOMM_MODEM_RTC | RFCOMM_MODEM_RTR | RFCOMM_MODEM_DV);

        pcb->mtu = RFCOMM_DEFAULT_MTU;
        pcb->tx_cred = 0;
        pcb->rx_cred = RFCOMM_DEFAULT_CREDITS;

        lockinit(&pcb->pcb_lock, "btsocks_rfcomm_pcb_lock", 0, 0);
        callout_init_mp(&pcb->timo);

        /* Add the PCB to the list */
        lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_EXCLUSIVE);
        LIST_INSERT_HEAD(&ng_btsocket_rfcomm_sockets, pcb, next);
        lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_RELEASE);

out:
        lwkt_replymsg(&msg->attach.base.lmsg, error);
} /* ng_btsocket_rfcomm_attach */

/*
 * Bind socket
 */

void
ng_btsocket_rfcomm_bind(netmsg_t msg)
{
        struct socket                   *so = msg->bind.base.nm_so;
        struct sockaddr                 *nam = msg->bind.nm_nam;
        ng_btsocket_rfcomm_pcb_t        *pcb = so2rfcomm_pcb(so), *pcb1;
        struct sockaddr_rfcomm          *sa = (struct sockaddr_rfcomm *) nam;
        int                              error = 0;

        if (pcb == NULL) {
                error = EINVAL;
                goto out;
        }

        /* Verify address */
        if (sa == NULL) {
                error = EINVAL;
                goto out;
        }
        if (sa->rfcomm_family != AF_BLUETOOTH) {
                error = EAFNOSUPPORT;
                goto out;
        }
        if (sa->rfcomm_len != sizeof(*sa)) {
                error = EINVAL;
                goto out;
        }
        if (sa->rfcomm_channel > 30) {
                error = EINVAL;
                goto out;
        }

        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        if (sa->rfcomm_channel != 0) {
                lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_EXCLUSIVE);

                LIST_FOREACH(pcb1, &ng_btsocket_rfcomm_sockets, next) {
                        if (pcb1->channel == sa->rfcomm_channel &&
                            bcmp(&pcb1->src, &sa->rfcomm_bdaddr,
                                        sizeof(pcb1->src)) == 0) {
                                lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_RELEASE);
                                lockmgr(&pcb->pcb_lock, LK_RELEASE);

                                error = EADDRINUSE;
                                goto out;
                        }
                }

                lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_RELEASE);
        }

        bcopy(&sa->rfcomm_bdaddr, &pcb->src, sizeof(pcb->src));
        pcb->channel = sa->rfcomm_channel;

        lockmgr(&pcb->pcb_lock, LK_RELEASE);

out:
        lwkt_replymsg(&msg->bind.base.lmsg, error);
} /* ng_btsocket_rfcomm_bind */

/*
 * Connect socket
 */

void
ng_btsocket_rfcomm_connect(netmsg_t msg)
{
        struct socket                   *so = msg->connect.base.nm_so;
        struct sockaddr                 *nam = msg->connect.nm_nam;
        struct thread                   *td = msg->connect.nm_td;
        ng_btsocket_rfcomm_pcb_t        *pcb = so2rfcomm_pcb(so);
        struct sockaddr_rfcomm          *sa = (struct sockaddr_rfcomm *) nam;
        ng_btsocket_rfcomm_session_t    *s = NULL;
        struct socket                   *l2so = NULL;
        int                              dlci, error = 0;

        if (pcb == NULL) {
                error = EINVAL;
                goto out;
        }

        /* Verify address */
        if (sa == NULL) {
                error = EINVAL;
                goto out;
        }
        if (sa->rfcomm_family != AF_BLUETOOTH) {
                error = EAFNOSUPPORT;
                goto out;
        }
        if (sa->rfcomm_len != sizeof(*sa)) {
                error = EINVAL;
                goto out;
        }
        if (sa->rfcomm_channel > 30) {
                error = EINVAL;
                goto out;
        }
        if (sa->rfcomm_channel == 0 ||
            bcmp(&sa->rfcomm_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) {
                error = EDESTADDRREQ;
                goto out;
        }

        /*
         * XXX FIXME - This is FUBAR. socreate() will call soalloc(1), i.e.
         * soalloc() is allowed to sleep in MALLOC. This creates "could sleep"
         * WITNESS warnings. To work around this problem we will create L2CAP
         * socket first and then check if we actually need it. Note that we 
         * will not check for errors in socreate() because if we failed to 
         * create L2CAP socket at this point we still might have already open
         * session.
         */

        error = socreate(PF_BLUETOOTH, &l2so, SOCK_SEQPACKET,
                        BLUETOOTH_PROTO_L2CAP, td);

        /* 
         * Look for session between "pcb->src" and "sa->rfcomm_bdaddr" (dst)
         */

        lockmgr(&ng_btsocket_rfcomm_sessions_lock, LK_EXCLUSIVE);

        s = ng_btsocket_rfcomm_session_by_addr(&pcb->src, &sa->rfcomm_bdaddr);
        if (s == NULL) {
                /*
                 * We need to create new RFCOMM session. Check if we have L2CAP
                 * socket. If l2so == NULL then error has the error code from
                 * socreate()
                 */

                if (l2so == NULL) {
                        lockmgr(&ng_btsocket_rfcomm_sessions_lock, LK_RELEASE);
                        goto out;
                }

                error = ng_btsocket_rfcomm_session_create(&s, l2so,
                                &pcb->src, &sa->rfcomm_bdaddr, td);
                if (error != 0) {
                        lockmgr(&ng_btsocket_rfcomm_sessions_lock, LK_RELEASE);
                        soclose(l2so, FNONBLOCK);

                        goto out;
                }
        } else if (l2so != NULL)
                soclose(l2so, FNONBLOCK); /* we don't need new L2CAP socket */

        /*
         * Check if we already have the same DLCI the the same session
         */

        lockmgr(&s->session_lock, LK_EXCLUSIVE);
        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        dlci = RFCOMM_MKDLCI(!INITIATOR(s), sa->rfcomm_channel);

        if (ng_btsocket_rfcomm_pcb_by_dlci(s, dlci) != NULL) {
                lockmgr(&pcb->pcb_lock, LK_RELEASE);
                lockmgr(&s->session_lock, LK_RELEASE);
                lockmgr(&ng_btsocket_rfcomm_sessions_lock, LK_RELEASE);

                error = EBUSY;
                goto out;
        }

        /*
         * Check session state and if its not acceptable then refuse connection
         */

        switch (s->state) {
        case NG_BTSOCKET_RFCOMM_SESSION_CONNECTING:
        case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
        case NG_BTSOCKET_RFCOMM_SESSION_OPEN:
                /*
                 * Update destination address and channel and attach 
                 * DLC to the session
                 */

                bcopy(&sa->rfcomm_bdaddr, &pcb->dst, sizeof(pcb->dst));
                pcb->channel = sa->rfcomm_channel;
                pcb->dlci = dlci;

                LIST_INSERT_HEAD(&s->dlcs, pcb, session_next);
                pcb->session = s;

                ng_btsocket_rfcomm_timeout(pcb);
                soisconnecting(pcb->so);

                if (s->state == NG_BTSOCKET_RFCOMM_SESSION_OPEN) {
                        pcb->mtu = s->mtu;
                        bcopy(&so2l2cap_pcb(s->l2so)->src, &pcb->src,
                                sizeof(pcb->src));

                        pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONFIGURING;

                        error = ng_btsocket_rfcomm_send_pn(pcb);
                        if (error == 0)
                                error = ng_btsocket_rfcomm_task_wakeup();
                } else
                        pcb->state = NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT;
                break;

        default:
                error = ECONNRESET;
                break;
        }

        lockmgr(&pcb->pcb_lock, LK_RELEASE);
        lockmgr(&s->session_lock, LK_RELEASE);
        lockmgr(&ng_btsocket_rfcomm_sessions_lock, LK_RELEASE);

out:
        lwkt_replymsg(&msg->connect.base.lmsg, error);
} /* ng_btsocket_rfcomm_connect */

/*
 * Process ioctl's calls on socket.
 * XXX FIXME this should provide interface to the RFCOMM multiplexor channel
 */

void
ng_btsocket_rfcomm_control(netmsg_t msg)
{
        lwkt_replymsg(&msg->control.base.lmsg, EINVAL);
} /* ng_btsocket_rfcomm_control */

/*
 * Process getsockopt/setsockopt system calls
 */

void
ng_btsocket_rfcomm_ctloutput(netmsg_t msg)
{
        struct socket                           *so = msg->ctloutput.base.nm_so;
        struct sockopt                          *sopt = msg->ctloutput.nm_sopt;
        ng_btsocket_rfcomm_pcb_p                 pcb = so2rfcomm_pcb(so);
        struct ng_btsocket_rfcomm_fc_info        fcinfo;
        int                                      error = 0;

        if (pcb == NULL) {
                error = EINVAL;
                goto out;
        }
        if (sopt->sopt_level != SOL_RFCOMM)
                goto out;

        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        switch (sopt->sopt_dir) {
        case SOPT_GET:
                switch (sopt->sopt_name) {
                case SO_RFCOMM_MTU:
                        error = sooptcopyout(sopt, &pcb->mtu, sizeof(pcb->mtu));
                        break;

                case SO_RFCOMM_FC_INFO:
                        fcinfo.lmodem = pcb->lmodem;
                        fcinfo.rmodem = pcb->rmodem;
                        fcinfo.tx_cred = pcb->tx_cred;
                        fcinfo.rx_cred = pcb->rx_cred;
                        fcinfo.cfc = (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC)?
                                1 : 0;
                        fcinfo.reserved = 0;

                        error = sooptcopyout(sopt, &fcinfo, sizeof(fcinfo));
                        break;

                default:
                        error = ENOPROTOOPT;
                        break;
                }
                break;

        case SOPT_SET:
                switch (sopt->sopt_name) {
                default:
                        error = ENOPROTOOPT;
                        break;
                }
                break;

        default:
                error = EINVAL;
                break;
        }

        lockmgr(&pcb->pcb_lock, LK_RELEASE);

out:
        lwkt_replymsg(&msg->ctloutput.base.lmsg, error);
} /* ng_btsocket_rfcomm_ctloutput */

/*
 * Detach and destroy socket
 */

void
ng_btsocket_rfcomm_detach(netmsg_t msg)
{
        struct socket                   *so = msg->detach.base.nm_so;
        ng_btsocket_rfcomm_pcb_p         pcb = so2rfcomm_pcb(so);
        int                              error = 0;

        KASSERT(pcb != NULL, ("ng_btsocket_rfcomm_detach: pcb == NULL"));

        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        so->so_pcb = NULL;

        switch (pcb->state) {
        case NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT:
        case NG_BTSOCKET_RFCOMM_DLC_CONFIGURING:
        case NG_BTSOCKET_RFCOMM_DLC_CONNECTING:
        case NG_BTSOCKET_RFCOMM_DLC_CONNECTED:
                /* XXX What to do with pending request? */
                if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)
                        ng_btsocket_rfcomm_untimeout(pcb);

                if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT)
                        pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_DETACHED;
                else
                        pcb->state = NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING;

                ng_btsocket_rfcomm_task_wakeup();
                break;

        case NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING:
                ng_btsocket_rfcomm_task_wakeup();
                break;
        }
        
        while (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CLOSED)
                lksleep(&pcb->state, &pcb->pcb_lock, 0, "rf_det", 0);

        if (pcb->session != NULL)
                panic("%s: pcb->session != NULL", __func__);
        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)
                panic("%s: timeout on closed DLC, flags=%#x",
                        __func__, pcb->flags);

        lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_EXCLUSIVE);
        LIST_REMOVE(pcb, next);
        lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_RELEASE);

        lockmgr(&pcb->pcb_lock, LK_RELEASE);

        lockuninit(&pcb->pcb_lock);
        bzero(pcb, sizeof(*pcb));
        kfree(pcb, M_NETGRAPH_BTSOCKET_RFCOMM);

        soisdisconnected(so);
        sofree(so);             /* for so_pcb = NULL */

        lwkt_replymsg(&msg->detach.base.lmsg, error);
} /* ng_btsocket_rfcomm_detach */

/*
 * Disconnect socket
 */

void
ng_btsocket_rfcomm_disconnect(netmsg_t msg)
{
        struct socket                   *so = msg->disconnect.base.nm_so;
        ng_btsocket_rfcomm_pcb_p         pcb = so2rfcomm_pcb(so);
        int                              error = 0;

        if (pcb == NULL) {
                error = EINVAL;
                goto out;
        }

        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING) {
                lockmgr(&pcb->pcb_lock, LK_RELEASE);
                error = EINPROGRESS;
                goto out;
        }

        /* XXX What to do with pending request? */
        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)
                ng_btsocket_rfcomm_untimeout(pcb);

        switch (pcb->state) {
        case NG_BTSOCKET_RFCOMM_DLC_CONFIGURING: /* XXX can we get here? */
        case NG_BTSOCKET_RFCOMM_DLC_CONNECTING: /* XXX can we get here? */
        case NG_BTSOCKET_RFCOMM_DLC_CONNECTED:

                /*
                 * Just change DLC state and enqueue RFCOMM task. It will
                 * queue and send DISC on the DLC.
                 */ 

                pcb->state = NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING;
                soisdisconnecting(so);

                ng_btsocket_rfcomm_task_wakeup();
                break;

        case NG_BTSOCKET_RFCOMM_DLC_CLOSED:
        case NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT:
                break;

        default:
                panic("%s: Invalid DLC state=%d, flags=%#x",
                        __func__, pcb->state, pcb->flags);
                break;
        }

        lockmgr(&pcb->pcb_lock, LK_RELEASE);

out:
        lwkt_replymsg(&msg->disconnect.base.lmsg, error);
} /* ng_btsocket_rfcomm_disconnect */

/*
 * Listen on socket. First call to listen() will create listening RFCOMM session
 */

void
ng_btsocket_rfcomm_listen(netmsg_t msg)
{
        struct socket                   *so = msg->listen.base.nm_so;
        struct thread                   *td = msg->listen.nm_td;
        int                              backlog = msg->listen.nm_flags; /* XXX */
        ng_btsocket_rfcomm_pcb_p         pcb = so2rfcomm_pcb(so), pcb1;
        ng_btsocket_rfcomm_session_p     s = NULL;
        struct socket                   *l2so = NULL;
        int                              error = 0, socreate_error, usedchannels;

        if (pcb == NULL) {
                error = EINVAL;
                goto out2;
        }
        if (pcb->channel > 30) {
                error = EADDRNOTAVAIL;
                goto out2;
        }

        usedchannels = 0;

        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        if (pcb->channel == 0) {
                lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_EXCLUSIVE);

                LIST_FOREACH(pcb1, &ng_btsocket_rfcomm_sockets, next)
                        if (pcb1->channel != 0 &&
                            bcmp(&pcb1->src, &pcb->src, sizeof(pcb->src)) == 0)
                                usedchannels |= (1 << (pcb1->channel - 1));

                for (pcb->channel = 30; pcb->channel > 0; pcb->channel --)
                        if (!(usedchannels & (1 << (pcb->channel - 1))))
                                break;

                if (pcb->channel == 0) {
                        lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_RELEASE);
                        lockmgr(&pcb->pcb_lock, LK_RELEASE);

                        error = EADDRNOTAVAIL;
                        goto out2;
                }

                lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_RELEASE);
        }

        lockmgr(&pcb->pcb_lock, LK_RELEASE);

        /*
         * XXX FIXME - This is FUBAR. socreate() will call soalloc(1), i.e.
         * soalloc() is allowed to sleep in MALLOC. This creates "could sleep"
         * WITNESS warnings. To work around this problem we will create L2CAP
         * socket first and then check if we actually need it. Note that we 
         * will not check for errors in socreate() because if we failed to 
         * create L2CAP socket at this point we still might have already open
         * session.
         */

        socreate_error = socreate(PF_BLUETOOTH, &l2so, SOCK_SEQPACKET,
                        BLUETOOTH_PROTO_L2CAP, td);

        /*
         * Transition the socket and session into the LISTENING state.  Check
         * for collisions first, as there can only be one.
         */
        lockmgr(&ng_btsocket_rfcomm_sessions_lock, LK_EXCLUSIVE);
        if (so->so_state &
            (SS_ISCONNECTED | SS_ISCONNECTING | SS_ISDISCONNECTING)) {
                error = EINVAL;
                goto out;
        }

        LIST_FOREACH(s, &ng_btsocket_rfcomm_sessions, next)
                if (s->state == NG_BTSOCKET_RFCOMM_SESSION_LISTENING)
                        break;

        if (s == NULL) {
                /*
                 * We need to create default RFCOMM session. Check if we have 
                 * L2CAP socket. If l2so == NULL then error has the error code 
                 * from socreate()
                 */
                if (l2so == NULL) {
                        error = socreate_error;
                        goto out;
                }

                /* 
                 * Create default listen RFCOMM session. The default RFCOMM 
                 * session will listen on ANY address.
                 *
                 * XXX FIXME Note that currently there is no way to adjust MTU
                 * for the default session.
                 */
                error = ng_btsocket_rfcomm_session_create(&s, l2so,
                                        NG_HCI_BDADDR_ANY, NULL, td);
                if (error != 0)
                        goto out;
                l2so = NULL;
        }
        solisten(so, backlog, td);

out:
        lockmgr(&ng_btsocket_rfcomm_sessions_lock, LK_RELEASE);
        /*
         * If we still have an l2so reference here, it's unneeded, so release
         * it.
         */
        if (l2so != NULL)
                soclose(l2so, FNONBLOCK);

out2:
        lwkt_replymsg(&msg->listen.base.lmsg, error);
} /* ng_btsocket_listen */

/*
 * Get peer address
 */

void
ng_btsocket_rfcomm_peeraddr(netmsg_t msg)
{
        struct socket                    *so = msg->peeraddr.base.nm_so;
        struct sockaddr                 **nam = msg->peeraddr.nm_nam;
        ng_btsocket_rfcomm_pcb_p          pcb = so2rfcomm_pcb(so);
        struct sockaddr_rfcomm            sa;
        int                               error = 0;

        if (pcb == NULL) {
                error = EINVAL;
                goto out;
        }

        bcopy(&pcb->dst, &sa.rfcomm_bdaddr, sizeof(sa.rfcomm_bdaddr));
        sa.rfcomm_channel = pcb->channel;
        sa.rfcomm_len = sizeof(sa);
        sa.rfcomm_family = AF_BLUETOOTH;

        *nam = dup_sockaddr((struct sockaddr *) &sa);

        if (*nam == NULL)
                error = ENOMEM;

out:
        lwkt_replymsg(&msg->peeraddr.base.lmsg, error);
} /* ng_btsocket_rfcomm_peeraddr */

/*
 * Send data to socket
 */

void
ng_btsocket_rfcomm_send(netmsg_t msg)
{
        struct socket                   *so = msg->send.base.nm_so;
        struct mbuf                     *m = msg->send.nm_m;
        struct mbuf                     *control = msg->send.nm_control;
        ng_btsocket_rfcomm_pcb_t        *pcb = so2rfcomm_pcb(so);
        int                              error = 0;

        /* Check socket and input */
        if (pcb == NULL || m == NULL || control != NULL) {
                error = EINVAL;
                goto drop;
        }

        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        /* Make sure DLC is connected */
        if (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTED) {
                lockmgr(&pcb->pcb_lock, LK_RELEASE);
                error = ENOTCONN;
                goto drop;
        }

        /* Put the packet on the socket's send queue and wakeup RFCOMM task */
        sbappend(&pcb->so->so_snd.sb, m);
        m = NULL;
        
        if (!(pcb->flags & NG_BTSOCKET_RFCOMM_DLC_SENDING)) {
                pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_SENDING;
                error = ng_btsocket_rfcomm_task_wakeup();
        }

        lockmgr(&pcb->pcb_lock, LK_RELEASE);
drop:
        NG_FREE_M(m); /* checks for != NULL */
        NG_FREE_M(control);

        lwkt_replymsg(&msg->send.base.lmsg, error);
} /* ng_btsocket_rfcomm_send */

/*
 * Get socket address
 */

void
ng_btsocket_rfcomm_sockaddr(netmsg_t msg)
{
        struct socket                    *so = msg->sockaddr.base.nm_so;
        struct sockaddr                 **nam = msg->sockaddr.nm_nam;
        ng_btsocket_rfcomm_pcb_p          pcb = so2rfcomm_pcb(so);
        struct sockaddr_rfcomm            sa;
        int                               error = 0;

        if (pcb == NULL) {
                error = EINVAL;
                goto out;
        }

        bcopy(&pcb->src, &sa.rfcomm_bdaddr, sizeof(sa.rfcomm_bdaddr));
        sa.rfcomm_channel = pcb->channel;
        sa.rfcomm_len = sizeof(sa);
        sa.rfcomm_family = AF_BLUETOOTH;

        *nam = dup_sockaddr((struct sockaddr *) &sa);

        if (*nam == NULL)
                error = ENOMEM;

out:
        lwkt_replymsg(&msg->sockaddr.base.lmsg, error);
} /* ng_btsocket_rfcomm_sockaddr */

/*
 * Upcall function for L2CAP sockets. Enqueue RFCOMM task.
 */

static void
ng_btsocket_rfcomm_upcall(struct socket *so, void *arg, int waitflag)
{
        int     error;

        if (so == NULL)
                panic("%s: so == NULL", __func__);

        if ((error = ng_btsocket_rfcomm_task_wakeup()) != 0)
                NG_BTSOCKET_RFCOMM_ALERT(
"%s: Could not enqueue RFCOMM task, error=%d\n", __func__, error);
} /* ng_btsocket_rfcomm_upcall */

/*
 * RFCOMM task. Will handle all RFCOMM sessions in one pass.
 * XXX FIXME does not scale very well
 */

static void
ng_btsocket_rfcomm_sessions_task(void *ctx, int pending)
{
        ng_btsocket_rfcomm_session_p    s = NULL, s_next = NULL;

        lockmgr(&ng_btsocket_rfcomm_sessions_lock, LK_EXCLUSIVE);

        for (s = LIST_FIRST(&ng_btsocket_rfcomm_sessions); s != NULL; ) {
                lockmgr(&s->session_lock, LK_EXCLUSIVE);
                s_next = LIST_NEXT(s, next);

                ng_btsocket_rfcomm_session_task(s);

                if (s->state == NG_BTSOCKET_RFCOMM_SESSION_CLOSED) {
                        /* Unlink and clean the session */
                        LIST_REMOVE(s, next);

                        NG_BT_MBUFQ_DRAIN(&s->outq);
                        if (!LIST_EMPTY(&s->dlcs))
                                panic("%s: DLC list is not empty", __func__);

                        /* Close L2CAP socket */
                        s->l2so->so_upcallarg = NULL;
                        s->l2so->so_upcall = NULL;
                        atomic_clear_int(&s->l2so->so_rcv.ssb_flags, SSB_UPCALL);
                        atomic_clear_int(&s->l2so->so_snd.ssb_flags, SSB_UPCALL);
                        soclose(s->l2so, FNONBLOCK);

                        lockmgr(&s->session_lock, LK_RELEASE);

                        lockuninit(&s->session_lock);
                        bzero(s, sizeof(*s));
                        kfree(s, M_NETGRAPH_BTSOCKET_RFCOMM);
                } else
                        lockmgr(&s->session_lock, LK_RELEASE);

                s = s_next;
        }

        lockmgr(&ng_btsocket_rfcomm_sessions_lock, LK_RELEASE);
} /* ng_btsocket_rfcomm_sessions_task */

/*
 * Process RFCOMM session. Will handle all RFCOMM sockets in one pass.
 */

static void
ng_btsocket_rfcomm_session_task(ng_btsocket_rfcomm_session_p s)
{
        KKASSERT(lockowned(&s->session_lock) != 0);

        if (s->l2so->so_state & SS_CANTRCVMORE) {
                NG_BTSOCKET_RFCOMM_INFO(
"%s: L2CAP connection has been terminated, so=%p, so_state=%#x, " \
"state=%d, flags=%#x\n", __func__, s->l2so, s->l2so->so_state, 
                        s->state, s->flags);

                s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
                ng_btsocket_rfcomm_session_clean(s);
        }

        /* Now process upcall */
        switch (s->state) {
        /* Try to accept new L2CAP connection(s) */
        case NG_BTSOCKET_RFCOMM_SESSION_LISTENING:
                while (ng_btsocket_rfcomm_session_accept(s) == 0)
                        ;
                break;

        /* Process the results of the L2CAP connect */
        case NG_BTSOCKET_RFCOMM_SESSION_CONNECTING:
                ng_btsocket_rfcomm_session_process_pcb(s);

                if (ng_btsocket_rfcomm_session_connect(s) != 0) {
                        s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
                        ng_btsocket_rfcomm_session_clean(s);
                } 
                break;

        /* Try to receive/send more data */
        case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
        case NG_BTSOCKET_RFCOMM_SESSION_OPEN:
        case NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING:
                ng_btsocket_rfcomm_session_process_pcb(s);

                if (ng_btsocket_rfcomm_session_receive(s) != 0) {
                        s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
                        ng_btsocket_rfcomm_session_clean(s);
                } else if (ng_btsocket_rfcomm_session_send(s) != 0) {
                        s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
                        ng_btsocket_rfcomm_session_clean(s);
                }
                break;

        case NG_BTSOCKET_RFCOMM_SESSION_CLOSED:
                break;

        default:
                panic("%s: Invalid session state=%d, flags=%#x",
                        __func__, s->state, s->flags);
                break;
        }
} /* ng_btsocket_rfcomm_session_task */

/*
 * Process RFCOMM connection indicator. Caller must hold s->session_mtx
 */

static ng_btsocket_rfcomm_pcb_p
ng_btsocket_rfcomm_connect_ind(ng_btsocket_rfcomm_session_p s, int channel)
{
        ng_btsocket_rfcomm_pcb_p         pcb = NULL, pcb1 = NULL;
        ng_btsocket_l2cap_pcb_p          l2pcb = NULL;
        struct socket                   *so1 = NULL;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /*
         * Try to find RFCOMM socket that listens on given source address 
         * and channel. This will return the best possible match.
         */

        l2pcb = so2l2cap_pcb(s->l2so);
        pcb = ng_btsocket_rfcomm_pcb_listener(&l2pcb->src, channel);
        if (pcb == NULL)
                return (NULL);

        /*
         * Check the pending connections queue and if we have space then 
         * create new socket and set proper source and destination address,
         * and channel.
         */

        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        if (pcb->so->so_qlen <= pcb->so->so_qlimit)
                so1 = sonewconn(pcb->so, 0);

        lockmgr(&pcb->pcb_lock, LK_RELEASE);

        if (so1 == NULL)
                return (NULL);

        /*
         * If we got here than we have created new socket. So complete the 
         * connection. Set source and destination address from the session.
         */

        pcb1 = so2rfcomm_pcb(so1);
        if (pcb1 == NULL)
                panic("%s: pcb1 == NULL", __func__);

        lockmgr(&pcb1->pcb_lock, LK_EXCLUSIVE);

        bcopy(&l2pcb->src, &pcb1->src, sizeof(pcb1->src));
        bcopy(&l2pcb->dst, &pcb1->dst, sizeof(pcb1->dst));
        pcb1->channel = channel;

        /* Link new DLC to the session. We already hold s->session_mtx */
        LIST_INSERT_HEAD(&s->dlcs, pcb1, session_next);
        pcb1->session = s;
                        
        lockmgr(&pcb1->pcb_lock, LK_RELEASE);

        return (pcb1);
} /* ng_btsocket_rfcomm_connect_ind */

/*
 * Process RFCOMM connect confirmation. Caller must hold s->session_mtx.
 */

static void
ng_btsocket_rfcomm_connect_cfm(ng_btsocket_rfcomm_session_p s)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL, pcb_next = NULL;
        int                             error;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /*
         * Wake up all waiting sockets and send PN request for each of them. 
         * Note that timeout already been set in ng_btsocket_rfcomm_connect()
         *
         * Note: cannot use LIST_FOREACH because ng_btsocket_rfcomm_pcb_kill
         * will unlink DLC from the session
         */

        for (pcb = LIST_FIRST(&s->dlcs); pcb != NULL; ) {
                lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
                pcb_next = LIST_NEXT(pcb, session_next);

                if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT) {
                        pcb->mtu = s->mtu;
                        bcopy(&so2l2cap_pcb(s->l2so)->src, &pcb->src,
                                sizeof(pcb->src));

                        error = ng_btsocket_rfcomm_send_pn(pcb);
                        if (error == 0)
                                pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONFIGURING;
                        else
                                ng_btsocket_rfcomm_pcb_kill(pcb, error);
                }

                lockmgr(&pcb->pcb_lock, LK_RELEASE);
                pcb = pcb_next;
        }
} /* ng_btsocket_rfcomm_connect_cfm */

/*****************************************************************************
 *****************************************************************************
 **                              RFCOMM sessions
 *****************************************************************************
 *****************************************************************************/

/*
 * Create new RFCOMM session. That function WILL NOT take ownership over l2so.
 * Caller MUST free l2so if function failed.
 */

static int
ng_btsocket_rfcomm_session_create(ng_btsocket_rfcomm_session_p *sp,
                struct socket *l2so, bdaddr_p src, bdaddr_p dst,
                struct thread *td)
{
        ng_btsocket_rfcomm_session_p    s = NULL;
        struct sockaddr_l2cap           l2sa;
        struct sockopt                  l2sopt;
        int                             error;
        u_int16_t                       mtu;

        KKASSERT(lockowned(&ng_btsocket_rfcomm_sessions_lock) != 0);

        /* Allocate the RFCOMM session */
        s = kmalloc(sizeof(*s), M_NETGRAPH_BTSOCKET_RFCOMM,
                    M_WAITOK | M_NULLOK | M_ZERO);
        if (s == NULL)
                return (ENOMEM);

        /* Set defaults */
        s->mtu = RFCOMM_DEFAULT_MTU;
        s->flags = 0;
        s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
        NG_BT_MBUFQ_INIT(&s->outq, ifqmaxlen);

        /*
         * XXX Mark session mutex as DUPOK to prevent "duplicated lock of 
         * the same type" message. When accepting new L2CAP connection
         * ng_btsocket_rfcomm_session_accept() holds both session mutexes 
         * for "old" (accepting) session and "new" (created) session.
         */

        lockinit(&s->session_lock, "btsocks_rfcomm_session_lock", 0,
            LK_CANRECURSE);

        LIST_INIT(&s->dlcs);

        /* Prepare L2CAP socket */
        l2so->so_upcallarg = NULL;
        l2so->so_upcall = ng_btsocket_rfcomm_upcall;
        atomic_set_int(&l2so->so_rcv.ssb_flags, SSB_UPCALL);
        atomic_set_int(&l2so->so_snd.ssb_flags, SSB_UPCALL);
        s->l2so = l2so;

        lockmgr(&s->session_lock, LK_EXCLUSIVE);

        /*
         * "src" == NULL and "dst" == NULL means just create session.
         * caller must do the rest
         */

        if (src == NULL && dst == NULL)
                goto done;

        /*
         * Set incoming MTU on L2CAP socket. It is RFCOMM session default MTU 
         * plus 5 bytes: RFCOMM frame header, one extra byte for length and one
         * extra byte for credits.
         */

        mtu = s->mtu + sizeof(struct rfcomm_frame_hdr) + 1 + 1;

        l2sopt.sopt_dir = SOPT_SET;
        l2sopt.sopt_level = SOL_L2CAP;
        l2sopt.sopt_name = SO_L2CAP_IMTU;
        l2sopt.sopt_val = (void *) &mtu;
        l2sopt.sopt_valsize = sizeof(mtu);
        l2sopt.sopt_td = NULL;

        error = sosetopt(s->l2so, &l2sopt);
        if (error != 0)
                goto bad;

        /* Bind socket to "src" address */
        l2sa.l2cap_len = sizeof(l2sa);
        l2sa.l2cap_family = AF_BLUETOOTH;
        l2sa.l2cap_psm = (dst == NULL)? htole16(NG_L2CAP_PSM_RFCOMM) : 0;
        bcopy(src, &l2sa.l2cap_bdaddr, sizeof(l2sa.l2cap_bdaddr));

        error = sobind(s->l2so, (struct sockaddr *) &l2sa, td);
        if (error != 0)
                goto bad;

        /* If "dst" is not NULL then initiate connect(), otherwise listen() */
        if (dst == NULL) {
                s->flags = 0;
                s->state = NG_BTSOCKET_RFCOMM_SESSION_LISTENING;

                error = solisten(s->l2so, 10, td);
                if (error != 0)
                        goto bad;
        } else {
                s->flags = NG_BTSOCKET_RFCOMM_SESSION_INITIATOR;
                s->state = NG_BTSOCKET_RFCOMM_SESSION_CONNECTING;

                l2sa.l2cap_len = sizeof(l2sa);   
                l2sa.l2cap_family = AF_BLUETOOTH;
                l2sa.l2cap_psm = htole16(NG_L2CAP_PSM_RFCOMM);
                bcopy(dst, &l2sa.l2cap_bdaddr, sizeof(l2sa.l2cap_bdaddr));

                error = soconnect(s->l2so, (struct sockaddr *) &l2sa, td, TRUE);
                if (error != 0)
                        goto bad;
        }

done:
        LIST_INSERT_HEAD(&ng_btsocket_rfcomm_sessions, s, next);
        *sp = s;

        lockmgr(&s->session_lock, LK_RELEASE);

        return (0);

bad:
        lockmgr(&s->session_lock, LK_RELEASE);

        /* Return L2CAP socket back to its original state */
        l2so->so_upcallarg = NULL;
        l2so->so_upcall = NULL;
        atomic_clear_int(&l2so->so_rcv.ssb_flags, SSB_UPCALL);
        atomic_clear_int(&l2so->so_snd.ssb_flags, SSB_UPCALL);

        lockuninit(&s->session_lock);
        bzero(s, sizeof(*s));
        kfree(s, M_NETGRAPH_BTSOCKET_RFCOMM);

        return (error);
} /* ng_btsocket_rfcomm_session_create */

/*
 * Process accept() on RFCOMM session
 * XXX FIXME locking for "l2so"?
 */

static int
ng_btsocket_rfcomm_session_accept(ng_btsocket_rfcomm_session_p s0)
{
        struct socket                   *l2so = NULL;
        struct sockaddr_l2cap           *l2sa = NULL;
        ng_btsocket_l2cap_pcb_t         *l2pcb = NULL;
        ng_btsocket_rfcomm_session_p     s = NULL;
        int                              error = 0;

        KKASSERT(lockowned(&ng_btsocket_rfcomm_sessions_lock) != 0);
        KKASSERT(lockowned(&s0->session_lock) != 0);

        /* Check if there is a complete L2CAP connection in the queue */
        if ((error = s0->l2so->so_error) != 0) {
                NG_BTSOCKET_RFCOMM_ERR(
"%s: Could not accept connection on L2CAP socket, error=%d\n", __func__, error);
                s0->l2so->so_error = 0;

                return (error);
        }

        if (TAILQ_EMPTY(&s0->l2so->so_comp)) {
                if (s0->l2so->so_state & SS_CANTRCVMORE)
                        return (ECONNABORTED);
                return (EWOULDBLOCK);
        }

        /* Accept incoming L2CAP connection */
        l2so = TAILQ_FIRST(&s0->l2so->so_comp);
        if (l2so == NULL)
                panic("%s: l2so == NULL", __func__);

        TAILQ_REMOVE(&s0->l2so->so_comp, l2so, so_list);
        s0->l2so->so_qlen --;
        l2so->so_head = NULL;
        soreference(l2so);

        error = soaccept(l2so, (struct sockaddr **) &l2sa);
        if (error != 0) {
                NG_BTSOCKET_RFCOMM_ERR(
"%s: soaccept() on L2CAP socket failed, error=%d\n", __func__, error);
                soclose(l2so, FNONBLOCK);

                return (error);
        }

        /*
         * Check if there is already active RFCOMM session between two devices.
         * If so then close L2CAP connection. We only support one RFCOMM session
         * between each pair of devices. Note that here we assume session in any
         * state. The session even could be in the middle of disconnecting.
         */

        l2pcb = so2l2cap_pcb(l2so);
        s = ng_btsocket_rfcomm_session_by_addr(&l2pcb->src, &l2pcb->dst);
        if (s == NULL) {
                /* Create a new RFCOMM session */
                error = ng_btsocket_rfcomm_session_create(&s, l2so, NULL, NULL,
                                curthread /* XXX */);
                if (error == 0) {
                        lockmgr(&s->session_lock, LK_EXCLUSIVE);

                        s->flags = 0;
                        s->state = NG_BTSOCKET_RFCOMM_SESSION_CONNECTED;

                        /*
                         * Adjust MTU on incomming connection. Reserve 5 bytes:
                         * RFCOMM frame header, one extra byte for length and 
                         * one extra byte for credits.
                         */

                        s->mtu = min(l2pcb->imtu, l2pcb->omtu) -
                                        sizeof(struct rfcomm_frame_hdr) - 1 - 1;

                        lockmgr(&s->session_lock, LK_RELEASE);
                } else {
                        NG_BTSOCKET_RFCOMM_ALERT(
"%s: Failed to create new RFCOMM session, error=%d\n", __func__, error);

                        soclose(l2so, FNONBLOCK);
                }
        } else {
                NG_BTSOCKET_RFCOMM_WARN(
"%s: Rejecting duplicating RFCOMM session between src=%x:%x:%x:%x:%x:%x and " \
"dst=%x:%x:%x:%x:%x:%x, state=%d, flags=%#x\n", __func__,
                        l2pcb->src.b[5], l2pcb->src.b[4], l2pcb->src.b[3],
                        l2pcb->src.b[2], l2pcb->src.b[1], l2pcb->src.b[0],
                        l2pcb->dst.b[5], l2pcb->dst.b[4], l2pcb->dst.b[3],
                        l2pcb->dst.b[2], l2pcb->dst.b[1], l2pcb->dst.b[0],
                        s->state, s->flags);

                error = EBUSY;
                soclose(l2so, FNONBLOCK);
        }

        return (error);
} /* ng_btsocket_rfcomm_session_accept */

/*
 * Process connect() on RFCOMM session
 * XXX FIXME locking for "l2so"?
 */

static int
ng_btsocket_rfcomm_session_connect(ng_btsocket_rfcomm_session_p s)
{
        ng_btsocket_l2cap_pcb_p l2pcb = so2l2cap_pcb(s->l2so);
        int                     error;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /* First check if connection has failed */
        if ((error = s->l2so->so_error) != 0) {
                s->l2so->so_error = 0;

                NG_BTSOCKET_RFCOMM_ERR(
"%s: Could not connect RFCOMM session, error=%d, state=%d, flags=%#x\n",
                        __func__, error, s->state, s->flags);

                return (error);
        }

        /* Is connection still in progress? */
        if (s->l2so->so_state & SS_ISCONNECTING)
                return (0); 

        /* 
         * If we got here then we are connected. Send SABM on DLCI 0 to 
         * open multiplexor channel.
         */

        if (error == 0) {
                s->state = NG_BTSOCKET_RFCOMM_SESSION_CONNECTED;

                /*
                 * Adjust MTU on outgoing connection. Reserve 5 bytes: RFCOMM 
                 * frame header, one extra byte for length and one extra byte 
                 * for credits.
                 */

                s->mtu = min(l2pcb->imtu, l2pcb->omtu) -
                                sizeof(struct rfcomm_frame_hdr) - 1 - 1;

                error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_SABM,0);
                if (error == 0)
                        error = ng_btsocket_rfcomm_task_wakeup();
        }

        return (error);
}/* ng_btsocket_rfcomm_session_connect */

/*
 * Receive data on RFCOMM session
 * XXX FIXME locking for "l2so"?
 */

static int
ng_btsocket_rfcomm_session_receive(ng_btsocket_rfcomm_session_p s)
{
        struct sockbuf   sio;
        int              more, error, rcvflg, savelen = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /* Can we read from the L2CAP socket? */
        if (!soreadable(s->l2so))
                return (0);

        /* First check for error on L2CAP socket */
        if ((error = s->l2so->so_error) != 0) {
                s->l2so->so_error = 0;

                NG_BTSOCKET_RFCOMM_ERR(
"%s: Could not receive data from L2CAP socket, error=%d, state=%d, flags=%#x\n",
                        __func__, error, s->state, s->flags);

                return (error);
        }

        /*
         * Read all packets from the L2CAP socket. 
         * XXX FIXME/VERIFY is that correct? For now use m->m_nextpkt as
         * indication that there is more packets on the socket's buffer.
         * Also what should we use in uio.uio_resid?
         * May be s->mtu + sizeof(struct rfcomm_frame_hdr) + 1 + 1?
         */

        sbinit(&sio, 0);
        for (more = 1; more; ) {
                /* Try to get next packet from socket */
                sbinit(&sio, savelen);
                rcvflg = MSG_WAITALL;
                error = soreceive(s->l2so, NULL, NULL, &sio, NULL, &rcvflg);
                if (error != 0) {
                        if (error == EWOULDBLOCK)
                                return (0); /* XXX can happen? */

                        NG_BTSOCKET_RFCOMM_ERR(
"%s: Could not receive data from L2CAP socket, error=%d\n", __func__, error);

                        return (error);
                }
        
                more = (sio.sb_mb->m_nextpkt != NULL);
                sio.sb_mb->m_nextpkt = NULL;

                ng_btsocket_rfcomm_receive_frame(s, sio.sb_mb);
        }

        return (0);
} /* ng_btsocket_rfcomm_session_receive */

/*
 * Send data on RFCOMM session
 * XXX FIXME locking for "l2so"?
 */

static int
ng_btsocket_rfcomm_session_send(ng_btsocket_rfcomm_session_p s)
{
        struct mbuf     *m = NULL;
        int              error;
        struct netmsg_pru_send msg;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /* Send as much as we can from the session queue */
        while (sowriteable(s->l2so)) {
                /* Check if socket still OK */
                if ((error = s->l2so->so_error) != 0) {
                        s->l2so->so_error = 0;

                        NG_BTSOCKET_RFCOMM_ERR(
"%s: Detected error=%d on L2CAP socket, state=%d, flags=%#x\n",
                                __func__, error, s->state, s->flags);

                        return (error);
                }

                NG_BT_MBUFQ_DEQUEUE(&s->outq, m);
                if (m == NULL)
                        return (0); /* we are done */

                /* Call send function on the L2CAP socket */
                netmsg_init(&msg.base, s->l2so, &curthread->td_msgport,
                    0, s->l2so->so_proto->pr_usrreqs->pru_send);
                msg.nm_flags = 0;
                msg.nm_m = m;
                msg.nm_addr = NULL;
                msg.nm_control = NULL;
                msg.nm_td = curthread; /* XXX */
                error = lwkt_domsg(s->l2so->so_port, &msg.base.lmsg, 0);
                if (error != 0) {
                        NG_BTSOCKET_RFCOMM_ERR(
"%s: Could not send data to L2CAP socket, error=%d\n", __func__, error);

                        return (error);
                }
        }

        return (0);
} /* ng_btsocket_rfcomm_session_send */

/*
 * Close and disconnect all DLCs for the given session. Caller must hold 
 * s->sesson_mtx. Will wakeup session.
 */

static void
ng_btsocket_rfcomm_session_clean(ng_btsocket_rfcomm_session_p s)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL, pcb_next = NULL;
        int                             error;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /*
         * Note: cannot use LIST_FOREACH because ng_btsocket_rfcomm_pcb_kill
         * will unlink DLC from the session
         */

        for (pcb = LIST_FIRST(&s->dlcs); pcb != NULL; ) {
                lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
                pcb_next = LIST_NEXT(pcb, session_next);

                NG_BTSOCKET_RFCOMM_INFO(
"%s: Disconnecting dlci=%d, state=%d, flags=%#x\n",
                        __func__, pcb->dlci, pcb->state, pcb->flags);

                if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_CONNECTED)
                        error = ECONNRESET;
                else
                        error = ECONNREFUSED;

                ng_btsocket_rfcomm_pcb_kill(pcb, error);

                lockmgr(&pcb->pcb_lock, LK_RELEASE);
                pcb = pcb_next;
        }
} /* ng_btsocket_rfcomm_session_clean */

/*
 * Process all DLCs on the session. Caller MUST hold s->session_mtx.
 */

static void
ng_btsocket_rfcomm_session_process_pcb(ng_btsocket_rfcomm_session_p s)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL, pcb_next = NULL;
        int                             error;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /*
         * Note: cannot use LIST_FOREACH because ng_btsocket_rfcomm_pcb_kill
         * will unlink DLC from the session
         */

        for (pcb = LIST_FIRST(&s->dlcs); pcb != NULL; ) {
                lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
                pcb_next = LIST_NEXT(pcb, session_next);

                switch (pcb->state) {

                /*
                 * If DLC in W4_CONNECT state then we should check for both
                 * timeout and detach.
                 */

                case NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT:
                        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_DETACHED)
                                ng_btsocket_rfcomm_pcb_kill(pcb, 0);
                        else if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT)
                                ng_btsocket_rfcomm_pcb_kill(pcb, ETIMEDOUT);
                        break;

                /*
                 * If DLC in CONFIGURING or CONNECTING state then we only
                 * should check for timeout. If detach() was called then
                 * DLC will be moved into DISCONNECTING state.
                 */

                case NG_BTSOCKET_RFCOMM_DLC_CONFIGURING:
                case NG_BTSOCKET_RFCOMM_DLC_CONNECTING:
                        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT)
                                ng_btsocket_rfcomm_pcb_kill(pcb, ETIMEDOUT);
                        break;

                /*
                 * If DLC in CONNECTED state then we need to send data (if any)
                 * from the socket's send queue. Note that we will send data
                 * from either all sockets or none. This may overload session's
                 * outgoing queue (but we do not check for that).
                 *
                 * XXX FIXME need scheduler for RFCOMM sockets
                 */

                case NG_BTSOCKET_RFCOMM_DLC_CONNECTED:
                        error = ng_btsocket_rfcomm_pcb_send(pcb, ALOT);
                        if (error != 0)
                                ng_btsocket_rfcomm_pcb_kill(pcb, error);
                        break;

                /*
                 * If DLC in DISCONNECTING state then we must send DISC frame.
                 * Note that if DLC has timeout set then we do not need to 
                 * resend DISC frame.
                 *
                 * XXX FIXME need to drain all data from the socket's queue
                 * if LINGER option was set
                 */

                case NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING:
                        if (!(pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)) {
                                error = ng_btsocket_rfcomm_send_command(
                                                pcb->session, RFCOMM_FRAME_DISC,
                                                pcb->dlci);
                                if (error == 0)
                                        ng_btsocket_rfcomm_timeout(pcb);
                                else
                                        ng_btsocket_rfcomm_pcb_kill(pcb, error);
                        } else if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT)
                                ng_btsocket_rfcomm_pcb_kill(pcb, ETIMEDOUT);
                        break;
                
/*              case NG_BTSOCKET_RFCOMM_DLC_CLOSED: */
                default:
                        panic("%s: Invalid DLC state=%d, flags=%#x",
                                __func__, pcb->state, pcb->flags);
                        break;
                }

                lockmgr(&pcb->pcb_lock, LK_RELEASE);
                pcb = pcb_next;
        }
} /* ng_btsocket_rfcomm_session_process_pcb */

/*
 * Find RFCOMM session between "src" and "dst".
 * Caller MUST hold ng_btsocket_rfcomm_sessions_mtx.
 */

static ng_btsocket_rfcomm_session_p
ng_btsocket_rfcomm_session_by_addr(bdaddr_p src, bdaddr_p dst)
{
        ng_btsocket_rfcomm_session_p    s = NULL;
        ng_btsocket_l2cap_pcb_p         l2pcb = NULL;
        int                             any_src;

        KKASSERT(lockowned(&ng_btsocket_rfcomm_sessions_lock) != 0);

        any_src = (bcmp(src, NG_HCI_BDADDR_ANY, sizeof(*src)) == 0);

        LIST_FOREACH(s, &ng_btsocket_rfcomm_sessions, next) {
                l2pcb = so2l2cap_pcb(s->l2so);

                if ((any_src || bcmp(&l2pcb->src, src, sizeof(*src)) == 0) &&
                    bcmp(&l2pcb->dst, dst, sizeof(*dst)) == 0)
                        break;
        }

        return (s);
} /* ng_btsocket_rfcomm_session_by_addr */

/*****************************************************************************
 *****************************************************************************
 **                                  RFCOMM 
 *****************************************************************************
 *****************************************************************************/

/*
 * Process incoming RFCOMM frame. Caller must hold s->session_mtx.
 * XXX FIXME check frame length
 */

static int
ng_btsocket_rfcomm_receive_frame(ng_btsocket_rfcomm_session_p s,
                struct mbuf *m0)
{
        struct rfcomm_frame_hdr *hdr = NULL;
        struct mbuf             *m = NULL;
        u_int16_t                length;
        u_int8_t                 dlci, type;
        int                      error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /* Pullup as much as we can into first mbuf (for direct access) */
        length = min(m0->m_pkthdr.len, MHLEN);
        if (m0->m_len < length) {
                if ((m0 = m_pullup(m0, length)) == NULL) {
                        NG_BTSOCKET_RFCOMM_ALERT(
"%s: m_pullup(%d) failed\n", __func__, length);

                        return (ENOBUFS);
                }
        }

        hdr = mtod(m0, struct rfcomm_frame_hdr *);
        dlci = RFCOMM_DLCI(hdr->address);
        type = RFCOMM_TYPE(hdr->control);

        /* Test EA bit in length. If not set then we have 2 bytes of length */
        if (!RFCOMM_EA(hdr->length)) {
                bcopy(&hdr->length, &length, sizeof(length));
                length = le16toh(length) >> 1;
                m_adj(m0, sizeof(*hdr) + 1);
        } else {
                length = hdr->length >> 1;
                m_adj(m0, sizeof(*hdr));
        }

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got frame type=%#x, dlci=%d, length=%d, cr=%d, pf=%d, len=%d\n",
                __func__, type, dlci, length, RFCOMM_CR(hdr->address),
                RFCOMM_PF(hdr->control), m0->m_pkthdr.len);

        /*
         * Get FCS (the last byte in the frame)
         * XXX this will not work if mbuf chain ends with empty mbuf.
         * XXX let's hope it never happens :)
         */

        for (m = m0; m->m_next != NULL; m = m->m_next)
                ;
        if (m->m_len <= 0)
                panic("%s: Empty mbuf at the end of the chain, len=%d",
                        __func__, m->m_len);

        /*
         * Check FCS. We only need to calculate FCS on first 2 or 3 bytes
         * and already m_pullup'ed mbuf chain, so it should be safe.
         */

        if (ng_btsocket_rfcomm_check_fcs((u_int8_t *) hdr, type, m->m_data[m->m_len - 1])) {
                NG_BTSOCKET_RFCOMM_ERR(
"%s: Invalid RFCOMM packet. Bad checksum\n", __func__);
                NG_FREE_M(m0);

                return (EINVAL);
        }

        m_adj(m0, -1); /* Trim FCS byte */

        /*
         * Process RFCOMM frame.
         *
         * From TS 07.10 spec
         * 
         * "... In the case where a SABM or DISC command with the P bit set
         * to 0 is received then the received frame shall be discarded..."
         *
         * "... If a unsolicited DM response is received then the frame shall
         * be processed irrespective of the P/F setting... "
         *
         * "... The station may transmit response frames with the F bit set 
         * to 0 at any opportunity on an asynchronous basis. However, in the 
         * case where a UA response is received with the F bit set to 0 then 
         * the received frame shall be discarded..."
         *
         * From Bluetooth spec
         *
         * "... When credit based flow control is being used, the meaning of
         * the P/F bit in the control field of the RFCOMM header is redefined
         * for UIH frames..."
         */

        switch (type) {
        case RFCOMM_FRAME_SABM:
                if (RFCOMM_PF(hdr->control))
                        error = ng_btsocket_rfcomm_receive_sabm(s, dlci);
                break;

        case RFCOMM_FRAME_DISC:
                if (RFCOMM_PF(hdr->control))
                        error = ng_btsocket_rfcomm_receive_disc(s, dlci);
                break;

        case RFCOMM_FRAME_UA:
                if (RFCOMM_PF(hdr->control))
                        error = ng_btsocket_rfcomm_receive_ua(s, dlci);
                break;

        case RFCOMM_FRAME_DM:
                error = ng_btsocket_rfcomm_receive_dm(s, dlci);
                break;

        case RFCOMM_FRAME_UIH:
                if (dlci == 0)
                        error = ng_btsocket_rfcomm_receive_mcc(s, m0);
                else
                        error = ng_btsocket_rfcomm_receive_uih(s, dlci,
                                        RFCOMM_PF(hdr->control), m0);

                return (error);
                /* NOT REACHED */

        default:
                NG_BTSOCKET_RFCOMM_ERR(
"%s: Invalid RFCOMM packet. Unknown type=%#x\n", __func__, type);
                error = EINVAL;
                break;
        }

        NG_FREE_M(m0);

        return (error);
} /* ng_btsocket_rfcomm_receive_frame */

/*
 * Process RFCOMM SABM frame
 */

static int
ng_btsocket_rfcomm_receive_sabm(ng_btsocket_rfcomm_session_p s, int dlci)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL;
        int                             error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got SABM, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
                __func__, s->state, s->flags, s->mtu, dlci);

        /* DLCI == 0 means open multiplexor channel */
        if (dlci == 0) {
                switch (s->state) {
                case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
                case NG_BTSOCKET_RFCOMM_SESSION_OPEN:
                        error = ng_btsocket_rfcomm_send_command(s,
                                        RFCOMM_FRAME_UA, dlci);
                        if (error == 0) {
                                s->state = NG_BTSOCKET_RFCOMM_SESSION_OPEN;
                                ng_btsocket_rfcomm_connect_cfm(s);
                        } else {
                                s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
                                ng_btsocket_rfcomm_session_clean(s);
                        }
                        break;

                default:
                        NG_BTSOCKET_RFCOMM_WARN(
"%s: Got SABM for session in invalid state state=%d, flags=%#x\n",
                                __func__, s->state, s->flags);
                        error = EINVAL;
                        break;
                }

                return (error);
        }

        /* Make sure multiplexor channel is open */
        if (s->state != NG_BTSOCKET_RFCOMM_SESSION_OPEN) {
                NG_BTSOCKET_RFCOMM_ERR(
"%s: Got SABM for dlci=%d with mulitplexor channel closed, state=%d, " \
"flags=%#x\n",          __func__, dlci, s->state, s->flags);

                return (EINVAL);
        }

        /*
         * Check if we have this DLCI. This might happen when remote
         * peer uses PN command before actual open (SABM) happens.
         */

        pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
        if (pcb != NULL) {
                lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

                if (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTING) {
                        NG_BTSOCKET_RFCOMM_ERR(
"%s: Got SABM for dlci=%d in invalid state=%d, flags=%#x\n",
                                __func__, dlci, pcb->state, pcb->flags);
                        lockmgr(&pcb->pcb_lock, LK_RELEASE);

                        return (ENOENT);
                }

                ng_btsocket_rfcomm_untimeout(pcb);

                error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_UA,dlci);
                if (error == 0)
                        error = ng_btsocket_rfcomm_send_msc(pcb);

                if (error == 0) {
                        pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTED;
                        soisconnected(pcb->so);
                } else
                        ng_btsocket_rfcomm_pcb_kill(pcb, error);

                lockmgr(&pcb->pcb_lock, LK_RELEASE);

                return (error);
        }

        /*
         * We do not have requested DLCI, so it must be an incoming connection
         * with default parameters. Try to accept it.
         */ 

        pcb = ng_btsocket_rfcomm_connect_ind(s, RFCOMM_SRVCHANNEL(dlci));
        if (pcb != NULL) {
                lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

                pcb->dlci = dlci;

                error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_UA,dlci);
                if (error == 0)
                        error = ng_btsocket_rfcomm_send_msc(pcb);

                if (error == 0) {
                        pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTED;
                        soisconnected(pcb->so);
                } else
                        ng_btsocket_rfcomm_pcb_kill(pcb, error);

                lockmgr(&pcb->pcb_lock, LK_RELEASE);
        } else
                /* Nobody is listen()ing on the requested DLCI */
                error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_DM,dlci);

        return (error);
} /* ng_btsocket_rfcomm_receive_sabm */

/*
 * Process RFCOMM DISC frame
 */

static int
ng_btsocket_rfcomm_receive_disc(ng_btsocket_rfcomm_session_p s, int dlci)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL;
        int                             error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got DISC, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
                __func__, s->state, s->flags, s->mtu, dlci);

        /* DLCI == 0 means close multiplexor channel */
        if (dlci == 0) {
                /* XXX FIXME assume that remote side will close the socket */
                error = ng_btsocket_rfcomm_send_command(s, RFCOMM_FRAME_UA, 0);
                if (error == 0) {
                        if (s->state == NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING)
                                s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED; /* XXX */
                        else
                                s->state = NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING;
                } else
                        s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED; /* XXX */

                ng_btsocket_rfcomm_session_clean(s);
        } else {
                pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
                if (pcb != NULL) {
                        int     err;

                        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

                        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got DISC for dlci=%d, state=%d, flags=%#x\n",
                                __func__, dlci, pcb->state, pcb->flags);

                        error = ng_btsocket_rfcomm_send_command(s,
                                        RFCOMM_FRAME_UA, dlci);

                        if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_CONNECTED)
                                err = 0;
                        else
                                err = ECONNREFUSED;

                        ng_btsocket_rfcomm_pcb_kill(pcb, err);

                        lockmgr(&pcb->pcb_lock, LK_RELEASE);
                } else {
                        NG_BTSOCKET_RFCOMM_WARN(
"%s: Got DISC for non-existing dlci=%d\n", __func__, dlci);

                        error = ng_btsocket_rfcomm_send_command(s,
                                        RFCOMM_FRAME_DM, dlci);
                }
        }

        return (error);
} /* ng_btsocket_rfcomm_receive_disc */

/*
 * Process RFCOMM UA frame
 */

static int
ng_btsocket_rfcomm_receive_ua(ng_btsocket_rfcomm_session_p s, int dlci)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL;
        int                             error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got UA, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
                __func__, s->state, s->flags, s->mtu, dlci);

        /* dlci == 0 means multiplexor channel */
        if (dlci == 0) {
                switch (s->state) {
                case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
                        s->state = NG_BTSOCKET_RFCOMM_SESSION_OPEN;
                        ng_btsocket_rfcomm_connect_cfm(s);
                        break;

                case NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING:
                        s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
                        ng_btsocket_rfcomm_session_clean(s);
                        break;

                default:
                        NG_BTSOCKET_RFCOMM_WARN(
"%s: Got UA for session in invalid state=%d(%d), flags=%#x, mtu=%d\n",
                                __func__, s->state, INITIATOR(s), s->flags,
                                s->mtu);
                        error = ENOENT;
                        break;
                }

                return (error);
        }

        /* Check if we have this DLCI */
        pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
        if (pcb != NULL) {
                lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

                NG_BTSOCKET_RFCOMM_INFO(
"%s: Got UA for dlci=%d, state=%d, flags=%#x\n",
                        __func__, dlci, pcb->state, pcb->flags);

                switch (pcb->state) {
                case NG_BTSOCKET_RFCOMM_DLC_CONNECTING:
                        ng_btsocket_rfcomm_untimeout(pcb);

                        error = ng_btsocket_rfcomm_send_msc(pcb);
                        if (error == 0) {
                                pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTED;
                                soisconnected(pcb->so);
                        }
                        break;

                case NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING:
                        ng_btsocket_rfcomm_pcb_kill(pcb, 0);
                        break;

                default:
                        NG_BTSOCKET_RFCOMM_WARN(
"%s: Got UA for dlci=%d in invalid state=%d, flags=%#x\n",
                                __func__, dlci, pcb->state, pcb->flags);
                        error = ENOENT;
                        break;
                }

                lockmgr(&pcb->pcb_lock, LK_RELEASE);
        } else {
                NG_BTSOCKET_RFCOMM_WARN(
"%s: Got UA for non-existing dlci=%d\n", __func__, dlci);

                error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_DM,dlci);
        }

        return (error);
} /* ng_btsocket_rfcomm_receive_ua */

/*
 * Process RFCOMM DM frame
 */

static int
ng_btsocket_rfcomm_receive_dm(ng_btsocket_rfcomm_session_p s, int dlci)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL;
        int                             error;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got DM, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
                __func__, s->state, s->flags, s->mtu, dlci);

        /* DLCI == 0 means multiplexor channel */
        if (dlci == 0) {
                /* Disconnect all dlc's on the session */
                s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
                ng_btsocket_rfcomm_session_clean(s);
        } else {
                pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
                if (pcb != NULL) {
                        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

                        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got DM for dlci=%d, state=%d, flags=%#x\n",
                                __func__, dlci, pcb->state, pcb->flags);

                        if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_CONNECTED)
                                error = ECONNRESET;
                        else
                                error = ECONNREFUSED;

                        ng_btsocket_rfcomm_pcb_kill(pcb, error);

                        lockmgr(&pcb->pcb_lock, LK_RELEASE);
                } else
                        NG_BTSOCKET_RFCOMM_WARN(
"%s: Got DM for non-existing dlci=%d\n", __func__, dlci);
        }

        return (0);
} /* ng_btsocket_rfcomm_receive_dm */

/*
 * Process RFCOMM UIH frame (data)
 */

static int
ng_btsocket_rfcomm_receive_uih(ng_btsocket_rfcomm_session_p s, int dlci,
                int pf, struct mbuf *m0)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL;
        int                             error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got UIH, session state=%d, flags=%#x, mtu=%d, dlci=%d, pf=%d, len=%d\n",
                __func__, s->state, s->flags, s->mtu, dlci, pf,
                m0->m_pkthdr.len);

        /* XXX should we do it here? Check for session flow control */
        if (s->flags & NG_BTSOCKET_RFCOMM_SESSION_LFC) {
                NG_BTSOCKET_RFCOMM_WARN(
"%s: Got UIH with session flow control asserted, state=%d, flags=%#x\n",
                        __func__, s->state, s->flags);
                goto drop;
        }

        /* Check if we have this dlci */
        pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
        if (pcb == NULL) {
                NG_BTSOCKET_RFCOMM_WARN(
"%s: Got UIH for non-existing dlci=%d\n", __func__, dlci);
                error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_DM,dlci);
                goto drop;
        }

        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        /* Check dlci state */  
        if (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTED) {
                NG_BTSOCKET_RFCOMM_WARN(
"%s: Got UIH for dlci=%d in invalid state=%d, flags=%#x\n",
                        __func__, dlci, pcb->state, pcb->flags);
                error = EINVAL;
                goto drop1;
        }

        /* Check dlci flow control */
        if (((pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) && pcb->rx_cred <= 0) ||
             (pcb->lmodem & RFCOMM_MODEM_FC)) {
                NG_BTSOCKET_RFCOMM_ERR(
"%s: Got UIH for dlci=%d with asserted flow control, state=%d, " \
"flags=%#x, rx_cred=%d, lmodem=%#x\n",
                        __func__, dlci, pcb->state, pcb->flags,
                        pcb->rx_cred, pcb->lmodem);
                goto drop1;
        }

        /* Did we get any credits? */
        if ((pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) && pf) {
                NG_BTSOCKET_RFCOMM_INFO(
"%s: Got %d more credits for dlci=%d, state=%d, flags=%#x, " \
"rx_cred=%d, tx_cred=%d\n",
                        __func__, *mtod(m0, u_int8_t *), dlci, pcb->state, 
                        pcb->flags, pcb->rx_cred, pcb->tx_cred);

                pcb->tx_cred += *mtod(m0, u_int8_t *);
                m_adj(m0, 1);

                /* Send more from the DLC. XXX check for errors? */
                ng_btsocket_rfcomm_pcb_send(pcb, ALOT);
        } 

        /* OK the of the rest of the mbuf is the data */
        if (m0->m_pkthdr.len > 0) {
                /* If we are using credit flow control decrease rx_cred here */
                if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) {
                        /* Give remote peer more credits (if needed) */
                        if (-- pcb->rx_cred <= RFCOMM_MAX_CREDITS / 2)
                                ng_btsocket_rfcomm_send_credits(pcb);
                        else
                                NG_BTSOCKET_RFCOMM_INFO(
"%s: Remote side still has credits, dlci=%d, state=%d, flags=%#x, " \
"rx_cred=%d, tx_cred=%d\n",             __func__, dlci, pcb->state, pcb->flags,
                                        pcb->rx_cred, pcb->tx_cred);
                }
                
                /* Check packet against mtu on dlci */
                if (m0->m_pkthdr.len > pcb->mtu) {
                        NG_BTSOCKET_RFCOMM_ERR(
"%s: Got oversized UIH for dlci=%d, state=%d, flags=%#x, mtu=%d, len=%d\n",
                                __func__, dlci, pcb->state, pcb->flags,
                                pcb->mtu, m0->m_pkthdr.len);

                        error = EMSGSIZE;
                } else if (m0->m_pkthdr.len > ssb_space(&pcb->so->so_rcv)) {
 
                        /*
                         * This is really bad. Receive queue on socket does
                         * not have enough space for the packet. We do not
                         * have any other choice but drop the packet. 
                         */
 
                        NG_BTSOCKET_RFCOMM_ERR(
"%s: Not enough space in socket receive queue. Dropping UIH for dlci=%d, " \
"state=%d, flags=%#x, len=%d, space=%ld\n",
                                __func__, dlci, pcb->state, pcb->flags,
                                m0->m_pkthdr.len, ssb_space(&pcb->so->so_rcv));

                        error = ENOBUFS;
                } else {
                        /* Append packet to the socket receive queue */
                        sbappend(&pcb->so->so_rcv.sb, m0);
                        m0 = NULL;

                        sorwakeup(pcb->so);
                }
        }
drop1:
        lockmgr(&pcb->pcb_lock, LK_RELEASE);
drop:
        NG_FREE_M(m0); /* checks for != NULL */

        return (error);
} /* ng_btsocket_rfcomm_receive_uih */

/*
 * Process RFCOMM MCC command (Multiplexor)
 * 
 * From TS 07.10 spec
 *
 * "5.4.3.1 Information Data
 * 
 *  ...The frames (UIH) sent by the initiating station have the C/R bit set 
 *  to 1 and those sent by the responding station have the C/R bit set to 0..."
 *
 * "5.4.6.2 Operating procedures
 *
 *  Messages always exist in pairs; a command message and a corresponding 
 *  response message. If the C/R bit is set to 1 the message is a command, 
 *  if it is set to 0 the message is a response...
 *
 *  ...
 * 
 *  NOTE: Notice that when UIH frames are used to convey information on DLCI 0
 *  there are at least two different fields that contain a C/R bit, and the 
 *  bits are set of different form. The C/R bit in the Type field shall be set
 *  as it is stated above, while the C/R bit in the Address field (see subclause
 *  5.2.1.2) shall be set as it is described in subclause 5.4.3.1."
 */

static int
ng_btsocket_rfcomm_receive_mcc(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
{
        struct rfcomm_mcc_hdr   *hdr = NULL;
        u_int8_t                 cr, type, length;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /*
         * We can access data directly in the first mbuf, because we have
         * m_pullup()'ed mbuf chain in ng_btsocket_rfcomm_receive_frame().
         * All MCC commands should fit into single mbuf (except probably TEST).
         */

        hdr = mtod(m0, struct rfcomm_mcc_hdr *);
        cr = RFCOMM_CR(hdr->type);
        type = RFCOMM_MCC_TYPE(hdr->type);
        length = RFCOMM_MCC_LENGTH(hdr->length);

        /* Check MCC frame length */
        if (sizeof(*hdr) + length != m0->m_pkthdr.len) {
                NG_BTSOCKET_RFCOMM_ERR(
"%s: Invalid MCC frame length=%d, len=%d\n",
                        __func__, length, m0->m_pkthdr.len);
                NG_FREE_M(m0);

                return (EMSGSIZE);
        }

        switch (type) {
        case RFCOMM_MCC_TEST:
                return (ng_btsocket_rfcomm_receive_test(s, m0));
                /* NOT REACHED */

        case RFCOMM_MCC_FCON:
        case RFCOMM_MCC_FCOFF:
                return (ng_btsocket_rfcomm_receive_fc(s, m0));
                /* NOT REACHED */

        case RFCOMM_MCC_MSC:
                return (ng_btsocket_rfcomm_receive_msc(s, m0));
                /* NOT REACHED */

        case RFCOMM_MCC_RPN:
                return (ng_btsocket_rfcomm_receive_rpn(s, m0));
                /* NOT REACHED */

        case RFCOMM_MCC_RLS:
                return (ng_btsocket_rfcomm_receive_rls(s, m0));
                /* NOT REACHED */

        case RFCOMM_MCC_PN:
                return (ng_btsocket_rfcomm_receive_pn(s, m0));
                /* NOT REACHED */

        case RFCOMM_MCC_NSC:
                NG_BTSOCKET_RFCOMM_ERR(
"%s: Got MCC NSC, type=%#x, cr=%d, length=%d, session state=%d, flags=%#x, " \
"mtu=%d, len=%d\n",     __func__, RFCOMM_MCC_TYPE(*((u_int8_t *)(hdr + 1))), cr,
                         length, s->state, s->flags, s->mtu, m0->m_pkthdr.len);
                NG_FREE_M(m0);
                break;

        default:
                NG_BTSOCKET_RFCOMM_ERR(
"%s: Got unknown MCC, type=%#x, cr=%d, length=%d, session state=%d, " \
"flags=%#x, mtu=%d, len=%d\n",
                        __func__, type, cr, length, s->state, s->flags,
                        s->mtu, m0->m_pkthdr.len);

                /* Reuse mbuf to send NSC */
                hdr = mtod(m0, struct rfcomm_mcc_hdr *);
                m0->m_pkthdr.len = m0->m_len = sizeof(*hdr);

                /* Create MCC NSC header */
                hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_NSC);
                hdr->length = RFCOMM_MKLEN8(1);

                /* Put back MCC command type we did not like */
                m0->m_data[m0->m_len] = RFCOMM_MKMCC_TYPE(cr, type);
                m0->m_pkthdr.len ++;
                m0->m_len ++;

                /* Send UIH frame */
                return (ng_btsocket_rfcomm_send_uih(s,
                                RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0));
                /* NOT REACHED */
        }

        return (0);
} /* ng_btsocket_rfcomm_receive_mcc */

/*
 * Receive RFCOMM TEST MCC command
 */

static int
ng_btsocket_rfcomm_receive_test(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
{
        struct rfcomm_mcc_hdr   *hdr = mtod(m0, struct rfcomm_mcc_hdr *);
        int                      error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got MCC TEST, cr=%d, length=%d, session state=%d, flags=%#x, mtu=%d, " \
"len=%d\n",     __func__, RFCOMM_CR(hdr->type), RFCOMM_MCC_LENGTH(hdr->length),
                s->state, s->flags, s->mtu, m0->m_pkthdr.len);

        if (RFCOMM_CR(hdr->type)) {
                hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_TEST);
                error = ng_btsocket_rfcomm_send_uih(s,
                                RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);
        } else
                NG_FREE_M(m0); /* XXX ignore response */

        return (error);
} /* ng_btsocket_rfcomm_receive_test */

/*
 * Receive RFCOMM FCON/FCOFF MCC command
 */

static int
ng_btsocket_rfcomm_receive_fc(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
{
        struct rfcomm_mcc_hdr   *hdr = mtod(m0, struct rfcomm_mcc_hdr *);
        u_int8_t                 type = RFCOMM_MCC_TYPE(hdr->type);
        int                      error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /*
         * Turn ON/OFF aggregate flow on the entire session. When remote peer 
         * asserted flow control no transmission shall occur except on dlci 0
         * (control channel).
         */

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got MCC FC%s, cr=%d, length=%d, session state=%d, flags=%#x, mtu=%d, " \
"len=%d\n",     __func__, (type == RFCOMM_MCC_FCON)? "ON" : "OFF",
                RFCOMM_CR(hdr->type), RFCOMM_MCC_LENGTH(hdr->length),
                s->state, s->flags, s->mtu, m0->m_pkthdr.len);

        if (RFCOMM_CR(hdr->type)) {
                if (type == RFCOMM_MCC_FCON)
                        s->flags &= ~NG_BTSOCKET_RFCOMM_SESSION_RFC;
                else
                        s->flags |= NG_BTSOCKET_RFCOMM_SESSION_RFC;

                hdr->type = RFCOMM_MKMCC_TYPE(0, type);
                error = ng_btsocket_rfcomm_send_uih(s,
                                RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);
        } else
                NG_FREE_M(m0); /* XXX ignore response */

        return (error);
} /* ng_btsocket_rfcomm_receive_fc  */

/*
 * Receive RFCOMM MSC MCC command
 */

static int
ng_btsocket_rfcomm_receive_msc(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
{
        struct rfcomm_mcc_hdr           *hdr = mtod(m0, struct rfcomm_mcc_hdr*);
        struct rfcomm_mcc_msc           *msc = (struct rfcomm_mcc_msc *)(hdr+1);
        ng_btsocket_rfcomm_pcb_t        *pcb = NULL;
        int                              error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got MCC MSC, dlci=%d, cr=%d, length=%d, session state=%d, flags=%#x, " \
"mtu=%d, len=%d\n",
                __func__,  RFCOMM_DLCI(msc->address), RFCOMM_CR(hdr->type),
                RFCOMM_MCC_LENGTH(hdr->length), s->state, s->flags,
                s->mtu, m0->m_pkthdr.len);

        if (RFCOMM_CR(hdr->type)) {
                pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, RFCOMM_DLCI(msc->address));
                if (pcb == NULL) {
                        NG_BTSOCKET_RFCOMM_WARN(
"%s: Got MSC command for non-existing dlci=%d\n",
                                __func__, RFCOMM_DLCI(msc->address));
                        NG_FREE_M(m0);

                        return (ENOENT);
                }

                lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

                if (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTING &&
                    pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTED) {
                        NG_BTSOCKET_RFCOMM_WARN(
"%s: Got MSC on dlci=%d in invalid state=%d\n",
                                __func__, RFCOMM_DLCI(msc->address),
                                pcb->state);

                        lockmgr(&pcb->pcb_lock, LK_RELEASE);
                        NG_FREE_M(m0);

                        return (EINVAL);
                }

                pcb->rmodem = msc->modem; /* Update remote port signals */

                hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_MSC);
                error = ng_btsocket_rfcomm_send_uih(s,
                                RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);

#if 0 /* YYY */
                /* Send more data from DLC. XXX check for errors? */
                if (!(pcb->rmodem & RFCOMM_MODEM_FC) &&
                    !(pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC))
                        ng_btsocket_rfcomm_pcb_send(pcb, ALOT);
#endif /* YYY */

                lockmgr(&pcb->pcb_lock, LK_RELEASE);
        } else
                NG_FREE_M(m0); /* XXX ignore response */

        return (error);
} /* ng_btsocket_rfcomm_receive_msc */

/*
 * Receive RFCOMM RPN MCC command
 * XXX FIXME do we need htole16/le16toh for RPN param_mask?
 */

static int
ng_btsocket_rfcomm_receive_rpn(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
{
        struct rfcomm_mcc_hdr   *hdr = mtod(m0, struct rfcomm_mcc_hdr *);
        struct rfcomm_mcc_rpn   *rpn = (struct rfcomm_mcc_rpn *)(hdr + 1);
        int                      error = 0;
        u_int16_t                param_mask;
        u_int8_t                 bit_rate, data_bits, stop_bits, parity,
                                 flow_control, xon_char, xoff_char;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got MCC RPN, dlci=%d, cr=%d, length=%d, session state=%d, flags=%#x, " \
"mtu=%d, len=%d\n",
                __func__, RFCOMM_DLCI(rpn->dlci), RFCOMM_CR(hdr->type),
                RFCOMM_MCC_LENGTH(hdr->length), s->state, s->flags,
                s->mtu, m0->m_pkthdr.len);

        if (RFCOMM_CR(hdr->type)) {
                param_mask = RFCOMM_RPN_PM_ALL;

                if (RFCOMM_MCC_LENGTH(hdr->length) == 1) {
                        /* Request - return default setting */
                        bit_rate = RFCOMM_RPN_BR_115200;
                        data_bits = RFCOMM_RPN_DATA_8;
                        stop_bits = RFCOMM_RPN_STOP_1;
                        parity = RFCOMM_RPN_PARITY_NONE;
                        flow_control = RFCOMM_RPN_FLOW_NONE;
                        xon_char = RFCOMM_RPN_XON_CHAR;
                        xoff_char = RFCOMM_RPN_XOFF_CHAR;
                } else {
                        /*
                         * Ignore/accept bit_rate, 8 bits, 1 stop bit, no 
                         * parity, no flow control lines, default XON/XOFF 
                         * chars.
                         */

                        bit_rate = rpn->bit_rate;
                        rpn->param_mask = le16toh(rpn->param_mask); /* XXX */

                        data_bits = RFCOMM_RPN_DATA_BITS(rpn->line_settings);
                        if (rpn->param_mask & RFCOMM_RPN_PM_DATA &&
                            data_bits != RFCOMM_RPN_DATA_8) {
                                data_bits = RFCOMM_RPN_DATA_8;
                                param_mask ^= RFCOMM_RPN_PM_DATA;
                        }

                        stop_bits = RFCOMM_RPN_STOP_BITS(rpn->line_settings);
                        if (rpn->param_mask & RFCOMM_RPN_PM_STOP &&
                            stop_bits != RFCOMM_RPN_STOP_1) {
                                stop_bits = RFCOMM_RPN_STOP_1;
                                param_mask ^= RFCOMM_RPN_PM_STOP;
                        }

                        parity = RFCOMM_RPN_PARITY(rpn->line_settings);
                        if (rpn->param_mask & RFCOMM_RPN_PM_PARITY &&
                            parity != RFCOMM_RPN_PARITY_NONE) {
                                parity = RFCOMM_RPN_PARITY_NONE;
                                param_mask ^= RFCOMM_RPN_PM_PARITY;
                        }

                        flow_control = rpn->flow_control;
                        if (rpn->param_mask & RFCOMM_RPN_PM_FLOW &&
                            flow_control != RFCOMM_RPN_FLOW_NONE) {
                                flow_control = RFCOMM_RPN_FLOW_NONE;
                                param_mask ^= RFCOMM_RPN_PM_FLOW;
                        }

                        xon_char = rpn->xon_char;
                        if (rpn->param_mask & RFCOMM_RPN_PM_XON &&
                            xon_char != RFCOMM_RPN_XON_CHAR) {
                                xon_char = RFCOMM_RPN_XON_CHAR;
                                param_mask ^= RFCOMM_RPN_PM_XON;
                        }

                        xoff_char = rpn->xoff_char;
                        if (rpn->param_mask & RFCOMM_RPN_PM_XOFF &&
                            xoff_char != RFCOMM_RPN_XOFF_CHAR) {
                                xoff_char = RFCOMM_RPN_XOFF_CHAR;
                                param_mask ^= RFCOMM_RPN_PM_XOFF;
                        }
                }

                rpn->bit_rate = bit_rate;
                rpn->line_settings = RFCOMM_MKRPN_LINE_SETTINGS(data_bits, 
                                                stop_bits, parity);
                rpn->flow_control = flow_control;
                rpn->xon_char = xon_char;
                rpn->xoff_char = xoff_char;
                rpn->param_mask = htole16(param_mask); /* XXX */

                m0->m_pkthdr.len = m0->m_len = sizeof(*hdr) + sizeof(*rpn);

                hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_RPN);
                error = ng_btsocket_rfcomm_send_uih(s,
                                RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);
        } else
                NG_FREE_M(m0); /* XXX ignore response */

        return (error);
} /* ng_btsocket_rfcomm_receive_rpn */

/*
 * Receive RFCOMM RLS MCC command
 */

static int
ng_btsocket_rfcomm_receive_rls(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
{
        struct rfcomm_mcc_hdr   *hdr = mtod(m0, struct rfcomm_mcc_hdr *);
        struct rfcomm_mcc_rls   *rls = (struct rfcomm_mcc_rls *)(hdr + 1);
        int                      error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        /*
         * XXX FIXME Do we have to do anything else here? Remote peer tries to 
         * tell us something about DLCI. Just report what we have received and
         * return back received values as required by TS 07.10 spec.
         */

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got MCC RLS, dlci=%d, status=%#x, cr=%d, length=%d, session state=%d, " \
"flags=%#x, mtu=%d, len=%d\n",
                __func__, RFCOMM_DLCI(rls->address), rls->status,
                RFCOMM_CR(hdr->type), RFCOMM_MCC_LENGTH(hdr->length),
                s->state, s->flags, s->mtu, m0->m_pkthdr.len);

        if (RFCOMM_CR(hdr->type)) {
                if (rls->status & 0x1)
                        NG_BTSOCKET_RFCOMM_ERR(
"%s: Got RLS dlci=%d, error=%#x\n", __func__, RFCOMM_DLCI(rls->address),
                                rls->status >> 1);

                hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_RLS);
                error = ng_btsocket_rfcomm_send_uih(s,
                                RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);
        } else
                NG_FREE_M(m0); /* XXX ignore responses */

        return (error);
} /* ng_btsocket_rfcomm_receive_rls */

/*
 * Receive RFCOMM PN MCC command
 */

static int
ng_btsocket_rfcomm_receive_pn(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
{
        struct rfcomm_mcc_hdr           *hdr = mtod(m0, struct rfcomm_mcc_hdr*);
        struct rfcomm_mcc_pn            *pn = (struct rfcomm_mcc_pn *)(hdr+1);
        ng_btsocket_rfcomm_pcb_t        *pcb = NULL;
        int                              error = 0;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Got MCC PN, dlci=%d, cr=%d, length=%d, flow_control=%#x, priority=%d, " \
"ack_timer=%d, mtu=%d, max_retrans=%d, credits=%d, session state=%d, " \
"flags=%#x, session mtu=%d, len=%d\n",
                __func__, pn->dlci, RFCOMM_CR(hdr->type),
                RFCOMM_MCC_LENGTH(hdr->length), pn->flow_control, pn->priority,
                pn->ack_timer, le16toh(pn->mtu), pn->max_retrans, pn->credits,
                s->state, s->flags, s->mtu, m0->m_pkthdr.len);

        if (pn->dlci == 0) {
                NG_BTSOCKET_RFCOMM_ERR("%s: Zero dlci in MCC PN\n", __func__);
                NG_FREE_M(m0);

                return (EINVAL);
        }

        /* Check if we have this dlci */
        pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, pn->dlci);
        if (pcb != NULL) {
                lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

                if (RFCOMM_CR(hdr->type)) {
                        /* PN Request */
                        ng_btsocket_rfcomm_set_pn(pcb, 1, pn->flow_control,
                                pn->credits, pn->mtu);

                        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) {
                                pn->flow_control = 0xe0;
                                pn->credits = RFCOMM_DEFAULT_CREDITS;
                        } else {
                                pn->flow_control = 0;
                                pn->credits = 0;
                        }

                        hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_PN);
                        error = ng_btsocket_rfcomm_send_uih(s, 
                                        RFCOMM_MKADDRESS(INITIATOR(s), 0),
                                        0, 0, m0);
                } else {
                        /* PN Response - proceed with SABM. Timeout still set */
                        if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_CONFIGURING) {
                                ng_btsocket_rfcomm_set_pn(pcb, 0,
                                        pn->flow_control, pn->credits, pn->mtu);

                                pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTING;
                                error = ng_btsocket_rfcomm_send_command(s,
                                                RFCOMM_FRAME_SABM, pn->dlci);
                        } else
                                NG_BTSOCKET_RFCOMM_WARN(
"%s: Got PN response for dlci=%d in invalid state=%d\n",
                                        __func__, pn->dlci, pcb->state);

                        NG_FREE_M(m0);
                }

                lockmgr(&pcb->pcb_lock, LK_RELEASE);
        } else if (RFCOMM_CR(hdr->type)) {
                /* PN request to non-existing dlci - incomming connection */
                pcb = ng_btsocket_rfcomm_connect_ind(s,
                                RFCOMM_SRVCHANNEL(pn->dlci));
                if (pcb != NULL) {
                        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

                        pcb->dlci = pn->dlci;

                        ng_btsocket_rfcomm_set_pn(pcb, 1, pn->flow_control,
                                pn->credits, pn->mtu);

                        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) {
                                pn->flow_control = 0xe0;
                                pn->credits = RFCOMM_DEFAULT_CREDITS;
                        } else {
                                pn->flow_control = 0;
                                pn->credits = 0;
                        }

                        hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_PN);
                        error = ng_btsocket_rfcomm_send_uih(s, 
                                        RFCOMM_MKADDRESS(INITIATOR(s), 0),
                                        0, 0, m0);

                        if (error == 0) {
                                ng_btsocket_rfcomm_timeout(pcb);
                                pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTING;
                                soisconnecting(pcb->so);
                        } else
                                ng_btsocket_rfcomm_pcb_kill(pcb, error);

                        lockmgr(&pcb->pcb_lock, LK_RELEASE);
                } else {
                        /* Nobody is listen()ing on this channel */
                        error = ng_btsocket_rfcomm_send_command(s,
                                        RFCOMM_FRAME_DM, pn->dlci);
                        NG_FREE_M(m0);
                }
        } else
                NG_FREE_M(m0); /* XXX ignore response to non-existing dlci */

        return (error);
} /* ng_btsocket_rfcomm_receive_pn */

/*
 * Set PN parameters for dlci. Caller must hold pcb->pcb_mtx.
 * 
 * From Bluetooth spec.
 * 
 * "... The CL1 - CL4 field is completely redefined. (In TS07.10 this defines 
 *  the convergence layer to use, which is not applicable to RFCOMM. In RFCOMM,
 *  in Bluetooth versions up to 1.0B, this field was forced to 0).
 *
 *  In the PN request sent prior to a DLC establishment, this field must contain
 *  the value 15 (0xF), indicating support of credit based flow control in the 
 *  sender. See Table 5.3 below. If the PN response contains any other value 
 *  than 14 (0xE) in this field, it is inferred that the peer RFCOMM entity is 
 *  not supporting the credit based flow control feature. (This is only possible
 *  if the peer RFCOMM implementation is only conforming to Bluetooth version 
 *  1.0B.) If a PN request is sent on an already open DLC, then this field must
 *  contain the value zero; it is not possible to set initial credits  more 
 *  than once per DLC activation. A responding implementation must set this 
 *  field in the PN response to 14 (0xE), if (and only if) the value in the PN 
 *  request was 15..."
 */

static void
ng_btsocket_rfcomm_set_pn(ng_btsocket_rfcomm_pcb_p pcb, u_int8_t cr,
                u_int8_t flow_control, u_int8_t credits, u_int16_t mtu)
{
        KKASSERT(lockowned(&pcb->pcb_lock) != 0);

        pcb->mtu = le16toh(mtu);

        if (cr) {
                if (flow_control == 0xf0) {
                        pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_CFC;
                        pcb->tx_cred = credits;
                } else {
                        pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_CFC;
                        pcb->tx_cred = 0;
                }
        } else {
                if (flow_control == 0xe0) {
                        pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_CFC;
                        pcb->tx_cred = credits;
                } else {
                        pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_CFC;
                        pcb->tx_cred = 0;
                }
        }

        NG_BTSOCKET_RFCOMM_INFO(
"%s: cr=%d, dlci=%d, state=%d, flags=%#x, mtu=%d, rx_cred=%d, tx_cred=%d\n",
                __func__, cr, pcb->dlci, pcb->state, pcb->flags, pcb->mtu,
                pcb->rx_cred, pcb->tx_cred);
} /* ng_btsocket_rfcomm_set_pn */

/*
 * Send RFCOMM SABM/DISC/UA/DM frames. Caller must hold s->session_mtx
 */

static int
ng_btsocket_rfcomm_send_command(ng_btsocket_rfcomm_session_p s,
                u_int8_t type, u_int8_t dlci)
{
        struct rfcomm_cmd_hdr   *hdr = NULL;
        struct mbuf             *m = NULL;
        int                      cr;

        KKASSERT(lockowned(&s->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Sending command type %#x, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
                __func__, type, s->state, s->flags, s->mtu, dlci);

        switch (type) {
        case RFCOMM_FRAME_SABM:
        case RFCOMM_FRAME_DISC:
                cr = INITIATOR(s);
                break;

        case RFCOMM_FRAME_UA:
        case RFCOMM_FRAME_DM:
                cr = !INITIATOR(s);
                break;

        default:
                panic("%s: Invalid frame type=%#x", __func__, type);
                return (EINVAL);
                /* NOT REACHED */
        }

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

        m->m_pkthdr.len = m->m_len = sizeof(*hdr);

        hdr = mtod(m, struct rfcomm_cmd_hdr *);
        hdr->address = RFCOMM_MKADDRESS(cr, dlci);
        hdr->control = RFCOMM_MKCONTROL(type, 1);
        hdr->length = RFCOMM_MKLEN8(0);
        hdr->fcs = ng_btsocket_rfcomm_fcs3((u_int8_t *) hdr);

        NG_BT_MBUFQ_ENQUEUE(&s->outq, m);

        return (0);
} /* ng_btsocket_rfcomm_send_command */

/*
 * Send RFCOMM UIH frame. Caller must hold s->session_mtx
 */

static int
ng_btsocket_rfcomm_send_uih(ng_btsocket_rfcomm_session_p s, u_int8_t address,
                u_int8_t pf, u_int8_t credits, struct mbuf *data)
{
        struct rfcomm_frame_hdr *hdr = NULL;
        struct mbuf             *m = NULL, *mcrc = NULL;
        u_int16_t                length;

        KKASSERT(lockowned(&s->session_lock) != 0);

        MGETHDR(m, M_NOWAIT, MT_DATA);
        if (m == NULL) {
                NG_FREE_M(data);
                return (ENOBUFS);
        }
        m->m_pkthdr.len = m->m_len = sizeof(*hdr);

        MGET(mcrc, M_NOWAIT, MT_DATA);
        if (mcrc == NULL) {
                NG_FREE_M(data);
                return (ENOBUFS);
        }
        mcrc->m_len = 1;

        /* Fill UIH frame header */
        hdr = mtod(m, struct rfcomm_frame_hdr *);
        hdr->address = address;
        hdr->control = RFCOMM_MKCONTROL(RFCOMM_FRAME_UIH, pf);

        /* Calculate FCS */
        mcrc->m_data[0] = ng_btsocket_rfcomm_fcs2((u_int8_t *) hdr);

        /* Put length back */
        length = (data != NULL)? data->m_pkthdr.len : 0;
        if (length > 127) {
                u_int16_t       l = htole16(RFCOMM_MKLEN16(length));

                bcopy(&l, &hdr->length, sizeof(l));
                m->m_pkthdr.len ++;
                m->m_len ++;
        } else
                hdr->length = RFCOMM_MKLEN8(length);

        if (pf) {
                m->m_data[m->m_len] = credits;
                m->m_pkthdr.len ++;
                m->m_len ++;
        }

        /* Add payload */
        if (data != NULL) {
                m_cat(m, data);
                m->m_pkthdr.len += length;
        }

        /* Put FCS back */
        m_cat(m, mcrc);
        m->m_pkthdr.len ++;

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Sending UIH state=%d, flags=%#x, address=%d, length=%d, pf=%d, " \
"credits=%d, len=%d\n",
                __func__, s->state, s->flags, address, length, pf, credits,
                m->m_pkthdr.len);

        NG_BT_MBUFQ_ENQUEUE(&s->outq, m);

        return (0);
} /* ng_btsocket_rfcomm_send_uih */

/*
 * Send MSC request. Caller must hold pcb->pcb_mtx and pcb->session->session_mtx
 */

static int
ng_btsocket_rfcomm_send_msc(ng_btsocket_rfcomm_pcb_p pcb)
{
        struct mbuf             *m = NULL;
        struct rfcomm_mcc_hdr   *hdr = NULL;
        struct rfcomm_mcc_msc   *msc = NULL;

        KKASSERT(lockowned(&pcb->session->session_lock) != 0);
        KKASSERT(lockowned(&pcb->pcb_lock) != 0);

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

        m->m_pkthdr.len = m->m_len = sizeof(*hdr) + sizeof(*msc);

        hdr = mtod(m, struct rfcomm_mcc_hdr *);
        msc = (struct rfcomm_mcc_msc *)(hdr + 1);

        hdr->type = RFCOMM_MKMCC_TYPE(1, RFCOMM_MCC_MSC);
        hdr->length = RFCOMM_MKLEN8(sizeof(*msc));

        msc->address = RFCOMM_MKADDRESS(1, pcb->dlci);
        msc->modem = pcb->lmodem;

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Sending MSC dlci=%d, state=%d, flags=%#x, address=%d, modem=%#x\n",
                __func__, pcb->dlci, pcb->state, pcb->flags, msc->address,
                msc->modem);

        return (ng_btsocket_rfcomm_send_uih(pcb->session,
                        RFCOMM_MKADDRESS(INITIATOR(pcb->session), 0), 0, 0, m));
} /* ng_btsocket_rfcomm_send_msc */

/*
 * Send PN request. Caller must hold pcb->pcb_mtx and pcb->session->session_mtx
 */

static int
ng_btsocket_rfcomm_send_pn(ng_btsocket_rfcomm_pcb_p pcb)
{
        struct mbuf             *m = NULL;
        struct rfcomm_mcc_hdr   *hdr = NULL;
        struct rfcomm_mcc_pn    *pn = NULL;

        KKASSERT(lockowned(&pcb->session->session_lock) != 0);
        KKASSERT(lockowned(&pcb->pcb_lock) != 0);

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

        m->m_pkthdr.len = m->m_len = sizeof(*hdr) + sizeof(*pn);

        hdr = mtod(m, struct rfcomm_mcc_hdr *);
        pn = (struct rfcomm_mcc_pn *)(hdr + 1);

        hdr->type = RFCOMM_MKMCC_TYPE(1, RFCOMM_MCC_PN);
        hdr->length = RFCOMM_MKLEN8(sizeof(*pn));

        pn->dlci = pcb->dlci;

        /*
         * Set default DLCI priority as described in GSM 07.10
         * (ETSI TS 101 369) clause 5.6 page 42
         */

        pn->priority = (pcb->dlci < 56)? (((pcb->dlci >> 3) << 3) + 7) : 61;
        pn->ack_timer = 0;
        pn->mtu = htole16(pcb->mtu);
        pn->max_retrans = 0;

        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) {
                pn->flow_control = 0xf0;
                pn->credits = pcb->rx_cred;
        } else {
                pn->flow_control = 0;
                pn->credits = 0;
        }

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Sending PN dlci=%d, state=%d, flags=%#x, mtu=%d, flow_control=%#x, " \
"credits=%d\n", __func__, pcb->dlci, pcb->state, pcb->flags, pcb->mtu,
                pn->flow_control, pn->credits);

        return (ng_btsocket_rfcomm_send_uih(pcb->session,
                        RFCOMM_MKADDRESS(INITIATOR(pcb->session), 0), 0, 0, m));
} /* ng_btsocket_rfcomm_send_pn */

/*
 * Calculate and send credits based on available space in receive buffer
 */

static int
ng_btsocket_rfcomm_send_credits(ng_btsocket_rfcomm_pcb_p pcb)
{
        int             error = 0;
        u_int8_t        credits;

        KKASSERT(lockowned(&pcb->pcb_lock) != 0);
        KKASSERT(lockowned(&pcb->session->session_lock) != 0);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Sending more credits, dlci=%d, state=%d, flags=%#x, mtu=%d, " \
"space=%ld, tx_cred=%d, rx_cred=%d\n",
                __func__, pcb->dlci, pcb->state, pcb->flags, pcb->mtu,
                ssb_space(&pcb->so->so_rcv), pcb->tx_cred, pcb->rx_cred);

        credits = ssb_space(&pcb->so->so_rcv) / pcb->mtu;
        if (credits > 0) {
                if (pcb->rx_cred + credits > RFCOMM_MAX_CREDITS)
                        credits = RFCOMM_MAX_CREDITS - pcb->rx_cred;

                error = ng_btsocket_rfcomm_send_uih(
                                pcb->session,
                                RFCOMM_MKADDRESS(INITIATOR(pcb->session),
                                        pcb->dlci), 1, credits, NULL);
                if (error == 0) {
                        pcb->rx_cred += credits;

                        NG_BTSOCKET_RFCOMM_INFO(
"%s: Gave remote side %d more credits, dlci=%d, state=%d, flags=%#x, " \
"rx_cred=%d, tx_cred=%d\n",     __func__, credits, pcb->dlci, pcb->state,
                                pcb->flags, pcb->rx_cred, pcb->tx_cred);
                } else
                        NG_BTSOCKET_RFCOMM_ERR(
"%s: Could not send credits, error=%d, dlci=%d, state=%d, flags=%#x, " \
"mtu=%d, space=%ld, tx_cred=%d, rx_cred=%d\n",
                                __func__, error, pcb->dlci, pcb->state,
                                pcb->flags, pcb->mtu, ssb_space(&pcb->so->so_rcv),
                                pcb->tx_cred, pcb->rx_cred);
        }

        return (error);
} /* ng_btsocket_rfcomm_send_credits */

/*****************************************************************************
 *****************************************************************************
 **                              RFCOMM DLCs
 *****************************************************************************
 *****************************************************************************/

/*
 * Send data from socket send buffer
 * Caller must hold pcb->pcb_mtx and pcb->session->session_mtx
 */

static int
ng_btsocket_rfcomm_pcb_send(ng_btsocket_rfcomm_pcb_p pcb, int limit)
{
        struct mbuf     *m = NULL;
        int              sent, length, error;

        KKASSERT(lockowned(&pcb->session->session_lock) != 0);
        KKASSERT(lockowned(&pcb->pcb_lock) != 0);

        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC)
                limit = min(limit, pcb->tx_cred);
        else if (!(pcb->rmodem & RFCOMM_MODEM_FC))
                limit = min(limit, RFCOMM_MAX_CREDITS); /* XXX ??? */
        else
                limit = 0;

        if (limit == 0) {
                NG_BTSOCKET_RFCOMM_INFO(
"%s: Could not send - remote flow control asserted, dlci=%d, flags=%#x, " \
"rmodem=%#x, tx_cred=%d\n",
                        __func__, pcb->dlci, pcb->flags, pcb->rmodem,
                        pcb->tx_cred);

                return (0);
        }

        for (error = 0, sent = 0; sent < limit; sent ++) { 
                length = min(pcb->mtu, pcb->so->so_snd.sb.sb_cc);
                if (length == 0)
                        break;

                /* Get the chunk from the socket's send buffer */
                m = ng_btsocket_rfcomm_prepare_packet(&pcb->so->so_snd.sb,
                    length);
                if (m == NULL) {
                        error = ENOBUFS;
                        break;
                }

                sbdrop(&pcb->so->so_snd.sb, length);

                error = ng_btsocket_rfcomm_send_uih(pcb->session,
                                RFCOMM_MKADDRESS(INITIATOR(pcb->session),
                                        pcb->dlci), 0, 0, m);
                if (error != 0)
                        break;
        }

        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC)
                pcb->tx_cred -= sent;

        if (error == 0 && sent > 0) {
                pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_SENDING;
                sowwakeup(pcb->so);
        }

        return (error);
} /* ng_btsocket_rfcomm_pcb_send */

/*
 * Unlink and disconnect DLC. If ng_btsocket_rfcomm_pcb_kill() returns
 * non zero value than socket has no reference and has to be detached.
 * Caller must hold pcb->pcb_mtx and pcb->session->session_mtx
 */

static void
ng_btsocket_rfcomm_pcb_kill(ng_btsocket_rfcomm_pcb_p pcb, int error)
{
        ng_btsocket_rfcomm_session_p    s = pcb->session;

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Killing DLC, so=%p, dlci=%d, state=%d, flags=%#x, error=%d\n",
                __func__, pcb->so, pcb->dlci, pcb->state, pcb->flags, error);

        if (pcb->session == NULL)
                panic("%s: DLC without session, pcb=%p, state=%d, flags=%#x",
                        __func__, pcb, pcb->state, pcb->flags);

        KKASSERT(lockowned(&pcb->session->session_lock) != 0);
        KKASSERT(lockowned(&pcb->pcb_lock) != 0);

        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)
                ng_btsocket_rfcomm_untimeout(pcb);

        /* Detach DLC from the session. Does not matter which state DLC in */
        LIST_REMOVE(pcb, session_next);
        pcb->session = NULL;

        /* Change DLC state and wakeup all sleepers */
        pcb->state = NG_BTSOCKET_RFCOMM_DLC_CLOSED;
        pcb->so->so_error = error;
        soisdisconnected(pcb->so);
        wakeup(&pcb->state);

        /* Check if we have any DLCs left on the session */
        if (LIST_EMPTY(&s->dlcs) && INITIATOR(s)) {
                NG_BTSOCKET_RFCOMM_INFO(
"%s: Disconnecting session, state=%d, flags=%#x, mtu=%d\n",
                        __func__, s->state, s->flags, s->mtu);

                switch (s->state) {
                case NG_BTSOCKET_RFCOMM_SESSION_CLOSED:
                case NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING:
                        /*
                         * Do not have to do anything here. We can get here
                         * when L2CAP connection was terminated or we have 
                         * received DISC on multiplexor channel
                         */
                        break;

                case NG_BTSOCKET_RFCOMM_SESSION_OPEN:
                        /* Send DISC on multiplexor channel */
                        error = ng_btsocket_rfcomm_send_command(s,
                                        RFCOMM_FRAME_DISC, 0);
                        if (error == 0) {
                                s->state = NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING;
                                break;
                        }
                        /* FALL THROUGH */

                case NG_BTSOCKET_RFCOMM_SESSION_CONNECTING:
                case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
                        s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
                        break;

/*              case NG_BTSOCKET_RFCOMM_SESSION_LISTENING: */
                default:
                        panic("%s: Invalid session state=%d, flags=%#x",
                                __func__, s->state, s->flags);
                        break;
                }

                ng_btsocket_rfcomm_task_wakeup();
        }
} /* ng_btsocket_rfcomm_pcb_kill */

/*
 * Look for given dlci for given RFCOMM session. Caller must hold s->session_mtx
 */

static ng_btsocket_rfcomm_pcb_p
ng_btsocket_rfcomm_pcb_by_dlci(ng_btsocket_rfcomm_session_p s, int dlci)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL;

        KKASSERT(lockowned(&s->session_lock) != 0);

        LIST_FOREACH(pcb, &s->dlcs, session_next)
                if (pcb->dlci == dlci)
                        break;

        return (pcb);
} /* ng_btsocket_rfcomm_pcb_by_dlci */

/*
 * Look for socket that listens on given src address and given channel
 */

static ng_btsocket_rfcomm_pcb_p
ng_btsocket_rfcomm_pcb_listener(bdaddr_p src, int channel)
{
        ng_btsocket_rfcomm_pcb_p        pcb = NULL, pcb1 = NULL;

        lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_EXCLUSIVE);

        LIST_FOREACH(pcb, &ng_btsocket_rfcomm_sockets, next) {
                if (pcb->channel != channel ||
                    !(pcb->so->so_options & SO_ACCEPTCONN))
                        continue;

                if (bcmp(&pcb->src, src, sizeof(*src)) == 0)
                        break;

                if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
                        pcb1 = pcb;
        }

        lockmgr(&ng_btsocket_rfcomm_sockets_lock, LK_RELEASE);

        return ((pcb != NULL)? pcb : pcb1);
} /* ng_btsocket_rfcomm_pcb_listener */

/*****************************************************************************
 *****************************************************************************
 **                              Misc. functions 
 *****************************************************************************
 *****************************************************************************/

/*
 *  Set timeout. Caller MUST hold pcb_mtx
 */

static void
ng_btsocket_rfcomm_timeout(ng_btsocket_rfcomm_pcb_p pcb)
{
        KKASSERT(lockowned(&pcb->pcb_lock) != 0);

        if (!(pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)) {
                pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_TIMO;
                pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT;
                callout_reset(&pcb->timo, ng_btsocket_rfcomm_timo * hz,
                    ng_btsocket_rfcomm_process_timeout, pcb);
        } else
                panic("%s: Duplicated socket timeout?!", __func__);
} /* ng_btsocket_rfcomm_timeout */

/*
 *  Unset pcb timeout. Caller MUST hold pcb_mtx
 */

static void
ng_btsocket_rfcomm_untimeout(ng_btsocket_rfcomm_pcb_p pcb)
{
        KKASSERT(lockowned(&pcb->pcb_lock) != 0);

        if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO) {
                callout_stop(&pcb->timo);
                pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_TIMO;
                pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT;
        } else
                panic("%s: No socket timeout?!", __func__);
} /* ng_btsocket_rfcomm_timeout */

/*
 * Process pcb timeout
 */

static void
ng_btsocket_rfcomm_process_timeout(void *xpcb)
{
        ng_btsocket_rfcomm_pcb_p        pcb = (ng_btsocket_rfcomm_pcb_p) xpcb;

        lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);

        NG_BTSOCKET_RFCOMM_INFO(
"%s: Timeout, so=%p, dlci=%d, state=%d, flags=%#x\n",
                __func__, pcb->so, pcb->dlci, pcb->state, pcb->flags);

        pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_TIMO;
        pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT;

        switch (pcb->state) {
        case NG_BTSOCKET_RFCOMM_DLC_CONFIGURING:
        case NG_BTSOCKET_RFCOMM_DLC_CONNECTING:
                pcb->state = NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING;
                break;

        case NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT:
        case NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING:
                break;

        default:
                panic(
"%s: DLC timeout in invalid state, dlci=%d, state=%d, flags=%#x\n",
                        __func__, pcb->dlci, pcb->state, pcb->flags);
                break;
        }

        ng_btsocket_rfcomm_task_wakeup();

        lockmgr(&pcb->pcb_lock, LK_RELEASE);
} /* ng_btsocket_rfcomm_process_timeout */

/*
 * Get up to length bytes from the socket buffer
 */

static struct mbuf *
ng_btsocket_rfcomm_prepare_packet(struct sockbuf *sb, int length)
{
        struct mbuf     *top = NULL, *m = NULL, *n = NULL, *nextpkt = NULL;
        int              mlen, noff, len;

        MGETHDR(top, M_NOWAIT, MT_DATA);
        if (top == NULL)
                return (NULL);

        top->m_pkthdr.len = length;
        top->m_len = 0;
        mlen = MHLEN;

        m = top;
        n = sb->sb_mb;
        nextpkt = n->m_nextpkt;
        noff = 0;

        while (length > 0 && n != NULL) {
                len = min(mlen - m->m_len, n->m_len - noff);
                if (len > length)
                        len = length;

                bcopy(mtod(n, caddr_t)+noff, mtod(m, caddr_t)+m->m_len, len);
                m->m_len += len;
                noff += len;
                length -= len;

                if (length > 0 && m->m_len == mlen) {
                        MGET(m->m_next, M_NOWAIT, MT_DATA);
                        if (m->m_next == NULL) {
                                NG_FREE_M(top);
                                return (NULL);
                        }

                        m = m->m_next;
                        m->m_len = 0;
                        mlen = MLEN;
                }

                if (noff == n->m_len) {
                        noff = 0;
                        n = n->m_next;

                        if (n == NULL)
                                n = nextpkt;

                        nextpkt = (n != NULL)? n->m_nextpkt : NULL;
                }
        }

        if (length < 0)
                panic("%s: length=%d", __func__, length);
        if (length > 0 && n == NULL)
                panic("%s: bogus length=%d, n=%p", __func__, length, n);

        return (top);
} /* ng_btsocket_rfcomm_prepare_packet */