bthidctl.c

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

	close ( fd );
	if (err) {
		fprintf(stderr, "ERROR: %s!\n", hci_error(err));
		exit(1);
	}
	if (num == 0) {
		fprintf(stdout, "done: No 'connectable' devices found.\n");
	} else {
		fprintf(stdout, "done: %d 'connectable' device%s found.\n", found_devs = num, (num>1?"s":""));
	}
	fflush ( stdout );
	return 0;
}

int dolisten ( char * strbda, char doactive ) {
	// doactive is set to != 0 if an active connection is requested (like needed when first
	// connecting that device, so that it creates a fixation onto the host)
	int	fd;
	char	filenam[] = { DIRECTORY_HIDDB "/01234560123456" };
	int	i;
	for ( i = 0; i < 6; ++i ) {
		filenam[(2*i)+23] = strbda[(3*i)  ];
		filenam[(2*i)+24] = strbda[(3*i)+1];
	}
	if ( strlen ( strbda ) != 17 ) {
		fprintf ( stderr, "Bluetooth address not accepted [%s]!\n", strbda );
		return	1;
	}
	filenam[35] = 0;
	if ( 0 > (fd = open ( filenam, O_RDONLY ) ) ) {
		fprintf ( stderr, "Failed to read database file [%s] (%s)! (Did you do a 'bthidctl connect' before?)\n", filenam, strerror(errno) );
		return	1;
	}
	if ( sizeof(iocstruct) != read ( fd, (void *)&iocstruct, sizeof(iocstruct) ) ) {
		fprintf ( stderr, "Failed to read from database file [%s] (%s)!\n", filenam, strerror(errno) );
		close ( fd );
		return	1;
	}
	i = iocstruct.conn_info.rd_size;
	if ( ( i < 1 ) || ( i > 65535 ) ) {
		fprintf ( stderr, "Descriptor size seems invalid, aborting!\n" );
		close ( fd );
		return	1;
	}
	if ( NULL == ( iocstruct.conn_info.rd_data = malloc ( ( i + 1023 ) & 0x400 ) ) ) {
		fprintf ( stderr, "Failed to allocate memory for HID descriptor!\n" );
		close ( fd );
		return	1;
	}
	if ( i != read ( fd, iocstruct.conn_info.rd_data, i ) ) {
		fprintf ( stderr, "Failed to read the HID descriptor from file!\n" );
		close ( fd );
		return	1;
	}
	close ( fd );
	if ( 0 == str2bda ( &iocstruct.saddr.bda, strbda ) ) {
		fprintf ( stderr, "Malformed Bluetooth address: [%s]!\n", strbda );
		return	1;
	}
	if ( doactive ) { // Force active connection, for example on first contact
		iocstruct.status |= HIDP_STATUS_ACTIVE_ADD;
	}
	if ( hci_ioctl ( BTIOC_HIDP_MODIFY, &iocstruct ) < 0 ) {
		fprintf ( stderr, "Error %d while submitting info to the kernel (ioctl failed: %s)!\n", errno, strerror(errno) );
		return	1;
	}
	free ( iocstruct.conn_info.rd_data );
	iocstruct.conn_info.rd_data = NULL;
	fprintf ( stdout, "Successfully added [%s] to kernel device list%s.\n", strbda, \
			doactive ? " and requested immediate connection" : "");	
	return	0;
}

int dodisconnect ( char * strbda ) {
	if ( 0 == str2bda ( &iocstruct.saddr.bda, strbda ) ) {
		fprintf ( stderr, "Malformed Bluetooth address: [%s]!\n", strbda );
		return	1;
	}

	if ( hci_ioctl ( BTIOC_HIDP_DELETE, &iocstruct ) < 0 ) {
		fprintf ( stderr, "Error (%s) submitting info to the kernel (ioctl failed)!\n", strerror(errno) );
		return	1;
	}
	fprintf ( stdout, "Disconnected device [%s].\n", strbda );
	return	0;
}

int dodelete ( char * strbda ) {
	char	filenam[] = { DIRECTORY_HIDDB "/01234560123456" };
	BD_ADDR	bda;
	char *	bdap;
	int	i;
	if ( 0 == str2bda ( &bda, strbda ) ) {
		fprintf ( stderr, "Malformed Bluetooth address: [%s]!\n", strbda );
		return	1;
	}
	dodisconnect ( strbda ); // Just in case it was connected, try to disconnect.
	bdap = bda2str ( &bda );
	if ( NULL == bdap ) {
		fprintf ( stderr, "Failed to convert bluetooth address to string!" );
		return	1;
	}
	bdap[2] = bdap[5] = bdap[8] = bdap[11] = bdap[14] = bdap[17] = 0;
	sprintf ( filenam, "%s/%s%s%s%s%s%s", DIRECTORY_HIDDB,
		bdap, bdap + 3, bdap + 6, bdap + 9, bdap + 12, bdap + 15 );
	for ( bdap = filenam + strlen ( DIRECTORY_HIDDB ) + 1; *bdap != 0; ++bdap )
		*bdap = tolower ( *bdap );
	i = unlink ( filenam );
	if ( ( i == ENOENT ) || ( i == 0 ) ) {
		fprintf ( stdout, "Successfully removed device from database.\n" );
		return	0;
	}
	fprintf ( stderr, "Deleting device [%s] failed: Removing database file failed!\n", strbda );
	return	1;
}

int dostatus ( char dodiscovery )
{
	// List all devices that the kernel currently knows about
	struct hidp_ioc_getlist * hic;
	int	i;
	DIR	*dir;
	struct dirent *dire;
	char	buf[1024];
	int     status;
	BD_ADDR bda;
	int	fd;
	if ( NULL == ( hic = malloc ( sizeof ( struct hidp_ioc_getlist ) + ( sizeof ( struct hidp_ioc ) * HIDP_MAX_DEVICES ) ) ) ) {
		fprintf ( stderr, "Memory allocation failed - cannot retrieve status\n" );
		return	1;
	}
	hic->count = HIDP_MAX_DEVICES;
	hic->left = 0;
	if ( hci_ioctl ( BTIOC_HIDP_GET_LIST, hic ) < 0 ) {
		fprintf ( stderr, "Error submitting info to the kernel (ioctl failed): %d [%s]\n", errno, strerror(errno) );
		return	1;
	}
	if ( hic->count > HIDP_MAX_DEVICES ) {
		fprintf ( stderr, "Kernel managed to screw our device list.... claims %d devices in list! Abort.\n", hic->count );
		return	1;
	}
	found_devs = 0;
	if ( dodiscovery ) {
		if ( do_inquiry ( 0 ) ) { // 0 => use default inq length (like 8 seconds)
			found_devs = 0;
		}
	}
	// Read in devices from database
	if ( NULL == ( dir = opendir ( DIRECTORY_HIDDB ) ) ) {
		fprintf ( stderr, "Error: Cannot read database directory\n" );
		return	1;
	}
	fprintf ( stdout, "Bluetooth address  status   Device identifier%s\n", dodiscovery?" (only for devices in database)":"" );
	while ( NULL != ( dire = readdir ( dir ) ) ) {
		if ( strlen ( dire->d_name ) == 12 ) {
			for ( i = 2; i < 17; i+=3 ) buf[i] = ':';
			for ( i = 0; i < 12; ++i )  buf[(3*i)/2] = dire->d_name[i];
			buf[17] = 0;
			// Now check if it's in the list of kernel-known devices
			if ( str2bda ( &bda, buf ) ) { // Address is OK
				status = 0;
				for ( i = 0; i < hic->count; ++i ) {
					if ( 0 == memcmp ( &bda, &(hic->list[i].saddr.bda), sizeof ( bda ) ) ) {
						status = hic->list[i].status | HIDP_STATUS_ACTIVE_ADD; // Used for 'is kernel device'
						i = hic->count;
					}
				}
				// Now check discovered devices
				for ( i = 0; i < found_devs; ++i ) {
					if ( 0 == memcmp ( &bda, &devs[i].bda, sizeof ( bda ) ) ) {
						// Remove from the list, as it's _not_ an unknown/new device
						if ( i < ( found_devs - 1 ) ) {
							memcpy ( &devs[found_devs - 1].bda, &devs[i].bda, sizeof(bda) );
						}
						--found_devs;
					}
				}
				// Read in device information
				sprintf ( buf + 32, "%s/%s", DIRECTORY_HIDDB, dire->d_name );
				if ( 0 > (fd = open ( buf + 32, O_RDONLY ) ) ) {
					fprintf ( stderr, "Failed to read database file [%s] (%s)!\n", buf + 32, strerror(errno) );
					return	1;
				}
				if ( sizeof(iocstruct) != read ( fd, (void *)&iocstruct, sizeof(iocstruct) ) ) {
					fprintf ( stderr, "Failed to read from database file [%s] (%s)!\n", buf + 32, strerror(errno) );
					close ( fd );
					return	1;
				}
				close ( fd );
				// iocstruct now contains the additional info we wanted
				fprintf ( stdout, "%s  %s %s\n", buf, ((status & HIDP_STATUS_CONNECTED) ? "ACTIVE  " : ((status & HIDP_STATUS_ACTIVE_ADD) ? "STANDBY " : "DATABASE" ) ), iocstruct.conn_info.name );
			}
		}
	}
	closedir ( dir );
	for ( i = 0; i < found_devs; ++i ) {
		fprintf ( stdout, "%s  IN RANGE\n", bda2str ( &devs[i].bda ) );
	}
	//fprintf ( stdout, "Device list (%d entries):\n", hic->count );
	//for ( i = 0; i < hic->count; ++i ) {
	//	fprintf ( stdout, "[%s]\n", bda2str ( &(hic->list[i].saddr.bda) ));
	//}
	return	0;
}
	

int doconnect ( char * strbda )
{
	// Add the given BDA to the device database (or refresh record)
	// Must be in range to do so, of course, as SDP queries are involved
	char	filenam[] = { DIRECTORY_HIDDB "/01234560123456" };
	char *	bdap;
	BD_ADDR bda;
	int	i;
	if ( 0 == str2bda ( &bda, strbda ) ) {
		fprintf ( stderr, "Malformed Bluetooth address: [%s]\n", strbda );
		return	1;
	}
	// Trying to do the connection, retrieve via SDP the HID profile information
	if ( 0 == str2bda ( &iocstruct.saddr.bda, strbda ) ) return 1;
	iocstruct.status = 0;
	iocstruct.conn_info.vendor = 0;
	iocstruct.conn_info.product = 0;
	iocstruct.conn_info.version = 0;
	if ( sdp_probe_pnp ( &bda ) ) {
		fprintf ( stderr, "\nERROR: SDP device PNP probe for [%s] failed\n" \
				"This can be related to the device not being 'connectable', or\n" \
				"not being in radio range. Check address, and don't forget to press\n" \
				"'connect' button on the bottom of your bluetooth HID device.\n", strbda );
		return	1;
	}
	iocstruct.conn_info.country = 0;
	iocstruct.conn_info.flags = 0;
	iocstruct.conn_info.idle_to = 0;
	iocstruct.conn_info.parser = 0;
	iocstruct.conn_info.rd_size = 0;
	iocstruct.conn_info.rd_data = NULL;
	strcpy(iocstruct.conn_info.name, "<Generic Bluetooth HID Device>");
	iocstruct.status &= HIDP_STATUS_ACTIVE_ADD;
	if ( sdp_probe ( &bda ) ) {
		fprintf ( stderr, "SDP probe for HID record of device [%s] failed!\n", strbda );
		return	1;
	}
	fprintf ( stdout, "Identified device: Vendor ID[%04x:%04x], Name[%s].\n", iocstruct.conn_info.vendor,
			iocstruct.conn_info.product, iocstruct.conn_info.name );
	bdap = bda2str ( &bda );
	if ( NULL == bdap ) {
		return	1;
	} 
	bdap[2] = bdap[5] = bdap[8] = bdap[11] = bdap[14] = bdap[17] = 0;
	if ( rmkdir ( DIRECTORY_HIDDB, 0775 ) != 0 ) {
		fprintf ( stderr, "Failed to create directory " DIRECTORY_HIDDB " !\n" );
		return	1;
	}
	sprintf ( filenam, "%s/%s%s%s%s%s%s", DIRECTORY_HIDDB,
		bdap, bdap + 3, bdap + 6, bdap + 9, bdap + 12, bdap + 15 );
	for ( bdap = filenam + strlen ( DIRECTORY_HIDDB ) + 1; *bdap != 0; ++bdap )
		*bdap = tolower ( *bdap );
	if ( iocstruct.conn_info.rd_data == NULL ) {
		fprintf ( stderr, "Failed to retrieve a HID descriptor via SDP!\n" );
		return	1;
	}
	umask ( 002 ); // Create file ug_writeable.
	if ( 0 > ( i = open ( filenam, O_CREAT | O_TRUNC | O_RDWR, 0600 ) ) ) {
		fprintf ( stderr, "Failed to open database file [%s] for writing - check directory\n" \
				"exists and permissions are correct for " DIRECTORY_HIDDB " !\n", filenam );
		return	1;
	}
	if ( sizeof ( iocstruct ) != write ( i, (void *)&iocstruct, sizeof(iocstruct) ) ) {
		fprintf ( stderr, "Failed to write to file [%s] (though opening succeeded)!\n", strerror(errno) );
		return	1;
	}
	if ( iocstruct.conn_info.rd_size != write ( i, iocstruct.conn_info.rd_data, iocstruct.conn_info.rd_size ) ) {
		fprintf ( stderr, "Failed to write to file [%s] (though opening succeeded)!\n", strerror(errno) );
		return	1;
	}
	close ( i );

	fprintf ( stdout, "Successfully added device [%s] to database.\n", bda2str ( &bda ) );
	return	0;
}

int main ( int argc, char * argv[] )
{
	int	i = 1;
	char	buf[20];
	DIR *	directory;
	struct dirent * direntry;
	if ( argc < 2 ) {
		return	dohelp(NULL);
	}
	if ( 0 == strcasecmp ( argv[i], "help" ) ) {
		return	dohelp((i+1) == argc?"":argv[i+1]);
	}
	if ( 0 == strcasecmp ( argv[i], "connect" ) ) {
		if ( (argc - 1) == i ) {
			fprintf ( stderr, "bthidctl connect <bda>   -- additional parameter needed\n" );
			return	1;
		}
		if ( ( 0 == strncasecmp ( argv[i+1], "discover", 8 ) ) ||  // This catches 'discovery' as well
		     ( 0 == strncasecmp ( argv[i+1], "all"     , 3 ) ) ) { // 'all' possible as short form
			if ( do_inquiry ( 0 ) ) { // 0 => use default inq length (like 8 seconds)
				return	1;
			}
			for ( i = 0; i < found_devs; ++i ) {
				if ( doconnect ( bda2str ( &devs[i].bda ) ) ) {
					fprintf ( stdout, "Skipping device [%s], continuing.\n", bda2str ( &devs[i].bda ) );
				} else {
					dolisten ( bda2str ( &devs[i].bda ), 1 );
				}
				dolisten ( bda2str ( &devs[i].bda ), 1 );
			}
			fprintf ( stdout, "Finished adding all devices in range.\n" );
			return	0;
		}
		if ( 0 == doconnect ( argv[i+1] ) ) return dolisten ( argv[i+1], 1 );
	}
	if ( 0 == strcasecmp ( argv[i], "listen" ) ) {
		if ( (argc - 1) == i ) {
			fprintf ( stderr, "bthidctl listen [--active] <bda|ALL>   -- if you want to reconnect all\n" \
				 "                     devices in database, use   'bthidctl listen ALL'\n" );
			return	1;
		}
		iocstruct.status = 0;
		if ( 0 == strcasecmp ( argv[i+1], "--active" ) ) {
			if ( argc == i ) {
				fprintf ( stderr, "ERROR: You need to specify an address along the '--active' flag.\n" );
				return	1;
			}
			++i;
			iocstruct.status = HIDP_STATUS_ACTIVE_ADD;
		}
		if ( 0 == strcasecmp ( argv[i+1], "ALL" ) ) {
			if ( NULL == ( directory = opendir ( DIRECTORY_HIDDB ) ) ) {
				fprintf ( stderr, "Cannot scan database directory for device information files\n" );
				return	1;
			}
			while ( NULL != ( direntry = readdir ( directory ) ) ) {
				if ( strlen ( direntry->d_name ) == 12 ) {
					for ( i = 0; i < 6; ++i ) {
						buf[(3*i)  ] = direntry->d_name[(2*i)  ];
						buf[(3*i)+1] = direntry->d_name[(2*i)+1];
						buf[(3*i)+2] = ':';
					}
					buf[17] = 0;
					dolisten ( buf, 0 );
				}
			}
			closedir ( directory );
			if ( errno != 0 ) {
				fprintf ( stderr, "Error %d occured (%s) while adding all database devices to kernel device list\n",
						errno, strerror(errno) );
			}
			return	1;
		}
		return	dolisten ( argv[i+1], 0 );
	}
	if ( 0 == strcasecmp ( argv[i], "disconnect" ) ) {
		if ( ( argc - 1 ) == i ) {
			fprintf ( stderr, "bthidctl disconnect <bda|ALL>  -- if you want to disconnect all devices\n" \
				 "                                  in database, use  bthidctl disconnect ALL\n" );
			return	1;
		}
		if ( 0 == strcasecmp ( argv[i+1], "ALL" ) ) {
			struct	hidp_ioc_getlist * hic;
			int	i, j;
			if ( NULL == ( hic = malloc ( sizeof ( struct hidp_ioc_getlist ) + ( sizeof ( struct hidp_ioc ) * HIDP_MAX_DEVICES ) ) ) ) {
				fprintf ( stderr, "Memory allocation failed - cannot retrieve status (for disconnect all)\n" );
				return	1;
			}
			hic->count = HIDP_MAX_DEVICES;
			hic->left = 0;
			if ( hci_ioctl ( BTIOC_HIDP_GET_LIST, hic ) < 0 ) {
				fprintf ( stderr, "Error submitting info to the kernel (ioctl failed): %d [%s]\n", errno, strerror(errno) );
				return	1;
			}
			if ( hic->count > HIDP_MAX_DEVICES ) {
				fprintf ( stderr, "Kernel managed to screw our device list.... claims %d devices in list! Abort.\n", hic->count );
				return	1;
			}
			for ( i = j = 0; i < hic->count; ++i ) {
				j += dodisconnect ( bda2str ( &(hic->list[i].saddr.bda) ) );
			}
			fprintf ( stdout, "Disconnected %d device(s).\n", hic->count );
			return	0;
		}
		return	dodisconnect ( argv[i+1] );
	}
	if ( 0 == strcasecmp ( argv[i], "delete" ) ) {
		if ( ( argc - 1 ) == i ) {
			fprintf ( stderr, "Argument needed. Usage:   bthidctl delete <bda>\n" );
			return	1;
		}
		return	dodelete ( argv[i+1] );
	}
	if ( 0 == strcasecmp ( argv[i], "status" ) ) {
		if ( ( argc - 1 ) > i ) if ( strncasecmp ( argv[i+1], "discover", 8 ) == 0 ) return dostatus (1);
		return	dostatus (0);
	}
	fprintf ( stderr, "Invalid arguments given, use    bthidctl help    to learn more!\n" );
	return	1;
}