l2cap_core.c

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

	} else {
		l2cap_chan_del(sk, ECONNREFUSED);
	}

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

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

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

	bh_lock_sock(sk);

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

	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;

	bh_lock_sock(sk);

	if (result) {
		l2cap_disconn_req req;

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

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

		l2cap_sock_set_timer(sk, sk->sndtimeo);
		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);

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

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

	bh_lock_sock(sk);

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

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

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

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

	bh_lock_sock(sk);
	l2cap_sock_clear_timer(sk);
	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);

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

		if (cmd.len > len || !cmd.ident) {
			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:
			ERR("Uknown signaling command 0x%2.2x", cmd.code);
			err = -EINVAL;
			break;
		};

		if (err) {
			l2cap_cmd_rej rej;
			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;

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

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

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

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

	skb_queue_tail(&sk->receive_queue, skb);
	sk->data_ready(sk, skb->len);

	return 0;

drop:
	kfree_skb(skb);

	return 0;
}

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

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

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

	if (cid == 0x0001)
		l2cap_sig_channel(conn, skb);
	else	
		l2cap_data_channel(conn, cid, skb);
}

/* ------------ L2CAP interface with lower layer (HCI) ------------- */
static int l2cap_dev_event(struct notifier_block *this, unsigned long event, void *ptr)
{
	struct hci_dev *hdev = (struct hci_dev *) ptr;

	DBG("hdev %s, event %ld", hdev->name, event);

	write_lock(&l2cap_rt_lock);

	switch (event) {
	case HCI_DEV_UP:
		l2cap_iff_add(hdev);
		break;

	case HCI_DEV_DOWN:
		l2cap_iff_del(hdev);
		break;
	};

	write_unlock(&l2cap_rt_lock);

	return NOTIFY_DONE;
}

int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
{
	struct l2cap_iff *iff;

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

	if (!(iff = hdev->l2cap_data)) {
		ERR("unknown interface");
		return 0;
	}

	/* Always accept connection */
	return 1;
}

int l2cap_connect_cfm(struct hci_dev *hdev, bdaddr_t *bdaddr, __u8 status, struct hci_conn *hconn)
{
	struct l2cap_conn *conn;
	struct l2cap_iff *iff;
	int err = 0;

	DBG("hdev %s bdaddr %s hconn %p", hdev->name, batostr(bdaddr), hconn);

	if (!(iff = hdev->l2cap_data)) {
		ERR("unknown interface");
		return 0;
	}

	l2cap_iff_lock(iff);

	conn = l2cap_get_conn_by_addr(iff, bdaddr);

	if (conn) {
		/* Outgoing connection */
		DBG("Outgoing connection: %s -> %s, %p, %2.2x", batostr(iff->bdaddr), batostr(bdaddr), conn, status);

		if (!status && hconn) {
			conn->state = BT_CONNECTED;
			conn->hconn = hconn;

			hconn->l2cap_data = (void *)conn;

			/* Establish channels */
			l2cap_conn_ready(conn);
		} else {
			l2cap_conn_del(conn, bterr(status));
		}
	} else {
		/* Incomming connection */
		DBG("Incomming connection: %s -> %s, %2.2x", batostr(iff->bdaddr), batostr(bdaddr), status);
	
		if (status || !hconn)
			goto done;

		if (!(conn = l2cap_conn_add(iff, bdaddr))) {
			err = -ENOMEM;
			goto done;
		}

		conn->hconn = hconn;
		hconn->l2cap_data = (void *)conn;

		conn->state = BT_CONNECTED;
	}

done:
	l2cap_iff_unlock(iff);

	return err;
}

int l2cap_disconn_ind(struct hci_conn *hconn, __u8 reason)
{
	struct l2cap_conn *conn = hconn->l2cap_data;

	DBG("hconn %p reason %d", hconn, reason);

	if (!conn) {
		ERR("unknown connection");
		return 0;
	}
	conn->hconn = NULL;

	l2cap_iff_lock(conn->iff);
	l2cap_conn_del(conn, bterr(reason));
	l2cap_iff_unlock(conn->iff);

	return 0;
}

int l2cap_recv_acldata(struct hci_conn *hconn, struct sk_buff *skb, __u16 flags)
{
	struct l2cap_conn *conn = hconn->l2cap_data;

	if (!conn) {
		ERR("unknown connection %p", hconn);
		goto drop;
	}

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

	if (flags & ACL_START) {
		int flen, tlen, size;
		l2cap_hdr *lh;

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

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

		lh = (l2cap_hdr *)skb->data;
		tlen = __le16_to_cpu(lh->len);
		flen = skb->len - L2CAP_HDR_SIZE;

		DBG("Start: total len %d, frag len %d", tlen, flen);

		if (flen == tlen) {
			/* Complete frame received */
			l2cap_recv_frame(conn, skb);
			return 0;
		}

		/* Allocate skb for the complete frame (with header) */
		size = L2CAP_HDR_SIZE + tlen;
		if (!(conn->rx_skb = bluez_skb_alloc(size, GFP_ATOMIC)))
			goto drop;

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

		conn->rx_len = tlen - flen;
	} else {
		DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);

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

		if (skb->len > conn->rx_len) {
			ERR("Fragment is too large (len %d)", skb->len);
			kfree_skb(conn->rx_skb); conn->rx_skb = NULL;
			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;
}

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:	l2cap_sock_recvmsg,
	poll:		l2cap_sock_poll,
	socketpair:	sock_no_socketpair,
	ioctl:		sock_no_ioctl,
	shutdown:	sock_no_shutdown,
	setsockopt:	l2cap_sock_setsockopt,
	getsockopt:	l2cap_sock_getsockopt,
	mmap:		sock_no_mmap
};

struct net_proto_family l2cap_sock_family_ops = {
	family:		PF_BLUETOOTH,
	create:		l2cap_sock_create
};

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

struct notifier_block l2cap_nblock = {
	notifier_call: l2cap_dev_event
};

int __init l2cap_init(void)
{
	INF("BlueZ L2CAP ver %s Copyright (C) 2000,2001 Qualcomm Inc",
		VERSION);
	INF("Written 2000,2001 by Maxim Krasnyansky <[e-mail]>");

	if (bluez_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops)) {
		ERR("Can't register L2CAP socket");
		return -EPROTO;
	}

	if (hci_register_proto(&l2cap_hci_proto) < 0) {
		ERR("Can't register L2CAP protocol");
		return -EPROTO;
	}

	hci_register_notifier(&l2cap_nblock);

	l2cap_register_proc();

	return 0;
}

void l2cap_cleanup(void)
{
	l2cap_unregister_proc();

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

	if (hci_unregister_proto(&l2cap_hci_proto) < 0)
		ERR("Can't unregister L2CAP protocol");

	hci_unregister_notifier(&l2cap_nblock);

	/* We _must_ not have any sockets and/or connections
	 * at this stage.
	 */

	/* Free interface list and unlock HCI devices */
	{
		struct list_head *list = &l2cap_iff_list;

		while (!list_empty(list)) {
			struct l2cap_iff *iff;

			iff = list_entry(list->next, struct l2cap_iff, list);
			l2cap_iff_del(iff->hdev);
		}
	}
}

module_init(l2cap_init);
module_exit(l2cap_cleanup);

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