diff --git a/examples/L2CAP/L2CAP_Client/L2CAP_Client.ino b/examples/L2CAP/L2CAP_Client/L2CAP_Client.ino new file mode 100644 index 00000000..6d923c27 --- /dev/null +++ b/examples/L2CAP/L2CAP_Client/L2CAP_Client.ino @@ -0,0 +1,143 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// + +#include +#include + +#if CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM <= 0 +# error "CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM must be set to 1 or greater" +#endif + +// See the following for generating UUIDs: +// https://www.uuidgenerator.net/ + +// The remote service we wish to connect to. +static NimBLEUUID serviceUUID("dcbc7255-1e9e-49a0-a360-b0430b6c6905"); +// The characteristic of the remote service we are interested in. +static NimBLEUUID charUUID("371a55c8-f251-4ad2-90b3-c7c195b049be"); + +#define L2CAP_CHANNEL 150 +#define L2CAP_MTU 5000 + +const NimBLEAdvertisedDevice* theDevice = NULL; +NimBLEClient* theClient = NULL; +NimBLEL2CAPChannel* theChannel = NULL; + +size_t bytesSent = 0; +size_t bytesReceived = 0; +size_t numberOfSeconds = 0; + +class L2CAPChannelCallbacks : public NimBLEL2CAPChannelCallbacks { + public: + void onConnect(NimBLEL2CAPChannel* channel) { Serial.println("L2CAP connection established"); } + + void onMTUChange(NimBLEL2CAPChannel* channel, uint16_t mtu) { Serial.printf("L2CAP MTU changed to %d\n", mtu); } + + void onRead(NimBLEL2CAPChannel* channel, std::vector& data) { + Serial.printf("L2CAP read %d bytes\n", data.size()); + } + void onDisconnect(NimBLEL2CAPChannel* channel) { Serial.println("L2CAP disconnected"); } +} L2Callbacks; + +class ClientCallbacks : public NimBLEClientCallbacks { + void onConnect(NimBLEClient* pClient) { + Serial.println("GAP connected"); + pClient->setDataLen(251); + + theChannel = NimBLEL2CAPChannel::connect(pClient, L2CAP_CHANNEL, L2CAP_MTU, &L2Callbacks); + } + + void onDisconnect(NimBLEClient* pClient, int reason) { + printf("GAP disconnected (reason: %d)\n", reason); + theDevice = NULL; + theChannel = NULL; + NimBLEDevice::getScan()->start(5 * 1000, true); + } +} clientCallbacks; + +class ScanCallbacks : public NimBLEScanCallbacks { + void onResult(const NimBLEAdvertisedDevice* advertisedDevice) { + if (theDevice) { + return; + } + Serial.printf("BLE Advertised Device found: %s\n", advertisedDevice->toString().c_str()); + + if (!advertisedDevice->haveServiceUUID()) { + return; + } + if (!advertisedDevice->isAdvertisingService(serviceUUID)) { + return; + } + + Serial.println("Found the device we're interested in!"); + NimBLEDevice::getScan()->stop(); + + // Hand over the device to the other task + theDevice = advertisedDevice; + } +} scanCallbacks; + +void setup() { + Serial.begin(115200); + Serial.println("Starting L2CAP client example"); + + NimBLEDevice::init("L2CAP-Client"); + NimBLEDevice::setMTU(BLE_ATT_MTU_MAX); + + auto scan = NimBLEDevice::getScan(); + scan->setScanCallbacks(&scanCallbacks); + scan->setInterval(1349); + scan->setWindow(449); + scan->setActiveScan(true); + scan->start(25 * 1000, false); +} + +void loop() { + static uint8_t sequenceNumber = 0; + static unsigned long firstBytesTime = 0; + auto now = millis(); + + if (!theDevice) { + delay(1000); + return; + } + + if (!theClient) { + theClient = NimBLEDevice::createClient(); + theClient->setConnectionParams(6, 6, 0, 42); + theClient->setClientCallbacks(&clientCallbacks); + if (!theClient->connect(theDevice)) { + Serial.println("Error: Could not connect to device"); + return; + } + delay(2000); + } + + if (!theChannel) { + Serial.println("l2cap channel not initialized"); + delay(2000); + return; + } + + if (!theChannel->isConnected()) { + Serial.println("l2cap channel not connected\n"); + delay(2000); + return; + } + + std::vector data(5000, sequenceNumber++); + if (theChannel->write(data)) { + if (bytesSent == 0) { + firstBytesTime = now; + } + bytesSent += data.size(); + if (now - firstBytesTime > 1000) { + int bytesSentPerSeconds = bytesSent / ((now - firstBytesTime) / 1000); + printf("Bandwidth: %d b/sec = %d KB/sec\n", bytesSentPerSeconds, bytesSentPerSeconds / 1024); + } + } else { + Serial.println("failed to send!"); + abort(); + } +} diff --git a/examples/L2CAP/L2CAP_Server/L2CAP_Server.ino b/examples/L2CAP/L2CAP_Server/L2CAP_Server.ino new file mode 100644 index 00000000..9a20cddc --- /dev/null +++ b/examples/L2CAP/L2CAP_Server/L2CAP_Server.ino @@ -0,0 +1,97 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// + +#include +#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 NimBLEServerCallbacks { + public: + void onConnect(NimBLEServer* pServer, NimBLEConnInfo& info) { + /// Booster #1 + pServer->setDataLen(info.getConnHandle(), 251); + /// Booster #2 (especially for Apple devices) + NimBLEDevice::getServer()->updateConnParams(info.getConnHandle(), 12, 12, 0, 200); + } +} gattCallbacks; + +class L2CAPChannelCallbacks : public NimBLEL2CAPChannelCallbacks { + public: + bool connected = false; + size_t numberOfReceivedBytes; + uint8_t nextSequenceNumber; + int numberOfSeconds; + + public: + void onConnect(NimBLEL2CAPChannel* channel) { + Serial.println("L2CAP connection established"); + connected = true; + numberOfSeconds = numberOfReceivedBytes = nextSequenceNumber = 0; + } + + void onRead(NimBLEL2CAPChannel* channel, std::vector& data) { + numberOfReceivedBytes += data.size(); + size_t sequenceNumber = data[0]; + Serial.printf("L2CAP read %d bytes w/ sequence number %d", data.size(), sequenceNumber); + if (sequenceNumber != nextSequenceNumber) { + Serial.printf("(wrong sequence number %d, expected %d)\n", sequenceNumber, nextSequenceNumber); + } else { + nextSequenceNumber++; + } + } + + void onDisconnect(NimBLEL2CAPChannel* channel) { + Serial.println("L2CAP disconnected"); + connected = false; + } +} l2capCallbacks; + +void setup() { + Serial.begin(115200); + Serial.println("Starting L2CAP server example"); + + NimBLEDevice::init("L2CAP-Server"); + NimBLEDevice::setMTU(BLE_ATT_MTU_MAX); + + auto cocServer = NimBLEDevice::createL2CAPServer(); + auto channel = cocServer->createService(L2CAP_CHANNEL, L2CAP_MTU, &l2capCallbacks); + + auto server = NimBLEDevice::createServer(); + server->setCallbacks(&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); + + NimBLEDevice::startAdvertising(); + Serial.println("Server waiting for connection requests"); +} + +void loop() { + // Wait until transfer actually starts... + if (!l2capCallbacks.numberOfReceivedBytes) { + delay(10); + return; + } + + delay(1000); + if (!l2capCallbacks.connected) { + return; + } + + int bps = l2capCallbacks.numberOfReceivedBytes / ++l2capCallbacks.numberOfSeconds; + Serial.printf("Bandwidth: %d b/sec = %d KB/sec\n", bps, bps / 1024); +} diff --git a/src/NimBLEDevice.cpp b/src/NimBLEDevice.cpp index 0f58f006..17edfa5b 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) /* -------------------------------------------------------------------------- */ @@ -965,6 +992,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 b76388c1..a0f2f16e 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 00000000..87235394 --- /dev/null +++ b/src/NimBLEL2CAPChannel.cpp @@ -0,0 +1,304 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#include "NimBLEL2CAPChannel.h" + +#include "NimBLEClient.h" +#include "NimBLELog.h" +#include "NimBLEUtils.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 uint32_t RetryTimeout = 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; + } + + 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..."); + NimBLETaskData taskData; + m_pTaskData = &taskData; + NimBLEUtils::taskWait(taskData, BLE_NPL_TIME_FOREVER); + m_pTaskData = nullptr; + 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 0: + NIMBLE_LOGD(LOG_TAG, "L2CAP COC 0x%04X sent %d bytes.", this->psm, toSend); + return 0; + + 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); + ble_npl_time_delay(ble_npl_time_ms_to_ticks32(RetryTimeout)); + continue; + + 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) { + if (m_pTaskData != nullptr) { + NimBLEUtils::taskRelease(*m_pTaskData, event->tx_unstalled.status); + } + + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X transmit unstalled.", psm); + 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 00000000..636df60f --- /dev/null +++ b/src/NimBLEL2CAPChannel.h @@ -0,0 +1,124 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#pragma once +#ifndef NIMBLEL2CAPCHANNEL_H +# define NIMBLEL2CAPCHANNEL_H + +# include "nimconfig.h" + +# include "inttypes.h" +# if defined(CONFIG_NIMBLE_CPP_IDF) +# include "host/ble_l2cap.h" +# include "os/os_mbuf.h" +# else +# include "nimble/nimble/host/include/host/ble_l2cap.h" +# include "nimble/porting/nimble/include/os/os_mbuf.h" +# endif + +/**** FIX COMPILATION ****/ +# undef min +# undef max +/**************************/ + +# include +# include + +class NimBLEClient; +class NimBLEL2CAPChannelCallbacks; +struct NimBLETaskData; + +/** + * @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}; + NimBLETaskData* m_pTaskData{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 00000000..422291f0 --- /dev/null +++ b/src/NimBLEL2CAPServer.cpp @@ -0,0 +1,35 @@ +// +// (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 00000000..9231c4e9 --- /dev/null +++ b/src/NimBLEL2CAPServer.h @@ -0,0 +1,38 @@ +// +// (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 diff --git a/src/nimconfig.h b/src/nimconfig.h index 6d3ff3e7..9e53a4f8 100644 --- a/src/nimconfig.h +++ b/src/nimconfig.h @@ -255,6 +255,11 @@ #define CONFIG_NIMBLE_STACK_USE_MEM_POOLS 0 #endif +/** @brief Maximum number of connection oriented channels */ +#ifndef CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM +#define CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM 0 +#endif + /** @brief Set if CCCD's and bond data should be stored in NVS */ #define CONFIG_BT_NIMBLE_NVS_PERSIST 1 @@ -286,8 +291,6 @@ /** @brief Number of low priority HCI event buffers */ #define CONFIG_BT_NIMBLE_TRANSPORT_EVT_DISCARD_COUNT 8 -/** @brief Maximum number of connection oriented channels */ -#define CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM 0 #define CONFIG_BT_NIMBLE_L2CAP_COC_SDU_BUFF_COUNT 1 #define CONFIG_BT_NIMBLE_EATT_CHAN_NUM 0 #define CONFIG_BT_NIMBLE_SVC_GAP_CENT_ADDR_RESOLUTION -1