l2cap_core.c

嵌入式系统设计与实验教材二源码linux内核移植与编译

/* 
   BlueZ - Bluetooth protocol stack for Linux
   Copyright (C) 2000-2001 Qualcomm Incorporated

   Written 2000,2001 by Maxim Krasnyansky <[e-mail]>

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License version 2 as
   published by the Free Software Foundation;

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
   IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
   CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES 
   WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 
   ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 
   OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

   ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 
   COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 
   SOFTWARE IS DISCLAIMED.
*/

/*
 * BlueZ L2CAP core and sockets.
 *
 * $Id: l2cap_core.c,v 1.19 2001/08/03 04:19:50 maxk Exp $
 */
#define VERSION "1.1"

#include <linux/config.h>
#include <linux/module.h>

#include <linux/types.h>
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/major.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/poll.h>
#include <linux/fcntl.h>
#include <linux/init.h>
#include <linux/skbuff.h>
#include <linux/interrupt.h>
#include <linux/socket.h>
#include <linux/skbuff.h>
#include <linux/proc_fs.h>
#include <linux/list.h>
#include <net/sock.h>

#include <asm/system.h>
#include <asm/uaccess.h>

#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/bluez.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>
#include <net/bluetooth/l2cap_core.h>

#ifndef L2CAP_DEBUG
#undef  DBG
#define DBG( A... )
#endif

struct proto_ops l2cap_sock_ops;

struct bluez_sock_list l2cap_sk_list = {
	lock: RW_LOCK_UNLOCKED
};

struct list_head l2cap_iff_list = LIST_HEAD_INIT(l2cap_iff_list);
rwlock_t l2cap_rt_lock = RW_LOCK_UNLOCKED;

static int  l2cap_conn_del(struct l2cap_conn *conn, int err);

static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent);
static void l2cap_chan_del(struct sock *sk, int err);
static int  l2cap_chan_send(struct sock *sk, struct msghdr *msg, int len);

static void l2cap_sock_close(struct sock *sk);
static void l2cap_sock_kill(struct sock *sk);

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

/* -------- L2CAP interfaces & routing --------- */
/* Add/delete L2CAP interface.
 * Must be called with locked rt_lock
 */ 

static void l2cap_iff_add(struct hci_dev *hdev)
{
	struct l2cap_iff *iff;

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

	DBG("iff_list %p next %p prev %p", &l2cap_iff_list, l2cap_iff_list.next, l2cap_iff_list.prev);

	/* Allocate new interface and lock HCI device */
	if (!(iff = kmalloc(sizeof(struct l2cap_iff), GFP_KERNEL))) {
		ERR("Can't allocate new interface %s", hdev->name);
		return;
	}
	memset(iff, 0, sizeof(struct l2cap_iff));

	hci_dev_hold(hdev);
	hdev->l2cap_data = iff;
	iff->hdev   = hdev;
	iff->mtu    = hdev->acl_mtu - HCI_ACL_HDR_SIZE;
	iff->bdaddr = &hdev->bdaddr;

	spin_lock_init(&iff->lock);
	INIT_LIST_HEAD(&iff->conn_list);

	list_add(&iff->list, &l2cap_iff_list);
}

static void l2cap_iff_del(struct hci_dev *hdev)
{
	struct l2cap_iff *iff;

	if (!(iff = hdev->l2cap_data))
		return;

	DBG("%s iff %p", hdev->name, iff);

	list_del(&iff->list);

	l2cap_iff_lock(iff);

	/* Drop connections */
	while (!list_empty(&iff->conn_list)) {
		struct l2cap_conn *c;

		c = list_entry(iff->conn_list.next, struct l2cap_conn, list);
		l2cap_conn_del(c, ENODEV);
	}

	l2cap_iff_unlock(iff);

	/* Unlock HCI device */
	hdev->l2cap_data = NULL;
	hci_dev_put(hdev);

	kfree(iff);
}

/* Get route. Returns L2CAP interface.
 * Must be called with locked rt_lock
 */
static struct l2cap_iff *l2cap_get_route(bdaddr_t *src, bdaddr_t *dst)
{
	struct list_head *p;
	int use_src;

	DBG("%s -> %s", batostr(src), batostr(dst));

	use_src = bacmp(src, BDADDR_ANY) ? 0 : 1;
	
	/* Simple routing: 
	 * 	No source address - find interface with bdaddr != dst 
	 *	Source address 	  - find interface with bdaddr == src 
	 */

	list_for_each(p, &l2cap_iff_list) {
		struct l2cap_iff *iff;

		iff = list_entry(p, struct l2cap_iff, list);

		if (use_src && !bacmp(iff->bdaddr, src))
			return iff;
		else if (bacmp(iff->bdaddr, dst))
			return iff;
	}
	return NULL;
}

/* ----- L2CAP timers ------ */
static void l2cap_sock_timeout(unsigned long arg)
{
	struct sock *sk = (struct sock *) arg;

	DBG("sock %p state %d", sk, sk->state);

	bh_lock_sock(sk);
	switch (sk->state) {
	case BT_DISCONN:
		l2cap_chan_del(sk, ETIMEDOUT);
		break;

	default:
		sk->err = ETIMEDOUT;
		sk->state_change(sk);
		break;
	};
	bh_unlock_sock(sk);

	l2cap_sock_kill(sk);
	sock_put(sk);
}

static void l2cap_sock_set_timer(struct sock *sk, long timeout)
{
	DBG("sock %p state %d timeout %ld", sk, sk->state, timeout);

	if (!mod_timer(&sk->timer, jiffies + timeout))
		sock_hold(sk);
}

static void l2cap_sock_clear_timer(struct sock *sk)
{
	DBG("sock %p state %d", sk, sk->state);

	if (timer_pending(&sk->timer) && del_timer(&sk->timer))
		__sock_put(sk);
}

static void l2cap_sock_init_timer(struct sock *sk)
{
	init_timer(&sk->timer);
	sk->timer.function = l2cap_sock_timeout;
	sk->timer.data = (unsigned long)sk;
}

static void l2cap_conn_timeout(unsigned long arg)
{
	struct l2cap_conn *conn = (void *)arg;
	
	DBG("conn %p state %d", conn, conn->state);

	if (conn->state == BT_CONNECTED) {
		hci_disconnect(conn->hconn, 0x13);
	}
		
	return;
}

static void l2cap_conn_set_timer(struct l2cap_conn *conn, long timeout)
{
	DBG("conn %p state %d timeout %ld", conn, conn->state, timeout);

	mod_timer(&conn->timer, jiffies + timeout);
}

static void l2cap_conn_clear_timer(struct l2cap_conn *conn)
{
	DBG("conn %p state %d", conn, conn->state);

	del_timer(&conn->timer);
}

static void l2cap_conn_init_timer(struct l2cap_conn *conn)
{
	init_timer(&conn->timer);
	conn->timer.function = l2cap_conn_timeout;
	conn->timer.data = (unsigned long)conn;
}

/* -------- L2CAP connections --------- */
/* Add new connection to the interface.
 * Interface must be locked
 */
static struct l2cap_conn *l2cap_conn_add(struct l2cap_iff *iff, bdaddr_t *dst)
{
	struct l2cap_conn *conn;
	bdaddr_t *src = iff->bdaddr;

	if (!(conn = kmalloc(sizeof(struct l2cap_conn), GFP_KERNEL)))
		return NULL;

	memset(conn, 0, sizeof(struct l2cap_conn));

	conn->state = BT_OPEN;
	conn->iff   = iff;
	bacpy(&conn->src, src);
	bacpy(&conn->dst, dst);

	spin_lock_init(&conn->lock);
	conn->chan_list.lock = RW_LOCK_UNLOCKED;

	l2cap_conn_init_timer(conn);
	
	__l2cap_conn_link(iff, conn);

	DBG("%s -> %s, %p", batostr(src), batostr(dst), conn);

	MOD_INC_USE_COUNT;

	return conn;
}

/* Delete connection on the interface.
 * Interface must be locked
 */
static int l2cap_conn_del(struct l2cap_conn *conn, int err)
{
	struct sock *sk;

	DBG("conn %p, state %d, err %d", conn, conn->state, err);

	l2cap_conn_clear_timer(conn);
	__l2cap_conn_unlink(conn->iff, conn);

	conn->state = BT_CLOSED;

	if (conn->rx_skb)
		kfree_skb(conn->rx_skb);

	/* Kill channels */
	while ((sk = conn->chan_list.head)) {
		bh_lock_sock(sk);
		l2cap_sock_clear_timer(sk);
		l2cap_chan_del(sk, err);
		bh_unlock_sock(sk);

		l2cap_sock_kill(sk);
	}

	kfree(conn);

	MOD_DEC_USE_COUNT;
	return 0;
}

static inline struct l2cap_conn *l2cap_get_conn_by_addr(struct l2cap_iff *iff, bdaddr_t *dst)
{
	struct list_head *p;

	list_for_each(p, &iff->conn_list) {
		struct l2cap_conn *c;

		c = list_entry(p, struct l2cap_conn, list);
		if (!bacmp(&c->dst, dst))
			return c;
	}
	return NULL;
}

int l2cap_connect(struct sock *sk)
{
	bdaddr_t *src = &l2cap_pi(sk)->src;
	bdaddr_t *dst = &l2cap_pi(sk)->dst;
	struct l2cap_conn *conn;
	struct l2cap_iff *iff;
	int err = 0;

	DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), l2cap_pi(sk)->psm);

	read_lock_bh(&l2cap_rt_lock);

	/* Get route to remote BD address */
	if (!(iff = l2cap_get_route(src, dst))) {
		err = -EHOSTUNREACH;
		goto done;
	}

	/* Update source addr of the socket */
	bacpy(src, iff->bdaddr);

	l2cap_iff_lock(iff);

	if (!(conn = l2cap_get_conn_by_addr(iff, dst))) {
		/* Connection doesn't exist */
		if (!(conn = l2cap_conn_add(iff, dst))) {
			l2cap_iff_unlock(iff);
			err = -ENOMEM;
			goto done;
		}
		conn->out = 1;
	}

	l2cap_iff_unlock(iff);

	l2cap_chan_add(conn, sk, NULL);

	sk->state = BT_CONNECT;
	l2cap_sock_set_timer(sk, sk->sndtimeo);

	switch (conn->state) {
	case BT_CONNECTED:
		if (sk->type == SOCK_SEQPACKET) {
			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);
		} else {
			l2cap_sock_clear_timer(sk);
			sk->state = BT_CONNECTED;
		}
		break;

	case BT_CONNECT:
		break;

	default:
		/* Create ACL connection */
		conn->state = BT_CONNECT;
		hci_connect(iff->hdev, dst);
		break;
	};

done:
	read_unlock_bh(&l2cap_rt_lock);
	return err;
}

/* ------ Channel queues for listening sockets ------ */
void l2cap_accept_queue(struct sock *parent, struct sock *sk)
{
	struct l2cap_accept_q *q = &l2cap_pi(parent)->accept_q;

	DBG("parent %p, sk %p", parent, sk);

	sock_hold(sk);
	l2cap_pi(sk)->parent = parent;
	l2cap_pi(sk)->next_q = NULL;

	if (!q->head) {
		q->head = q->tail = sk;
	} else {
		struct sock *tail = q->tail;

		l2cap_pi(sk)->prev_q = tail;
		l2cap_pi(tail)->next_q = sk;
		q->tail = sk;
	}

	parent->ack_backlog++;
}

void l2cap_accept_unlink(struct sock *sk)
{
	struct sock *parent = l2cap_pi(sk)->parent;
	struct l2cap_accept_q *q = &l2cap_pi(parent)->accept_q;
	struct sock *next, *prev;

	DBG("sk %p", sk);

	next = l2cap_pi(sk)->next_q;
	prev = l2cap_pi(sk)->prev_q;

	if (sk == q->head)
		q->head = next;
	if (sk == q->tail)
		q->tail = prev;

	if (next)
		l2cap_pi(next)->prev_q = prev;
	if (prev)
		l2cap_pi(prev)->next_q = next;

	l2cap_pi(sk)->parent = NULL;

	parent->ack_backlog--;
	__sock_put(sk);
}

/* Get next connected channel in queue. */
struct sock *l2cap_accept_dequeue(struct sock *parent, int state)
{
	struct l2cap_accept_q *q = &l2cap_pi(parent)->accept_q;
	struct sock *sk;

	for (sk = q->head; sk; sk = l2cap_pi(sk)->next_q) {
		if (!state || sk->state == state) {
			l2cap_accept_unlink(sk);
			break;
		}
	}

	DBG("parent %p, sk %p", parent, sk);

	return sk;
}

/* -------- Socket interface ---------- */
static struct sock *__l2cap_get_sock_by_addr(struct sockaddr_l2 *addr)
{
	bdaddr_t *src = &addr->l2_bdaddr;
	__u16 psm = addr->l2_psm;
	struct sock *sk;

	for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
		if (l2cap_pi(sk)->psm == psm &&
		    !bacmp(&l2cap_pi(sk)->src, src))
			break;
	}

	return sk;
}

/* Find socket listening on psm and source bdaddr.
 * Returns closest match.
 */
static struct sock *l2cap_get_sock_listen(bdaddr_t *src, __u16 psm)
{
	struct sock *sk, *sk1 = NULL;

	read_lock(&l2cap_sk_list.lock);

	for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
		struct l2cap_pinfo *pi;

		if (sk->state != BT_LISTEN)
			continue;

		pi = l2cap_pi(sk);

		if (pi->psm == psm) {
			/* Exact match. */
			if (!bacmp(&pi->src, src))
				break;

			/* Closest match */
			if (!bacmp(&pi->src, BDADDR_ANY))
				sk1 = sk;
		}
	}

	read_unlock(&l2cap_sk_list.lock);

	return sk ? sk : sk1;
}

static void l2cap_sock_destruct(struct sock *sk)
{
	DBG("sk %p", sk);

	skb_queue_purge(&sk->receive_queue);
	skb_queue_purge(&sk->write_queue);

	MOD_DEC_USE_COUNT;
}

static void l2cap_sock_cleanup_listen(struct sock *parent)
{
	struct sock *sk;

	DBG("parent %p", parent);

	/* Close not yet accepted channels */
	while ((sk = l2cap_accept_dequeue(parent, 0)))
		l2cap_sock_close(sk);

	parent->state  = BT_CLOSED;
	parent->zapped = 1;
}

/* Kill socket (only if zapped and orphan)
 * Must be called on unlocked socket.
 */
static void l2cap_sock_kill(struct sock *sk)
{
	if (!sk->zapped || sk->socket)
		return;

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

	/* Kill poor orphan */
	bluez_sock_unlink(&l2cap_sk_list, sk);
	sk->dead = 1;
	sock_put(sk);
}

/* Close socket.
 * Must be called on unlocked socket.
 */
static void l2cap_sock_close(struct sock *sk)
{
	struct l2cap_conn *conn;

	l2cap_sock_clear_timer(sk);

	lock_sock(sk);

	conn = l2cap_pi(sk)->conn;

	DBG("sk %p state %d conn %p socket %p", sk, sk->state, conn, sk->socket);