Skip to content

Refactor L2Cap (style) #331

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
h2zero merged 1 commit into from
May 6, 2025
Merged

Refactor L2Cap (style) #331

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
69 changes: 36 additions & 33 deletions src/NimBLEL2CAPChannel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,21 +7,18 @@
#include "NimBLELog.h"
#include "NimBLEUtils.h"

#include "nimble/nimble_port.h"

// L2CAP buffer block size
#define L2CAP_BUF_BLOCK_SIZE (250)
#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))
#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) {

: 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
Expand All @@ -30,14 +27,12 @@ NimBLEL2CAPChannel::NimBLEL2CAPChannel(uint16_t psm, uint16_t mtu, NimBLEL2CAPCh
};

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

Expand All @@ -59,7 +54,7 @@ bool NimBLEL2CAPChannel::setupMemPool() {
return false;
}

this->receiveBuffer = (uint8_t*) malloc(mtu);
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;
Expand All @@ -69,14 +64,18 @@ bool NimBLEL2CAPChannel::setupMemPool() {
}

void NimBLEL2CAPChannel::teardownMemPool() {

if (this->callbacks) { delete this->callbacks; }
if (this->receiveBuffer) { free(this->receiveBuffer); }
if (_coc_memory) { free(_coc_memory); }
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) {
Expand All @@ -101,7 +100,6 @@ int NimBLEL2CAPChannel::writeFragment(std::vector<uint8_t>::const_iterator begin
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.");
Expand All @@ -115,10 +113,15 @@ int NimBLEL2CAPChannel::writeFragment(std::vector<uint8_t>::const_iterator begin

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...");
NIMBLE_LOGW(LOG_TAG,
"ble_l2cap_send returned BLE_HS_ESTALLED. Next send will wait for unstalled event...");
return 0;

case BLE_HS_ENOMEM:
Expand All @@ -129,25 +132,24 @@ int NimBLEL2CAPChannel::writeFragment(std::vector<uint8_t>::const_iterator begin
ble_npl_time_delay(ble_npl_time_ms_to_ticks32(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) {

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");
NIMBLE_LOGE(
LOG_TAG,
"Client is not connected. Before connecting via L2CAP, a GAP connection must have been established");
return nullptr;
};

Expand All @@ -167,7 +169,6 @@ NimBLEL2CAPChannel* NimBLEL2CAPChannel::connect(NimBLEClient* client, uint16_t p
#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;
Expand All @@ -177,7 +178,6 @@ bool NimBLEL2CAPChannel::write(const std::vector<uint8_t>& bytes) {
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();
Expand All @@ -191,12 +191,16 @@ bool NimBLEL2CAPChannel::write(const std::vector<uint8_t>& bytes) {

// 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);
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);
}
Expand All @@ -212,7 +216,7 @@ int NimBLEL2CAPChannel::handleAcceptEvent(struct ble_l2cap_event* event) {
return -1;
}

struct os_mbuf *sdu_rx = os_mbuf_get_pkthdr(&_coc_mbuf_pool, 0);
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;
Expand Down Expand Up @@ -264,8 +268,7 @@ int NimBLEL2CAPChannel::handleDisconnectionEvent(struct ble_l2cap_event* event)
}

/* STATIC */
int NimBLEL2CAPChannel::handleL2capEvent(struct ble_l2cap_event *event, void *arg) {

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

Expand Down
47 changes: 25 additions & 22 deletions src/NimBLEL2CAPChannel.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,20 +3,26 @@
//
#pragma once
#ifndef NIMBLEL2CAPCHANNEL_H
#define NIMBLEL2CAPCHANNEL_H
# define NIMBLEL2CAPCHANNEL_H

#include "inttypes.h"
#include "host/ble_l2cap.h"
#include "os/os_mbuf.h"
#include "os/os_mempool.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 <vector>
#include <atomic>
# include <vector>
# include <atomic>

class NimBLEClient;
class NimBLEL2CAPChannelCallbacks;
Expand All @@ -30,8 +36,7 @@ struct NimBLETaskData;
* (which opens the connection) point of view.
*/
class NimBLEL2CAPChannel {

public:
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,
Expand All @@ -53,8 +58,7 @@ class NimBLEL2CAPChannel {
/// @return True, if the channel is connected. False, otherwise.
bool isConnected() const { return !!channel; }

protected:

protected:
NimBLEL2CAPChannel(uint16_t psm, uint16_t mtu, NimBLEL2CAPChannelCallbacks* callbacks);
~NimBLEL2CAPChannel();

Expand All @@ -64,19 +68,19 @@ class NimBLEL2CAPChannel {
int handleTxUnstalledEvent(struct ble_l2cap_event* event);
int handleDisconnectionEvent(struct ble_l2cap_event* event);

private:
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;
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
uint8_t* receiveBuffer = nullptr; // buffers a full (local) MTU

// NimBLE memory pool
void* _coc_memory = nullptr;
struct os_mempool _coc_mempool;
void* _coc_memory = nullptr;
struct os_mempool _coc_mempool;
struct os_mbuf_pool _coc_mbuf_pool;

// Runtime handling
Expand All @@ -91,16 +95,15 @@ class NimBLEL2CAPChannel {
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);
static int handleL2capEvent(struct ble_l2cap_event* event, void* arg);
};

/**
* @brief Callbacks base class for the L2CAP channel.
*/
class NimBLEL2CAPChannelCallbacks {

public:
NimBLEL2CAPChannelCallbacks() = default;
public:
NimBLEL2CAPChannelCallbacks() = default;
virtual ~NimBLEL2CAPChannelCallbacks() = default;

/// Called when the client attempts to open a channel on the server.
Expand Down
11 changes: 5 additions & 6 deletions src/NimBLEL2CAPServer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,22 +9,21 @@
static const char* LOG_TAG = "NimBLEL2CAPServer";

NimBLEL2CAPServer::NimBLEL2CAPServer() {

// Nothing to do here...
}

NimBLEL2CAPServer::~NimBLEL2CAPServer() {

// Delete all services
for (auto service: this->services) {
for (auto service : this->services) {
delete service;
}
}

NimBLEL2CAPChannel* NimBLEL2CAPServer::createService(const uint16_t psm, const uint16_t mtu, NimBLEL2CAPChannelCallbacks* callbacks) {

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);
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);
Expand Down
7 changes: 3 additions & 4 deletions src/NimBLEL2CAPServer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,23 +11,22 @@
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:
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:
private:
NimBLEL2CAPServer();
~NimBLEL2CAPServer();
std::vector<NimBLEL2CAPChannel*> services;
Expand Down