drivers/nble: Add buffer pool for prepare writes

This is similar to previous patch which makes the API much more simple
when handling long writes.

Change-Id: Ibd3856863a43927195e23936872a160d5ff94648
Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

7 files changed, 105 insertions, 66 deletions

diff --git a/drivers/bluetooth/nble/Kconfig b/drivers/bluetooth/nble/Kconfig
index 59dc3fe50..cb7bd9378 100644
--- a/drivers/bluetooth/nble/Kconfig
+++ b/drivers/bluetooth/nble/Kconfig
@@ -49,6 +49,14 @@ config BLUETOOTH_CENTRAL
 	bool
 	default n
 
+config BLUETOOTH_ATT_PREPARE_COUNT
+	int "Number of ATT prepare write buffers"
+	default 2
+	range 0 64
+	help
+	  Number of buffers available for ATT prepare write, setting
+	  this to 0 disables GATT long/reliable writes.
+
 config BLUETOOTH_GATT_CLIENT
 	bool
 	default n
diff --git a/drivers/bluetooth/nble/gap.c b/drivers/bluetooth/nble/gap.c
index c42ad6cc4..37185f44a 100644
--- a/drivers/bluetooth/nble/gap.c
+++ b/drivers/bluetooth/nble/gap.c
@@ -22,10 +22,12 @@
 
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/conn.h>
+#include <bluetooth/gatt.h>
 #include <bluetooth/log.h>
 
 #include "gap_internal.h"
 #include "conn_internal.h"
+#include "gatt_internal.h"
 #include "uart.h"
 #include "conn.h"
 #include "rpc.h"
@@ -366,6 +368,7 @@ void on_nble_up(void)
 	BT_DBG("");
 
 	bt_smp_init();
+	bt_gatt_init();
 }
 
 void bt_storage_register(const struct bt_storage *bt_storage)
diff --git a/drivers/bluetooth/nble/gatt.c b/drivers/bluetooth/nble/gatt.c
index 531556602..45613eaa6 100644
--- a/drivers/bluetooth/nble/gatt.c
+++ b/drivers/bluetooth/nble/gatt.c
@@ -18,7 +18,9 @@
 #include <atomic.h>
 #include <misc/byteorder.h>
 
+#include <net/buf.h>
 #include <bluetooth/gatt.h>
+#include <bluetooth/att.h>
 #include <bluetooth/log.h>
 
 #include "conn.h"
@@ -35,6 +37,16 @@
 /* TODO: Get this value during negotiation */
 #define BLE_GATT_MTU_SIZE 23
 
+#if CONFIG_BLUETOOTH_ATT_PREPARE_COUNT > 0
+/* Pool for incoming ATT packets */
+static struct nano_fifo prep_data;
+static NET_BUF_POOL(prep_pool, CONFIG_BLUETOOTH_ATT_PREPARE_COUNT,
+		    BLE_GATT_MTU_SIZE, &prep_data, NULL,
+		    sizeof(struct nble_gatts_write_evt));
+
+static struct nano_fifo queue;
+#endif
+
 struct nble_gatt_service {
 	const struct bt_gatt_attr *attrs;
 	uint16_t attr_count;
@@ -1347,6 +1359,43 @@ void bt_gatt_cancel(struct bt_conn *conn)
 	BT_DBG("");
 }
 
+static uint8_t
+on_nble_gatts_prep_write_evt(const struct nble_gatts_write_evt *ev,
+			     const uint8_t *data, uint8_t len)
+{
+#if CONFIG_BLUETOOTH_ATT_PREPARE_COUNT > 0
+	struct net_buf *buf;
+
+	BT_DBG("handle 0x%04x flag %d len %u", attr->handle, ev->flag, buflen);
+
+	buf = net_buf_get_timeout(&prep_data, 0, TICKS_NONE);
+	if (!buf) {
+		return BT_GATT_ERR(BT_ATT_ERR_PREPARE_QUEUE_FULL);
+	}
+
+	/* Copy data into the outstanding queue */
+	memcpy(net_buf_user_data(buf), ev, sizeof(*ev));
+	memcpy(net_buf_add(buf, len), data, len);
+
+	nano_fifo_put(&queue, buf);
+
+	return 0;
+#else
+	return BT_GATT_ERR(BT_ATT_ERR_NOT_SUPPORTED);
+#endif
+}
+
+static uint8_t gatts_write_evt(const struct nble_gatts_write_evt *ev,
+			       const uint8_t *buf, uint8_t buflen)
+{
+	const struct bt_gatt_attr *attr = ev->attr;
+	struct bt_conn *conn = bt_conn_lookup_handle(ev->conn_handle);
+
+	BT_DBG("handle 0x%04x offset %u", attr->handle, ev->offset);
+
+	return attr->write(conn, attr, buf, buflen, ev->offset);
+}
+
 void on_nble_gatts_write_evt(const struct nble_gatts_write_evt *ev,
 			     const uint8_t *buf, uint8_t buflen)
 {
@@ -1357,40 +1406,20 @@ void on_nble_gatts_write_evt(const struct nble_gatts_write_evt *ev,
 	BT_DBG("handle 0x%04x flag %d len %u", attr->handle, ev->flag, buflen);
 
 	/* Check for write support and flush support in case of prepare */
-	if (!attr->write ||
-	    ((ev->flag & NBLE_GATT_WR_FLAG_PREP) && !attr->flush)) {
+	if (!attr->write) {
 		reply_data.status = BT_GATT_ERR(BT_ATT_ERR_WRITE_NOT_PERMITTED);
 
 		goto reply;
 	}
 
-	reply_data.status = attr->write(conn, attr, buf, buflen, ev->offset);
-	if (reply_data.status < 0) {
-		if (ev->flag & NBLE_GATT_WR_FLAG_PREP) {
-			/**
-			 * Note: The Attribute Value validation is done when an
-			 * Execute Write Request is received. Hence, any Invalid
-			 * Offset or Invalid Attribute Value Length errors are
-			 * generated when an Execute Write Request is received.
-			 * BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part F]
-			 * page 504.
-			 * So we end up storing errors to be sent later on.
-			 */
-
-			BT_DBG("Prepare write error %d", reply_data.status);
-
-			switch (reply_data.status) {
-			case BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET):
-			case BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN):
-				BT_DBG("Defer error to the execute write");
-				conn->gatt_private = (void *)reply_data.status;
-				reply_data.status = buflen;
-				break;
-			default:
-				break;
-			}
-		}
+	if (ev->flag & NBLE_GATT_WR_FLAG_PREP) {
+		reply_data.status = on_nble_gatts_prep_write_evt(ev, buf,
+								 buflen);
+		goto reply;
+	}
 
+	reply_data.status = gatts_write_evt(ev, buf, buflen);
+	if (reply_data.status < 0) {
 		goto reply;
 	}
 
@@ -1401,11 +1430,6 @@ void on_nble_gatts_write_evt(const struct nble_gatts_write_evt *ev,
 		goto reply;
 	}
 
-	/* Flush in case of regular write operation */
-	if (attr->flush && !(ev->flag & NBLE_GATT_WR_FLAG_PREP)) {
-		reply_data.status = attr->flush(conn, attr, BT_GATT_FLUSH_SYNC);
-	}
-
 reply:
 	if (ev->flag & NBLE_GATT_WR_FLAG_REPLY) {
 		reply_data.conn_handle = ev->conn_handle;
@@ -1418,33 +1442,10 @@ reply:
 	}
 }
 
-struct nble_gatts_flush_all {
-	struct bt_conn *conn;
-	int status;
-	uint8_t flag;
-};
-
-static uint8_t flush_all(const struct bt_gatt_attr *attr, void *user_data)
-{
-	struct nble_gatts_flush_all *flush_data = user_data;
-
-	if (attr->flush) {
-		int status = attr->flush(flush_data->conn, attr,
-					 flush_data->flag);
-		if (status < 0 && flush_data->status == 0)
-			flush_data->status = status;
-	}
-
-	return BT_GATT_ITER_CONTINUE;
-}
-
 void on_nble_gatts_write_exec_evt(const struct nble_gatts_write_exec_evt *evt)
 {
 	struct bt_conn *conn;
-	struct nble_gatts_flush_all flush_data = {
-		.flag = evt->flag,
-		.status = 0,
-	};
+	struct net_buf *buf;
 	struct nble_gatts_write_reply_req rsp = {
 		.conn_handle = evt->conn_handle,
 	};
@@ -1457,19 +1458,23 @@ void on_nble_gatts_write_exec_evt(const struct nble_gatts_write_exec_evt *evt)
 		return;
 	}
 
-	flush_data.conn = conn;
+	while ((buf = nano_fifo_get(&queue, TICKS_NONE))) {
+		struct nble_gatts_write_evt *ev = net_buf_user_data(buf);
 
-	if (conn->gatt_private) {
-		rsp.status = (int)gatt_get_private(conn);
-		BT_DBG("Return deferred error %d", rsp.status);
-		goto reply;
-	}
+		/* Skip buffer for other connections */
+		if (ev->conn_handle != evt->conn_handle) {
+			nano_fifo_put(&queue, buf);
+			continue;
+		}
 
-	bt_gatt_foreach_attr(0x0001, 0xFFFF, flush_all, &flush_data);
+		/* Just discard the data if an error was set */
+		if (!rsp.status && evt->flag == 0x01) {
+			rsp.status = gatts_write_evt(ev, buf->data, buf->len);
+		}
 
-	rsp.status = flush_data.status;
+		net_buf_unref(buf);
+	}
 
-reply:
 	nble_gatts_write_reply_req(&rsp);
 
 	bt_conn_unref(conn);
@@ -1514,10 +1519,28 @@ void on_nble_gatts_read_evt(const struct nble_gatts_read_evt *ev)
 	nble_gatts_read_reply_req(&reply_data, data, len);
 }
 
+void bt_gatt_init(void)
+{
+	BT_DBG("");
+
+#if CONFIG_BLUETOOTH_ATT_PREPARE_COUNT > 0
+	net_buf_pool_init(prep_pool);
+#endif
+}
+
 void bt_gatt_disconnected(struct bt_conn *conn)
 {
+	struct net_buf *buf;
+
 	BT_DBG("conn %p", conn);
 
+#if CONFIG_BLUETOOTH_ATT_PREPARE_COUNT > 0
+	/* Discard queued buffers */
+	while ((buf = nano_fifo_get(&queue, TICKS_NONE))) {
+		net_buf_unref(buf);
+	}
+#endif
+
 	conn->gatt_private = NULL;
 
 	/* TODO: If bonded don't remove subscriptions */
diff --git a/drivers/bluetooth/nble/gatt_internal.h b/drivers/bluetooth/nble/gatt_internal.h
index 083e52e01..be9fd56e6 100644
--- a/drivers/bluetooth/nble/gatt_internal.h
+++ b/drivers/bluetooth/nble/gatt_internal.h
@@ -23,6 +23,8 @@
  */
 #define BLE_GATTS_MAX_SERVICES 10
 
+void bt_gatt_init(void);
+
 /*
  * GATT Attribute stream structure.
  *
diff --git a/samples/bluetooth/peripheral/prj_nble.conf b/samples/bluetooth/peripheral/prj_nble.conf
index 97e882cc1..07df5845e 100644
--- a/samples/bluetooth/peripheral/prj_nble.conf
+++ b/samples/bluetooth/peripheral/prj_nble.conf
@@ -3,3 +3,4 @@ CONFIG_BLUETOOTH_STACK_NBLE=y
 CONFIG_NBLE=y
 CONFIG_ARC_INIT=n
 CONFIG_BLUETOOTH_DEBUG_LOG=y
+CONFIG_BLUETOOTH_ATT_PREPARE_COUNT=2
diff --git a/tests/bluetooth/shell/prj_nble.conf b/tests/bluetooth/shell/prj_nble.conf
index 44fbc998a..36e43eeff 100644
--- a/tests/bluetooth/shell/prj_nble.conf
+++ b/tests/bluetooth/shell/prj_nble.conf
@@ -7,3 +7,4 @@ CONFIG_ARC_INIT=n
 CONFIG_CONSOLE_HANDLER=y
 CONFIG_CONSOLE_HANDLER_SHELL=y
 CONFIG_BLUETOOTH_DEBUG_LOG=y
+CONFIG_BLUETOOTH_ATT_PREPARE_COUNT=2
diff --git a/tests/bluetooth/tester/prj_nble.conf b/tests/bluetooth/tester/prj_nble.conf
index 7c2a730b5..e0a61f1e5 100644
--- a/tests/bluetooth/tester/prj_nble.conf
+++ b/tests/bluetooth/tester/prj_nble.conf
@@ -5,3 +5,4 @@ CONFIG_BLUETOOTH_STACK_NBLE=y
 CONFIG_NBLE=y
 CONFIG_ARC_INIT=n
 CONFIG_UART_CONSOLE=n
+CONFIG_BLUETOOTH_ATT_PREPARE_COUNT=2