lmp.c

Linux下蓝牙探测工具

	p_indent(level, frm);
	printf("CompId %d (%s)\n", compid, bt_compidtostr(compid));

	p_indent(level, frm);
	printf("SubVersNr %d\n", subver);
}

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

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

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

static inline void set_afh_dump(int level, struct frame *frm)
{
	uint32_t instant = LMP_U32(frm);
	uint8_t mode = LMP_U8(frm);
	uint8_t *map = frm->ptr;
	int i;

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

	p_indent(level, frm);
	printf("AFH_instant 0x%04x\n", instant);

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

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

static inline void encapsulated_header_dump(int level, struct frame *frm)
{
	uint8_t major = LMP_U8(frm);
	uint8_t minor = LMP_U8(frm);
	uint8_t length = LMP_U8(frm);

	p_indent(level, frm);
	printf("major type %d minor type %d payload length %d\n",
						major, minor, length);

	if (major == 1 && minor == 1) {
		p_indent(level, frm);
		printf("P-192 Public Key\n");
	}
}

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

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

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

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

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

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

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

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

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

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

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

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

static inline void accepted_ext_dump(int level, struct frame *frm)
{
	uint16_t opcode = LMP_U8(frm) + (LMP_U8(frm) << 7);

	p_indent(level, frm);
	printf("op code %d/%d (%s)\n", opcode & 0x7f, opcode >> 7, opcode2str(opcode));
}

static inline void not_accepted_ext_dump(int level, struct frame *frm)
{
	uint16_t opcode = LMP_U8(frm) + (LMP_U8(frm) << 7);
	uint8_t error = LMP_U8(frm);

	p_indent(level, frm);
	printf("op code %d/%d (%s)\n", opcode & 0x7f, opcode >> 7, opcode2str(opcode));

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

static inline void features_ext_dump(int level, struct frame *frm)
{
	uint8_t page = LMP_U8(frm);
	uint8_t max = LMP_U8(frm);
	uint8_t *features = frm->ptr;
	int i;

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

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

	p_indent(level, frm);
	printf("max supported page %d\n", max);

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

static inline void quality_of_service_dump(int level, struct frame *frm)
{
	uint16_t interval = LMP_U16(frm);
	uint8_t nbc = LMP_U8(frm);

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

	p_indent(level, frm);
	printf("N_BC %d\n", nbc);
}

static inline void sco_link_req_dump(int level, struct frame *frm)
{
	uint8_t handle = LMP_U8(frm);
	uint8_t timing = LMP_U8(frm);
	uint8_t dsco = LMP_U8(frm);
	uint8_t tsco = LMP_U8(frm);
	uint8_t packet = LMP_U8(frm);
	uint8_t airmode = LMP_U8(frm);

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

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

	p_indent(level, frm);
	printf("D_SCO %d T_SCO %d\n", dsco, tsco);

	p_indent(level, frm);
	printf("SCO packet 0x%2.2x\n", packet);

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

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

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

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

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

	p_indent(level, frm);
	printf("max slots %d\n", slots);
}

static inline void timing_accuracy_dump(int level, struct frame *frm)
{
	uint8_t drift = LMP_U8(frm);
	uint8_t jitter = LMP_U8(frm);

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

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

static inline void slot_offset_dump(int level, struct frame *frm)
{
	uint16_t offset = LMP_U16(frm);
	char addr[18];

	p_ba2str((bdaddr_t *) frm->ptr, addr);

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

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

static inline void page_mode_dump(int level, struct frame *frm)
{
	uint8_t scheme = LMP_U8(frm);
	uint8_t settings = LMP_U8(frm);

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

	p_indent(level, frm);
	printf("page scheme settings %d\n", settings);
}

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

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

static inline void test_control_dump(int level, struct frame *frm)
{
	uint8_t scenario = LMP_U8(frm);
	uint8_t hopping = LMP_U8(frm);
	uint8_t txfreq = LMP_U8(frm);
	uint8_t rxfreq = LMP_U8(frm);
	uint8_t power = LMP_U8(frm);
	uint8_t poll = LMP_U8(frm);
	uint8_t packet = LMP_U8(frm);
	uint16_t length = LMP_U16(frm);

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

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

	p_indent(level, frm);
	printf("TX frequency %d\n", txfreq);

	p_indent(level, frm);
	printf("RX frequency %d\n", rxfreq);

	p_indent(level, frm);
	printf("power control mode %d\n", power);

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

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

	p_indent(level, frm);
	printf("packet type 0x%2.2x\n", packet);

	p_indent(level, frm);
	printf("length of test data %d\n", length);
}

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

	p_indent(level, frm);
	printf("key size mask 0x%4.4x\n", mask);
}

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

	p_indent(level, frm);
	printf("packet type table %d ", type);
	switch (type) {
	case 0:
		printf("(1Mbps only)\n");
		break;
	case 1:
		printf("(2/3Mbps)\n");
		break;
	default:
		printf("(Reserved)\n");
		break;
	}
}

static inline void esco_link_req_dump(int level, struct frame *frm)
{
	uint8_t handle = LMP_U8(frm);
	uint8_t ltaddr = LMP_U8(frm);
	uint8_t timing = LMP_U8(frm);
	uint8_t desco = LMP_U8(frm);
	uint8_t tesco = LMP_U8(frm);
	uint8_t wesco = LMP_U8(frm);
	uint8_t mspkt = LMP_U8(frm);
	uint8_t smpkt = LMP_U8(frm);
	uint16_t mslen = LMP_U16(frm);
	uint16_t smlen = LMP_U16(frm);
	uint8_t airmode = LMP_U8(frm);
	uint8_t negstate = LMP_U8(frm);

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

	p_indent(level, frm);
	printf("eSCO LT_ADDR %d\n", ltaddr);

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

	p_indent(level, frm);
	printf("D_eSCO %d T_eSCO %d W_eSCO %d\n", desco, tesco, wesco);

	p_indent(level, frm);
	printf("eSCO M->S packet type 0x%2.2x length %d\n", mspkt, mslen);

	p_indent(level, frm);
	printf("eSCO S->M packet type 0x%2.2x length %d\n", smpkt, smlen);

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

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

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

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

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

static inline void channel_classification_req_dump(int level, struct frame *frm)
{
	uint8_t mode = LMP_U8(frm);
	uint16_t min = LMP_U16(frm);
	uint16_t max = LMP_U16(frm);

	p_indent(level, frm);
	printf("AFH reporting mode %d\n", mode);

	p_indent(level, frm);
	printf("AFH min interval 0x%4.4x\n", min);

	p_indent(level, frm);
	printf("AFH max interval 0x%4.4x\n", max);
}

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

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

	p_indent(level, frm);
	printf("AFH channel classification 0x");
	for (i = 0; i < 10; i++)
		printf("%2.2x", map[i]);
	printf("\n");
}

static inline void sniff_subrating_dump(int level, struct frame *frm)
{
	uint8_t subrate = LMP_U8(frm);
	uint16_t timeout = LMP_U16(frm);
	uint32_t instant = LMP_U32(frm);

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

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

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

static inline void io_capability_dump(int level, struct frame *frm)
{
	uint8_t capability = LMP_U8(frm);
	uint8_t oob_data = LMP_U8(frm);
	uint8_t authentication = LMP_U8(frm);

	p_indent(level, frm);
	printf("capability 0x%2.2x oob 0x%2.2x auth 0x%2.2x\n",
				capability, oob_data, authentication);
}

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

	p_indent(level, frm);
	printf("notification value %d\n", value);
}

void lmp_dump(int level, struct frame *frm)
{
	uint8_t tmp, tid;
	uint16_t opcode;

	p_indent(level, frm);

	tmp = LMP_U8(frm);
	tid = tmp & 0x01;
	opcode = (tmp & 0xfe) >> 1;
	if (opcode > 123) {
		tmp = LMP_U8(frm);
		opcode += tmp << 7;
	}

	printf("LMP(%c): %s(%c): ", frm->master ? 's' : 'r',
				opcode2str(opcode), tid ? 's' : 'm');

	if (opcode > 123)
		printf("op code %d/%d", opcode & 0x7f, opcode >> 7);
	else
		printf("op code %d", opcode);

	if (frm->handle > 17)
		printf(" handle %d\n", frm->handle);
	else
		printf("\n");

	if (!(parser.flags & DUMP_VERBOSE)) {
		raw_dump(level, frm);
		return;
	}

	switch (opcode) {
	case 1:
		name_req_dump(level + 1, frm);
		return;
	case 2:
		name_res_dump(level + 1, frm);
		return;
	case 3:
		accepted_dump(level + 1, frm);
		return;
	case 4:
		not_accepted_dump(level + 1, frm);
		return;
	case 6:
		clkoffset_dump(level + 1, frm);
		return;
	case 7:
		detach_dump(level + 1, frm);
		return;
	case 8:
		in_rand(frm);
		random_number_dump(level + 1, frm);
		return;
	case 9:
		comb_key(frm);
		random_number_dump(level + 1, frm);
		return;
	case 11:
		au_rand(frm);
		random_number_dump(level + 1, frm);
		return;
	case 12:
		sres(frm);
		auth_resp_dump(level + 1, frm);
		return;
	case 13:
	case 17:
		random_number_dump(level + 1, frm);
		return;
	case 10:
	case 14:
		key_dump(level + 1, frm);
		return;
	case 15:
		encryption_mode_req_dump(level + 1, frm);
		return;
	case 16:
		encryption_key_size_req_dump(level + 1, frm);
		return;
	case 19:
		switch_req_dump(level + 1, frm);
		return;
	case 20:
	case 21:
		hold_dump(level + 1, frm);
		return;
	case 23:
		sniff_req_dump(level + 1, frm);
		return;
	case 25:
		park_req_dump(level + 1, frm);
		return;
	case 28:
		modify_beacon_dump(level + 1, frm);
		return;
	case 31:
	case 32:
		power_req_dump(level + 1, frm);
		return;
	case 36:
		preferred_rate_dump(level + 1, frm);
		return;
	case 37:
	case 38:
		version_dump(level + 1, frm);
		return;
	case 39:
	case 40:
		features_dump(level + 1, frm);
		return;
	case 41:
	case 42:
		quality_of_service_dump(level + 1, frm);
		return;
	case 43:
		sco_link_req_dump(level + 1, frm);
		return;
	case 44:
		remove_sco_link_req_dump(level + 1, frm);
		return;
	case 45:
	case 46:
		max_slots_dump(level + 1, frm);
		return;
	case 48:
		timing_accuracy_dump(level + 1, frm);
		return;
	case 52:
		slot_offset_dump(level + 1, frm);
		return;
	case 53:
	case 54:
		page_mode_dump(level + 1, frm);
		return;
	case 55:
		supervision_timeout_dump(level + 1, frm);
		return;
	case 57:
		test_control_dump(level + 1, frm);
		return;
	case 59:
		encryption_key_size_mask_res_dump(level + 1, frm);
		return;
	case 60:
		set_afh_dump(level + 1, frm);
		return;
	case 61:
		encapsulated_header_dump(level + 1, frm);
		return;
	case 62:
		encapsulated_payload_dump(level + 1, frm);
		return;
	case 63:
		simple_pairing_confirm_dump(level + 1, frm);
		return;
	case 64:
		simple_pairing_number_dump(level + 1, frm);
		return;
	case 65:
		dhkey_check_dump(level + 1, frm);
		return;
	case 5:
	case 18:
	case 24:
	case 33:
	case 34:
	case 35:
	case 47:
	case 49:
	case 50:
	case 51:
	case 56:
	case 58:
	case 127 + (23 << 7):
	case 127 + (24 << 7):
	case 127 + (27 << 7):
	case 127 + (28 << 7):
	case 127 + (29 << 7):
		return;
	case 127 + (1 << 7):
		accepted_ext_dump(level + 1, frm);
		return;
	case 127 + (2 << 7):
		not_accepted_ext_dump(level + 1, frm);
		return;
	case 127 + (3 << 7):
	case 127 + (4 << 7):
		features_ext_dump(level + 1, frm);
		return;
	case 127 + (11 << 7):
		packet_type_table_dump(level + 1, frm);
		return;
	case 127 + (12 << 7):
		esco_link_req_dump(level + 1, frm);
		return;
	case 127 + (13 << 7):
		remove_esco_link_req_dump(level + 1, frm);
		return;
	case 127 + (16 << 7):
		channel_classification_req_dump(level + 1, frm);
		return;
	case 127 + (17 << 7):
		channel_classification_dump(level + 1, frm);
		return;
	case 127 + (21 << 7):
	case 127 + (22 << 7):
		sniff_subrating_dump(level + 1, frm);
		return;
	case 127 + (25 << 7):
	case 127 + (26 << 7):
		io_capability_dump(level + 1, frm);
		return;
	case 127 + (30 << 7):
		keypress_notification_dump(level + 1, frm);
		return;
	}

	raw_dump(level, frm);
}