From e55ad9019cc6bdc5e3d2acfce5e0ccfd47717f4c Mon Sep 17 00:00:00 2001 From: "Dr. Michael Lauer" Date: Wed, 23 Apr 2025 15:39:05 +0200 Subject: [PATCH] Introduce L2CAP infrastructure. L2CAP is the underlying technology powering GATT. BLE 5 exposes L2CAP COC (Connection Oriented Channels) allowing a streaming API that leads to much higher throughputs than you can achieve with updating GATT characteristics. The patch follows the established infrastructure very closely. The main components are: - `NimBLEL2CAPChannel`, encapsulating an L2CAP COC. - `NimBLEL2CAPServer`, encapsulating the L2CAP service. - `Examples/L2CAP`, containing a client and a server application. Apart from these, only minor adjustments to the existing code was necessary. --- CMakeLists.txt | 2 + examples/L2CAP/.gitignore | 5 + examples/L2CAP/L2CAP_Client/CMakeLists.txt | 7 + examples/L2CAP/L2CAP_Client/Makefile | 3 + .../L2CAP/L2CAP_Client/main/CMakeLists.txt | 4 + examples/L2CAP/L2CAP_Client/main/component.mk | 4 + .../L2CAP/L2CAP_Client/main/idf_component.yml | 3 + examples/L2CAP/L2CAP_Client/main/main.cpp | 165 ++++++++++ .../L2CAP/L2CAP_Client/sdkconfig.defaults | 13 + examples/L2CAP/L2CAP_Server/CMakeLists.txt | 7 + examples/L2CAP/L2CAP_Server/Makefile | 3 + .../L2CAP/L2CAP_Server/main/CMakeLists.txt | 4 + examples/L2CAP/L2CAP_Server/main/component.mk | 4 + .../L2CAP/L2CAP_Server/main/idf_component.yml | 3 + examples/L2CAP/L2CAP_Server/main/main.cpp | 90 ++++++ .../L2CAP/L2CAP_Server/sdkconfig.defaults | 13 + src/NimBLEDevice.cpp | 33 ++ src/NimBLEDevice.h | 21 ++ src/NimBLEL2CAPChannel.cpp | 297 ++++++++++++++++++ src/NimBLEL2CAPChannel.h | 120 +++++++ src/NimBLEL2CAPServer.cpp | 36 +++ src/NimBLEL2CAPServer.h | 39 +++ 22 files changed, 876 insertions(+) create mode 100644 examples/L2CAP/.gitignore create mode 100644 examples/L2CAP/L2CAP_Client/CMakeLists.txt create mode 100644 examples/L2CAP/L2CAP_Client/Makefile create mode 100644 examples/L2CAP/L2CAP_Client/main/CMakeLists.txt create mode 100644 examples/L2CAP/L2CAP_Client/main/component.mk create mode 100644 examples/L2CAP/L2CAP_Client/main/idf_component.yml create mode 100644 examples/L2CAP/L2CAP_Client/main/main.cpp create mode 100644 examples/L2CAP/L2CAP_Client/sdkconfig.defaults create mode 100644 examples/L2CAP/L2CAP_Server/CMakeLists.txt create mode 100644 examples/L2CAP/L2CAP_Server/Makefile create mode 100644 examples/L2CAP/L2CAP_Server/main/CMakeLists.txt create mode 100644 examples/L2CAP/L2CAP_Server/main/component.mk create mode 100644 examples/L2CAP/L2CAP_Server/main/idf_component.yml create mode 100644 examples/L2CAP/L2CAP_Server/main/main.cpp create mode 100644 examples/L2CAP/L2CAP_Server/sdkconfig.defaults create mode 100644 src/NimBLEL2CAPChannel.cpp create mode 100644 src/NimBLEL2CAPChannel.h create mode 100644 src/NimBLEL2CAPServer.cpp create mode 100644 src/NimBLEL2CAPServer.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 60df811..d5eb047 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,6 +55,8 @@ idf_component_register( "src/NimBLEEddystoneTLM.cpp" "src/NimBLEExtAdvertising.cpp" "src/NimBLEHIDDevice.cpp" + "src/NimBLEL2CAPChannel.cpp" + "src/NimBLEL2CAPServer.cpp" "src/NimBLERemoteCharacteristic.cpp" "src/NimBLERemoteDescriptor.cpp" "src/NimBLERemoteService.cpp" diff --git a/examples/L2CAP/.gitignore b/examples/L2CAP/.gitignore new file mode 100644 index 0000000..f955377 --- /dev/null +++ b/examples/L2CAP/.gitignore @@ -0,0 +1,5 @@ +.vscode +build +sdkconfig +sdkconfig.old +dependencies.lock diff --git a/examples/L2CAP/L2CAP_Client/CMakeLists.txt b/examples/L2CAP/L2CAP_Client/CMakeLists.txt new file mode 100644 index 0000000..57b6882 --- /dev/null +++ b/examples/L2CAP/L2CAP_Client/CMakeLists.txt @@ -0,0 +1,7 @@ +# The following lines of boilerplate have to be in your project's +# CMakeLists in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.5) + +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +set(SUPPORTED_TARGETS esp32 esp32s3 esp32c3 esp32c6) +project(L2CAP_client) diff --git a/examples/L2CAP/L2CAP_Client/Makefile b/examples/L2CAP/L2CAP_Client/Makefile new file mode 100644 index 0000000..7eeac87 --- /dev/null +++ b/examples/L2CAP/L2CAP_Client/Makefile @@ -0,0 +1,3 @@ +PROJECT_NAME := L2CAP_client + +include $(IDF_PATH)/make/project.mk diff --git a/examples/L2CAP/L2CAP_Client/main/CMakeLists.txt b/examples/L2CAP/L2CAP_Client/main/CMakeLists.txt new file mode 100644 index 0000000..9be9075 --- /dev/null +++ b/examples/L2CAP/L2CAP_Client/main/CMakeLists.txt @@ -0,0 +1,4 @@ +set(COMPONENT_SRCS "main.cpp") +set(COMPONENT_ADD_INCLUDEDIRS ".") + +register_component() \ No newline at end of file diff --git a/examples/L2CAP/L2CAP_Client/main/component.mk b/examples/L2CAP/L2CAP_Client/main/component.mk new file mode 100644 index 0000000..a98f634 --- /dev/null +++ b/examples/L2CAP/L2CAP_Client/main/component.mk @@ -0,0 +1,4 @@ +# +# "main" pseudo-component makefile. +# +# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.) diff --git a/examples/L2CAP/L2CAP_Client/main/idf_component.yml b/examples/L2CAP/L2CAP_Client/main/idf_component.yml new file mode 100644 index 0000000..66f47ca --- /dev/null +++ b/examples/L2CAP/L2CAP_Client/main/idf_component.yml @@ -0,0 +1,3 @@ +dependencies: + local/esp-nimble-cpp: + path: ../../../../../esp-nimble-cpp/ diff --git a/examples/L2CAP/L2CAP_Client/main/main.cpp b/examples/L2CAP/L2CAP_Client/main/main.cpp new file mode 100644 index 0000000..e4f2191 --- /dev/null +++ b/examples/L2CAP/L2CAP_Client/main/main.cpp @@ -0,0 +1,165 @@ +#include + +// See the following for generating UUIDs: +// https://www.uuidgenerator.net/ + +// The remote service we wish to connect to. +static BLEUUID serviceUUID("dcbc7255-1e9e-49a0-a360-b0430b6c6905"); +// The characteristic of the remote service we are interested in. +static BLEUUID charUUID("371a55c8-f251-4ad2-90b3-c7c195b049be"); + +#define L2CAP_CHANNEL 150 +#define L2CAP_MTU 5000 + +const BLEAdvertisedDevice* theDevice = NULL; +BLEClient* theClient = NULL; +BLEL2CAPChannel* theChannel = NULL; + +size_t bytesSent = 0; +size_t bytesReceived = 0; + +class L2CAPChannelCallbacks: public BLEL2CAPChannelCallbacks { + +public: + void onConnect(NimBLEL2CAPChannel* channel) { + printf("L2CAP connection established\n"); + } + + void onMTUChange(NimBLEL2CAPChannel* channel, uint16_t mtu) { + printf("L2CAP MTU changed to %d\n", mtu); + } + + void onRead(NimBLEL2CAPChannel* channel, std::vector& data) { + printf("L2CAP read %d bytes\n", data.size()); + } + void onDisconnect(NimBLEL2CAPChannel* channel) { + printf("L2CAP disconnected\n"); + } +}; + +class MyClientCallbacks: public BLEClientCallbacks { + + void onConnect(BLEClient* pClient) { + printf("GAP connected\n"); + pClient->setDataLen(251); + + theChannel = BLEL2CAPChannel::connect(pClient, L2CAP_CHANNEL, L2CAP_MTU, new L2CAPChannelCallbacks()); + } + + void onDisconnect(BLEClient* pClient, int reason) { + printf("GAP disconnected (reason: %d)\n", reason); + theDevice = NULL; + theChannel = NULL; + vTaskDelay(1000 / portTICK_PERIOD_MS); + BLEDevice::getScan()->start(5 * 1000, true); + } +}; + +class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks { + + void onResult(const BLEAdvertisedDevice* advertisedDevice) { + if (theDevice) { return; } + printf("BLE Advertised Device found: %s\n", advertisedDevice->toString().c_str()); + + if (!advertisedDevice->haveServiceUUID()) { return; } + if (!advertisedDevice->isAdvertisingService(serviceUUID)) { return; } + + printf("Found the device we're interested in!\n"); + BLEDevice::getScan()->stop(); + + // Hand over the device to the other task + theDevice = advertisedDevice; + } +}; + +void connectTask(void *pvParameters) { + + uint8_t sequenceNumber = 0; + + while (true) { + + if (!theDevice) { + vTaskDelay(1000 / portTICK_PERIOD_MS); + continue; + } + + if (!theClient) { + theClient = BLEDevice::createClient(); + theClient->setConnectionParams(6, 6, 0, 42); + + auto callbacks = new MyClientCallbacks(); + theClient->setClientCallbacks(callbacks); + + auto success = theClient->connect(theDevice); + if (!success) { + printf("Error: Could not connect to device\n"); + break; + } + vTaskDelay(2000 / portTICK_PERIOD_MS); + continue; + } + + if (!theChannel) { + printf("l2cap channel not initialized\n"); + vTaskDelay(2000 / portTICK_PERIOD_MS); + continue; + } + + if (!theChannel->isConnected()) { + printf("l2cap channel not connected\n"); + vTaskDelay(2000 / portTICK_PERIOD_MS); + continue; + } + + while (theChannel->isConnected()) { + + /* + static auto initialDelay = true; + if (initialDelay) { + printf("Waiting gracefully 3 seconds before sending data\n"); + vTaskDelay(3000 / portTICK_PERIOD_MS); + initialDelay = false; + }; +*/ + std::vector data(5000, sequenceNumber++); + if (theChannel->write(data)) { + bytesSent += data.size(); + } else { + printf("failed to send!\n"); + abort(); + } + } + + vTaskDelay(1000 / portTICK_PERIOD_MS); + } +} + +extern "C" +void app_main(void) { + printf("Starting L2CAP client example\n"); + + xTaskCreate(connectTask, "connectTask", 5000, NULL, 1, NULL); + + BLEDevice::init("L2CAP-Client"); + BLEDevice::setMTU(BLE_ATT_MTU_MAX); + + auto scan = BLEDevice::getScan(); + auto callbacks = new MyAdvertisedDeviceCallbacks(); + scan->setScanCallbacks(callbacks); + scan->setInterval(1349); + scan->setWindow(449); + scan->setActiveScan(true); + scan->start(25 * 1000, false); + + int numberOfSeconds = 0; + + while (bytesSent == 0) { + vTaskDelay(10 / portTICK_PERIOD_MS); + } + + while (true) { + vTaskDelay(1000 / portTICK_PERIOD_MS); + int bytesSentPerSeconds = bytesSent / ++numberOfSeconds; + printf("Bandwidth: %d b/sec = %d KB/sec\n", bytesSentPerSeconds, bytesSentPerSeconds / 1024); + } +} diff --git a/examples/L2CAP/L2CAP_Client/sdkconfig.defaults b/examples/L2CAP/L2CAP_Client/sdkconfig.defaults new file mode 100644 index 0000000..5d239dc --- /dev/null +++ b/examples/L2CAP/L2CAP_Client/sdkconfig.defaults @@ -0,0 +1,13 @@ +# Override some defaults so BT stack is enabled +# in this example + +# +# BT config +# +CONFIG_BT_ENABLED=y +CONFIG_BTDM_CTRL_MODE_BLE_ONLY=y +CONFIG_BTDM_CTRL_MODE_BR_EDR_ONLY=n +CONFIG_BTDM_CTRL_MODE_BTDM=n +CONFIG_BT_BLUEDROID_ENABLED=n +CONFIG_BT_NIMBLE_ENABLED=y +CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM=1 diff --git a/examples/L2CAP/L2CAP_Server/CMakeLists.txt b/examples/L2CAP/L2CAP_Server/CMakeLists.txt new file mode 100644 index 0000000..ba68ecc --- /dev/null +++ b/examples/L2CAP/L2CAP_Server/CMakeLists.txt @@ -0,0 +1,7 @@ +# The following lines of boilerplate have to be in your project's +# CMakeLists in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.5) + +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +set(SUPPORTED_TARGETS esp32 esp32s3 esp32c3 esp32c6) +project(L2CAP_server) diff --git a/examples/L2CAP/L2CAP_Server/Makefile b/examples/L2CAP/L2CAP_Server/Makefile new file mode 100644 index 0000000..f889e7e --- /dev/null +++ b/examples/L2CAP/L2CAP_Server/Makefile @@ -0,0 +1,3 @@ +PROJECT_NAME := L2CAP_server + +include $(IDF_PATH)/make/project.mk diff --git a/examples/L2CAP/L2CAP_Server/main/CMakeLists.txt b/examples/L2CAP/L2CAP_Server/main/CMakeLists.txt new file mode 100644 index 0000000..9be9075 --- /dev/null +++ b/examples/L2CAP/L2CAP_Server/main/CMakeLists.txt @@ -0,0 +1,4 @@ +set(COMPONENT_SRCS "main.cpp") +set(COMPONENT_ADD_INCLUDEDIRS ".") + +register_component() \ No newline at end of file diff --git a/examples/L2CAP/L2CAP_Server/main/component.mk b/examples/L2CAP/L2CAP_Server/main/component.mk new file mode 100644 index 0000000..a98f634 --- /dev/null +++ b/examples/L2CAP/L2CAP_Server/main/component.mk @@ -0,0 +1,4 @@ +# +# "main" pseudo-component makefile. +# +# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.) diff --git a/examples/L2CAP/L2CAP_Server/main/idf_component.yml b/examples/L2CAP/L2CAP_Server/main/idf_component.yml new file mode 100644 index 0000000..66f47ca --- /dev/null +++ b/examples/L2CAP/L2CAP_Server/main/idf_component.yml @@ -0,0 +1,3 @@ +dependencies: + local/esp-nimble-cpp: + path: ../../../../../esp-nimble-cpp/ diff --git a/examples/L2CAP/L2CAP_Server/main/main.cpp b/examples/L2CAP/L2CAP_Server/main/main.cpp new file mode 100644 index 0000000..4763907 --- /dev/null +++ b/examples/L2CAP/L2CAP_Server/main/main.cpp @@ -0,0 +1,90 @@ +#include + +// See the following for generating UUIDs: +// https://www.uuidgenerator.net/ + +#define SERVICE_UUID "dcbc7255-1e9e-49a0-a360-b0430b6c6905" +#define CHARACTERISTIC_UUID "371a55c8-f251-4ad2-90b3-c7c195b049be" +#define L2CAP_CHANNEL 150 +#define L2CAP_MTU 5000 + +class GATTCallbacks: public BLEServerCallbacks { + +public: + void onConnect(BLEServer* pServer, BLEConnInfo& info) { + /// Booster #1 + pServer->setDataLen(info.getConnHandle(), 251); + /// Booster #2 (especially for Apple devices) + BLEDevice::getServer()->updateConnParams(info.getConnHandle(), 12, 12, 0, 200); + } +}; + +class L2CAPChannelCallbacks: public BLEL2CAPChannelCallbacks { + +public: + bool connected = false; + size_t numberOfReceivedBytes; + uint8_t nextSequenceNumber; + +public: + void onConnect(NimBLEL2CAPChannel* channel) { + printf("L2CAP connection established\n"); + connected = true; + numberOfReceivedBytes = nextSequenceNumber = 0; + } + + void onRead(NimBLEL2CAPChannel* channel, std::vector& data) { + numberOfReceivedBytes += data.size(); + size_t sequenceNumber = data[0]; + printf("L2CAP read %d bytes w/ sequence number %d", data.size(), sequenceNumber); + if (sequenceNumber != nextSequenceNumber) { + printf("(wrong sequence number %d, expected %d)\n", sequenceNumber, nextSequenceNumber); + } else { + printf("\n"); + nextSequenceNumber++; + } + } + void onDisconnect(NimBLEL2CAPChannel* channel) { + printf("L2CAP disconnected\n"); + connected = false; + } +}; + +extern "C" +void app_main(void) { + printf("Starting L2CAP server example [%lu free] [%lu min]\n", esp_get_free_heap_size(), esp_get_minimum_free_heap_size()); + + BLEDevice::init("L2CAP-Server"); + BLEDevice::setMTU(BLE_ATT_MTU_MAX); + + auto cocServer = BLEDevice::createL2CAPServer(); + auto l2capChannelCallbacks = new L2CAPChannelCallbacks(); + auto channel = cocServer->createService(L2CAP_CHANNEL, L2CAP_MTU, l2capChannelCallbacks); + + auto server = BLEDevice::createServer(); + server->setCallbacks(new GATTCallbacks()); + auto service = server->createService(SERVICE_UUID); + auto characteristic = service->createCharacteristic(CHARACTERISTIC_UUID, NIMBLE_PROPERTY::READ); + characteristic->setValue(L2CAP_CHANNEL); + service->start(); + auto advertising = BLEDevice::getAdvertising(); + advertising->addServiceUUID(SERVICE_UUID); + advertising->enableScanResponse(true); + + BLEDevice::startAdvertising(); + printf("Server waiting for connection requests [%lu free] [%lu min]\n", esp_get_free_heap_size(), esp_get_minimum_free_heap_size()); + + // Wait until transfer actually starts... + while (!l2capChannelCallbacks->numberOfReceivedBytes) { + vTaskDelay(10 / portTICK_PERIOD_MS); + } + printf("\n\n\n"); + int numberOfSeconds = 0; + + while (true) { + vTaskDelay(1000 / portTICK_PERIOD_MS); + if (!l2capChannelCallbacks->connected) { continue; } + int bps = l2capChannelCallbacks->numberOfReceivedBytes / ++numberOfSeconds; + printf("Bandwidth: %d b/sec = %d KB/sec [%lu free] [%lu min]\n", bps, bps / 1024, esp_get_free_heap_size(), esp_get_minimum_free_heap_size()); + } +} diff --git a/examples/L2CAP/L2CAP_Server/sdkconfig.defaults b/examples/L2CAP/L2CAP_Server/sdkconfig.defaults new file mode 100644 index 0000000..5d239dc --- /dev/null +++ b/examples/L2CAP/L2CAP_Server/sdkconfig.defaults @@ -0,0 +1,13 @@ +# Override some defaults so BT stack is enabled +# in this example + +# +# BT config +# +CONFIG_BT_ENABLED=y +CONFIG_BTDM_CTRL_MODE_BLE_ONLY=y +CONFIG_BTDM_CTRL_MODE_BR_EDR_ONLY=n +CONFIG_BTDM_CTRL_MODE_BTDM=n +CONFIG_BT_BLUEDROID_ENABLED=n +CONFIG_BT_NIMBLE_ENABLED=y +CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM=1 diff --git a/src/NimBLEDevice.cpp b/src/NimBLEDevice.cpp index 4eb264d..826e660 100644 --- a/src/NimBLEDevice.cpp +++ b/src/NimBLEDevice.cpp @@ -65,6 +65,9 @@ # if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL) # include "NimBLEServer.h" +# if CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM > 0 +# include "NimBLEL2CAPServer.h" +# endif # endif # include "NimBLELog.h" @@ -85,6 +88,9 @@ NimBLEScan* NimBLEDevice::m_pScan = nullptr; # if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL) NimBLEServer* NimBLEDevice::m_pServer = nullptr; +# if CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM > 0 +NimBLEL2CAPServer* NimBLEDevice::m_pL2CAPServer = nullptr; +# endif # endif # if defined(CONFIG_BT_NIMBLE_ROLE_BROADCASTER) @@ -140,6 +146,27 @@ NimBLEServer* NimBLEDevice::createServer() { NimBLEServer* NimBLEDevice::getServer() { return m_pServer; } // getServer + +# if CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM > 0 +/** + * @brief Create an instance of a L2CAP server. + * @return A pointer to the instance of the L2CAP server. + */ +NimBLEL2CAPServer* NimBLEDevice::createL2CAPServer() { + if (NimBLEDevice::m_pL2CAPServer == nullptr) { + NimBLEDevice::m_pL2CAPServer = new NimBLEL2CAPServer(); + } + return m_pL2CAPServer; +} // createL2CAPServer + +/** + * @brief Get the instance of the L2CAP server. + * @return A pointer to the L2CAP server instance or nullptr if none have been created. + */ +NimBLEL2CAPServer* NimBLEDevice::getL2CAPServer() { + return m_pL2CAPServer; +} // getL2CAPServer +# endif # endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL) /* -------------------------------------------------------------------------- */ @@ -963,6 +990,12 @@ bool NimBLEDevice::deinit(bool clearAll) { delete NimBLEDevice::m_pServer; NimBLEDevice::m_pServer = nullptr; } +# if CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM > 0 + if (NimBLEDevice::m_pL2CAPServer != nullptr) { + delete NimBLEDevice::m_pL2CAPServer; + NimBLEDevice::m_pL2CAPServer = nullptr; + } +# endif # endif # if defined(CONFIG_BT_NIMBLE_ROLE_BROADCASTER) diff --git a/src/NimBLEDevice.h b/src/NimBLEDevice.h index b76388c..a0f2f16 100644 --- a/src/NimBLEDevice.h +++ b/src/NimBLEDevice.h @@ -59,6 +59,9 @@ class NimBLEAdvertising; # if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL) class NimBLEServer; +# if CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM > 0 +class NimBLEL2CAPServer; +# endif # endif # if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL) || defined(CONFIG_BT_NIMBLE_ROLE_CENTRAL) @@ -95,6 +98,13 @@ class NimBLEDeviceCallbacks; # define BLEEddystoneTLM NimBLEEddystoneTLM # define BLEEddystoneURL NimBLEEddystoneURL # define BLEConnInfo NimBLEConnInfo +# define BLEL2CAPServer NimBLEL2CAPServer +# define BLEL2CAPService NimBLEL2CAPService +# define BLEL2CAPServiceCallbacks NimBLEL2CAPServiceCallbacks +# define BLEL2CAPClient NimBLEL2CAPClient +# define BLEL2CAPClientCallbacks NimBLEL2CAPClientCallbacks +# define BLEL2CAPChannel NimBLEL2CAPChannel +# define BLEL2CAPChannelCallbacks NimBLEL2CAPChannelCallbacks # ifdef CONFIG_BT_NIMBLE_MAX_CONNECTIONS # define NIMBLE_MAX_CONNECTIONS CONFIG_BT_NIMBLE_MAX_CONNECTIONS @@ -160,6 +170,10 @@ class NimBLEDevice { # if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL) static NimBLEServer* createServer(); static NimBLEServer* getServer(); +# if CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM > 0 + static NimBLEL2CAPServer* createL2CAPServer(); + static NimBLEL2CAPServer* getL2CAPServer(); +# endif # endif # if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL) || defined(CONFIG_BT_NIMBLE_ROLE_CENTRAL) @@ -216,6 +230,9 @@ class NimBLEDevice { # if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL) static NimBLEServer* m_pServer; +# if CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM > 0 + static NimBLEL2CAPServer* m_pL2CAPServer; +# endif # endif # if defined(CONFIG_BT_NIMBLE_ROLE_BROADCASTER) @@ -275,6 +292,10 @@ class NimBLEDevice { # include "NimBLEService.h" # include "NimBLECharacteristic.h" # include "NimBLEDescriptor.h" +# if CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM > 0 +# include "NimBLEL2CAPServer.h" +# include "NimBLEL2CAPChannel.h" +# endif # endif # if defined(CONFIG_BT_NIMBLE_ROLE_BROADCASTER) diff --git a/src/NimBLEL2CAPChannel.cpp b/src/NimBLEL2CAPChannel.cpp new file mode 100644 index 0000000..6f9a488 --- /dev/null +++ b/src/NimBLEL2CAPChannel.cpp @@ -0,0 +1,297 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#include "NimBLEL2CAPChannel.h" + +#include "NimBLEClient.h" +#include "NimBLELog.h" +#include "NimBLEUtils.h" + +#include "nimble/nimble_port.h" + +// L2CAP buffer block size +#define L2CAP_BUF_BLOCK_SIZE (250) +#define L2CAP_BUF_SIZE_MTUS_PER_CHANNEL (3) +// Round-up integer division +#define CEIL_DIVIDE(a, b) (((a) + (b) - 1) / (b)) +#define ROUND_DIVIDE(a, b) (((a) + (b) / 2) / (b)) +// Retry +constexpr TickType_t RetryTimeout = pdMS_TO_TICKS(50); +constexpr int RetryCounter = 3; + +NimBLEL2CAPChannel::NimBLEL2CAPChannel(uint16_t psm, uint16_t mtu, NimBLEL2CAPChannelCallbacks* callbacks) + :psm(psm), mtu(mtu), callbacks(callbacks) { + + assert(mtu); // fail here, if MTU is too little + assert(callbacks); // fail here, if no callbacks are given + assert(setupMemPool()); // fail here, if the memory pool could not be setup + + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X initialized w/ L2CAP MTU %i", this->psm, this->mtu); +}; + +NimBLEL2CAPChannel::~NimBLEL2CAPChannel() { + + teardownMemPool(); + + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X shutdown and freed.", this->psm); +} + +bool NimBLEL2CAPChannel::setupMemPool() { + + const size_t buf_blocks = CEIL_DIVIDE(mtu, L2CAP_BUF_BLOCK_SIZE) * L2CAP_BUF_SIZE_MTUS_PER_CHANNEL; + NIMBLE_LOGD(LOG_TAG, "Computed number of buf_blocks = %d", buf_blocks); + + _coc_memory = malloc(OS_MEMPOOL_SIZE(buf_blocks, L2CAP_BUF_BLOCK_SIZE) * sizeof(os_membuf_t)); + if (_coc_memory == 0) { + NIMBLE_LOGE(LOG_TAG, "Can't allocate _coc_memory: %d", errno); + return false; + } + + auto rc = os_mempool_init(&_coc_mempool, buf_blocks, L2CAP_BUF_BLOCK_SIZE, _coc_memory, "appbuf"); + if (rc != 0) { + NIMBLE_LOGE(LOG_TAG, "Can't os_mempool_init: %d", rc); + return false; + } + + auto rc2 = os_mbuf_pool_init(&_coc_mbuf_pool, &_coc_mempool, L2CAP_BUF_BLOCK_SIZE, buf_blocks); + if (rc2 != 0) { + NIMBLE_LOGE(LOG_TAG, "Can't os_mbuf_pool_init: %d", rc); + return false; + } + + this->receiveBuffer = (uint8_t*) malloc(mtu); + if (!this->receiveBuffer) { + NIMBLE_LOGE(LOG_TAG, "Can't malloc receive buffer: %d, %s", errno, strerror(errno)); + return false; + } + + this->stalledSemaphore = xSemaphoreCreateBinary(); + + return true; +} + +void NimBLEL2CAPChannel::teardownMemPool() { + + if (this->callbacks) { delete this->callbacks; } + if (this->receiveBuffer) { free(this->receiveBuffer); } + if (_coc_memory) { free(_coc_memory); } +} + +int NimBLEL2CAPChannel::writeFragment(std::vector::const_iterator begin, std::vector::const_iterator end) { + + auto toSend = end - begin; + + if (stalled) { + NIMBLE_LOGD(LOG_TAG, "L2CAP Channel waiting for unstall..."); + xSemaphoreTake(this->stalledSemaphore, portMAX_DELAY); + stalled = false; + NIMBLE_LOGD(LOG_TAG, "L2CAP Channel unstalled!"); + } + + struct ble_l2cap_chan_info info; + ble_l2cap_get_chan_info(channel, &info); + // Take the minimum of our and peer MTU + auto mtu = info.peer_coc_mtu < info.our_coc_mtu ? info.peer_coc_mtu : info.our_coc_mtu; + + if (toSend > mtu) { + return -BLE_HS_EBADDATA; + } + + auto retries = RetryCounter; + + while (retries--) { + + auto txd = os_mbuf_get_pkthdr(&_coc_mbuf_pool, 0); + if (!txd) { + NIMBLE_LOGE(LOG_TAG, "Can't os_mbuf_get_pkthdr."); + return -BLE_HS_ENOMEM; + } + auto append = os_mbuf_append(txd, &(*begin), toSend); + if (append != 0) { + NIMBLE_LOGE(LOG_TAG, "Can't os_mbuf_append: %d", append); + return append; + } + + auto res = ble_l2cap_send(channel, txd); + switch (res) { + case BLE_HS_ESTALLED: + stalled = true; + NIMBLE_LOGD(LOG_TAG, "L2CAP COC 0x%04X sent %d bytes.", this->psm, toSend); + NIMBLE_LOGW(LOG_TAG, "ble_l2cap_send returned BLE_HS_ESTALLED. Next send will wait for unstalled event..."); + return 0; + + case BLE_HS_ENOMEM: + case BLE_HS_EAGAIN: + case BLE_HS_EBUSY: + NIMBLE_LOGD(LOG_TAG, "ble_l2cap_send returned %d. Retrying shortly...", res); + os_mbuf_free_chain(txd); + vTaskDelay(RetryTimeout); + continue; + + case ESP_OK: + NIMBLE_LOGD(LOG_TAG, "L2CAP COC 0x%04X sent %d bytes.", this->psm, toSend); + return 0; + + default: + NIMBLE_LOGE(LOG_TAG, "ble_l2cap_send failed: %d", res); + return res; + + } + } + NIMBLE_LOGE(LOG_TAG, "Retries exhausted, dropping %d bytes to send.", toSend); + return -BLE_HS_EREJECT; +} + +#if defined(CONFIG_BT_ENABLED) && defined(CONFIG_BT_NIMBLE_ROLE_CENTRAL) +NimBLEL2CAPChannel* NimBLEL2CAPChannel::connect(NimBLEClient* client, uint16_t psm, uint16_t mtu, NimBLEL2CAPChannelCallbacks* callbacks) { + + if (!client->isConnected()) { + NIMBLE_LOGE(LOG_TAG, "Client is not connected. Before connecting via L2CAP, a GAP connection must have been established"); + return nullptr; + }; + + auto channel = new NimBLEL2CAPChannel(psm, mtu, callbacks); + + auto sdu_rx = os_mbuf_get_pkthdr(&channel->_coc_mbuf_pool, 0); + if (!sdu_rx) { + NIMBLE_LOGE(LOG_TAG, "Can't allocate SDU buffer: %d, %s", errno, strerror(errno)); + return nullptr; + } + auto rc = ble_l2cap_connect(client->getConnHandle(), psm, mtu, sdu_rx, NimBLEL2CAPChannel::handleL2capEvent, channel); + if (rc != 0) { + NIMBLE_LOGE(LOG_TAG, "ble_l2cap_connect failed: %d", rc); + } + return channel; +} +#endif // CONFIG_BT_ENABLED && CONFIG_BT_NIMBLE_ROLE_CENTRAL + +bool NimBLEL2CAPChannel::write(const std::vector& bytes) { + + if (!this->channel) { + NIMBLE_LOGW(LOG_TAG, "L2CAP Channel not open"); + return false; + } + + struct ble_l2cap_chan_info info; + ble_l2cap_get_chan_info(channel, &info); + auto mtu = info.peer_coc_mtu < info.our_coc_mtu ? info.peer_coc_mtu : info.our_coc_mtu; + + + auto start = bytes.begin(); + while (start != bytes.end()) { + auto end = start + mtu < bytes.end() ? start + mtu : bytes.end(); + if (writeFragment(start, end) < 0) { + return false; + } + start = end; + } + return true; +} + +// private +int NimBLEL2CAPChannel::handleConnectionEvent(struct ble_l2cap_event* event) { + + channel = event->connect.chan; + struct ble_l2cap_chan_info info; + ble_l2cap_get_chan_info(channel, &info); + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X connected. Local MTU = %d [%d], remote MTU = %d [%d].", psm, + info.our_coc_mtu, info.our_l2cap_mtu, info.peer_coc_mtu, info.peer_l2cap_mtu); + if (info.our_coc_mtu > info.peer_coc_mtu) { + NIMBLE_LOGW(LOG_TAG, "L2CAP COC 0x%04X connected, but local MTU is bigger than remote MTU.", psm); + } + auto mtu = info.peer_coc_mtu < info.our_coc_mtu ? info.peer_coc_mtu : info.our_coc_mtu; + callbacks->onConnect(this, mtu); + return 0; +} + +int NimBLEL2CAPChannel::handleAcceptEvent(struct ble_l2cap_event* event) { + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X accept.", psm); + if (!callbacks->shouldAcceptConnection(this)) { + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X refused by delegate.", psm); + return -1; + } + + struct os_mbuf *sdu_rx = os_mbuf_get_pkthdr(&_coc_mbuf_pool, 0); + assert(sdu_rx != NULL); + ble_l2cap_recv_ready(event->accept.chan, sdu_rx); + return 0; +} + +int NimBLEL2CAPChannel::handleDataReceivedEvent(struct ble_l2cap_event* event) { + NIMBLE_LOGD(LOG_TAG, "L2CAP COC 0x%04X data received.", psm); + + struct os_mbuf* rxd = event->receive.sdu_rx; + assert(rxd != NULL); + + int rx_len = (int)OS_MBUF_PKTLEN(rxd); + assert(rx_len <= (int)mtu); + + int res = os_mbuf_copydata(rxd, 0, rx_len, receiveBuffer); + assert(res == 0); + + NIMBLE_LOGD(LOG_TAG, "L2CAP COC 0x%04X received %d bytes.", psm, rx_len); + + res = os_mbuf_free_chain(rxd); + assert(res == 0); + + std::vector incomingData(receiveBuffer, receiveBuffer + rx_len); + callbacks->onRead(this, incomingData); + + struct os_mbuf* next = os_mbuf_get_pkthdr(&_coc_mbuf_pool, 0); + assert(next != NULL); + + res = ble_l2cap_recv_ready(channel, next); + assert(res == 0); + + return 0; +} + +int NimBLEL2CAPChannel::handleTxUnstalledEvent(struct ble_l2cap_event* event) { + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X transmit unstalled.", psm); + xSemaphoreGive(this->stalledSemaphore); + return 0; +} + +int NimBLEL2CAPChannel::handleDisconnectionEvent(struct ble_l2cap_event* event) { + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X disconnected.", psm); + channel = NULL; + callbacks->onDisconnect(this); + return 0; +} + +/* STATIC */ +int NimBLEL2CAPChannel::handleL2capEvent(struct ble_l2cap_event *event, void *arg) { + + NIMBLE_LOGD(LOG_TAG, "handleL2capEvent: handling l2cap event %d", event->type); + NimBLEL2CAPChannel* self = reinterpret_cast(arg); + + int returnValue = 0; + + switch (event->type) { + case BLE_L2CAP_EVENT_COC_CONNECTED: + returnValue = self->handleConnectionEvent(event); + break; + + case BLE_L2CAP_EVENT_COC_DISCONNECTED: + returnValue = self->handleDisconnectionEvent(event); + break; + + case BLE_L2CAP_EVENT_COC_ACCEPT: + returnValue = self->handleAcceptEvent(event); + break; + + case BLE_L2CAP_EVENT_COC_DATA_RECEIVED: + returnValue = self->handleDataReceivedEvent(event); + break; + + case BLE_L2CAP_EVENT_COC_TX_UNSTALLED: + returnValue = self->handleTxUnstalledEvent(event); + break; + + default: + NIMBLE_LOGW(LOG_TAG, "Unhandled l2cap event %d", event->type); + break; + } + + return returnValue; +} diff --git a/src/NimBLEL2CAPChannel.h b/src/NimBLEL2CAPChannel.h new file mode 100644 index 0000000..db3680b --- /dev/null +++ b/src/NimBLEL2CAPChannel.h @@ -0,0 +1,120 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#pragma once +#ifndef NIMBLEL2CAPCHANNEL_H +#define NIMBLEL2CAPCHANNEL_H + +#include "inttypes.h" +#include "host/ble_l2cap.h" +#include "os/os_mbuf.h" +#include "os/os_mempool.h" + +/**** FIX COMPILATION ****/ +# undef min +# undef max +/**************************/ + +#include +#include + +class NimBLEClient; +class NimBLEL2CAPChannelCallbacks; + +/** + * @brief Encapsulates a L2CAP channel. + * + * This class is used to encapsulate a L2CAP connection oriented channel, both + * from the "server" (which waits for the connection to be opened) and the "client" + * (which opens the connection) point of view. + */ +class NimBLEL2CAPChannel { + +public: + /// @brief Open an L2CAP channel via the specified PSM and MTU. + /// @param[in] psm The PSM to use. + /// @param[in] mtu The MTU to use. Note that this is the local MTU. Upon opening the channel, + /// the final MTU will be negotiated to be the minimum of local and remote. + /// @param[in] callbacks The callbacks to use. NOTE that these callbacks are called from the + /// context of the NimBLE bluetooth task (`nimble_host`) and MUST be handled as fast as possible. + /// @return True if the channel was opened successfully, false otherwise. + static NimBLEL2CAPChannel* connect(NimBLEClient* client, uint16_t psm, uint16_t mtu, NimBLEL2CAPChannelCallbacks* callbacks); + + /// @brief Write data to the channel. + /// + /// If the size of the data exceeds the MTU, the data will be split into multiple fragments. + /// @return true on success, after the data has been sent. + /// @return false, if the data can't be sent. + /// + /// NOTE: This function will block until the data has been sent or an error occurred. + bool write(const std::vector& bytes); + + /// @return True, if the channel is connected. False, otherwise. + bool isConnected() const { return !!channel; } + +protected: + + NimBLEL2CAPChannel(uint16_t psm, uint16_t mtu, NimBLEL2CAPChannelCallbacks* callbacks); + ~NimBLEL2CAPChannel(); + + int handleConnectionEvent(struct ble_l2cap_event* event); + int handleAcceptEvent(struct ble_l2cap_event* event); + int handleDataReceivedEvent(struct ble_l2cap_event* event); + int handleTxUnstalledEvent(struct ble_l2cap_event* event); + int handleDisconnectionEvent(struct ble_l2cap_event* event); + +private: + friend class NimBLEL2CAPServer; + static constexpr const char* LOG_TAG = "NimBLEL2CAPChannel"; + + const uint16_t psm; // PSM of the channel + const uint16_t mtu; // The requested (local) MTU of the channel, might be larger than negotiated MTU + struct ble_l2cap_chan* channel = nullptr; + NimBLEL2CAPChannelCallbacks* callbacks; + uint8_t* receiveBuffer = nullptr; // buffers a full (local) MTU + + // NimBLE memory pool + void* _coc_memory = nullptr; + struct os_mempool _coc_mempool; + struct os_mbuf_pool _coc_mbuf_pool; + + // Runtime handling + std::atomic stalled{false}; + SemaphoreHandle_t stalledSemaphore = nullptr; + + // Allocate / deallocate NimBLE memory pool + bool setupMemPool(); + void teardownMemPool(); + + // Writes data up to the size of the negotiated MTU to the channel. + int writeFragment(std::vector::const_iterator begin, std::vector::const_iterator end); + + // L2CAP event handler + static int handleL2capEvent(struct ble_l2cap_event* event, void *arg); +}; + +/** + * @brief Callbacks base class for the L2CAP channel. + */ +class NimBLEL2CAPChannelCallbacks { + +public: + NimBLEL2CAPChannelCallbacks() = default; + virtual ~NimBLEL2CAPChannelCallbacks() = default; + + /// Called when the client attempts to open a channel on the server. + /// You can choose to accept or deny the connection. + /// Default implementation returns true. + virtual bool shouldAcceptConnection(NimBLEL2CAPChannel* channel) { return true; } + /// Called after a connection has been made. + /// Default implementation does nothing. + virtual void onConnect(NimBLEL2CAPChannel* channel, uint16_t negotiatedMTU) {}; + /// Called when data has been read from the channel. + /// Default implementation does nothing. + virtual void onRead(NimBLEL2CAPChannel* channel, std::vector& data) {}; + /// Called after the channel has been disconnected. + /// Default implementation does nothing. + virtual void onDisconnect(NimBLEL2CAPChannel* channel) {}; +}; + +#endif diff --git a/src/NimBLEL2CAPServer.cpp b/src/NimBLEL2CAPServer.cpp new file mode 100644 index 0000000..1d4d0f3 --- /dev/null +++ b/src/NimBLEL2CAPServer.cpp @@ -0,0 +1,36 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#include "NimBLEL2CAPServer.h" +#include "NimBLEL2CAPChannel.h" +#include "NimBLEDevice.h" +#include "NimBLELog.h" + +static const char* LOG_TAG = "NimBLEL2CAPServer"; + +NimBLEL2CAPServer::NimBLEL2CAPServer() { + + // Nothing to do here... +} + +NimBLEL2CAPServer::~NimBLEL2CAPServer() { + + // Delete all services + for (auto service: this->services) { + delete service; + } +} + +NimBLEL2CAPChannel* NimBLEL2CAPServer::createService(const uint16_t psm, const uint16_t mtu, NimBLEL2CAPChannelCallbacks* callbacks) { + + auto service = new NimBLEL2CAPChannel(psm, mtu, callbacks); + auto rc = ble_l2cap_create_server(psm, mtu, NimBLEL2CAPChannel::handleL2capEvent, service); + + if (rc != 0) { + NIMBLE_LOGE(LOG_TAG, "Could not ble_l2cap_create_server: %d", rc); + return nullptr; + } + + this->services.push_back(service); + return service; +} diff --git a/src/NimBLEL2CAPServer.h b/src/NimBLEL2CAPServer.h new file mode 100644 index 0000000..f07f950 --- /dev/null +++ b/src/NimBLEL2CAPServer.h @@ -0,0 +1,39 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#ifndef NIMBLEL2CAPSERVER_H +#define NIMBLEL2CAPSERVER_H +#pragma once + +#include "inttypes.h" +#include + +class NimBLEL2CAPChannel; +class NimBLEL2CAPChannelCallbacks; + + +/** + * @brief L2CAP server class. + * + * Encapsulates a L2CAP server that can hold multiple services. Every service is represented by a channel object + * and an assorted set of callbacks. + */ +class NimBLEL2CAPServer { +public: + /// @brief Register a new L2CAP service instance. + /// @param psm The port multiplexor service number. + /// @param mtu The maximum transmission unit. + /// @param callbacks The callbacks for this service. + /// @return the newly created object, if the server registration was successful. + NimBLEL2CAPChannel* createService(const uint16_t psm, const uint16_t mtu, NimBLEL2CAPChannelCallbacks* callbacks); + +private: + NimBLEL2CAPServer(); + ~NimBLEL2CAPServer(); + std::vector services; + + friend class NimBLEL2CAPChannel; + friend class NimBLEDevice; +}; + +#endif \ No newline at end of file