bnep_core.c

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

	if (request->bnep_conn) {
		struct sk_buff *skb;
		__u16 list_size;
		__u8 cmd_type;

		//Eval request dep values for multicast/protocol packet
		switch (filter_request->filter_type) {
		case FILTER_TYPE_PROTOCOL:
			cmd_type = BNEP_FILTER_NET_TYPE_SET_MSG;
			break;
		case FILTER_TYPE_MULTICAST:
			cmd_type = BNEP_FILTER_MULT_ADDR_SET_MSG;
			break;
		default:
			BT_DBG("filter request with unknown type");
			return;
		}

		if (!(skb = alloc_skb(2 + 2 + filter_request->data_length, GFP_ATOMIC))) {
			BT_ERR("can't alloc space for filter packet");
			return;
		}
		//Reserve headspace for control packet
		skb_reserve(skb, 2);

		//List size = Number of bytes in request
		list_size = cpu_to_bnep16(filter_request->data_length);
		memcpy(skb_put(skb, 2), &list_size, 2);

		//Copy request values
		if (filter_request->data_length > 0) {
			memcpy(skb_put(skb, filter_request->data_length), filter_request->data,
			       filter_request->data_length);
		}
		//complete and send control package
		bnep_tx_controldata(request->bnep_conn, cmd_type, 0, skb);
	}
}

/***************************************************************************/
int _bnep_tx_filter(struct bnep_connection *bnep_conn, struct filter_request_struct *filter_request)
{
	int resend_counter = 0;
	int status = 0;

	if (!bnep_conn) {
		BT_DBG("not l2cap connected");
		return BNEP_ERR_NO_L2CAP;
	}

	if (bnep_conn->filter_request.req_status != BNEP_REQ_DONE) {
		BT_DBG("pending filter request found on this connection");
		return BNEP_ERR_BNEP_FILTER_REQ_PENDING;
	}

	do {
		BT_DBG("issue filter request #%d", resend_counter);
		bnep_conn->filter_request.req_status = BNEP_REQ_PENDING;
		bnep_conn->filter_request.bnep_conn = bnep_conn;
		bnep_request(&bnep_conn->filter_request, bnep_filter_req,
			     (unsigned long) filter_request, BNEP_FILTER_TIMEOUT * HZ);
		resend_counter++;
	} while (bnep_conn->filter_request.req_status != BNEP_REQ_DONE && resend_counter < 3);

	if (bnep_conn->filter_request.req_status != BNEP_REQ_DONE)
		status = BNEP_ERR_FILTER_TIMEOUT;
	else {
		if (bnep_conn->filter_request.req_result != 0x0)
			status = BNEP_ERR_FILTER_ERROR;
	}

	bnep_conn->filter_request.req_status = BNEP_REQ_DONE;

	return status;

}

/***************************************************************************/
int bnep_tx_filter(struct net_device *net_dev, struct filter_request_struct *filter_request)
{
	struct bnep_connection *bnep_conn =
	    bnep_get_conn_by_bdaddr(net_dev, &filter_request->destination_address);
	return _bnep_tx_filter(bnep_conn, filter_request);
}

/***************************************************************************/
static void bnep_start_session(struct socket *sock, struct net_device *net_dev)
{
	struct bnep_connection *bnep_conn = NULL;
	struct bnep_private *priv = bnep_get_priv(net_dev);

	BT_DBG("L2CAP connection for BNEP open");

	if ((bnep_conn =
	     (struct bnep_connection *) kmalloc(sizeof(struct bnep_connection), GFP_KERNEL))) {
		BT_DBG("adding connection list element");
		bnep_conn -> status = BNEP_CONN_CLOSED;
		bnep_conn->sk = sock;
		bnep_conn->net_dev = net_dev;

		//Init filter req data
		bnep_conn->filter_info.NetFilter = NULL;
		bnep_conn->filter_info.MulticastFilter = NULL;

		//Init connect req struct
		init_waitqueue_head(&(bnep_conn->connect_request.req_wait_q));
		bnep_conn->connect_request.req_status = BNEP_REQ_DONE;
		init_MUTEX(&(bnep_conn->connect_request.req_lock));

		//Init filter req struct        
		init_waitqueue_head(&(bnep_conn->filter_request.req_wait_q));
		bnep_conn->filter_request.req_status = BNEP_REQ_DONE;
		init_MUTEX(&(bnep_conn->filter_request.req_lock));

		//Register new connection
		list_add(&bnep_conn->list, &priv->bnep_connections);
		priv->connection_counter++;
		MOD_INC_USE_COUNT;

		//Prepare and start session receive thread
		bnep_conn->session.arg = (void *) bnep_conn;
		start_kthread(bnep_session, &bnep_conn->session);
	} else {
		BT_ERR("can't allocate mem for new connection");
	}
}

/***************************************************************************/
static void bnep_tx_frame(struct socket *sk, struct sk_buff *skb)
{
	mm_segment_t old_fs;
	struct msghdr msg;
	struct iovec iv;
	int status;

	iv.iov_base = (char *) skb->data;
	iv.iov_len = (size_t) skb->len;
	memset(&msg, 0, sizeof(msg));
	msg.msg_iov = &iv;
	msg.msg_iovlen = 1;
	msg.msg_flags = MSG_NOSIGNAL;

	old_fs = get_fs();
	set_fs(KERNEL_DS);
	status = sock_sendmsg(sk, &msg, (size_t) skb->len);
	BT_DBG("%d bytes send", status);
	set_fs(old_fs);

	dev_kfree_skb(skb);
}

/***************************************************************************/
static void bnep_session(kthread_t * kthread)
{
	struct list_head *ptr;
	struct bnep_connection *entry = (struct bnep_connection *) kthread->arg;
	struct socket *session_sk = entry->sk;
	struct bnep_private *priv = bnep_get_priv(entry->net_dev);
	wait_queue_t wait;

	BT_DBG("start session thread");

	init_kthread(kthread, "bnep_rcv");

	init_waitqueue_entry(&wait, current);
	add_wait_queue(session_sk->sk->sleep, &wait);
	while (1) {
		struct sk_buff *skb;

		set_current_state(TASK_INTERRUPTIBLE);

		//Close connection
		if (session_sk->sk->state == BT_CLOSED || kthread->terminate) {
			break;
		}

		//Get all data from L2CAP
		while ((skb = skb_dequeue(&session_sk->sk->receive_queue))) {
			//Send skb to bnep
			skb_orphan(skb);
			bnep_rx_frame(entry, skb);
		}

		//Send all data to L2CAP
		while ((skb = skb_dequeue(&session_sk->sk->write_queue))) {
			bnep_tx_frame(session_sk, skb);
		}

		schedule();
	}
	remove_wait_queue(session_sk->sk->sleep, &wait);

	for (ptr = priv->bnep_connections.next; ptr != &priv->bnep_connections; ptr = ptr->next) {
		entry = list_entry(ptr, struct bnep_connection, list);
		if (entry->sk == session_sk) {
			BT_DBG("deleting connection list entry");
			--(priv->connection_counter);
			MOD_DEC_USE_COUNT;
			list_del(&entry->list);
			if (entry->filter_info.NetFilter)
				kfree(entry->filter_info.NetFilter);
			if (entry->filter_info.MulticastFilter)
				kfree(entry->filter_info.MulticastFilter);
			break;
		}
	}

	BT_DBG("releasing socket");
	sock_release(session_sk);

	//Leave thread
	exit_kthread(kthread);
}

/***************************************************************************/
static void bnep_connect_req(struct request_struct *request, unsigned long opt)
{
	struct bnep_connect_mgnt_struct *bnep_connect_mgnt =
	    (struct bnep_connect_mgnt_struct *) opt;
	int role_uuid = bnep_get_priv(request->bnep_conn->net_dev)->role_uuid;

	if (request->bnep_conn) {
		//unsigned long data = 0;
		int packet_size;
		struct sk_buff *skb;

		//Connect parameter
		__u16 source_uuid = cpu_to_bnep16(role_uuid);
		__u16 dest_uuid = cpu_to_bnep16(bnep_connect_mgnt->destination_uuid);
		__u8 uuid_length = UUID16;

		//Prepare skb
		packet_size = 2 + 1 + 2 * uuid_length;
		if (!(skb = alloc_skb(packet_size, GFP_ATOMIC))) {
			request->req_result = -ENOMEM;
			return;
		}
		skb_reserve(skb, packet_size);

		//Insert connection request data
		memcpy(skb_push(skb, uuid_length), &source_uuid, uuid_length);
		memcpy(skb_push(skb, uuid_length), &dest_uuid, uuid_length);
		memcpy(skb_push(skb, 1), &uuid_length, 1);

		//complete and send control package
		bnep_tx_controldata(request->bnep_conn, BNEP_SETUP_CONNECTION_REQUEST_MSG, 0, skb);
	}
	return;
}

/***************************************************************************/
static int bnep_l2cap_connect(struct net_device *net_dev,
			      struct bnep_connect_mgnt_struct *bnep_connect_mgnt)
{
	struct l2cap_options opts;
	struct sockaddr_l2 addr;
	struct socket *nsk;
	int err, size;
	struct bnep_private *bnep_priv = bnep_get_priv(net_dev);

	if (bnep_get_conn_by_bdaddr(net_dev, &bnep_connect_mgnt->destination_address)) {
		BT_DBG("L2CAP connection on PAN PSM to addr %s already exists",
				batostr(&bnep_connect_mgnt->destination_address));
		return 0;
	}

	err = sock_create(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP, &nsk);
	if (nsk) {
		BT_DBG("new socket accepted %p", nsk);
	} else {
		BT_DBG("new socket not accepted");
		return BNEP_ERR_SOCK_ACCEPT;
	}

	memset(&addr, 0, sizeof(struct sockaddr_l2));
	addr.l2_family = AF_BLUETOOTH;
	baswap(&addr.l2_bdaddr, (bdaddr_t *) & bnep_priv->source_address);
	err = nsk->ops->bind(nsk, (struct sockaddr *) &addr, sizeof(addr));
	if (err < 0) {
		BT_DBG("bind failed %d", err);
		return BNEP_ERR_SOCK_BIND;
	}
	//Set L2CAP options 
	size = sizeof(opts);
	nsk->ops->getsockopt(nsk, SOL_L2CAP, L2CAP_OPTIONS, (void *) &opts, &size);
	opts.imtu = PAN_IMTU;
	opts.omtu = PAN_OMTU;
	opts.flush_to = PAN_FLUSH_TO;

	set_fs(KERNEL_DS);
	nsk->ops->setsockopt(nsk, SOL_L2CAP, L2CAP_OPTIONS, (void *) &opts, size);

	addr.l2_psm = htobs(PAN_PSM);
	baswap(&addr.l2_bdaddr, &bnep_connect_mgnt->destination_address);
	if (!nsk->ops->connect(nsk, (struct sockaddr *) &addr, sizeof(struct sockaddr), 0)) {
		BT_DBG("Connection ready");
		bnep_start_session(nsk, net_dev);
	} else {
		BT_DBG("Can't connect %s -> %s", batostr(&bnep_priv->source_address),
				batostr(&bnep_connect_mgnt->destination_address));
		sock_release(nsk);
		return BNEP_ERR_CONNECT_L2CAP;
	}

	return 0;
}

/***************************************************************************/
static int bnep_connect(struct net_device *net_dev,
			struct bnep_connect_mgnt_struct *bnep_connect_mgnt)
{
	struct bnep_connection *bnep_conn =
	    bnep_get_conn_by_bdaddr(net_dev, &bnep_connect_mgnt->destination_address);
	int status = 0;
	int resend_counter = 0;
	int role_uuid = bnep_get_priv(net_dev)->role_uuid;

	if (role_uuid != UUID_PANU && role_uuid != UUID_GN && role_uuid != UUID_NAP)
		return BNEP_ERR_CONN_UNKNOWN_DEF_ROLE;

	if (!bnep_conn) {
		BT_DBG("l2cap connection not ready => start connection");
		if (bnep_l2cap_connect(net_dev, bnep_connect_mgnt) != 0 ||
			!(bnep_conn = bnep_get_conn_by_bdaddr(net_dev,
						&bnep_connect_mgnt->destination_address))) {
			BT_DBG("could not establish l2cap connection");
			return BNEP_ERR_NO_L2CAP;
		}
		BT_DBG("l2cap connection now ready");
	}

	if (bnep_conn->connect_request.req_status != BNEP_REQ_DONE) {
		BT_DBG("pending bnep connection request found on this connection");
		return BNEP_ERR_BNEP_CONN_REQ_PENDING;
	}

	do {
		BT_DBG("issue bnep connect request #%d", resend_counter);
		bnep_conn->connect_request.req_status = BNEP_REQ_PENDING;
		bnep_conn->connect_request.bnep_conn = bnep_conn;
		bnep_request(&bnep_conn->connect_request, bnep_connect_req,
			     (unsigned long) bnep_connect_mgnt, BNEP_CONN_TIMEOUT * HZ);
		resend_counter++;
	} while (bnep_conn->connect_request.req_status != BNEP_REQ_DONE && resend_counter < 3);

	if (bnep_conn->connect_request.req_status != BNEP_REQ_DONE)
		status = BNEP_ERR_CONNECT_BNEP_TIMEOUT;
	else {
		if (bnep_conn->connect_request.req_result != CONN_OPERATION_SUCCESSFUL)
			status = BNEP_ERR_CONNECT_BNEP_ERROR;
		else
			bnep_conn->destination_uuid = bnep_connect_mgnt->destination_uuid;
	}

	bnep_conn->connect_request.req_status = BNEP_REQ_DONE;

	net_dev->set_multicast_list(net_dev);

	return status;
}

/***************************************************************************/
static int _bnep_request(struct request_struct *request,
	 void (*req) (struct request_struct *, unsigned long), unsigned long opt, __u32 timeout)
{
	DECLARE_WAITQUEUE(wait, current);
	int err = 0;

	add_wait_queue(&request->req_wait_q, &wait);
	current->state = TASK_INTERRUPTIBLE;

	req(request, opt);
	schedule_timeout(timeout);

	current->state = TASK_RUNNING;
	remove_wait_queue(&request->req_wait_q, &wait);

	if (signal_pending(current))
		return -EINTR;

	switch (request->req_status) {
	case BNEP_REQ_DONE:
		err = 0;
		break;
	case BNEP_REQ_CANCELED:
		err = -request->req_result;
		break;

	default:
		err = -ETIMEDOUT;
		break;
	};

	BT_DBG("end: err %d", err);

	return err;
}

/***************************************************************************/
static inline int bnep_request(struct request_struct *request,
				void (*req) (struct request_struct *, unsigned long),
				unsigned long opt, __u32 timeout)
{
	int ret;
	//Serialize all requests
	down(&request->req_lock);
	init_waitqueue_head(&request->req_wait_q);
	ret = _bnep_request(request, req, opt, timeout);
	up(&request->req_lock);
	return ret;
}

/***************************************************************************/
static int bnep_update_role(struct net_device *dev, int new_role)
{
	struct list_head *ptr;
	struct bnep_connection *entry;
	struct bnep_connect_mgnt_struct bnep_connect_mgnt;
	bdaddr_t tmp_addr;
	struct bnep_private *priv = bnep_get_priv(dev);

	if (new_role != UUID_PANU && new_role != UUID_GN && new_role != UUID_NAP)
		return BNEP_ERR_CONN_UNKNOWN_DEF_ROLE;

	priv->role_uuid = new_role;
	BT_DBG("set role to 0x%x", new_role);

	//Send updated BNEP connect message to all connected nodes
	for (ptr = priv->bnep_connections.next; ptr != &priv->bnep_connections; ptr = ptr->next) {
		entry = list_entry(ptr, struct bnep_connection, list);
		baswap(&tmp_addr, &bluez_pi(entry->sk->sk)->dst);
		//prepare updated bnep connect request
		bacpy(&bnep_connect_mgnt.destination_address, &tmp_addr);
		bnep_connect_mgnt.protocol = CONN_BNEP_LAYER;
		bnep_connect_mgnt.destination_uuid = entry->destination_uuid;
		//send updated connect request
		bnep_connect(dev, &bnep_connect_mgnt);
	}