lmp.c

Linux下蓝牙探测工具

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2004-2007  Marcel Holtmann <[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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdio.h>
#include <errno.h>
#include <ctype.h>
#include <sys/socket.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>

#include "parser.h"

#define LMP_U8(frm)  (get_u8(frm))
#define LMP_U16(frm) (btohs(htons(get_u16(frm))))
#define LMP_U32(frm) (btohl(htonl(get_u32(frm))))

static enum {
	IN_RAND,
	COMB_KEY_M,
	COMB_KEY_S,
	AU_RAND_M,
	AU_RAND_S,
	SRES_M,
	SRES_S,
} pairing_state = IN_RAND;

static struct {
	uint8_t in_rand[16];
	uint8_t comb_key_m[16];
	uint8_t comb_key_s[16];
	uint8_t au_rand_m[16];
	uint8_t au_rand_s[16];
	uint8_t sres_m[4];
	uint8_t sres_s[4];
} pairing_data;

static inline void pairing_data_dump(void)
{
	int i;

	p_indent(6, NULL);
	printf("IN_RAND  ");
	for (i = 0; i < 16; i++)
		printf("%2.2x", pairing_data.in_rand[i]);
	printf("\n");

	p_indent(6, NULL);
	printf("COMB_KEY ");
	for (i = 0; i < 16; i++)
		printf("%2.2x", pairing_data.comb_key_m[i]);
	printf(" (M)\n");

	p_indent(6, NULL);
	printf("COMB_KEY ");
	for (i = 0; i < 16; i++)
		printf("%2.2x", pairing_data.comb_key_s[i]);
	printf(" (S)\n");

	p_indent(6, NULL);
	printf("AU_RAND  ");
	for (i = 0; i < 16; i++)
		printf("%2.2x", pairing_data.au_rand_m[i]);
	printf(" SRES ");
	for (i = 0; i < 4; i++)
		printf("%2.2x", pairing_data.sres_m[i]);
	printf(" (M)\n");

	p_indent(6, NULL);
	printf("AU_RAND  ");
	for (i = 0; i < 16; i++)
		printf("%2.2x", pairing_data.au_rand_s[i]);
	printf(" SRES ");
	for (i = 0; i < 4; i++)
		printf("%2.2x", pairing_data.sres_s[i]);
	printf(" (S)\n");
}

static inline void in_rand(struct frame *frm)
{
	uint8_t *val = frm->ptr;

	memcpy(pairing_data.in_rand, val, 16);
	pairing_state = COMB_KEY_M;
}

static inline void comb_key(struct frame *frm)
{
	uint8_t *val = frm->ptr;

	switch (pairing_state) {
	case COMB_KEY_M:
		memcpy(pairing_data.comb_key_m, val, 16);
		pairing_state = COMB_KEY_S;
		break;
	case COMB_KEY_S:
		memcpy(pairing_data.comb_key_s, val, 16);
		pairing_state = AU_RAND_M;
		break;
	default:
		pairing_state = IN_RAND;
		break;
	}
}

static inline void au_rand(struct frame *frm)
{
	uint8_t *val = frm->ptr;

	switch (pairing_state) {
	case AU_RAND_M:
		memcpy(pairing_data.au_rand_m, val, 16);
		pairing_state = SRES_M;
		break;
	case AU_RAND_S:
		memcpy(pairing_data.au_rand_s, val, 16);
		pairing_state = SRES_S;
		break;
	default:
		pairing_state = IN_RAND;
		break;
	}
}

static inline void sres(struct frame *frm)
{
	uint8_t *val = frm->ptr;

	switch (pairing_state) {
	case SRES_M:
		memcpy(pairing_data.sres_m, val, 4);
		pairing_state = AU_RAND_S;
		break;
	case SRES_S:
		memcpy(pairing_data.sres_s, val, 4);
		pairing_state = IN_RAND;
		pairing_data_dump();
		break;
	default:
		pairing_state = IN_RAND;
		break;
	}
}

static char *opcode2str(uint16_t opcode)
{
	switch (opcode) {
	case 1:
		return "name_req";
	case 2:
		return "name_res";
	case 3:
		return "accepted";
	case 4:
		return "not_accepted";
	case 5:
		return "clkoffset_req";
	case 6:
		return "clkoffset_res";
	case 7:
		return "detach";
	case 8:
		return "in_rand";
	case 9:
		return "comb_key";
	case 10:
		return "unit_key";
	case 11:
		return "au_rand";
	case 12:
		return "sres";
	case 13:
		return "temp_rand";
	case 14:
		return "temp_key";
	case 15:
		return "encryption_mode_req";
	case 16:
		return "encryption_key_size_req";
	case 17:
		return "start_encryption_req";
	case 18:
		return "stop_encryption_req";
	case 19:
		return "switch_req";
	case 20:
		return "hold";
	case 21:
		return "hold_req";
	case 22:
		return "sniff";
	case 23:
		return "sniff_req";
	case 24:
		return "unsniff_req";
	case 25:
		return "park_req";
	case 26:
		return "park";
	case 27:
		return "set_broadcast_scan_window";
	case 28:
		return "modify_beacon";
	case 29:
		return "unpark_BD_ADDR_req";
	case 30:
		return "unpark_PM_ADDR_req";
	case 31:
		return "incr_power_req";
	case 32:
		return "decr_power_req";
	case 33:
		return "max_power";
	case 34:
		return "min_power";
	case 35:
		return "auto_rate";
	case 36:
		return "preferred_rate";
	case 37:
		return "version_req";
	case 38:
		return "version_res";
	case 39:
		return "feature_req";
	case 40:
		return "feature_res";
	case 41:
		return "quality_of_service";
	case 42:
		return "quality_of_service_req";
	case 43:
		return "SCO_link_req";
	case 44:
		return "remove_SCO_link_req";
	case 45:
		return "max_slot";
	case 46:
		return "max_slot_req";
	case 47:
		return "timing_accuracy_req";
	case 48:
		return "timing_accuracy_res";
	case 49:
		return "setup_complete";
	case 50:
		return "use_semi_permanent_key";
	case 51:
		return "host_connection_req";
	case 52:
		return "slot_offset";
	case 53:
		return "page_mode_req";
	case 54:
		return "page_scan_mode_req";
	case 55:
		return "supervision_timeout";
	case 56:
		return "test_activate";
	case 57:
		return "test_control";
	case 58:
		return "encryption_key_size_mask_req";
	case 59:
		return "encryption_key_size_mask_res";
	case 60:
		return "set_AFH";
	case 61:
		return "encapsulated_header";
	case 62:
		return "encapsulated_payload";
	case 63:
		return "simple_pairing_confirm";
	case 64:
		return "simple_pairing_number";
	case 65:
		return "DHkey_check";
	case 127 + (1 << 7):
		return "accepted_ext";
	case 127 + (2 << 7):
		return "not_accepted_ext";
	case 127 + (3 << 7):
		return "features_req_ext";
	case 127 + (4 << 7):
		return "features_res_ext";
	case 127 + (11 << 7):
		return "packet_type_table_req";
	case 127 + (12 << 7):
		return "eSCO_link_req";
	case 127 + (13 << 7):
		return "remove_eSCO_link_req";
	case 127 + (16 << 7):
		return "channel_classification_req";
	case 127 + (17 << 7):
		return "channel_classification";
	case 127 + (21 << 7):
		return "sniff_subrating_req";
	case 127 + (22 << 7):
		return "sniff_subrating_res";
	case 127 + (23 << 7):
		return "pause_encryption_req";
	case 127 + (24 << 7):
		return "resume_encryption_req";
	case 127 + (25 << 7):
		return "IO_capability_req";
	case 127 + (26 << 7):
		return "IO_capability_res";
	case 127 + (27 << 7):
		return "numeric_comparison_failed";
	case 127 + (28 << 7):
		return "passkey_failed";
	case 127 + (29 << 7):
		return "oob_failed";
	case 127 + (30 << 7):
		return "keypress_notification";
	default:
		return "unknown";
	}
}

static inline void name_req_dump(int level, struct frame *frm)
{
	uint8_t offset = LMP_U8(frm);

	p_indent(level, frm);
	printf("name offset %d\n", offset);
}

static inline void name_res_dump(int level, struct frame *frm)
{
	uint8_t offset = LMP_U8(frm);
	uint8_t length = LMP_U8(frm);
	uint8_t *name = frm->ptr;
	int i, size;

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

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

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

	size = length - offset;
	if (size > 14)
		size = 14;

	p_indent(level, frm);
	printf("name fragment '");
	for (i = 0; i < size; i++)
		if (isprint(name[i]))
			printf("%c", name[i]);
		else
			printf(".");
	printf("'\n");
}

static inline void accepted_dump(int level, struct frame *frm)
{
	uint8_t opcode = LMP_U8(frm);

	p_indent(level, frm);
	printf("op code %d (%s)\n", opcode, opcode2str(opcode));
}

static inline void not_accepted_dump(int level, struct frame *frm)
{
	uint8_t opcode = LMP_U8(frm);
	uint8_t error = LMP_U8(frm);

	p_indent(level, frm);
	printf("op code %d (%s)\n", opcode, opcode2str(opcode));

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

static inline void clkoffset_dump(int level, struct frame *frm)
{
	uint16_t clkoffset = LMP_U16(frm);

	p_indent(level, frm);
	printf("clock offset 0x%4.4x\n", clkoffset);
}

static inline void detach_dump(int level, struct frame *frm)
{
	uint8_t error = LMP_U8(frm);

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

static inline void random_number_dump(int level, struct frame *frm)
{
	uint8_t *number = frm->ptr;
	int i;

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

	p_indent(level, frm);
	printf("random number ");
	for (i = 0; i < 16; i++)
		printf("%2.2x", number[i]);
	printf("\n");
}

static inline void key_dump(int level, struct frame *frm)
{
	uint8_t *key = frm->ptr;
	int i;

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

	p_indent(level, frm);
	printf("key ");
	for (i = 0; i < 16; i++)
		printf("%2.2x", key[i]);
	printf("\n");
}

static inline void auth_resp_dump(int level, struct frame *frm)
{
	uint8_t *resp = frm->ptr;
	int i;

	frm->ptr += 4;
	frm->ptr -= 4;

	p_indent(level, frm);
	printf("authentication response ");
	for (i = 0; i < 4; i++)
		printf("%2.2x", resp[i]);
	printf("\n");
}

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

	p_indent(level, frm);
	printf("encryption mode %d\n", mode);
}

static inline void encryption_key_size_req_dump(int level, struct frame *frm)
{
	uint8_t keysize = LMP_U8(frm);

	p_indent(level, frm);
	printf("key size %d\n", keysize);
}

static inline void switch_req_dump(int level, struct frame *frm)
{
	uint32_t instant = LMP_U32(frm);

	p_indent(level, frm);
	printf("switch instant 0x%4.4x\n", instant);
}

static inline void hold_dump(int level, struct frame *frm)
{
	uint16_t time = LMP_U16(frm);
	uint32_t instant = LMP_U32(frm);

	p_indent(level, frm);
	printf("hold time 0x%4.4x\n", time);

	p_indent(level, frm);
	printf("hold instant 0x%4.4x\n", instant);
}

static inline void sniff_req_dump(int level, struct frame *frm)
{
	uint8_t timing = LMP_U8(frm);
	uint16_t dsniff = LMP_U16(frm);
	uint16_t tsniff = LMP_U16(frm);
	uint16_t attempt = LMP_U16(frm);
	uint16_t timeout = LMP_U16(frm);

	p_indent(level, frm);
	printf("timing control flags 0x%2.2x\n", timing);

	p_indent(level, frm);
	printf("D_sniff %d T_sniff %d\n", dsniff, tsniff);

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

	p_indent(level, frm);
	printf("sniff timeout %d\n", timeout);
}

static inline void park_req_dump(int level, struct frame *frm)
{
	uint8_t timing = LMP_U8(frm);
	uint16_t db = LMP_U16(frm);
	uint16_t tb = LMP_U16(frm);
	uint8_t nb = LMP_U8(frm);
	uint8_t xb = LMP_U8(frm);
	uint8_t pmaddr = LMP_U8(frm);
	uint8_t araddr = LMP_U8(frm);
	uint8_t nbsleep = LMP_U8(frm);
	uint8_t dbsleep = LMP_U8(frm);
	uint8_t daccess = LMP_U8(frm);
	uint8_t taccess = LMP_U8(frm);
	uint8_t nslots = LMP_U8(frm);
	uint8_t npoll = LMP_U8(frm);
	uint8_t access = LMP_U8(frm);

	p_indent(level, frm);
	printf("timing control flags 0x%2.2x\n", timing);

	p_indent(level, frm);
	printf("D_B %d T_B %d N_B %d X_B %d\n", db, tb, nb, xb);

	p_indent(level, frm);
	printf("PM_ADDR %d AR_ADDR %d\n", pmaddr, araddr);

	p_indent(level, frm);
	printf("N_Bsleep %d D_Bsleep %d\n", nbsleep, dbsleep);

	p_indent(level, frm);
	printf("D_access %d T_access %d\n", daccess, taccess);

	p_indent(level, frm);
	printf("N_acc-slots %d N_poll %d\n", nslots, npoll);

	p_indent(level, frm);
	printf("M_access %d\n", access & 0x0f);

	p_indent(level, frm);
	printf("access scheme 0x%2.2x\n", access >> 4);
}

static inline void modify_beacon_dump(int level, struct frame *frm)
{
	uint8_t timing = LMP_U8(frm);
	uint16_t db = LMP_U16(frm);
	uint16_t tb = LMP_U16(frm);
	uint8_t nb = LMP_U8(frm);
	uint8_t xb = LMP_U8(frm);
	uint8_t daccess = LMP_U8(frm);
	uint8_t taccess = LMP_U8(frm);
	uint8_t nslots = LMP_U8(frm);
	uint8_t npoll = LMP_U8(frm);
	uint8_t access = LMP_U8(frm);

	p_indent(level, frm);
	printf("timing control flags 0x%2.2x\n", timing);

	p_indent(level, frm);
	printf("D_B %d T_B %d N_B %d X_B %d\n", db, tb, nb, xb);

	p_indent(level, frm);
	printf("D_access %d T_access %d\n", daccess, taccess);

	p_indent(level, frm);
	printf("N_acc-slots %d N_poll %d\n", nslots, npoll);

	p_indent(level, frm);
	printf("M_access %d\n", access & 0x0f);

	p_indent(level, frm);
	printf("access scheme 0x%2.2x\n", access >> 4);
}

static inline void power_req_dump(int level, struct frame *frm)
{
	uint8_t val = LMP_U8(frm);

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

static inline void preferred_rate_dump(int level, struct frame *frm)
{
	uint8_t rate = LMP_U8(frm);

	p_indent(level, frm);
	printf("data rate 0x%2.2x\n", rate);

	p_indent(level, frm);
	printf("Basic: ");

	printf("%suse FEC, ", rate & 0x01 ? "do not " : "");

	switch ((rate >> 1) & 0x03) {
	case 0x00:
		printf("no packet-size preference\n");
		break;
	case 0x01:
		printf("use 1-slot packets\n");
		break;
	case 0x02:
		printf("use 3-slot packets\n");
		break;
	case 0x03:
		printf("use 5-slot packets\n");
		break;
	}

	p_indent(level, frm);
	printf("EDR: ");

	switch ((rate >> 3) & 0x03) {
	case 0x00:
		printf("use DM1 packets, ");
		break;
	case 0x01:
		printf("use 2 Mbps packets, ");
		break;
	case 0x02:
		printf("use 3 Mbps packets, ");
		break;
	case 0x03:
		printf("reserved, \n");
		break;
	}

	switch ((rate >> 5) & 0x03) {
	case 0x00:
		printf("no packet-size preference\n");
		break;
	case 0x01:
		printf("use 1-slot packets\n");
		break;
	case 0x02:
		printf("use 3-slot packets\n");
		break;
	case 0x03:
		printf("use 5-slot packets\n");
		break;
	}
}

static inline void version_dump(int level, struct frame *frm)
{
	uint8_t ver = LMP_U8(frm);
	uint16_t compid = LMP_U16(frm);
	uint16_t subver = LMP_U16(frm);
	char *tmp;

	p_indent(level, frm);
	tmp = lmp_vertostr(ver);
	printf("VersNr %d (%s)\n", ver, tmp);
	bt_free(tmp);