feat(L2CAP): add disconnect API and harden CoC send/error handling

- Add NimBLEL2CAPChannel::disconnect() and getConnHandle().
- Fix CoC TX mbuf ownership handling in writeFragment():
  treat BLE_HS_ENOMEM / BLE_HS_EAGAIN as consumed buffer,
  only free local tx mbuf on BLE_HS_EBUSY.
- Refresh L2CAP client/server examples for stress testing and
  runtime stats.
- Pin example dependency mickeyl/esp-hpl to tag 1.1.0.
- Clean trailing whitespace in updated example sources.

Closes #391
This commit is contained in:
Dr. Michael Lauer
2026-03-30 17:09:53 +02:00
committed by h2zero
parent 1d39b8fd05
commit aede439dab
6 changed files with 323 additions and 79 deletions
@@ -1,3 +1,6 @@
dependencies:
local/esp-nimble-cpp:
path: ../../../../../esp-nimble-cpp/
mickeyl/esp-hpl:
git: https://github.com/mickeyl/esp-hpl.git
version: "1.1.0"
+128 -40
View File
@@ -1,15 +1,12 @@
#include <NimBLEDevice.h>
#include <esp_hpl.hpp>
#include <esp_timer.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_PSM 192
#define L2CAP_MTU 5000
#define INITIAL_PAYLOAD_SIZE 64
#define BLOCKS_BEFORE_DOUBLE 50
#define MAX_PAYLOAD_SIZE 4900
const BLEAdvertisedDevice* theDevice = NULL;
BLEClient* theClient = NULL;
@@ -17,6 +14,15 @@ BLEL2CAPChannel* theChannel = NULL;
size_t bytesSent = 0;
size_t bytesReceived = 0;
size_t currentPayloadSize = INITIAL_PAYLOAD_SIZE;
uint32_t blocksSent = 0;
uint64_t startTime = 0;
// Heap monitoring
size_t initialHeap = 0;
size_t lastHeap = 0;
size_t heapDecreaseCount = 0;
const size_t HEAP_LEAK_THRESHOLD = 10; // Warn after 10 consecutive decreases
class L2CAPChannelCallbacks: public BLEL2CAPChannelCallbacks {
@@ -43,7 +49,7 @@ class MyClientCallbacks: public BLEClientCallbacks {
printf("GAP connected\n");
pClient->setDataLen(251);
theChannel = BLEL2CAPChannel::connect(pClient, L2CAP_CHANNEL, L2CAP_MTU, new L2CAPChannelCallbacks());
theChannel = BLEL2CAPChannel::connect(pClient, L2CAP_PSM, L2CAP_MTU, new L2CAPChannelCallbacks());
}
void onDisconnect(BLEClient* pClient, int reason) {
@@ -61,23 +67,72 @@ class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
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;
// Look for device named "l2cap"
if (advertisedDevice->haveName() && advertisedDevice->getName() == "l2cap") {
printf("Found l2cap device!\n");
BLEDevice::getScan()->stop();
theDevice = advertisedDevice;
}
}
};
void statusTask(void *pvParameters) {
while (true) {
vTaskDelay(1000 / portTICK_PERIOD_MS);
if (startTime > 0 && blocksSent > 0) {
uint64_t currentTime = esp_timer_get_time();
double elapsedSeconds = (currentTime - startTime) / 1000000.0;
double bytesPerSecond = 0.0;
double kbPerSecond = 0.0;
if (elapsedSeconds > 0.0) {
bytesPerSecond = bytesSent / elapsedSeconds;
kbPerSecond = bytesPerSecond / 1024.0;
}
// Heap monitoring
size_t currentHeap = esp_get_free_heap_size();
size_t minHeap = esp_get_minimum_free_heap_size();
// Track heap for leak detection
if (initialHeap == 0) {
initialHeap = currentHeap;
lastHeap = currentHeap;
}
// Check for consistent heap decrease
if (currentHeap < lastHeap) {
heapDecreaseCount++;
if (heapDecreaseCount >= HEAP_LEAK_THRESHOLD) {
printf("\n⚠️ WARNING: POSSIBLE MEMORY LEAK DETECTED! ⚠️\n");
printf("Heap has decreased %zu times in a row\n", heapDecreaseCount);
printf("Initial heap: %zu, Current heap: %zu, Lost: %zu bytes\n",
initialHeap, currentHeap, initialHeap - currentHeap);
}
} else if (currentHeap >= lastHeap) {
heapDecreaseCount = 0; // Reset counter if heap stabilizes or increases
}
lastHeap = currentHeap;
printf("\n=== STATUS UPDATE ===\n");
printf("Blocks sent: %lu\n", (unsigned long)blocksSent);
printf("Total bytes sent: %zu\n", bytesSent);
printf("Current payload size: %zu bytes\n", currentPayloadSize);
printf("Elapsed time: %.1f seconds\n", elapsedSeconds);
printf("Bandwidth: %.2f KB/s (%.2f Mbps)\n", kbPerSecond, (bytesPerSecond * 8) / 1000000.0);
printf("Heap: %zu free (min: %zu), Used since start: %zu\n",
currentHeap, minHeap, initialHeap > 0 ? initialHeap - currentHeap : 0);
printf("==================\n\n");
}
}
}
void connectTask(void *pvParameters) {
uint8_t sequenceNumber = 0;
while (true) {
if (!theDevice) {
vTaskDelay(1000 / portTICK_PERIOD_MS);
continue;
@@ -96,7 +151,7 @@ void connectTask(void *pvParameters) {
break;
}
vTaskDelay(2000 / portTICK_PERIOD_MS);
continue;
continue;
}
if (!theChannel) {
@@ -112,22 +167,58 @@ void connectTask(void *pvParameters) {
}
while (theChannel->isConnected()) {
// Create framed packet: [seqno 8bit] [16bit payload length] [payload]
std::vector<uint8_t> packet;
packet.reserve(3 + currentPayloadSize);
/*
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();
// Add sequence number (8 bits)
packet.push_back(sequenceNumber);
// Add payload length (16 bits, big endian - network byte order)
uint16_t payloadLen = currentPayloadSize;
packet.push_back((payloadLen >> 8) & 0xFF); // High byte first
packet.push_back(payloadLen & 0xFF); // Low byte second
// Add payload
for (size_t i = 0; i < currentPayloadSize; i++) {
packet.push_back(i & 0xFF);
}
if (theChannel->write(packet)) {
if (startTime == 0) {
startTime = esp_timer_get_time();
}
bytesSent += packet.size();
blocksSent++;
// Print every block since we're sending slowly now
printf("Sent block %lu (seq=%d, payload=%zu bytes, frame_size=%zu)\n",
(unsigned long)blocksSent, sequenceNumber, currentPayloadSize, packet.size());
sequenceNumber++;
// After every 50 blocks, double payload size
if (blocksSent % BLOCKS_BEFORE_DOUBLE == 0) {
size_t newSize = currentPayloadSize * 2;
// Cap at maximum safe payload size
if (newSize > MAX_PAYLOAD_SIZE) {
if (currentPayloadSize < MAX_PAYLOAD_SIZE) {
currentPayloadSize = MAX_PAYLOAD_SIZE;
printf("\n=== Reached maximum payload size of %zu bytes after %lu blocks ===\n", currentPayloadSize, (unsigned long)blocksSent);
}
// Already at max, don't increase further
} else {
currentPayloadSize = newSize;
printf("\n=== Doubling payload size to %zu bytes after %lu blocks ===\n", currentPayloadSize, (unsigned long)blocksSent);
}
}
} else {
printf("failed to send!\n");
abort();
abort();
}
// No delay - send as fast as possible
}
vTaskDelay(1000 / portTICK_PERIOD_MS);
@@ -136,9 +227,13 @@ void connectTask(void *pvParameters) {
extern "C"
void app_main(void) {
// Install high performance logging before any output
esp_hpl::HighPerformanceLogger::init();
printf("Starting L2CAP client example\n");
xTaskCreate(connectTask, "connectTask", 5000, NULL, 1, NULL);
xTaskCreate(statusTask, "statusTask", 3000, NULL, 1, NULL);
BLEDevice::init("L2CAP-Client");
BLEDevice::setMTU(BLE_ATT_MTU_MAX);
@@ -151,15 +246,8 @@ void app_main(void) {
scan->setActiveScan(true);
scan->start(25 * 1000, false);
int numberOfSeconds = 0;
while (bytesSent == 0) {
vTaskDelay(10 / portTICK_PERIOD_MS);
}
// Main task just waits
while (true) {
vTaskDelay(1000 / portTICK_PERIOD_MS);
int bytesSentPerSeconds = bytesSent / ++numberOfSeconds;
printf("Bandwidth: %d b/sec = %d KB/sec\n", bytesSentPerSeconds, bytesSentPerSeconds / 1024);
}
}
@@ -1,3 +1,6 @@
dependencies:
local/esp-nimble-cpp:
path: ../../../../../esp-nimble-cpp/
mickeyl/esp-hpl:
git: https://github.com/mickeyl/esp-hpl.git
version: "1.1.0"
+152 -38
View File
@@ -1,13 +1,16 @@
#include <NimBLEDevice.h>
#include <esp_hpl.hpp>
#include <esp_timer.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_PSM 192
#define L2CAP_MTU 5000
// Heap monitoring
size_t initialHeap = 0;
size_t lastHeap = 0;
size_t heapDecreaseCount = 0;
const size_t HEAP_LEAK_THRESHOLD = 10; // Warn after 10 consecutive decreases
class GATTCallbacks: public BLEServerCallbacks {
public:
@@ -23,68 +26,179 @@ class L2CAPChannelCallbacks: public BLEL2CAPChannelCallbacks {
public:
bool connected = false;
size_t numberOfReceivedBytes;
uint8_t nextSequenceNumber;
size_t totalBytesReceived = 0;
size_t totalFramesReceived = 0;
size_t totalPayloadBytes = 0;
uint8_t expectedSequenceNumber = 0;
size_t sequenceErrors = 0;
size_t frameErrors = 0;
uint64_t startTime = 0;
std::vector<uint8_t> buffer; // Buffer for incomplete frames
public:
void onConnect(NimBLEL2CAPChannel* channel) {
printf("L2CAP connection established\n");
printf("L2CAP connection established on PSM %d\n", L2CAP_PSM);
connected = true;
numberOfReceivedBytes = nextSequenceNumber = 0;
totalBytesReceived = 0;
totalFramesReceived = 0;
totalPayloadBytes = 0;
expectedSequenceNumber = 0;
sequenceErrors = 0;
frameErrors = 0;
startTime = esp_timer_get_time();
buffer.clear();
}
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++;
// Append new data to buffer
buffer.insert(buffer.end(), data.begin(), data.end());
totalBytesReceived += data.size();
if (startTime == 0) {
startTime = esp_timer_get_time(); // start measuring once data flows
}
// Process complete frames from buffer
while (buffer.size() >= 3) { // Minimum frame size: seqno(1) + len(2)
// Parse frame header
uint8_t seqno = buffer[0];
uint16_t payloadLen = (buffer[1] << 8) | buffer[2]; // Big-endian
size_t frameSize = 3 + payloadLen;
// Check if we have complete frame
if (buffer.size() < frameSize) {
break; // Wait for more data
}
// Validate and process frame
totalFramesReceived++;
totalPayloadBytes += payloadLen;
// Check sequence number
if (seqno != expectedSequenceNumber) {
sequenceErrors++;
printf("Frame %zu: Sequence error - got %d, expected %d (payload=%d bytes)\n",
totalFramesReceived, seqno, expectedSequenceNumber, payloadLen);
}
// Update expected sequence number (wraps at 256)
expectedSequenceNumber = (seqno + 1) & 0xFF;
// Remove processed frame from buffer
buffer.erase(buffer.begin(), buffer.begin() + frameSize);
// Print progress every 100 frames
if (totalFramesReceived % 100 == 0) {
double elapsedSeconds = (esp_timer_get_time() - startTime) / 1000000.0;
double bytesPerSecond = elapsedSeconds > 0 ? totalBytesReceived / elapsedSeconds : 0.0;
printf("Received %zu frames (%zu payload bytes) - Bandwidth: %.2f KB/s (%.2f Mbps)\n",
totalFramesReceived, totalPayloadBytes,
bytesPerSecond / 1024.0, (bytesPerSecond * 8) / 1000000.0);
}
}
}
void onDisconnect(NimBLEL2CAPChannel* channel) {
printf("L2CAP disconnected\n");
printf("\nL2CAP disconnected\n");
double elapsedSeconds = startTime > 0 ? (esp_timer_get_time() - startTime) / 1000000.0 : 0.0;
double bytesPerSecond = elapsedSeconds > 0 ? totalBytesReceived / elapsedSeconds : 0.0;
printf("Final statistics:\n");
printf(" Total frames: %zu\n", totalFramesReceived);
printf(" Total bytes: %zu\n", totalBytesReceived);
printf(" Payload bytes: %zu\n", totalPayloadBytes);
printf(" Sequence errors: %zu\n", sequenceErrors);
printf(" Frame errors: %zu\n", frameErrors);
printf(" Bandwidth: %.2f KB/s (%.2f Mbps)\n", bytesPerSecond / 1024.0, (bytesPerSecond * 8) / 1000000.0);
// Reset state for the next connection
buffer.clear();
totalBytesReceived = 0;
totalFramesReceived = 0;
totalPayloadBytes = 0;
expectedSequenceNumber = 0;
sequenceErrors = 0;
frameErrors = 0;
startTime = 0;
connected = false;
// Restart advertising so another client can connect
BLEDevice::startAdvertising();
}
};
extern "C"
void app_main(void) {
// Install high performance logging before any other output
esp_hpl::HighPerformanceLogger::init();
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::init("l2cap"); // Match the name the client is looking for
BLEDevice::setMTU(BLE_ATT_MTU_MAX);
auto cocServer = BLEDevice::createL2CAPServer();
auto l2capChannelCallbacks = new L2CAPChannelCallbacks();
auto channel = cocServer->createService(L2CAP_CHANNEL, L2CAP_MTU, l2capChannelCallbacks);
auto channel = cocServer->createService(L2CAP_PSM, L2CAP_MTU, l2capChannelCallbacks);
(void)channel; // prevent unused warning
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);
NimBLEAdvertisementData scanData;
scanData.setName("l2cap");
advertising->setScanResponseData(scanData);
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;
// Status reporting loop
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());
if (l2capChannelCallbacks->connected && l2capChannelCallbacks->totalBytesReceived > 0) {
uint64_t currentTime = esp_timer_get_time();
double elapsedSeconds = (currentTime - l2capChannelCallbacks->startTime) / 1000000.0;
if (elapsedSeconds > 0) {
double bytesPerSecond = l2capChannelCallbacks->totalBytesReceived / elapsedSeconds;
double framesPerSecond = l2capChannelCallbacks->totalFramesReceived / elapsedSeconds;
// Heap monitoring
size_t currentHeap = esp_get_free_heap_size();
size_t minHeap = esp_get_minimum_free_heap_size();
// Track heap for leak detection
if (initialHeap == 0) {
initialHeap = currentHeap;
lastHeap = currentHeap;
}
// Check for consistent heap decrease
if (currentHeap < lastHeap) {
heapDecreaseCount++;
if (heapDecreaseCount >= HEAP_LEAK_THRESHOLD) {
printf("\n⚠️ WARNING: POSSIBLE MEMORY LEAK DETECTED! ⚠️\n");
printf("Heap has decreased %zu times in a row\n", heapDecreaseCount);
printf("Initial heap: %zu, Current heap: %zu, Lost: %zu bytes\n",
initialHeap, currentHeap, initialHeap - currentHeap);
}
} else if (currentHeap >= lastHeap) {
heapDecreaseCount = 0; // Reset counter if heap stabilizes or increases
}
lastHeap = currentHeap;
printf("\n=== STATUS UPDATE ===\n");
printf("Frames received: %zu (%.1f fps)\n", l2capChannelCallbacks->totalFramesReceived, framesPerSecond);
printf("Total bytes: %zu\n", l2capChannelCallbacks->totalBytesReceived);
printf("Payload bytes: %zu\n", l2capChannelCallbacks->totalPayloadBytes);
printf("Bandwidth: %.2f KB/s (%.2f Mbps)\n", bytesPerSecond / 1024.0, (bytesPerSecond * 8) / 1000000.0);
printf("Sequence errors: %zu\n", l2capChannelCallbacks->sequenceErrors);
printf("Heap: %zu free (min: %zu), Used since start: %zu\n",
currentHeap, minHeap, initialHeap > 0 ? initialHeap - currentHeap : 0);
printf("==================\n");
}
}
}
}