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

@@ -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"

examples/L2CAP/.gitignore

@@ -0,0 +1,5 @@
+.vscode
+build
+sdkconfig
+sdkconfig.old
+dependencies.lock

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)

examples/L2CAP/L2CAP_Client/Makefile

@@ -0,0 +1,3 @@
+PROJECT_NAME := L2CAP_client
+
+include $(IDF_PATH)/make/project.mk

examples/L2CAP/L2CAP_Client/main/CMakeLists.txt

@@ -0,0 +1,4 @@
+set(COMPONENT_SRCS "main.cpp")
+set(COMPONENT_ADD_INCLUDEDIRS ".")
+
+register_component()

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.)

examples/L2CAP/L2CAP_Client/main/idf_component.yml

@@ -0,0 +1,3 @@
+dependencies:
+  local/esp-nimble-cpp:
+    path: ../../../../../esp-nimble-cpp/

examples/L2CAP/L2CAP_Client/main/main.cpp

@@ -0,0 +1,165 @@
+#include <NimBLEDevice.h>
+
+// 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<uint8_t>& 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<uint8_t> 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);
+    }
+}

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

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)

examples/L2CAP/L2CAP_Server/Makefile

@@ -0,0 +1,3 @@
+PROJECT_NAME := L2CAP_server
+
+include $(IDF_PATH)/make/project.mk

examples/L2CAP/L2CAP_Server/main/CMakeLists.txt

@@ -0,0 +1,4 @@
+set(COMPONENT_SRCS "main.cpp")
+set(COMPONENT_ADD_INCLUDEDIRS ".")
+
+register_component()

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.)

examples/L2CAP/L2CAP_Server/main/idf_component.yml

@@ -0,0 +1,3 @@
+dependencies:
+  local/esp-nimble-cpp:
+    path: ../../../../../esp-nimble-cpp/

examples/L2CAP/L2CAP_Server/main/main.cpp

@@ -0,0 +1,90 @@
+#include <NimBLEDevice.h>
+
+// 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<uint8_t>& 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());
+    }
+}

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

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)

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)

src/NimBLEL2CAPChannel.cpp

@@ -0,0 +1,297 @@
+//
+// (C) Dr. Michael 'Mickey' Lauer <mickey@vanille-media.de>
+//
+#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<uint8_t>::const_iterator begin, std::vector<uint8_t>::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<uint8_t>& 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<uint8_t> 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<NimBLEL2CAPChannel*>(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;
+}

src/NimBLEL2CAPChannel.h

@@ -0,0 +1,120 @@
+//
+// (C) Dr. Michael 'Mickey' Lauer <mickey@vanille-media.de>
+//
+#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 <vector>
+#include <atomic>
+
+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<uint8_t>& 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<bool> 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<uint8_t>::const_iterator begin, std::vector<uint8_t>::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<uint8_t>& data) {};
+    /// Called after the channel has been disconnected.
+    /// Default implementation does nothing.
+    virtual void onDisconnect(NimBLEL2CAPChannel* channel) {};
+};
+
+#endif

src/NimBLEL2CAPServer.cpp

@@ -0,0 +1,36 @@
+//
+// (C) Dr. Michael 'Mickey' Lauer <mickey@vanille-media.de>
+//
+#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;
+}

src/NimBLEL2CAPServer.h

@@ -0,0 +1,39 @@
+//
+// (C) Dr. Michael 'Mickey' Lauer <mickey@vanille-media.de>
+//
+#ifndef NIMBLEL2CAPSERVER_H
+#define NIMBLEL2CAPSERVER_H
+#pragma once
+
+#include "inttypes.h"
+#include <vector>
+
+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<NimBLEL2CAPChannel*> services;
+
+    friend class NimBLEL2CAPChannel;
+    friend class NimBLEDevice;
+};
+
+#endif