C++ packet_hexdump函数代码示例

void avctp_packet(const struct l2cap_frame *frame)
{
	struct l2cap_frame *l2cap_frame;
	struct avctp_frame avctp_frame;
	const char *pdu_color;

	l2cap_frame_pull(&avctp_frame.l2cap_frame, frame, 0);

	l2cap_frame = &avctp_frame.l2cap_frame;

	if (!l2cap_frame_get_u8(l2cap_frame, &avctp_frame.hdr) ||
			!l2cap_frame_get_be16(l2cap_frame, &avctp_frame.pid)) {
		print_text(COLOR_ERROR, "frame too short");
		packet_hexdump(frame->data, frame->size);
		return;
	}

	if (frame->in)
		pdu_color = COLOR_MAGENTA;
	else
		pdu_color = COLOR_BLUE;

	print_indent(6, pdu_color, "AVCTP", "", COLOR_OFF,
				" %s: %s: type 0x%02x label %d PID 0x%04x",
				frame->psm == 23 ? "Control" : "Browsing",
				avctp_frame.hdr & 0x02 ? "Response" : "Command",
				avctp_frame.hdr & 0x0c, avctp_frame.hdr >> 4,
				avctp_frame.pid);

	if (avctp_frame.pid == 0x110e || avctp_frame.pid == 0x110c)
		avrcp_packet(&avctp_frame);
	else
		packet_hexdump(frame->data, frame->size);
}

static void attr_req(const struct l2cap_frame *frame, struct tid_data *tid)
{
	uint32_t attr_bytes;

	if (frame->size < 6) {
		print_text(COLOR_ERROR, "invalid size");
		packet_hexdump(frame->data, frame->size);
		return;
	}

	print_field("Record handle: 0x%4.4x", get_be32(frame->data));
	print_field("Max attribute bytes: %d", get_be16(frame->data + 4));

	attr_bytes = get_bytes(frame->data + 6, frame->size - 6);
	print_field("Attribute list: [len %d]", attr_bytes);

	if (attr_bytes + 6 > frame->size) {
		print_text(COLOR_ERROR, "invalid attribute list length");
		packet_hexdump(frame->data, frame->size);
		return;
	}

	decode_data_elements(0, 2, frame->data + 6, attr_bytes, NULL);

	store_continuation(tid, frame->data + 6 + attr_bytes,
					frame->size - 6 - attr_bytes);
}

void sdp_packet(const struct l2cap_frame *frame)
{
	uint8_t pdu;
	uint16_t tid, plen;
	struct l2cap_frame sdp_frame;
	struct tid_data *tid_info;
	const struct sdp_data *sdp_data = NULL;
	const char *pdu_color, *pdu_str;
	int i;

	l2cap_frame_pull(&sdp_frame, frame, 0);

	if (!l2cap_frame_get_u8(&sdp_frame, &pdu) ||
				!l2cap_frame_get_be16(&sdp_frame, &tid) ||
				!l2cap_frame_get_be16(&sdp_frame, &plen)) {
		print_text(COLOR_ERROR, "frame too short");
		packet_hexdump(frame->data, frame->size);
		return;
	}

	if (sdp_frame.size != plen) {
		print_text(COLOR_ERROR, "invalid frame size");
		packet_hexdump(sdp_frame.data, sdp_frame.size);
		return;
	}

	for (i = 0; sdp_table[i].str; i++) {
		if (sdp_table[i].pdu == pdu) {
			sdp_data = &sdp_table[i];
			break;
		}
	}

	if (sdp_data) {
		if (sdp_data->func) {
			if (frame->in)
				pdu_color = COLOR_MAGENTA;
			else
				pdu_color = COLOR_BLUE;
		} else
			pdu_color = COLOR_WHITE_BG;
		pdu_str = sdp_data->str;
	} else {
		pdu_color = COLOR_WHITE_BG;
		pdu_str = "Unknown";
	}

	print_indent(6, pdu_color, "SDP: ", pdu_str, COLOR_OFF,
				" (0x%2.2x) tid %d len %d", pdu, tid, plen);

	tid_info = get_tid(tid, frame->chan);

	if (!sdp_data || !sdp_data->func || !tid_info) {
		packet_hexdump(sdp_frame.data, sdp_frame.size);
		return;
	}

	sdp_data->func(&sdp_frame, tid_info);
}

static void print_continuation(const uint8_t *data, uint16_t size)
{
	if (data[0] != size - 1) {
		print_text(COLOR_ERROR, "invalid continuation state");
		packet_hexdump(data, size);
		return;
	}

	print_field("Continuation state: %d", data[0]);
	packet_hexdump(data + 1, size - 1);
}

static void data_packet(const void *data, uint8_t size)
{
	const uint8_t *ptr = data;
	uint8_t llid, length;
	bool nesn, sn, md;
	const char *str;

	if (size < 2) {
		print_text(COLOR_ERROR, "packet too short");
		packet_hexdump(data, size);
		return;
	}

	llid = ptr[0] & 0x03;
	nesn = !!(ptr[0] & 0x04);
	sn = !!(ptr[0] & 0x08);
	md = !!(ptr[0] & 0x10);
	length = ptr[1] & 0x1f;

	switch (llid) {
	case 0x01:
		if (length > 0)
			str = "Continuation fragement of L2CAP message";
		else
			str = "Empty message";
		break;
	case 0x02:
		str = "Start of L2CAP message";
		break;
	case 0x03:
		str = "Control";
		break;
	default:
		str = "Reserved";
		break;
	}

	print_field("LLID: %s (0x%2.2x)", str, llid);
	print_field("Next expected sequence number: %u", nesn);
	print_field("Sequence number: %u", sn);
	print_field("More data: %u", md);
	print_field("Length: %u", length);

	switch (llid) {
	case 0x03:
		llcp_packet(data + 2, size - 2);
		break;

	default:
		packet_hexdump(data + 2, size - 2);
		break;
	}
}

static bool avrcp_control_packet(struct avctp_frame *avctp_frame)
{
	struct l2cap_frame *frame = &avctp_frame->l2cap_frame;

	uint8_t ctype, address, subunit, opcode, company[3], indent = 2;

	if (!l2cap_frame_get_u8(frame, &ctype) ||
				!l2cap_frame_get_u8(frame, &address) ||
				!l2cap_frame_get_u8(frame, &opcode))
		return false;

	print_field("AV/C: %s: address 0x%02x opcode 0x%02x",
				ctype2str(ctype), address, opcode);

	subunit = address >> 3;

	print_field("%*cSubunit: %s", indent, ' ', subunit2str(subunit));

	print_field("%*cOpcode: %s", indent, ' ', opcode2str(opcode));

	/* Skip non-panel subunit packets */
	if (subunit != 0x09) {
		packet_hexdump(frame->data, frame->size);
		return true;
	}

	/* Not implemented should not contain any operand */
	if (ctype == 0x8) {
		packet_hexdump(frame->data, frame->size);
		return true;
	}

	switch (opcode) {
	case 0x7c:
		return avrcp_passthrough_packet(avctp_frame);
	case 0x00:
		if (!l2cap_frame_get_u8(frame, &company[0]) ||
				!l2cap_frame_get_u8(frame, &company[1]) ||
				!l2cap_frame_get_u8(frame, &company[2]))
			return false;

		print_field("%*cCompany ID: 0x%02x%02x%02x", indent, ' ',
					company[0], company[1], company[2]);

		return avrcp_pdu_packet(avctp_frame, ctype, 10);
	default:
		packet_hexdump(frame->data, frame->size);
		return true;
	}
}

void llcp_packet(const void *data, uint8_t size)
{
	uint8_t opcode = ((const uint8_t *) data)[0];
	const struct llcp_data *llcp_data = NULL;
	const char *opcode_color, *opcode_str;
	int i;

	for (i = 0; llcp_table[i].str; i++) {
		if (llcp_table[i].opcode == opcode) {
			llcp_data = &llcp_table[i];
			break;
		}
	}

	if (llcp_data) {
		if (llcp_data->func)
			opcode_color = COLOR_OPCODE;
		else
			opcode_color = COLOR_OPCODE_UNKNOWN;
		opcode_str = llcp_data->str;
	} else {
		opcode_color = COLOR_OPCODE_UNKNOWN;
		opcode_str = "Unknown";
	}

	print_indent(6, opcode_color, "", opcode_str, COLOR_OFF,
						" (0x%2.2x)", opcode);

	if (!llcp_data || !llcp_data->func) {
		packet_hexdump(data + 1, size - 1);
		return;
	}

	if (llcp_data->fixed) {
		if (size - 1 != llcp_data->size) {
			print_text(COLOR_ERROR, "invalid packet size");
			packet_hexdump(data + 1, size - 1);
			return;
		}
	} else {
		if (size - 1 < llcp_data->size) {
			print_text(COLOR_ERROR, "too short packet");
			packet_hexdump(data + 1, size - 1);
			return;
		}
	}

	llcp_data->func(data + 1, size - 1);
}

static void service_rsp(const struct l2cap_frame *frame, struct tid_data *tid)
{
	uint16_t count;
	int i;

	clear_tid(tid);

	if (frame->size < 4) {
		print_text(COLOR_ERROR, "invalid size");
		packet_hexdump(frame->data, frame->size);
		return;
	}

	count = get_be16(frame->data + 2);
	if (count * 4 > frame->size) {
		print_text(COLOR_ERROR, "invalid record count");
                return;
	}

	print_field("Total record count: %d", get_be16(frame->data));
	print_field("Current record count: %d", count);

	for (i = 0; i < count; i++)
		print_field("Record handle: 0x%4.4x",
				get_be32(frame->data + 4 + (i * 4)));

	print_continuation(frame->data + 4 + (count * 4),
					frame->size - 4 - (count * 4));
}

static void print_uuid(uint8_t indent, const uint8_t *data, uint32_t size)
{
	switch (size) {
	case 2:
		print_field("%*c%s (0x%4.4x)", indent, ' ',
			bt_uuid16_to_str(get_be16(data)), get_be16(data));
		break;
	case 4:
		print_field("%*c%s (0x%8.8x)", indent, ' ',
			bt_uuid32_to_str(get_be32(data)), get_be32(data));
		break;
	case 16:
		/* BASE_UUID = 00000000-0000-1000-8000-00805F9B34FB */
		print_field("%*c%8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.4x",
				indent, ' ',
				get_be32(data), get_be16(data + 4),
				get_be16(data + 6), get_be16(data + 8),
				get_be16(data + 10), get_be32(data + 12));
		if (get_be16(data + 4) == 0x0000 &&
				get_be16(data + 6) == 0x1000 &&
				get_be16(data + 8) == 0x8000 &&
				get_be16(data + 10) == 0x0080 &&
				get_be32(data + 12) == 0x5F9B34FB)
			print_field("%*c%s", indent, ' ',
				bt_uuid32_to_str(get_be32(data)));
		break;
	default:
		packet_hexdump(data, size);
		break;
	}
}

static bool avrcp_passthrough_packet(struct avctp_frame *avctp_frame)
{
	struct l2cap_frame *frame = &avctp_frame->l2cap_frame;

	packet_hexdump(frame->data, frame->size);
	return true;
}

void packet_hci_acldata(struct timeval *tv, uint16_t index, bool in,
					const void *data, uint16_t size)
{
	const hci_acl_hdr *hdr = data;
	uint16_t handle = btohs(hdr->handle);
	uint16_t dlen = btohs(hdr->dlen);
	uint8_t flags = acl_flags(handle);

	btsnoop_write(tv, index, in ? 0x01 : 0x00, data, size);

	print_header(tv, index);

	if (size < HCI_ACL_HDR_SIZE) {
		printf("* Malformed ACL Data %s packet\n", in ? "RX" : "TX");
		return;
	}

	printf("%c ACL Data: handle %d flags 0x%2.2x dlen %d\n",
			in ? '>' : '<', acl_handle(handle), flags, dlen);

	data += HCI_ACL_HDR_SIZE;
	size -= HCI_ACL_HDR_SIZE;

	if (filter_mask & PACKET_FILTER_SHOW_ACL_DATA)
		packet_hexdump(data, size);
}

static void mgmt_device_disconnected(uint16_t len, const void *buf)
{
	const struct mgmt_ev_device_disconnected *ev = buf;
	char str[18];
	uint8_t reason;
	uint16_t consumed_len;

	if (len < sizeof(struct mgmt_addr_info)) {
		printf("* Malformed Device Disconnected control\n");
		return;
	}

	if (len < sizeof(*ev)) {
		reason = MGMT_DEV_DISCONN_UNKNOWN;
		consumed_len = len;
	} else {
		reason = ev->reason;
		consumed_len = sizeof(*ev);
	}

	ba2str(&ev->addr.bdaddr, str);

	printf("@ Device Disconnected: %s (%d) reason %u\n", str, ev->addr.type,
									reason);

	buf += consumed_len;
	len -= consumed_len;

	packet_hexdump(buf, len);
}

static void search_attr_req(const struct l2cap_frame *frame,
						struct tid_data *tid)
{
	uint32_t search_bytes, attr_bytes;

	search_bytes = get_bytes(frame->data, frame->size);
	print_field("Search pattern: [len %d]", search_bytes);

	if (search_bytes + 2 > frame->size) {
		print_text(COLOR_ERROR, "invalid search list length");
		packet_hexdump(frame->data, frame->size);
		return;
	}

	decode_data_elements(0, 2, frame->data, search_bytes, NULL);

	print_field("Max record count: %d",
				get_be16(frame->data + search_bytes));

	attr_bytes = get_bytes(frame->data + search_bytes + 2,
				frame->size - search_bytes - 2);
	print_field("Attribute list: [len %d]", attr_bytes);

	if (search_bytes + attr_bytes > frame->size) {
		print_text(COLOR_ERROR, "invalid attribute list length");
		return;
	}

	decode_data_elements(0, 2, frame->data + search_bytes + 2,
						attr_bytes, NULL);

	store_continuation(tid, frame->data + search_bytes + 2 + attr_bytes,
				frame->size - search_bytes - 2 - attr_bytes);
}

static inline bool mcc_msc(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
{
	struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
	struct rfcomm_lmsc msc;

	if (!l2cap_frame_get_u8(frame, &msc.dlci))
		return false;

	print_field("%*cdlci %d ", indent, ' ', RFCOMM_GET_DLCI(msc.dlci));

	if (!l2cap_frame_get_u8(frame, &msc.v24_sig))
		return false;

	/* v24 control signals */
	print_field("%*cfc %d rtc %d rtr %d ic %d dv %d", indent, ' ',
		GET_V24_FC(msc.v24_sig), GET_V24_RTC(msc.v24_sig),
		GET_V24_RTR(msc.v24_sig), GET_V24_IC(msc.v24_sig),
					GET_V24_DV(msc.v24_sig));

	if (frame->size < 2)
		goto done;

	/*
	 * TODO: Implement the break signals decoding.
	 */

	packet_hexdump(frame->data, frame->size);

done:
	return true;
}

static void mgmt_new_settings(uint16_t len, const void *buf)
{
	uint32_t settings;
	unsigned int i;

	if (len < 4) {
		printf("* Malformed New Settings control\n");
		return;
	}

	settings = get_le32(buf);

	printf("@ New Settings: 0x%4.4x\n", settings);

	if (settings) {
		printf("%-12c", ' ');
		for (i = 0; i < NELEM(settings_str); i++) {
			if (settings & (1 << i))
				printf("%s ", settings_str[i]);
		}
		printf("\n");
	}

	buf += 4;
	len -= 4;

	packet_hexdump(buf, len);
}