bnep_core.c

linux下蓝牙

/*
  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.13 2002/07/08 14:28:08 johanloe 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 "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;

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

struct semaphore bnep_mc_send_sem;
kthread_t bnep_mc_send_thread;

/**********************************************************************/
struct net_device *root_dev;

/**********************************************************************/
#ifdef CONFIG_BLUEZ_DEBUG
static unsigned long rx_counter = 0;
static unsigned long tx_counter = 0;
#endif

/***************************************************************************/
struct bnep_connection *bnep_get_conn_by_bdaddr(struct net_device *dev, bdaddr_t * target_addr)
{
	struct list_head *ptr;
	struct bnep_connection *entry;
	struct bnep_connection *conn = NULL;
	bdaddr_t addr;
	struct bnep_private *priv = bnep_get_priv(dev);

	baswap(&addr, target_addr);

	for (ptr = priv->bnep_connections.next; ptr != &priv->bnep_connections; ptr = ptr->next) {
		entry = list_entry(ptr, struct bnep_connection, list);
		if (bacmp(&bluez_pi(entry->sk->sk)->dst, &addr) == 0) {
			conn = entry;
			break;
		}
	}

	if (conn == NULL) {
		BT_DBG("connection %s not found in connection table", batostr(target_addr));
	}

	return conn;
}


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

	//Copy response code and remove info from header
	if (skb_input->len < 2) {	
		BT_ERR("packet to short => discard");
		return 1;
	}
	memcpy(&ResponseMessage, skb_input->data, 2);
	skb_pull(skb_input, 2);
	
	BT_DBG("multicast set control rsp: 0x%.4x", bnep16_to_cpu(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("connection not waiting for filter response => discard");
	}

	return 0;
}

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

	//Copy response code and remove info from header
	if (skb_input->len < 2) {	
		BT_ERR("packet to short => discard");
		return 1;
	}
	memcpy(&ResponseMessage, skb_input->data, 2);
	skb_pull(skb_input, 1);

	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("connection not waiting for filter response => discard");
	}
	return 0;
}

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

	//Copy control type and remove info from header
	if (skb_input->len < 1) {	
		BT_ERR("packet to short => discard");
		return 1;
	}
	memcpy(&unknown_command, skb_input->data, 1);
	skb_pull(skb_input, 1);

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

	return 0;
}

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

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

	if (skb_headroom(skb) < 2) {
		//Not enough headroom for the two bytes for a control packet
		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->sk->sk->write_queue, skb);
		wake_up_interruptible(bnep_conn->sk->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, __u16 * h802_1p,
			       int ExtensionFlag, __u8 packet_type)
{
	struct sk_buff *skb = NULL;
	__u16 proto;
	__u8 bnep_header;
	bdaddr_t tmp_addr;
	__u8 header_size = 1 + 2 * 6 + 2;

	if (!entry || entry -> status == BNEP_CONN_CLOSED) {
		BT_DBG("Attempt to send on non existing or closed connection");
		return 1;
	}

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

	if (h802_1p) {
		//Increase header size for 802.1p header
		header_size += 4;
	}
	//Prepare packet
	if (!(skb = alloc_skb(skb_input->len + header_size, GFP_ATOMIC)))
		return -ENOMEM;
	skb_reserve(skb, header_size);

	//Copy packet data
	memcpy(skb_put(skb, skb_input->len), skb_input->data, skb_input->len);

	//Insert packet type
	if (h802_1p) {
		//Insert real protocol
		proto = (__u16) eth->h_proto;
		memcpy(skb_push(skb, 2), &proto, 2);
		//Insert 802.1p header
		memcpy(skb_push(skb, 2), &h802_1p, 2);
		//Insert 801.p protocol
		proto = 0x0081;
		memcpy(skb_push(skb, 2), &proto, 2);
	} else {
		proto = (__u16) eth->h_proto;
		memcpy(skb_push(skb, 2), &proto, 2);
	}

	if (packet_type == BNEP_PACKET_TYPE_AUTO) {
		//Automatic compression detection
		if (bnep_conn == NULL) {
			//Packet was created by local node
			bdaddr_t bd_source_addr;
			baswap(&bd_source_addr, &bluez_pi(entry->sk->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;
			}
		} 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;
			}
		}
	}

	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 (ExtensionFlag)
		bnep_header = bnep_header | 0x80;

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

	if (entry->sk) {
		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 %s", bnep16_to_cpu(eth->h_proto),
				(h802_1p) ? "(802.1p)" : "");

		//memcpy(skb -> cb , &entry -> sk , sizeof(struct socket*));
		skb_queue_tail(&entry->sk->sk->write_queue, skb);
		wake_up_interruptible(entry->sk->sk->sleep);
	} else {
		BT_DBG("transmission error. SK not connected");
		kfree_skb(skb);
	}

	return 0;
}

/***************************************************************************/
static int bnep_tx_apply_multicast_filter(struct bnep_connection *bnep_conn, struct ethhdr *eth)
{
	int result = 0;

	if (bnep_conn->filter_info.MulticastFilter) {
		int counter = 0;
		__u64 addr = 0;
		result = 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 0;
			}
		}
	}
	return result;
}

/***************************************************************************/
static int bnep_tx_apply_protocol_filter(struct bnep_connection *bnep_conn, struct ethhdr *eth)
{
	int result = 0;

	if (bnep_conn->filter_info.NetFilter) {
		int counter = 0;
		__u16 protocol = bnep16_to_cpu(eth->h_proto);
		result = 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 0;
		}
	}
	return result;
}

/***************************************************************************/
//Processing packet for BT domain
int bnep_tx_etherdata_multiplexor(struct net_device *dev, struct bnep_connection *bnep_conn,
			       struct sk_buff *skb_input, struct ethhdr *eth, __u16 *h802_1p,
			       int ExtensionFlag, __u8 packet_type)
{
	//eth is just a reference don't free
	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);

	for (ptr = priv->bnep_connections.next; ptr != &priv->bnep_connections; ptr = ptr->next) {
		entry = list_entry(ptr, struct bnep_connection, list);
		baswap(&tmp_addr, &bluez_pi(entry->sk->sk)->dst);

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

		//Initial setting