bnep_core.c

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

/*
  BlueNIC - Bluetooth PAN profile implementation for BlueZ
  Copyright (C) 2002 Sony Corporation
  
  Author: Johannes Loebbert <[e-mail]>

  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License, version 2, as
  published by the Free Software Foundation.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
*/

/* 
  ChangeLog:
  2001/04/17 created by Johannes Loebbert
*/

/*
 * $Id: bnep_core.c,v 1.14 2002/08/20 08:46:47 loebb1 Exp $
*/

#define __KERNEL_SYSCALLS__

#include <linux/config.h>
#include <linux/module.h>

#include <linux/slab.h>
#include <linux/list.h>
#include <linux/smp_lock.h>
#include <linux/proc_fs.h>
#include <asm/uaccess.h>

#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/l2cap.h>
#include <net/bluetooth/hci_core.h>

#include "bnep_common.h"
#include "bnep_core.h"

#ifndef CONFIG_BNEP_DEBUG
#undef  BT_DBG
#define BT_DBG( A... )
#endif

//Counter for loaded eth-interfaces
static int bnep_dev_counter;
static struct bnep_owner_struct bnep_owner;
struct list_head devices;

static DECLARE_RWSEM (bnep_dev_sem);
static DECLARE_RWSEM (bnep_net_sem);


/***************************************************************************/
#ifdef BNEP_TEST
 int test_ignore_connect;
 int test_ignore_netfilter;
 int test_ignore_multicastfilter;
#endif


/***************************************************************************/
struct bnep_connection *bnep_get_conn_by_bdaddr(struct net_device *dev, bdaddr_t * target_addr)
{
#ifndef PANU_ONLY
	struct list_head *ptr = NULL;
	struct bnep_connection *bnep_conn;
#endif
	bdaddr_t addr;
	struct bnep_private *priv = bnep_get_priv(dev);
	baswap(&addr, target_addr);
	
#ifdef PANU_ONLY
	if (priv ->bnep_conn && bacmp(&bluez_pi(priv->bnep_conn->sock->sk)->dst, &addr) == 0)
		return priv ->bnep_conn;
	else 
		return NULL;
#else
	down_read(&priv->bnep_conn_sem);
	list_for_each (ptr , &priv->bnep_connections) {
		bnep_conn = list_entry(ptr, struct bnep_connection, list);
		if (bacmp(&bluez_pi(bnep_conn->sock->sk)->dst, &addr) == 0) {
			up_read(&priv->bnep_conn_sem);
			return bnep_conn;
		}
	}
	up_read(&priv->bnep_conn_sem);
#endif
	BT_DBG("Connection %s not found in connection table", batostr(target_addr));
	return NULL;
}

/***************************************************************************/
static int bnep_rx_multicast_filter_rsp(struct bnep_connection *bnep_conn,
					struct sk_buff *skb_input)
{
	__u16 ResponseMessage;

	//Validate packet length
	if (skb_input->len < 2) goto packet_lenght_error;
	//Copy response code and remove info from header
	memcpy(&ResponseMessage, skb_input->data, 2);
	ResponseMessage = bnep16_to_cpu(ResponseMessage);
	skb_pull(skb_input, 2);
	 
	BT_DBG("Multicast set control rsp: 0x%.4x", ResponseMessage);

	if (bnep_conn->filter_request.req_status == BNEP_REQ_PENDING) {
		bnep_conn->filter_request.req_status = BNEP_REQ_DONE;
		bnep_conn->filter_request.req_result = ResponseMessage;
		wake_up_interruptible(&(bnep_conn->filter_request.req_wait_q));
	} else {
		BT_DBG("No request registered");
	}
	return 0;

packet_lenght_error:
	BT_ERR("Packet too short => discard");
	return 1;
}

/***************************************************************************/
static int bnep_rx_net_filter_rsp(struct bnep_connection *bnep_conn, struct sk_buff *skb_input)
{

	__u16 ResponseMessage;
	//Validate packet length
	if (skb_input->len < 2) goto packet_lenght_error;
	//Copy response code and remove info from header
	memcpy(&ResponseMessage, skb_input->data, 2);
	ResponseMessage = bnep16_to_cpu(ResponseMessage);
	skb_pull(skb_input, 2);

	BT_DBG("Protocol filter set control rsp: 0x%.4x", ResponseMessage);

	if (bnep_conn->filter_request.req_status == BNEP_REQ_PENDING) {
		bnep_conn->filter_request.req_status = BNEP_REQ_DONE;
		bnep_conn->filter_request.req_result = ResponseMessage;
		wake_up_interruptible(&(bnep_conn->filter_request.req_wait_q));
	} else {
		BT_DBG("No request registered");
	}
	return 0;

packet_lenght_error:
	BT_ERR("Packet too short => discard");
	return 1;

}

/***************************************************************************/
static int bnep_rx_unknown_controldata(struct bnep_connection *bnep_conn, struct sk_buff *skb_input)
{
	__u8 unknown_command;

	//Validate packet length
	if (skb_input->len < 1) goto packet_length_error;
	//Copy command code and remove info from header
	memcpy(&unknown_command, skb_input->data, 1);
	skb_pull(skb_input, 1);

	BT_DBG("Unknown command: 0x%x", unknown_command);

	return 0;

packet_length_error:
	BT_ERR("Packet too short => discard");
	return 1;
}

/***************************************************************************/
static int bnep_tx_unknown_controldata(struct bnep_connection *bnep_conn, 
			__u8 unknown_control_code)
{
	struct sk_buff *skb;
	int packet_size = 3;

	//Create control packet
	if (!(skb = alloc_skb(packet_size, GFP_ATOMIC))) return -ENOMEM;
	//Reserve space for control header 
	skb_reserve(skb, 2);
	//Insert unknown command
	memcpy(skb_put(skb, 1), &unknown_control_code, 1);
	//Add bnep header and send message    
	bnep_tx_controldata(bnep_conn, BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD, 0, skb);

	return 0;
}

/***************************************************************************/
/***************************************************************************/
int bnep_tx_controldata(struct bnep_connection *bnep_conn, __u8 ControlType, int ExtensionFlag,
			struct sk_buff *skb)
{
	if (!bnep_conn) {
		BT_DBG("Attempt to send on non existing connection");
		dev_kfree_skb(skb);
		return 1;
	}
#ifndef BNEP_TEST
	if (atomic_read(&bnep_conn -> status) == BNEP_CONN_CLOSED && 
		ControlType != BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD &&
		ControlType != BNEP_SETUP_CONNECTION_RESPONSE_MSG &&
		ControlType != BNEP_SETUP_CONNECTION_REQUEST_MSG) {
		BT_DBG("Attempt to send to closed connection");
		dev_kfree_skb(skb);
		return 1;
	}
#endif

	//Check headroom
	if (skb_headroom(skb) < 2) {
		BT_ERR("Not enough headspace to insert control packet header");
		dev_kfree_skb(skb);
	} else {
		__u8 bnep_header = BNEP_CONTROL;
		__u8 control_type = ControlType;

		if (ExtensionFlag) {
			BT_DBG("Set extension flag");
			bnep_header = bnep_header | 0X80;
		}
		//Add control type
		memcpy(skb_push(skb, 1), &control_type, 1);
		//Add bnep type with extension flag
		memcpy(skb_push(skb, 1), &bnep_header, 1);
		skb_queue_tail(&bnep_conn->sock->sk->write_queue, skb);
		wake_up_interruptible(bnep_conn->sock->sk->sleep);
	}
	return 0;
}

/***************************************************************************/
static int bnep_tx_etherdata(struct bnep_connection *bnep_conn, struct bnep_connection *entry,
			       struct sk_buff *skb_input, struct ethhdr *eth,
			       int ExtensionHeaderLength, __u8 packet_type , int forward_mode)
{
	
	struct sk_buff *skb = NULL;
	__u8 bnep_header;
	bdaddr_t tmp_addr;
	__u16 proto = eth->h_proto;
	int pkt_length;
#ifndef PANU_ONLY
	int hdr_len = 1 + 2*ETH_ALEN + 2;
#endif
	if (!entry || atomic_read(&entry -> status) == BNEP_CONN_CLOSED) {
		BT_DBG("Attempt to send to non existing or closed connection");
		return 1;
	}

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

#ifdef PANU_ONLY
	//PANU only reuses send skb
	skb = skb_input;
#else
	if (forward_mode == FORWARD_MODE_EXT_HEADER)
		pkt_length = hdr_len + ExtensionHeaderLength + ((eth->h_proto == 0x0081)?4:0);
	else	
		pkt_length = hdr_len + skb_input -> len;

	if (!(skb = alloc_skb(pkt_length, GFP_ATOMIC)))
			return -ENOMEM;
	skb_reserve(skb , hdr_len);

	if (forward_mode == FORWARD_MODE_EXT_HEADER) {
		//Copy only [ext_header]802_1q
		if (eth->h_proto == 0x0081) {
			memcpy(skb_put(skb, ExtensionHeaderLength+2),skb_input->data, ExtensionHeaderLength+2);
			//Add packet lenght: zero
			memset(skb_put(skb , 2) , 0 , 2);
		}
		else  {
			//Copy only [ext_header]
			memcpy(skb_put(skb, ExtensionHeaderLength),skb_input->data, ExtensionHeaderLength);
			proto = 0x0000;
		}
	}
	else {
		//Copy payload
		memcpy(skb_put(skb, skb_input->len), skb_input->data, skb_input->len);
	}
#endif

	//Packet type selector
	if (packet_type == BNEP_PACKET_TYPE_AUTO) {
		//Automatic compression detection
#ifndef PANU_ONLY
		if (bnep_conn == NULL) {
#endif
			//Packet was created by local node
			bdaddr_t bd_source_addr;
			baswap(&bd_source_addr, &bluez_pi(entry->sock->sk)->src);
			if (memcmp(eth->h_source, &bd_source_addr, ETH_ALEN) != 0) {
				//Issued from the bridge code => must be general ethernet
				packet_type = BNEP_GENERAL_ETHERNET;
			} else if (memcmp(eth->h_dest, &tmp_addr, ETH_ALEN) == 0) {
				//Local issued packet to next hop => compressed
				packet_type = BNEP_COMPRESSED_ETHERNET;
			} else {
				//Local issued packet => Dest. only
				packet_type = BNEP_COMPRESSED_ETHERNET_DEST_ONLY;
			}
#ifndef PANU_ONLY
		} else {
			//Packet forwarding code
			if (memcmp(eth->h_dest, &tmp_addr, ETH_ALEN) == 0) {
				//Delivering to final destination => GeneralEthernet Source Only
				packet_type = BNEP_COMPRESSED_ETHERNET_SOURCE_ONLY;
			} else {
				//Delivered to general node
				packet_type = BNEP_GENERAL_ETHERNET;
			}
		}
#endif
	}

	//Insert ethernet protocol type
	memcpy(skb_push(skb, 2), &proto , 2);
	//Insert different bnep header
	switch (packet_type) {
	case BNEP_GENERAL_ETHERNET:
		BT_DBG("Packet type: General Ethernet");
		memcpy(skb_push(skb, 6), eth->h_source, 6);
		memcpy(skb_push(skb, 6), eth->h_dest, 6);
		bnep_header = BNEP_GENERAL_ETHERNET;
		break;
	case BNEP_COMPRESSED_ETHERNET:
		BT_DBG("Packet type: Compressed Ethernet");
		bnep_header = BNEP_COMPRESSED_ETHERNET;
		break;
	case BNEP_COMPRESSED_ETHERNET_SOURCE_ONLY:
		BT_DBG("Packet type: Compressed Ethernet Source only");
		memcpy(skb_push(skb, 6), eth->h_source, ETH_ALEN);
		bnep_header = BNEP_COMPRESSED_ETHERNET_SOURCE_ONLY;
		break;
	case BNEP_COMPRESSED_ETHERNET_DEST_ONLY:
		BT_DBG("Packet type: Compressed Ethernet Destination only");
		memcpy(skb_push(skb, 6), eth->h_dest, ETH_ALEN);
		bnep_header = BNEP_COMPRESSED_ETHERNET_DEST_ONLY;
		break;
	}

	//Modify for extension flag
	if (ExtensionHeaderLength)
		bnep_header = bnep_header | 0x80;

	//Adding BNEP header
	memcpy(skb_push(skb, 1), &bnep_header, 1);

	if (entry->sock) {
		BT_DBG("Source address: %s", batostr((bdaddr_t *) eth->h_source));
		BT_DBG("Dst. address:   %s", batostr((bdaddr_t *) eth->h_dest));
		BT_DBG("Protocol:       0x%x", bnep16_to_cpu(proto));

		skb_queue_tail(&entry->sock->sk->write_queue, skb);
		wake_up_interruptible(entry->sock->sk->sleep);
	} else {
		BT_DBG("sock not connected");
		kfree_skb(skb);
	}

	return 0;
}

#ifdef FILTER_SUPPORT
/***************************************************************************/
static int bnep_tx_apply_multicast_filter(struct bnep_connection *bnep_conn, struct ethhdr *eth)
{
	int counter = 0;
	__u64 addr = 0;

	if (!bnep_conn->filter_info.MulticastFilter) 
		return 1;

	for (counter = 0; counter < 6; counter++)
		((__u8 *) & addr)[counter] = eth->h_dest[5 - counter];

	for (counter = 0; counter < bnep_conn->filter_info.multicast_filter_count;
		 counter++) {
		BT_DBG("Filter %d: Value: 0x%.4x%.8x Filter: 0x%.4x%.8x - 0x%.4x%.8x", counter,
			((__u32 *) & addr)[1], ((__u32 *) & addr)[0],
			((__u32 *) & bnep_conn->filter_info.MulticastFilter[counter].start)[1],
			((__u32 *) & bnep_conn->filter_info.MulticastFilter[counter].start)[0],
			((__u32 *) & bnep_conn->filter_info.MulticastFilter[counter].stop)[1],
			((__u32 *) & bnep_conn->filter_info.MulticastFilter[counter].stop)[0]);

		if (bnep_conn->filter_info.MulticastFilter[counter].start <= addr) {
			if (addr <= bnep_conn->filter_info.MulticastFilter[counter].stop)
				return 1;
		}
	}
	
	return 0;
}
#endif


/***************************************************************************/
#ifdef FILTER_SUPPORT
static int bnep_tx_apply_protocol_filter(struct bnep_connection *bnep_conn, __u16 protocol)
{
	int counter;

	if (!bnep_conn->filter_info.NetFilter) 
		return 1;

	for (counter = 0; counter < bnep_conn->filter_info.net_filter_count; counter++) {
		BT_DBG("Filter %d: 0x%.4x - 0x%.4x Value: 0x%.4x", counter,
				bnep_conn->filter_info.NetFilter[counter].start,
				bnep_conn->filter_info.NetFilter[counter].stop, protocol);
		if (bnep_conn->filter_info.NetFilter[counter].start <= protocol
			&& protocol <= bnep_conn->filter_info.NetFilter[counter].stop)
			return 1;
	}

	return 0;
}