l2cap_core.c

嵌入式系统设计与实例开发实验教材二源码 多线程应用程序设计 串行端口程序设计 AD接口实验 CAN总线通信实验 GPS通信实验 Linux内核移植与编译实验 IC卡读写实验 SD驱动使

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;

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

	l2cap_conn_clear_timer(conn);

	atomic_inc(&conn->refcnt);
	l2cap_pi(sk)->conn = conn;

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

	__l2cap_chan_link(l, sk);

	if (parent)
		l2cap_accept_queue(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;
	struct sock *parent;

	conn = l2cap_pi(sk)->conn;
	parent = l2cap_pi(sk)->parent;

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

	if (parent) {
		/* Unlink from parent accept queue */
		bh_lock_sock(parent);
		l2cap_accept_unlink(sk);
		bh_unlock_sock(parent);
	}

	if (conn) { 
		long timeout;

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

		if (conn->out)
			timeout = L2CAP_DISCONN_TIMEOUT;
		else
			timeout = L2CAP_CONN_IDLE_TIMEOUT;
		
		if (atomic_dec_and_test(&conn->refcnt) && conn->state == BT_CONNECTED) {
			/* Schedule Baseband disconnect */
			l2cap_conn_set_timer(conn, timeout);
		}
	}

	sk->state  = BT_CLOSED;
	sk->err    = err;
	sk->state_change(sk);

	sk->zapped = 1;
}

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

	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) {
			sk->state = BT_CONNECTED;
			sk->state_change(sk);
			l2cap_sock_clear_timer(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);

			l2cap_sock_set_timer(sk, sk->sndtimeo);
		}

		bh_unlock_sock(sk);
	}

	read_unlock(&l->lock);
}

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

	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, 1);
	}
}

/* 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;

	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;

		skb_queue_tail(&sk->receive_queue, nskb);
		sk->data_ready(sk, nskb->len);
	}
	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, size, count, sent=0;
	l2cap_hdr *lh;

	/* Check outgoing MTU */
	if (len > l2cap_pi(sk)->omtu)
		return -EINVAL;

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

	/* First fragment (with L2CAP header) */
	count = MIN(conn->iff->mtu - L2CAP_HDR_SIZE, len);
	size  = L2CAP_HDR_SIZE + count;
	if (!(skb = bluez_skb_send_alloc(sk, size, msg->msg_flags & MSG_DONTWAIT, &err)))
		return err;

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

	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->iff->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->hconn, 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 inline struct sk_buff *l2cap_build_cmd(__u8 code, __u8 ident, __u16 len, void *data)
{
	struct sk_buff *skb;
	l2cap_cmd_hdr *cmd;
	l2cap_hdr *lh;
	int size;

	DBG("code 0x%2.2x, ident 0x%2.2x, len %d", code, ident, len);

	size = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + len;
	if (!(skb = bluez_skb_alloc(size, GFP_ATOMIC)))
		return NULL;

	lh = (l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
	lh->len = __cpu_to_le16(L2CAP_CMD_HDR_SIZE + len);
	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(len);

	if (len)
		memcpy(skb_put(skb, len), data, len);

	return skb;
}

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

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

	ident = l2cap_get_ident(conn);
	if (!(skb = l2cap_build_cmd(code, ident, len, data)))
		return -ENOMEM;
	return hci_send_acl(conn->hconn, skb, 0);
}

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

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

	if (!(skb = l2cap_build_cmd(code, ident, len, data)))
		return -ENOMEM;
	return hci_send_acl(conn->hconn, skb, 0);
}

static inline int l2cap_get_conf_opt(__u8 **ptr, __u8 *type, __u32 *val)
{
	l2cap_conf_opt *opt = (l2cap_conf_opt *) (*ptr);
	int len;

	*type = opt->type;
	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 = 0L;
		break;
	};

	DBG("type 0x%2.2x len %d val 0x%8.8x", *type, opt->len, *val);

	len = L2CAP_CONF_OPT_SIZE + opt->len;

	*ptr += len;

	return len;
}

static inline void l2cap_parse_conf_req(struct sock *sk, char *data, int len)
{
	__u8 type, hint; __u32 val;
	__u8 *ptr = data;

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

	while (len >= L2CAP_CONF_OPT_SIZE) {
		len -= l2cap_get_conf_opt(&ptr, &type, &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 unknon option */
			break;
		};
	}
}

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

	DBG("type 0x%2.2x len %d val 0x%8.8x", 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;
	};

	*ptr += L2CAP_CONF_OPT_SIZE + len;
}

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

	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 int l2cap_conf_output(struct sock *sk, __u8 **ptr)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	int result = 0;

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

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

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

	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;

	__u16 scid = __le16_to_cpu(req->scid);
	__u16 psm  = req->psm;

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

	/* Check if we have socket listening on psm */
	if (!(parent = l2cap_get_sock_listen(&conn->src, psm)))
		goto reject;

	bh_lock_sock(parent);
	write_lock(&list->lock);

	/* Check if we already have channel with that dcid */
	if (__l2cap_get_chan_by_dcid(list, scid))
		goto unlock;

	/* Check for backlog size */
	if (parent->ack_backlog > parent->max_ack_backlog)
		goto unlock;

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

	l2cap_sock_init(sk, parent);

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

	__l2cap_chan_add(conn, sk, parent);
	sk->state = BT_CONFIG;

	write_unlock(&list->lock);
	bh_unlock_sock(parent);

	rsp.dcid   = __cpu_to_le16(l2cap_pi(sk)->scid);
	rsp.scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
	rsp.result = __cpu_to_le16(0);
	rsp.status = __cpu_to_le16(0);
	l2cap_send_rsp(conn, cmd->ident, L2CAP_CONN_RSP, L2CAP_CONN_RSP_SIZE, &rsp);

	return 0;

unlock:
	write_unlock(&list->lock);
	bh_unlock_sock(parent);

reject:
	rsp.scid   = __cpu_to_le16(scid);
	rsp.dcid   = __cpu_to_le16(0);
	rsp.status = __cpu_to_le16(0);
	rsp.result = __cpu_to_le16(L2CAP_CONN_NO_MEM);
	l2cap_send_rsp(conn, cmd->ident, L2CAP_CONN_RSP, L2CAP_CONN_RSP_SIZE, &rsp);

	return 0;
}

static inline int l2cap_connect_rsp(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
{
	l2cap_conn_rsp *rsp = (l2cap_conn_rsp *) data;
	__u16 scid, dcid, result, status;
	struct sock *sk;

	scid   = __le16_to_cpu(rsp->scid);
	dcid   = __le16_to_cpu(rsp->dcid);
	result = __le16_to_cpu(rsp->result);
	status = __le16_to_cpu(rsp->status);

	DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);

	if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
		return -ENOENT;

	bh_lock_sock(sk);

	if (!result) {
		char req[64];

		sk->state = BT_CONFIG;
		l2cap_pi(sk)->dcid = dcid;
		l2cap_pi(sk)->conf_state |= CONF_REQ_SENT;

		l2cap_send_req(conn, L2CAP_CONF_REQ, l2cap_build_conf_req(sk, req), req);