core.c

linux-2.6.15.6

	/* Decompress header and construct ether frame */
	switch (type & BNEP_TYPE_MASK) {
	case BNEP_COMPRESSED:
		memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
		break;
	
	case BNEP_COMPRESSED_SRC_ONLY:
		memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
		memcpy(__skb_put(nskb, ETH_ALEN), skb->mac.raw, ETH_ALEN);
		put_unaligned(s->eh.h_proto, (u16 *) __skb_put(nskb, 2));
		break;

	case BNEP_COMPRESSED_DST_ONLY:
		memcpy(__skb_put(nskb, ETH_ALEN), skb->mac.raw, ETH_ALEN);
		memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source, ETH_ALEN + 2);
		break;

	case BNEP_GENERAL:
		memcpy(__skb_put(nskb, ETH_ALEN * 2), skb->mac.raw, ETH_ALEN * 2);
		put_unaligned(s->eh.h_proto, (u16 *) __skb_put(nskb, 2));
		break;
	}

	memcpy(__skb_put(nskb, skb->len), skb->data, skb->len);
	kfree_skb(skb);
	
	s->stats.rx_packets++;
	nskb->dev       = dev;
	nskb->ip_summed = CHECKSUM_NONE;
	nskb->protocol  = eth_type_trans(nskb, dev);
	netif_rx_ni(nskb);
	return 0;

badframe:
	s->stats.rx_errors++;
	kfree_skb(skb);
	return 0;
}

static u8 __bnep_tx_types[] = {
	BNEP_GENERAL,
	BNEP_COMPRESSED_SRC_ONLY,
	BNEP_COMPRESSED_DST_ONLY,
	BNEP_COMPRESSED
};

static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
{
	struct ethhdr *eh = (void *) skb->data;
	struct socket *sock = s->sock;
	struct kvec iv[3];
	int len = 0, il = 0;
	u8 type = 0;

	BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);

	if (!skb->dev) {
		/* Control frame sent by us */
		goto send;
	}

	iv[il++] = (struct kvec) { &type, 1 };
	len++;

	if (!memcmp(eh->h_dest, s->eh.h_source, ETH_ALEN))
		type |= 0x01;

	if (!memcmp(eh->h_source, s->eh.h_dest, ETH_ALEN))
		type |= 0x02;

	if (type)
		skb_pull(skb, ETH_ALEN * 2);

	type = __bnep_tx_types[type];
	switch (type) {
	case BNEP_COMPRESSED_SRC_ONLY:
		iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
		len += ETH_ALEN;
		break;
		
	case BNEP_COMPRESSED_DST_ONLY:
		iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
		len += ETH_ALEN;
		break;
	}

send:
	iv[il++] = (struct kvec) { skb->data, skb->len };
	len += skb->len;
	
	/* FIXME: linearize skb */
	{
		len = kernel_sendmsg(sock, &s->msg, iv, il, len);
	}
	kfree_skb(skb);

	if (len > 0) {
		s->stats.tx_bytes += len;
		s->stats.tx_packets++;
		return 0;
	}

	return len;
}

static int bnep_session(void *arg)
{
	struct bnep_session *s = arg;
	struct net_device *dev = s->dev;
	struct sock *sk = s->sock->sk;
	struct sk_buff *skb;
	wait_queue_t wait;

	BT_DBG("");

        daemonize("kbnepd %s", dev->name);
	set_user_nice(current, -15);
	current->flags |= PF_NOFREEZE;

	init_waitqueue_entry(&wait, current);
	add_wait_queue(sk->sk_sleep, &wait);
	while (!atomic_read(&s->killed)) {
		set_current_state(TASK_INTERRUPTIBLE);

		// RX
		while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
			skb_orphan(skb);
			bnep_rx_frame(s, skb);
		}

		if (sk->sk_state != BT_CONNECTED)
			break;
	
		// TX
		while ((skb = skb_dequeue(&sk->sk_write_queue)))
			if (bnep_tx_frame(s, skb))
				break;
		netif_wake_queue(dev);
	
		schedule();
	}
	set_current_state(TASK_RUNNING);
	remove_wait_queue(sk->sk_sleep, &wait);

	/* Cleanup session */
	down_write(&bnep_session_sem);

	/* Delete network device */
	unregister_netdev(dev);

	/* Release the socket */
	fput(s->sock->file);

	__bnep_unlink_session(s);

	up_write(&bnep_session_sem);
	free_netdev(dev);
	return 0;
}

int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
{
	struct net_device *dev;
	struct bnep_session *s, *ss;
	u8 dst[ETH_ALEN], src[ETH_ALEN];
	int err;

	BT_DBG("");

	baswap((void *) dst, &bt_sk(sock->sk)->dst);
	baswap((void *) src, &bt_sk(sock->sk)->src);

	/* session struct allocated as private part of net_device */
	dev = alloc_netdev(sizeof(struct bnep_session),
			   (*req->device) ? req->device : "bnep%d",
			   bnep_net_setup);
	if (!dev) 
		return ENOMEM;


	down_write(&bnep_session_sem);

	ss = __bnep_get_session(dst);
	if (ss && ss->state == BT_CONNECTED) {
		err = -EEXIST;
		goto failed;
	}

	s = dev->priv;

	/* This is rx header therefore addresses are swapped.
	 * ie eh.h_dest is our local address. */
	memcpy(s->eh.h_dest,   &src, ETH_ALEN);
	memcpy(s->eh.h_source, &dst, ETH_ALEN);
	memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);

	s->dev = dev;
	s->sock  = sock;
	s->role  = req->role;
	s->state = BT_CONNECTED;
	
	s->msg.msg_flags = MSG_NOSIGNAL;

#ifdef CONFIG_BT_BNEP_MC_FILTER
	/* Set default mc filter */
	set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
#endif

#ifdef CONFIG_BT_BNEP_PROTO_FILTER
	/* Set default protocol filter */
	bnep_set_default_proto_filter(s);
#endif

	err = register_netdev(dev);
	if (err) {
		goto failed;
	}

	__bnep_link_session(s);
	
	err = kernel_thread(bnep_session, s, CLONE_KERNEL);
	if (err < 0) {
		/* Session thread start failed, gotta cleanup. */
		unregister_netdev(dev);
		__bnep_unlink_session(s);
		goto failed;
	}

	up_write(&bnep_session_sem);
	strcpy(req->device, dev->name);
	return 0;

failed:
	up_write(&bnep_session_sem);
	free_netdev(dev);
	return err;
}

int bnep_del_connection(struct bnep_conndel_req *req)
{
	struct bnep_session *s;
	int  err = 0;

	BT_DBG("");

	down_read(&bnep_session_sem);

	s = __bnep_get_session(req->dst);
	if (s) {
		/* Wakeup user-space which is polling for socket errors.
		 * This is temporary hack untill we have shutdown in L2CAP */
		s->sock->sk->sk_err = EUNATCH;
		
		/* Kill session thread */
		atomic_inc(&s->killed);
		wake_up_interruptible(s->sock->sk->sk_sleep);
	} else
		err = -ENOENT;

	up_read(&bnep_session_sem);
	return err;
}

static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
{
	memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
	strcpy(ci->device, s->dev->name);
	ci->flags = s->flags;
	ci->state = s->state;
	ci->role  = s->role;
}

int bnep_get_connlist(struct bnep_connlist_req *req)
{
	struct list_head *p;
	int err = 0, n = 0;

	down_read(&bnep_session_sem);

	list_for_each(p, &bnep_session_list) {
		struct bnep_session *s;
		struct bnep_conninfo ci;

		s = list_entry(p, struct bnep_session, list);

		__bnep_copy_ci(&ci, s);
		
		if (copy_to_user(req->ci, &ci, sizeof(ci))) {
			err = -EFAULT;
			break;
		}

		if (++n >= req->cnum)
			break;

		req->ci++;
	}
	req->cnum = n;

	up_read(&bnep_session_sem);
	return err;
}

int bnep_get_conninfo(struct bnep_conninfo *ci)
{
	struct bnep_session *s;
	int err = 0;

	down_read(&bnep_session_sem);

	s = __bnep_get_session(ci->dst);
	if (s)
		__bnep_copy_ci(ci, s);
	else
		err = -ENOENT;

	up_read(&bnep_session_sem);
	return err;
}

static int __init bnep_init(void)
{	
	char flt[50] = "";

	l2cap_load();

#ifdef CONFIG_BT_BNEP_PROTO_FILTER
	strcat(flt, "protocol ");
#endif

#ifdef CONFIG_BT_BNEP_MC_FILTER
	strcat(flt, "multicast");
#endif

	BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
	if (flt[0])
		BT_INFO("BNEP filters: %s", flt);

	bnep_sock_init();
	return 0;
}

static void __exit bnep_exit(void)
{
	bnep_sock_cleanup();
}

module_init(bnep_init);
module_exit(bnep_exit);

MODULE_AUTHOR("David Libault <[e-mail]>, Maxim Krasnyansky <[e-mail]>");
MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
MODULE_VERSION(VERSION);
MODULE_LICENSE("GPL");
MODULE_ALIAS("bt-proto-4");