bnep_core.c

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

	}

set_multicast_filter:
	if (response_code == MCFILTER_OPERATION_SUCCESSFUL) {
		//Delete old filter settings
		if (bnep_conn->filter_info.MulticastFilter)
			kfree(bnep_conn->filter_info.MulticastFilter);

		//Set new filter
		bnep_conn->filter_info.MulticastFilter = new_filter;
		bnep_conn->filter_info.multicast_filter_count = list_length / 12;
	}
#endif
	//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 
	BT_DBG("Send response code 0x%.4x" , bnep16_to_cpu(response_code));   
	bnep_tx_controldata(bnep_conn, BNEP_FILTER_MULT_ADDR_RESPONSE_MSG, 0, skb);
	return 0;

packet_length_error:
	BT_ERR("Illegal packet length");
	return 1;	
}


/***************************************************************************/
static int bnep_rx_net_filter(struct bnep_connection *bnep_conn, struct sk_buff *skb_input)
{
	__u16 list_length;
	struct sk_buff *skb;
	__u16 response_code = NETFILTER_OPERATION_SUCCESSFUL;
#ifdef FILTER_SUPPORT
	int counter;
	struct protocol_range *new_filter = NULL;
#endif

	//Validate packet length
	if (skb_input->len < 2) goto packet_length_error;
	//Copy list length and remove info from header
	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);
	//Validate list length info
	if (list_length > skb_input->len) goto packet_length_error;
#ifndef FILTER_SUPPORT
	response_code = cpu_to_bnep16(NETFILTER_UNSUPPORTED_REQUEST);
	skb_pull(skb_input, list_length);
#else
#ifdef BNEP_TEST
	if (test_ignore_netfilter == TESTCASE_IGNORE_MESSAGE_ON) {
		BT_DBG("Ignore protocol filter activated (response will not be sent)\n");
		skb_pull(skb_input, list_length);
		return 0;
	}
#endif	
	if (list_length == 0) {
		BT_DBG("Resetting filter");
		goto set_net_filter;
	} 
	if (list_length % 4 != 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;
	}
	//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;
	}
#endif
	//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    
	BT_DBG("Send response code 0x%.4x" , bnep16_to_cpu(response_code));   
	bnep_tx_controldata(bnep_conn, BNEP_FILTER_NET_TYPE_RESPONSE_MSG, 0, skb);

	return 0;
	
packet_length_error:
	BT_ERR("Illegal packet length");
	return 1;	
}

/***************************************************************************/
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;
	//Send data to higher layer
	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);
		__u16 proto = 0;
		
		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);
		
		//Detect real protocol in case of 802.1q
		if (bnep16_to_cpu(eth.h_proto) == 0x8100) {
			memcpy(&proto , &(skb->data[2]) , 2);
			proto = bnep16_to_cpu(proto);
		}
		else 
			proto = bnep16_to_cpu(eth.h_proto);
			
#ifdef PANU_ONLY
		bnep_tx_etherdata(NULL , priv->bnep_conn, skb , &eth , 0, BNEP_PACKET_TYPE_AUTO, FORWARD_MODE_ALL);
#else				
		bnep_tx_etherdata_multiplexor(dev, NULL, skb, &eth , proto , 0 ,BNEP_PACKET_TYPE_AUTO);
#endif	
		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;
	int status = 0;
	
	//Eval packet lenght
	if (skb_input->len < 1) goto packet_length_error;
	//Copy BNEP header
	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) 
		status = bnep_rx_controldata(bnep_conn, skb_input, bnep_header & 0x80 );
	else 
		status = bnep_rx_etherdata(bnep_conn, skb_input, bnep_header & 0x7F, bnep_header & 0x80);
	return status;
	
packet_length_error:
	BT_ERR("Packet to short => discard");
	kfree_skb(skb_input);
	return 1;

}
 
#ifdef FILTER_SUPPORT
/***************************************************************************/
static int bnep_set_multicast_list(struct net_device *dev)
{
	struct bnep_private *priv = bnep_get_priv(dev);
	struct request_struct request;
	struct filter_request_struct filter_request;
	struct list_head *ptr = NULL;
	
	BT_DBG("Send multicast list update");

	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 -ENOMEM;
			}
			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
		down_read(&priv->bnep_conn_sem);
		list_for_each (ptr , &priv->bnep_connections) {
			request . bnep_conn = list_entry(ptr, struct bnep_connection, list);
			BT_DBG("send settings to node %s",
					batostr(&bluez_pi(request.bnep_conn->sock->sk)->dst));

			//complete and send control package
			bnep_filter_req(&request, (unsigned long)&filter_request);
		}
		up_read(&priv->bnep_conn_sem);
		//Clean up request
		if (filter_request.data)
			kfree(filter_request.data);
	}
	return 0;
}
#endif

/***************************************************************************/
static void device_set_multicast_list(struct net_device *dev)
{
#ifndef FILTER_SUPPORT
	BT_DBG("Autmatic multicast set disabled in NON_FILTER build");
#else
	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 (role GN/NAP)");
		return;
	}
	//Send new values to all connections
	bnep_set_multicast_list(dev);
#endif
}

/***************************************************************************/
/***************************************************************************/
#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)
{
	char base_uuid[12] = {0x00, 0x00, 0x10, 0x00, 0x80, 0x00,
			      0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb};
	__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_LEN:
		memcpy(&destination_uuid, skb_input->data, UUID16_LEN);
		destination_uuid = bnep16_to_cpu(destination_uuid);
		memcpy(&source_uuid, &(skb_input->data)[UUID16_LEN], UUID16_LEN);
		source_uuid = bnep16_to_cpu(source_uuid);
		break;

	case UUID32_LEN:
		memcpy(&destination_uuid , skb_input->data , UUID16_LEN);
		if (destination_uuid != 0) {
			BT_DBG("Illegal base uuid");
			ResponseMessage = cpu_to_bnep16(CONN_OPERATION_FAILED_INVALID_DESTINATION_UUID);
			break;
		}
		memcpy(&destination_uuid , &(skb_input->data)[UUID16_LEN], UUID16_LEN);
		destination_uuid = bnep16_to_cpu(destination_uuid);
		
		memcpy(&source_uuid , &(skb_input->data)[UUID32_LEN], UUID16_LEN);
		if (source_uuid != 0) {
			BT_DBG("Illegal base uuid");
			ResponseMessage = cpu_to_bnep16(CONN_OPERATION_FAILED_INVALID_SOURCE_UUID);
			break;
		}
		memcpy(&source_uuid , &(skb_input->data)[UUID32_LEN+UUID16_LEN], UUID16_LEN);
		source_uuid = bnep16_to_cpu(source_uuid);
		break;

	case UUID128_LEN: {
		//BASE_UUID: 00000000-0000-1000-8000-00805F9B34FB
		memcpy(&destination_uuid , skb_input->data , UUID16_LEN);
		if ((destination_uuid = 0) && 
		      (!memcmp(base_uuid, &(skb_input->data)[UUID128_LEN-UUID32_LEN], UUID128_LEN-UUID32_LEN))) {
			memcpy(&destination_uuid , &(skb_input->data)[UUID16_LEN], UUID16_LEN);
			destination_uuid = bnep16_to_cpu(destination_uuid);
		} else {
			BT_DBG("Illegal base uuid");
			ResponseMessage = cpu_to_bnep16(CONN_OPERATION_FAILED_INVALID_DESTINATION_UUID);
			break;
		}
		memcpy(&source_uuid , &(skb_input->data)[UUID128_LEN] , UUID16_LEN);
		if ((source_uuid = 0) && 
		    (!memcmp(base_uuid, &(skb_input->data)[UUID128_LEN*2-UUID32_LEN], UUID128_LEN-UUID32_LEN))) {
			memcpy(&source_uuid , &(skb_input->data)[UUID128_LEN+UUID16_LEN], UUID16_LEN);
			source_uuid = bnep16_to_cpu(source_uuid);
		} else {
			BT_DBG("Illegal base uuid");
			ResponseMessage = cpu_to_bnep16(CONN_OPERATION_FAILED_INVALID_SOURCE_UUID);
			break;
		}
		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_SUCCESSFUL)) {
		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;
					atomic_set(&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("Ignore connection request activated (reponse will not be sent)");
		return 0;