[PATCH v2 4/7] monitor: Move ATT decoding function into its own file

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



From: Luiz Augusto von Dentz <luiz.von.dentz@xxxxxxxxx>

This moves ATT decoding function from l2cap.c to att.c.
---
 Makefile.tools  |   6 +-
 monitor/att.c   | 642 ++++++++++++++++++++++++++++++++++++++++++++++++
 monitor/att.h   |  16 ++
 monitor/l2cap.c | 616 +---------------------------------------------
 monitor/l2cap.h |   5 +
 5 files changed, 669 insertions(+), 616 deletions(-)
 create mode 100644 monitor/att.c
 create mode 100644 monitor/att.h

diff --git a/Makefile.tools b/Makefile.tools
index b7d893248..a152dad6f 100644
--- a/Makefile.tools
+++ b/Makefile.tools
@@ -48,9 +48,11 @@ monitor_btmon_SOURCES = monitor/main.c monitor/bt.h \
 				monitor/broadcom.h monitor/broadcom.c \
 				monitor/msft.h monitor/msft.c \
 				monitor/jlink.h monitor/jlink.c \
-				monitor/tty.h monitor/emulator.h
+				monitor/tty.h monitor/emulator.h \
+				monitor/att.h monitor/att.c
 monitor_btmon_LDADD = lib/libbluetooth-internal.la \
-				src/libshared-mainloop.la $(UDEV_LIBS) -ldl
+				src/libshared-mainloop.la \
+				$(GLIB_LIBS) $(UDEV_LIBS) -ldl
 
 if MANPAGES
 man_MANS += monitor/btmon.1
diff --git a/monitor/att.c b/monitor/att.c
new file mode 100644
index 000000000..5ad8244db
--- /dev/null
+++ b/monitor/att.c
@@ -0,0 +1,642 @@
+// SPDX-License-Identifier: LGPL-2.1-or-later
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2011-2014  Intel Corporation
+ *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@xxxxxxxxxxxx>
+ *
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#define _GNU_SOURCE
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <inttypes.h>
+
+#include "lib/bluetooth.h"
+#include "lib/uuid.h"
+
+#include "src/shared/util.h"
+#include "bt.h"
+#include "packet.h"
+#include "display.h"
+#include "l2cap.h"
+#include "att.h"
+
+static void print_uuid(const char *label, const void *data, uint16_t size)
+{
+	const char *str;
+	char uuidstr[MAX_LEN_UUID_STR];
+
+	switch (size) {
+	case 2:
+		str = bt_uuid16_to_str(get_le16(data));
+		print_field("%s: %s (0x%4.4x)", label, str, get_le16(data));
+		break;
+	case 4:
+		str = bt_uuid32_to_str(get_le32(data));
+		print_field("%s: %s (0x%8.8x)", label, str, get_le32(data));
+		break;
+	case 16:
+		sprintf(uuidstr, "%8.8x-%4.4x-%4.4x-%4.4x-%8.8x%4.4x",
+				get_le32(data + 12), get_le16(data + 10),
+				get_le16(data + 8), get_le16(data + 6),
+				get_le32(data + 2), get_le16(data + 0));
+		str = bt_uuidstr_to_str(uuidstr);
+		print_field("%s: %s (%s)", label, str, uuidstr);
+		break;
+	default:
+		packet_hexdump(data, size);
+		break;
+	}
+}
+
+static void print_handle_range(const char *label, const void *data)
+{
+	print_field("%s: 0x%4.4x-0x%4.4x", label,
+				get_le16(data), get_le16(data + 2));
+}
+
+static void print_data_list(const char *label, uint8_t length,
+					const void *data, uint16_t size)
+{
+	uint8_t count;
+
+	if (length == 0)
+		return;
+
+	count = size / length;
+
+	print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies");
+
+	while (size >= length) {
+		print_field("Handle: 0x%4.4x", get_le16(data));
+		print_hex_field("Value", data + 2, length - 2);
+
+		data += length;
+		size -= length;
+	}
+
+	packet_hexdump(data, size);
+}
+
+static void print_attribute_info(uint16_t type, const void *data, uint16_t len)
+{
+	const char *str = bt_uuid16_to_str(type);
+
+	print_field("%s: %s (0x%4.4x)", "Attribute type", str, type);
+
+	switch (type) {
+	case 0x2800:	/* Primary Service */
+	case 0x2801:	/* Secondary Service */
+		print_uuid("  UUID", data, len);
+		break;
+	case 0x2802:	/* Include */
+		if (len < 4) {
+			print_hex_field("  Value", data, len);
+			break;
+		}
+		print_handle_range("  Handle range", data);
+		print_uuid("  UUID", data + 4, len - 4);
+		break;
+	case 0x2803:	/* Characteristic */
+		if (len < 3) {
+			print_hex_field("  Value", data, len);
+			break;
+		}
+		print_field("  Properties: 0x%2.2x", *((uint8_t *) data));
+		print_field("  Handle: 0x%2.2x", get_le16(data + 1));
+		print_uuid("  UUID", data + 3, len - 3);
+		break;
+	default:
+		print_hex_field("Value", data, len);
+		break;
+	}
+}
+
+static const char *att_opcode_to_str(uint8_t opcode);
+
+static void att_error_response(const struct l2cap_frame *frame)
+{
+	const struct bt_l2cap_att_error_response *pdu = frame->data;
+	const char *str;
+
+	switch (pdu->error) {
+	case 0x01:
+		str = "Invalid Handle";
+		break;
+	case 0x02:
+		str = "Read Not Permitted";
+		break;
+	case 0x03:
+		str = "Write Not Permitted";
+		break;
+	case 0x04:
+		str = "Invalid PDU";
+		break;
+	case 0x05:
+		str = "Insufficient Authentication";
+		break;
+	case 0x06:
+		str = "Request Not Supported";
+		break;
+	case 0x07:
+		str = "Invalid Offset";
+		break;
+	case 0x08:
+		str = "Insufficient Authorization";
+		break;
+	case 0x09:
+		str = "Prepare Queue Full";
+		break;
+	case 0x0a:
+		str = "Attribute Not Found";
+		break;
+	case 0x0b:
+		str = "Attribute Not Long";
+		break;
+	case 0x0c:
+		str = "Insufficient Encryption Key Size";
+		break;
+	case 0x0d:
+		str = "Invalid Attribute Value Length";
+		break;
+	case 0x0e:
+		str = "Unlikely Error";
+		break;
+	case 0x0f:
+		str = "Insufficient Encryption";
+		break;
+	case 0x10:
+		str = "Unsupported Group Type";
+		break;
+	case 0x11:
+		str = "Insufficient Resources";
+		break;
+	case 0x12:
+		str = "Database Out of Sync";
+		break;
+	case 0x13:
+		str = "Value Not Allowed";
+		break;
+	case 0xfd:
+		str = "CCC Improperly Configured";
+		break;
+	case 0xfe:
+		str = "Procedure Already in Progress";
+		break;
+	case 0xff:
+		str = "Out of Range";
+		break;
+	default:
+		str = "Reserved";
+		break;
+	}
+
+	print_field("%s (0x%2.2x)", att_opcode_to_str(pdu->request),
+							pdu->request);
+	print_field("Handle: 0x%4.4x", le16_to_cpu(pdu->handle));
+	print_field("Error: %s (0x%2.2x)", str, pdu->error);
+}
+
+static void att_exchange_mtu_req(const struct l2cap_frame *frame)
+{
+	const struct bt_l2cap_att_exchange_mtu_req *pdu = frame->data;
+
+	print_field("Client RX MTU: %d", le16_to_cpu(pdu->mtu));
+}
+
+static void att_exchange_mtu_rsp(const struct l2cap_frame *frame)
+{
+	const struct bt_l2cap_att_exchange_mtu_rsp *pdu = frame->data;
+
+	print_field("Server RX MTU: %d", le16_to_cpu(pdu->mtu));
+}
+
+static void att_find_info_req(const struct l2cap_frame *frame)
+{
+	print_handle_range("Handle range", frame->data);
+}
+
+static const char *att_format_str(uint8_t format)
+{
+	switch (format) {
+	case 0x01:
+		return "UUID-16";
+	case 0x02:
+		return "UUID-128";
+	default:
+		return "unknown";
+	}
+}
+
+static uint16_t print_info_data_16(const void *data, uint16_t len)
+{
+	while (len >= 4) {
+		print_field("Handle: 0x%4.4x", get_le16(data));
+		print_uuid("UUID", data + 2, 2);
+		data += 4;
+		len -= 4;
+	}
+
+	return len;
+}
+
+static uint16_t print_info_data_128(const void *data, uint16_t len)
+{
+	while (len >= 18) {
+		print_field("Handle: 0x%4.4x", get_le16(data));
+		print_uuid("UUID", data + 2, 16);
+		data += 18;
+		len -= 18;
+	}
+
+	return len;
+}
+
+static void att_find_info_rsp(const struct l2cap_frame *frame)
+{
+	const uint8_t *format = frame->data;
+	uint16_t len;
+
+	print_field("Format: %s (0x%2.2x)", att_format_str(*format), *format);
+
+	if (*format == 0x01)
+		len = print_info_data_16(frame->data + 1, frame->size - 1);
+	else if (*format == 0x02)
+		len = print_info_data_128(frame->data + 1, frame->size - 1);
+	else
+		len = frame->size - 1;
+
+	packet_hexdump(frame->data + (frame->size - len), len);
+}
+
+static void att_find_by_type_val_req(const struct l2cap_frame *frame)
+{
+	uint16_t type;
+
+	print_handle_range("Handle range", frame->data);
+
+	type = get_le16(frame->data + 4);
+	print_attribute_info(type, frame->data + 6, frame->size - 6);
+}
+
+static void att_find_by_type_val_rsp(const struct l2cap_frame *frame)
+{
+	const uint8_t *ptr = frame->data;
+	uint16_t len = frame->size;
+
+	while (len >= 4) {
+		print_handle_range("Handle range", ptr);
+		ptr += 4;
+		len -= 4;
+	}
+
+	packet_hexdump(ptr, len);
+}
+
+static void att_read_type_req(const struct l2cap_frame *frame)
+{
+	print_handle_range("Handle range", frame->data);
+	print_uuid("Attribute type", frame->data + 4, frame->size - 4);
+}
+
+static void att_read_type_rsp(const struct l2cap_frame *frame)
+{
+	const struct bt_l2cap_att_read_group_type_rsp *pdu = frame->data;
+
+	print_field("Attribute data length: %d", pdu->length);
+	print_data_list("Attribute data list", pdu->length,
+					frame->data + 1, frame->size - 1);
+}
+
+static void att_read_req(const struct l2cap_frame *frame)
+{
+	const struct bt_l2cap_att_read_req *pdu = frame->data;
+
+	print_field("Handle: 0x%4.4x", le16_to_cpu(pdu->handle));
+}
+
+static void att_read_rsp(const struct l2cap_frame *frame)
+{
+	print_hex_field("Value", frame->data, frame->size);
+}
+
+static void att_read_blob_req(const struct l2cap_frame *frame)
+{
+	print_field("Handle: 0x%4.4x", get_le16(frame->data));
+	print_field("Offset: 0x%4.4x", get_le16(frame->data + 2));
+}
+
+static void att_read_blob_rsp(const struct l2cap_frame *frame)
+{
+	packet_hexdump(frame->data, frame->size);
+}
+
+static void att_read_multiple_req(const struct l2cap_frame *frame)
+{
+	int i, count;
+
+	count = frame->size / 2;
+
+	for (i = 0; i < count; i++)
+		print_field("Handle: 0x%4.4x",
+					get_le16(frame->data + (i * 2)));
+}
+
+static void att_read_group_type_req(const struct l2cap_frame *frame)
+{
+	print_handle_range("Handle range", frame->data);
+	print_uuid("Attribute group type", frame->data + 4, frame->size - 4);
+}
+
+static void print_group_list(const char *label, uint8_t length,
+					const void *data, uint16_t size)
+{
+	uint8_t count;
+
+	if (length == 0)
+		return;
+
+	count = size / length;
+
+	print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies");
+
+	while (size >= length) {
+		print_handle_range("Handle range", data);
+		print_uuid("UUID", data + 4, length - 4);
+
+		data += length;
+		size -= length;
+	}
+
+	packet_hexdump(data, size);
+}
+
+static void att_read_group_type_rsp(const struct l2cap_frame *frame)
+{
+	const struct bt_l2cap_att_read_group_type_rsp *pdu = frame->data;
+
+	print_field("Attribute data length: %d", pdu->length);
+	print_group_list("Attribute group list", pdu->length,
+					frame->data + 1, frame->size - 1);
+}
+
+static void att_write_req(const struct l2cap_frame *frame)
+{
+	print_field("Handle: 0x%4.4x", get_le16(frame->data));
+	print_hex_field("  Data", frame->data + 2, frame->size - 2);
+}
+
+static void att_write_rsp(const struct l2cap_frame *frame)
+{
+}
+
+static void att_prepare_write_req(const struct l2cap_frame *frame)
+{
+	print_field("Handle: 0x%4.4x", get_le16(frame->data));
+	print_field("Offset: 0x%4.4x", get_le16(frame->data + 2));
+	print_hex_field("  Data", frame->data + 4, frame->size - 4);
+}
+
+static void att_prepare_write_rsp(const struct l2cap_frame *frame)
+{
+	print_field("Handle: 0x%4.4x", get_le16(frame->data));
+	print_field("Offset: 0x%4.4x", get_le16(frame->data + 2));
+	print_hex_field("  Data", frame->data + 4, frame->size - 4);
+}
+
+static void att_execute_write_req(const struct l2cap_frame *frame)
+{
+	uint8_t flags = *(uint8_t *) frame->data;
+	const char *flags_str;
+
+	switch (flags) {
+	case 0x00:
+		flags_str = "Cancel all prepared writes";
+		break;
+	case 0x01:
+		flags_str = "Immediately write all pending values";
+		break;
+	default:
+		flags_str = "Unknown";
+		break;
+	}
+
+	print_field("Flags: %s (0x%02x)", flags_str, flags);
+}
+
+static void att_handle_value_notify(const struct l2cap_frame *frame)
+{
+	const struct bt_l2cap_att_handle_value_notify *pdu = frame->data;
+
+	print_field("Handle: 0x%4.4x", le16_to_cpu(pdu->handle));
+	print_hex_field("  Data", frame->data + 2, frame->size - 2);
+}
+
+static void att_handle_value_ind(const struct l2cap_frame *frame)
+{
+	const struct bt_l2cap_att_handle_value_ind *pdu = frame->data;
+
+	print_field("Handle: 0x%4.4x", le16_to_cpu(pdu->handle));
+	print_hex_field("  Data", frame->data + 2, frame->size - 2);
+}
+
+static void att_handle_value_conf(const struct l2cap_frame *frame)
+{
+}
+
+static void att_multiple_vl_rsp(const struct l2cap_frame *frame)
+{
+	struct l2cap_frame *f = (void *) frame;
+
+	while (frame->size) {
+		uint16_t handle;
+		uint16_t len;
+
+		if (!l2cap_frame_get_le16(f, &handle))
+			return;
+
+		print_field("Handle: 0x%4.4x", handle);
+
+		if (!l2cap_frame_get_le16(f, &len))
+			return;
+
+		print_field("Length: 0x%4.4x", len);
+
+		print_hex_field("  Data", f->data,
+				len < f->size ? len : f->size);
+
+		if (len > f->size) {
+			print_text(COLOR_ERROR, "invalid size");
+			return;
+		}
+
+		l2cap_frame_pull(f, f, len);
+	}
+}
+
+static void att_write_command(const struct l2cap_frame *frame)
+{
+	print_field("Handle: 0x%4.4x", get_le16(frame->data));
+	print_hex_field("  Data", frame->data + 2, frame->size - 2);
+}
+
+static void att_signed_write_command(const struct l2cap_frame *frame)
+{
+	print_field("Handle: 0x%4.4x", get_le16(frame->data));
+	print_hex_field("  Data", frame->data + 2, frame->size - 2 - 12);
+	print_hex_field("  Signature", frame->data + frame->size - 12, 12);
+}
+
+struct att_opcode_data {
+	uint8_t opcode;
+	const char *str;
+	void (*func) (const struct l2cap_frame *frame);
+	uint8_t size;
+	bool fixed;
+};
+
+static const struct att_opcode_data att_opcode_table[] = {
+	{ 0x01, "Error Response",
+			att_error_response, 4, true },
+	{ 0x02, "Exchange MTU Request",
+			att_exchange_mtu_req, 2, true },
+	{ 0x03, "Exchange MTU Response",
+			att_exchange_mtu_rsp, 2, true },
+	{ 0x04, "Find Information Request",
+			att_find_info_req, 4, true },
+	{ 0x05, "Find Information Response",
+			att_find_info_rsp, 5, false },
+	{ 0x06, "Find By Type Value Request",
+			att_find_by_type_val_req, 6, false },
+	{ 0x07, "Find By Type Value Response",
+			att_find_by_type_val_rsp, 4, false },
+	{ 0x08, "Read By Type Request",
+			att_read_type_req, 6, false },
+	{ 0x09, "Read By Type Response",
+			att_read_type_rsp, 3, false },
+	{ 0x0a, "Read Request",
+			att_read_req, 2, true },
+	{ 0x0b, "Read Response",
+			att_read_rsp, 0, false },
+	{ 0x0c, "Read Blob Request",
+			att_read_blob_req, 4, true },
+	{ 0x0d, "Read Blob Response",
+			att_read_blob_rsp, 0, false },
+	{ 0x0e, "Read Multiple Request",
+			att_read_multiple_req, 4, false },
+	{ 0x0f, "Read Multiple Response"	},
+	{ 0x10, "Read By Group Type Request",
+			att_read_group_type_req, 6, false },
+	{ 0x11, "Read By Group Type Response",
+			att_read_group_type_rsp, 4, false },
+	{ 0x12, "Write Request"	,
+			att_write_req, 2, false	},
+	{ 0x13, "Write Response",
+			att_write_rsp, 0, true	},
+	{ 0x16, "Prepare Write Request",
+			att_prepare_write_req, 4, false },
+	{ 0x17, "Prepare Write Response",
+			att_prepare_write_rsp, 4, false },
+	{ 0x18, "Execute Write Request",
+			att_execute_write_req, 1, true },
+	{ 0x19, "Execute Write Response"	},
+	{ 0x1b, "Handle Value Notification",
+			att_handle_value_notify, 2, false },
+	{ 0x1d, "Handle Value Indication",
+			att_handle_value_ind, 2, false },
+	{ 0x1e, "Handle Value Confirmation",
+			att_handle_value_conf, 0, true },
+	{ 0x20, "Read Multiple Request Variable Length",
+			att_read_multiple_req, 4, false },
+	{ 0x21, "Read Multiple Response Variable Length",
+			att_multiple_vl_rsp, 4, false },
+	{ 0x23, "Handle Multiple Value Notification",
+			att_multiple_vl_rsp, 4, false },
+	{ 0x52, "Write Command",
+			att_write_command, 2, false },
+	{ 0xd2, "Signed Write Command", att_signed_write_command, 14, false },
+	{ }
+};
+
+static const char *att_opcode_to_str(uint8_t opcode)
+{
+	int i;
+
+	for (i = 0; att_opcode_table[i].str; i++) {
+		if (att_opcode_table[i].opcode == opcode)
+			return att_opcode_table[i].str;
+	}
+
+	return "Unknown";
+}
+
+void att_packet(uint16_t index, bool in, uint16_t handle, uint16_t cid,
+					const void *data, uint16_t size)
+{
+	struct l2cap_frame frame;
+	uint8_t opcode = *((const uint8_t *) data);
+	const struct att_opcode_data *opcode_data = NULL;
+	const char *opcode_color, *opcode_str;
+	int i;
+
+	if (size < 1) {
+		print_text(COLOR_ERROR, "malformed attribute packet");
+		packet_hexdump(data, size);
+		return;
+	}
+
+	for (i = 0; att_opcode_table[i].str; i++) {
+		if (att_opcode_table[i].opcode == opcode) {
+			opcode_data = &att_opcode_table[i];
+			break;
+		}
+	}
+
+	if (opcode_data) {
+		if (opcode_data->func) {
+			if (in)
+				opcode_color = COLOR_MAGENTA;
+			else
+				opcode_color = COLOR_BLUE;
+		} else
+			opcode_color = COLOR_WHITE_BG;
+		opcode_str = opcode_data->str;
+	} else {
+		opcode_color = COLOR_WHITE_BG;
+		opcode_str = "Unknown";
+	}
+
+	print_indent(6, opcode_color, "ATT: ", opcode_str, COLOR_OFF,
+				" (0x%2.2x) len %d", opcode, size - 1);
+
+	if (!opcode_data || !opcode_data->func) {
+		packet_hexdump(data + 1, size - 1);
+		return;
+	}
+
+	if (opcode_data->fixed) {
+		if (size - 1 != opcode_data->size) {
+			print_text(COLOR_ERROR, "invalid size");
+			packet_hexdump(data + 1, size - 1);
+			return;
+		}
+	} else {
+		if (size - 1 < opcode_data->size) {
+			print_text(COLOR_ERROR, "too short packet");
+			packet_hexdump(data + 1, size - 1);
+			return;
+		}
+	}
+
+	l2cap_frame_init(&frame, index, in, handle, 0, cid, 0,
+						data + 1, size - 1);
+	opcode_data->func(&frame);
+}
diff --git a/monitor/att.h b/monitor/att.h
new file mode 100644
index 000000000..07951b200
--- /dev/null
+++ b/monitor/att.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2011-2014  Intel Corporation
+ *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@xxxxxxxxxxxx>
+ *
+ *
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+void att_packet(uint16_t index, bool in, uint16_t handle, uint16_t cid,
+					const void *data, uint16_t size);
diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index 192b6c920..3f5554e5e 100644
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
@@ -33,7 +33,7 @@
 #include "avdtp.h"
 #include "rfcomm.h"
 #include "bnep.h"
-
+#include "att.h"
 
 #define L2CAP_MODE_BASIC		0x00
 #define L2CAP_MODE_RETRANS		0x01
@@ -1538,7 +1538,7 @@ static const struct sig_opcode_data le_sig_opcode_table[] = {
 	{ },
 };
 
-static void l2cap_frame_init(struct l2cap_frame *frame, uint16_t index, bool in,
+void l2cap_frame_init(struct l2cap_frame *frame, uint16_t index, bool in,
 				uint16_t handle, uint8_t ident,
 				uint16_t cid, uint16_t psm,
 				const void *data, uint16_t size)
@@ -2123,618 +2123,6 @@ static void amp_packet(uint16_t index, bool in, uint16_t handle,
 	opcode_data->func(&frame);
 }
 
-static void print_uuid(const char *label, const void *data, uint16_t size)
-{
-	const char *str;
-	char uuidstr[MAX_LEN_UUID_STR];
-
-	switch (size) {
-	case 2:
-		str = bt_uuid16_to_str(get_le16(data));
-		print_field("%s: %s (0x%4.4x)", label, str, get_le16(data));
-		break;
-	case 4:
-		str = bt_uuid32_to_str(get_le32(data));
-		print_field("%s: %s (0x%8.8x)", label, str, get_le32(data));
-		break;
-	case 16:
-		sprintf(uuidstr, "%8.8x-%4.4x-%4.4x-%4.4x-%8.8x%4.4x",
-				get_le32(data + 12), get_le16(data + 10),
-				get_le16(data + 8), get_le16(data + 6),
-				get_le32(data + 2), get_le16(data + 0));
-		str = bt_uuidstr_to_str(uuidstr);
-		print_field("%s: %s (%s)", label, str, uuidstr);
-		break;
-	default:
-		packet_hexdump(data, size);
-		break;
-	}
-}
-
-static void print_handle_range(const char *label, const void *data)
-{
-	print_field("%s: 0x%4.4x-0x%4.4x", label,
-				get_le16(data), get_le16(data + 2));
-}
-
-static void print_data_list(const char *label, uint8_t length,
-					const void *data, uint16_t size)
-{
-	uint8_t count;
-
-	if (length == 0)
-		return;
-
-	count = size / length;
-
-	print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies");
-
-	while (size >= length) {
-		print_field("Handle: 0x%4.4x", get_le16(data));
-		print_hex_field("Value", data + 2, length - 2);
-
-		data += length;
-		size -= length;
-	}
-
-	packet_hexdump(data, size);
-}
-
-static void print_attribute_info(uint16_t type, const void *data, uint16_t len)
-{
-	const char *str = bt_uuid16_to_str(type);
-
-	print_field("%s: %s (0x%4.4x)", "Attribute type", str, type);
-
-	switch (type) {
-	case 0x2800:	/* Primary Service */
-	case 0x2801:	/* Secondary Service */
-		print_uuid("  UUID", data, len);
-		break;
-	case 0x2802:	/* Include */
-		if (len < 4) {
-			print_hex_field("  Value", data, len);
-			break;
-		}
-		print_handle_range("  Handle range", data);
-		print_uuid("  UUID", data + 4, len - 4);
-		break;
-	case 0x2803:	/* Characteristic */
-		if (len < 3) {
-			print_hex_field("  Value", data, len);
-			break;
-		}
-		print_field("  Properties: 0x%2.2x", *((uint8_t *) data));
-		print_field("  Handle: 0x%2.2x", get_le16(data + 1));
-		print_uuid("  UUID", data + 3, len - 3);
-		break;
-	default:
-		print_hex_field("Value", data, len);
-		break;
-	}
-}
-
-static const char *att_opcode_to_str(uint8_t opcode);
-
-static void att_error_response(const struct l2cap_frame *frame)
-{
-	const struct bt_l2cap_att_error_response *pdu = frame->data;
-	const char *str;
-
-	switch (pdu->error) {
-	case 0x01:
-		str = "Invalid Handle";
-		break;
-	case 0x02:
-		str = "Read Not Permitted";
-		break;
-	case 0x03:
-		str = "Write Not Permitted";
-		break;
-	case 0x04:
-		str = "Invalid PDU";
-		break;
-	case 0x05:
-		str = "Insufficient Authentication";
-		break;
-	case 0x06:
-		str = "Request Not Supported";
-		break;
-	case 0x07:
-		str = "Invalid Offset";
-		break;
-	case 0x08:
-		str = "Insufficient Authorization";
-		break;
-	case 0x09:
-		str = "Prepare Queue Full";
-		break;
-	case 0x0a:
-		str = "Attribute Not Found";
-		break;
-	case 0x0b:
-		str = "Attribute Not Long";
-		break;
-	case 0x0c:
-		str = "Insufficient Encryption Key Size";
-		break;
-	case 0x0d:
-		str = "Invalid Attribute Value Length";
-		break;
-	case 0x0e:
-		str = "Unlikely Error";
-		break;
-	case 0x0f:
-		str = "Insufficient Encryption";
-		break;
-	case 0x10:
-		str = "Unsupported Group Type";
-		break;
-	case 0x11:
-		str = "Insufficient Resources";
-		break;
-	case 0x12:
-		str = "Database Out of Sync";
-		break;
-	case 0x13:
-		str = "Value Not Allowed";
-		break;
-	case 0xfd:
-		str = "CCC Improperly Configured";
-		break;
-	case 0xfe:
-		str = "Procedure Already in Progress";
-		break;
-	case 0xff:
-		str = "Out of Range";
-		break;
-	default:
-		str = "Reserved";
-		break;
-	}
-
-	print_field("%s (0x%2.2x)", att_opcode_to_str(pdu->request),
-							pdu->request);
-	print_field("Handle: 0x%4.4x", le16_to_cpu(pdu->handle));
-	print_field("Error: %s (0x%2.2x)", str, pdu->error);
-}
-
-static void att_exchange_mtu_req(const struct l2cap_frame *frame)
-{
-	const struct bt_l2cap_att_exchange_mtu_req *pdu = frame->data;
-
-	print_field("Client RX MTU: %d", le16_to_cpu(pdu->mtu));
-}
-
-static void att_exchange_mtu_rsp(const struct l2cap_frame *frame)
-{
-	const struct bt_l2cap_att_exchange_mtu_rsp *pdu = frame->data;
-
-	print_field("Server RX MTU: %d", le16_to_cpu(pdu->mtu));
-}
-
-static void att_find_info_req(const struct l2cap_frame *frame)
-{
-	print_handle_range("Handle range", frame->data);
-}
-
-static const char *att_format_str(uint8_t format)
-{
-	switch (format) {
-	case 0x01:
-		return "UUID-16";
-	case 0x02:
-		return "UUID-128";
-	default:
-		return "unknown";
-	}
-}
-
-static uint16_t print_info_data_16(const void *data, uint16_t len)
-{
-	while (len >= 4) {
-		print_field("Handle: 0x%4.4x", get_le16(data));
-		print_uuid("UUID", data + 2, 2);
-		data += 4;
-		len -= 4;
-	}
-
-	return len;
-}
-
-static uint16_t print_info_data_128(const void *data, uint16_t len)
-{
-	while (len >= 18) {
-		print_field("Handle: 0x%4.4x", get_le16(data));
-		print_uuid("UUID", data + 2, 16);
-		data += 18;
-		len -= 18;
-	}
-
-	return len;
-}
-
-static void att_find_info_rsp(const struct l2cap_frame *frame)
-{
-	const uint8_t *format = frame->data;
-	uint16_t len;
-
-	print_field("Format: %s (0x%2.2x)", att_format_str(*format), *format);
-
-	if (*format == 0x01)
-		len = print_info_data_16(frame->data + 1, frame->size - 1);
-	else if (*format == 0x02)
-		len = print_info_data_128(frame->data + 1, frame->size - 1);
-	else
-		len = frame->size - 1;
-
-	packet_hexdump(frame->data + (frame->size - len), len);
-}
-
-static void att_find_by_type_val_req(const struct l2cap_frame *frame)
-{
-	uint16_t type;
-
-	print_handle_range("Handle range", frame->data);
-
-	type = get_le16(frame->data + 4);
-	print_attribute_info(type, frame->data + 6, frame->size - 6);
-}
-
-static void att_find_by_type_val_rsp(const struct l2cap_frame *frame)
-{
-	const uint8_t *ptr = frame->data;
-	uint16_t len = frame->size;
-
-	while (len >= 4) {
-		print_handle_range("Handle range", ptr);
-		ptr += 4;
-		len -= 4;
-	}
-
-	packet_hexdump(ptr, len);
-}
-
-static void att_read_type_req(const struct l2cap_frame *frame)
-{
-	print_handle_range("Handle range", frame->data);
-	print_uuid("Attribute type", frame->data + 4, frame->size - 4);
-}
-
-static void att_read_type_rsp(const struct l2cap_frame *frame)
-{
-	const struct bt_l2cap_att_read_group_type_rsp *pdu = frame->data;
-
-	print_field("Attribute data length: %d", pdu->length);
-	print_data_list("Attribute data list", pdu->length,
-					frame->data + 1, frame->size - 1);
-}
-
-static void att_read_req(const struct l2cap_frame *frame)
-{
-	const struct bt_l2cap_att_read_req *pdu = frame->data;
-
-	print_field("Handle: 0x%4.4x", le16_to_cpu(pdu->handle));
-}
-
-static void att_read_rsp(const struct l2cap_frame *frame)
-{
-	print_hex_field("Value", frame->data, frame->size);
-}
-
-static void att_read_blob_req(const struct l2cap_frame *frame)
-{
-	print_field("Handle: 0x%4.4x", get_le16(frame->data));
-	print_field("Offset: 0x%4.4x", get_le16(frame->data + 2));
-}
-
-static void att_read_blob_rsp(const struct l2cap_frame *frame)
-{
-	packet_hexdump(frame->data, frame->size);
-}
-
-static void att_read_multiple_req(const struct l2cap_frame *frame)
-{
-	int i, count;
-
-	count = frame->size / 2;
-
-	for (i = 0; i < count; i++)
-		print_field("Handle: 0x%4.4x",
-					get_le16(frame->data + (i * 2)));
-}
-
-static void att_read_group_type_req(const struct l2cap_frame *frame)
-{
-	print_handle_range("Handle range", frame->data);
-	print_uuid("Attribute group type", frame->data + 4, frame->size - 4);
-}
-
-static void print_group_list(const char *label, uint8_t length,
-					const void *data, uint16_t size)
-{
-	uint8_t count;
-
-	if (length == 0)
-		return;
-
-	count = size / length;
-
-	print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies");
-
-	while (size >= length) {
-		print_handle_range("Handle range", data);
-		print_uuid("UUID", data + 4, length - 4);
-
-		data += length;
-		size -= length;
-	}
-
-	packet_hexdump(data, size);
-}
-
-static void att_read_group_type_rsp(const struct l2cap_frame *frame)
-{
-	const struct bt_l2cap_att_read_group_type_rsp *pdu = frame->data;
-
-	print_field("Attribute data length: %d", pdu->length);
-	print_group_list("Attribute group list", pdu->length,
-					frame->data + 1, frame->size - 1);
-}
-
-static void att_write_req(const struct l2cap_frame *frame)
-{
-	print_field("Handle: 0x%4.4x", get_le16(frame->data));
-	print_hex_field("  Data", frame->data + 2, frame->size - 2);
-}
-
-static void att_write_rsp(const struct l2cap_frame *frame)
-{
-}
-
-static void att_prepare_write_req(const struct l2cap_frame *frame)
-{
-	print_field("Handle: 0x%4.4x", get_le16(frame->data));
-	print_field("Offset: 0x%4.4x", get_le16(frame->data + 2));
-	print_hex_field("  Data", frame->data + 4, frame->size - 4);
-}
-
-static void att_prepare_write_rsp(const struct l2cap_frame *frame)
-{
-	print_field("Handle: 0x%4.4x", get_le16(frame->data));
-	print_field("Offset: 0x%4.4x", get_le16(frame->data + 2));
-	print_hex_field("  Data", frame->data + 4, frame->size - 4);
-}
-
-static void att_execute_write_req(const struct l2cap_frame *frame)
-{
-	uint8_t flags = *(uint8_t *) frame->data;
-	const char *flags_str;
-
-	switch (flags) {
-	case 0x00:
-		flags_str = "Cancel all prepared writes";
-		break;
-	case 0x01:
-		flags_str = "Immediately write all pending values";
-		break;
-	default:
-		flags_str = "Unknown";
-		break;
-	}
-
-	print_field("Flags: %s (0x%02x)", flags_str, flags);
-}
-
-static void att_handle_value_notify(const struct l2cap_frame *frame)
-{
-	const struct bt_l2cap_att_handle_value_notify *pdu = frame->data;
-
-	print_field("Handle: 0x%4.4x", le16_to_cpu(pdu->handle));
-	print_hex_field("  Data", frame->data + 2, frame->size - 2);
-}
-
-static void att_handle_value_ind(const struct l2cap_frame *frame)
-{
-	const struct bt_l2cap_att_handle_value_ind *pdu = frame->data;
-
-	print_field("Handle: 0x%4.4x", le16_to_cpu(pdu->handle));
-	print_hex_field("  Data", frame->data + 2, frame->size - 2);
-}
-
-static void att_handle_value_conf(const struct l2cap_frame *frame)
-{
-}
-
-static void att_multiple_vl_rsp(const struct l2cap_frame *frame)
-{
-	struct l2cap_frame *f = (void *) frame;
-
-	while (frame->size) {
-		uint16_t handle;
-		uint16_t len;
-
-		if (!l2cap_frame_get_le16(f, &handle))
-			return;
-
-		print_field("Handle: 0x%4.4x", handle);
-
-		if (!l2cap_frame_get_le16(f, &len))
-			return;
-
-		print_field("Length: 0x%4.4x", len);
-
-		print_hex_field("  Data", f->data,
-				len < f->size ? len : f->size);
-
-		if (len > f->size) {
-			print_text(COLOR_ERROR, "invalid size");
-			return;
-		}
-
-		l2cap_frame_pull(f, f, len);
-	}
-}
-
-static void att_write_command(const struct l2cap_frame *frame)
-{
-	print_field("Handle: 0x%4.4x", get_le16(frame->data));
-	print_hex_field("  Data", frame->data + 2, frame->size - 2);
-}
-
-static void att_signed_write_command(const struct l2cap_frame *frame)
-{
-	print_field("Handle: 0x%4.4x", get_le16(frame->data));
-	print_hex_field("  Data", frame->data + 2, frame->size - 2 - 12);
-	print_hex_field("  Signature", frame->data + frame->size - 12, 12);
-}
-
-struct att_opcode_data {
-	uint8_t opcode;
-	const char *str;
-	void (*func) (const struct l2cap_frame *frame);
-	uint8_t size;
-	bool fixed;
-};
-
-static const struct att_opcode_data att_opcode_table[] = {
-	{ 0x01, "Error Response",
-			att_error_response, 4, true },
-	{ 0x02, "Exchange MTU Request",
-			att_exchange_mtu_req, 2, true },
-	{ 0x03, "Exchange MTU Response",
-			att_exchange_mtu_rsp, 2, true },
-	{ 0x04, "Find Information Request",
-			att_find_info_req, 4, true },
-	{ 0x05, "Find Information Response",
-			att_find_info_rsp, 5, false },
-	{ 0x06, "Find By Type Value Request",
-			att_find_by_type_val_req, 6, false },
-	{ 0x07, "Find By Type Value Response",
-			att_find_by_type_val_rsp, 4, false },
-	{ 0x08, "Read By Type Request",
-			att_read_type_req, 6, false },
-	{ 0x09, "Read By Type Response",
-			att_read_type_rsp, 3, false },
-	{ 0x0a, "Read Request",
-			att_read_req, 2, true },
-	{ 0x0b, "Read Response",
-			att_read_rsp, 0, false },
-	{ 0x0c, "Read Blob Request",
-			att_read_blob_req, 4, true },
-	{ 0x0d, "Read Blob Response",
-			att_read_blob_rsp, 0, false },
-	{ 0x0e, "Read Multiple Request",
-			att_read_multiple_req, 4, false },
-	{ 0x0f, "Read Multiple Response"	},
-	{ 0x10, "Read By Group Type Request",
-			att_read_group_type_req, 6, false },
-	{ 0x11, "Read By Group Type Response",
-			att_read_group_type_rsp, 4, false },
-	{ 0x12, "Write Request"	,
-			att_write_req, 2, false	},
-	{ 0x13, "Write Response",
-			att_write_rsp, 0, true	},
-	{ 0x16, "Prepare Write Request",
-			att_prepare_write_req, 4, false },
-	{ 0x17, "Prepare Write Response",
-			att_prepare_write_rsp, 4, false },
-	{ 0x18, "Execute Write Request",
-			att_execute_write_req, 1, true },
-	{ 0x19, "Execute Write Response"	},
-	{ 0x1b, "Handle Value Notification",
-			att_handle_value_notify, 2, false },
-	{ 0x1d, "Handle Value Indication",
-			att_handle_value_ind, 2, false },
-	{ 0x1e, "Handle Value Confirmation",
-			att_handle_value_conf, 0, true },
-	{ 0x20, "Read Multiple Request Variable Length",
-			att_read_multiple_req, 4, false },
-	{ 0x21, "Read Multiple Response Variable Length",
-			att_multiple_vl_rsp, 4, false },
-	{ 0x23, "Handle Multiple Value Notification",
-			att_multiple_vl_rsp, 4, false },
-	{ 0x52, "Write Command",
-			att_write_command, 2, false },
-	{ 0xd2, "Signed Write Command", att_signed_write_command, 14, false },
-	{ }
-};
-
-static const char *att_opcode_to_str(uint8_t opcode)
-{
-	int i;
-
-	for (i = 0; att_opcode_table[i].str; i++) {
-		if (att_opcode_table[i].opcode == opcode)
-			return att_opcode_table[i].str;
-	}
-
-	return "Unknown";
-}
-
-static void att_packet(uint16_t index, bool in, uint16_t handle,
-			uint16_t cid, const void *data, uint16_t size)
-{
-	struct l2cap_frame frame;
-	uint8_t opcode = *((const uint8_t *) data);
-	const struct att_opcode_data *opcode_data = NULL;
-	const char *opcode_color, *opcode_str;
-	int i;
-
-	if (size < 1) {
-		print_text(COLOR_ERROR, "malformed attribute packet");
-		packet_hexdump(data, size);
-		return;
-	}
-
-	for (i = 0; att_opcode_table[i].str; i++) {
-		if (att_opcode_table[i].opcode == opcode) {
-			opcode_data = &att_opcode_table[i];
-			break;
-		}
-	}
-
-	if (opcode_data) {
-		if (opcode_data->func) {
-			if (in)
-				opcode_color = COLOR_MAGENTA;
-			else
-				opcode_color = COLOR_BLUE;
-		} else
-			opcode_color = COLOR_WHITE_BG;
-		opcode_str = opcode_data->str;
-	} else {
-		opcode_color = COLOR_WHITE_BG;
-		opcode_str = "Unknown";
-	}
-
-	print_indent(6, opcode_color, "ATT: ", opcode_str, COLOR_OFF,
-				" (0x%2.2x) len %d", opcode, size - 1);
-
-	if (!opcode_data || !opcode_data->func) {
-		packet_hexdump(data + 1, size - 1);
-		return;
-	}
-
-	if (opcode_data->fixed) {
-		if (size - 1 != opcode_data->size) {
-			print_text(COLOR_ERROR, "invalid size");
-			packet_hexdump(data + 1, size - 1);
-			return;
-		}
-	} else {
-		if (size - 1 < opcode_data->size) {
-			print_text(COLOR_ERROR, "too short packet");
-			packet_hexdump(data + 1, size - 1);
-			return;
-		}
-	}
-
-	l2cap_frame_init(&frame, index, in, handle, 0, cid, 0,
-						data + 1, size - 1);
-	opcode_data->func(&frame);
-}
-
 static void print_smp_io_capa(uint8_t io_capa)
 {
 	const char *str;
diff --git a/monitor/l2cap.h b/monitor/l2cap.h
index e46042db4..951b411e0 100644
--- a/monitor/l2cap.h
+++ b/monitor/l2cap.h
@@ -26,6 +26,11 @@ struct l2cap_frame {
 	uint16_t size;
 };
 
+void l2cap_frame_init(struct l2cap_frame *frame, uint16_t index, bool in,
+				uint16_t handle, uint8_t ident,
+				uint16_t cid, uint16_t psm,
+				const void *data, uint16_t size);
+
 static inline void l2cap_frame_pull(struct l2cap_frame *frame,
 				const struct l2cap_frame *source, uint16_t len)
 {
-- 
2.35.1




[Index of Archives]     [Bluez Devel]     [Linux Wireless Networking]     [Linux Wireless Personal Area Networking]     [Linux ATH6KL]     [Linux USB Devel]     [Linux Media Drivers]     [Linux Audio Users]     [Linux Kernel]     [Linux SCSI]     [Big List of Linux Books]

  Powered by Linux