bnep_core.c

linux下蓝牙

	skb_reserve(skb, 2);

	//Insert response code
	memcpy(skb_put(skb, 2), &response_code, 2);

	//Add bnep header and send message    
	bnep_tx_controldata(bnep_conn, BNEP_FILTER_MULT_ADDR_RESPONSE_MSG, 0, skb);

	return 0;
}
/***************************************************************************/
static int bnep_rx_net_filter(struct bnep_connection *bnep_conn, struct sk_buff *skb_input)
{
	struct protocol_range *new_filter = NULL;
	__u16 list_length;

	struct sk_buff *skb;
	__u16 response_code = NETFILTER_OPERATION_SUCCESSFUL;
	int counter;

	//Copy list length and remove info from header
	if (skb_input->len < 2) {	
		BT_ERR("packet to short => discard");
		return 1;
	}
	memcpy(&list_length, skb_input->data, 2);
	list_length = bnep16_to_cpu(list_length);
	skb_pull(skb_input, 2);
	BT_DBG("List length:%.4x", list_length);

	if (list_length == 0) {
		BT_DBG("resetting filter");
		goto set_net_filter;
	} 
	
	if (list_length > skb_input->len) {
		BT_ERR("filter list lenght error: 0x%x > 0x%x => skip remaning packet",
				list_length, skb_input->len);
		return 1;
	}
	
	if (list_length % 2 != 0) {
		BT_ERR("filter list lenght error: odd number of bytes (0x%x) => skip filter",
				list_length);
		skb_pull(skb_input, list_length);
		return 0;
	}
	
#ifdef BNEP_TEST
	if (test_ignore_netfilter == TESTCASE_IGNORE_MESSAGE_ON) {
		BT_DBG("Ignore netfilter\n");
		skb_pull(skb_input, list_length);
		return 0;
	}
#endif

	//Alloc space for new filter
	if (!(new_filter = (struct protocol_range *) 
		kmalloc(sizeof(struct protocol_range[list_length / 4]), GFP_KERNEL))) {
		return -ENOMEM;
	}

	//Copy values
	for (counter = 0; counter < list_length / 4; counter++) {
		memcpy(&new_filter[counter].start, skb_input->data, 2);
		new_filter[counter].start = bnep16_to_cpu(new_filter[counter].start);
		memcpy(&new_filter[counter].stop, &(skb_input->data)[2], 2);
		new_filter[counter].stop = bnep16_to_cpu(new_filter[counter].stop);
		skb_pull(skb_input, 4);

		BT_DBG("filter %d: 0x%.4x - 0x%.4x (%s)", counter,
				new_filter[counter].start,
				new_filter[counter].stop ,
				(new_filter[counter].start <= new_filter[counter].stop) ? "valid" : "invalid");
				
		if (new_filter[counter].start > new_filter[counter].stop) {
			response_code = cpu_to_bnep16(NETFILTER_INVALID_RANGE);
		}		
	}

set_net_filter:

	if (response_code == NETFILTER_OPERATION_SUCCESSFUL) {
		//Delete old filter settings
		if (bnep_conn->filter_info.NetFilter)
			kfree(bnep_conn->filter_info.NetFilter);

		//Set new filter
		bnep_conn->filter_info.NetFilter = new_filter;
		bnep_conn->filter_info.net_filter_count = list_length / 4;
	}

	//Create response control packet
	if (!(skb = alloc_skb(4, GFP_ATOMIC)))
		return -ENOMEM;
	skb_reserve(skb, 2);

	//Insert response code
	memcpy(skb_put(skb, 2), &response_code, 2);

	//Add bnep header and send message    
	bnep_tx_controldata(bnep_conn, BNEP_FILTER_NET_TYPE_RESPONSE_MSG, 0, skb);

	return 0;
}

/***************************************************************************/
static int device_rx_data(struct sk_buff *skb, struct net_device *dev)
{
	struct bnep_private *priv = (struct bnep_private *) dev->priv;
	BT_DBG("Sending packet to higher layer %d", skb->len);

	//Set package options and remove ethernet header
	skb->dev = dev;
	skb->protocol = eth_type_trans(skb, dev);
	skb->ip_summed = CHECKSUM_UNNECESSARY;

	//Update statistics
	priv->stats.rx_packets++;
	priv->stats.rx_bytes += skb->len;

	netif_rx_ni(skb);

	return 0;
}

/***************************************************************************/
static int device_tx_data(struct sk_buff *skb, struct net_device *dev)
{
	if (bnep_dev_counter > 0) {
		struct ethhdr eth;
		struct bnep_private *priv = bnep_get_priv(dev);

		spin_lock_irq(&priv->lock);

		BT_DBG("Sending packet to lower layer %d", skb->len);

		//Update statistics
		priv->stats.tx_packets++;
		priv->stats.tx_bytes += skb->len;

		//Store and remove ethernet header
		memcpy(&eth, skb->data, sizeof(struct ethhdr));
		skb_pull(skb, ETH_HLEN);

		//Forward payload to other slaves
		bnep_tx_etherdata_multiplexor(dev, NULL, skb, &eth, NULL, 0, BNEP_PACKET_TYPE_AUTO);
	
		dev->trans_start = jiffies;

		spin_unlock_irq(&priv->lock);
	}

	return 0;
}

/***************************************************************************/
static int bnep_rx_frame(struct bnep_connection *bnep_conn, struct sk_buff *skb_input)
{
	//Local var
	__u8 bnep_header;

	//Copy BNEP header
	if (skb_input->len < 1) {
		BT_ERR("packet to short => discard");
		kfree_skb(skb_input);
		return 1;
	}
	
	memcpy(&bnep_header, skb_input->data, 1);
	skb_pull(skb_input, 1);

	BT_DBG("L2CAP Length: 0x%.4x BNEP Type: 0x%.2x Extension: %s",
			skb_input->len + 1, (bnep_header & 0x7F),
			(bnep_header & 0x80) ? "Yes" : "No");

	if ((bnep_header & 0x7F) == BNEP_CONTROL) 
		bnep_rx_controldata(bnep_conn, skb_input, bnep_header & 0x80 );
	else 
		bnep_rx_etherdata(bnep_conn, skb_input, bnep_header & 0x7F, bnep_header & 0x80);
	return 0;
}

/***************************************************************************/
static void bnep_set_multicast_list(kthread_t * kthread)
{
	struct filter_request_struct filter_request;
	struct net_device *dev = NULL;

	BT_DBG("start send multicast thread");

	init_kthread(kthread, "bnep-mc-send");

	while (1) {
		//Wait for changed mc list on any device
		down_interruptible(&bnep_mc_send_sem);
		//Quit thread on terminate signal
		mb();
		if (kthread->terminate) {
			BT_DBG("exiting mc send thread");
			break;
		}
		//Check all devices, if mc list has to be resend
		dev = root_dev;
		while (dev) {
			struct bnep_private *priv = bnep_get_priv(dev);
			struct bnep_connection *bnep_conn;
			struct list_head *ptr;

			if (priv->connection_counter > 0) {
				BT_DBG("dev %s", dev->name);
				//Set general filter settings
				filter_request.filter_type = FILTER_TYPE_MULTICAST;
				filter_request.data = NULL;
				//Set settings for promisc => No filter active
				if (dev->flags & IFF_PROMISC) {
					BT_DBG("promisc => reset multicast filter ");
					filter_request.data_length = 0;
				} else {
					//Set multicast filtering
					bdaddr_t *bdaddr;
					//Prepare multicast filter
					if (dev->flags & IFF_BROADCAST)
						filter_request.data_length =
						    (dev->mc_count + 1) * 2 * sizeof(bdaddr_t);
					else
						filter_request.data_length =
						    (dev->mc_count) * 2 * sizeof(bdaddr_t);
					//Alloc data packet
					if (!(filter_request.data =
					     (void *) kmalloc(filter_request.data_length, GFP_ATOMIC))) {
						BT_ERR(" can't allocate packet for multicast filter");
						return;
					}
					bdaddr = (bdaddr_t *) filter_request.data;
					//Insert broadcast value
					if (dev->flags & IFF_BROADCAST) {
						unsigned char bcast[6] =
						    { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
						BT_DBG("multicastlist: Broadcast");
						memcpy(bdaddr++, bcast, sizeof(bdaddr_t));
						memcpy(bdaddr++, bcast, sizeof(bdaddr_t));
					}
					//Add system filter settings
					BT_DBG("multicastlist: add %d entries", dev->mc_count);
					if (dev->mc_count > 0) {
						struct dev_mc_list *mc_ptr = dev->mc_list;
						for (mc_ptr = dev->mc_list; mc_ptr;
						     mc_ptr = mc_ptr->next) {
							BT_DBG("%s", batostr((bdaddr_t *) mc_ptr->dmi_addr));
							//Copy start / stop range
							bacpy(bdaddr++,
							      (bdaddr_t *) mc_ptr->dmi_addr);
							bacpy(bdaddr++,
							      (bdaddr_t *) mc_ptr->dmi_addr);
						}
					}
				}
				//Send settings
				for (ptr = priv->bnep_connections.next;
				     ptr != &priv->bnep_connections; ptr = ptr->next) {
					bnep_conn = list_entry(ptr, struct bnep_connection, list);
					BT_DBG("send settings to node %s",
							batostr(&bluez_pi(bnep_conn->sk->sk)->dst));
					_bnep_tx_filter(bnep_conn, &filter_request);
				}
				//Clean up request
				if (filter_request.data)
					kfree(filter_request.data);
			}
			dev = priv->next_dev;
		}
	}
	//Leave thread
	exit_kthread(kthread);
}

/***************************************************************************/
static void device_set_multicast_list(struct net_device *dev)
{
	struct bnep_private *priv = bnep_get_priv(dev);
	BT_DBG("set new settings");

	//Check, if local node is allowed to set filter
	if (priv->role_uuid != UUID_PANU) {
		BT_DBG("no filter settings allowed on this node due to role GN/NAP");
		return;
	}
	//Send new values to all connections
	up(&bnep_mc_send_sem);
}

/***************************************************************************/
/***************************************************************************/
#ifdef CONFIG_NET_FASTROUTE
static int device_accept_fastpath(struct net_device *dev, struct dst_entry *dst)
{
	return -1;
}
#endif

/***************************************************************************/
/***************************************************************************/
static int bnep_rx_connect_req(struct bnep_connection *bnep_conn, struct sk_buff *skb_input)
{
	__u8 uuid_length;
	__u16 ResponseMessage = cpu_to_bnep16(CONN_OPERATION_SUCCESSFUL);
	__u16 source_uuid = 0;
	__u16 destination_uuid = 0;
	struct sk_buff *skb;
	int role_uuid = bnep_get_priv(bnep_conn->net_dev)->role_uuid;

	BT_DBG("connection request");

	//Copy & check UUID size 
	if (skb_input->len < 1) goto packet_length_error;
	memcpy(&uuid_length, skb_input->data, 1);
	skb_pull(skb_input, 1);

	//Evaluate UUIDs
	if (skb_input->len < 2 * uuid_length) goto packet_length_error;
	switch (uuid_length) {
	case UUID16:
		memcpy(&destination_uuid, skb_input->data, UUID16);
		destination_uuid = bnep16_to_cpu(destination_uuid);
		memcpy(&source_uuid, &(skb_input->data)[UUID16], UUID16);
		source_uuid = bnep16_to_cpu(source_uuid);
		break;

	case UUID32:	
		memcpy(&destination_uuid , &(skb_input->data)[UUID16] , UUID16);
		destination_uuid = bnep16_to_cpu(destination_uuid);
		memcpy(&source_uuid , &(skb_input->data)[3*UUID16] , UUID16); 						
		source_uuid = bnep16_to_cpu(source_uuid);
		break;
						
	case UUID128:	
		memcpy(&destination_uuid , &(skb_input->data)[UUID16] , UUID16); 
		destination_uuid = bnep16_to_cpu(destination_uuid);
		memcpy(&source_uuid , &(skb_input -> data)[UUID16 + UUID128] , UUID16); 
		source_uuid = bnep16_to_cpu(source_uuid);			
		break;			
	default:
		BT_DBG("illegal uuid_length");
		uuid_length = 0;
		ResponseMessage  = cpu_to_bnep16(CONN_OPERATION_FAILED_INVALID_SERVICE_UUID_SIZE);
	}

	//Remove UUIDs from packet
	skb_pull(skb_input , 2*uuid_length);

	BT_DBG("request uuid_length:0x%x src:0x%x dst:0x%x loc.role:0x%x", uuid_length,
			source_uuid, destination_uuid, role_uuid);

        if (ResponseMessage != cpu_to_bnep16(CONN_OPERATION_FAILED_INVALID_SERVICE_UUID_SIZE)) {
		//Check source_uuid
		if (source_uuid != UUID_PANU && source_uuid != UUID_GN && source_uuid != UUID_NAP)
			ResponseMessage = cpu_to_bnep16(CONN_OPERATION_FAILED_INVALID_SOURCE_UUID);
		//Check source_uuid
		else if (destination_uuid != UUID_PANU && destination_uuid != UUID_GN
			 && destination_uuid != UUID_NAP)
			ResponseMessage = cpu_to_bnep16(CONN_OPERATION_FAILED_INVALID_DESTINATION_UUID);
		//Check connection_uuid
		else {
			if (role_uuid == destination_uuid) {
				if (source_uuid == destination_uuid && source_uuid != UUID_PANU)
					ResponseMessage =
					    cpu_to_bnep16(CONN_OPERATION_FAILED_CONNECTION_NOT_ALLOWED);
				else {
					bnep_conn->destination_uuid = source_uuid;
					bnep_conn -> status = BNEP_CONN_OPEN;
					ResponseMessage = cpu_to_bnep16(CONN_OPERATION_SUCCESSFUL);
				}
			} else
				ResponseMessage = cpu_to_bnep16(CONN_OPERATION_FAILED_INVALID_DESTINATION_UUID);
		}
	}

#ifdef BNEP_TEST
	if (test_ignore_connect == TESTCASE_IGNORE_MESSAGE_ON) {
		BT_DBG("Connection request test cases activated. Reponse will not be sent");
		return 0;
	} 
#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;

	//Process connection request data
	if (skb_input->len < 2) {
		BT_ERR("packet to short => discard");
		return 1;
	}
	
	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\n");
		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("connection not waiting for connection response => discard");
	}
	
	return 0;
}

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