btcore.c

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

/* 
   Affix - Bluetooth Protocol Stack for Linux
   Copyright (C) 2001 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: btcore.c,v 1.72 2004/03/24 12:28:09 kassatki Exp $

   HCI Command Library

   Fixes:	Dmitry Kasatkin <[e-mail]>
*/

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

#include <sys/time.h>
#include <sys/file.h>

#include <fcntl.h>
#include <unistd.h>
#include <syslog.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <time.h>
#include <ctype.h>
#include <arpa/inet.h>

/* affix specific */
#include <affix/config.h>
#include <affix/bluetooth.h>
#include <affix/btcore.h>

/* Library variables */
char		*affix_version = PACKAGE_STRING;
unsigned long	affix_logmask;		

char		btdev[IFNAMSIZ] = "bt0";
int		linkmode = PF_AFFIX;
int		promptmode = 0;
int		verboseflag = 0;
char		*progname = "(none)";
int		__argc;
char		**__argv;
char		affix_userpref[80];
char		affix_cachefile[80];

btdev_list	devcache;

struct affix_tupla debug_flags_map[] = {
	/* core */
	{DBHCI, "hcicore"},
	{DBAFHCI, "afhci"},
	{DBHCIMGR, "hcimgr"},
	{DBHCISCHED, "hcisched"},
	{DBHCI|DBAFHCI|DBHCIMGR|DBHCISCHED, "hci"},
	/* l2cap */
	{DBL2CAP, "pl2cap"},
	{DBAFL2CAP, "afl2cap"},
	{DBL2CAP|DBAFL2CAP, "l2cap"},
	/* rfcomm */
	{DBRFCOMM, "prfcomm"},
	{DBAFRFCOMM, "afrfcomm"},
	{DBBTY, "bty"},
	{DBRFCOMM|DBAFRFCOMM|DBBTY, "rfcomm"},
	/* pan */
	{DBPAN, "pan"},
	/* drivers */
	{DBDRV, "drv"},
	{DBALLMOD, "allmod"},
	/* details */
	{DBCTRL, "ctrl"},
	{DBPARSE, "parse"},
	{DBCHARDUMP, "chardump"},
	{DBHEXDUMP, "dump"},
	{DBFNAME, "fname"},
	{DBFUNC, "func"},
	{DBALLDETAIL, "alldetail"},

	{0xFFFFFFFF, "all"},
	{0, 0}
};

/* ERROR messages */
char *hci_errlist[] = {
	"NO ERROR",
	"Unknown HCI command",
	"No connection",
	"Hardware failure",
	"Page timeout",
	"Authentication failure",
	"Key missing",
	"Memory full",
	"Connection timeout",
	"Max number of connections",
	"Max number of SCO connections to a device",
	"ACL connection already exists",
	"Command disallowed",
	"Host rejected due to limited resources",
	"Host rejected due to security reason",
	"Host rejected due to remote device is only a personal device",
	"Host timeout",
	"Unsupported feature or parameter value",
	"Invalid HCI command parameters",
	"Other end terminated connection: user ended connection",
	"Other end terminated connection: low resources",
	"Other end terminated connection: about to power off",
	"Connection terminated by local host",
	"Repeated attempts",
	"Pairing not allowed",
	"Unknow LMP PDU",
	"Unsupported remote feature",
	"SCO offset rejected",
	"SCO interval rejected",
	"SCO air mode reejected",
	"Invalid LMP parameters",
	"Unspecified error",
	"unsupported LMP parameters value",
	"Role change not allowed",
	"LMP response timeout",
	"LMP error transaction collision",
	"LMP PDU not allowed",
	NULL
};

char *lmp_compid[] = {
	"Ericsson Mobile Communications",
	"Nokia Mobile Phones",
	"Intel Corp",
	"IBM Corp",
	"Toshiba Corp",
	"3Com",
	"Microsoft",
	"Lucent",
	"Motorola",
	"Infineon Technologies AG",
	"Cambridge Silicon Radio",
	"Silicon Wave",
	"Digianswer",
	"Texas Instruments Inc.",
	"Parthus Technologies Inc.",
	"Broadcom Corporation",
	"Mitel Semiconductor",
	"Widcomm Inc.",
	"Telencomm Inc.",
	"Atmel Corporation",
	"Mitsubishi Electric Corporation",
	"RTX Telecom A/S",
	"KC Technology Inc.",
	"Newlogic",
	"Transilica Inc.",
	"Rohde & Schwartz GmbH &Co. KG",
	"TTPCom Limited",
	"Signia Technologies Inc.",
	"Conexant Systems Inc.",
	"Qualcomm",
	"Inventel",
	"AVM Berlin",
	"BrandSpeed Inc.",
	"Mansella Ltd",
	"NEC Corporation",
	"WavePlus Technology Co., Ltd.",
	"Alcatel",
	NULL
};

char *lmp_features[] = {
	NULL
};

struct affix_tupla affix_protos[] = {
	{BTPROTO_L2CAP, "L2CAP", "L2CAP"},
	{BTPROTO_RFCOMM, "RFCOMM", "RFCOMM"},
	{0, NULL, NULL}
};


/* devices classes */
struct affix_tupla codsdp_service_map[] =  {
	{HCI_COD_NETWORKING, "netw", "Networking"},
	{HCI_COD_RENDERING, "rend", "Rendering"},
	{HCI_COD_CAPTURING, "capt", "Capturing"},
	{HCI_COD_TRANSFER, "tran", "Object Transfer"},
	{HCI_COD_AUDIO, "audi", "Audio"},
	{HCI_COD_TELEPHONY, "tele", "Telephony"},
	{HCI_COD_INFORMATION, "info", "Information"},
	{0, NULL, NULL}
};

struct affix_tupla codMajorClassMnemonic[] =  {
	{HCI_COD_MISC, "misc", "Miscellaneous"},
	{HCI_COD_COMPUTER, "comp", "Computer"},
	{HCI_COD_PHONE, "phon", "Phone"},
	{HCI_COD_LAP, "lap", "LAN Access Point"},
	{HCI_COD_MAUDIO, "audi", "Audio"},
	{HCI_COD_PERIPHERAL, "peri", "Peripheral"},
	{0, NULL, NULL}
};

struct affix_tupla codMinorComputerMnemonic[] =  {
	{HCI_COD_DESKTOP, "desk", "Desktop"},
	{HCI_COD_COMPUTER, "serv", "Server"},
	{HCI_COD_LAPTOP, "lapt", "Laptop"},
	{HCI_COD_HANDPC, "hand", "Handheld PC/PDA"},
	{HCI_COD_PALMPC, "palm", "Palm PC/PDA"},
	{0, NULL, NULL}
};

struct affix_tupla codMinorPhoneMnemonic[] =  {
	{HCI_COD_CELLULAR, "cell", "Cellular"},
	{HCI_COD_CORDLESS, "cord", "Cordless"},
	{HCI_COD_SMART, "smart", "Smart phone"},
	{HCI_COD_MODEM, "modem", "Wired Modem/VoiceGW"},
	{0, NULL, NULL}
};

struct affix_tupla codMinorAudioMnemonic[] =  {
	{HCI_COD_HEADSET, "head", "Headset"},
	{0, NULL, NULL}
};

struct affix_tupla pkt_type_map[] = {
	{HCI_PT_DM1, "DM1"},
	{HCI_PT_DH1, "DH1"},
	{HCI_PT_DM3, "DM3"},
	{HCI_PT_DH3, "DH3"},
	{HCI_PT_DM5, "DM5"},
	{HCI_PT_DH5, "DH5"},
	{HCI_PT_HV1, "HV1"},
	{HCI_PT_HV2, "HV2"},
	{HCI_PT_HV3, "HV3"},
	{0, 0}
};

struct affix_tupla sec_level_map[] = {
	{HCI_SECURITY_OPEN, "open"},
	{HCI_SECURITY_AUTHOR, "author"},
	{HCI_SECURITY_AUTH, "auth"},
	{HCI_SECURITY_ENCRYPT, "encrypt"},
	{0, 0}
};

struct affix_tupla sec_mode_map[] = {
	/* modes */
	{HCI_SECURITY_OPEN, "open"},
	{HCI_SECURITY_LINK, "link"},
	{HCI_SECURITY_SERVICE, "service"},
	{HCI_SECURITY_PAIRABLE, "pair"},
	/* levels */
	{HCI_SECURITY_AUTH, "auth"},
	{HCI_SECURITY_AUTHOR, "author"},
	{HCI_SECURITY_ENCRYPT, "encrypt"},
	{0, 0}
};

struct affix_tupla role_map[] = {
	{HCI_ROLE_DENY_SWITCH, "deny"},
	{HCI_ROLE_ALLOW_SWITCH, "allow"},
	{HCI_ROLE_BECOME_MASTER, "master"},
	{HCI_ROLE_REMAIN_SLAVE, "slave"},
	{0, 0}
};

struct affix_tupla scan_map[] = {
	{HCI_SCAN_INQUIRY, "disc"},
	{HCI_SCAN_PAGE, "conn"},
	{0, 0}
};

struct affix_tupla audio_features_map[] = {
	{HCI_LF_SCO, "SCO"},
	{HCI_LF_HV2, "HV2"},
	{HCI_LF_HV3, "HV3"},
	{HCI_LF_ULAWLOG, "u-Law log"},
	{HCI_LF_ALAWLOG, "a-Law log"},
	{HCI_LF_CVSD, "CVSD"},
	{HCI_LF_TRANSPARENT_SCO, "transparent SCO"},
	{0, 0}
};

struct affix_tupla policy_features_map[] = {
	{HCI_LF_SWITCH, "switch"},
	{HCI_LF_HOLD_MODE, "hold mode"},
	{HCI_LF_SNIFF_MODE, "sniff mode"},
	{HCI_LF_PARK_MODE, "park mode"},
	{0, 0}
};

struct affix_tupla timing_features_map[] = {
	{HCI_LF_SLOT_OFFSET, "slot offset"},
	{HCI_LF_TIMING_ACCURACY, "timing accuracy"},
	{0, 0}
};

struct affix_tupla radio_features_map[] = {
	{HCI_LF_RSSI, "RSSI"},
	{HCI_LF_CQD_DATARATE, "CQD data rate"},
	{HCI_LF_PAGING_SCHEME, "paging scheme"},
	{0, 0}
};

struct affix_tupla packets_features_map[] = {
	{HCI_LF_3SLOTS, "3-slots"},
	{HCI_LF_5SLOTS, "5-slots"},
	{0, 0}
};


/* ------------------------------------------ */

void _hci_error(char *buf, int err)
{
	if (err < 0)
		sprintf(buf, "System error: %s (%d)", strerror(errno), errno);
	else if (err > 0)
		sprintf(buf, "HCI error: %s (%d)", hci_errlist[err], err);
	else
		sprintf(buf, "No error (0)");
}

char *hci_error(int err)
{
	static unsigned char 	buf[80][2];
	static int 		num = 0; 

	num = 1 - num; /* switch buf */
	_hci_error(buf[num], err);
	return buf[num];
}


/* general purpose */
int str2bda(BD_ADDR *p, char *str)
{
	int	i, val, err;
	char	*res=(char*)p;

	for (i = 5; i >= 0; i--) {
		err = sscanf(str, "%2x", &val);
		if (err == 0)
			return 0;
		res[i] = val;
		if (i == 0)
			break;
		str = strchr(str,':');
		if (str == 0)
			return 0;
		str++;
	}
	return 1;
}

void _bda2str(char *str, BD_ADDR *p)
{
	__u8	*ma=(__u8*)p;

	sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x", 
		ma[5], ma[4], ma[3], ma[2], ma[1], ma[0]);

}

char *bda2str(BD_ADDR *bda)
{
	static unsigned char 	buf[2][18];
	static int 		num = 0; 

	num = 1 - num; /* switch buf */
	_bda2str(buf[num], bda);
	return buf[num];
}

void btlog_hexdump(const char *fname, const unsigned char *data, int len)
{
	int 	i, j;
	char	buf[81];

	for (i = 0; i < len; i++) {
		if (((i % 16) == 0) && i > 0)
			syslog(LOG_DEBUG, "%s: %s\n", fname, buf);
		j = (i % 16) * 3;
		sprintf(&(buf[j]), "%02x ", data[i]);
	}
	if (len)
		syslog(LOG_DEBUG, "%s: %s\n", fname, buf);
}

static void do_exit(void)
{
	btdev_cache_free();
}

int affix_init(int argc, char *argv[], int facility)
{
	int		fd, err;
	static int	affix_running = 0;
	
	if (affix_running)
		return 0;

	affix_logmask = 0xffff;

	fd = socket(PF_AFFIX, SOCK_RAW, BTPROTO_HCI);
	if (fd < 0 && errno == EAFNOSUPPORT) {
		// try to load module
		system("/sbin/modprobe affix");
		fd = socket(PF_AFFIX, SOCK_RAW, BTPROTO_HCI);
		if (fd < 0)
			return fd;
	}
	close(fd);
	affix_running = 1;

	/* set environment */
	__argc = argc;
	__argv = argv;
	progname = argv[0];

	openlog(argv[0], LOG_PERROR, facility);
	
	if (getuid() == 0) {
		/* root -> put the stuff to /var/spool/affix */
		sprintf(affix_userpref, "/var/spool/affix");
	} else
		sprintf(affix_userpref, "%s/.bluetooth", getenv("HOME"));
	err = rmkdir(affix_userpref, 0700);
	if (err < 0) {
		perror("Unable to create dir\n");
		return err;
	}
	sprintf(affix_cachefile, "%s/devcache", affix_userpref);
	btdev_cache_init();
	atexit(do_exit);
	return 0;
}

#include <affix/hci_cmds.h>	//XXX:


/* **********   Control Commands  ************** */

int hci_add_pin(BD_ADDR *bda, int Length, __u8 *Code)
{
	struct PIN_Code	pin;
	int		err;

	pin.bda = *bda;
	pin.Length = Length;
	memcpy(pin.Code, Code, 16);
	err = hci_ioctl(BTIOC_ADDPINCODE, &pin);
	if (err)
		return err;
	return hci_remove_key(bda);
}

int hci_add_key(BD_ADDR *bda, __u8 key_type, __u8 *key)
{
	struct link_key	k;

	k.bda = *bda;
	k.key_type = key_type;
	memcpy(k.key, key, 16);
	return hci_ioctl(BTIOC_ADDLINKKEY, &k);
}

int hci_remove_key(BD_ADDR *bda)
{
	int		err = 0;
	int		fd;
	__u16		deleted;
	int		i, num, flags;
	int		devs[HCI_MAX_DEVS];

	num = hci_get_devs(devs);
	for (i = 0; i < num; i++) {
		hci_get_flags_id(devs[i], &flags);
		if (!(flags & HCI_FLAGS_UP))
			continue;
		fd = hci_open_id(devs[i]);
		if (fd < 0) {
			err = fd;
			break;
		}
		err = HCI_DeleteStoredLinkKey(fd, bda, (!bda) ? 1 : 0, &deleted);
		hci_close(fd);
		if (err < 0)
			return err;
	}
	return hci_ioctl(BTIOC_REMOVELINKKEY, bda);
}


int hci_open_uart(char *dev, int type, int proto, int speed, int flags)
{
	struct open_uart	line;

	realpath(dev, line.dev);
	line.type = type;
	line.proto = proto;
	line.speed = speed;
	line.flags = flags;
	return hci_ioctl(BTIOC_OPEN_UART, &line);
}

int hci_setup_uart(char *name, int proto, int speed, int flags)
{
	struct open_uart	line;

	strncpy(line.dev, name, IFNAMSIZ);
	line.proto = proto;
	line.speed = speed;
	line.flags = flags;
	return hci_ioctl(BTIOC_SETUP_UART, &line);
}

int hci_close_uart(char *dev)
{
	struct open_uart	line;

	realpath(dev, line.dev);
	line.flags = 0;
	return hci_ioctl(BTIOC_CLOSE_UART, &line);
}

/* L2CAP - user mode stuff */

/*
 * returns L2CAP channel (socket) MTU
 */
int l2cap_sendping(int fd, char *data, int size, int type)
{
	int		err = 0;
	struct msghdr	msg;
	struct iovec	iov;
	struct cmsghdr	*cmsg;
	char		buf[CMSG_SPACE(0)];
	int		len = size;

	while (len) {
		iov.iov_base = data;
		iov.iov_len = len;
		msg.msg_name = NULL;
		msg.msg_namelen = 0;
		msg.msg_iov = &iov;
		msg.msg_iovlen = 1;
		msg.msg_control = buf;
		msg.msg_controllen = sizeof buf;
		cmsg = CMSG_FIRSTHDR(&msg);
		cmsg->cmsg_level = SOL_AFFIX;
		cmsg->cmsg_type = type;
		cmsg->cmsg_len = CMSG_LEN(0);
		msg.msg_controllen = cmsg->cmsg_len;	

		err = sendmsg(fd, &msg, 0);
		if (err < 0)
			return err;
		if (err == 0)
			return (size - len);
		len -= err;
	}
	return size;
}

int l2cap_ping(int fd, char *data, int size)
{
	int	err = 0;
	int	len = 0;

	err = l2cap_sendping(fd, data, size, L2CAP_PING);
	if (err < 0)
		return err;
	while (len < size) {
		err = recv(fd, data+len, size - len, 0);
		if (err < 0)
			return err;
		if (err == 0)
			break;
		len += err;
	}
	return len;
}



/* RFCOMM - user space stuff */

int rfcomm_get_ports(struct rfcomm_port *pi, int size)
{
	int	fd, err;
	struct rfcomm_ports	cp;
	
	fd = socket(PF_AFFIX, SOCK_STREAM, BTPROTO_RFCOMM);
	if (fd < 0)
		return fd;

	cp.ports = pi;
	cp.size = size;
	err = ioctl(fd, SIOCRFCOMM_GET_PORTS, &cp);
	if (err < 0)
		return err;

	return cp.count;
}


/* **************************  Control Commands ******************* */

int hci_ioctl(int cmd, void *arg)
{
	int	fd, err;
	fd = btsys_socket(PF_AFFIX, SOCK_RAW, BTPROTO_HCI);
	if (fd < 0)
		return fd;
	err = btsys_ioctl(fd, cmd, arg);
	btsys_close(fd);
	return err;
}

int hci_get_conn(int fd, BD_ADDR *bda)
{
	struct affix_conn_info	ci;
	int			err;

	ci.bda = *bda;
	err = btsys_ioctl(fd, BTIOC_GET_CONN, &ci);
	if (err)
		return err;
	return ci.dport;
}

/*
 * PAN
 */

int affix_pan_init(char *name, int mode)
{
	struct pan_init		pan;

	if (name) {
		strncpy(pan.name, name, IFNAMSIZ);
		pan.name[IFNAMSIZ-1] = '\0';
	} else
		pan.name[0] = '\0';
	pan.mode = mode;
	return hci_ioctl(BTIOC_PAN_INIT, &pan);
}

int affix_pan_connect(struct sockaddr_affix *sa, int role)
{
	struct pan_connect      pan;
	memcpy(&pan.saddr, sa, sizeof(struct sockaddr_affix));
	pan.peer_role = role;
	return hci_ioctl(BTIOC_PAN_CONNECT, &pan);
}


/* CORE utils */
/* Info about a process. */
typedef struct _proc_ {
	char *fullname;	/* Name as found out from argv[0] */
	char *basename;	/* Only the part after the last / */
	char *statname;	/* the statname without braces    */
	ino_t ino;		/* Inode number			  */
	dev_t dev;		/* Device it is on		  */
	pid_t pid;		/* Process ID.			  */
	int sid;		/* Session ID.			  */
	struct _proc_ *next;	/* Pointer to next struct. 	  */
} PROC;

/* pid queue */
typedef struct _pidq_ {
	struct _pidq_	*front;
	struct _pidq_	*next;
	struct _pidq_	*rear;
	PROC		*proc;
} PIDQ;

/* List of processes. */
static PROC *plist = NULL;
/* Did we stop a number of processes? */
static int scripts_too = 0;

#if 1
//
// Get some file-info. (size and lastmod)
//
int get_fileinfo(const char *name, char *lastmod)
{
	struct stat stats;
	struct tm *tm;
	
	stat(name, &stats);
	tm = gmtime(&stats.st_mtime);
	snprintf(lastmod, 21, "%04d-%02d-%02dT%02d:%02d:%02dZ",
			tm->tm_year+1900, tm->tm_mon+1, tm->tm_mday,
			tm->tm_hour, tm->tm_min, tm->tm_sec);
	return (int) stats.st_size;
}