bnep_core.c

linux下蓝牙

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

		if (send_packet) {
			//Packet type filter
			send_packet = bnep_tx_apply_protocol_filter(entry, eth) ? 0 : 1;
			if (send_packet && *eth->h_dest & 1) {
				//Multicast filter
				send_packet = bnep_tx_apply_multicast_filter(entry, eth) ? 0 : 1;
			}
		}

		if (send_packet)
			bnep_tx_etherdata(bnep_conn, entry, skb_input, eth, h802_1p,
					ExtensionFlag, packet_type);
		else {
			BT_DBG("packet will not be send to %s "
				"due to filter/bridging settings", batostr(&tmp_addr));
		}
	}

	if (h802_1p)
		kfree(h802_1p);

	kfree_skb(skb_input);
	return 0;
}

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

	if (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->sk->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->sk->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 
	BT_DBG("ExtensionHeader: %s", ExtensionFlag ? "Yes" : "No");
	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 + 2) goto packet_length_error; 	
		h802_1p = (__u16 *) kmalloc(2, GFP_ATOMIC);
		//Remove 801.1p header
		memcpy(h802_1p, &skb_input->data[ExtensionHeadersLength], 2);
		ExtensionHeadersLength += 2;
		//Copy real protocol from BNEP header
		memcpy(&(eth.h_proto), &skb_input->data[ExtensionHeadersLength], 2);
		ExtensionHeadersLength += 2;
		BT_DBG("Protocol: 0x%x (802.1p)", bnep16_to_cpu(eth.h_proto));
	} else {
		BT_DBG("Protocol: 0x%x", bnep16_to_cpu(eth.h_proto));
	}

	if (skb_input->len < ExtensionHeadersLength) 
		goto packet_length_error;
	else {
		//Send data payload package to own protocol stack
		struct sk_buff *skb;
		int PayloadLength = skb_input->len - ExtensionHeadersLength;

		//Mem alloc check
		if (!(skb = alloc_skb(PayloadLength + ETH_HLEN + (h802_1p ? 4 : 0) + 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 + (h802_1p ? 4 : 0));

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

		if (h802_1p) {
			//Restore 802.1q header
			memcpy(skb_push(skb , 2) , &eth.h_proto , 2);
			eth.h_proto = 0x0081;
			memcpy(skb_push(skb , 2) , h802_1p , 2);
		}		       
		
		memcpy(skb_push(skb, ETH_HLEN), &eth, ETH_HLEN);

		//Sending ethernet packet to device driver layer
		device_rx_data(skb, bnep_conn->net_dev);
	}

	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,
						   h802_1p, ExtensionFlag, BNEP_PACKET_TYPE_AUTO);
		} else 
			kfree_skb(skb_input);
	} else {
		//Delete input packet
		kfree_skb(skb_input);

		//Delete 802.1p header
		if (h802_1p)
			kfree(h802_1p);
	}

	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;

	//Copy control type and remove info from header
	if (skb_input -> len < 1) goto packet_length_error;
	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:
		if (bnep_conn -> status == BNEP_CONN_CLOSED) goto connection_not_ready;
		return bnep_rx_unknown_controldata(bnep_conn, skb_input);
		break;

	case BNEP_FILTER_NET_TYPE_SET_MSG:
		if (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 (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 (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 (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("illegal/unknwon control 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 {
		BT_DBG("extension header: %s" , ExtensionFlag ? "yes" : "no");
		if (ExtensionFlag) {
			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;
	int ExtensionFlag = 0;
	int error = 0;
	__u8 ExtensionPayloadLength;

	*ExtensionsLength = 0;

	//Evaluate all header and update action packet
	do {
		if (skb_input->len < 2) {
			//Packet to short for extension header => cancel evaluation
			BT_ERR("packet to short to contain extension");
			error = 1;
			break;
		}

		//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("evaluate extension control header");
			
			//Remove type & length
			skb_pull(skb_input, 2);

			//Execute command in packet
			bnep_execute_controldata(bnep_conn , skb_input , ExtensionPayloadLength);
		} 
		else {
			//Ignore all extensions after first unknown extension was found
			BT_DBG("ignore unknown extension: type: 0x%x length: 0x%x (%d)",
				(ExtensionType & 0x7F), ExtensionPayloadLength, ExtensionPayloadLength);
			//Length of first header
			*ExtensionsLength = ExtensionPayloadLength + 2;
			//Iterate over remaining headers
			while (!error && ExtensionFlag) {
				if (*ExtensionsLength + 2 <= skb_input->len) {
					memcpy(&ExtensionType, &skb_input->data[*ExtensionsLength], 1);
					ExtensionFlag = ExtensionType & 0x80;
					memcpy(&ExtensionPayloadLength,
						&skb_input->data[*ExtensionsLength + 1], 1);
					BT_DBG("ignore unknown extension: type: 0x%x length: 0x%x (%d)",
						(ExtensionType & 0x7F), ExtensionPayloadLength , ExtensionPayloadLength);
					*ExtensionsLength += ExtensionPayloadLength + 2;
				} else {
					//Error: Packet is shorter than extension length => Cancel processing
					error = 1;
				}
			}
		}
	} while (ExtensionFlag && !error);

	return error;
}


/***************************************************************************/
static int bnep_rx_multicast_filter(struct bnep_connection *bnep_conn, struct sk_buff *skb_input)
{
	struct multicast_range *new_filter = NULL;
	__u16 list_length;
	int valid_range = 1;
	
	struct sk_buff *skb;
	__u16 response_code = MCFILTER_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_multicast_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 % 6 != 0) {
		BT_ERR("filter list lenght error: odd 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 test case 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);

		valid_range = (new_filter[counter].start <= new_filter[counter].stop &&
					   new_filter[counter].start & 0x010000000000 && 
					   new_filter[counter].stop & 0x010000000000);

		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] ,
				valid_range ? "valid" : "invalid");

				
		if (!valid_range) {
			response_code = cpu_to_bnep16(MCFILTER_FAILD_INVALID_MC_ADDR);
		}		
	}

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

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