l2cap.c

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

	rsp.result = __cpu_to_le16(result);
	rsp.status = __cpu_to_le16(status);
	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;
	char req[128];

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

	BT_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;

	switch (result) {
	case L2CAP_CR_SUCCESS:
		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);
		break;

	case L2CAP_CR_PEND:
		break;

	default:
		l2cap_chan_del(sk, ECONNREFUSED);
		break;
	}

	bh_unlock_sock(sk);
	return 0;
}

static inline int l2cap_config_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
{
	l2cap_conf_req * req = (l2cap_conf_req *) data;
	__u16 dcid, flags;
	__u8 rsp[64];
	struct sock *sk;
	int result;

	dcid  = __le16_to_cpu(req->dcid);
	flags = __le16_to_cpu(req->flags);

	BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);

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

	l2cap_parse_conf_req(sk, req->data, cmd->len - L2CAP_CONF_REQ_SIZE);

	if (flags & 0x01) {
		/* Incomplete config. Send empty response. */
		l2cap_send_rsp(conn, cmd->ident, L2CAP_CONF_RSP, l2cap_build_conf_rsp(sk, rsp, NULL), rsp);
		goto unlock;
	}

	/* Complete config. */
	l2cap_send_rsp(conn, cmd->ident, L2CAP_CONF_RSP, l2cap_build_conf_rsp(sk, rsp, &result), rsp);

	if (result)
		goto unlock;

	/* Output config done */
	l2cap_pi(sk)->conf_state |= CONF_OUTPUT_DONE;

	if (l2cap_pi(sk)->conf_state & CONF_INPUT_DONE) {
		sk->state = BT_CONNECTED;
		l2cap_chan_ready(sk);
	} else if (!(l2cap_pi(sk)->conf_state & CONF_REQ_SENT)) {
		char req[64];
		l2cap_send_req(conn, L2CAP_CONF_REQ, l2cap_build_conf_req(sk, req), req);
	}

unlock:
	bh_unlock_sock(sk);
	return 0;
}

static inline int l2cap_config_rsp(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
{
	l2cap_conf_rsp *rsp = (l2cap_conf_rsp *)data;
	__u16 scid, flags, result;
	struct sock *sk;
	int err = 0;

	scid   = __le16_to_cpu(rsp->scid);
	flags  = __le16_to_cpu(rsp->flags);
	result = __le16_to_cpu(rsp->result);

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

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

	if (result) {
		l2cap_disconn_req req;

		/* They didn't like our options. Well... we do not negotiate.
		 * Close channel.
		 */
		sk->state = BT_DISCONN;
		l2cap_sock_set_timer(sk, HZ * 5);

		req.dcid = __cpu_to_le16(l2cap_pi(sk)->dcid);
		req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
		l2cap_send_req(conn, L2CAP_DISCONN_REQ, L2CAP_DISCONN_REQ_SIZE, &req);
		goto done;
	}

	if (flags & 0x01)
		goto done;

	/* Input config done */
	l2cap_pi(sk)->conf_state |= CONF_INPUT_DONE;

	if (l2cap_pi(sk)->conf_state & CONF_OUTPUT_DONE) {
		sk->state = BT_CONNECTED;
		l2cap_chan_ready(sk);
	}

done:
	bh_unlock_sock(sk);
	return err;
}

static inline int l2cap_disconnect_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
{
	l2cap_disconn_req *req = (l2cap_disconn_req *) data;
	l2cap_disconn_rsp rsp;
	__u16 dcid, scid;
	struct sock *sk;

	scid = __le16_to_cpu(req->scid);
	dcid = __le16_to_cpu(req->dcid);

	BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);

	if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
		return 0;

	rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
	rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
	l2cap_send_rsp(conn, cmd->ident, L2CAP_DISCONN_RSP, L2CAP_DISCONN_RSP_SIZE, &rsp);

	sk->shutdown = SHUTDOWN_MASK;
	
	l2cap_chan_del(sk, ECONNRESET);
	bh_unlock_sock(sk);

	l2cap_sock_kill(sk);
	return 0;
}

static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
{
	l2cap_disconn_rsp *rsp = (l2cap_disconn_rsp *) data;
	__u16 dcid, scid;
	struct sock *sk;

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

	BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);

	if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
		return 0;
	l2cap_chan_del(sk, ECONNABORTED);
	bh_unlock_sock(sk);

	l2cap_sock_kill(sk);
	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, 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;
		}

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

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

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

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

		/* Allocate skb for the complete frame (with header) */
		if (!(conn->rx_skb = bluez_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 large (len %d, expect %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 ------ */
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");
}

module_init(l2cap_init);
module_exit(l2cap_cleanup);

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