Import Mbed OS hard-float snapshot

connectivity/FEATURE_BLE/source/BLE.cpp

@@ -0,0 +1,220 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2020 ARM Limited
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdio.h>
+#include "platform/mbed_critical.h"
+#include "ble/internal/BLEInstanceBase.h"
+
+#if defined(TARGET_OTA_ENABLED)
+#include "services/DFUService.h"
+#endif
+
+#include <mbed_error.h>
+#include <mbed_toolchain.h>
+
+#include "ble/BLE.h"
+#include "ble/internal/BLEInstanceBase.h"
+
+static const char *error_strings[] = {
+    "BLE_ERROR_NONE: No error",
+    "BLE_ERROR_BUFFER_OVERFLOW: The requested action would cause a buffer overflow and has been aborted",
+    "BLE_ERROR_NOT_IMPLEMENTED: Requested a feature that isn't yet implement or isn't supported by the target HW",
+    "BLE_ERROR_PARAM_OUT_OF_RANGE: One of the supplied parameters is outside the valid range",
+    "BLE_ERROR_INVALID_PARAM: One of the supplied parameters is invalid",
+    "BLE_STACK_BUSY: The stack is busy",
+    "BLE_ERROR_INVALID_STATE: Invalid state",
+    "BLE_ERROR_NO_MEM: Out of Memory",
+    "BLE_ERROR_OPERATION_NOT_PERMITTED: The operation requested is not permitted",
+    "BLE_ERROR_INITIALIZATION_INCOMPLETE: The BLE subsystem has not completed its initialisation",
+    "BLE_ERROR_ALREADY_INITIALIZED: The BLE system has already been initialised",
+    "BLE_ERROR_UNSPECIFIED: Unknown error",
+    "BLE_ERROR_INTERNAL_STACK_FAILURE: The platform-specific stack failed"
+};
+
+const char *BLE::errorToString(ble_error_t error)
+{
+    if (error > BLE_ERROR_INTERNAL_STACK_FAILURE) {
+        return "Illegal error code";
+    }
+    return error_strings[error];
+}
+
+ble_error_t
+BLE::initImplementation(FunctionPointerWithContext<InitializationCompleteCallbackContext *> callback)
+{
+    ble_error_t err = transport.init(callback);
+    if (err != BLE_ERROR_NONE) {
+        return err;
+    }
+
+    /* Platforms enabled for DFU should introduce the DFU Service into
+     * applications automatically. */
+#if defined(TARGET_OTA_ENABLED)
+    static DFUService dfu(*this); // defined static so that the object remains alive
+#endif // TARGET_OTA_ENABLED
+
+    return BLE_ERROR_NONE;
+}
+
+#if defined(INITIALIZER_LIST_FOR_INSTANCE_CONSTRUCTORS)
+#error "INITIALIZER_LIST_FOR_INSTANCE_CONSTRUCTORS no longer supported, you may now create BLE objects freely by passing in the transport. Cordio Transport supports only one instance."
+#endif
+
+// this stub is required by ARMCC otherwise link will systematically fail
+MBED_WEAK ble::BLEInstanceBase *createBLEInstance()
+{
+    MBED_ASSERT("No BLE instance implementation.");
+    printf("Please provide an implementation for mbed BLE");
+    return nullptr;
+}
+
+BLE &BLE::Instance()
+{
+    static ble::BLEInstanceBase *transport = createBLEInstance();
+
+    if (!transport) {
+        MBED_ERROR(MBED_MAKE_ERROR(MBED_MODULE_BLE, MBED_ERROR_CODE_BLE_BACKEND_NOT_INITIALIZED),
+                   "bad handle to underlying transport");
+    }
+
+    static BLE ble_instance(*transport);
+
+    return ble_instance;
+}
+
+bool BLE::hasInitialized(void) const
+{
+    return transport.hasInitialized();
+}
+
+ble_error_t BLE::shutdown(void)
+{
+    event_signaled = false;
+    return transport.shutdown();
+}
+
+const char *BLE::getVersion(void)
+{
+    return transport.getVersion();
+}
+
+const ble::Gap &BLE::gap() const
+{
+    return transport.getGap();
+}
+
+ble::Gap &BLE::gap()
+{
+    return transport.getGap();
+}
+
+#if BLE_FEATURE_GATT_SERVER
+
+const ble::GattServer &BLE::gattServer() const
+{
+    return transport.getGattServer();
+}
+
+ble::GattServer &BLE::gattServer()
+{
+    return transport.getGattServer();
+}
+
+#endif // BLE_FEATURE_GATT_SERVER
+
+#if BLE_FEATURE_GATT_CLIENT
+
+const ble::GattClient &BLE::gattClient() const
+{
+    return transport.getGattClient();
+}
+
+ble::GattClient &BLE::gattClient()
+{
+    return transport.getGattClient();
+}
+
+#endif // BLE_FEATURE_GATT_CLIENT
+
+#if BLE_FEATURE_SECURITY
+
+const ble::SecurityManager &BLE::securityManager() const
+{
+    return transport.getSecurityManager();
+}
+
+ble::SecurityManager &BLE::securityManager()
+{
+    return transport.getSecurityManager();
+}
+
+#endif // BLE_FEATURE_SECURITY
+
+void BLE::processEvents()
+{
+    core_util_critical_section_enter();
+    if (event_signaled == false) {
+        core_util_critical_section_exit();
+        return;
+    }
+
+    event_signaled = false;
+    core_util_critical_section_exit();
+
+    transport.processEvents();
+}
+
+void BLE::onEventsToProcess(const BLE::OnEventsToProcessCallback_t &callback)
+{
+    whenEventsToProcess = callback;
+
+    // If events were previously signaled but the handler was not in place then
+    // signal immediately events availability
+    if (event_signaled && whenEventsToProcess) {
+        OnEventsToProcessCallbackContext params = {
+            *this
+        };
+        whenEventsToProcess(&params);
+    }
+}
+
+void BLE::signalEventsToProcess()
+{
+    core_util_critical_section_enter();
+    if (event_signaled == true) {
+        core_util_critical_section_exit();
+        return;
+    }
+
+    event_signaled = true;
+    core_util_critical_section_exit();
+
+    if (whenEventsToProcess) {
+        OnEventsToProcessCallbackContext params = {
+            *this
+        };
+        whenEventsToProcess(&params);
+    }
+}
+
+BLE::BLE(ble::BLEInstanceBase &base) :
+    transport(base),
+    whenEventsToProcess(nullptr),
+    event_signaled(false)
+{
+}

connectivity/FEATURE_BLE/source/DiscoveredCharacteristic.cpp

@@ -0,0 +1,169 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2020 ARM Limited
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ble/common/ble/DiscoveredCharacteristic.h"
+#include "ble/GattClient.h"
+
+ble_error_t
+DiscoveredCharacteristic::read(uint16_t offset) const
+{
+    if (!props.read()) {
+        return BLE_ERROR_OPERATION_NOT_PERMITTED;
+    }
+
+    if (!gattc) {
+        return BLE_ERROR_INVALID_STATE;
+    }
+
+    return gattc->read(connHandle, valueHandle, offset);
+}
+
+struct OneShotReadCallback {
+    static void launch(ble::GattClient* client, ble::connection_handle_t connHandle,
+                       GattAttribute::Handle_t handle, const ble::GattClient::ReadCallback_t& cb) {
+        OneShotReadCallback* oneShot = new OneShotReadCallback(client, connHandle, handle, cb);
+        oneShot->attach();
+        // delete will be made when this callback is called
+    }
+
+private:
+    OneShotReadCallback(ble::GattClient* client, ble::connection_handle_t connHandle,
+                        GattAttribute::Handle_t handle, const ble::GattClient::ReadCallback_t& cb) :
+        _client(client),
+        _connHandle(connHandle),
+        _handle(handle),
+        _callback(cb) { }
+
+    void attach() {
+        _client->onDataRead(makeFunctionPointer(this, &OneShotReadCallback::call));
+    }
+
+    void call(const GattReadCallbackParams* params) {
+        // verifiy that it is the right characteristic on the right connection
+        if (params->connHandle == _connHandle && params->handle == _handle) {
+            _callback(params);
+            _client->onDataRead().detach(makeFunctionPointer(this, &OneShotReadCallback::call));
+            delete this;
+        }
+    }
+
+    ble::GattClient* _client;
+    ble::connection_handle_t _connHandle;
+    GattAttribute::Handle_t _handle;
+    ble::GattClient::ReadCallback_t _callback;
+};
+
+ble_error_t DiscoveredCharacteristic::read(uint16_t offset, const ble::GattClient::ReadCallback_t& onRead) const {
+    ble_error_t error = read(offset);
+    if (error) {
+        return error;
+    }
+
+    OneShotReadCallback::launch(gattc, connHandle, valueHandle, onRead);
+
+    return error;
+}
+
+ble_error_t
+DiscoveredCharacteristic::write(uint16_t length, const uint8_t *value) const
+{
+    if (!props.write()) {
+        return BLE_ERROR_OPERATION_NOT_PERMITTED;
+    }
+
+    if (!gattc) {
+        return BLE_ERROR_INVALID_STATE;
+    }
+
+    return gattc->write(ble::GattClient::GATT_OP_WRITE_REQ, connHandle, valueHandle, length, value);
+}
+
+ble_error_t
+DiscoveredCharacteristic::writeWoResponse(uint16_t length, const uint8_t *value) const
+{
+    if (!props.writeWoResp()) {
+        return BLE_ERROR_OPERATION_NOT_PERMITTED;
+    }
+
+    if (!gattc) {
+        return BLE_ERROR_INVALID_STATE;
+    }
+
+    return gattc->write(ble::GattClient::GATT_OP_WRITE_CMD, connHandle, valueHandle, length, value);
+}
+
+struct OneShotWriteCallback {
+    static void launch(ble::GattClient* client, ble::connection_handle_t connHandle,
+                       GattAttribute::Handle_t handle, const ble::GattClient::WriteCallback_t& cb) {
+        OneShotWriteCallback* oneShot = new OneShotWriteCallback(client, connHandle, handle, cb);
+        oneShot->attach();
+        // delete will be made when this callback is called
+    }
+
+private:
+    OneShotWriteCallback(ble::GattClient* client, ble::connection_handle_t connHandle,
+                        GattAttribute::Handle_t handle, const ble::GattClient::WriteCallback_t& cb) :
+        _client(client),
+        _connHandle(connHandle),
+        _handle(handle),
+        _callback(cb) { }
+
+    void attach() {
+        _client->onDataWritten(makeFunctionPointer(this, &OneShotWriteCallback::call));
+    }
+
+    void call(const GattWriteCallbackParams* params) {
+        // verifiy that it is the right characteristic on the right connection
+        if (params->connHandle == _connHandle && params->handle == _handle) {
+            _callback(params);
+            _client->onDataWritten().detach(makeFunctionPointer(this, &OneShotWriteCallback::call));
+            delete this;
+        }
+    }
+
+    ble::GattClient* _client;
+    ble::connection_handle_t _connHandle;
+    GattAttribute::Handle_t _handle;
+    ble::GattClient::WriteCallback_t _callback;
+};
+
+ble_error_t DiscoveredCharacteristic::write(uint16_t length, const uint8_t *value, const ble::GattClient::WriteCallback_t& onRead) const {
+    ble_error_t error = write(length, value);
+    if (error) {
+        return error;
+    }
+
+    OneShotWriteCallback::launch(gattc, connHandle, valueHandle, onRead);
+
+    return error;
+}
+
+ble_error_t DiscoveredCharacteristic::discoverDescriptors(
+    const CharacteristicDescriptorDiscovery::DiscoveryCallback_t& onCharacteristicDiscovered,
+    const CharacteristicDescriptorDiscovery::TerminationCallback_t& onTermination) const {
+
+    if(!gattc) {
+        return BLE_ERROR_INVALID_STATE;
+    }
+
+    ble_error_t err = gattc->discoverCharacteristicDescriptors(
+        *this, onCharacteristicDiscovered, onTermination
+    );
+
+    return err;
+}

connectivity/FEATURE_BLE/source/FileSecurityDb.cpp

@@ -0,0 +1,440 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2020 ARM Limited
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if BLE_SECURITY_DATABASE_FILESYSTEM
+
+#include "ble/internal/FileSecurityDb.h"
+
+namespace ble {
+
+const uint16_t DB_VERSION = 1;
+
+#define DB_STORE_OFFSET_FLAGS         (0)
+#define DB_STORE_OFFSET_LOCAL_KEYS    (DB_STORE_OFFSET_FLAGS + sizeof(SecurityDistributionFlags_t))
+#define DB_STORE_OFFSET_PEER_KEYS     (DB_STORE_OFFSET_LOCAL_KEYS + sizeof(SecurityEntryKeys_t))
+#define DB_STORE_OFFSET_PEER_IDENTITY (DB_STORE_OFFSET_PEER_KEYS + sizeof(SecurityEntryKeys_t))
+#define DB_STORE_OFFSET_PEER_SIGNING  (DB_STORE_OFFSET_PEER_IDENTITY + sizeof(SecurityEntryIdentity_t))
+
+#define DB_STORE_OFFSET_LOCAL_KEYS_LTK  (DB_STORE_OFFSET_LOCAL_KEYS)
+#define DB_STORE_OFFSET_LOCAL_KEYS_EDIV (DB_STORE_OFFSET_LOCAL_KEYS_LTK + sizeof(ltk_t))
+#define DB_STORE_OFFSET_LOCAL_KEYS_RAND (DB_STORE_OFFSET_LOCAL_KEYS_EDIV + sizeof(ediv_t))
+
+#define DB_STORE_OFFSET_PEER_KEYS_LTK  (DB_STORE_OFFSET_PEER_KEYS)
+#define DB_STORE_OFFSET_PEER_KEYS_EDIV (DB_STORE_OFFSET_PEER_KEYS_LTK + sizeof(ltk_t))
+#define DB_STORE_OFFSET_PEER_KEYS_RAND (DB_STORE_OFFSET_PEER_KEYS_EDIV + sizeof(ediv_t))
+
+#define DB_STORE_OFFSET_PEER_IDENTITY_ADDRESS (DB_STORE_OFFSET_PEER_IDENTITY)
+#define DB_STORE_OFFSET_PEER_IDENTITY_IRK     (DB_STORE_OFFSET_PEER_IDENTITY + sizeof(address_t))
+#define DB_STORE_OFFSET_PEER_IDENTITY_ADDRESS_IS_PUBLIC (DB_STORE_OFFSET_PEER_IDENTITY_IRK + sizeof(irk_t))
+
+#define DB_STORE_OFFSET_PEER_SIGNING_COUNT (DB_STORE_OFFSET_PEER_SIGNING + sizeof(csrk_t))
+
+/* make size multiple of 4 */
+#define PAD4(value) ((((value - 1) / 4) * 4) + 4)
+
+#define DB_SIZE_STORE \
+    PAD4( \
+        sizeof(SecurityDistributionFlags_t) + \
+        sizeof(SecurityEntryKeys_t) + \
+        sizeof(SecurityEntryKeys_t) + \
+        sizeof(SecurityEntryIdentity_t) + \
+        sizeof(SecurityEntrySigning_t) \
+    )
+
+#define DB_SIZE_STORES \
+    (BLE_SECURITY_DATABASE_MAX_ENTRIES * DB_SIZE_STORE)
+
+#define DB_OFFSET_VERSION          (0)
+#define DB_OFFSET_RESTORE          (DB_OFFSET_VERSION + sizeof(DB_VERSION))
+#define DB_OFFSET_LOCAL_IDENTITY   (DB_OFFSET_RESTORE + sizeof(bool))
+#define DB_OFFSET_LOCAL_CSRK       (DB_OFFSET_LOCAL_IDENTITY + sizeof(SecurityEntryIdentity_t))
+#define DB_OFFSET_LOCAL_SIGN_COUNT (DB_OFFSET_LOCAL_CSRK + sizeof(csrk_t))
+#define DB_OFFSET_STORES           (DB_OFFSET_LOCAL_SIGN_COUNT + sizeof(sign_count_t))
+#define DB_OFFSET_MAX              (DB_OFFSET_STORES + DB_SIZE_STORES)
+#define DB_SIZE                    PAD4(DB_OFFSET_MAX)
+
+typedef SecurityDb::entry_handle_t entry_handle_t;
+
+FileSecurityDb::FileSecurityDb(FILE *db_file)
+    : SecurityDb(),
+      _db_file(db_file) {
+    /* init the offset in entries so they point to file positions */
+    for (size_t i = 0; i < get_entry_count(); i++) {
+        _entries[i].file_offset = DB_OFFSET_STORES + i * DB_SIZE_STORE;
+    }
+}
+
+FileSecurityDb::~FileSecurityDb()
+{
+    fclose(_db_file);
+}
+
+FILE* FileSecurityDb::open_db_file(const char *db_path)
+{
+    if (!db_path) {
+        return nullptr;
+    }
+
+    /* try to open an existing file */
+    FILE *db_file = fopen(db_path, "rb+");
+
+    if (!db_file) {
+        /* file doesn't exist, create it */
+        db_file = fopen(db_path, "wb+");
+    }
+
+    if (!db_file) {
+        /* failed to create a file, abort */
+        return nullptr;
+    }
+
+    /* we will check the db file and if the version or size doesn't match
+     * what we expect we will blank it */
+    bool init = false;
+    uint16_t version;
+
+    fseek(db_file, DB_OFFSET_VERSION, SEEK_SET);
+
+    if ((fread(&version, sizeof(version), 1, db_file) == 1) &&
+        (version == DB_VERSION)) {
+        /* if file size differs from database size init the file */
+        fseek(db_file, 0, SEEK_END);
+        if (ftell(db_file) != DB_SIZE) {
+            init = true;
+        }
+    } else {
+        init = true;
+    }
+
+    if (init) {
+        return erase_db_file(db_file);
+    }
+
+    return db_file;
+}
+
+FILE* FileSecurityDb::erase_db_file(FILE* db_file)
+{
+    fseek(db_file, 0, SEEK_SET);
+
+    /* zero the file */
+    const uint32_t zero = 0;
+    size_t count = DB_SIZE / 4;
+    while (count--) {
+        if (fwrite(&zero, sizeof(zero), 1, db_file) != 1) {
+            fclose(db_file);
+            return nullptr;
+        }
+    }
+
+    if (fflush(db_file)) {
+        fclose(db_file);
+        return nullptr;
+    }
+
+    return db_file;
+}
+
+SecurityDistributionFlags_t* FileSecurityDb::get_distribution_flags(
+    entry_handle_t db_handle
+)
+{
+    return reinterpret_cast<SecurityDistributionFlags_t*>(db_handle);
+}
+
+/* local keys */
+
+/* set */
+void FileSecurityDb::set_entry_local_ltk(
+    entry_handle_t db_handle,
+    const ltk_t &ltk
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    entry->flags.ltk_sent = true;
+
+    db_write(&ltk, entry->file_offset + DB_STORE_OFFSET_LOCAL_KEYS_LTK);
+}
+
+void FileSecurityDb::set_entry_local_ediv_rand(
+    entry_handle_t db_handle,
+    const ediv_t &ediv,
+    const rand_t &rand
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    db_write(&ediv, entry->file_offset + DB_STORE_OFFSET_LOCAL_KEYS_EDIV);
+    db_write(&rand, entry->file_offset + DB_STORE_OFFSET_LOCAL_KEYS_RAND);
+}
+
+/* peer's keys */
+
+/* set */
+
+void FileSecurityDb::set_entry_peer_ltk(
+    entry_handle_t db_handle,
+    const ltk_t &ltk
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    entry->flags.ltk_stored = true;
+
+    db_write(&ltk, entry->file_offset + DB_STORE_OFFSET_PEER_KEYS_LTK);
+}
+
+void FileSecurityDb::set_entry_peer_ediv_rand(
+    entry_handle_t db_handle,
+    const ediv_t &ediv,
+    const rand_t &rand
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    db_write(&ediv, entry->file_offset + DB_STORE_OFFSET_PEER_KEYS_EDIV);
+    db_write(&rand, entry->file_offset + DB_STORE_OFFSET_PEER_KEYS_RAND);
+}
+
+void FileSecurityDb::set_entry_peer_irk(
+    entry_handle_t db_handle,
+    const irk_t &irk
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    entry->flags.irk_stored = true;
+
+    db_write(&irk, entry->file_offset + DB_STORE_OFFSET_PEER_IDENTITY_IRK);
+}
+
+void FileSecurityDb::set_entry_peer_bdaddr(
+    entry_handle_t db_handle,
+    bool address_is_public,
+    const address_t &peer_address
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    db_write(&peer_address, entry->file_offset + DB_STORE_OFFSET_PEER_IDENTITY_ADDRESS);
+    db_write(&address_is_public, entry->file_offset + DB_STORE_OFFSET_PEER_IDENTITY_ADDRESS_IS_PUBLIC);
+}
+
+void FileSecurityDb::set_entry_peer_csrk(
+    entry_handle_t db_handle,
+    const csrk_t &csrk
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    entry->flags.csrk_stored = true;
+
+    db_write(&csrk, entry->file_offset + DB_STORE_OFFSET_PEER_SIGNING);
+}
+
+void FileSecurityDb::set_entry_peer_sign_counter(
+    entry_handle_t db_handle,
+    sign_count_t sign_counter
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (entry) {
+        entry->peer_sign_counter = sign_counter;
+    }
+}
+
+void FileSecurityDb::set_local_csrk(
+    const csrk_t &csrk
+)
+{
+    this->SecurityDb::set_local_csrk(csrk);
+    db_write(&_local_csrk, DB_OFFSET_LOCAL_CSRK);
+}
+
+void FileSecurityDb::set_local_identity(
+    const irk_t &irk,
+    const address_t &identity_address,
+    bool public_address
+)
+{
+    this->SecurityDb::set_local_identity(irk, identity_address, public_address);
+    db_write(&_local_identity, DB_OFFSET_LOCAL_IDENTITY);
+}
+
+/* saving and loading from nvm */
+
+void FileSecurityDb::restore()
+{
+    /* restore if requested */
+    bool restore_toggle = false;
+    db_read(&restore_toggle, DB_OFFSET_RESTORE);
+
+    if (!restore_toggle) {
+        erase_db_file(_db_file);
+
+        db_write(&DB_VERSION, DB_OFFSET_VERSION);
+        return;
+    }
+
+    db_read(&_local_identity, DB_OFFSET_LOCAL_IDENTITY);
+    db_read(&_local_csrk, DB_OFFSET_LOCAL_CSRK);
+    db_read(&_local_sign_counter, DB_OFFSET_LOCAL_SIGN_COUNT);
+
+    /* read flags and sign counters */
+    for (size_t i = 0; i < get_entry_count(); i++) {
+        db_read(&_entries[i].flags, _entries[i].file_offset + DB_STORE_OFFSET_FLAGS);
+        db_read(&_entries[i].peer_sign_counter, _entries[i].file_offset + DB_STORE_OFFSET_PEER_SIGNING_COUNT);
+    }
+
+}
+
+void FileSecurityDb::sync(entry_handle_t db_handle)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    db_write(&entry->peer_sign_counter, entry->file_offset + DB_STORE_OFFSET_PEER_SIGNING_COUNT);
+    db_write(&entry->flags, entry->file_offset + DB_STORE_OFFSET_FLAGS);
+    db_write(&_local_sign_counter, DB_OFFSET_LOCAL_SIGN_COUNT);
+}
+
+void FileSecurityDb::set_restore(bool reload)
+{
+    db_write(&reload, DB_OFFSET_RESTORE);
+}
+
+/* helper functions */
+
+uint8_t FileSecurityDb::get_entry_count()
+{
+    return BLE_SECURITY_DATABASE_MAX_ENTRIES;
+}
+
+SecurityDistributionFlags_t* FileSecurityDb::get_entry_handle_by_index(uint8_t index)
+{
+    if (index < BLE_SECURITY_DATABASE_MAX_ENTRIES) {
+        return &_entries[index].flags;
+    } else {
+        return nullptr;
+    }
+}
+
+void FileSecurityDb::reset_entry(entry_handle_t db_entry)
+{
+    entry_t *entry = as_entry(db_entry);
+    if (!entry) {
+        return;
+    }
+
+    fseek(_db_file, entry->file_offset, SEEK_SET);
+    const uint32_t zero = 0;
+    size_t count = DB_SIZE_STORE / 4;
+    while (count--) {
+        fwrite(&zero, sizeof(zero), 1, _db_file);
+    }
+
+    entry->flags = SecurityDistributionFlags_t();
+    entry->peer_sign_counter = 0;
+}
+
+SecurityEntryIdentity_t* FileSecurityDb::read_in_entry_peer_identity(entry_handle_t db_entry)
+{
+    entry_t *entry = as_entry(db_entry);
+    if (!entry) {
+        return nullptr;
+    }
+
+    SecurityEntryIdentity_t* identity = reinterpret_cast<SecurityEntryIdentity_t*>(_buffer);
+    db_read(identity, entry->file_offset + DB_STORE_OFFSET_PEER_IDENTITY);
+
+    return identity;
+};
+
+SecurityEntryKeys_t* FileSecurityDb::read_in_entry_peer_keys(entry_handle_t db_entry)
+{
+    entry_t *entry = as_entry(db_entry);
+    if (!entry) {
+        return nullptr;
+    }
+
+    SecurityEntryKeys_t* keys = reinterpret_cast<SecurityEntryKeys_t*>(_buffer);
+    db_read(keys, entry->file_offset + DB_STORE_OFFSET_PEER_KEYS);
+
+    return keys;
+};
+
+SecurityEntryKeys_t* FileSecurityDb::read_in_entry_local_keys(entry_handle_t db_entry)
+{
+    entry_t *entry = as_entry(db_entry);
+    if (!entry) {
+        return nullptr;
+    }
+
+    SecurityEntryKeys_t* keys = reinterpret_cast<SecurityEntryKeys_t*>(_buffer);
+    db_read(keys, entry->file_offset + DB_STORE_OFFSET_LOCAL_KEYS);
+
+    return keys;
+};
+
+SecurityEntrySigning_t* FileSecurityDb::read_in_entry_peer_signing(entry_handle_t db_entry)
+{
+    entry_t *entry = as_entry(db_entry);
+    if (!entry) {
+        return nullptr;
+    }
+
+    /* only read in the csrk */
+    csrk_t* csrk = reinterpret_cast<csrk_t*>(_buffer);
+    db_read(csrk, entry->file_offset + DB_STORE_OFFSET_PEER_SIGNING);
+
+
+    /* use the counter held in memory */
+    SecurityEntrySigning_t* signing = reinterpret_cast<SecurityEntrySigning_t*>(_buffer);
+    signing->counter = entry->peer_sign_counter;
+
+    return signing;
+};
+
+} /* namespace ble */
+
+#endif // BLE_SECURITY_DATABASE_FILESYSTEM
+

connectivity/FEATURE_BLE/source/KVStoreSecurityDb.cpp

@@ -0,0 +1,427 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2020 ARM Limited
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if BLE_SECURITY_DATABASE_KVSTORE
+
+#include "ble/internal/KVStoreSecurityDb.h"
+
+namespace ble {
+
+#if BLE_SECURITY_DATABASE_MAX_ENTRIES > 9
+#error "BLE_SECURITY_DATABASE_MAX_ENTRIES must be only one digit long"
+#endif
+
+#define ENTRY_INVALID (0xFF)
+
+constexpr uint8_t KVStoreSecurityDb::KVSTORESECURITYDB_VERSION;
+constexpr size_t KVStoreSecurityDb::DB_PREFIX_SIZE;
+constexpr size_t KVStoreSecurityDb::DB_KEY_SIZE;
+constexpr size_t KVStoreSecurityDb::DB_ENTRY_KEY_SIZE;
+constexpr size_t KVStoreSecurityDb::DB_FULL_KEY_SIZE;
+constexpr char KVStoreSecurityDb::DB_PREFIX[DB_PREFIX_SIZE+1];
+
+constexpr char KVStoreSecurityDb::DB_ENTRIES[DB_KEY_SIZE];
+
+constexpr char KVStoreSecurityDb::DB_ENTRY_PEER_IDENTITY[DB_ENTRY_KEY_SIZE];
+constexpr char KVStoreSecurityDb::DB_ENTRY_LOCAL_KEYS[DB_ENTRY_KEY_SIZE];
+constexpr char KVStoreSecurityDb::DB_ENTRY_PEER_KEYS[DB_ENTRY_KEY_SIZE];
+constexpr char KVStoreSecurityDb::DB_ENTRY_PEER_SIGNING[DB_ENTRY_KEY_SIZE];
+
+constexpr char KVStoreSecurityDb::DB_LOCAL_IDENTITY[DB_KEY_SIZE];
+constexpr char KVStoreSecurityDb::DB_LOCAL_CSRK[DB_KEY_SIZE];
+constexpr char KVStoreSecurityDb::DB_LOCAL_SIGN_COUNT[DB_KEY_SIZE];
+
+constexpr char KVStoreSecurityDb::DB_VERSION[DB_KEY_SIZE];
+constexpr char KVStoreSecurityDb::DB_RESTORE[DB_KEY_SIZE];
+
+typedef SecurityDb::entry_handle_t entry_handle_t;
+
+KVStoreSecurityDb::KVStoreSecurityDb()
+    : SecurityDb() {
+    memset(_entries, 0, sizeof(_entries));
+    for (size_t i = 0; i < get_entry_count(); i++) {
+        _entries[i].index = ENTRY_INVALID;
+    }
+}
+
+KVStoreSecurityDb::~KVStoreSecurityDb()
+{
+}
+
+bool KVStoreSecurityDb::open_db()
+{
+    uint8_t version = 0;
+    char db_key[DB_FULL_KEY_SIZE];
+    create_key(db_key, DB_VERSION);
+    size_t size;
+    int ret = kv_get(db_key, &version, sizeof(uint8_t), &size);
+
+    /* kvstore problem (check if it's been successfully initialised before this call) */
+    if (ret != MBED_ERROR_ITEM_NOT_FOUND && (ret != MBED_SUCCESS || size != sizeof(uint8_t))) {
+        return false;
+    }
+
+    /* wipe the db if it's the wrong version or it doesn't exist */
+    if (version != KVSTORESECURITYDB_VERSION) {
+        return erase_db();
+    }
+
+    return true;
+}
+
+bool KVStoreSecurityDb::erase_db()
+{
+    union zero_t {
+        int dummy; /* we need a dummy for initialisation */
+        uint8_t buffer[sizeof(SecurityEntryKeys_t)];
+        entry_t entries[BLE_SECURITY_DATABASE_MAX_ENTRIES];
+    } zero = { 0 };
+    memset(&zero, 0, sizeof(zero));
+
+    /* we zero the database and make sure we can fit all our keys */
+
+    db_write(zero.entries, DB_ENTRIES);
+    db_write((SecurityEntryIdentity_t*)zero.buffer, DB_LOCAL_IDENTITY);
+    db_write((csrk_t*)zero.buffer, DB_LOCAL_CSRK);
+    db_write((sign_count_t*)zero.buffer, DB_LOCAL_SIGN_COUNT);
+
+    bool reload = false;
+    db_write(&reload, DB_RESTORE);
+
+    for (int index = 0; index < BLE_SECURITY_DATABASE_MAX_ENTRIES; ++index) {
+        db_write_entry((SecurityEntryKeys_t*)zero.buffer, DB_ENTRY_LOCAL_KEYS, index);
+        db_write_entry((SecurityEntryIdentity_t*)zero.buffer, DB_ENTRY_PEER_IDENTITY, index);
+        db_write_entry((SecurityEntryKeys_t*)zero.buffer, DB_ENTRY_PEER_KEYS, index);
+        db_write_entry((SecurityEntrySigning_t*)zero.buffer, DB_ENTRY_PEER_SIGNING, index);
+    }
+
+    /* now we write the version and read it back to see if was written succesfully */
+    uint8_t version = KVSTORESECURITYDB_VERSION;
+    db_write(&version, DB_VERSION);
+    version = 0;
+    db_read(&version, DB_VERSION);
+
+    return (version == KVSTORESECURITYDB_VERSION);
+}
+
+SecurityDistributionFlags_t* KVStoreSecurityDb::get_distribution_flags(
+    entry_handle_t db_handle
+)
+{
+    return reinterpret_cast<SecurityDistributionFlags_t*>(db_handle);
+}
+
+/* local keys */
+
+/* set */
+void KVStoreSecurityDb::set_entry_local_ltk(
+    entry_handle_t db_handle,
+    const ltk_t &ltk
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    entry->flags.ltk_sent = true;
+
+    SecurityEntryKeys_t* current_entry = read_in_entry_local_keys(db_handle);
+    current_entry->ltk = ltk;
+
+    db_write_entry(current_entry, DB_ENTRY_LOCAL_KEYS, entry->index);
+}
+
+void KVStoreSecurityDb::set_entry_local_ediv_rand(
+    entry_handle_t db_handle,
+    const ediv_t &ediv,
+    const rand_t &rand
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    SecurityEntryKeys_t* current_entry = read_in_entry_local_keys(db_handle);
+    current_entry->ediv = ediv;
+    current_entry->rand = rand;
+
+    db_write_entry(current_entry, DB_ENTRY_LOCAL_KEYS, entry->index);
+}
+
+/* peer's keys */
+
+/* set */
+
+void KVStoreSecurityDb::set_entry_peer_ltk(
+    entry_handle_t db_handle,
+    const ltk_t &ltk
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    entry->flags.ltk_stored = true;
+
+    SecurityEntryKeys_t* current_entry = read_in_entry_peer_keys(db_handle);
+    current_entry->ltk = ltk;
+
+    db_write_entry(current_entry, DB_ENTRY_PEER_KEYS, entry->index);
+}
+
+void KVStoreSecurityDb::set_entry_peer_ediv_rand(
+    entry_handle_t db_handle,
+    const ediv_t &ediv,
+    const rand_t &rand
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    SecurityEntryKeys_t* current_entry = read_in_entry_peer_keys(db_handle);
+    current_entry->ediv = ediv;
+    current_entry->rand = rand;
+
+    db_write_entry(current_entry, DB_ENTRY_PEER_KEYS, entry->index);
+}
+
+void KVStoreSecurityDb::set_entry_peer_irk(
+    entry_handle_t db_handle,
+    const irk_t &irk
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    entry->flags.irk_stored = true;
+
+    SecurityEntryIdentity_t* current_entry = read_in_entry_peer_identity(db_handle);
+    current_entry->irk = irk;
+
+    db_write_entry(current_entry, DB_ENTRY_PEER_IDENTITY, entry->index);
+}
+
+void KVStoreSecurityDb::set_entry_peer_bdaddr(
+    entry_handle_t db_handle,
+    bool address_is_public,
+    const address_t &peer_address
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    SecurityEntryIdentity_t* current_entry = read_in_entry_peer_identity(db_handle);
+    current_entry->identity_address = peer_address;
+    current_entry->identity_address_is_public = address_is_public;
+
+    db_write_entry(current_entry, DB_ENTRY_PEER_IDENTITY, entry->index);
+}
+
+void KVStoreSecurityDb::set_entry_peer_csrk(
+    entry_handle_t db_handle,
+    const csrk_t &csrk
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    entry->flags.csrk_stored = true;
+
+    SecurityEntrySigning_t* current_entry = read_in_entry_peer_signing(db_handle);
+    current_entry->csrk = csrk;
+
+    db_write_entry(current_entry, DB_ENTRY_PEER_SIGNING, entry->index);
+}
+
+void KVStoreSecurityDb::set_entry_peer_sign_counter(
+    entry_handle_t db_handle,
+    sign_count_t sign_counter
+)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (entry) {
+        entry->peer_sign_counter = sign_counter;
+    }
+}
+
+void KVStoreSecurityDb::set_local_csrk(
+    const csrk_t &csrk
+)
+{
+    this->SecurityDb::set_local_csrk(csrk);
+    db_write(&_local_csrk, DB_LOCAL_CSRK);
+}
+
+void KVStoreSecurityDb::set_local_identity(
+    const irk_t &irk,
+    const address_t &identity_address,
+    bool public_address
+)
+{
+    this->SecurityDb::set_local_identity(irk, identity_address, public_address);
+    db_write(&_local_identity, DB_LOCAL_IDENTITY);
+}
+
+/* saving and loading from nvm */
+
+void KVStoreSecurityDb::restore()
+{
+    /* restore if requested */
+    bool restore_toggle = false;
+    db_read(&restore_toggle, DB_RESTORE);
+
+    if (!restore_toggle) {
+        erase_db();
+        return;
+    }
+
+    db_read(&_entries, DB_ENTRIES);
+    db_read(&_local_identity, DB_LOCAL_IDENTITY);
+    db_read(&_local_csrk, DB_LOCAL_CSRK);
+    db_read(&_local_sign_counter, DB_LOCAL_SIGN_COUNT);
+}
+
+void KVStoreSecurityDb::sync(entry_handle_t db_handle)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    /* all entries are stored in a single key so we store them all*/
+    db_write(&_entries, DB_ENTRIES);
+    db_write(&_local_identity, DB_LOCAL_IDENTITY);
+    db_write(&_local_csrk, DB_LOCAL_CSRK);
+    db_write(&_local_sign_counter, DB_LOCAL_SIGN_COUNT);
+}
+
+void KVStoreSecurityDb::set_restore(bool reload)
+{
+    db_write(&reload, DB_RESTORE);
+}
+
+/* helper functions */
+
+uint8_t KVStoreSecurityDb::get_entry_count()
+{
+    return BLE_SECURITY_DATABASE_MAX_ENTRIES;
+}
+
+SecurityDistributionFlags_t* KVStoreSecurityDb::get_entry_handle_by_index(uint8_t index)
+{
+    if (index < get_entry_count()) {
+        return &_entries[index].flags;
+    } else {
+        return nullptr;
+    }
+}
+
+void KVStoreSecurityDb::reset_entry(entry_handle_t db_handle)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return;
+    }
+
+    if (entry->index != ENTRY_INVALID) {
+        uint8_t zero_buffer[sizeof(SecurityEntryKeys_t)] = {0};
+
+        db_write_entry((SecurityEntryKeys_t*)zero_buffer, DB_ENTRY_LOCAL_KEYS, entry->index);
+        db_write_entry((SecurityEntryIdentity_t*)zero_buffer, DB_ENTRY_PEER_IDENTITY, entry->index);
+        db_write_entry((SecurityEntryKeys_t*)zero_buffer, DB_ENTRY_PEER_KEYS, entry->index);
+        db_write_entry((SecurityEntrySigning_t*)zero_buffer, DB_ENTRY_PEER_SIGNING, entry->index);
+
+        entry->index = ENTRY_INVALID;
+    }
+
+    entry->flags = SecurityDistributionFlags_t();
+    entry->peer_sign_counter = 0;
+}
+
+SecurityEntryIdentity_t* KVStoreSecurityDb::read_in_entry_peer_identity(entry_handle_t db_handle)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return nullptr;
+    }
+
+    SecurityEntryIdentity_t* identity = reinterpret_cast<SecurityEntryIdentity_t*>(_buffer);
+    db_read_entry(identity, DB_ENTRY_PEER_IDENTITY, entry->index);
+
+    return identity;
+};
+
+SecurityEntryKeys_t* KVStoreSecurityDb::read_in_entry_peer_keys(entry_handle_t db_handle)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return nullptr;
+    }
+
+    SecurityEntryKeys_t* keys = reinterpret_cast<SecurityEntryKeys_t*>(_buffer);
+    db_read_entry(keys, DB_ENTRY_PEER_KEYS, entry->index);
+
+    return keys;
+};
+
+SecurityEntryKeys_t* KVStoreSecurityDb::read_in_entry_local_keys(entry_handle_t db_handle)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return nullptr;
+    }
+
+    SecurityEntryKeys_t* keys = reinterpret_cast<SecurityEntryKeys_t*>(_buffer);
+    db_read_entry(keys, DB_ENTRY_LOCAL_KEYS, entry->index);
+
+    return keys;
+};
+
+SecurityEntrySigning_t* KVStoreSecurityDb::read_in_entry_peer_signing(entry_handle_t db_handle)
+{
+    entry_t *entry = as_entry(db_handle);
+    if (!entry) {
+        return nullptr;
+    }
+
+    /* only read in the csrk */
+    csrk_t* csrk = reinterpret_cast<csrk_t*>(_buffer);
+    db_read_entry(csrk, DB_ENTRY_PEER_SIGNING, entry->index);
+
+
+    /* use the counter held in memory */
+    SecurityEntrySigning_t* signing = reinterpret_cast<SecurityEntrySigning_t*>(_buffer);
+    signing->counter = entry->peer_sign_counter;
+
+    return signing;
+};
+
+} /* namespace generic */
+
+#endif // BLE_SECURITY_DATABASE_KVSTORE

connectivity/FEATURE_BLE/source/gap/AdvertisingDataBuilder.cpp

@@ -0,0 +1,539 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "gap/AdvertisingDataBuilder.h"
+
+// Implementation notes
+// Advertising data are organized as follow:
+//   - byte 0: Size of the rest of the field
+//   - byte 1: type of the field
+//   - byte 2 to the last byte: field value.
+// An advertising data can contain at most a single instance of a field type.
+
+#define FIELD_TYPE_INDEX 1
+#define COMPANY_IDENTIFIER_SIZE 2
+
+// A field is represented by a type and a value. The size of the field
+// must fit in a byte therefore, the size of DATA cannot be larger than
+// 0xFE
+#define MAX_DATA_FIELD_SIZE 0xFE
+
+#define FIELD_HEADER_SIZE 2
+
+namespace ble {
+
+namespace {
+
+mbed::Span<const uint8_t> as_span(const int8_t& v)
+{
+    return mbed::Span<const uint8_t>(reinterpret_cast<const uint8_t*>(&v), sizeof(v));
+}
+
+mbed::Span<const uint8_t> as_span(const uint8_t& v)
+{
+    return mbed::Span<const uint8_t>(static_cast<const uint8_t*>(&v), sizeof(v));
+}
+
+template<typename Rep, uint32_t TB, typename Range, typename F>
+mbed::Span<const uint8_t> as_span(const Duration<Rep, TB, Range, F>& d)
+{
+    return mbed::Span<const uint8_t>(reinterpret_cast<const uint8_t*>(d.storage()), sizeof(d.value()));
+}
+
+template<typename T, typename Rep>
+mbed::Span<const uint8_t> as_span(const SafeEnum<T, Rep>& v)
+{
+    return mbed::Span<const uint8_t>(reinterpret_cast<const uint8_t*>(v.storage()), sizeof(v.value()));
+}
+
+}
+
+AdvertisingDataBuilder::AdvertisingDataBuilder(mbed::Span<uint8_t> buffer) :
+    _buffer(buffer),
+    _payload_length(0)
+{
+}
+
+AdvertisingDataBuilder::AdvertisingDataBuilder(uint8_t *buffer, size_t buffer_size) :
+    _buffer(buffer, buffer_size),
+    _payload_length(0)
+{
+}
+
+mbed::Span<const uint8_t> AdvertisingDataBuilder::getAdvertisingData() const
+{
+    return _buffer.first(_payload_length);
+}
+
+ble_error_t AdvertisingDataBuilder::addData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    if (findField(advDataType) != nullptr) {
+        return BLE_ERROR_OPERATION_NOT_PERMITTED;
+    } else {
+        return addField(advDataType, fieldData);
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::replaceData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field == nullptr) {
+        return BLE_ERROR_NOT_FOUND;
+    }
+
+    return replaceField(advDataType, fieldData, field);
+}
+
+ble_error_t AdvertisingDataBuilder::appendData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field == nullptr) {
+        return BLE_ERROR_NOT_FOUND;
+    }
+
+    return appendToField(fieldData, field);
+}
+
+ble_error_t AdvertisingDataBuilder::removeData(
+    adv_data_type_t advDataType
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field == nullptr) {
+        return BLE_ERROR_NOT_FOUND;
+    }
+
+    return removeField(field);
+}
+
+ble_error_t AdvertisingDataBuilder::addOrReplaceData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field != nullptr) {
+        return replaceField(advDataType, fieldData, field);
+    } else {
+        return addField(advDataType, fieldData);
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::addOrAppendData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field != nullptr) {
+        return appendToField(fieldData, field);
+    } else {
+        return addField(advDataType, fieldData);
+    }
+}
+
+void AdvertisingDataBuilder::clear()
+{
+    memset(_buffer.data(), 0, _buffer.size());
+    _payload_length = 0;
+}
+
+ble_error_t AdvertisingDataBuilder::setAppearance(
+    adv_data_appearance_t appearance
+)
+{
+    return addOrReplaceData(adv_data_type_t::APPEARANCE, as_span(appearance));
+}
+
+ble_error_t AdvertisingDataBuilder::setFlags(adv_data_flags_t flags)
+{
+    uint8_t flags_byte = flags.value();
+    return addOrReplaceData(adv_data_type_t::FLAGS, as_span(flags_byte));
+}
+
+ble_error_t AdvertisingDataBuilder::setTxPowerAdvertised(
+    advertising_power_t txPower
+)
+{
+    return addOrReplaceData(adv_data_type_t::TX_POWER_LEVEL, as_span(txPower));
+}
+
+ble_error_t AdvertisingDataBuilder::setName(
+    const char *name,
+    bool complete
+)
+{
+    mbed::Span<const uint8_t> name_span((const uint8_t *) name, strlen(name));
+
+    if (complete) {
+        return addOrReplaceData(adv_data_type_t::COMPLETE_LOCAL_NAME, name_span);
+    } else {
+        return addOrReplaceData(adv_data_type_t::SHORTENED_LOCAL_NAME, name_span);
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::setManufacturerSpecificData(
+    mbed::Span<const uint8_t> data
+)
+{
+    // manufacturer specific data should at least contain the vendor ID.
+    if (data.size() < COMPANY_IDENTIFIER_SIZE) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    return addOrReplaceData(adv_data_type_t::MANUFACTURER_SPECIFIC_DATA, data);
+}
+
+ble_error_t AdvertisingDataBuilder::setAdvertisingInterval(
+    adv_interval_t interval
+)
+{
+    // Note: Advertising interval in advertisement MUST be represented in a 16bit
+    // value.
+    if (interval.value() > 0xFFFF) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    return addOrReplaceData(
+        adv_data_type_t::ADVERTISING_INTERVAL,
+        as_span(interval)
+    );
+}
+
+ble_error_t AdvertisingDataBuilder::setConnectionIntervalPreference(
+    conn_interval_t min,
+    conn_interval_t max
+)
+{
+    uint8_t interval[2 * sizeof(conn_interval_t::representation_t)];
+    memcpy(interval, max.storage(), sizeof(max.value()));
+    memcpy(interval + sizeof(max.value()), min.storage(), sizeof(min.value()));
+
+    return addOrReplaceData(
+        adv_data_type_t::SLAVE_CONNECTION_INTERVAL_RANGE,
+        interval
+    );
+}
+
+ble_error_t AdvertisingDataBuilder::setServiceData(
+    UUID service,
+    mbed::Span<const uint8_t> data
+)
+{
+    if (service.getLen() + data.size() > MAX_DATA_FIELD_SIZE) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    adv_data_type_t short_type = adv_data_type_t::SERVICE_DATA_16BIT_ID;
+    adv_data_type_t long_type = adv_data_type_t::SERVICE_DATA_128BIT_ID;
+
+    size_t total_size = FIELD_HEADER_SIZE + service.getLen() + data.size();
+    size_t old_size = getFieldSize(
+        (service.shortOrLong() == UUID::UUID_TYPE_SHORT) ? short_type : long_type
+    );
+
+    /* if we can't fit the new data do not proceed */
+    if (total_size > (_buffer.size() - (_payload_length - old_size))) {
+        return BLE_ERROR_BUFFER_OVERFLOW;
+    }
+
+    /* this will insert only the UUID (and remove old data) */
+    ble_error_t status = setUUIDData(
+        mbed::make_Span(&service, 1),
+        short_type,
+        long_type
+    );
+
+    if (status != BLE_ERROR_NONE) {
+        /* we already checked for size so this must not happen */
+        return BLE_ERROR_INTERNAL_STACK_FAILURE;
+    }
+
+    status = appendData(
+        (service.shortOrLong() == UUID::UUID_TYPE_SHORT) ? short_type : long_type,
+        data
+    );
+
+    if (status != BLE_ERROR_NONE) {
+        return BLE_ERROR_INTERNAL_STACK_FAILURE;
+    }
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t AdvertisingDataBuilder::setLocalServiceList(
+    mbed::Span<const UUID> data,
+    bool complete
+)
+{
+    adv_data_type_t short_type = complete ?
+        adv_data_type_t::COMPLETE_LIST_16BIT_SERVICE_IDS :
+        adv_data_type_t::INCOMPLETE_LIST_16BIT_SERVICE_IDS;
+
+    adv_data_type_t long_type = complete ?
+        adv_data_type_t::COMPLETE_LIST_128BIT_SERVICE_IDS :
+        adv_data_type_t::INCOMPLETE_LIST_128BIT_SERVICE_IDS;
+
+    return setUUIDData(data, short_type, long_type);
+}
+
+ble_error_t AdvertisingDataBuilder::setRequestedServiceList(
+    mbed::Span<const UUID> data
+)
+{
+    adv_data_type_t short_type = adv_data_type_t::LIST_16BIT_SOLICITATION_IDS;
+    adv_data_type_t long_type = adv_data_type_t::LIST_128BIT_SOLICITATION_IDS;
+
+    return setUUIDData(data, short_type, long_type);
+}
+
+ble_error_t AdvertisingDataBuilder::getData(
+    mbed::Span<const uint8_t> &data,
+    adv_data_type_t advDataType
+)
+{
+    uint8_t *field = findField(advDataType);
+    if (field) {
+        uint8_t data_length = field[0] - 1 /* skip type */;
+        data = mbed::make_Span((const uint8_t *) (field + FIELD_HEADER_SIZE), data_length);
+        return BLE_ERROR_NONE;
+    } else {
+        return BLE_ERROR_NOT_FOUND;
+    }
+}
+
+uint8_t *AdvertisingDataBuilder::findField(adv_data_type_t type)
+{
+    /* Scan through advertisement data */
+    for (uint8_t idx = 0; idx < _payload_length;) {
+        uint8_t fieldType = _buffer[idx + FIELD_TYPE_INDEX];
+
+        if (fieldType == type.value()) {
+            return _buffer.data() + idx;
+        }
+
+        /* Advance to next field */
+        idx += _buffer[idx] + 1;
+    }
+
+    return nullptr;
+}
+
+uint8_t AdvertisingDataBuilder::getFieldSize(adv_data_type_t type)
+{
+    uint8_t *field = findField(type);
+    if (field) {
+        return field[0] + 1 /* field size is not included so we add it */;
+    } else {
+        return 0;
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::addField(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    if (fieldData.size() > MAX_DATA_FIELD_SIZE) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    /* Make sure we don't exceed the buffer size */
+    if (_payload_length + fieldData.size() + FIELD_HEADER_SIZE > _buffer.size()) {
+        return BLE_ERROR_BUFFER_OVERFLOW;
+    }
+
+    /* Field length (includes field ID byte) */
+    _buffer[_payload_length] = fieldData.size() + /* type */ 1;
+    ++_payload_length;
+
+    /* Field ID. */
+    _buffer[_payload_length] = advDataType.value();
+    ++_payload_length;
+
+    /* Payload. */
+    memcpy(&_buffer[_payload_length], fieldData.data(), fieldData.size());
+    _payload_length += fieldData.size();
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t AdvertisingDataBuilder::appendToField(
+    mbed::Span<const uint8_t> fieldData,
+    uint8_t *field
+)
+{
+    if (fieldData.size() + field[0] > 0xFF /* field[0] already includes the type byte */) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    /* Check if data fits */
+    if ((_payload_length + fieldData.size()) <= _buffer.size()) {
+        uint8_t old_data_length = field[0];
+
+        /* get the size of bytes in the payload after the field */
+        size_t remainder_size = _payload_length -
+            (field - _buffer.data()) - /* length of all data before the field */
+            (old_data_length + 1) /* length of the old field */;
+
+        /* move data after the field to fit new data */
+        if (remainder_size) {
+            memmove(
+                field + old_data_length + 1 + fieldData.size(),
+                field + old_data_length + 1,
+                remainder_size
+            );
+        }
+
+        /* append new data */
+        memcpy(field + old_data_length + 1, fieldData.data(), fieldData.size());
+
+        /* Increment lengths */
+        field[0] += fieldData.size();
+        _payload_length += fieldData.size();
+
+        return BLE_ERROR_NONE;
+    } else {
+        return BLE_ERROR_BUFFER_OVERFLOW;
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::replaceField(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData,
+    uint8_t *field
+)
+{
+    if (fieldData.size() > MAX_DATA_FIELD_SIZE) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    uint8_t old_data_length = field[0] - 1;
+
+    /* New data has same length, do in-order replacement */
+    if (fieldData.size() == old_data_length) {
+        memcpy(field + 2, fieldData.data(), old_data_length);
+
+        return BLE_ERROR_NONE;
+    } else {
+        /* Check if data fits */
+        if ((_payload_length - old_data_length + fieldData.size()) <= _buffer.size()) {
+            removeField(field);
+
+            /* Add new field */
+            return addField(advDataType, fieldData);
+        } else {
+            return BLE_ERROR_BUFFER_OVERFLOW;
+        }
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::removeField(uint8_t *field)
+{
+    /* stored length + the byte containing length */
+    uint8_t old_field_length = field[0] + 1;
+
+    memmove(field, field + old_field_length, old_field_length);
+
+    _payload_length -= old_field_length;
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t AdvertisingDataBuilder::setUUIDData(
+    mbed::Span<const UUID> data,
+    adv_data_type_t shortType,
+    adv_data_type_t longType
+)
+{
+    ble_error_t status = BLE_ERROR_NONE;
+
+    /* first count all the bytes we need to store all the UUIDs */
+    size_t size_long = 0;
+    size_t size_short = 0;
+
+    for (size_t i = 0, end = data.size(); i < end; ++i) {
+        if (data[i].shortOrLong() == UUID::UUID_TYPE_SHORT) {
+            size_short += data[i].getLen();
+        } else {
+            size_long += data[i].getLen();
+        }
+    }
+
+    if ((size_long > MAX_DATA_FIELD_SIZE) || (size_short > MAX_DATA_FIELD_SIZE)) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    /* UUID data consists of a type byte, size byte and the list UUIDs itself, we include
+     * the header (type and size bytes) size only if the size of the UUIDs is non-zero
+     * (!!non_zero_variable) == 1 */
+    size_t long_uuid_data_size = (!!size_long) * FIELD_HEADER_SIZE + size_long;
+    size_t short_uuid_data_size = (!!size_short) * FIELD_HEADER_SIZE + size_short;
+    size_t new_size = long_uuid_data_size + short_uuid_data_size;
+
+    /* count all the bytes of existing data */
+    size_t old_size = getFieldSize(shortType) + getFieldSize(longType);
+
+    /* if we can't fit the new data do not proceed */
+    if (new_size > _buffer.size() - (_payload_length - old_size)) {
+        return BLE_ERROR_BUFFER_OVERFLOW;
+    }
+
+    /* otherwise wipe old data */
+    removeData(shortType);
+    removeData(longType);
+
+    /* and insert individual UUIDs into appropriate fields */
+    for (size_t i = 0, end = data.size(); i < end; ++i) {
+        adv_data_type_t field_type = (data[i].shortOrLong() == UUID::UUID_TYPE_SHORT) ? shortType : longType;
+
+        mbed::Span<const uint8_t> span(data[i].getBaseUUID(), data[i].getLen());
+
+        uint8_t *field = findField(field_type);
+
+        if (field) {
+            status = appendToField(span, field);
+            if (status != BLE_ERROR_NONE) {
+                /* we already checked for size so this must not happen */
+                return BLE_ERROR_INTERNAL_STACK_FAILURE;
+            }
+        } else {
+            status = addField(field_type, span);
+            if (status != BLE_ERROR_NONE) {
+                /* we already checked for size so this must not happen */
+                return BLE_ERROR_INTERNAL_STACK_FAILURE;
+            }
+        }
+    }
+
+    return status;
+}
+
+} // end of namespace ble

connectivity/FEATURE_BLE/source/gap/AdvertisingParameters.cpp

@@ -0,0 +1,25 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+*/
+
+#include "gap/AdvertisingParameters.h"
+
+namespace ble {
+
+const uint32_t AdvertisingParameters::DEFAULT_ADVERTISING_INTERVAL_MIN;
+const uint32_t AdvertisingParameters::DEFAULT_ADVERTISING_INTERVAL_MAX;
+const uint32_t AdvertisingParameters::GAP_ADV_PARAMS_INTERVAL_MIN_NONCON;
+
+} // namespace ble

connectivity/FEATURE_BLE/source/gap/ConnectionParameters.cpp

@@ -0,0 +1,136 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+*/
+
+#include "gap/ConnectionParameters.h"
+
+namespace ble {
+
+ConnectionParameters::ConnectionParameters(
+    phy_t phy,
+    scan_interval_t scanInterval,
+    scan_window_t scanWindow,
+    conn_interval_t minConnectionInterval,
+    conn_interval_t maxConnectionInterval,
+    slave_latency_t slaveLatency,
+    supervision_timeout_t connectionSupervisionTimeout,
+    conn_event_length_t minEventLength,
+    conn_event_length_t maxEventLength
+) :
+    _filterPolicy(initiator_filter_policy_t::NO_FILTER),
+    _ownAddressType(own_address_type_t::RANDOM)
+{
+    for (uint8_t i = 0; i < MAX_PARAM_PHYS; ++i) {
+        _enabledPhy[i] = false;
+    }
+    if (phy != phy_t::NONE) {
+        uint8_t phy_index = phyToIndex(phy);
+
+        if (phy_index < MAX_PARAM_PHYS) {
+            _scanInterval[phy_index] = scanInterval.value();
+            _scanWindow[phy_index] = scanWindow.value();
+            _minConnectionInterval[phy_index] = minConnectionInterval.value();
+            _maxConnectionInterval[phy_index] = maxConnectionInterval.value();
+            _slaveLatency[phy_index] = slaveLatency.value();
+            _connectionSupervisionTimeout[phy_index] = connectionSupervisionTimeout.value();
+            _enabledPhy[phy_index] = true;
+            _minEventLength[phy_index] = minEventLength.value();
+            _maxEventLength[phy_index] = maxEventLength.value();
+        }
+    }
+}
+
+/* setters */
+
+ConnectionParameters &ConnectionParameters::setScanParameters(
+    phy_t phy,
+    scan_interval_t scanInterval,
+    scan_window_t scanWindow
+)
+{
+    uint8_t phy_index = handlePhyToggle(phy, true);
+
+    if (phy_index < MAX_PARAM_PHYS) {
+        _scanInterval[phy_index] = scanInterval.value();
+        _scanWindow[phy_index] = scanWindow.value();
+    }
+
+    return *this;
+}
+
+ConnectionParameters &ConnectionParameters::setConnectionParameters(
+    phy_t phy,
+    conn_interval_t minConnectionInterval,
+    conn_interval_t maxConnectionInterval,
+    slave_latency_t slaveLatency,
+    supervision_timeout_t connectionSupervisionTimeout,
+    conn_event_length_t minEventLength,
+    conn_event_length_t maxEventLength
+)
+{
+    uint8_t phy_index = handlePhyToggle(phy, true);
+
+    if (phy_index < MAX_PARAM_PHYS) {
+        _minConnectionInterval[phy_index] = minConnectionInterval.value();
+        _maxConnectionInterval[phy_index] = maxConnectionInterval.value();
+        _slaveLatency[phy_index] = slaveLatency.value();
+        _connectionSupervisionTimeout[phy_index] = connectionSupervisionTimeout.value();
+
+        /* avoid overflows and truncation */
+        if (minEventLength.value() > maxEventLength.value()) {
+            minEventLength = maxEventLength;
+        }
+
+        _minEventLength[phy_index] = minEventLength.value();
+        _maxEventLength[phy_index] = maxEventLength.value();
+    }
+
+    return *this;
+}
+
+#if BLE_FEATURE_PHY_MANAGEMENT
+/** Handle the swapping of 2M and CODED so that the array is ready for the pal call. */
+void ConnectionParameters::swapCodedAnd2M()
+{
+    uint16_t scanInterval = _scanInterval[LE_2M_INDEX];
+    uint16_t scanWindow = _scanWindow[LE_2M_INDEX];
+    uint16_t minConnectionInterval = _minConnectionInterval[LE_2M_INDEX];
+    uint16_t maxConnectionInterval = _maxConnectionInterval[LE_2M_INDEX];
+    uint16_t slaveLatency = _slaveLatency[LE_2M_INDEX];
+    uint16_t connectionSupervisionTimeout = _connectionSupervisionTimeout[LE_2M_INDEX];
+    uint16_t minEventLength = _minEventLength[LE_2M_INDEX];
+    uint16_t maxEventLength = _maxEventLength[LE_2M_INDEX];
+
+    _scanInterval[LE_2M_INDEX] = _scanInterval[LE_CODED_INDEX];
+    _scanWindow[LE_2M_INDEX] = _scanWindow[LE_CODED_INDEX];
+    _minConnectionInterval[LE_2M_INDEX] = _minConnectionInterval[LE_CODED_INDEX];
+    _maxConnectionInterval[LE_2M_INDEX] = _maxConnectionInterval[LE_CODED_INDEX];
+    _slaveLatency[LE_2M_INDEX] = _slaveLatency[LE_CODED_INDEX];
+    _connectionSupervisionTimeout[LE_2M_INDEX] = _connectionSupervisionTimeout[LE_CODED_INDEX];
+    _minEventLength[LE_2M_INDEX] = _minEventLength[LE_CODED_INDEX];
+    _maxEventLength[LE_2M_INDEX] = _maxEventLength[LE_CODED_INDEX];
+
+    _scanInterval[LE_CODED_INDEX] = scanInterval;
+    _scanWindow[LE_CODED_INDEX] = scanWindow;
+    _minConnectionInterval[LE_CODED_INDEX] = minConnectionInterval;
+    _maxConnectionInterval[LE_CODED_INDEX] = maxConnectionInterval;
+    _slaveLatency[LE_CODED_INDEX] = slaveLatency;
+    _connectionSupervisionTimeout[LE_CODED_INDEX] = connectionSupervisionTimeout;
+    _minEventLength[LE_CODED_INDEX] = minEventLength;
+    _maxEventLength[LE_CODED_INDEX] = maxEventLength;
+}
+#endif // BLE_FEATURE_PHY_MANAGEMENT
+
+} // namespace ble