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.

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