sdp.c

blue tooth 核心协议栈在linux上的实现

/****************** INCLUDE FILES SECTION ***********************************/

#define __NO_VERSION__ /* don't define kernel_version in module.h */

#ifdef __KERNEL__
#include <linux/bluetooth/sysdep-2.1.h>
#include <linux/malloc.h>
#include <linux/bluetooth/btcommon.h>
#include <linux/bluetooth/sdp.h>
#include <linux/bluetooth/l2cap.h>
#include <linux/bluetooth/bluetooth.h>
#include <linux/bluetooth/btmem.h>
#include <linux/proc_fs.h>
#include <linux/random.h>
#else /* user mode */
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <asm/unaligned.h>

#include "include/bluetooth.h"
#include "include/btcommon.h"
#include "include/sdp.h"
#include "include/l2cap.h"
#include "include/btmem.h"
#endif

/****************** DEBUG CONSTANT AND MACRO SECTION ************************/

#if SDP_DEBUG_XMIT
#define D_XMIT(fmt...) printk(SDP_DBG_STR fmt)
#else
#define D_XMIT(fmt...)
#endif

#if SDP_DEBUG_REC
#define D_REC(fmt...) printk(SDP_DBG_STR fmt)
#else
#define D_REC(fmt...)
#endif

#if SDP_DEBUG_MISC
#define D_MISC(fmt...) printk(SDP_DBG_STR fmt)
#else
#define D_MISC(fmt...)
#endif

#if SDP_DEBUG_PROC
#define D_PROC(fmt...) printk(fmt)
#else
#define D_PROC(fmt...)
#endif

#if SDP_PRINT_DATA
#define PRINTPKT(str, data, len) print_data(str, data, len)
#else
#define PRINTPKT(str, data, len)
#endif

/****************** CONSTANT AND MACRO SECTION ******************************/

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,2,14)
#define USE_NEW_PROC
#endif

#ifndef __KERNEL__
#define SDP_SRV_SOCK "/tmp/sdp_sock"
#endif

#define GET_TYPE(ch) (((ch) >> 3) & 0x1f)
#define GET_SIZE(ch) ((ch) & 0x7)
#define SET_DE_HDR(type,size) ((((type) << 3) & 0xf8) + ((size) & 0x7))

#define SDP_HDR_SIZE 5
/* The size of the SDP packet header */

#define DES_HDR 0x35
#define DES_HDR_LEN 2

#define UUID16_HDR 0x19
#define UUID32_HDR 0x1a

/* Maximum record counts */
#define MAX_SERVICERECORD_COUNT 8
#define MAX_ATTR_COUNT 0x100

/* The maximum number of sdp connections */
#define MAX_NBR_SDP 7

/* PDU ID */
#define SDP_ERROR_RSP 1
#define SDP_SERVICESEARCH_REQ 2
#define SDP_SERVICESEARCH_RSP 3
#define SDP_SERVICEATTR_REQ 4
#define SDP_SERVICEATTR_RSP 5
#define SDP_SERVICESEARCHATTR_REQ 6
#define SDP_SERVICESEARCHATTR_RSP 7

/****************** TYPE DEFINITION SECTION *********************************/

/****************** LOCAL FUNCTION DECLARATION SECTION **********************/

static sdp_con* get_free_sdp_con(void);

#ifdef __KERNEL__

#ifdef USE_NEW_PROC
static ssize_t sdp_database_read(struct file *f, char *buf, size_t count, 
				 loff_t *offset);
static ssize_t sdp_database_write(struct file *f, const char *buf, 
				  size_t count, loff_t *offset);
#else
static s32 sdp_database_read(struct inode *inode, struct file * file,
			     char * buf, s32 count);
static s32 sdp_database_write(struct inode *inode, struct file * file,
			      const char * buf, s32 count);

#endif
static s32 sdp_proc_dir_entry_read(char *buf, char **start, off_t offset,
				   s32 len, s32 unused);

#else /* USERMODE STACK */
static s32 open_socket(char *name);
static s32 sdp_doquery(s32 fd, u8 *request, s32 len);
static s32 send_error_rsp(sdp_con *sdp, u16 trans_id, u16 err_code);
#endif

/****************** GLOBAL VARIABLE DECLARATION SECTION *********************/

/****************** LOCAL VARIABLE DECLARATION SECTION **********************/

#ifdef __KERNEL__
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0)
static struct wait_queue *database_wq = NULL;
#else
static wait_queue_head_t database_wq;
#endif

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0)
static struct file_operations sdp_procfile_operation = {
	NULL,                   /* lseek - default */
	sdp_database_read,      /* read - bad */
	sdp_database_write,     /* write - bad */
	NULL,                   /* readdir */
	NULL,                   /* select - default */
	NULL,                   /* ioctl - default */
	NULL,                   /* mmap */
	NULL,                   /* no special open code */
	NULL,                   /* no special release code */
	NULL                    /* can't fsync */
};
#else 
static struct file_operations sdp_procfile_operation = {
	NULL,                   /* module owner */
	NULL,                   /* lseek - default */
	sdp_database_read,      /* read - bad */
	sdp_database_write,     /* write - bad */
	NULL,                   /* readdir */
	NULL,                   /* select - default */
	NULL,                   /* ioctl - default */
	NULL,                   /* mmap */
	NULL,                   /* no special open code */
	NULL,                   /* no special release code */
	NULL                    /* can't fsync */
};
#endif
 
 /*
  * proc directories can do almost nothing..
  */
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0)
static struct inode_operations sdp_proc_inode_operations = {
	&sdp_procfile_operation,   /* default net file-ops */
	NULL,                   /* create */
	NULL,                   /* lookup */
	NULL,                   /* link */
	NULL,                   /* unlink */
	NULL,                   /* symlink */
	NULL,                   /* mkdir */
	NULL,                   /* rmdir */
	NULL,                   /* mknod */
	NULL,                   /* rename */
	NULL,                   /* readlink */
	NULL,                   /* follow_link */
	NULL,                   /* readpage */
	NULL,                   /* writepage */
	NULL,                   /* bmap */
	NULL,                   /* truncate */
	NULL                    /* permission */
};
#else
static struct inode_operations sdp_proc_inode_operations = {
	NULL,                   /* create */
	NULL,                   /* lookup */
	NULL,                   /* link */
	NULL,                   /* unlink */
	NULL,                   /* symlink */
	NULL,                   /* mkdir */
	NULL,                   /* rmdir */
	NULL,                   /* mknod */
	NULL,                   /* rename */
	NULL,                   /* readlink */
	NULL,                   /* follow_link */
	NULL,                   /* readpage */
	NULL,                   /* writepage */
	NULL,                   /* bmap */
	NULL,                   /* truncate */
	NULL                    /* permission */
};
#endif

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0)
static struct proc_dir_entry sdp_proc_entry = {
	0,
	7, "sdp_srv",
	S_IFREG | S_IRUGO | S_IWUSR,
	1, 0, 0,
	0,
	&sdp_proc_inode_operations,
	sdp_proc_dir_entry_read,
};
#else
static struct proc_dir_entry sdp_proc_entry = {
	0,
	7, "sdp_srv",
	S_IFREG | S_IRUGO | S_IWUSR,
	1, 0, 0,
	0,
	&sdp_proc_inode_operations,
	&sdp_procfile_operation,
};
#endif /* LINUX_VERSION_CODE */

#endif /* __KERNEL__ */

typedef struct data_struct {
	u16 l2cap_mtu;
	u16 sdp_con_id;
	u16 len;
	u8 data[0];
} __attribute__ ((packed)) data_struct;

struct database_query {
	u32 count;
	u8 query[256];
} database_query;

sdp_con sdp_con_list[MAX_NBR_SDP];

static int role;

/* Transaction id used for request */
//static u16 req_trans_id = 0xaabb;

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0)
static struct wait_queue *sdp_disc_wq = NULL;
#else
static wait_queue_head_t sdp_disc_wq;
#endif /* LINUX_VERSION_CODE */

#ifdef __KERNEL__
static bt_tx_buf *db_write_tx_buf;
static s32 db_write_recv;
#endif

#ifndef __KERNEL__
static s32 sdp_sock;
#endif

/****************** FUNCTION DEFINITION SECTION *****************************/

#ifndef __KERNEL__
int
open_socket(char *name)
{
	struct sockaddr_un server_address;
	s32 client_sockfd;
	s32 server_len;

	printf("Opening socket %s\n", name);

	client_sockfd = socket(AF_UNIX, SOCK_STREAM, 0);

	/* 'destination' socket */
	server_address.sun_family = AF_UNIX;
	strcpy(server_address.sun_path, name);
	server_len = sizeof(server_address);
  
	if (connect(client_sockfd, 
		    (struct sockaddr *)&server_address, server_len) < 0) {
		perror("connect client socket");
		D_ERR("Couldn't open socket, sdp will not be used\n");
		return -1;
	}

	printf("Socket connected to %s\n", server_address.sun_path);
	return client_sockfd;
}
#endif /* !__KERNEL__ */

void 
sdp_init(s32 srv)
{
	s32 i;
	protocol_layer this_layer;
	DSYS("Initialising SDP\n");

	database_query.count = 0;
	
	/* Set the confirm and indication functions for the L2CAP-layer */

	this_layer.con_ind = sdp_connect_ind;
	this_layer.con_pnd = sdp_connect_pnd;
	this_layer.conf_ind = sdp_config_ind;
	this_layer.disc_ind = sdp_disconnect_ind;

	this_layer.con_cfm = sdp_connect_cfm;
	this_layer.conf_cfm = sdp_config_cfm;
	this_layer.disc_cfm = sdp_disconnect_cfm;

	this_layer.receive_data = sdp_receive_data;

	/* Register SDP in the L2AP layer*/
	l2cap_register_upper(SDP_LAYER, &this_layer); 

	for (i = 0; i < MAX_NBR_SDP; i++) {
		sdp_con_list[i].id = i;
		sdp_con_list[i].l2cap = NULL;
		sdp_con_list[i].state = SDP_DISCONNECTED;
		sdp_con_list[i].initiator = 0;
		sdp_con_list[i].line = 0;
	}

	if (srv) {
		DSYS("Init SDP as server\n");
		role = 1;
#ifndef __KERNEL__
		sdp_sock = open_socket(SDP_SRV_SOCK);
#endif
	} else {
		DSYS("Init SDP as client\n");
		role = 0;
	}
}

void 
sdp_shutdown(void)
{
	s32 i;
	DSYS("Shutting down SDP\n");

	for (i = 0; i < MAX_NBR_SDP; i++) {
		if (sdp_con_list[i].state != SDP_DISCONNECTED) {
			sdp_disconnect_req(i);
		}
	}  

#ifdef __KERNEL__
	unsubscribe_bt_buf(db_write_tx_buf);
	db_write_tx_buf = NULL;
	db_write_recv = 0;
#endif
}

#ifdef __KERNEL__
s32
sdp_create_proc_file(void)
{
	s32 procfs_status = -ENOENT;

	/* The database_wq wait queue must be initialised before anyone tries
	   to read from the /proc/sdp_srv file, so we initialise the wait
	   queues here */
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0)
	init_waitqueue_head(&database_wq);
	init_waitqueue_head(&sdp_disc_wq);
#endif /* LINUX_VERSION_CODE */

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0)
	{
		struct proc_dir_entry *entry;

		if ((entry = create_proc_entry(sdp_proc_entry.name,
					       sdp_proc_entry.mode,
					       &proc_root))) {
			/*---------------------------------------------------*/
			/* If the proc entry was registered successfully,    */
			/* then set all the necessary structure information. */
			/*---------------------------------------------------*/
			entry->proc_iops = sdp_proc_entry.proc_iops;
			entry->proc_fops = sdp_proc_entry.proc_fops;
			procfs_status = 0;
		}
	}
#elif LINUX_VERSION_CODE > KERNEL_VERSION(2,2,0)
	procfs_status = proc_register(&proc_root, &sdp_proc_entry);
#else
	procfs_status = proc_register_dynamic(&proc_root, &sdp_proc_entry);
#endif /* LINUX_VERSION_CODE */

	if (procfs_status < 0) {
		D_ERR("Couldn't register proc file for sdp database %d\n",
		      procfs_status);
	}

	return procfs_status;
}

s32
sdp_remove_proc_file(void)
{
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0)
	remove_proc_entry(sdp_proc_entry.name, &proc_root);
#else
	proc_unregister(&proc_root, sdp_proc_entry.low_ino);
#endif
	return 0;
}
#endif /* __KERNEL__ */

s32 
sdp_connect_req(u8* bd_addr, u8 line)
{
	sdp_con *sdp;
  
	sdp = get_free_sdp_con();
	if (!sdp) {
		DSYS(__FUNCTION__ " WARNING couldn't find free sdp connection\n");
		/* fixme -- use bt_connect_cfm with correct con_id */
		return 0;
	}
	sdp->state = SDP_CONNECTING;
	sdp->initiator = TRUE;
	sdp->line = line;
	
	if (l2ca_connect_req(bd_addr, SDP_LAYER)) {
		D_ERR(__FUNCTION__ " ERROR l2ca_connect_req failed\n");
		return -1;
	}
	return sdp->id;
}

/* only client receives connect pnd */
void sdp_connect_pnd(l2cap_con *l2cap, s32 status)
{
	printk("sdp_connect_pnd : reason %d\n", status);	
	//PRINTPSM(l2cap);
}

void 
sdp_connect_ind(l2cap_con *l2cap) 
{
	sdp_con *sdp;
	D_MISC(__FUNCTION__ " remote cid : %d\n", l2cap->remote_cid);
   
	sdp = get_free_sdp_con();
	if (!sdp) {
		DSYS(__FUNCTION__ " WARNING couldn't find free sdp connection\n");
		if (l2ca_connect_rsp(l2cap, RES_NOSRC, STAT_NOINFO)) {
			D_ERR(__FUNCTION__ " l2ca_connect_rsp failed\n");
		}
	} else {
		if (l2ca_connect_rsp(l2cap, RES_SUCCESS, STAT_NOINFO)) {
			D_ERR(__FUNCTION__ " l2ca_connect_rsp failed\n"); 
		}
		sdp->initiator = FALSE;
		sdp->state = SDP_CONNECTING;
		sdp->l2cap = l2cap;
		l2cap->upper_con = (void*) sdp;
	}
}

void 
sdp_connect_cfm(l2cap_con *l2cap, s32 status)
{
	sdp_con *sdp = NULL;
	s32 i = 0;
	s32 stop = 0;
  
	if (status) {
		DSYS(__FUNCTION__ " Connection failed\n");
		bt_connect_cfm(CREATE_SDP_ID(sdp->line, 0), -1);
		return;
	}

	/* Find the connecting sdp_con */
	while ((i < MAX_NBR_SDP) && (!stop)) {
		if ((sdp_con_list[i].state == SDP_CONNECTING) && 
		    (sdp_con_list[i].initiator == TRUE)) {
			sdp = &sdp_con_list[i];
			stop = TRUE;
		}
		i++;
	}
  
	if (sdp != NULL) {
		/*-----------------------------------------------------------*/
		/* Set the connection in bt.                                 */
		/*-----------------------------------------------------------*/
		bt_register_sdp(sdp->line, sdp->id);

		if (!l2ca_local_conf_done(l2cap)) {
			/* still haven't sent config request yet */
			if (l2ca_config_req(l2cap, 0, NULL, 0, 0)) {
				D_ERR(__FUNCTION__ " Configuration request failed\n");
			}
			/* store connection */
			sdp->l2cap = l2cap;