btctl.c

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

}

int cmd_read_clock(struct command *cmd)
{
	int 	err,fd,handle = 0;
	__u8	which_clock = 0;
	__u16	accuracy;
	__u32	clock;
	BD_ADDR bda;

	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return -1;
	}

	if (__argv[optind] != NULL) {
		err = btdev_get_bda(&bda, __argv[optind]);
		if (err) {
			printf("Wrong address format\n");
			return 1;
		}

		if (__argv[optind+1] == NULL) {
			print_usage(cmd);
			return -1;
		}
		which_clock = atoi(__argv[optind+1]);
	
		handle = hci_get_conn(fd, &bda);
		if (handle < 0) {
			printf("Connection not found\n");
			return 1;
		}
	}
	err = HCI_ReadClock(fd, handle, which_clock, &clock, &accuracy);
	hci_close(fd);
	if (err){
			fprintf(stderr, "%s\n", hci_error(err));
			exit(1);
	}
	printf("Clock: %x, Accuracy: %x\n", clock, accuracy);
	return 0;
}

int cmd_afh_channel_map(struct command *cmd)
{
	int 	err,fd,handle;
	__u8	AFH_Mode;
	__u8	AFH_Channel_Map[8];
	BD_ADDR	bda;
	
	if (__argv[optind] == NULL) {
		print_usage(cmd);
		return -1;
	}
	
	err = btdev_get_bda(&bda, __argv[optind]);
	if (err) {
		printf("Wrong address format\n");
		return 1;
	}

	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return -1;
	}
	
	handle = hci_get_conn(fd, &bda);
	if (handle < 0) {
		printf("Connection not found\n");
		return 1;
	}
	
	err = HCI_ReadAFHChannelMap(fd, handle, &AFH_Mode, AFH_Channel_Map);
	hci_close(fd);
	if (err){
			fprintf(stderr, "%s\n", hci_error(err));
			exit(1);
	}
	printf("AFH Mode: %x\nAFH_Channel_Map [Channels]:\n [0-7]:%x\n [8-15]:%x\n [16-23]:%x\n [24-31]:%x\n [32-39]:%x\n [40-47]:%x\n [48-55]:%x\n [56-63]:%x\n [64-71]:%x\n [72-78]:%x\n",AFH_Mode,AFH_Channel_Map[0],AFH_Channel_Map[1],AFH_Channel_Map[2],AFH_Channel_Map[3],AFH_Channel_Map[4],AFH_Channel_Map[5],AFH_Channel_Map[6],AFH_Channel_Map[7],AFH_Channel_Map[8],AFH_Channel_Map[9]);
	return 0;
}

#endif

int cmd_bdaddr(struct command *cmd)
{
	int	err, fd;
	BD_ADDR	bda;

	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return -1;
	}

	err = HCI_ReadBDAddr(fd, &bda);
	if (err) {
		fprintf(stderr, "%s\n", hci_error(err));
		exit(1);
	}

	printf("Local Address: %s\n", bda2str(&bda));
	pause();

	hci_close(fd);
	return 0;
}

int cmd_name(struct command *cmd)
{
	int	err, fd;
	char	name[248];

	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return -1;
	}

	if (__argv[optind] == NULL) {
		// read name
		err = HCI_ReadLocalName(fd, name);
	} else {
		strncpy(name, __argv[optind], 248);
		err = HCI_ChangeLocalName(fd, name);
	}
	if (err) {
		fprintf(stderr, "%s\n", hci_error(err));
		exit(1);
	}

	if (__argv[optind] == NULL) {
		// read name
		printf("Name: %s\n", name);
	}

	hci_close(fd);
	return 0;
}

int cmd_scan(struct command *cmd)
{
	int	err, fd, flag;
	__u8	mode = HCI_SCAN_OFF;
	char	*param;

	if (!__argv[optind]) {
		printf("scan mode missed\n");
		return -1;
	}
	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return -1;
	}
	while (__argv[optind]) {
		param = __argv[optind];
		if (param[0] == '+') flag = 1, param++;
		else if (param[0] == '-') flag = 0, param++;
		else flag = 1;

		if (strcmp(param, "disc") == 0)
			flag ? (mode |= HCI_SCAN_INQUIRY) : (mode &= ~HCI_SCAN_INQUIRY);
		else if (strcmp(param, "conn") == 0)
			flag ? (mode |= HCI_SCAN_PAGE) : (mode &= ~HCI_SCAN_PAGE);

		optind++;
	}
	err = HCI_WriteScanEnable(fd, mode);
	if (err) {
		fprintf(stderr, "%s\n", hci_error(err));
		exit(1);
	}
	hci_close(fd);
	return 0;
}

int cmd_pincode(struct command *cmd)
{
	int		err;
	BD_ADDR		bda;
	int		Length;
	__u8		Code[16];

	if (cmd->cmd == 1 && (__argv[optind] == NULL || __argv[optind+1] == NULL)) {
		printf("Parameters missed\n");
		return -1;
	}

	if (__argv[optind]) {
		if (strcasecmp(__argv[optind], "default") == 0)
			memset(&bda, 0, 6);
		else {
			err = btdev_get_bda(&bda, __argv[optind]);
			if (err) {
				printf("Incorrect address given\n");
				return 1;
			}
		}
		if (cmd->cmd == 1) {
			Length = strlen(__argv[optind+1]);
			strncpy((char*)Code, __argv[optind+1], 16);
			err = hci_add_pin(&bda, Length, Code);
		} else
			err = hci_remove_pin(&bda);
	} else
		err = hci_remove_pin(NULL);

	if (err) {
		fprintf(stderr, "%s\n", hci_error(err));
		exit(1);
	}
	return err;
}

int cmd_rmkey(struct command *cmd)
{
	char		*peer =__argv[optind];
	int		err, i;
	BD_ADDR		bda;
	btdev_struct	*btdev;

	btdev_cache_reload();
	if (peer) {
		err = btdev_get_bda(&bda, peer);
		if (err) {
			fprintf(stderr, "Incorrect address given\n");
			return 1;
		}
		err = hci_remove_key(&bda);
		btdev = btdev_cache_lookup(&bda);
		if (btdev)
			btdev->flags &= ~BTDEV_KEY;
	} else {
		err = hci_remove_key(NULL);
		/* remove all keys */
		for (i = 0; (btdev = s_list_nth_data(devcache.head, i)); i++)
			btdev->flags &= ~BTDEV_KEY;
	}
	btdev_cache_save();
	return err;
}

int cmd_security(struct command *cmd)
{
	int	err, fd, sec_flags;

	if (!__argv[optind]) {
		print_usage(cmd);
		return 1;
	}
	if (!str2mask(sec_mode_map, argv2str(__argv + optind), &sec_flags)) {
		printf("format error\n");
		return -1;
	}
	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return -1;
	}
	err = HCI_WriteSecurityMode(fd, sec_flags);
	if (err) {
		fprintf(stderr, "%s\n", hci_error(err));
		exit(1);
	}
	close(fd);
	return 0;
}



int cmd_remotename(struct command *cmd)
{
	int		err, fd;
	BD_ADDR		bda;
	INQUIRY_ITEM	dev;
	char		Name[248];

	if (__argv[optind] == NULL) {
		printf("Bluetooth address missed\n");
		return -1;
	}

	err = btdev_get_bda(&bda, __argv[optind]);
	if (err) {
		printf("Wrong address format\n");
		return 1;
	}

	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return fd;
	}

	dev.bda = bda;
	dev.PS_Repetition_Mode = 0x00;
	dev.PS_Mode = 0x00;
	dev.Clock_Offset = 0x00;
	err = HCI_RemoteNameRequest(fd, &dev, Name);
	if (err) {
		fprintf(stderr, "%s\n", hci_error(err));
		exit(1);
	}

	printf("Remote name: %s\n", Name);
	return 0;
}

int cmd_role(struct command *cmd)
{
	int	err, fd, role_flags = 0;
	
	if (__argv[optind] == NULL) {
		print_usage(cmd);
		return -1;
	}

	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return -1;
	}
	if (strcmp(__argv[optind++], "deny") == 0)
		role_flags |= HCI_ROLE_DENY_SWITCH;

	if (strcmp(__argv[optind++], "master") == 0)
		role_flags |= HCI_ROLE_BECOME_MASTER;

	err = hci_set_role(fd, role_flags);
	if (err) {
		fprintf(stderr, "%s\n", hci_error(err));
		exit(1);
	}
	hci_close(fd);
	return 0;
}

int cmd_class(struct command *cmd)
{
	int		err, fd;
	__u32		cod;
	__u32		value;

	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return -1;
	}

	if (__argv[optind] == NULL) {
		// read name
		err = HCI_ReadClassOfDevice(fd, &cod);
		if (!err) {
			char	buf[80];
			parse_cod(buf, cod);
			printf("Class: 0x%06X, %s\n", cod, buf);
		}
	} else {
		if (strstr(__argv[optind], "0x")) {
			err = sscanf(__argv[optind], "%x ", &value);
			if (err > 0)
				cod = value;
		} else {
			// parse command line
			err = str2cod(argv2str(__argv + optind), &cod);
			if (err) {
				printf("bad arguments\n");
				return -1;
			}
		}
		err = HCI_WriteClassOfDevice(fd, cod);
	}
	if (err) {
		fprintf(stderr, "%s\n", hci_error(err));
		exit(1);
	}
	hci_close(fd);
	return 0;
}

int cmd_auth(struct command *cmd)
{
	int		err, fd;
	int		handle;
	BD_ADDR		bda;


	if (__argv[optind] == NULL) {
		print_usage(cmd);
		return -1;
	}

	fd = hci_open(btdev);
	if (fd < 0) {
		printf("Unable to open device %s: %s\n", btdev, strerror(errno));
		return -1;
	}

	err = btdev_get_bda(&bda, __argv[optind]);
	if (err) {
		printf("Wrong address format\n");
		return 1;
	}
	handle = hci_get_conn(fd, &bda);
	if (handle < 0) {
		printf("Connection not found\n");
		return 1;
	}
	err = HCI_AuthenticationRequested(fd, handle);
	if (err) {
		fprintf(stderr, "%s\n", hci_error(err));
		exit(1);
	}
	hci_close(fd);
	return 0;
}

/*
 * L2CAP
 */

void print_data(__u8 *p, int len)
{
	int	i,j;
	char 	buf[81];

	if (len > 100)
		return;
	for (i = 0; i < len; i++) {
		if ( ((i % 16) == 0) && i > 0)
			printf("%s\n", buf);
		j = (i % 16) * 3;
		sprintf( &(buf[j]), "%02x ", p[i]);
	}
	if (len)
		printf("%s\n", buf);
}


int cmd_ping(struct command *cmd)
{
	int		err, fd, i;
	BD_ADDR		bda;
	struct sockaddr_affix	saddr;
	int		size;
	__u8		*data;

	if (__argv[optind] == NULL || __argv[optind+1] == NULL) {
		printf("parameters missing\n");
		print_usage(cmd);
		return 1;
	}

	err = btdev_get_bda(&bda, __argv[optind++]);
	if (err) {
		printf("Incorrect address given\n");
		return 1;
	}
	printf("Connecting to host %s ...\n", bda2str(&bda));

	size = atoi(__argv[optind++]);

	fd = socket(PF_AFFIX, SOCK_SEQPACKET, BTPROTO_L2CAP);
	if (fd < 0) {
		printf("Unable to create RFCOMM socket: %d\n", PF_AFFIX);
		return 1;
	}

	saddr.bda = bda;
	saddr.port = 0;
	saddr.devnum = HCIDEV_ANY;
	err = connect(fd, (struct sockaddr*)&saddr, sizeof(saddr));
	if (err < 0) {
		printf("Unable to connect to remote side\n");
		close(fd);
		return 1;
	}

	data = (__u8*)malloc(size);
	if( data == NULL )
		return 0;

	for(i = 0; i < size; i++)
		data[i] = i;

	printf("Sending %d bytes...\n", size);
	print_data(data, size);
	if (cmd->cmd == 0) {
		err = l2cap_ping(fd, data, size);
		if (err < 0) {
			printf("ping error\n");
			exit(1);
		}
		printf("Received %d bytes back\n", err);
		print_data(data, err);
	} else if (cmd->cmd == 1) {
		struct timeval	tv_start, tv_end;
		long int	sec, rsec;
		long int	usec, rusec;
		double		speed;

		gettimeofday(&tv_start, NULL);
		err = l2cap_singleping(fd, data, size);
		if (err < 0) {
			printf("ping error\n");
			exit(1);
		}
		printf("flushing...\n");
		err = l2cap_flush(fd);
		if (err < 0) {
			printf("flush error\n");
			exit(1);