hci.c

Linux下蓝牙探测工具

	}
}

static char *authentication2str(uint8_t authentication)
{
	switch (authentication) {
	case 0x00:
		return "No Bonding (No MITM Protection)";
	case 0x01:
		return "No Bonding (MITM Protection)";
	case 0x02:
		return "Dedicated Bonding (No MITM Protection)";
	case 0x03:
		return "Dedicated Bonding (MITM Protection)";
	case 0x04:
		return "General Bonding (No MITM Protection)";
	case 0x05:
		return "General Bonding (MITM Protection)";
	default:
		return "Reserved";
	}
}

static inline void ext_inquiry_response_dump(int level, struct frame *frm)
{
	void *ptr = frm->ptr;
	uint32_t len = frm->len;
	uint8_t type, length;
	char *str;
	int i;

	length = get_u8(frm);

	while (length > 0) {
		type = get_u8(frm);
		length--;

		switch (type) {
		case 0x01:
			p_indent(level, frm);
			printf("Flags:");
			for (i = 0; i < length; i++)
				printf(" 0x%2.2x", *((uint8_t *) (frm->ptr + i)));
			printf("\n");
			break;

		case 0x02:
		case 0x03:
			p_indent(level, frm);
			printf("%s service classes:",
					type == 0x02 ? "Shortened" : "Complete");
			for (i = 0; i < length / 2; i++) {
				uint16_t val = btohs(bt_get_unaligned((uint16_t *) (frm->ptr + (i * 2))));
				printf(" 0x%4.4x", val);
			}
			printf("\n");
			break;

		case 0x08:
		case 0x09:
			str = malloc(length + 1);
			if (str) {
				snprintf(str, length + 1, "%s", (char *) frm->ptr);
				for (i = 0; i < length; i++)
					if (!isprint(str[i]))
						str[i] = '.';
				p_indent(level, frm);
				printf("%s local name: \'%s\'\n",
					type == 0x08 ? "Shortened" : "Complete", str);
				free(str);
			}
			break;

		case 0x0a:
			p_indent(level, frm);
			printf("TX power level: %d\n", *((uint8_t *) frm->ptr));
			break;

		default:
			p_indent(level, frm);
			printf("Unknown type 0x%02x with %d bytes data\n",
								type, length);
			break;
		}

		frm->ptr += length;
		frm->len -= length;

		length = get_u8(frm);
	}

	frm->ptr = ptr + (EXTENDED_INQUIRY_INFO_SIZE - INQUIRY_INFO_WITH_RSSI_SIZE);
	frm->len = len + (EXTENDED_INQUIRY_INFO_SIZE - INQUIRY_INFO_WITH_RSSI_SIZE);
}

static inline void bdaddr_command_dump(int level, struct frame *frm)
{
	bdaddr_t *bdaddr = frm->ptr;
	char addr[18];

	frm->ptr += sizeof(bdaddr_t);
	frm->len -= sizeof(bdaddr_t);

	p_indent(level, frm);
	p_ba2str(bdaddr, addr);
	printf("bdaddr %s\n", addr);

	raw_dump(level, frm);
}

static inline void generic_command_dump(int level, struct frame *frm)
{
	uint16_t handle = btohs(htons(get_u16(frm)));

	p_indent(level, frm);
	printf("handle %d\n", handle);

	raw_dump(level, frm);
}

static inline void generic_write_mode_dump(int level, struct frame *frm)
{
	uint8_t mode = get_u8(frm);

	p_indent(level, frm);
	printf("mode 0x%2.2x\n", mode);
}

static inline void inquiry_dump(int level, struct frame *frm)
{
	inquiry_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("lap 0x%2.2x%2.2x%2.2x len %d num %d\n",
		cp->lap[2], cp->lap[1], cp->lap[0], cp->length, cp->num_rsp);
}

static inline void periodic_inquiry_dump(int level, struct frame *frm)
{
	periodic_inquiry_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("max %d min %d lap 0x%2.2x%2.2x%2.2x len %d num %d\n",
		btohs(cp->max_period), btohs(cp->min_period),
		cp->lap[2], cp->lap[1], cp->lap[0], cp->length, cp->num_rsp);
}

static inline void create_conn_dump(int level, struct frame *frm)
{
	create_conn_cp *cp = frm->ptr;
	uint16_t ptype = btohs(cp->pkt_type);
	uint16_t clkoffset = btohs(cp->clock_offset);
	char addr[18], *str;

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	printf("bdaddr %s ptype 0x%4.4x rswitch 0x%2.2x clkoffset 0x%4.4x%s\n",
		addr, ptype, cp->role_switch,
		clkoffset & 0x7fff, clkoffset & 0x8000 ? " (valid)" : "");

	str = hci_ptypetostr(ptype);
	if (str) {
		p_indent(level, frm);
		printf("Packet type: %s\n", str);
		free(str);
	}
}

static inline void disconnect_dump(int level, struct frame *frm)
{
	disconnect_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("handle %d reason 0x%2.2x\n", btohs(cp->handle), cp->reason);

	p_indent(level, frm);
	printf("Reason: %s\n", status2str(cp->reason));
}

static inline void add_sco_dump(int level, struct frame *frm)
{
	add_sco_cp *cp = frm->ptr;
	uint16_t ptype = btohs(cp->pkt_type);
	char *str;

	p_indent(level, frm);
	printf("handle %d ptype 0x%4.4x\n", btohs(cp->handle), ptype);

	str = hci_ptypetostr(ptype);
	if (str) {
		p_indent(level, frm);
		printf("Packet type: %s\n", str);
		free(str);
	}
}

static inline void accept_conn_req_dump(int level, struct frame *frm)
{
	accept_conn_req_cp *cp = frm->ptr;
	char addr[18];

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	printf("bdaddr %s role 0x%2.2x\n", addr, cp->role);

	p_indent(level, frm);
	printf("Role: %s\n", role2str(cp->role));
}

static inline void reject_conn_req_dump(int level, struct frame *frm)
{
	reject_conn_req_cp *cp = frm->ptr;
	char addr[18];

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	printf("bdaddr %s reason 0x%2.2x\n", addr, cp->reason);

	p_indent(level, frm);
	printf("Reason: %s\n", status2str(cp->reason));
}

static inline void pin_code_reply_dump(int level, struct frame *frm)
{
	pin_code_reply_cp *cp = frm->ptr;
	char addr[18], pin[17];

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	memset(pin, 0, sizeof(pin));
	if (parser.flags & DUMP_NOVENDOR)
		memset(pin, '*', cp->pin_len);
	else
		memcpy(pin, cp->pin_code, cp->pin_len);
	printf("bdaddr %s len %d pin \'%s\'\n", addr, cp->pin_len, pin);
}

static inline void link_key_reply_dump(int level, struct frame *frm)
{
	link_key_reply_cp *cp = frm->ptr;
	char addr[18];
	int i;

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	printf("bdaddr %s key ", addr);
	for (i = 0; i < 16; i++)
		if (parser.flags & DUMP_NOVENDOR)
			printf("**");
		else
			printf("%2.2X", cp->link_key[i]);
	printf("\n");
}

static inline void pin_code_neg_reply_dump(int level, struct frame *frm)
{
	bdaddr_t *bdaddr = frm->ptr;
	char addr[18];

	p_indent(level, frm);
	p_ba2str(bdaddr, addr);
	printf("bdaddr %s\n", addr);
}

static inline void user_passkey_reply_dump(int level, struct frame *frm)
{
	user_passkey_reply_cp *cp = frm->ptr;
	char addr[18];

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	printf("bdaddr %s passkey %d\n", addr, btohl(cp->passkey));
}

static inline void remote_oob_data_reply_dump(int level, struct frame *frm)
{
	remote_oob_data_reply_cp *cp = frm->ptr;
	char addr[18];
	int i;

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	printf("bdaddr %s\n", addr);

	p_indent(level, frm);
	printf("hash 0x");
	for (i = 0; i < 16; i++)
		printf("%02x", cp->hash[i]);
	printf("\n");

	p_indent(level, frm);
	printf("randomizer 0x");
	for (i = 0; i < 16; i++)
			printf("%02x", cp->randomizer[i]);
	printf("\n");
}

static inline void io_capability_reply_dump(int level, struct frame *frm)
{
	io_capability_reply_cp *cp = frm->ptr;
	char addr[18];

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	printf("bdaddr %s capability 0x%2.2x oob 0x%2.2x auth 0x%2.2x\n",
					addr, cp->capability, cp->oob_data,
							cp->authentication);

	p_indent(level, frm);
	printf("Capability: %s (OOB data %s)\n",
			capability2str(cp->capability),
			cp->oob_data == 0x00 ? "not present" : "available");

	p_indent(level, frm);
	printf("Authentication: %s\n", authentication2str(cp->authentication));
}

static inline void set_conn_encrypt_dump(int level, struct frame *frm)
{
	set_conn_encrypt_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("handle %d encrypt 0x%2.2x\n", btohs(cp->handle), cp->encrypt);
}

static inline void remote_name_req_dump(int level, struct frame *frm)
{
	remote_name_req_cp *cp = frm->ptr;
	uint16_t clkoffset = btohs(cp->clock_offset);
	char addr[18];

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	printf("bdaddr %s mode %d clkoffset 0x%4.4x%s\n",
		addr, cp->pscan_rep_mode,
		clkoffset & 0x7fff, clkoffset & 0x8000 ? " (valid)" : "");
}

static inline void master_link_key_dump(int level, struct frame *frm)
{
	master_link_key_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("flag %d\n", cp->key_flag);
}

static inline void read_remote_ext_features_dump(int level, struct frame *frm)
{
	read_remote_ext_features_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("handle %d page %d\n", btohs(cp->handle), cp->page_num);
}

static inline void setup_sync_conn_dump(int level, struct frame *frm)
{
	setup_sync_conn_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("handle %d voice setting 0x%4.4x\n", btohs(cp->handle),
						btohs(cp->voice_setting));
}

static inline void hold_mode_dump(int level, struct frame *frm)
{
	hold_mode_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("handle %d max %d min %d\n", btohs(cp->handle),
			btohs(cp->max_interval), btohs(cp->min_interval));
}

static inline void sniff_mode_dump(int level, struct frame *frm)
{
	sniff_mode_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("handle %d max %d min %d attempt %d timeout %d\n",
		btohs(cp->handle), btohs(cp->max_interval),
		btohs(cp->min_interval), btohs(cp->attempt), btohs(cp->timeout));
}

static inline void qos_setup_dump(int level, struct frame *frm)
{
	qos_setup_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("handle %d flags 0x%2.2x\n", btohs(cp->handle), cp->flags);

	p_indent(level, frm);
	printf("Service type: %d\n", cp->qos.service_type);
	p_indent(level, frm);
	printf("Token rate: %d\n", btohl(cp->qos.token_rate));
	p_indent(level, frm);
	printf("Peak bandwith: %d\n", btohl(cp->qos.peak_bandwidth));
	p_indent(level, frm);
	printf("Latency: %d\n", btohl(cp->qos.latency));
	p_indent(level, frm);
	printf("Delay variation: %d\n", btohl(cp->qos.delay_variation));
}

static inline void write_link_policy_dump(int level, struct frame *frm)
{
	write_link_policy_cp *cp = frm->ptr;
	uint16_t policy = btohs(cp->policy);
	char *str;

	p_indent(level, frm);
	printf("handle %d policy 0x%2.2x\n", btohs(cp->handle), policy);

	str = hci_lptostr(policy);
	if (str) {
		p_indent(level, frm);
		printf("Link policy: %s\n", str);
		free(str);
	}
}

static inline void sniff_subrating_dump(int level, struct frame *frm)
{
	sniff_subrating_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("handle %d\n", btohs(cp->handle));

	p_indent(level, frm);
	printf("max latency %d\n", btohs(cp->max_latency));

	p_indent(level, frm);
	printf("min timeout remote %d local %d\n",
		btohs(cp->min_remote_timeout), btohs(cp->min_local_timeout));
}

static inline void set_event_mask_dump(int level, struct frame *frm)
{
	set_event_mask_cp *cp = frm->ptr;
	int i;

	p_indent(level, frm);
	printf("Mask: 0x");
	for (i = 0; i < 8; i++)
		printf("%2.2x", cp->mask[i]);
	printf("\n");
}

static inline void set_event_flt_dump(int level, struct frame *frm)
{
	set_event_flt_cp *cp = frm->ptr;
	uint8_t dev_class[3], dev_mask[3];
	char addr[18];

	p_indent(level, frm);
	printf("type %d condition %d\n", cp->flt_type,
				(cp->flt_type == 0) ? 0 : cp->cond_type);

	switch (cp->flt_type) {
	case FLT_CLEAR_ALL:
		p_indent(level, frm);
		printf("Clear all filters\n");
		break;
	case FLT_INQ_RESULT:
		p_indent(level, frm);
		printf("Inquiry result");
		switch (cp->cond_type) {
		case INQ_RESULT_RETURN_ALL:
			printf(" for all devices\n");
			break;
		case INQ_RESULT_RETURN_CLASS:
			memcpy(dev_class, cp->condition, 3);
			memcpy(dev_mask, cp->condition + 3, 3);
			printf(" with class 0x%2.2x%2.2x%2.2x mask 0x%2.2x%2.2x%2.2x\n",
				dev_class[2], dev_class[1], dev_class[0],
				dev_mask[2], dev_mask[1], dev_mask[0]);
			break;
		case INQ_RESULT_RETURN_BDADDR:
			p_ba2str((bdaddr_t *) cp->condition, addr);
			printf(" with bdaddr %s\n", addr);
			break;
		default:
			printf("\n");
			break;
		}
		break;
	case FLT_CONN_SETUP:
		p_indent(level, frm);
		printf("Connection setup");
		switch (cp->cond_type) {
		case CONN_SETUP_ALLOW_ALL:
		case CONN_SETUP_ALLOW_CLASS:
		case CONN_SETUP_ALLOW_BDADDR:
		default:
			printf("\n");
			break;
		}
		break;
	}
}

static inline void write_pin_type_dump(int level, struct frame *frm)
{
	write_pin_type_cp *cp = frm->ptr;

	p_indent(level, frm);
	printf("type %d\n", cp->pin_type);
}

static inline void request_stored_link_key_dump(int level, struct frame *frm)
{
	read_stored_link_key_cp *cp = frm->ptr;
	char addr[18];

	p_indent(level, frm);
	p_ba2str(&cp->bdaddr, addr);
	printf("bdaddr %s all %d\n", addr, cp->read_all);
}

static inline void return_link_keys_dump(int level, struct frame *frm)
{
	uint8_t num = get_u8(frm);
	uint8_t key[16];
	char addr[18];
	int i, n;

	for (n = 0; n < num; n++) {
		p_ba2str(frm->ptr, addr);
		memcpy(key, frm->ptr + 6, 16);

		p_indent(level, frm);
		printf("bdaddr %s key ", addr);
		for (i = 0; i < 16; i++)
			if (parser.flags & DUMP_NOVENDOR)
				printf("**");
			else
				printf("%2.2X", key[i]);
		printf("\n");

		frm->ptr += 2;
		frm->len -= 2;
	}
}

static inline void change_local_name_dump(int level, struct frame *frm)
{
	change_local_name_cp *cp = frm->ptr;
	char name[249];
	int i;