l2cap_core.c

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

	switch (sk->state) {
	case BT_LISTEN:
		l2cap_sock_cleanup_listen(sk);
		break;

	case BT_CONNECTED:
	case BT_CONFIG:
		if (sk->type == SOCK_SEQPACKET) {
			l2cap_disconn_req req;

			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);
		} else {
			l2cap_chan_del(sk, ECONNRESET);
		}
		break;

	case BT_CONNECT:
	case BT_DISCONN:
		l2cap_chan_del(sk, ECONNRESET);
		break;

	default:
		sk->zapped = 1;
		break;
	};

	release_sock(sk);

	l2cap_sock_kill(sk);
}

static void l2cap_sock_init(struct sock *sk, struct sock *parent)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);

	DBG("sk %p", sk);

	if (parent) {
		sk->type = parent->type;

		pi->imtu = l2cap_pi(parent)->imtu;
		pi->omtu = l2cap_pi(parent)->omtu;
	} else {
		pi->imtu = L2CAP_DEFAULT_MTU;
		pi->omtu = 0;
	}

	/* Default config options */
	pi->conf_mtu = L2CAP_DEFAULT_MTU;
	pi->flush_to = L2CAP_DEFAULT_FLUSH_TO;
}

static struct sock *l2cap_sock_alloc(struct socket *sock, int proto, int prio)
{
	struct sock *sk;

	if (!(sk = sk_alloc(PF_BLUETOOTH, prio, 1)))
		return NULL;

	sock_init_data(sock, sk);

	sk->zapped   = 0;

	sk->destruct = l2cap_sock_destruct;
	sk->sndtimeo = L2CAP_CONN_TIMEOUT;

	sk->protocol = proto;
	sk->state    = BT_OPEN;

	l2cap_sock_init_timer(sk);

	bluez_sock_link(&l2cap_sk_list, sk);

	MOD_INC_USE_COUNT;

	return sk;
}

static int l2cap_sock_create(struct socket *sock, int protocol)
{
	struct sock *sk;

	DBG("sock %p", sock);

	sock->state = SS_UNCONNECTED;

	if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_RAW)
		return -ESOCKTNOSUPPORT;

	sock->ops = &l2cap_sock_ops;

	if (!(sk = l2cap_sock_alloc(sock, protocol, GFP_KERNEL)))
		return -ENOMEM;

	l2cap_sock_init(sk, NULL);

	return 0;
}

static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
{
	struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
	struct sock *sk = sock->sk;
	int err = 0;

	DBG("sk %p, %s %d", sk, batostr(&la->l2_bdaddr), la->l2_psm);

	if (!addr || addr->sa_family != AF_BLUETOOTH)
		return -EINVAL;

	lock_sock(sk);

	if (sk->state != BT_OPEN) {
		err = -EBADFD;
		goto done;
	}

	write_lock(&l2cap_sk_list.lock);

	if (la->l2_psm && __l2cap_get_sock_by_addr(la)) {
		err = -EADDRINUSE;
		goto unlock;
	}

	/* Save source address */
	bacpy(&l2cap_pi(sk)->src, &la->l2_bdaddr);
	l2cap_pi(sk)->psm = la->l2_psm;
	sk->state = BT_BOUND;

unlock:
	write_unlock(&l2cap_sk_list.lock);

done:
	release_sock(sk);

	return err;
}

static int l2cap_sock_w4_connect(struct sock *sk, int flags)
{
	DECLARE_WAITQUEUE(wait, current);
	long timeo = sock_sndtimeo(sk, flags & O_NONBLOCK);
	int err = 0;

	DBG("sk %p", sk);

	add_wait_queue(sk->sleep, &wait);
	current->state = TASK_INTERRUPTIBLE;

	while (sk->state != BT_CONNECTED) {
		if (!timeo) {
			err = -EAGAIN;
			break;
		}

		release_sock(sk);
		timeo = schedule_timeout(timeo);
		lock_sock(sk);

		err = 0;
		if (sk->state == BT_CONNECTED)
			break;

		if (sk->err) {
			err = sock_error(sk);
			break;
		}

		if (signal_pending(current)) {
			err = sock_intr_errno(timeo);
			break;
		}
	}
	current->state = TASK_RUNNING;
	remove_wait_queue(sk->sleep, &wait);

	return err;
}

static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
{
	struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
	struct sock *sk = sock->sk;
	int err = 0;

	lock_sock(sk);

	DBG("sk %p", sk);

	if (addr->sa_family != AF_BLUETOOTH || alen < sizeof(struct sockaddr_l2)) {
		err = -EINVAL;
		goto done;
	}

	if (sk->state != BT_OPEN && sk->state != BT_BOUND) {
		err = -EBADFD;
		goto done;
	}

	if (sk->type == SOCK_SEQPACKET && !la->l2_psm) {
		err = -EINVAL;
		goto done;
	}

	/* Set destination address and psm */
	bacpy(&l2cap_pi(sk)->dst, &la->l2_bdaddr);
	l2cap_pi(sk)->psm = la->l2_psm;

	if ((err = l2cap_connect(sk)))
		goto done;

	err = l2cap_sock_w4_connect(sk, flags);

done:
	release_sock(sk);
	return err;
}

int l2cap_sock_listen(struct socket *sock, int backlog)
{
	struct sock *sk = sock->sk;
	int err = 0;

	DBG("sk %p backlog %d", sk, backlog);

	lock_sock(sk);

	if (sk->state != BT_BOUND || sock->type != SOCK_SEQPACKET) {
		err = -EBADFD;
		goto done;
	}

	if (!l2cap_pi(sk)->psm) {
		err = -EINVAL;
		goto done;
	}

	sk->max_ack_backlog = backlog;
	sk->ack_backlog = 0;
	sk->state = BT_LISTEN;

done:
	release_sock(sk);
	return err;
}

int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags)
{
	DECLARE_WAITQUEUE(wait, current);
	struct sock *sk = sock->sk, *ch;
	long timeo;
	int err = 0;

	lock_sock(sk);

	if (sk->state != BT_LISTEN) {
		err = -EBADFD;
		goto done;
	}

	timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);

	DBG("sk %p timeo %ld", sk, timeo);

	/* Wait for an incoming connection. (wake-one). */
	add_wait_queue_exclusive(sk->sleep, &wait);
	current->state = TASK_INTERRUPTIBLE;
	while (!(ch = l2cap_accept_dequeue(sk, BT_CONNECTED))) {
		if (!timeo) {
			err = -EAGAIN;
			break;
		}

		release_sock(sk);
		timeo = schedule_timeout(timeo);
		lock_sock(sk);

		if (sk->state != BT_LISTEN) {
			err = -EBADFD;
			break;
		}

		if (signal_pending(current)) {
			err = sock_intr_errno(timeo);
			break;
		}
	}
	current->state = TASK_RUNNING;
	remove_wait_queue(sk->sleep, &wait);

	if (err)
		goto done;

	sock_graft(ch, newsock);
	newsock->state = SS_CONNECTED;

	DBG("new socket %p", ch);

done:
	release_sock(sk);

	return err;
}

static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
{
	struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
	struct sock *sk = sock->sk;

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

	addr->sa_family = AF_BLUETOOTH;
	*len = sizeof(struct sockaddr_l2);

	if (peer)
		bacpy(&la->l2_bdaddr, &l2cap_pi(sk)->dst);
	else
		bacpy(&la->l2_bdaddr, &l2cap_pi(sk)->src);

	la->l2_psm = l2cap_pi(sk)->psm;

	return 0;
}

static int l2cap_sock_sendmsg(struct socket *sock, struct msghdr *msg, int len, struct scm_cookie *scm)
{
	struct sock *sk = sock->sk;
	int err = 0;

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

	if (sk->err)
		return sock_error(sk);

	if (msg->msg_flags & MSG_OOB)
		return -EOPNOTSUPP;

	lock_sock(sk);

	if (sk->state == BT_CONNECTED)
		err = l2cap_chan_send(sk, msg, len);
	else
		err = -ENOTCONN;

	release_sock(sk);
	return err;
}

static int l2cap_sock_recvmsg(struct socket *sock, struct msghdr *msg, int len, int flags, struct scm_cookie *scm)
{
	struct sock *sk = sock->sk;
	int noblock = flags & MSG_DONTWAIT;
	int copied, err;
	struct sk_buff *skb;

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

	if (flags & (MSG_OOB))
		return -EOPNOTSUPP;

	if (sk->state == BT_CLOSED)
		return 0;

	if (!(skb = skb_recv_datagram(sk, flags, noblock, &err)))
		return err;

	msg->msg_namelen = 0;

	copied = skb->len;
	if (len < copied) {
		msg->msg_flags |= MSG_TRUNC;
		copied = len;
	}

	skb->h.raw = skb->data;
	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);

	skb_free_datagram(sk, skb);

	return err ? : copied;
}

int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char *optval, int optlen)
{
	struct sock *sk = sock->sk;
	struct l2cap_options opts;
	int err = 0;

	DBG("sk %p", sk);

	lock_sock(sk);

	switch (optname) {
	case L2CAP_OPTIONS:
		if (copy_from_user((char *)&opts, optval, optlen)) {
			err = -EFAULT;
			break;
		}
		l2cap_pi(sk)->imtu = opts.imtu;
		l2cap_pi(sk)->omtu = opts.omtu;
		break;

	default:
		err = -ENOPROTOOPT;
		break;
	};

	release_sock(sk);
	return err;
}

int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char *optval, int *optlen)
{
	struct sock *sk = sock->sk;
	struct l2cap_options opts;
	struct l2cap_conninfo cinfo;
	int len, err = 0; 

	if (get_user(len, optlen))
		return -EFAULT;

	lock_sock(sk);

	switch (optname) {
	case L2CAP_OPTIONS:
		opts.imtu     = l2cap_pi(sk)->imtu;
		opts.omtu     = l2cap_pi(sk)->omtu;
		opts.flush_to = l2cap_pi(sk)->flush_to;

		len = MIN(len, sizeof(opts));
		if (copy_to_user(optval, (char *)&opts, len))
			err = -EFAULT;

		break;

	case L2CAP_CONNINFO:
		if (sk->state != BT_CONNECTED) {
			err = -ENOTCONN;
			break;
		}

		cinfo.hci_handle = l2cap_pi(sk)->conn->hconn->handle;

		len = MIN(len, sizeof(cinfo));
		if (copy_to_user(optval, (char *)&cinfo, len))
			err = -EFAULT;

		break;

	default:
		err = -ENOPROTOOPT;
		break;
	};

	release_sock(sk);
	return err;
}

static unsigned int l2cap_sock_poll(struct file * file, struct socket *sock, poll_table *wait)
{
	struct sock *sk = sock->sk;
	struct l2cap_accept_q *aq;
	unsigned int mask;

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

	poll_wait(file, sk->sleep, wait);
	mask = 0;

	if (sk->err || !skb_queue_empty(&sk->error_queue))
		mask |= POLLERR;

	if (sk->shutdown == SHUTDOWN_MASK)
		mask |= POLLHUP;

	aq = &l2cap_pi(sk)->accept_q;
	if (!skb_queue_empty(&sk->receive_queue) || aq->head || (sk->shutdown & RCV_SHUTDOWN))
		mask |= POLLIN | POLLRDNORM;

	if (sk->state == BT_CLOSED)
		mask |= POLLHUP;

	if (sock_writeable(sk))
		mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
	else
		set_bit(SOCK_ASYNC_NOSPACE, &sk->socket->flags);

	return mask;
}

static int l2cap_sock_release(struct socket *sock)
{
	struct sock *sk = sock->sk;

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

	if (!sk)
		return 0;

	sock_orphan(sk);

	l2cap_sock_close(sk);

	return 0;
}

/* --------- L2CAP channels --------- */
static struct sock * __l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, __u16 cid)
{
	struct sock *s;

	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
		if (l2cap_pi(s)->dcid == cid)
			break;
	}

	return s;
}

static inline struct sock *l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, __u16 cid)
{
	struct sock *s;

	read_lock(&l->lock);
	s = __l2cap_get_chan_by_dcid(l, cid);
	read_unlock(&l->lock);

	return s;
}

static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, __u16 cid)
{
	struct sock *s;

	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
		if (l2cap_pi(s)->scid == cid)
			break;
	}

	return s;
}
static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, __u16 cid)
{
	struct sock *s;

	read_lock(&l->lock);
	s = __l2cap_get_chan_by_scid(l, cid);
	read_unlock(&l->lock);

	return s;
}

static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, __u8 ident)
{
	struct sock *s;

	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
		if (l2cap_pi(s)->ident == ident)
			break;
	}

	return s;
}

static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, __u8 ident)
{
	struct sock *s;

	read_lock(&l->lock);
	s = __l2cap_get_chan_by_ident(l, ident);
	read_unlock(&l->lock);

	return s;
}