l2cap.c

蓝牙的各种编程接口和各种按理介绍,还有一些例子和说明

	struct sock *sk = sock->sk;
	struct l2cap_options opts;
	struct l2cap_conninfo cinfo;
	int len, err = 0; 

	if (get_user(len, optlen))
		return -EFAULT;

	lock_sock(sk);

	switch (optname) {
	case L2CAP_OPTIONS:
		opts.imtu     = l2cap_pi(sk)->imtu;
		opts.omtu     = l2cap_pi(sk)->omtu;
		opts.flush_to = l2cap_pi(sk)->flush_to;

		len = MIN(len, sizeof(opts));
		if (copy_to_user(optval, (char *)&opts, len))
			err = -EFAULT;

		break;

	case L2CAP_LM:
		if (put_user(l2cap_pi(sk)->link_mode, (__u32 *)optval))
			err = -EFAULT;
		break;

	case L2CAP_CONNINFO:
		if (sk->state != BT_CONNECTED) {
			err = -ENOTCONN;
			break;
		}

		cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;

		len = MIN(len, sizeof(cinfo));
		if (copy_to_user(optval, (char *)&cinfo, len))
			err = -EFAULT;

		break;

	default:
		err = -ENOPROTOOPT;
		break;
	}

	release_sock(sk);
	return err;
}

static int l2cap_sock_shutdown(struct socket *sock, int how)
{
	struct sock *sk = sock->sk;

	BT_DBG("sock %p, sk %p", sock, sk);

	if (!sk) return 0;

	l2cap_sock_clear_timer(sk);

	lock_sock(sk);
	sk->shutdown = SHUTDOWN_MASK;
	__l2cap_sock_close(sk, ECONNRESET);
	release_sock(sk);

	return 0;
}

static int l2cap_sock_release(struct socket *sock)
{
	struct sock *sk = sock->sk;

	BT_DBG("sock %p, sk %p", sock, sk);

	if (!sk) return 0;

	sock_orphan(sk);
	l2cap_sock_close(sk);
	return 0;
}

/* --------- L2CAP channels --------- */
static struct sock * __l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, __u16 cid)
{
	struct sock *s;
	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
		if (l2cap_pi(s)->dcid == cid)
			break;
	}
	return s;
}

static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, __u16 cid)
{
	struct sock *s;
	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
		if (l2cap_pi(s)->scid == cid)
			break;
	}
	return s;
}

/* Find channel with given SCID.
 * Returns locked socket */
static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, __u16 cid)
{
	struct sock *s;
	read_lock(&l->lock);
	s = __l2cap_get_chan_by_scid(l, cid);
	if (s) bh_lock_sock(s);
	read_unlock(&l->lock);
	return s;
}

static __u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
{
	__u16 cid = 0x0040;

	for (; cid < 0xffff; cid++) {
		if(!__l2cap_get_chan_by_scid(l, cid))
			return cid;
	}

	return 0;
}

static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
{
	sock_hold(sk);

	if (l->head)
		l2cap_pi(l->head)->prev_c = sk;

	l2cap_pi(sk)->next_c = l->head;
	l2cap_pi(sk)->prev_c = NULL;
	l->head = sk;
}

static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
{
	struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;

	write_lock(&l->lock);
	if (sk == l->head)
		l->head = next;

	if (next)
		l2cap_pi(next)->prev_c = prev;
	if (prev)
		l2cap_pi(prev)->next_c = next;
	write_unlock(&l->lock);

	__sock_put(sk);
}

static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
{
	struct l2cap_chan_list *l = &conn->chan_list;

	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);

	l2cap_pi(sk)->conn = conn;

	if (sk->type == SOCK_SEQPACKET) {
		/* Alloc CID for connection-oriented socket */
		l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
	} else if (sk->type == SOCK_DGRAM) {
		/* Connectionless socket */
		l2cap_pi(sk)->scid = 0x0002;
		l2cap_pi(sk)->dcid = 0x0002;
		l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
	} else {
		/* Raw socket can send/recv signalling messages only */
		l2cap_pi(sk)->scid = 0x0001;
		l2cap_pi(sk)->dcid = 0x0001;
		l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
	}

	__l2cap_chan_link(l, sk);

	if (parent)
		bluez_accept_enqueue(parent, sk);
}

static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
{
	struct l2cap_chan_list *l = &conn->chan_list;
	write_lock(&l->lock);
	__l2cap_chan_add(conn, sk, parent);
	write_unlock(&l->lock);
}

/* Delete channel. 
 * Must be called on the locked socket. */
static void l2cap_chan_del(struct sock *sk, int err)
{
	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
	struct sock *parent = bluez_pi(sk)->parent;

	l2cap_sock_clear_timer(sk);

	BT_DBG("sk %p, conn %p, err %d", sk, conn, err);

	if (conn) { 
		/* Unlink from channel list */
		l2cap_chan_unlink(&conn->chan_list, sk);
		l2cap_pi(sk)->conn = NULL;
		hci_conn_put(conn->hcon);
	}

	sk->state = BT_CLOSED;
	sk->err   = err;
	sk->zapped = 1;

	if (parent)
		parent->data_ready(parent, 0);
	else
		sk->state_change(sk);
}

static void l2cap_conn_ready(struct l2cap_conn *conn)
{
	struct l2cap_chan_list *l = &conn->chan_list;
	struct sock *sk;

	BT_DBG("conn %p", conn);

	read_lock(&l->lock);

	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
		bh_lock_sock(sk);

		if (sk->type != SOCK_SEQPACKET) {
			l2cap_sock_clear_timer(sk);
			sk->state = BT_CONNECTED;
			sk->state_change(sk);
		} else if (sk->state == BT_CONNECT) {
			l2cap_conn_req req;
			req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
			req.psm  = l2cap_pi(sk)->psm;
			l2cap_send_req(conn, L2CAP_CONN_REQ, L2CAP_CONN_REQ_SIZE, &req);
		}

		bh_unlock_sock(sk);
	}

	read_unlock(&l->lock);
}

static void l2cap_chan_ready(struct sock *sk)
{
	struct sock *parent = bluez_pi(sk)->parent;

	BT_DBG("sk %p, parent %p", sk, parent);

	l2cap_pi(sk)->conf_state = 0;
	l2cap_sock_clear_timer(sk);

	if (!parent) {
		/* Outgoing channel.
		 * Wake up socket sleeping on connect.
		 */
		sk->state = BT_CONNECTED;
		sk->state_change(sk);
	} else {
		/* Incomming channel.
		 * Wake up socket sleeping on accept.
		 */
		parent->data_ready(parent, 0);
	}
}

/* Copy frame to all raw sockets on that connection */
void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct l2cap_chan_list *l = &conn->chan_list;
	struct sk_buff *nskb;
	struct sock * sk;

	BT_DBG("conn %p", conn);

	read_lock(&l->lock);
	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
		if (sk->type != SOCK_RAW)
			continue;

		/* Don't send frame to the socket it came from */
		if (skb->sk == sk)
			continue;

		if (!(nskb = skb_clone(skb, GFP_ATOMIC)))
			continue;

		if (sock_queue_rcv_skb(sk, nskb))
			kfree_skb(nskb);
	}
	read_unlock(&l->lock);
}

static int l2cap_chan_send(struct sock *sk, struct msghdr *msg, int len)
{
	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
	struct sk_buff *skb, **frag;
	int err, hlen, count, sent=0;
	l2cap_hdr *lh;

	BT_DBG("sk %p len %d", sk, len);

	/* First fragment (with L2CAP header) */
	if (sk->type == SOCK_DGRAM)
		hlen = L2CAP_HDR_SIZE + 2;
	else
		hlen = L2CAP_HDR_SIZE;

	count = MIN(conn->mtu - hlen, len);

	skb = bluez_skb_send_alloc(sk, hlen + count,
			msg->msg_flags & MSG_DONTWAIT, &err);
	if (!skb)
		return err;

	/* Create L2CAP header */
	lh = (l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
	lh->cid = __cpu_to_le16(l2cap_pi(sk)->dcid);
	lh->len = __cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));

	if (sk->type == SOCK_DGRAM)
		put_unaligned(l2cap_pi(sk)->psm, (__u16 *) skb_put(skb, 2));

	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
		err = -EFAULT;
		goto fail;
	}

	sent += count;
	len  -= count;

	/* Continuation fragments (no L2CAP header) */
	frag = &skb_shinfo(skb)->frag_list;
	while (len) {
		count = MIN(conn->mtu, len);

		*frag = bluez_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
		if (!*frag)
			goto fail;
		
		if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) {
			err = -EFAULT;
			goto fail;
		}

		sent += count;
		len  -= count;

		frag = &(*frag)->next;
	}

	if ((err = hci_send_acl(conn->hcon, skb, 0)) < 0)
		goto fail;

	return sent;

fail:
	kfree_skb(skb);
	return err;
}

/* --------- L2CAP signalling commands --------- */
static inline __u8 l2cap_get_ident(struct l2cap_conn *conn)
{
	__u8 id;

	/* Get next available identificator.
	 *    1 - 199 are used by kernel.
	 *  200 - 254 are used by utilities like l2ping, etc 
	 */

	spin_lock(&conn->lock);

	if (++conn->tx_ident > 199)
		conn->tx_ident = 1;

	id = conn->tx_ident;

	spin_unlock(&conn->lock);

	return id;
}

static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
				__u8 code, __u8 ident, __u16 dlen, void *data)
{
	struct sk_buff *skb, **frag;
	l2cap_cmd_hdr *cmd;
	l2cap_hdr *lh;
	int len, count;

	BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", conn, code, ident, dlen);

	len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
	count = MIN(conn->mtu, len);
	
	skb = bluez_skb_alloc(count, GFP_ATOMIC);
	if (!skb)
		return NULL;

	lh = (l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
	lh->len = __cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
	lh->cid = __cpu_to_le16(0x0001);

	cmd = (l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
	cmd->code  = code;
	cmd->ident = ident;
	cmd->len   = __cpu_to_le16(dlen);

	if (dlen) {
		count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
		memcpy(skb_put(skb, count), data, count);
		data += count;
	}

	len -= skb->len;
	
	/* Continuation fragments (no L2CAP header) */
	frag = &skb_shinfo(skb)->frag_list;
	while (len) {
		count = MIN(conn->mtu, len);

		*frag = bluez_skb_alloc(count, GFP_ATOMIC);
		if (!*frag)
			goto fail;
		
		memcpy(skb_put(*frag, count), data, count);

		len  -= count;
		data += count;
		
		frag = &(*frag)->next;
	}

	return skb;

fail:
	kfree_skb(skb);
	return NULL;
}

static int l2cap_send_req(struct l2cap_conn *conn, __u8 code, __u16 len, void *data)
{
	__u8 ident = l2cap_get_ident(conn);
	struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);

	BT_DBG("code 0x%2.2x", code);

	if (!skb)
		return -ENOMEM;
	return hci_send_acl(conn->hcon, skb, 0);
}

static int l2cap_send_rsp(struct l2cap_conn *conn, __u8 ident, __u8 code, __u16 len, void *data)
{
	struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);

	BT_DBG("code 0x%2.2x", code);

	if (!skb)
		return -ENOMEM;
	return hci_send_acl(conn->hcon, skb, 0);
}

static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
{
	l2cap_conf_opt *opt = *ptr;
	int len;

	len = L2CAP_CONF_OPT_SIZE + opt->len;
	*ptr += len;

	*type = opt->type;
	*olen = opt->len;

	switch (opt->len) {
	case 1:
		*val = *((__u8 *) opt->val);
		break;

	case 2:
		*val = __le16_to_cpu(*((__u16 *)opt->val));
		break;

	case 4:
		*val = __le32_to_cpu(*((__u32 *)opt->val));
		break;

	default:
		*val = (unsigned long) opt->val;
		break;
	};

	BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
	return len;
}

static inline void l2cap_parse_conf_req(struct sock *sk, void *data, int len)
{
	int type, hint, olen; 
	unsigned long val;
	void *ptr = data;

	BT_DBG("sk %p len %d", sk, len);

	while (len >= L2CAP_CONF_OPT_SIZE) {
		len -= l2cap_get_conf_opt(&ptr, &type, &olen, &val);

		hint  = type & 0x80;
		type &= 0x7f;

		switch (type) {
		case L2CAP_CONF_MTU:
			l2cap_pi(sk)->conf_mtu = val;
			break;

		case L2CAP_CONF_FLUSH_TO:
			l2cap_pi(sk)->flush_to = val;
			break;

		case L2CAP_CONF_QOS:
			break;
		
		default:
			if (hint)
				break;

			/* FIXME: Reject unknown option */
			break;
		};
	}
}

static void l2cap_add_conf_opt(void **ptr, __u8 type, __u8 len, unsigned long val)
{
	register l2cap_conf_opt *opt = *ptr;

	BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);

	opt->type = type;
	opt->len  = len;

	switch (len) {
	case 1:
		*((__u8 *) opt->val)  = val;
		break;

	case 2:
		*((__u16 *) opt->val) = __cpu_to_le16(val);
		break;

	case 4:
		*((__u32 *) opt->val) = __cpu_to_le32(val);
		break;

	default:
		memcpy(opt->val, (void *) val, len);
		break;
	};

	*ptr += L2CAP_CONF_OPT_SIZE + len;
}

static int l2cap_build_conf_req(struct sock *sk, void *data)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	l2cap_conf_req *req = (l2cap_conf_req *) data;
	void *ptr = req->data;

	BT_DBG("sk %p", sk);

	if (pi->imtu != L2CAP_DEFAULT_MTU)
		l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);

	/* FIXME. Need actual value of the flush timeout */
	//if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
	//   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);

	req->dcid  = __cpu_to_le16(pi->dcid);
	req->flags = __cpu_to_le16(0);

	return ptr - data;
}

static inline int l2cap_conf_output(struct sock *sk, void **ptr)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	int result = 0;

	/* Configure output options and let the other side know
	 * which ones we don't like.
	 */
	if (pi->conf_mtu < pi->omtu) {
		l2cap_add_conf_opt(ptr, L2CAP_CONF_MTU, 2, pi->omtu);
		result = L2CAP_CONF_UNACCEPT;
	} else {
		pi->omtu = pi->conf_mtu;
	}

	BT_DBG("sk %p result %d", sk, result);
	return result;
}

static int l2cap_build_conf_rsp(struct sock *sk, void *data, int *result)
{
	l2cap_conf_rsp *rsp = (l2cap_conf_rsp *) data;
	void *ptr = rsp->data;

	BT_DBG("sk %p complete %d", sk, result ? 1 : 0);

	if (result)
		*result = l2cap_conf_output(sk, &ptr);

	rsp->scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
	rsp->result = __cpu_to_le16(result ? *result : 0);
	rsp->flags  = __cpu_to_le16(0);

	return ptr - data;
}

static inline int l2cap_connect_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
{
	struct l2cap_chan_list *list = &conn->chan_list;
	l2cap_conn_req *req = (l2cap_conn_req *) data;
	l2cap_conn_rsp rsp;
	struct sock *sk, *parent;
	int result = 0, status = 0;

	__u16 dcid = 0, scid = __le16_to_cpu(req->scid);
	__u16 psm  = req->psm;

	BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);

	/* Check if we have socket listening on psm */
	parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
	if (!parent) {
		result = L2CAP_CR_BAD_PSM;
		goto sendresp;
	}

	result = L2CAP_CR_NO_MEM;

	/* Check for backlog size */
	if (parent->ack_backlog > parent->max_ack_backlog) {
		BT_DBG("backlog full %d", parent->ack_backlog); 
		goto response;
	}

	sk = l2cap_sock_alloc(NULL, BTPROTO_L2CAP, GFP_ATOMIC);
	if (!sk)
		goto response;

	write_lock(&list->lock);

	/* Check if we already have channel with that dcid */
	if (__l2cap_get_chan_by_dcid(list, scid)) {
		write_unlock(&list->lock);
		sk->zapped = 1;
		l2cap_sock_kill(sk);
		goto response;
	}

	hci_conn_hold(conn->hcon);

	l2cap_sock_init(sk, parent);
	bacpy(&bluez_pi(sk)->src, conn->src);
	bacpy(&bluez_pi(sk)->dst, conn->dst);
	l2cap_pi(sk)->psm  = psm;
	l2cap_pi(sk)->dcid = scid;

	__l2cap_chan_add(conn, sk, parent);
	dcid = l2cap_pi(sk)->scid;

	l2cap_sock_set_timer(sk, sk->sndtimeo);

	/* Service level security */
	result = L2CAP_CR_PEND;
	status = L2CAP_CS_AUTHEN_PEND;
	sk->state = BT_CONNECT2;
	l2cap_pi(sk)->ident = cmd->ident;
	
	if (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) {
		if (!hci_conn_encrypt(conn->hcon))
			goto done;
	} else if (l2cap_pi(sk)->link_mode & L2CAP_LM_AUTH) {
		if (!hci_conn_auth(conn->hcon))
			goto done;
	}

	sk->state = BT_CONFIG;
	result = status = 0;

done:
	write_unlock(&list->lock);

response:
	bh_unlock_sock(parent);

sendresp:
	rsp.scid   = __cpu_to_le16(scid);
	rsp.dcid   = __cpu_to_le16(dcid);