l2cap.c

Linux嵌入式平台蓝牙协议栈软件

			break;

		case L2CAP_CONN_REQ:
			err = l2cap_connect_req(conn, &cmd, data);
			break;

		case L2CAP_CONN_RSP:
			err = l2cap_connect_rsp(conn, &cmd, data);
			break;

		case L2CAP_CONF_REQ:
			err = l2cap_config_req(conn, &cmd, data);
			break;

		case L2CAP_CONF_RSP:
			err = l2cap_config_rsp(conn, &cmd, data);
			break;

		case L2CAP_DISCONN_REQ:
			err = l2cap_disconnect_req(conn, &cmd, data);
			break;

		case L2CAP_DISCONN_RSP:
			err = l2cap_disconnect_rsp(conn, &cmd, data);
			break;

		case L2CAP_ECHO_REQ:
			l2cap_send_rsp(conn, cmd.ident, L2CAP_ECHO_RSP, cmd.len, data);
			break;

		case L2CAP_ECHO_RSP:
			break;

		case L2CAP_INFO_REQ:
			err = l2cap_info_req(conn, &cmd, data);
			break;

		case L2CAP_INFO_RSP:
			err = l2cap_info_rsp(conn, &cmd, data);
			break;

		default:
			BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
			err = -EINVAL;
			break;
		}

		if (err) {
			struct l2cap_cmd_rej rej;
			BT_DBG("error %d", err);

			/* FIXME: Map err to a valid reason */
			rej.reason = __cpu_to_le16(0);
			l2cap_send_rsp(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
		}

		data += cmd.len;
		len  -= cmd.len;
	}

	kfree_skb(skb);
}

static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
{
	struct sock *sk;

	sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
	if (!sk) {
		BT_DBG("unknown cid 0x%4.4x", cid);
		goto drop;
	}

	BT_DBG("sk %p, len %d", sk, skb->len);

	if (sk->sk_state != BT_CONNECTED)
		goto drop;

	if (l2cap_pi(sk)->imtu < skb->len)
		goto drop;

	/* If socket recv buffers overflows we drop data here
	 * which is *bad* because L2CAP has to be reliable.
	 * But we don't have any other choice. L2CAP doesn't
	 * provide flow control mechanism. */

	if (!sock_queue_rcv_skb(sk, skb))
		goto done;

drop:
	kfree_skb(skb);

done:
	if (sk) bh_unlock_sock(sk);
	return 0;
}

static inline int l2cap_conless_channel(struct l2cap_conn *conn, u16 psm, struct sk_buff *skb)
{
	struct sock *sk;

	sk = l2cap_get_sock_by_psm(0, psm, conn->src);
	if (!sk)
		goto drop;

	BT_DBG("sk %p, len %d", sk, skb->len);

	if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
		goto drop;

	if (l2cap_pi(sk)->imtu < skb->len)
		goto drop;

	if (!sock_queue_rcv_skb(sk, skb))
		goto done;

drop:
	kfree_skb(skb);

done:
	if (sk) bh_unlock_sock(sk);
	return 0;
}

static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct l2cap_hdr *lh = (void *) skb->data;
	u16 cid, psm, len;

	skb_pull(skb, L2CAP_HDR_SIZE);
	cid = __le16_to_cpu(lh->cid);
	len = __le16_to_cpu(lh->len);

	BT_DBG("len %d, cid 0x%4.4x", len, cid);

	switch (cid) {
	case 0x0001:
		l2cap_sig_channel(conn, skb);
		break;

	case 0x0002:
		psm = get_unaligned((u16 *) skb->data);
		skb_pull(skb, 2);
		l2cap_conless_channel(conn, psm, skb);
		break;

	default:
		l2cap_data_channel(conn, cid, skb);
		break;
	}
}

/* ---- L2CAP interface with lower layer (HCI) ---- */

static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
{
	int exact = 0, lm1 = 0, lm2 = 0;
	register struct sock *sk;
	struct hlist_node *node;

	if (type != ACL_LINK)
		return 0;

	BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));

	/* Find listening sockets and check their link_mode */
	read_lock(&l2cap_sk_list.lock);
	sk_for_each(sk, node, &l2cap_sk_list.head) {
		if (sk->sk_state != BT_LISTEN)
			continue;

		if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
			lm1 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
			exact++;
		} else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
			lm2 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
	}
	read_unlock(&l2cap_sk_list.lock);

	return exact ? lm1 : lm2;
}

static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
{
	BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);

	if (hcon->type != ACL_LINK)
		return 0;

	if (!status) {
		struct l2cap_conn *conn;

		conn = l2cap_conn_add(hcon, status);
		if (conn)
			l2cap_conn_ready(conn);
	} else 
		l2cap_conn_del(hcon, bt_err(status));

	return 0;
}

static int l2cap_disconn_ind(struct hci_conn *hcon, u8 reason)
{
	BT_DBG("hcon %p reason %d", hcon, reason);

	if (hcon->type != ACL_LINK)
		return 0;

	l2cap_conn_del(hcon, bt_err(reason));
	return 0;
}

static int l2cap_auth_cfm(struct hci_conn *hcon, u8 status)
{
	struct l2cap_chan_list *l;
	struct l2cap_conn *conn;
	struct l2cap_conn_rsp rsp;
	struct sock *sk;
	int result;

	if (!(conn = hcon->l2cap_data))
		return 0;
	l = &conn->chan_list;

	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->sk_state != BT_CONNECT2 ||
				(l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT)) {
			bh_unlock_sock(sk);
			continue;
		}

		if (!status) {
			sk->sk_state = BT_CONFIG;
			result = 0;
		} else {
			sk->sk_state = BT_DISCONN;
			l2cap_sock_set_timer(sk, HZ/10);
			result = L2CAP_CR_SEC_BLOCK;
		}

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

		bh_unlock_sock(sk);
	}

	read_unlock(&l->lock);
	return 0;
}

static int l2cap_encrypt_cfm(struct hci_conn *hcon, u8 status)
{
	struct l2cap_chan_list *l;
	struct l2cap_conn *conn;
	struct l2cap_conn_rsp rsp;
	struct sock *sk;
	int result;

	if (!(conn = hcon->l2cap_data))
		return 0;
	l = &conn->chan_list;

	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->sk_state != BT_CONNECT2) {
			bh_unlock_sock(sk);
			continue;
		}

		if (!status) {
			sk->sk_state = BT_CONFIG;
			result = 0;
		} else {
			sk->sk_state = BT_DISCONN;
			l2cap_sock_set_timer(sk, HZ/10);
			result = L2CAP_CR_SEC_BLOCK;
		}

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

		bh_unlock_sock(sk);
	}

	read_unlock(&l->lock);
	return 0;
}

static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
{
	struct l2cap_conn *conn = hcon->l2cap_data;

	if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
		goto drop;

	BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);

	if (flags & ACL_START) {
		struct l2cap_hdr *hdr;
		int len;

		if (conn->rx_len) {
			BT_ERR("Unexpected start frame (len %d)", skb->len);
			kfree_skb(conn->rx_skb);
			conn->rx_skb = NULL;
			conn->rx_len = 0;
		}

		if (skb->len < 2) {
			BT_ERR("Frame is too short (len %d)", skb->len);
			goto drop;
		}

		hdr = (struct l2cap_hdr *) skb->data;
		len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;

		if (len == skb->len) {
			/* Complete frame received */
			l2cap_recv_frame(conn, skb);
			return 0;
		}

		BT_DBG("Start: total len %d, frag len %d", len, skb->len);

		if (skb->len > len) {
			BT_ERR("Frame is too long (len %d, expected len %d)",
				skb->len, len);
			goto drop;
		}

		/* Allocate skb for the complete frame (with header) */
		if (!(conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC)))
			goto drop;

		memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
		conn->rx_len = len - skb->len;
	} else {
		BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);

		if (!conn->rx_len) {
			BT_ERR("Unexpected continuation frame (len %d)", skb->len);
			goto drop;
		}

		if (skb->len > conn->rx_len) {
			BT_ERR("Fragment is too long (len %d, expected %d)",
					skb->len, conn->rx_len);
			kfree_skb(conn->rx_skb);
			conn->rx_skb = NULL;
			conn->rx_len = 0;
			goto drop;
		}

		memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
		conn->rx_len -= skb->len;

		if (!conn->rx_len) {
			/* Complete frame received */
			l2cap_recv_frame(conn, conn->rx_skb);
			conn->rx_skb = NULL;
		}
	}

drop:
	kfree_skb(skb);
	return 0;
}

/* ---- Proc fs support ---- */
#ifdef CONFIG_PROC_FS
static void *l2cap_seq_start(struct seq_file *seq, loff_t *pos)
{
	struct sock *sk;
	struct hlist_node *node;
	loff_t l = *pos;

	read_lock_bh(&l2cap_sk_list.lock);

	sk_for_each(sk, node, &l2cap_sk_list.head)
		if (!l--)
			goto found;
	sk = NULL;
found:
	return sk;
}

static void *l2cap_seq_next(struct seq_file *seq, void *e, loff_t *pos)
{
	(*pos)++;
	return sk_next(e);
}

static void l2cap_seq_stop(struct seq_file *seq, void *e)
{
	read_unlock_bh(&l2cap_sk_list.lock);
}

static int  l2cap_seq_show(struct seq_file *seq, void *e)
{
	struct sock *sk = e;
	struct l2cap_pinfo *pi = l2cap_pi(sk);

	seq_printf(seq, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d 0x%x\n",
			batostr(&bt_sk(sk)->src), batostr(&bt_sk(sk)->dst), 
			sk->sk_state, pi->psm, pi->scid, pi->dcid, pi->imtu,
			pi->omtu, pi->link_mode);
	return 0;
}

static struct seq_operations l2cap_seq_ops = {
	.start	= l2cap_seq_start,
	.next	= l2cap_seq_next,
	.stop	= l2cap_seq_stop,
	.show	= l2cap_seq_show 
};

static int l2cap_seq_open(struct inode *inode, struct file *file)
{
	return seq_open(file, &l2cap_seq_ops);
}

static struct file_operations l2cap_seq_fops = {
	.owner		= THIS_MODULE,
	.open		= l2cap_seq_open,
	.read		= seq_read,
	.llseek		= seq_lseek,
	.release	= seq_release,
};

static int __init l2cap_proc_init(void)
{
	struct proc_dir_entry *p = create_proc_entry("l2cap", S_IRUGO, proc_bt);
	if (!p)
		return -ENOMEM;
	p->owner     = THIS_MODULE;
	p->proc_fops = &l2cap_seq_fops;
	return 0;
}

static void __exit l2cap_proc_cleanup(void)
{
	remove_proc_entry("l2cap", proc_bt);
}

#else /* CONFIG_PROC_FS */

static int __init l2cap_proc_init(void)
{
	return 0;
}

static void __exit l2cap_proc_cleanup(void)
{
	return;
}
#endif /* CONFIG_PROC_FS */

static struct proto_ops l2cap_sock_ops = {
	.family		= PF_BLUETOOTH,
	.owner		= THIS_MODULE,
	.release	= l2cap_sock_release,
	.bind		= l2cap_sock_bind,
	.connect	= l2cap_sock_connect,
	.listen		= l2cap_sock_listen,
	.accept		= l2cap_sock_accept,
	.getname	= l2cap_sock_getname,
	.sendmsg	= l2cap_sock_sendmsg,
	.recvmsg	= bt_sock_recvmsg,
	.poll		= bt_sock_poll,
	.mmap		= sock_no_mmap,
	.socketpair	= sock_no_socketpair,
	.ioctl		= sock_no_ioctl,
	.shutdown	= l2cap_sock_shutdown,
	.setsockopt	= l2cap_sock_setsockopt,
	.getsockopt	= l2cap_sock_getsockopt
};

static struct net_proto_family l2cap_sock_family_ops = {
	.family	= PF_BLUETOOTH,
	.owner	= THIS_MODULE,
	.create	= l2cap_sock_create,
};

static struct hci_proto l2cap_hci_proto = {
	.name		= "L2CAP",
	.id		= HCI_PROTO_L2CAP,
	.connect_ind	= l2cap_connect_ind,
	.connect_cfm	= l2cap_connect_cfm,
	.disconn_ind	= l2cap_disconn_ind,
	.auth_cfm	= l2cap_auth_cfm,
	.encrypt_cfm	= l2cap_encrypt_cfm,
	.recv_acldata	= l2cap_recv_acldata
};

static int __init l2cap_init(void)
{
	int err;

	if ((err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops))) {
		BT_ERR("L2CAP socket registration failed");
		return err;
	}

	if ((err = hci_register_proto(&l2cap_hci_proto))) {
		BT_ERR("L2CAP protocol registration failed");
		return err;
	}

	l2cap_proc_init();

	BT_INFO("L2CAP ver %s", VERSION);
	BT_INFO("L2CAP socket layer initialized");

	return 0;
}

static void __exit l2cap_exit(void)
{
	l2cap_proc_cleanup();

	/* Unregister socket and protocol */
	if (bt_sock_unregister(BTPROTO_L2CAP))
		BT_ERR("L2CAP socket unregistration failed");

	if (hci_unregister_proto(&l2cap_hci_proto))
		BT_ERR("L2CAP protocol unregistration failed");
}

void l2cap_load(void)
{
	/* Dummy function to trigger automatic L2CAP module loading by
	 * other modules that use L2CAP sockets but don not use any othe
	 * symbols from it. */
	return;
}
EXPORT_SYMBOL(l2cap_load);

module_init(l2cap_init);
module_exit(l2cap_exit);

MODULE_AUTHOR("Maxim Krasnyansky <[e-mail]>, Marcel Holtmann <[e-mail]>");
MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
MODULE_VERSION(VERSION);
MODULE_LICENSE("GPL");
MODULE_ALIAS("bt-proto-0");