bthidctl.c

affix是一个Open Source的蓝牙协议栈

/* 
   Affix - Bluetooth Protocol Stack for Linux
   Copyright (C) 2001 - 2004 Nokia Corporation
   Original Author: Dmitry Kasatkin <[e-mail]>

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

   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.
   */

/* 
   $Id: bthidctl.c,v 1.14 2004/07/15 16:01:33 hoffmeis Exp $

   bthidctl - Program for controlling devices used by the bluetooth
   hid kernel driver

   Original implementation of bthidctl by
   Anselm Martin Hoffmeister - Rheinische Friedrich-Wilhelms-Universit鋞 BONN
   using great parts of the bluez userspace hid tool by Peter Klausler
*/

#include <affix/config.h>

#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <sys/errno.h>

#include <ctype.h>
#include <fcntl.h>
#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdarg.h>
#include <getopt.h>
#include <string.h>
#include <termios.h>
#include <dirent.h>
#include <sys/types.h>
#include <dirent.h>

#include <affix/bluetooth.h>
#include <affix/btcore.h>
#include <affix/utils.h>

#define PSM_SDP 0x0001

#define	DIRECTORY_HIDDB "/var/spool/affix/hiddb"

static int		sdp_sock = -1;
static unsigned char *	sdp_attr = NULL;
static    int    	sdp_attr_bytes = 0;

static  unsigned short	sdp_tid;         /* transaction id of last SDP request */
static  unsigned long	sdp_handle;

INQUIRY_ITEM		devs[20]; /* For device inquiries, like 'bthidctl connect discovery' */
static int		found_devs = 0;

struct sdp_data {
	enum sdp_data_type {
		sdp_data_nil,
		sdp_data_uint,
		sdp_data_sint,
		sdp_data_uuid,
		sdp_data_str,
		sdp_data_bool,
		sdp_data_seq,
		sdp_data_alt,
		sdp_data_url
	} type;
	int     bytes;
	union {
		unsigned long uint;     /* uint, bool */
		long    sint;           /* sint */
		char    ch [1];         /* str, url, uuid */
		struct sdp_seq {        /* seq, alt */
			int     items;
			struct sdp_data **item;
		} seq;
	} u;
};

struct hidp_ioc iocstruct;

int bthid_channel_connect (BD_ADDR *bda, int psm) {

	int     sock, rv;
	char    buf [32];
	struct sockaddr_affix l2sa;

	if (0 > (sock = socket (PF_AFFIX, SOCK_SEQPACKET, BTPROTO_L2CAP))) {
		fprintf ( stderr, "Cannot create L2CAP socket!\n" );
		return sock;
	}
	l2sa.family = AF_AFFIX;
	l2sa.port = psm;  //check byteorder
	l2sa.bda = *bda;
	_bda2str (buf, &l2sa.bda);
	l2sa.devnum = HCIDEV_ANY;
	if ((rv = connect (sock, (struct sockaddr *) &l2sa, sizeof l2sa))) {
		fprintf ( stderr, "Cannot connect L2CAP socket %d to " \
				 "BD %s PSM %d!\n", sock, buf, psm);
		close (sock);
		return rv;
	}
	return sock;
}


int dohelp ( char * topic ) {
	char	*p = (topic == NULL ? "" : topic );
	if ( 0 == strcmp ( p, "connect" ) ) {
		fprintf ( stdout, "bthidctl connect <bda>|'discovery'\n" \
			"This command is used to add devices to the HID device database (which lives\n" \
			"in " DIRECTORY_HIDDB " or reconnect those that had a connection to another\n" \
			"machine in the meantime. For 'connecting' a device, it must be in range and\n" \
			"ready to connect - many devices have a button you can press to force them into" \
			"a 'connectable' state.\n" \
			"After the device has been added to the database, you can (e.g. after a reboot,\n" \
			"or at any later time) use the 'bthidctl listen' command to request the kernel\n" \
			"to recognize that device again.\n" \
			"The 'bthidctl connect' command needs a parameter, which is either a valid\n" \
			"bluetooth device address ('bda') of the device or 'discovery' which tells\n" \
			"bthidctl to run a discovery and connect all available HID devices in range.\n" );
	} else if ( 0 == strcmp ( p, "listen" ) ) {
		fprintf ( stdout, "bthidctl listen [--active] <bda>|'all'\n" \
			"This command is used to notify the kernel of another device to allow HID\n" \
			"connections to/from. The given Bluetooth address must be registered to the\n" \
			"HID database (use 'bthidctl connect' command) and will be activated by this call.\n" \
			"You can give the string 'all' instead of a Bluetooth address, which will cause\n" \
			"all devices currently in database to be added to the kernel's device list.\n" \
			"This will prove particularly useful for init script usage or similar.\n" \
			"The '--active' option makes the kernel perform an active connection to the\n" \
			"given device (most devices connect from themselves, provided this machine is\n" \
			"the last one they had connection with, when a key is pressed or similar events\n" \
			"occur) - this will only be necessary for non-auto-reconnect capable HID devices\n" \
			"that don't announce their lack of capability.\n" \
			"If you used your HID device on another machine since you 'connect'ed it to this\n" \
			"one, you will have to do a 'bthidctl connect' again to update the HID device's\n" \
			"memory of which machine it is bundled to.\n" );
	} else if ( 0 == strcmp ( p, "disconnect" ) ) {
		fprintf ( stdout, "bthidctl disconnect <bda>|'all'\n" \
			"This command is used to delete devices from the kernel's list of active and\n" \
			"allowed devices. In case a connection to that device is active at time of\n" \
			"command execution, that connection will be terminated. This command has no\n" \
			"impact on the device database, so the device can be reconnected with the\n" \
			"'bthidctl connect' command at a later time.\n" );
	} else if ( 0 == strcmp ( p, "delete" ) ) {
		fprintf ( stdout, "bthidctl delete <bda>\n" \
			"This command removes the given device permanently from the device database.\n" \
			"In case this device is listed as active or connected in the kernel, it will\n" \
			"be disconnected. There is intentionally no 'all' parameter allowed for this\n" \
			"command. You can manually delete database files in case you need this.\n" );
	} else if ( 0 == strcmp ( p, "status" ) ) {
		fprintf ( stdout, "bthidctl status [discovery]\n" \
			"This command is used to retrieve a list of HID devices. It lists\n" \
			"those devices currently connected as well as the ones the kernel\n" \
			"currently has no connection to, but would accept HID events from, as well\n" \
			"as those only listed in the database of known devices, but which currently\n" \
			"are not allowed to connect to the HID daemon.\n" \
			"When specifying 'discovery', a discovery for HID devices will be done first;\n" \
			"any newly found HID devices will be listed along those already known and\n" \
			"those devices which are present in the database.\n" \
			"Discovered devices without database entry are listed without further\n" \
			"verification, so neither name nor type (HID or not HID) of those is known.\n" \
			"Sample output of 'bthidctl status discovery':\n=============\n" \
			"Performing device inquiry for 8 seconds...done: 4 'connectable' devices found.\n" \
			"Bluetooth address  status\n" \
			"00:00:00:c0:ff:ee  ACTIVE   Logitake bluetooth coffee mug thermic sensor\n" \
			"00:00:de:ad:be:ef  STANDBY  Epics input device\n" \
			"00:00:00:af:f1:c5  DATABASE Anymake WithoutAnyPurpose device\n" \
			"01:23:45:67:89:ab  IN RANGE\n" );
			
	} else if ( 0 == strcmp ( p, "help" ) ) {
		fprintf ( stdout, "bthidctl help <topic>\n" \
			"This command is used to retrieve information about bthidctl functions.\n" \
			"As you see this text, you obviously succeeded in using the\n" \
			"  'bthidctl help help'\n" \
			"command. To retrieve a list of available commands, please enter\n" \
			"  'bthidctl help'\n" );
	} else if ( 0 == strcmp ( p, "" ) ) {
		fprintf ( stdout, "bthidctl - Bluetooth Human Interface Device Control Utility for Affix stack\n" \
			"The following commands are valid:\n" \
			"   connect  listen  disconnect  delete  status  help\n" \
			"which will be discussed in detail when you enter 'bthidctl help <command>'\n" );
	} else {
		fprintf ( stdout, "No help for topic '%s'.\n" \
			"Use 'bthidctl help' to get a list of available commands/help topics!\n", p );
	}
	return	0;
}

static void hid_descriptor (struct sdp_data *classtype, struct sdp_data *classval) {
	// We received a HIDP descriptor. Copy it so that it can be used (iocstruct)
	if (classtype->type != sdp_data_uint ||
		classtype->u.uint != 0x22 /* report descriptor */ ||
		classval->type != sdp_data_str) {
		fprintf (stderr, "Device sent a bad HID descriptor\n");
		return;
	}
	if ( NULL == iocstruct.conn_info.rd_data ) {
		if ( NULL == ( iocstruct.conn_info.rd_data = malloc ( ( 1023 + classval->bytes ) & 0x400 ) ) ) {
			fprintf ( stderr, "Failed to allocate memory for the HID report descriptor!\n" );
			return;
		}
		iocstruct.conn_info.rd_size = classval->bytes;
		memcpy ( iocstruct.conn_info.rd_data, classval->u.ch, classval->bytes );
	}
	return;
}


static void sdp_free (struct sdp_data *s) {
	int     i;
	if (s->type == sdp_data_seq || s->type == sdp_data_alt) {
		for (i = 0; i < s->u.seq.items; i++) {
			sdp_free (s->u.seq.item [i]);
		}
	}
	free (s);
}

static void sdp_add_to_seq (struct sdp_data *s, struct sdp_data *it) {
	s->u.seq.item = realloc (s->u.seq.item, sizeof *s->u.seq.item * (s->u.seq.items + 1));
	s->u.seq.item [s->u.seq.items++] = it;
}

static int sdp_get_data_length (unsigned char *buff, int *idx) {
	int     i = *idx;
	int     tcode;
	int     n;
	tcode = buff [i++];
	if (!(tcode >> 3)) {
		n = 0;  /* nil */
	} else {
		switch (tcode & 7) {
			default:
				n = 1 << (tcode & 7);
				break;
			case 5:
				n = buff [i++];
				break;
			case 6:
				n = buff [i+0] << 8 |
					buff [i+1] << 0;
				i += 2;
				break;
			case 7:
				n = (unsigned long) buff [i+0] << 24 |
					(unsigned long) buff [i+1] << 16 |
					(unsigned long) buff [i+2] <<  8 |
					(unsigned long) buff [i+3] <<  0;
				i += 4;
				break;
		}
	}
	*idx = i;
	return n;
}

static void hid_attr_pnp (unsigned long attr, struct sdp_data *val) {
	// We received in 1st SDP query an attribute. If usable, copy to iocstruct
	switch (attr) { 
		case 0x201:
			if (val->type == sdp_data_uint) {
				iocstruct.conn_info.vendor = val->u.uint & 0xffff;
			}
			break;
		case 0x202:
			if (val->type == sdp_data_uint) {
				iocstruct.conn_info.product = val->u.uint & 0xffff;
			}
			break;
		case 0x203:
			if ( val->type == sdp_data_uint) {
				iocstruct.conn_info.version = (int)(val->u.uint) & 0xffff;
			}
			break;
	}
}

static void hid_attr (unsigned long attr, struct sdp_data *val) {
	// We received in 2nd SDP query an attribute. If usable, copy to iocstruct
	int i;
	char * devname;
	switch (attr) {
		case 0x100: // Device name
			if (val->type == sdp_data_str) {
				devname = strdup (val->u.ch);
				if (devname) {
					strncpy ( iocstruct.conn_info.name, devname, 127 );
					iocstruct.conn_info.name[127] = 0;
				}
			}
			break;
		case 0x101: // device description - not used right now.
		case 0x102: // vendor name - not used right now.
			break;
		case 0x201: // Hid profile version
			if (val->type == sdp_data_uint) {
				//iocstruct.conn_info.version = (int)(val->u.uint) & 0xffff;
				// This won't matter, version is expected to be the USB-type
				// of manufacturer/product/version
				;
			}
			break;
		case 0x20b: // Hid parser version
			if (val->type == sdp_data_uint) {
				iocstruct.conn_info.parser = (int)(val->u.uint) & 0xffff;
			}
			break;
		case 0x203: // country code ("0" means not localized)
			if (val->type == sdp_data_uint) {
				iocstruct.conn_info.country = (int)(val->u.uint) & 0xff;
			}
			break;
		case 0x205: // ReconnectInitiate: If 0, needs active reconnection
			if ( val->type == sdp_data_bool ) {
				if ( ((int)(val->u.uint) & 0xff) == 0 ) {
					// Device needs '--active' flag not only on first connection
					//fprintf ( stdout, "DEBUG: ACTIVE_ADD permanently forced.\n" );
					iocstruct.status = HIDP_STATUS_ACTIVE_ADD;
					;
				}
			}
			break;
		case 0x206: // HID descriptor - check and if possible, use it
			if (val->type == sdp_data_seq) {
				for (i = 0; i < val->u.seq.items; i++) {
					if (val->u.seq.item [i]->type == sdp_data_seq && val->u.seq.item [i]->u.seq.items >= 2) {
						hid_descriptor (val->u.seq.item [i]->u.seq.item [0],
								val->u.seq.item [i]->u.seq.item [1]);
					}
				}
			}
			break;
		default:
			break;
	}
}
	
static int sdp_getattr (unsigned char *cont) {
	// Perform a ServiceSearchAttributeRequest (both for PNP and HIDP query)
	// Use the latest retrieved SDPHandle for this.
	char    buf [64];
	int     n = 0, plen, clen, i;

	if (!cont) cont = "\0";
	
	sdp_tid++;

	buf [n++] = 0x04;       /* SDP_ServiceAttributeRequest */
	buf [n++] = sdp_tid >> 8;
	buf [n++] = sdp_tid;
	plen = n, n += 2;               /* 2-byte parameter length */
	buf [n++] = sdp_handle >> 24;   /* 32-bit handle, big-endian */
	buf [n++] = sdp_handle >> 16;
	buf [n++] = sdp_handle >> 8;
	buf [n++] = sdp_handle;
	buf [n++] = 1024 >> 8;          /* max byte count */
	buf [n++] = (char) 1024;
	buf [n++] = sdp_data_seq << 3 | 0x05;   /* Data element sequence */
	buf [n++] = 5;                  /* 5 bytes in sequence */
	buf [n++] = sdp_data_uint << 3 | 2;     /* 4-byte unsigned int */
	buf [n++] = 0x0000 >> 8;        /* range 0x0000-0xffff */
	buf [n++] = 0x0000;
	buf [n++] = 0xffff >> 8;
	buf [n++] = 0xffff & 0xff;	/* I assume 0xffff was meant here, not 0000 */
	buf [n++] = clen = *cont++;     /* continuation state */
	for (i = 0; i < clen; i++)  buf [n++] = *cont++;
	buf [plen] = (n - (plen + 2)) >> 8;
	buf [plen+1] = n - (plen + 2);

	if ( 0 > (n = send (sdp_sock, buf, n, 0))) {
		fprintf (stderr, "SDP send failed!\n");
		return -1;
	}
	//fprintf ( stdout, "." ); fflush ( stdout );
	return 0;
}

static struct sdp_data * sdp_scan (unsigned char *data, int *idx) {

	int     i, lim;
	struct sdp_data *s = NULL, *it;
	enum sdp_data_type tcode = data [*idx] >> 3;
	int     len = sdp_get_data_length (data, idx);

	switch (tcode) {