[BLE - NRF5] Add support for long write requests.

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NRF5/source/nRF5xGattServer.cpp

@@ -26,6 +26,47 @@
 
 #include "nRF5xn.h"
 
+namespace {
+
+static const ble_gatts_rw_authorize_reply_params_t write_auth_queue_full_reply = {
+    .type = BLE_GATTS_AUTHORIZE_TYPE_WRITE,
+    .params = {
+        .write = {
+            .gatt_status = BLE_GATT_STATUS_ATTERR_PREPARE_QUEUE_FULL
+        }
+    }
+};
+
+static const ble_gatts_rw_authorize_reply_params_t write_auth_invalid_offset_reply = {
+    .type = BLE_GATTS_AUTHORIZE_TYPE_WRITE,
+    .params = {
+        .write = {
+            .gatt_status = BLE_GATT_STATUS_ATTERR_INVALID_OFFSET
+        }
+    }
+};
+
+static const ble_gatts_rw_authorize_reply_params_t write_auth_succes_reply = {
+    .type = BLE_GATTS_AUTHORIZE_TYPE_WRITE,
+    .params = {
+        .write = {
+            .gatt_status = BLE_GATT_STATUS_SUCCESS,
+            .update = 0
+        }
+    }
+};
+
+static const ble_gatts_rw_authorize_reply_params_t write_auth_invalid_reply = {
+    .type = BLE_GATTS_AUTHORIZE_TYPE_WRITE,
+    .params = {
+        .write = {
+            .gatt_status = BLE_GATT_STATUS_ATTERR_INVALID
+        }
+    }
+};
+
+}
+
 /**************************************************************************/
 /*!
     @brief  Adds a new service to the GATT table on the peripheral
@@ -354,6 +395,8 @@ ble_error_t nRF5xGattServer::reset(void)
     memset(nrfDescriptorHandles,     0, sizeof(nrfDescriptorHandles));
     descriptorCount = 0;
 
+    releaseAllWriteRequests();
+
     return BLE_ERROR_NONE;
 }
 
@@ -427,13 +470,33 @@ void nRF5xGattServer::hwCallback(ble_evt_t *p_ble_evt)
             }
             break;
 
+        case BLE_EVT_USER_MEM_REQUEST: {
+            uint16_t conn_handle = p_ble_evt->evt.common_evt.conn_handle;
+
+            // allocate a new long request for this connection
+            // NOTE: we don't care about the result at this stage,
+            // it is not possible to cancel the operation anyway.
+            // If the request was not allocated then it will gracefully failled
+            // at subsequent stages.
+            allocateLongWriteRequest(conn_handle);
+            sd_ble_user_mem_reply(conn_handle, NULL);
+            return;
+        }
+
         default:
             return;
     }
 
     int characteristicIndex = resolveValueHandleToCharIndex(handle_value);
     if (characteristicIndex == -1) {
-        return;
+        // filter out the case were the request is a long one,
+        // and there is no attribute handle provided
+        uint8_t write_op = gattsEventP->params.authorize_request.request.write.op;
+        if (eventType != GattServerEvents::GATT_EVENT_WRITE_AUTHORIZATION_REQ ||
+            (write_op != BLE_GATTS_OP_EXEC_WRITE_REQ_NOW &&
+             write_op != BLE_GATTS_OP_EXEC_WRITE_REQ_CANCEL)) {
+            return;
+        }
     }
 
     /* Find index (charHandle) in the pool */
@@ -451,6 +514,140 @@ void nRF5xGattServer::hwCallback(ble_evt_t *p_ble_evt)
             break;
         }
         case GattServerEvents::GATT_EVENT_WRITE_AUTHORIZATION_REQ: {
+            uint16_t conn_handle = gattsEventP->conn_handle;
+            const ble_gatts_evt_write_t& input_req = gattsEventP->params.authorize_request.request.write;
+            const uint16_t max_size = getBiggestCharacteristicSize();
+
+            // this is a long write request, handle it here.
+            switch (input_req.op) {
+                case BLE_GATTS_OP_PREP_WRITE_REQ: {
+                    // verify that the request is not outside of the possible range
+                    if ((input_req.offset + input_req.len) > max_size) {
+                        sd_ble_gatts_rw_authorize_reply(conn_handle, &write_auth_invalid_offset_reply);
+                        releaseLongWriteRequest(conn_handle);
+                        return;
+                    }
+
+                    // find the write request
+                    long_write_request_t* req = findLongWriteRequest(conn_handle);
+                    if (!req)  {
+                        sd_ble_gatts_rw_authorize_reply(conn_handle, &write_auth_invalid_reply);
+                        releaseLongWriteRequest(conn_handle);
+                        return;
+                    }
+
+                    // initialize the first request by setting the offset
+                    if (req->length == 0) {
+                        req->attr_handle = input_req.handle;
+                        req->offset = input_req.offset;
+                    }
+
+                    // it is disalowed to write backward
+                    if (input_req.offset < req->offset) {
+                        sd_ble_gatts_rw_authorize_reply(conn_handle, &write_auth_invalid_offset_reply);
+                        releaseLongWriteRequest(conn_handle);
+                        return;
+                    }
+
+                    // it should be the subsequent write
+                    if ((req->offset + req->length) != input_req.offset) {
+                        sd_ble_gatts_rw_authorize_reply(conn_handle, &write_auth_invalid_offset_reply);
+                        releaseLongWriteRequest(conn_handle);
+                        return;
+                    }
+
+                    // it is not allowed to write multiple characteristic with the same request
+                    if (input_req.handle != req->attr_handle) {
+                        sd_ble_gatts_rw_authorize_reply(conn_handle, &write_auth_invalid_reply);
+                        releaseLongWriteRequest(conn_handle);
+                        return;
+                    }
+
+                    // start the copy of what is in input
+                    memcpy(req->data + req->length, input_req.data, input_req.len);
+
+                    // update the lenght of the data written
+                    req->length = req->length + input_req.len;
+
+                    // success, signal it to the softdevice
+                    ble_gatts_rw_authorize_reply_params_t reply = {
+                        .type = BLE_GATTS_AUTHORIZE_TYPE_WRITE,
+                        .params = {
+                            .write = {
+                                .gatt_status = BLE_GATT_STATUS_SUCCESS,
+                                .update = 1,
+                                .offset = input_req.offset,
+                                .len = input_req.len,
+                                .p_data = input_req.data
+                            }
+                        }
+                    };
+
+                    sd_ble_gatts_rw_authorize_reply(conn_handle, &reply);
+                }   return;
+
+                case BLE_GATTS_OP_EXEC_WRITE_REQ_CANCEL: {
+                    releaseLongWriteRequest(conn_handle);
+                    sd_ble_gatts_rw_authorize_reply(conn_handle, &write_auth_succes_reply);
+                }   return;
+
+                case BLE_GATTS_OP_EXEC_WRITE_REQ_NOW: {
+                    long_write_request_t* req = findLongWriteRequest(conn_handle);
+                    if (!req) {
+                        sd_ble_gatts_rw_authorize_reply(conn_handle, &write_auth_invalid_reply);
+                        releaseLongWriteRequest(conn_handle);
+                        return;
+                    }
+
+                    GattWriteAuthCallbackParams cbParams = {
+                        .connHandle = conn_handle,
+                        .handle     = req->attr_handle,
+                        .offset     = req->offset,
+                        .len        = req->length,
+                        .data       = req->data,
+                        .authorizationReply = AUTH_CALLBACK_REPLY_SUCCESS /* the callback handler must leave this member
+                                                                           * set to AUTH_CALLBACK_REPLY_SUCCESS if the client
+                                                                           * request is to proceed. */
+                    };
+                    uint16_t write_authorization = p_characteristics[characteristicIndex]->authorizeWrite(&cbParams);
+
+                    // the user code didn't provide the write authorization,
+                    // just leave here.
+                    if (write_authorization != AUTH_CALLBACK_REPLY_SUCCESS) {
+                        // report the status of the operation in any cases
+                        sd_ble_gatts_rw_authorize_reply(gattsEventP->conn_handle, &write_auth_invalid_reply);
+                        releaseLongWriteRequest(conn_handle);
+                        return;
+                    }
+
+                    // FIXME can't use ::write here, this function doesn't take the offset into account ...
+                    ble_gatts_value_t value = {
+                        .len     = req->length,
+                        .offset  = req->offset,
+                        .p_value = req->data
+                    };
+                    uint32_t update_err = sd_ble_gatts_value_set(conn_handle, req->attr_handle, &value);
+                    if (update_err) {
+                        sd_ble_gatts_rw_authorize_reply(conn_handle, &write_auth_invalid_reply);
+                        releaseLongWriteRequest(conn_handle);
+                        return;
+                    }
+
+                    sd_ble_gatts_rw_authorize_reply(gattsEventP->conn_handle, &write_auth_succes_reply);
+
+                    GattWriteCallbackParams writeParams = {
+                        .connHandle = conn_handle,
+                        .handle     = req->attr_handle,
+                        .writeOp    = static_cast<GattWriteCallbackParams::WriteOp_t>(input_req.op),
+                        .offset     = req->offset,
+                        .len        = req->length,
+                        .data       = req->data,
+                    };
+                    handleDataWrittenEvent(&writeParams);
+                    releaseLongWriteRequest(conn_handle);
+                }   return;
+            }
+
             GattWriteAuthCallbackParams cbParams = {
                 .connHandle = gattsEventP->conn_handle,
                 .handle     = handle_value,
@@ -541,3 +738,64 @@ void nRF5xGattServer::hwCallback(ble_evt_t *p_ble_evt)
             break;
     }
 }
+
+uint16_t nRF5xGattServer::getBiggestCharacteristicSize() const {
+    uint16_t result = 0;
+    for (size_t i = 0; i < characteristicCount; ++i) {
+        uint16_t current_size = p_characteristics[i]->getValueAttribute().getMaxLength();
+        if (current_size > result) {
+            result = current_size;
+        }
+    }
+    return result;
+}
+
+nRF5xGattServer::long_write_request_t* nRF5xGattServer::allocateLongWriteRequest(uint16_t connection_handle) {
+    for (size_t i = 0; i < TOTAL_CONCURENT_LONG_WRITE_REQUEST; ++i) {
+        long_write_request_t& req = long_write_requests[i];
+        if (req.data == NULL) {
+            uint16_t block_size = getBiggestCharacteristicSize();
+            req.data = static_cast<uint8_t*>(malloc(block_size));
+            req.offset = 0;
+            req.length = 0;
+            req.conn_handle = connection_handle;
+            return &req;
+        }
+    }
+    // if nothing has been found then return null
+    return NULL;
+}
+
+bool nRF5xGattServer::releaseLongWriteRequest(uint16_t connection_handle) {
+    long_write_request_t* req = findLongWriteRequest(connection_handle);
+    if (!req) {
+        return false;
+    }
+
+    free(req->data);
+    req->data = NULL;
+
+    // the other fields are not relevant, return now
+    return true;
+}
+
+nRF5xGattServer::long_write_request_t* nRF5xGattServer::findLongWriteRequest(uint16_t connection_handle) {
+    for (size_t i = 0; i < TOTAL_CONCURENT_LONG_WRITE_REQUEST; ++i) {
+        long_write_request_t& req = long_write_requests[i];
+        if (req.data != NULL && req.conn_handle == connection_handle) {
+            return &req;
+        }
+    }
+    // if nothing has been found then return null
+    return NULL;
+}
+
+void nRF5xGattServer::releaseAllWriteRequests() {
+    for (size_t i = 0; i < TOTAL_CONCURENT_LONG_WRITE_REQUEST; ++i) {
+        long_write_request_t& req = long_write_requests[i];
+        if (req.data != NULL) {
+            free(req.data);
+            req.data = NULL;
+        }
+    }
+}

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NRF5/source/nRF5xGattServer.h

@@ -45,6 +45,29 @@ public:
 private:
     const static unsigned BLE_TOTAL_CHARACTERISTICS = 20;
     const static unsigned BLE_TOTAL_DESCRIPTORS     = 8;
+    const static unsigned TOTAL_CONCURENT_LONG_WRITE_REQUEST = 3;
+
+private:
+    struct long_write_request_t {
+        // the connection handle for a long write request
+        uint16_t conn_handle;
+
+        // the attribute handle for the long write request
+        // This implementation folow the bluetooth route
+        // where a write request target a single characteristic
+        // (see BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part G] - 4.9.4)
+        uint16_t attr_handle;
+
+        // offset of the transaction
+        uint16_t offset;
+
+        // length of the data
+        uint16_t length;
+
+        // current data
+        uint8_t* data;
+    };
+
 
 private:
     /**
@@ -79,19 +102,52 @@ private:
         return -1;
     }
 
+    /**
+     * Return the biggest size used by a characteristic in the server
+     */
+    uint16_t getBiggestCharacteristicSize() const;
+
+    /**
+     * Allocate a new write long request. return null if no requests are available.
+     * @param  connection_handle The connection handle which where the request will
+     * happen.
+     * @return the allocated request or NULL if no requests are available.
+     */
+    long_write_request_t* allocateLongWriteRequest(uint16_t connection_handle);
+
+    /**
+     * Release a long write request and free a slot for subsequent write long requests.
+     * @param  connection_handle The connection handle associated with the request
+     * @return true if the request where allocated and was release, false otherwise.
+     */
+    bool releaseLongWriteRequest(uint16_t connection_handle);
+
+    /**
+     * Find a long write request from a characteristic handle
+     * @param  connection_handle The connection handle associated with the reauest.
+     * @return a pointer to the request if found otherwise NULL.
+     */
+    long_write_request_t* findLongWriteRequest(uint16_t connection_handle);
+
+    /**
+     * Release all pending write requests.
+     */
+    void releaseAllWriteRequests();
+
 private:
     GattCharacteristic       *p_characteristics[BLE_TOTAL_CHARACTERISTICS];
     ble_gatts_char_handles_t  nrfCharacteristicHandles[BLE_TOTAL_CHARACTERISTICS];
     GattAttribute            *p_descriptors[BLE_TOTAL_DESCRIPTORS];
     uint8_t                   descriptorCount;
     uint16_t                  nrfDescriptorHandles[BLE_TOTAL_DESCRIPTORS];
+    long_write_request_t      long_write_requests[TOTAL_CONCURENT_LONG_WRITE_REQUEST];
 
     /*
      * Allow instantiation from nRF5xn when required.
      */
     friend class nRF5xn;
 
-    nRF5xGattServer() : GattServer(), p_characteristics(), nrfCharacteristicHandles(), p_descriptors(), descriptorCount(0), nrfDescriptorHandles() {
+    nRF5xGattServer() : GattServer(), p_characteristics(), nrfCharacteristicHandles(), p_descriptors(), descriptorCount(0), nrfDescriptorHandles(), long_write_requests() {
         /* empty */
     }