bnep_core.c

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

#endif

/***************************************************************************/
//Processing packet for BT domain
#ifndef PANU_ONLY
int bnep_tx_etherdata_multiplexor(struct net_device *dev, struct bnep_connection *bnep_conn,
			       struct sk_buff *skb_input, struct ethhdr *eth, __u16 proto ,
			       int ExtensionHeaderLength, __u8 packet_type)
{
	struct list_head *ptr;
	struct bnep_connection *entry;
	struct bnep_private *priv;
	bdaddr_t tmp_addr;
	int send_packet;

	if (dev == NULL) {
		BT_DBG("Can not send to unknown device");
		return 0;
	}

	priv = bnep_get_priv(dev);
	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);

		//Do not send back to incoming connection
		if (entry == bnep_conn)
			continue;

		//Initial setting
		send_packet = 0;

		//Apply bridging filter
		if (priv->connection_counter == 1) {
			//Slave => Default GW || Master -> Only one slave
			send_packet = 1;
		} else if (*eth->h_dest & 1 || memcmp(eth->h_dest, &tmp_addr, ETH_ALEN) == 0) {
			//Multi-Broadcast or unicast to directly connected node
			send_packet = 1;
		}
#ifdef FILTER_SUPPORT
		if (send_packet) {
			//Packet type filter
			if ((send_packet = bnep_tx_apply_protocol_filter(entry, proto)) && *eth->h_dest & 1) {
				//Multicast filter
				send_packet = bnep_tx_apply_multicast_filter(entry, eth);
			}
		}
#endif
		if (send_packet) {
			bnep_tx_etherdata(bnep_conn, entry , skb_input , eth ,
					ExtensionHeaderLength , packet_type , FORWARD_MODE_ALL);
		}
		else if (ExtensionHeaderLength) {
			BT_DBG("Packet for node %s filtered -> forward extension header only", batostr(&tmp_addr));
			bnep_tx_etherdata(bnep_conn, entry , skb_input , eth ,
					ExtensionHeaderLength , packet_type , FORWARD_MODE_EXT_HEADER);
		}
		else {
			BT_DBG("Packet for node %s filtered", batostr(&tmp_addr));
		}
	}
	up_read(&priv->bnep_conn_sem);
	kfree_skb(skb_input);
	return 0;
}
#endif


/***************************************************************************/
static int bnep_rx_etherdata(struct bnep_connection *bnep_conn, struct sk_buff *skb_input,
			     int PacketType, int ExtensionFlag)
{
	__u16 proto = 0;
	struct ethhdr eth;
	int ExtensionHeadersLength = 0;
	struct bnep_private *priv = bnep_get_priv(bnep_conn->net_dev);

	if (atomic_read(&bnep_conn -> status) == BNEP_CONN_CLOSED) {
		BT_DBG("Connection not ready => drop data packet");
		kfree_skb(skb_input);
		return 1;	
	}

	//Copy bnep address information in ethernet struct 
	switch (PacketType) {
	case BNEP_GENERAL_ETHERNET:{
			BT_DBG("Packet type: General Ethernet");
			if (skb_input->len < 2 * ETH_ALEN) goto packet_length_error; 	
			memcpy(&(eth.h_dest), skb_input->data, ETH_ALEN);
			memcpy(&(eth.h_source), &(skb_input->data)[ETH_ALEN], ETH_ALEN);
			skb_pull(skb_input, 2 * ETH_ALEN);
			break;
		}
	case BNEP_COMPRESSED_ETHERNET:{
			bdaddr_t tmp_addr;
			BT_DBG("Packet type: Compressed Ethernet");
			memcpy(&(eth.h_dest), &priv->source_address, ETH_ALEN);
			baswap(&tmp_addr, &bluez_pi(bnep_conn->sock->sk)->dst);
			memcpy(&(eth.h_source), &tmp_addr, ETH_ALEN);
			break;
		}
	case BNEP_COMPRESSED_ETHERNET_SOURCE_ONLY:{
			BT_DBG("Packet type: Compressed Ethernet Source only");
			if (skb_input->len < ETH_ALEN) goto packet_length_error; 	
			memcpy(&(eth.h_dest), &priv->source_address, ETH_ALEN);
			memcpy(&(eth.h_source), skb_input->data, ETH_ALEN);
			skb_pull(skb_input, ETH_ALEN);
			break;
		}
	case BNEP_COMPRESSED_ETHERNET_DEST_ONLY:{
			bdaddr_t tmp_addr;
			BT_DBG("Packet type: Compressed Ethernet Destination only");
			if (skb_input->len < ETH_ALEN) goto packet_length_error;	
			memcpy(&(eth.h_dest), skb_input->data, ETH_ALEN);
			skb_pull(skb_input, ETH_ALEN);
			baswap(&tmp_addr, &bluez_pi(bnep_conn->sock->sk)->dst);
			memcpy(&(eth.h_source), &tmp_addr, ETH_ALEN);
			break;
		}
	default : {
			BT_DBG("Packet type: Unknown");
			kfree_skb(skb_input);
			return 1;	
		}
	}

	//Copy protocol from BNEP header
	if (skb_input->len < 2) goto packet_length_error; 	
	memcpy(&(eth.h_proto), skb_input->data, 2);
	skb_pull(skb_input, 2);

	BT_DBG("Source address: %s", batostr((bdaddr_t *) eth.h_source));
	BT_DBG("Dst. address: %s", batostr((bdaddr_t *) eth.h_dest));

	//Handle extension header if available and calculate extensions lenght 
	if (ExtensionFlag) {
		//Evaluate extension header
		if (bnep_rx_extension_header(bnep_conn , skb_input, &ExtensionHeadersLength)) {
			BT_ERR("Error in extension header");
			goto packet_length_error;
		}
	}

	//802.1p handling                                             
	if (bnep16_to_cpu(eth.h_proto) == 0x8100) {
		if (skb_input->len < ExtensionHeadersLength + 4) goto packet_length_error; 	
		memcpy(&proto , &(skb_input->data[ExtensionHeadersLength+2]) , 2);
		proto = bnep16_to_cpu(proto);
		BT_DBG("Protocol: 0x%.4x (802.1p)", proto);
	} else {
		BT_DBG("Protocol: 0x%x", bnep16_to_cpu(eth.h_proto));
		proto = bnep16_to_cpu(eth.h_proto);
	}
	
	//Send data to own ethernet stack
	if (skb_input->len < ExtensionHeadersLength) 
		goto packet_length_error;
	else {
		struct sk_buff *skb;
		int PayloadLength = skb_input->len - ExtensionHeadersLength;

		//Mem alloc check
		if (!(skb = alloc_skb(PayloadLength + ETH_HLEN + 2, GFP_ATOMIC)))
			return -ENOMEM;

		//Set package header/tail - room in target frame 
		//(+2 => allignement of ip header with byte border)
		skb_reserve(skb, ETH_HLEN + 2);

		//Copy data and header to new frame
		memcpy(skb_put(skb, PayloadLength), &(skb_input->data[ExtensionHeadersLength]),
		       PayloadLength);
		memcpy(skb_push(skb, ETH_HLEN), &eth, ETH_HLEN);

		//Sending ethernet packet to device driver layer
		device_rx_data(skb, bnep_conn->net_dev);
	}
	
#ifdef PANU_ONLY
	kfree_skb(skb_input);
#else
	if (PacketType == BNEP_GENERAL_ETHERNET || PacketType == BNEP_COMPRESSED_ETHERNET_DEST_ONLY) {
		//forward only ethernet and dest_only packets
		if (priv->connection_counter > 1) {
			//Forward packet to other nodes
			bnep_tx_etherdata_multiplexor(bnep_conn->net_dev, bnep_conn, skb_input, &eth , proto
						   , ExtensionHeadersLength, BNEP_PACKET_TYPE_AUTO);
		} else 
			kfree_skb(skb_input);
	} else {
		//Delete input packet
		kfree_skb(skb_input);
	}
#endif
	return 0;

packet_length_error:
	BT_ERR("Illegal packet length info in data packet or extension header");
	kfree_skb(skb_input);
	return 1;	
}

/***************************************************************************/
static int bnep_execute_controldata(struct bnep_connection *bnep_conn, struct sk_buff *skb_input ,
				int packet_length)
{
	__u8 ControlType;

	//Validate packet length
	if (skb_input -> len < 1) goto packet_length_error;
	//Copy control type and remove info from header
	memcpy(&ControlType, skb_input->data, 1);
	skb_pull(skb_input, 1);
	packet_length--;

	//Process control packet
	BT_DBG("BNEP Control type 0x%.2x", ControlType);
	switch (ControlType) {
	case BNEP_SETUP_CONNECTION_REQUEST_MSG:
		return bnep_rx_connect_req(bnep_conn, skb_input);
		break;
		
	case BNEP_SETUP_CONNECTION_RESPONSE_MSG:
		return bnep_rx_connect_rsp(bnep_conn, skb_input);
		break;

	case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
		return bnep_rx_unknown_controldata(bnep_conn, skb_input);
		break;

	case BNEP_FILTER_NET_TYPE_SET_MSG:
		if (atomic_read(&bnep_conn -> status) == BNEP_CONN_CLOSED) goto connection_not_ready;
		return bnep_rx_net_filter(bnep_conn, skb_input);
		break;
		
	case BNEP_FILTER_NET_TYPE_RESPONSE_MSG:
		if (atomic_read(&bnep_conn -> status) == BNEP_CONN_CLOSED) goto connection_not_ready;
		return bnep_rx_net_filter_rsp(bnep_conn, skb_input);
		break;

	case BNEP_FILTER_MULT_ADDR_SET_MSG:
		if (atomic_read(&bnep_conn -> status) == BNEP_CONN_CLOSED) goto connection_not_ready;
		return bnep_rx_multicast_filter(bnep_conn, skb_input);
		break;

	case BNEP_FILTER_MULT_ADDR_RESPONSE_MSG:
		if (atomic_read(&bnep_conn -> status) == BNEP_CONN_CLOSED) goto connection_not_ready;
		return bnep_rx_multicast_filter_rsp(bnep_conn, skb_input);
		break;
	default:
		//Send UNKNOWN COMMAND response packet
		bnep_tx_unknown_controldata(bnep_conn , ControlType);
		if (packet_length != -1 && packet_length < skb_input->len) {
			//Remove remainings of packet if length is known
			BT_DBG("Unknown control (ignore 0x%x bytes)", packet_length);
			skb_pull(skb_input , packet_length);
		}
		else 
			goto packet_length_error;
		break;
	}
	
	return 0;
	
connection_not_ready:
	BT_DBG("Connection not ready => drop data packet");
	return 0;
	
packet_length_error:	
	BT_ERR("Unknown control or illegal packet length info => skip remaining parts");
	return 1;

}

/******************************************************************************/
static int bnep_rx_controldata(struct bnep_connection *bnep_conn, struct sk_buff *skb_input,
				int ExtensionFlag)
{
	int ExtensionHeadersLength;
	int status = 0;

	//Execute command
	if (bnep_execute_controldata(bnep_conn , skb_input , -1)) {
		BT_ERR("Error evaluating control packet => skip extension header");
		status = 1;	
	}
	else {
		if (ExtensionFlag) {
			//Evaluate extension header
			bnep_rx_extension_header(bnep_conn, skb_input , &ExtensionHeadersLength);
		}
	}
	//Delete input packet
	dev_kfree_skb(skb_input);
	return status;
}

/***************************************************************************/
static int bnep_rx_extension_header(struct bnep_connection *bnep_conn, struct sk_buff *skb_input,
				      int *ExtensionsLength)
{
	__u8 ExtensionType;
	__u8 ExtensionPayloadLength;
	int ExtensionFlag;

	//Init length of unknown header
	*ExtensionsLength = 0;
	//Evaluate all header and execute known extension header
	do {
		//Validate header lenght
		if (skb_input->len < 2) goto packet_length_error;
		//Copy extension type[extension flag]
		memcpy(&ExtensionType, skb_input->data, 1);
		ExtensionFlag = ExtensionType & 0x80;
		//Copy extension length
		memcpy(&ExtensionPayloadLength, &skb_input->data[1], 1);
		if ((ExtensionType & 0x7F) == BNEP_EXTENSION_CONTROL) {
			BT_DBG("Control extension header");
			//Remove type & length
			skb_pull(skb_input, 2);
			//Execute command in packet
			if (bnep_execute_controldata(bnep_conn , skb_input , ExtensionPayloadLength))	
				goto packet_length_error;
		} 
		else {
			//Ignore all extensions after first unknown extension was found
			BT_DBG("Unknown extension header: type: 0x%x length: 0x%x (%d)",
				(ExtensionType & 0x7F), ExtensionPayloadLength, ExtensionPayloadLength);
			//Compute length of unknown extension
			*ExtensionsLength = ExtensionPayloadLength + 2;
			while (ExtensionFlag) {
				//Length of first header
				//Validate extension length
				if (*ExtensionsLength + 2 > skb_input->len) goto packet_length_error;
				memcpy(&ExtensionType, &skb_input->data[*ExtensionsLength], 1);
				ExtensionFlag = ExtensionType & 0x80;
				memcpy(&ExtensionPayloadLength,
					&skb_input->data[*ExtensionsLength + 1], 1);
				BT_DBG("Unknown extension: type: 0x%x length: 0x%x (%d)",
					(ExtensionType & 0x7F), ExtensionPayloadLength , ExtensionPayloadLength);
				*ExtensionsLength += ExtensionPayloadLength + 2;
			}
		}
	} while (ExtensionFlag);

	return 0;
	
packet_length_error:
	BT_ERR("Illegal or unknown packet length info in extension header");
	return 1;	
}


/***************************************************************************/
static int bnep_rx_multicast_filter(struct bnep_connection *bnep_conn, struct sk_buff *skb_input)
{
	__u16 list_length;
	
	struct sk_buff *skb;
	__u16 response_code = MCFILTER_OPERATION_SUCCESSFUL;

#ifdef FILTER_SUPPORT
	struct multicast_range *new_filter = NULL;
	int counter;
#endif

	//Validate control packet lemgth
	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
	if (list_length > skb_input->len) goto packet_length_error;
	
#ifndef FILTER_SUPPORT
	response_code = cpu_to_bnep16(MCFILTER_UNSUPPORTED_REQUEST);
	skb_pull(skb_input, list_length);
#else
	if (list_length == 0) {
		BT_DBG("Resetting filter");
		goto set_multicast_filter;
	} 
		
	if (list_length % 12 != 0) {
		BT_ERR("Filter list lenght error: wrong number of addresses (0x%x) => skip filter",
				list_length);
		skb_pull(skb_input, list_length);
		return 0;
	}
	
#ifdef BNEP_TEST
		if (test_ignore_multicastfilter == TESTCASE_IGNORE_MESSAGE_ON) {
			BT_DBG("Ignore multicast filter activated (response will not be sent)");
		skb_pull(skb_input, list_length);
		return 0;
	}
#endif

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

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

		BT_DBG("Filter %d: 0x%.4x%.8x - 0x%.4x%.8x (%s)", counter,
				((__u32 *)&new_filter[counter].start)[1],
				((__u32 *)&new_filter[counter].start)[0],
				((__u32 *)&new_filter[counter].stop)[1],
				((__u32 *)&new_filter[counter].stop)[0] ,
				(new_filter[counter].start <= new_filter[counter].stop) ? "valid" : "invalid");
				
		if (new_filter[counter].start > new_filter[counter].stop) {
			response_code = cpu_to_bnep16(MCFILTER_FAILD_INVALID_MC_ADDR);
		}