l2cap.c

基于liunx的蓝牙协议栈源代码版本為2.13,可以方便的下載和移植！

	return 0;
}

static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
{
	__u8 *data = skb->data;
	int len = skb->len;
	l2cap_cmd_hdr cmd;
	int err = 0;

	while (len >= L2CAP_CMD_HDR_SIZE) {
		memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
		data += L2CAP_CMD_HDR_SIZE;
		len  -= L2CAP_CMD_HDR_SIZE;

		cmd.len = __le16_to_cpu(cmd.len);

		BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd.len, cmd.ident);

		if (cmd.len > len || !cmd.ident) {
			BT_DBG("corrupted command");
			break;
		}

		switch (cmd.code) {
		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_COMMAND_REJ:
			/* FIXME: We should process this */
			l2cap_raw_recv(conn, skb);
			break;

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

		case L2CAP_ECHO_RSP:
		case L2CAP_INFO_REQ:
		case L2CAP_INFO_RSP:
			l2cap_raw_recv(conn, skb);
			break;

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

		if (err) {
			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, L2CAP_CMD_REJ_SIZE, &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->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->state != BT_BOUND && 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)
{
	l2cap_hdr *lh = (l2cap_hdr *) 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;

	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);
	for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
		if (sk->state != BT_LISTEN)
			continue;

		if (!bacmp(&bluez_pi(sk)->src, &hdev->bdaddr)) {
			lm1 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
			exact++;
		} else if (!bacmp(&bluez_pi(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, bterr(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, bterr(reason));
	return 0;
}

static int l2cap_auth_cfm(struct hci_conn *hcon, __u8 status)
{
	struct l2cap_chan_list *l;
	struct l2cap_conn *conn;
	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->state != BT_CONNECT2 ||
				(l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT)) {
			bh_unlock_sock(sk);
			continue;
		}

		if (!status) {
			sk->state = BT_CONFIG;
			result = 0;
		} else {
			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,
			L2CAP_CONN_RSP_SIZE, &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;
	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->state != BT_CONNECT2) {
			bh_unlock_sock(sk);
			continue;
		}

		if (!status) {
			sk->state = BT_CONFIG;
			result = 0;
		} else {
			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, 
			L2CAP_CONN_RSP_SIZE, &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) {
		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;
			l2cap_conn_unreliable(conn, ECOMM);
		}

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

		hdr = (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);
			l2cap_conn_unreliable(conn, ECOMM);
			goto drop;
		}

		/* Allocate skb for the complete frame including header */
		conn->rx_skb = bluez_skb_alloc(len, GFP_ATOMIC);
		if (!conn->rx_skb)
			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);
			l2cap_conn_unreliable(conn, ECOMM);
			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;
			l2cap_conn_unreliable(conn, ECOMM);
			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 ------ */
static int l2cap_sock_dump(char *buf, struct bluez_sock_list *list)
{
	struct l2cap_pinfo *pi;
	struct sock *sk;
	char *ptr = buf;

	read_lock_bh(&list->lock);

	for (sk = list->head; sk; sk = sk->next) {
		pi = l2cap_pi(sk);
		ptr += sprintf(ptr, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d 0x%x\n",
				batostr(&bluez_pi(sk)->src), batostr(&bluez_pi(sk)->dst), 
				sk->state, pi->psm, pi->scid, pi->dcid, pi->imtu, pi->omtu,
				pi->link_mode);
	}

	read_unlock_bh(&list->lock);

	ptr += sprintf(ptr, "\n");
	return ptr - buf;
}

static int l2cap_read_proc(char *buf, char **start, off_t offset, int count, int *eof, void *priv)
{
	char *ptr = buf;
	int len;

	BT_DBG("count %d, offset %ld", count, offset);

	ptr += l2cap_sock_dump(ptr, &l2cap_sk_list);
	len  = ptr - buf;

	if (len <= count + offset)
		*eof = 1;

	*start = buf + offset;
	len -= offset;

	if (len > count)
		len = count;
	if (len < 0)
		len = 0;

	return len;
}

static struct proto_ops l2cap_sock_ops = {
	family:		PF_BLUETOOTH,
	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:	bluez_sock_recvmsg,
	poll:		bluez_sock_poll,
	socketpair:	sock_no_socketpair,
	ioctl:		sock_no_ioctl,
	shutdown:	l2cap_sock_shutdown,
	setsockopt:	l2cap_sock_setsockopt,
	getsockopt:	l2cap_sock_getsockopt,
	mmap:		sock_no_mmap
};

static struct net_proto_family l2cap_sock_family_ops = {
	family:		PF_BLUETOOTH,
	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,
	recv_acldata:	l2cap_recv_acldata,
	auth_cfm:	l2cap_auth_cfm,
	encrypt_cfm:	l2cap_encrypt_cfm
};

int __init l2cap_init(void)
{
	int err;

	if ((err = bluez_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops))) {
		BT_ERR("Can't register L2CAP socket");
		return err;
	}

	if ((err = hci_register_proto(&l2cap_hci_proto))) {
		BT_ERR("Can't register L2CAP protocol");
		return err;
	}

	create_proc_read_entry("bluetooth/l2cap", 0, 0, l2cap_read_proc, NULL);

	BT_INFO("BlueZ L2CAP ver %s Copyright (C) 2000,2001 Qualcomm Inc", VERSION);
	BT_INFO("Written 2000,2001 by Maxim Krasnyansky <[e-mail]>");
	return 0;
}

void l2cap_cleanup(void)
{
	remove_proc_entry("bluetooth/l2cap", NULL);

	/* Unregister socket and protocol */
	if (bluez_sock_unregister(BTPROTO_L2CAP))
		BT_ERR("Can't unregister L2CAP socket");

	if (hci_unregister_proto(&l2cap_hci_proto))
		BT_ERR("Can't unregister L2CAP protocol");
}

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

EXPORT_SYMBOL(l2cap_load);

module_init(l2cap_init);
module_exit(l2cap_cleanup);

MODULE_AUTHOR("Maxim Krasnyansky <[e-mail]>");
MODULE_DESCRIPTION("BlueZ L2CAP ver " VERSION);
MODULE_LICENSE("GPL");