diff --git a/CMakeLists.txt b/CMakeLists.txt index ae10819..e23f111 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,8 @@ idf_component_register( "src/NimBLEEddystoneURL.cpp" "src/NimBLEExtAdvertising.cpp" "src/NimBLEHIDDevice.cpp" + "src/NimBLEL2CAPServer.cpp" + "src/NimBLEL2CAPService.cpp" "src/NimBLERemoteCharacteristic.cpp" "src/NimBLERemoteDescriptor.cpp" "src/NimBLERemoteService.cpp" diff --git a/src/NimBLEDevice.h b/src/NimBLEDevice.h index 60baf59..c7810aa 100644 --- a/src/NimBLEDevice.h +++ b/src/NimBLEDevice.h @@ -38,6 +38,12 @@ #include "NimBLEServer.h" #endif +#if defined(CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM) +//FIXME: Insert preprocessor comparison with > 0 here +#include "NimBLEL2CAPServer.h" +#include "NimBLEL2CAPService.h" +#endif + #include "NimBLEUtils.h" #include "NimBLEAddress.h" @@ -79,6 +85,9 @@ #define BLEEddystoneTLM NimBLEEddystoneTLM #define BLEEddystoneURL NimBLEEddystoneURL #define BLEConnInfo NimBLEConnInfo +#define BLEL2CapServer NimBLEL2CAPServer +#define BLEL2CapService NimBLEL2CAPService +#define BLEL2CapServiceCallbacks NimBLEL2CAPServiceCallbacks #ifdef CONFIG_BT_NIMBLE_MAX_CONNECTIONS #define NIMBLE_MAX_CONNECTIONS CONFIG_BT_NIMBLE_MAX_CONNECTIONS diff --git a/src/NimBLEL2CAPServer.cpp b/src/NimBLEL2CAPServer.cpp new file mode 100644 index 0000000..cd2ca27 --- /dev/null +++ b/src/NimBLEL2CAPServer.cpp @@ -0,0 +1,23 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#include "NimBLEL2CAPServer.h" +#include "NimBLEL2CAPService.h" + +static const char* LOG_TAG = "NimBLEL2CAPServer"; + +NimBLEL2CAPServer::NimBLEL2CAPServer() { + +} + +NimBLEL2CAPServer::~NimBLEL2CAPServer() { + +} + +NimBLEL2CAPService* NimBLEL2CAPServer::createService(const uint16_t psm, const uint16_t mtu, NimBLEL2CAPServiceCallbacks* callbacks) { + + auto service = new NimBLEL2CAPService(psm, mtu, callbacks); + this->m_svcVec.push_back(service); + return service; +} + diff --git a/src/NimBLEL2CAPServer.h b/src/NimBLEL2CAPServer.h new file mode 100644 index 0000000..a9a2af8 --- /dev/null +++ b/src/NimBLEL2CAPServer.h @@ -0,0 +1,27 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#ifndef NIMBLEL2CAPSERVER_H +#define NIMBLEL2CAPSERVER_H + +#include "inttypes.h" +#include + +class NimBLEL2CAPService; +class NimBLEL2CAPServiceCallbacks; + +#pragma once + +class NimBLEL2CAPServer { +public: + NimBLEL2CAPServer(); + ~NimBLEL2CAPServer(); + + NimBLEL2CAPService* createService(const uint16_t psm, const uint16_t mtu, NimBLEL2CAPServiceCallbacks* callbacks); + +private: + friend class NimBLEL2CAPService; + std::vector m_svcVec; +}; + +#endif \ No newline at end of file diff --git a/src/NimBLEL2CAPService.cpp b/src/NimBLEL2CAPService.cpp new file mode 100644 index 0000000..270fc1b --- /dev/null +++ b/src/NimBLEL2CAPService.cpp @@ -0,0 +1,180 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#include "NimBLEL2CAPService.h" +#include "NimBLEL2CAPServer.h" + +#include "NimBLELog.h" +#include "NimBLEUtils.h" + +#include "nimble/nimble_port.h" + +// Round-up integer division +#define CEIL_DIVIDE(a, b) (((a) + (b) - 1) / (b)) +#define ROUND_DIVIDE(a, b) (((a) + (b) / 2) / (b)) + +static const char* LOG_TAG = "NimBLEL2CAPService"; + +NimBLEL2CAPService::NimBLEL2CAPService(uint16_t psm, uint16_t mtu, NimBLEL2CAPServiceCallbacks* callbacks) { + + assert(callbacks != NULL); + + const size_t buf_blocks = CEIL_DIVIDE(mtu, L2CAP_BUF_BLOCK_SIZE) * L2CAP_BUF_SIZE_MTUS_PER_CHANNEL; + printf("# of buf_blocks: %d\n", buf_blocks); + + int rc = ble_l2cap_create_server(psm, mtu, NimBLEL2CAPService::handleL2capEvent, this); + if (rc != 0) { + NIMBLE_LOGE(LOG_TAG, "L2CAP Server creation error: %d, %s", rc, NimBLEUtils::returnCodeToString(rc)); + return; + } + + _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; + } + + 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, %s", rc, NimBLEUtils::returnCodeToString(rc)); + return; + } + rc = os_mbuf_pool_init(&_coc_mbuf_pool, &_coc_mempool, L2CAP_BUF_BLOCK_SIZE, buf_blocks); + if (rc != 0) { + NIMBLE_LOGE(LOG_TAG, "Can't os_mbuf_pool_init: %d, %s", rc, NimBLEUtils::returnCodeToString(rc)); + return; + } + + receiveBuffer = (uint8_t*) malloc(mtu); + if (receiveBuffer == NULL) { + NIMBLE_LOGE(LOG_TAG, "Can't malloc receive buffer: %d, %s", errno, NimBLEUtils::returnCodeToString(errno)); + } + + this->psm = psm; + this->mtu = mtu; + this->callbacks = callbacks; + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X registered w/ L2CAP MTU %i", this->psm, this->mtu); +} + +void NimBLEL2CAPService::write(std::vector& bytes) { + + struct ble_l2cap_chan_info info; + ble_l2cap_get_chan_info(channel, &info); + auto mtu = info.peer_coc_mtu; + + while (!bytes.empty()) { + auto txd = os_mbuf_get_pkthdr(&_coc_mbuf_pool, 0); + assert(txd != NULL); + + auto chunk = bytes.size() < mtu ? bytes.size() : mtu; + auto res = os_mbuf_append(txd, bytes.data(), chunk); + //auto res = os_mbuf_copyinto(txd, 0, bytes.data(), chunk); + assert(res == 0); + res = ble_l2cap_send(channel, txd); + assert(res == 0 || (res == BLE_HS_ESTALLED)); + std::vector(bytes.begin() + chunk, bytes.end()).swap(bytes); + NIMBLE_LOGD(LOG_TAG, "L2CAP COC 0x%04X sent %d bytes.", this->psm, chunk); + } +} + +NimBLEL2CAPService::~NimBLEL2CAPService() { +} + +// private +int NimBLEL2CAPService::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. Our MTU is %i, remote MTU is %i.", psm, info.our_l2cap_mtu, info.peer_l2cap_mtu); + callbacks->onConnect(this); + return 0; +} + +int NimBLEL2CAPService::handleAcceptEvent(struct ble_l2cap_event* event) { + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X accept.", psm); + 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 NimBLEL2CAPService::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 NimBLEL2CAPService::handleTxUnstalledEvent(struct ble_l2cap_event* event) { + NIMBLE_LOGI(LOG_TAG, "L2CAP COC 0x%04X transmit unstalled.", psm); + return 0; +} + +int NimBLEL2CAPService::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 NimBLEL2CAPService::handleL2capEvent(struct ble_l2cap_event *event, void *arg) { + + NIMBLE_LOGD(LOG_TAG, "handleL2capEvent: handling l2cap event %d", event->type); + NimBLEL2CAPService* 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; +} + +NimBLEL2CAPServiceCallbacks::~NimBLEL2CAPServiceCallbacks() {} diff --git a/src/NimBLEL2CAPService.h b/src/NimBLEL2CAPService.h new file mode 100644 index 0000000..6f69142 --- /dev/null +++ b/src/NimBLEL2CAPService.h @@ -0,0 +1,61 @@ +// +// (C) Dr. Michael 'Mickey' Lauer +// +#ifndef NIMBLEL2CAPSERVICE_H +#define NIMBLEL2CAPSERVICE_H + +#pragma once + +#include "inttypes.h" +#include "host/ble_l2cap.h" +#include "os/os_mbuf.h" +#include "os/os_mempool.h" +#undef max +#undef min + +#include + +#define L2CAP_BUF_BLOCK_SIZE (250) +#define L2CAP_BUF_SIZE_MTUS_PER_CHANNEL (3) + +class NimBLEL2CAPServiceCallbacks; + +class NimBLEL2CAPService { +public: + NimBLEL2CAPService(uint16_t psm, uint16_t mtu, NimBLEL2CAPServiceCallbacks* callbacks); + ~NimBLEL2CAPService(); + + void write(std::vector& bytes); + +protected: + 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: + uint16_t psm; // protocol service multiplexer + uint16_t mtu; // maximum transmission unit + struct ble_l2cap_chan* channel; // channel handle + uint8_t* receiveBuffer; // MTU buffer + + NimBLEL2CAPServiceCallbacks* callbacks; + + void* _coc_memory; + struct os_mempool _coc_mempool; + struct os_mbuf_pool _coc_mbuf_pool; + + static int handleL2capEvent(struct ble_l2cap_event *event, void *arg); +}; + +class NimBLEL2CAPServiceCallbacks { + +public: + virtual ~NimBLEL2CAPServiceCallbacks(); + virtual void onConnect(NimBLEL2CAPService* pService) = 0; + virtual void onRead(NimBLEL2CAPService* pService, std::vector& data) = 0; + virtual void onDisconnect(NimBLEL2CAPService* pService) = 0; +}; + +#endif \ No newline at end of file