Received-SPF: pass (google.com: domain of linux-bluetooth-owner@vger.kernel.org designates 23.128.96.18 as permitted sender) client-ip=23.128.96.18;
Date:   Wed,  7 Apr 2021 15:17:08 +0800
Message-Id: <20210407151649.Bluez.1.I4ff127dde9bc6adb2a07507af2bf2cc6b6bcf0f2@changeid>
Mime-Version: 1.0
Subject: [Bluez PATCH] input/hog: support multiple variable length notification
From:   Archie Pusaka <apusaka@google.com>
To:     linux-bluetooth <linux-bluetooth@vger.kernel.org>,
        Luiz Augusto von Dentz <luiz.dentz@gmail.com>
Cc:     CrosBT Upstreaming <chromeos-bluetooth-upstreaming@chromium.org>,
        Archie Pusaka <apusaka@chromium.org>,
        Sonny Sasaka <sonnysasaka@chromium.org>
Content-Type: text/plain; charset="UTF-8"
Precedence: bulk

From: Archie Pusaka <apusaka@chromium.org>

Processing Multiple Variable Length Notification is mandatory if EATT
is enabled (Core Spec Vol 3 Part G Sec 4.2).

Reviewed-by: Sonny Sasaka <sonnysasaka@chromium.org>
---

 attrib/att.h             |   1 +
 profiles/input/hog-lib.c | 105 ++++++++++++++++++++++++++++-----------
 src/attrib-server.c      |   1 +
 3 files changed, 79 insertions(+), 28 deletions(-)

diff --git a/attrib/att.h b/attrib/att.h
index 13a0c3a31f..0dbfb14b83 100644
--- a/attrib/att.h
+++ b/attrib/att.h
@@ -42,6 +42,7 @@
 #define ATT_OP_HANDLE_NOTIFY		0x1B
 #define ATT_OP_HANDLE_IND		0x1D
 #define ATT_OP_HANDLE_CNF		0x1E
+#define ATT_OP_HANDLE_NOTIFY_MULTI	0x23
 #define ATT_OP_SIGNED_WRITE_CMD		0xD2
 
 /* Error codes for Error response PDU */
diff --git a/profiles/input/hog-lib.c b/profiles/input/hog-lib.c
index e5e3d3e7f5..2b81ee5b28 100644
--- a/profiles/input/hog-lib.c
+++ b/profiles/input/hog-lib.c
@@ -65,7 +65,6 @@
 
 #define HOG_REPORT_MAP_MAX_SIZE        512
 #define HID_INFO_SIZE			4
-#define ATT_NOTIFICATION_HEADER_SIZE	3
 
 struct bt_hog {
 	int			ref_count;
@@ -112,7 +111,8 @@ struct report {
 	uint16_t		value_handle;
 	uint8_t			properties;
 	uint16_t		ccc_handle;
-	guint			notifyid;
+	guint			notify_id;
+	guint			notify_multi_id;
 	uint16_t		len;
 	uint8_t			*value;
 };
@@ -283,22 +283,14 @@ static void find_included(struct bt_hog *hog, GAttrib *attrib,
 	free(req);
 }
 
-static void report_value_cb(const guint8 *pdu, guint16 len, gpointer user_data)
+static void process_notification(struct report *report, guint16 len,
+							const uint8_t *data)
 {
-	struct report *report = user_data;
 	struct bt_hog *hog = report->hog;
 	struct uhid_event ev;
 	uint8_t *buf;
 	int err;
 
-	if (len < ATT_NOTIFICATION_HEADER_SIZE) {
-		error("Malformed ATT notification");
-		return;
-	}
-
-	pdu += ATT_NOTIFICATION_HEADER_SIZE;
-	len -= ATT_NOTIFICATION_HEADER_SIZE;
-
 	memset(&ev, 0, sizeof(ev));
 	ev.type = UHID_INPUT;
 	buf = ev.u.input.data;
@@ -306,19 +298,78 @@ static void report_value_cb(const guint8 *pdu, guint16 len, gpointer user_data)
 	if (hog->has_report_id) {
 		buf[0] = report->id;
 		len = MIN(len, sizeof(ev.u.input.data) - 1);
-		memcpy(buf + 1, pdu, len);
-		ev.u.input.size = ++len;
+		memcpy(buf + 1, data, len);
+		ev.u.input.size = len + 1;
 	} else {
 		len = MIN(len, sizeof(ev.u.input.data));
-		memcpy(buf, pdu, len);
+		memcpy(buf, data, len);
 		ev.u.input.size = len;
 	}
 
 	err = bt_uhid_send(hog->uhid, &ev);
-	if (err < 0) {
+	if (err < 0)
 		error("bt_uhid_send: %s (%d)", strerror(-err), -err);
-		return;
+}
+
+static void report_value_cb(const guint8 *pdu, guint16 len, gpointer user_data)
+{
+	struct report *report = user_data;
+	uint8_t opcode = pdu[0];
+	guint16 report_len;
+	guint16 header_len;
+
+	/* Skip opcode field */
+	pdu += 1;
+	len -= 1;
+
+	if (opcode == ATT_OP_HANDLE_NOTIFY_MULTI)
+		header_len = 4;
+	else
+		header_len = 2;
+
+	if (len < header_len)
+		goto fail;
+
+	while (len >= header_len) {
+		/* Skip first 2 bytes (handle) */
+		pdu += 2;
+		len -= 2;
+
+		if (opcode == ATT_OP_HANDLE_NOTIFY_MULTI) {
+			report_len = get_le16(pdu);
+			pdu += 2;
+			len -= 2;
+
+			if (report_len > len)
+				goto fail;
+		} else {
+			report_len = len;
+		}
+
+		process_notification(report, report_len, pdu);
+
+		pdu += report_len;
+		len -= report_len;
 	}
+
+	if (len == 0)
+		return;
+
+fail:
+	error("Malformed ATT notification");
+}
+
+static void register_notify_handler(struct bt_hog *hog, struct report *report)
+{
+	report->notify_id = g_attrib_register(hog->attrib,
+					ATT_OP_HANDLE_NOTIFY,
+					report->value_handle,
+					report_value_cb, report, NULL);
+
+	report->notify_multi_id = g_attrib_register(hog->attrib,
+					ATT_OP_HANDLE_NOTIFY_MULTI,
+					report->value_handle,
+					report_value_cb, report, NULL);
 }
 
 static void report_ccc_written_cb(guint8 status, const guint8 *pdu,
@@ -339,10 +390,7 @@ static void report_ccc_written_cb(guint8 status, const guint8 *pdu,
 	if (report->notifyid)
 		return;
 
-	report->notifyid = g_attrib_register(hog->attrib,
-					ATT_OP_HANDLE_NOTIFY,
-					report->value_handle,
-					report_value_cb, report, NULL);
+	register_notify_handler(hog, report);
 
 	DBG("Report characteristic descriptor written: notifications enabled");
 }
@@ -1714,10 +1762,7 @@ bool bt_hog_attach(struct bt_hog *hog, void *gatt)
 		if (r->notifyid)
 			continue;
 
-		r->notifyid = g_attrib_register(hog->attrib,
-					ATT_OP_HANDLE_NOTIFY,
-					r->value_handle,
-					report_value_cb, r, NULL);
+		register_notify_handler(hog, r);
 	}
 
 	return true;
@@ -1764,9 +1809,13 @@ void bt_hog_detach(struct bt_hog *hog)
 	for (l = hog->reports; l; l = l->next) {
 		struct report *r = l->data;
 
-		if (r->notifyid > 0) {
-			g_attrib_unregister(hog->attrib, r->notifyid);
-			r->notifyid = 0;
+		if (r->notify_id > 0) {
+			g_attrib_unregister(hog->attrib, r->notify_id);
+			r->notify_id = 0;
+		}
+		if (r->notify_multi_id > 0) {
+			g_attrib_unregister(hog->attrib, r->notify_multi_id);
+			r->notify_multi_id = 0;
 		}
 	}
 
diff --git a/src/attrib-server.c b/src/attrib-server.c
index 5a178f95ea..fb11d3db2d 100644
--- a/src/attrib-server.c
+++ b/src/attrib-server.c
@@ -1085,6 +1085,7 @@ static void channel_handler(const uint8_t *ipdu, uint16_t len,
 		return;
 	case ATT_OP_HANDLE_IND:
 	case ATT_OP_HANDLE_NOTIFY:
+	case ATT_OP_HANDLE_NOTIFY_MULTI:
 		/* The attribute client is already handling these */
 		return;
 	case ATT_OP_READ_MULTI_REQ:
-- 
2.31.0.208.g409f899ff0-goog