From c61037cc77c8186ed1a8cb389419343a0c648995 Mon Sep 17 00:00:00 2001
From: Matthias Ringwald <matthias@ringwald.ch>
Date: Tue, 23 Jan 2024 15:36:25 +0100
Subject: [PATCH] att_sever: call att_read_callback on services with
 ATT_READ_RESPONSE_PENDING

# Conflicts:
#	CHANGELOG.md
---
 CHANGELOG.md         |  8 +++++++-
 src/ble/att_server.c | 46 +++++++++++++++++++++++++++++++++++---------
 2 files changed, 44 insertions(+), 10 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index b2f80b4b4..86d0950e0 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -7,13 +7,19 @@ and this project adheres to [Semantic Versioning](http://semver.org/spec/v2.0.0.
 ---
 
 ## Unreleased
+
 ### Added
+- GAP: support coordinated private random address updates
+
 ### Fixed
 - GAP: allow use of own address type different from gap_random_set_mode() incl. RPA in LE Extended Advertising
-- HFP HF: send HF Indicator update only if enabled by AG
 - SM: abort pairing with invalid parameter error for encryption key size > 16
+- ATT Server: support delayed read responses for registered services
+- HFP HF: send HF Indicator update only if enabled by AG
+
 ### Changed
 
+
 ## Release v1.6
 
 ### Added
diff --git a/src/ble/att_server.c b/src/ble/att_server.c
index b8585716e..1cde8c941 100644
--- a/src/ble/att_server.c
+++ b/src/ble/att_server.c
@@ -77,6 +77,8 @@
 #define NVN_NUM_GATT_SERVER_CCC 20
 #endif
 
+#define ATT_SERVICE_FLAGS_DELAYED_RESPONSE (1<<0u)
+
 static void att_run_for_context(att_server_t * att_server, att_connection_t * att_connection);
 static att_write_callback_t att_server_write_callback_for_handle(uint16_t handle);
 static btstack_packet_handler_t att_server_packet_handler_for_handle(uint16_t handle);
@@ -112,6 +114,8 @@ static att_write_callback_t                   att_server_client_write_callback;
 // round robin
 static hci_con_handle_t att_server_last_can_send_now = HCI_CON_HANDLE_INVALID;
 
+static uint8_t att_server_flags;
+
 #ifdef ENABLE_GATT_OVER_EATT
 typedef struct {
     btstack_linked_item_t item;
@@ -587,7 +591,22 @@ att_server_process_validated_request(att_server_t *att_server, att_connection_t
 
         // callback with handle ATT_READ_RESPONSE_PENDING for reads
         if (att_response_size == ATT_READ_RESPONSE_PENDING){
-            att_server_client_read_callback(att_connection->con_handle, ATT_READ_RESPONSE_PENDING, 0, NULL, 0);
+            // notify services that returned response pending
+            btstack_linked_list_iterator_t it;
+            btstack_linked_list_iterator_init(&it, &service_handlers);
+            while (btstack_linked_list_iterator_has_next(&it)) {
+                att_service_handler_t *handler = (att_service_handler_t *) btstack_linked_list_iterator_next(&it);
+                if ((handler->flags & ATT_SERVICE_FLAGS_DELAYED_RESPONSE) != 0){
+                    handler->flags &= ~ATT_SERVICE_FLAGS_DELAYED_RESPONSE;
+                    handler->read_callback(att_connection->con_handle, ATT_READ_RESPONSE_PENDING, 0, NULL, 0);
+                }
+            }
+            // notify main read callback if it returned response pending
+            if ((att_server_flags & ATT_SERVICE_FLAGS_DELAYED_RESPONSE) != 0){
+                // flag was set by read callback
+                btstack_assert(att_server_client_read_callback != NULL);
+                (*att_server_client_read_callback)(att_connection->con_handle, ATT_READ_RESPONSE_PENDING, 0, NULL, 0);
+            }
         }
 
         // free reserved buffer
@@ -1172,11 +1191,6 @@ static att_service_handler_t * att_service_handler_for_handle(uint16_t handle){
     }
     return NULL;
 }
-static att_read_callback_t att_server_read_callback_for_handle(uint16_t handle){
-    att_service_handler_t * handler = att_service_handler_for_handle(handle);
-    if (handler != NULL) return handler->read_callback;
-    return att_server_client_read_callback;
-}
 
 static att_write_callback_t att_server_write_callback_for_handle(uint16_t handle){
     att_service_handler_t * handler = att_service_handler_for_handle(handle);
@@ -1218,9 +1232,22 @@ static uint8_t att_validate_prepared_write(hci_con_handle_t con_handle){
 }
 
 static uint16_t att_server_read_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t offset, uint8_t * buffer, uint16_t buffer_size){
-    att_read_callback_t callback = att_server_read_callback_for_handle(attribute_handle);
-    if (!callback) return 0;
-    return (*callback)(con_handle, attribute_handle, offset, buffer, buffer_size);
+    att_service_handler_t * service = att_service_handler_for_handle(attribute_handle);
+    att_read_callback_t read_callback = (service != NULL) ? service->read_callback : att_server_client_read_callback;
+    uint16_t result = 0;
+    if (read_callback != NULL){
+        result = (*read_callback)(con_handle, attribute_handle, offset, buffer, buffer_size);
+#ifdef ENABLE_ATT_DELAYED_RESPONSE
+        if (result == ATT_READ_RESPONSE_PENDING){
+            if (service == NULL){
+                att_server_flags |= ATT_SERVICE_FLAGS_DELAYED_RESPONSE;
+            } else {
+                service->flags   |= ATT_SERVICE_FLAGS_DELAYED_RESPONSE;
+            }
+        }
+#endif
+    }
+    return result;
 }
 
 static int att_server_write_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t transaction_mode, uint16_t offset, uint8_t *buffer, uint16_t buffer_size){
@@ -1467,6 +1494,7 @@ void att_server_deinit(void){
     att_server_client_write_callback = NULL;
     att_client_packet_handler = NULL;
     service_handlers = NULL;
+    att_server_flags = 0;
 }
 
 #ifdef ENABLE_GATT_OVER_EATT