[PATCH BlueZ] gatt-server: Flush notify multiple buffer when full and fix overflow

[Sebastian Urban <surban@xxxxxxxxxx>]

This fixes the calculation of available buffer space in
bt_gatt_server_send_notification and sends pending notifications
immediately when there is no more room to add a notification.

Previously there was a buffer overflow caused by incorrect calculation
of available buffer space: data->offset can equal data->len
from a previous call to this function, leading
(data->len - data->offset) to underflow after data->offset += 2.
---
 src/shared/gatt-server.c | 43 +++++++++++++++++++++++++++++++++-------
 1 file changed, 36 insertions(+), 7 deletions(-)

diff --git a/src/shared/gatt-server.c b/src/shared/gatt-server.c
index 970c35f94..5dbe9d66c 100644
--- a/src/shared/gatt-server.c
+++ b/src/shared/gatt-server.c
@@ -1690,6 +1690,17 @@ static bool notify_multiple(void *user_data)
 	return false;
 }
 
+bool notify_append_le16(struct nfy_mult_data *data, uint16_t value)
+{
+	if (data->offset + sizeof(value) > data->len)
+		return false;
+
+	put_le16(value, data->pdu + data->offset);
+	data->offset += sizeof(value);
+
+	return true;
+}
+
 bool bt_gatt_server_send_notification(struct bt_gatt_server *server,
 					uint16_t handle, const uint8_t *value,
 					uint16_t length, bool multiple)
@@ -1700,23 +1711,35 @@ bool bt_gatt_server_send_notification(struct bt_gatt_server *server,
 	if (!server || (length && !value))
 		return false;
 
-	if (multiple)
+	if (multiple) {
 		data = server->nfy_mult;
 
+		/* flush buffered data if this request hits buffer size limit */
+		if (data && data->offset > 0 &&
+				data->len - data->offset < 4 + length) {
+			if (server->nfy_mult->id)
+				timeout_remove(server->nfy_mult->id);
+			notify_multiple(server);
+			/* data has been freed by notify_multiple */
+			data = NULL;
+		}
+	}
+
 	if (!data) {
 		data = new0(struct nfy_mult_data, 1);
 		data->len = bt_att_get_mtu(server->att) - 1;
 		data->pdu = malloc(data->len);
 	}
 
-	put_le16(handle, data->pdu + data->offset);
-	data->offset += 2;
-
-	length = MIN(data->len - data->offset, length);
+	if (!notify_append_le16(data, handle))
+		goto error;
 
 	if (multiple) {
-		put_le16(length, data->pdu + data->offset);
-		data->offset += 2;
+		length = MIN(data->len - data->offset - 2, length);
+		if (!notify_append_le16(data, length))
+			goto error;
+	} else {
+		length = MIN(data->len - data->offset, length);
 	}
 
 	memcpy(data->pdu + data->offset, value, length);
@@ -1740,6 +1763,12 @@ bool bt_gatt_server_send_notification(struct bt_gatt_server *server,
 	free(data);
 
 	return result;
+
+error:
+	if (data)
+		free(data);
+
+	return false;
 }
 
 struct ind_data {
-- 
2.25.1