EggDuino/src/BLE_Interface.cpp

#include "EggDuino.h"

#ifdef ESP32
#include <NimBLEDevice.h>
#include <string.h>

namespace
{
constexpr char kBleDeviceNamePrefix[] = "EggBot_";
constexpr char kBleServiceUuid[] = "6e400001-b5a3-f393-e0a9-e50e24dcca9e";
constexpr char kBleRxCharUuid[] = "6e400002-b5a3-f393-e0a9-e50e24dcca9e";
constexpr char kBleTxCharUuid[] = "6e400003-b5a3-f393-e0a9-e50e24dcca9e";

constexpr size_t kBleRxQueueSize = 512;
constexpr size_t kBleNotifyChunkSize = 20;

NimBLEServer *g_pBleServer = NULL;
NimBLECharacteristic *g_pBleTxCharacteristic = NULL;

uint8_t g_bleRxQueue[kBleRxQueueSize];
size_t g_bleRxHead = 0;
size_t g_bleRxTail = 0;
bool g_bleRxQueueOverflow = false;
bool g_bleClientConnected = false;

portMUX_TYPE g_bleQueueMux = portMUX_INITIALIZER_UNLOCKED;

void logBleDiag(const String &message)
{
	Log(message);
}

bool queueBleByte(uint8_t value)
{
	bool queued = false;
	portENTER_CRITICAL(&g_bleQueueMux);
	const size_t nextHead = (g_bleRxHead + 1) % kBleRxQueueSize;
	if (nextHead == g_bleRxTail)
	{
		g_bleRxQueueOverflow = true;
	}
	else
	{
		g_bleRxQueue[g_bleRxHead] = value;
		g_bleRxHead = nextHead;
		queued = true;
	}
	portEXIT_CRITICAL(&g_bleQueueMux);
	return queued;
}

bool dequeueBleByte(uint8_t *value)
{
	bool hasData = false;
	portENTER_CRITICAL(&g_bleQueueMux);
	if (g_bleRxHead != g_bleRxTail)
	{
		*value = g_bleRxQueue[g_bleRxTail];
		g_bleRxTail = (g_bleRxTail + 1) % kBleRxQueueSize;
		hasData = true;
	}
	portEXIT_CRITICAL(&g_bleQueueMux);
	return hasData;
}

class EggDuinoBleServerCallbacks : public NimBLEServerCallbacks
{
	void onConnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo)
	{
		(void)pServer;
		(void)connInfo;
		g_bleClientConnected = true;
		Log("BLE client connected");
	}

	void onDisconnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo, int reason)
	{
		(void)connInfo;
		(void)reason;
		g_bleClientConnected = false;
		const bool restartedAdvertising = pServer->startAdvertising();
		logBleDiag(String("BLE client disconnected; advertising restart: ") + (restartedAdvertising ? "ok" : "failed"));
	}
};

class EggDuinoBleRxCallbacks : public NimBLECharacteristicCallbacks
{
	void onWrite(NimBLECharacteristic *pCharacteristic, NimBLEConnInfo &connInfo)
	{
		(void)connInfo;
		const std::string value = pCharacteristic->getValue();
		for (size_t i = 0; i < value.size(); ++i)
		{
			queueBleByte(static_cast<uint8_t>(value[i]));
		}
	}
};
} // namespace

void startBleInterface()
{
	char bleDeviceName[32] = {0};
	buildDeviceName(bleDeviceName, sizeof(bleDeviceName));
	if (bleDeviceName[0] == '\0')
	{
		snprintf(bleDeviceName, sizeof(bleDeviceName), "%sUNKNOWN", kBleDeviceNamePrefix);
	}

	logBleDiag("BLE init begin");
	logBleDiag(String("BLE device name: ") + bleDeviceName);
	logBleDiag(String("BLE service UUID: ") + kBleServiceUuid);
	logBleDiag(String("BLE RX UUID: ") + kBleRxCharUuid);
	logBleDiag(String("BLE TX UUID: ") + kBleTxCharUuid);

	NimBLEDevice::init(bleDeviceName);
	const bool blePowerSet = NimBLEDevice::setPower(ESP_PWR_LVL_P6);
	const std::string bleAddress = NimBLEDevice::getAddress().toString();
	logBleDiag(String("BLE radio address: ") + bleAddress.c_str());
	logBleDiag(String("BLE TX power set: ") + (blePowerSet ? "ok" : "failed"));

	g_pBleServer = NimBLEDevice::createServer();
	if (g_pBleServer == NULL)
	{
		logBleDiag("BLE init failed: createServer returned null");
		return;
	}
	g_pBleServer->setCallbacks(new EggDuinoBleServerCallbacks());
	logBleDiag("BLE server created");

	NimBLEService *pService = g_pBleServer->createService(kBleServiceUuid);
	if (pService == NULL)
	{
		logBleDiag("BLE init failed: createService returned null");
		return;
	}
	g_pBleTxCharacteristic = pService->createCharacteristic(
		kBleTxCharUuid,
		NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ);
	if (g_pBleTxCharacteristic == NULL)
	{
		logBleDiag("BLE init failed: TX characteristic creation failed");
		return;
	}

	NimBLECharacteristic *pRxCharacteristic = pService->createCharacteristic(
		kBleRxCharUuid,
		NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR);
	if (pRxCharacteristic == NULL)
	{
		logBleDiag("BLE init failed: RX characteristic creation failed");
		return;
	}
	pRxCharacteristic->setCallbacks(new EggDuinoBleRxCallbacks());
	logBleDiag("BLE characteristics created");

	const bool serviceStarted = pService->start();
	logBleDiag(String("BLE service start: ") + (serviceStarted ? "ok" : "failed"));
	if (!serviceStarted)
	{
		return;
	}

	NimBLEAdvertising *pAdvertising = NimBLEDevice::getAdvertising();
	if (pAdvertising == NULL)
	{
		logBleDiag("BLE init failed: getAdvertising returned null");
		return;
	}
	pAdvertising->enableScanResponse(true);
	const bool localNameSet = pAdvertising->setName(bleDeviceName);
	const bool serviceUuidAdded = pAdvertising->addServiceUUID(kBleServiceUuid);
	const bool advertisingStarted = pAdvertising->start();
	logBleDiag(String("BLE advertising set local name: ") + (localNameSet ? "ok" : "failed"));
	logBleDiag(String("BLE advertising add service UUID: ") + (serviceUuidAdded ? "ok" : "failed"));
	logBleDiag(String("BLE advertising start: ") + (advertisingStarted ? "ok" : "failed"));
	if (serviceUuidAdded && advertisingStarted)
	{
		logBleDiag("BLE service started");
	}
}

void handleBleInterface()
{
	if (g_bleRxQueueOverflow)
	{
		g_bleRxQueueOverflow = false;
		Log("BLE RX queue overflow");
	}

	uint8_t value = 0;
	while (dequeueBleByte(&value))
	{
		setActiveProtocolContext(&g_BLECmd, PROTOCOL_TRANSPORT_BLE);
		g_BLECmd.readChar(static_cast<char>(value));
	}
}

bool bleProtocolWrite(const char *message)
{
	if ((message == NULL) || !g_bleClientConnected || (g_pBleTxCharacteristic == NULL))
	{
		return false;
	}

	const uint8_t *payload = reinterpret_cast<const uint8_t *>(message);
	size_t remaining = strlen(message);
	if (remaining == 0)
	{
		return true;
	}

	while (remaining > 0)
	{
		size_t chunkLen = remaining;
		if (chunkLen > kBleNotifyChunkSize)
		{
			chunkLen = kBleNotifyChunkSize;
		}

		g_pBleTxCharacteristic->setValue(payload, chunkLen);
		g_pBleTxCharacteristic->notify();

		payload += chunkLen;
		remaining -= chunkLen;
		delay(3);
	}

	return true;
}

#endif