bnep_test.c

linux下蓝牙

/*
  BlueNIC - Bluetooth PAN profile implementation for BlueZ
  bnep_test.c - test command for UPF
  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_test.c,v 1.4 2002/08/04 21:23:58 maxk 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/sock.h>
#include <linux/etherdevice.h>
#include <linux/timer.h>

#include "bnep_common.h"

#define MAX_EXTENSION_HEADER 10

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

#ifdef BNEP_TEST
/**********************************************************************/

extern struct filter_request_struct filter_request;

extern int test_ignore_connect;
extern int test_ignore_netfilter;
extern int test_ignore_multicastfilter;

/************************************************************************/
/************************************************************************/
struct test_packet_struct {
	int packet_type;
	int ExtensionCounter;
	struct sk_buff *payload;
	struct sk_buff *Extension[MAX_EXTENSION_HEADER];
	__u8 ExtensionType[MAX_EXTENSION_HEADER];
	__u8 control_value;
	__u8 ext_control_value[MAX_EXTENSION_HEADER];
};

static struct test_packet_struct test_packet;

/************************************************************************/
/************************************************************************/
static int test_bnep_packet_add_control(struct sk_buff **skb,
					struct bnep_test_ioctl_control_opt_struct
					*bnep_test_ioctl_control_opt)
{

	if (!(*skb = alloc_skb(bnep_test_ioctl_control_opt->length, GFP_ATOMIC)))
		return -ENOMEM;

	memset(skb_put(*skb, bnep_test_ioctl_control_opt->length), 0,
	       bnep_test_ioctl_control_opt->length);
	return 0;
}

/************************************************************************/
/************************************************************************/
static int test_bnep_packet_add_netfilter(struct sk_buff **skb,
					  struct filter_request_struct *filter_request)
{

	__u16 list_size;
	int packet_size = 2 + filter_request->data_length;

	//Alloc memory
	if (!(*skb = alloc_skb(packet_size, GFP_ATOMIC)))
		return -ENOMEM;

	//Insert list length
	list_size = cpu_to_bnep16(filter_request->data_length);
	memcpy(skb_put(*skb, 2), &list_size, 2);

	//Copy request values
	if (filter_request->data_length > 0) {
		unsigned char *data =
		    (unsigned char *) kmalloc(filter_request->data_length, GFP_ATOMIC);
		copy_from_user(data, filter_request->data, filter_request->data_length);
		memcpy(skb_put(*skb, filter_request->data_length), data,
		       filter_request->data_length);
		kfree(data);
	}

	return 0;
}

/************************************************************************/
/************************************************************************/
static int test_bnep_packet_add_unknown_header(struct sk_buff **skb,
					       struct bnep_test_ioctl_unknown_header_opt_struct
					       *bnep_test_ioctl_unknown_header_opt)
{
	if (!(*skb = alloc_skb(bnep_test_ioctl_unknown_header_opt->length, GFP_ATOMIC)))
		return -ENOMEM;

	memset(skb_put(*skb, bnep_test_ioctl_unknown_header_opt->length), 0,
	       bnep_test_ioctl_unknown_header_opt->length);
	return 0;
}

/************************************************************************/
/************************************************************************/
static int test_bnep_packet_add_multicastfilter(struct sk_buff **skb,
						struct filter_request_struct *filter_request)
{
	__u16 list_size;
	int packet_size = 2 + filter_request->data_length;

	//Alloc memory
	if (!(*skb = alloc_skb(packet_size, GFP_ATOMIC)))
		return -ENOMEM;

	//Insert list length
	list_size = cpu_to_bnep16(filter_request->data_length);
	memcpy(skb_put(*skb, 2), &list_size, 2);

	//Copy request values
	if (filter_request->data_length > 0) {
		unsigned char *data =
		    (unsigned char *) kmalloc(filter_request->data_length, GFP_ATOMIC);
		copy_from_user(data, filter_request->data, filter_request->data_length);
		memcpy(skb_put(*skb, filter_request->data_length), data,
		       filter_request->data_length);
		kfree(data);
	}

	return 0;
}

/************************************************************************/
/************************************************************************/
static int test_bnep_packet_free(void)
{
	int counter;
	test_packet.packet_type = TEST_PAYLOAD_DATA;
	if (test_packet.payload)
		dev_kfree_skb(test_packet.payload);
	test_packet.payload = NULL;
	for (counter = 0; counter < 10; counter++) {
		if (test_packet.Extension[counter])
			dev_kfree_skb(test_packet.Extension[counter]);
		test_packet.Extension[counter] = NULL;
	}
	test_packet.ExtensionCounter = 0;

	return 0;
}

/************************************************************************/
/************************************************************************/
int test_bnep_packet_prepare(void)
{
	int counter;

	test_packet.packet_type = TEST_PAYLOAD_DATA;
	test_packet.payload = NULL;
	for (counter = 0; counter < 10; counter++)
		test_packet.Extension[counter] = NULL;
	test_packet.ExtensionCounter = 0;
	return 0;
}

/************************************************************************/
/************************************************************************/
static int test_bnep_packet_add_payload_connect(struct sk_buff **skb,
			struct bnep_test_ioctl_connect_opt_struct *bnep_test_ioctl_connect_opt)
{
	__u8 uuid_length = UUID16;
	__u16 packet_size = 1 + 2 * uuid_length;
	__u16 source_uuid = cpu_to_bnep16(bnep_test_ioctl_connect_opt->source_uuid);
	__u16 dest_uuid = cpu_to_bnep16(bnep_test_ioctl_connect_opt->destination_uuid);

	//Create packet
	if (!(*skb = alloc_skb(packet_size, GFP_ATOMIC)))
		return -ENOMEM;

	//Insert connection request data
	memcpy(skb_put(*skb, 1), &uuid_length, 1);
	memcpy(skb_put(*skb, uuid_length), &dest_uuid, uuid_length);
	memcpy(skb_put(*skb, uuid_length), &source_uuid, uuid_length);

	test_packet.packet_type = TEST_PAYLOAD_CONNECT_CONTROL;
	return 0;
}

/************************************************************************/
/************************************************************************/
static int test_bnep_packet_send(struct net_device *net_dev,
				 struct bnep_test_ioctl_send_struct *bnep_test_ioctl_send)
{
	int counter = 0;
	int PacketSize = 0;
	int ExtensionLength = 0;
	struct ethhdr eth;
	struct sk_buff *skb;
	__u8 value8;

	//Len of extensions
	for (counter = 0; counter < test_packet.ExtensionCounter; counter++)
		ExtensionLength += (test_packet.Extension[counter]->len + 3);
	PacketSize += ExtensionLength;

	//Len of payload
	if (test_packet.payload)
		PacketSize += test_packet.payload->len;

	//Add space for control header
	PacketSize += 2;

	//Mem alloc & check
	if (!(skb = alloc_skb(PacketSize, GFP_ATOMIC)))
		return -ENOMEM;

	//Reserve headspace for control packets
	skb_reserve(skb, 2);

	//Control packet: Payload - Extensions
	if (test_packet.packet_type != TEST_PAYLOAD_DATA) {

		if (test_packet.payload) {
			memcpy(skb_put(skb, test_packet.payload->len), test_packet.payload->data,
			       test_packet.payload->len);
		}
	}

	BT_DBG("Processing %d extension header", test_packet.ExtensionCounter);
	for (counter = 0; counter < test_packet.ExtensionCounter; counter++) {

		BT_DBG("Header (%d): Type :%x Len: 0x%x More Extensions: %s", counter,
				test_packet.ExtensionType[counter],
				test_packet.Extension[counter]->len,
				(counter < test_packet.ExtensionCounter - 1) ? "Yes" : "No");

		value8 = (counter < test_packet.ExtensionCounter - 1) ? 0x80 : 0x00;

		if (test_packet.ExtensionType[counter] == BNEP_FILTER_NET_TYPE_SET_MSG
		    || test_packet.ExtensionType[counter] == BNEP_FILTER_MULT_ADDR_SET_MSG
		    || test_packet.ExtensionType[counter] == BNEP_UNKNOWN_COMMAND) {
			//Insert control header marker
			value8 |= BNEP_EXTENSION_CONTROL;
			memcpy(skb_put(skb, 1), &value8, 1);

			//Insert length
			value8 = test_packet.Extension[counter]->len + 1;
			memcpy(skb_put(skb, 1), &value8, 1);

			//Insert ext. header type
			value8 = (test_packet.ExtensionType[counter] == BNEP_UNKNOWN_COMMAND)
			    ? test_packet.ext_control_value[counter]
			    : test_packet.ExtensionType[counter];
			memcpy(skb_put(skb, 1), &value8, 1);
		} else {
			//Insert ext. header type
			value8 |= test_packet.ExtensionType[counter];
			memcpy(skb_put(skb, 1), &value8, 1);
			//Insert length
			value8 = test_packet.Extension[counter]->len;
			memcpy(skb_put(skb, 1), &value8, 1);
		}

		//Insert extension header
		memcpy(skb_put(skb, test_packet.Extension[counter]->len),
		       test_packet.Extension[counter]->data, test_packet.Extension[counter]->len);
	}