bnep_core.c

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

		}		
		resend_counter++;
	} while (bnep_conn->connect_request.req_status != BNEP_REQ_DONE && resend_counter < 2);

	if (bnep_conn->connect_request.req_status != BNEP_REQ_DONE) {
		status = BNEP_ERR_CONNECT_BNEP_TIMEOUT;
		BT_DBG("Can't connect (BNEP Connect Timeout)");
	}
	else {
		if (bnep_conn->connect_request.req_result != CONN_OPERATION_SUCCESSFUL) {
			BT_DBG("Can't connect (BNEP Connect Error)");
			status = BNEP_ERR_CONNECT_BNEP_ERROR;
		}
		else {
			net_dev->set_multicast_list(net_dev);
			bnep_conn->destination_uuid = bnep_connect_mgnt->destination_uuid;
		}
	}

	bnep_conn->connect_request.req_status = BNEP_REQ_DONE;

	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("Request result: %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)
{
#ifdef PANU_ONLY
	BT_DBG("Role update in PANU_only build not supported");
#else
	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 local role to 0x%x", new_role);

	//Send updated BNEP connect message to all connected nodes	
	down_read(&priv->bnep_conn_sem);
	list_for_each (ptr , &priv->bnep_connections) {
		entry = list_entry(ptr, struct bnep_connection, list);
		baswap(&tmp_addr, &bluez_pi(entry->sock->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);
	}
	up_read(&priv->bnep_conn_sem);
#endif
	return 0;
}

/***************************************************************************/
static int bnep_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
{

	switch (cmd) {
		//Close a connection    
		case SIOC_DISCONNECT:{
			struct bnep_connect_mgnt_struct bnep_connect_mgnt;
			struct bnep_connection *conn = NULL;
			copy_from_user(&bnep_connect_mgnt, rq->ifr_data,
				       sizeof(struct bnep_connect_mgnt_struct));
			conn = bnep_get_conn_by_bdaddr(dev, &bnep_connect_mgnt.destination_address);
			if (conn) {
				BT_DBG("Closing connection to %s",
					batostr(&bnep_connect_mgnt.destination_address));
				return bnep_disconnect(conn);
			}
			return BNEP_ERR_NO_L2CAP;
		}

		//Create a L2CAP/BNEP connection             
		case SIOC_CONNECT:{
			int status = 0;
			struct bnep_connect_mgnt_struct bnep_connect_mgnt;
			struct bnep_connection *bnep_conn;
			copy_from_user(&bnep_connect_mgnt, rq->ifr_data,
				       sizeof(struct bnep_connect_mgnt_struct));
			switch (bnep_connect_mgnt.protocol) {
			case CONN_L2CAP_LAYER:
				if (!(bnep_conn = bnep_l2cap_connect(dev, &bnep_connect_mgnt)))
					return BNEP_ERR_CONNECT_L2CAP;
				break;
			case CONN_BNEP_LAYER:
				status = bnep_connect(dev, &bnep_connect_mgnt);
				break;
			default:
				status = BNEP_ERR_CONN_UNKNOWN_LAYER;
			}
			return status;
		}

		//Set filter
		case SIOC_SET_FILTER:{
			struct filter_request_struct filter_request;
			struct bnep_connection *bnep_conn;
			unsigned char *data = NULL;
			int status;
			copy_from_user(&filter_request, rq->ifr_data,
				       sizeof(struct filter_request_struct));

			if (!(bnep_conn = bnep_get_conn_by_bdaddr(dev, &filter_request.destination_address))) {
				return BNEP_ERR_NO_L2CAP;
			}
	
			if (filter_request.data_length > 0) {
				data =
				    (unsigned char *) kmalloc(filter_request.data_length,
							      GFP_ATOMIC);
				copy_from_user(data, filter_request.data,
					       filter_request.data_length);
				filter_request.data = data;
			}
	
			status = bnep_request_filter(bnep_conn, &filter_request);
			kfree(data);
			return status;
		}

		//Change role of device
		case SIOC_SET_ROLE:{
#ifdef PANU_ONLY
			BT_DBG("Role change not allowed in PANU_only build\n");
			return -1;
#else
			int role_uuid;
			copy_from_user(&role_uuid, rq->ifr_data, sizeof(int));
			return bnep_update_role(dev, role_uuid);
#endif
		}

		//Test case execution
		case SIOC_TESTCASE:	/* Execute test case  */  {
#ifdef BNEP_TEST
			struct bnep_test_ctrl_struct bnep_test_ctrl;
			copy_from_user(&bnep_test_ctrl, rq->ifr_data,
				       sizeof(struct bnep_test_ctrl_struct));
			return test_execute_test(dev, &bnep_test_ctrl);
#else
			BT_DBG("Test mode not available in non_test build");
			return 1;
#endif
			break;
		}
		default: BT_DBG("Unknown IOCTL : %d" , cmd);
	}
	return -EOPNOTSUPP;
}

/***************************************************************************/
static struct net_device_stats *device_get_stats(struct net_device *dev)
{
	struct bnep_private *priv;
	priv = (struct bnep_private *) dev->priv;
	return &(priv->stats);
}

/***************************************************************************/
static int device_open_dev(struct net_device *dev)
{
	netif_start_queue(dev);
	return 0;
}

/***************************************************************************/
static int device_close_dev(struct net_device *dev)
{
	netif_stop_queue(dev);
	return 0;
}

/***************************************************************************/
static void device_dev_timeout(struct net_device *dev)
{
	BT_DBG("Not implemented");
}

/***************************************************************************/
static int __init bnep_init(struct net_device *dev)
{
	int i;
	struct bnep_private *priv = (struct bnep_private *) dev->priv;

	//init dev struct
	ether_setup(dev);

	//net device setup
	dev->hard_start_xmit = &device_tx_data;
	dev->get_stats = &device_get_stats;
	dev->do_ioctl = &bnep_ioctl;
	dev->set_multicast_list = &device_set_multicast_list;
	dev->open = &device_open_dev;
	dev->stop = &device_close_dev;
	dev->tx_timeout = &device_dev_timeout;
	dev->watchdog_timeo = HZ;
#ifdef CONFIG_NET_FASTROUTE
	dev->accept_fastpath = device_accept_fastpath;
#endif

	//Set device source address from private data
	BT_DBG("Binding %s <-> %s", batostr(&priv->source_address), dev->name);
	for (i = 0; i < 6; i++)
		dev->dev_addr[i] = ((unsigned char *) &priv->source_address)[i];

	dev->tx_queue_len = 100;
	dev->flags &= (IFF_MULTICAST | IFF_BROADCAST);

	return 0;
}

/***************************************************************************/
struct socket *bnep_sock_accept(struct socket *sock)
{
	struct socket *nsock;
	int err;

	nsock = sock_alloc();
	if (!nsock)
		return NULL;

	nsock->type = sock->type;
	nsock->ops = sock->ops;
	err = sock->ops->accept(sock, nsock, 0);
	BT_DBG("Sock accep result: %d", err);

	if (err < 0) {
		sock_release(nsock);
		return NULL;
	}

	return nsock;
}
 
/***************************************************************************/
static int bnep_connect_listener(struct kthread *thread , void *arg)
{
	struct sockaddr_l2 addr;
	struct l2cap_options opts;
	int err, size;
	struct net_device *dev = (struct net_device *)arg;
	struct bnep_private *bnep_priv = bnep_get_priv(dev);;
	
	set_fs(KERNEL_DS);
	
	// Create socket
	err = sock_create(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP, &bnep_priv->bnep_sock);
	if (err < 0) {
		BT_DBG("Create socket failed %d", err);
		return -1;
	}

	// Bind socket 
	baswap(&addr.l2_bdaddr, &bnep_priv->source_address);
	addr.l2_family = AF_BLUETOOTH;
	addr.l2_psm = htobs(PAN_PSM);
	err =
	    bnep_priv->bnep_sock->ops->bind(bnep_priv->bnep_sock, (struct sockaddr *) &addr,
					  sizeof(addr));
	if (err < 0) {
		BT_DBG("Bind failed %d", err);
		return -1;
	}

	// Set L2CAP options
	size = sizeof(opts);
	bnep_priv->bnep_sock->ops->getsockopt(bnep_priv->bnep_sock, SOL_L2CAP, L2CAP_OPTIONS,
					    (void *) &opts, &size);
	opts.imtu = PAN_IMTU;
	opts.imtu = PAN_OMTU;
	opts.flush_to = PAN_FLUSH_TO;
	bnep_priv->bnep_sock->ops->setsockopt(bnep_priv->bnep_sock, SOL_L2CAP, L2CAP_OPTIONS,
					    (void *) &opts, size);

	// Start listening on the socket
	err = bnep_priv->bnep_sock->ops->listen(bnep_priv->bnep_sock, 10);
	if (err) {
		BT_DBG("Listen failed %d", err);
		sock_release(bnep_priv->bnep_sock);
		return 0;
	}

	while (1) {
		struct socket *nsk;
		bdaddr_t tmp_addr;
	
		nsk = bnep_sock_accept(bnep_priv->bnep_sock);

		if (!nsk || !kthread_running(thread)) {
			BT_DBG("Exiting connect listener");
			break;
		}

		baswap(&tmp_addr, &bluez_pi(nsk->sk)->dst);

		//Check for existing bnep connection
		if (bnep_get_conn_by_bdaddr(dev, &tmp_addr)) {
			BT_DBG("Connection to %s exists => close new connection",
					batostr(&tmp_addr));
			sock_release(nsk);
		} else {
			//Prepare new connection and start session
			struct bnep_connection *bnep_conn = bnep_prepare_connection(nsk , dev);
			if (bnep_conn) {
				kthread_start(&bnep_conn -> thread , (void*)bnep_conn);
			}
			else
				BT_ERR("Can't init new connection");
		}
	}
	
	BT_DBG("Exit connect listener");
	return -1;
}

/***************************************************************************/
static int bnep_read_proc(char *buf, char **start, off_t offset, int count, int *eof, void *priv)
{
	char *ptr = buf;
	int len;
	bdaddr_t tmp_addr;
	struct bnep_private *bnep_priv;
	struct list_head *dev_list_ptr;

#ifndef PANU_ONLY
	struct list_head *conn_list_ptr;
	struct bnep_connection *bnep_conn;
#endif

#ifdef FILTER_SUPPORT
	int counter;
#endif	

	ptr += sprintf(ptr, "BNEP device status:\n");
	if (list_empty(&devices)) {
		ptr += sprintf(ptr, " No PAN devices loaded\n");
	} 
	else {
		down_read(&bnep_dev_sem);
		list_for_each (dev_list_ptr , &devices) {
			bnep_priv = list_entry(dev_list_ptr, struct bnep_private, list);
			ptr += sprintf(ptr, " BDAddr: %s Role: 0x%.4x\n",
				    batostr(&bnep_priv->source_address), bnep_priv->role_uuid);
#ifdef PANU_ONLY
			if (!bnep_priv->bnep_conn) 
				ptr += sprintf(ptr, "\t  Connection: None");					
			else {
				baswap(&tmp_addr, &bluez_pi(bnep_priv->bnep_conn->sock->sk)->dst);
				ptr += sprintf(ptr, "\t  Connection: %s Role 0x%.4x\n",
						    batostr(&tmp_addr), bnep_priv->bnep_conn->destination_uuid);
				ptr += sprintf(ptr, "\t\tFilter not supported in PANU only build");
			}
#else
			list_for_each(conn_list_ptr , &bnep_priv->bnep_connections) {
				bnep_conn = list_entry(conn_list_ptr, struct bnep_connection, list);
				baswap(&tmp_addr, &bluez_pi(bnep_conn->sock->sk)->dst);
				ptr += sprintf(ptr, "\t  Connection: %s Role 0x%.4x\n",
					batostr(&tmp_addr), bnep_conn->destination_uuid);
#ifdef FILTER_SUPPORT
				if (bnep_conn->filter_info.MulticastFilter) {
					for (counter = 0; counter <
						bnep_conn->filter_info.multicast_filter_count;counter++) {
						int byte_counter;
						ptr += sprintf(ptr, "\t\tMulticast Filter %d: ",counter);
						for (byte_counter = 5; byte_counter >= 0;
							byte_counter--) {
							ptr +=sprintf(ptr, "%.2x%s",
									((__u8 *) & bnep_conn->filter_info.MulticastFilter[counter].start)[byte_counter],
								    byte_counter == 0 ?