bnep_core.c

linux下官方协议栈bluez关于pan网络

	} 
#endif

	//Send Response packet
	if (!(skb = alloc_skb(4, GFP_ATOMIC))) return -ENOMEM;
	skb_reserve(skb, 2);

	//Insert connection response data
	BT_DBG("Send connection response packet");
	memcpy(skb_put(skb, 2), &ResponseMessage, 2);
	bnep_tx_controldata(bnep_conn, BNEP_SETUP_CONNECTION_RESPONSE_MSG, 0, skb);
	return 0;

packet_length_error:
	BT_ERR("Connect req packet to short => discard");
	return 1;

}

/***************************************************************************/
static int bnep_rx_connect_rsp(struct bnep_connection *bnep_conn, struct sk_buff *skb_input)
{

	__u16 ResponseMessage;
	
	//Eval packet length
	if (skb_input->len < 2) goto packet_length_error;
	//Copy message and remove from header
	memcpy(&ResponseMessage, skb_input->data, 2);
	ResponseMessage = bnep16_to_cpu(ResponseMessage);
	skb_pull(skb_input, 2);

	BT_DBG("Got connection response : 0x%x", ResponseMessage);

	if (ResponseMessage == CONN_OPERATION_SUCCESSFUL) {
		BT_DBG("Open now connection for communication");
		atomic_set(&bnep_conn -> status , BNEP_CONN_OPEN);
	}
	
	if (bnep_conn->connect_request.req_status == BNEP_REQ_PENDING) {
		bnep_conn->connect_request.req_status = BNEP_REQ_DONE;
		bnep_conn->connect_request.req_result = ResponseMessage;
		wake_up_interruptible(&(bnep_conn->connect_request.req_wait_q));
	} else {
		BT_DBG("No request registered");
	}
	return 0;

packet_length_error:
	BT_ERR("Connect req packet to short => discard");
	return 1;

}

/***************************************************************************/
static void bnep_filter_req(struct request_struct *request, unsigned long opt)
{
	struct filter_request_struct *filter_request = (struct filter_request_struct *) opt;

	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("Unknown filter request type");
			return;
		}

		if (!(skb = alloc_skb(2 + 2 + filter_request->data_length, GFP_ATOMIC))) 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_request_filter(struct bnep_connection *bnep_conn, struct filter_request_struct *filter_request)
{
	int resend_counter = 0;
	int status = 0;

	if (!bnep_conn) {
		BT_DBG("No L2CAP connection");
		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++;
		if (atomic_read(&bnep_conn ->status) == BNEP_CONN_CLOSING) {
			BT_DBG("Cancel request (session closing)");
			kfree(bnep_conn);
			return BNEP_ERR_FILTER_ERROR;
		}			
	} while (bnep_conn->filter_request.req_status != BNEP_REQ_DONE && resend_counter < 2);

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

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

	if (!(bnep_conn =
	     (struct bnep_connection *) kmalloc(sizeof(struct bnep_connection), GFP_KERNEL)))
		return NULL;

	atomic_set(&bnep_conn -> status , BNEP_CONN_CLOSED);
	bnep_conn->sock = sock;
	bnep_conn->net_dev = net_dev;
#ifdef FILTER_SUPPORT
	//Init filter req data
	bnep_conn->filter_info.NetFilter = NULL;
	bnep_conn->filter_info.MulticastFilter = NULL;
#endif
	//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
#ifdef PANU_ONLY
	priv->bnep_conn = bnep_conn;
#else	
	down_write(&priv->bnep_conn_sem);
	list_add(&bnep_conn->list, &priv->bnep_connections);
	up_write(&priv->bnep_conn_sem);
#endif
	priv->connection_counter++;
	MOD_INC_USE_COUNT;
	//Prepare thread 
	bnep_conn -> thread . name = "kBnepCond";
	bnep_conn -> thread . main = bnep_session;
	bnep_conn -> thread . init = NULL;
	bnep_conn -> thread . cleanup = NULL;
	return bnep_conn;
}

/***************************************************************************/
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 int bnep_session(struct kthread *thread , void *arg)
{
	struct bnep_connection *session_conn = (struct bnep_connection *) arg;
	struct socket *session_sock = session_conn->sock;
	struct bnep_private *priv = bnep_get_priv(session_conn->net_dev);
	wait_queue_t wait;
	
	BT_DBG("Start session thread");
	init_waitqueue_entry(&wait, current);
	add_wait_queue(session_sock->sk->sleep, &wait);
	while (session_sock->sk->state != BT_CLOSED) {
		struct sk_buff *skb;
		set_current_state(TASK_INTERRUPTIBLE);

		//Get all data from L2CAP
		while ((skb = skb_dequeue(&session_sock->sk->receive_queue))) {
			//Send skb to bnep
			skb_orphan(skb);
			bnep_rx_frame(session_conn, skb);
		}
		//Send all data to L2CAP
		while ((skb = skb_dequeue(&session_sock->sk->write_queue))) {
			bnep_tx_frame(session_sock, skb);
		}
		
		if (test_bit(KTH_SHUTDOWN,&thread->state))
			break;
			
		schedule();
	}

	remove_wait_queue(session_sock->sk->sleep, &wait);
	 
#ifdef PANU_ONLY
	BT_DBG("Deleting connection");
	--(priv->connection_counter);
	MOD_DEC_USE_COUNT;
	priv->bnep_conn = NULL;
#else
	BT_DBG("Deleting connection list entry");
	--(priv->connection_counter);
	MOD_DEC_USE_COUNT;

	down_write(&priv->bnep_conn_sem);
	list_del(&session_conn->list);
	up_write(&priv->bnep_conn_sem);
#endif
	
#ifdef FILTER_SUPPORT	
	if (session_conn->filter_info.NetFilter)
		kfree(session_conn->filter_info.NetFilter);
	if (session_conn->filter_info.MulticastFilter)
		kfree(session_conn->filter_info.MulticastFilter);		
#endif
	BT_DBG("Releasing socket");
	sock_release(session_sock);
	
	//Delete session, if no pending connect request
	if (session_conn->connect_request.req_status == BNEP_REQ_DONE &&
		session_conn->filter_request.req_status == BNEP_REQ_DONE)
		kfree(session_conn);
	else {
		BT_DBG("Close connection and wait for pending requests");
		atomic_set(&session_conn ->status , BNEP_CONN_CLOSING);	
	}
	return -1;
}

/***************************************************************************/
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) {
		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_LEN;

		//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, 2);

		//Insert connection request data
		memcpy(skb_put(skb, 1), &uuid_length, 1);
		memcpy(skb_put(skb, uuid_length), &dest_uuid, uuid_length);
		memcpy(skb_put(skb, uuid_length), &source_uuid, uuid_length);
		//complete and send control package
		bnep_tx_controldata(request->bnep_conn, BNEP_SETUP_CONNECTION_REQUEST_MSG, 0 , skb);
	}
	return;
}

/***************************************************************************/
static struct bnep_connection* 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_connection *bnep_conn = NULL;
	struct bnep_private *bnep_priv = bnep_get_priv(net_dev);


	if ((bnep_conn = 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 bnep_conn;
	}

#ifdef PANU_ONLY
	if (bnep_priv->connection_counter != 0){
		BT_DBG("PANU builds allows only one connection");
		return NULL;
	}
#endif
	
	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 NULL;
	}

	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 NULL;
	}
	//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)) {
		bnep_conn = bnep_prepare_connection(nsk , net_dev);
		if (bnep_conn) {
			BT_DBG("Start new thread");	
			kthread_start(&bnep_conn -> thread , (void*)bnep_conn);
		}
		else {
			BT_ERR("Can't init new connection");
		}
	} else {
		BT_DBG("Can't connect %s -> %s", batostr(&bnep_priv->source_address),
				batostr(&bnep_connect_mgnt->destination_address));
		sock_release(nsk);
		return NULL;
	}

	return bnep_conn;
}

/***************************************************************************/
static int bnep_disconnect (struct bnep_connection *conn) {
	
	//Kill session thread
	BT_DBG("Signal session exit");
	kthread_stop(&conn -> thread);
	return 0;
}
			
/***************************************************************************/
static int bnep_connect(struct net_device *net_dev,
			struct bnep_connect_mgnt_struct *bnep_connect_mgnt)
{
	int status = 0;
	int resend_counter = 0;
	int role_uuid = bnep_get_priv(net_dev)->role_uuid;
	struct bnep_connection *bnep_conn;
#ifdef PANU_ONLY
	if (role_uuid != UUID_PANU )
		return BNEP_ERR_CONN_UNKNOWN_DEF_ROLE;
	if (bnep_get_priv(net_dev)->connection_counter != 0){
		BT_DBG("PANU builds allows only one connection");
		return -1;
	}
#else
	if (role_uuid != UUID_PANU && role_uuid != UUID_GN && role_uuid != UUID_NAP)
		return BNEP_ERR_CONN_UNKNOWN_DEF_ROLE;
#endif

	if (!(bnep_conn = bnep_l2cap_connect(net_dev, bnep_connect_mgnt))) {
			BT_DBG("Can't connect (L2CAP)");
			return BNEP_ERR_NO_L2CAP;
	}

	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("Send 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);
		if (atomic_read(&bnep_conn ->status) == BNEP_CONN_CLOSING) {
			BT_DBG("Cancel request (session closing)");
			kfree(bnep_conn);
			return BNEP_ERR_CONNECT_BNEP_ERROR;