File content as of revision 79:0095bfb18c57:

#include "mbed.h"
#include "BLE.h"

#include "config.h"
#include "WatchDog.h"
#include "KeyboardService.h"
#include "BatteryService.h"
#include "DeviceInformationService.h"
#include "DFUService.h"
#include "HIDController_BLE.h"

static const char MANUFACTURERERS_NAME[] = "lowreal.net";
static const char MODEL_NAME[] = "keble";
static const char SERIAL_NUMBER[] = "X00000";
static const char HARDWARE_REVISION[] = "0.1";
static const char FIRMWARE_REVISION[] = "0.1";
static const char SOFTWARE_REVISION[] = "0.0";
static PnPID_t PNP_ID = {
    0x00,
    0x0000,
    0x0000,
    0x00,
};

static const uint8_t DEVICE_NAME[] = "Keble-dev";

static const bool ENABLE_BONDING = true;
static const bool REQUIRE_MITM = true;
static const uint8_t PASSKEY[6] = {'1','2','3','4','5','6'}; // must be 6-digits number

static const uint16_t uuid16_list[] =  {
	GattService::UUID_HUMAN_INTERFACE_DEVICE_SERVICE,
	GattService::UUID_DEVICE_INFORMATION_SERVICE,
	GattService::UUID_BATTERY_SERVICE
};

static KeyboardService* keyboardService;
static BatteryService* batteryService;
static DeviceInformationService* deviceInformationService;
// static DFUService* dfuService;


static BLEProtocol::Address_t peerAddress;

static volatile Status_t controllerStatus;

static void onConnect(const Gap::ConnectionCallbackParams_t *params) {
	peerAddress.type = params->peerAddrType;
	memcpy(peerAddress.address, params->peerAddr, Gap::ADDR_LEN);

	BLEProtocol::Address_t peerAddresses[2];
	Gap::Whitelist_t whitelist;
	whitelist.size = 0;
	whitelist.capacity = 2;
	whitelist.addresses = peerAddresses;
	BLE::Instance(BLE::DEFAULT_INSTANCE).gap().getWhitelist(whitelist);
	DEBUG_PRINTF_BLE_INTERRUPT("getWhitelist %d\r\n", whitelist.size);
	for (int i = 0; i < whitelist.size; i++) {
		if (whitelist.addresses[i].type == params->peerAddrType &&
				memcmp(whitelist.addresses[i].address, params->peerAddr, Gap::ADDR_LEN) == 0) {

			BLE::Instance(BLE::DEFAULT_INSTANCE).gap().setAdvertisingPolicyMode(Gap::ADV_POLICY_FILTER_ALL_REQS);
			controllerStatus = CONNECTED;
			DEBUG_PRINTF_BLE_INTERRUPT("peer is found in whitelist\r\n");
			return;
		}
	}

	controllerStatus = CONNECTING;
	DEBUG_PRINTF_BLE_INTERRUPT("peer is not found in whitelist\r\n");
}

static void onDisconnect(const Gap::DisconnectionCallbackParams_t *params) {
	controllerStatus = DISCONNECTED;
	DEBUG_PRINTF_BLE_INTERRUPT("onDisconnect\r\n");
}

static void onTimeout(const Gap::TimeoutSource_t source) {
	DEBUG_PRINTF_BLE_INTERRUPT("onTimeout %d\r\n", source);
	switch (source) {
		case Gap::TIMEOUT_SRC_ADVERTISING:
			controllerStatus = TIMEOUT;
			return;
		
		// treat following timeout as DISCONNECT (retry advertising)
		case Gap::TIMEOUT_SRC_SECURITY_REQUEST:
		case Gap::TIMEOUT_SRC_SCAN:
		case Gap::TIMEOUT_SRC_CONN:
			controllerStatus = DISCONNECTED;
			return;
	}
}

static void passkeyDisplayCallback(Gap::Handle_t handle, const SecurityManager::Passkey_t passkey) {
	DEBUG_PRINTF_BLE_INTERRUPT("Input passKey: ");
	for (unsigned i = 0; i < Gap::ADDR_LEN; i++) {
		DEBUG_PRINTF_BLE_INTERRUPT("%c", passkey[i]);
	}
	DEBUG_PRINTF_BLE_INTERRUPT("\r\n");
}

static void securitySetupCompletedCallback(Gap::Handle_t handle, SecurityManager::SecurityCompletionStatus_t status) {
	if (status == SecurityManager::SEC_STATUS_SUCCESS) {
		DEBUG_PRINTF_BLE_INTERRUPT("Security success %d\r\n", status);
		DEBUG_PRINTF_BLE_INTERRUPT("Set whitelist\r\n");
		Gap::Whitelist_t whitelist;
		whitelist.size = 1;
		whitelist.capacity = 1;
		whitelist.addresses = &peerAddress;

		BLE::Instance(BLE::DEFAULT_INSTANCE).gap().setWhitelist(whitelist);
		DEBUG_PRINTF_BLE_INTERRUPT("Set Advertising Policy Mode\r\n");
		// BLE::Instance(BLE::DEFAULT_INSTANCE).gap().setAdvertisingPolicyMode(Gap::ADV_POLICY_FILTER_SCAN_REQS);
		// BLE::Instance(BLE::DEFAULT_INSTANCE).gap().setAdvertisingPolicyMode(Gap::ADV_POLICY_FILTER_CONN_REQS);
		BLE::Instance(BLE::DEFAULT_INSTANCE).gap().setAdvertisingPolicyMode(Gap::ADV_POLICY_FILTER_ALL_REQS);
		controllerStatus = CONNECTED;
	} else {
		DEBUG_PRINTF_BLE_INTERRUPT("Security failed %d\r\n", status);
	}
}

static void securitySetupInitiatedCallback(Gap::Handle_t, bool allowBonding, bool requireMITM, SecurityManager::SecurityIOCapabilities_t iocaps) {
	DEBUG_PRINTF_BLE_INTERRUPT("Security setup initiated\r\n");
}

static void bleInitComplete(BLE::InitializationCompleteCallbackContext *params) {
	// https://developer.mbed.org/compiler/#nav:/keyboard/BLE_API/ble/blecommon.h;
	ble_error_t error;
	BLE &ble          = params->ble;
	
	controllerStatus = DISCONNECTED;

	/**< Minimum Connection Interval in 1.25 ms units, see BLE_GAP_CP_LIMITS.*/
	uint16_t minConnectionInterval = Gap::MSEC_TO_GAP_DURATION_UNITS(15);
	/**< Maximum Connection Interval in 1.25 ms units, see BLE_GAP_CP_LIMITS.*/
	uint16_t maxConnectionInterval = Gap::MSEC_TO_GAP_DURATION_UNITS(20);
	/**< Slave Latency in number of connection events, see BLE_GAP_CP_LIMITS.*/
	uint16_t slaveLatency = 4;
	/**< Connection Supervision Timeout in 10 ms units, see BLE_GAP_CP_LIMITS.*/ 
	uint16_t connectionSupervisionTimeout = 32 * 100;
	Gap::ConnectionParams_t connectionParams = {
		minConnectionInterval,
		maxConnectionInterval,
		slaveLatency,
		connectionSupervisionTimeout
	};

	error = params->error;
	if (error != BLE_ERROR_NONE) {
		DEBUG_PRINTF_BLE("error on ble.init() \r\n");
		goto return_error;
	}

	ble.gap().onDisconnection(onDisconnect);
	ble.gap().onConnection(onConnect);
	ble.gap().onTimeout(onTimeout);

	// DEBUG_PRINTF_BLE("setup ble security manager\r\n");
	ble.securityManager().onSecuritySetupInitiated(securitySetupInitiatedCallback);
	ble.securityManager().onPasskeyDisplay(passkeyDisplayCallback);
	ble.securityManager().onSecuritySetupCompleted(securitySetupCompletedCallback);
	// bonding with hard-coded passkey.
	error = ble.securityManager().init(ENABLE_BONDING, REQUIRE_MITM, SecurityManager::IO_CAPS_DISPLAY_ONLY, PASSKEY);
	if (error != BLE_ERROR_NONE) {
		DEBUG_PRINTF_BLE("error on ble.securityManager().init()");
		goto return_error;
	}
	
	// DEBUG_PRINTF_BLE("new KeyboardService\r\n");
	keyboardService = new KeyboardService(ble);
	keyboardService->init();
	DEBUG_PRINTF_BLE("new DeviceInformationService\r\n");
	deviceInformationService = new DeviceInformationService(ble, MANUFACTURERERS_NAME, MODEL_NAME, SERIAL_NUMBER, HARDWARE_REVISION, FIRMWARE_REVISION, SOFTWARE_REVISION, &PNP_ID);
	DEBUG_PRINTF_BLE("new BatteryService\r\n");
	batteryService = new BatteryService(ble, 100);
	/** TODO
	DEBUG_PRINTF_BLE("new DFUService\r\n");
	dfuService = new DFUService(ble);
	*/
	
	//DEBUG_PRINTF_BLE("setup connection params\r\n");

	ble.gap().setPreferredConnectionParams(&connectionParams);

	// DEBUG_PRINTF_BLE("general setup\r\n");
//	error = ble.gap().accumulateAdvertisingPayload(
//		GapAdvertisingData::BREDR_NOT_SUPPORTED |
//		GapAdvertisingData::LE_GENERAL_DISCOVERABLE
//	);
	// shoud be LE_LIMITED_DISCOVERABLE
	error = ble.gap().accumulateAdvertisingPayload(
		GapAdvertisingData::BREDR_NOT_SUPPORTED |
		GapAdvertisingData::LE_LIMITED_DISCOVERABLE
	);
	if (error != BLE_ERROR_NONE) goto return_error;

	// DEBUG_PRINTF_BLE("set COMPLETE_LIST_16BIT_SERVICE_IDS\r\n");
	error = ble.gap().accumulateAdvertisingPayload(
		GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
		(uint8_t*)uuid16_list, sizeof(uuid16_list)
	);
	if (error != BLE_ERROR_NONE) goto return_error;

	// DEBUG_PRINTF_BLE("set advertising\r\n");
	// see 5.1.2: HID over GATT Specification (pg. 25)
	ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);

	// DEBUG_PRINTF_BLE("set advertising interval\r\n");
	ble.gap().setAdvertisingInterval(20);
	// DEBUG_PRINTF_BLE("set advertising timeout\r\n");
	ble.gap().setAdvertisingTimeout(30);
	
	/*
	ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_DIRECTED);
	ble.gap().setAdvertisingTimeout(1.28);
	*/

	// DEBUG_PRINTF_BLE("set keyboard\r\n");
	error = ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::KEYBOARD);
	if (error != BLE_ERROR_NONE) goto return_error;

	// DEBUG_PRINTF_BLE("set complete local name\r\n");
	error = ble.gap().accumulateAdvertisingPayload(
		GapAdvertisingData::COMPLETE_LOCAL_NAME,
		DEVICE_NAME, sizeof(DEVICE_NAME)
	);
	if (error != BLE_ERROR_NONE) goto return_error;

	// DEBUG_PRINTF_BLE("set device name\r\n");
	error = ble.gap().setDeviceName(DEVICE_NAME);
	if (error != BLE_ERROR_NONE) goto return_error;
	/* (Valid values are -40, -20, -16, -12, -8, -4, 0, 4) */
	ble.gap().setTxPower(0);

	{
		BLEProtocol::Address_t peerAddresses[2];
		Gap::Whitelist_t whitelist;
		whitelist.size = 0;
		whitelist.capacity = 2;
		whitelist.addresses = peerAddresses;
		error = ble.securityManager().getAddressesFromBondTable(whitelist);
		DEBUG_PRINTF_BLE("getAddressesFromBondTable %d\r\n", whitelist.size);
		BLE::Instance(BLE::DEFAULT_INSTANCE).gap().setWhitelist(whitelist);
	//	for (int i = 0; i < whitelist.size; i++) {
	//		whitelist.addresses[i]
	//	}
	}

	ble.gap().setAdvertisingPolicyMode(Gap::ADV_POLICY_IGNORE_WHITELIST);

	// DEBUG_PRINTF_BLE("advertising\r\n");
	error = ble.gap().startAdvertising();
	if (error != BLE_ERROR_NONE) goto return_error;
	controllerStatus = ADVERTISING;
	return;

return_error:
	DEBUG_PRINTF_BLE("error with %d\r\n", error);
	return;
}

bool HIDController::connected() {
	return controllerStatus == CONNECTED;
}

Status_t HIDController::status() {
	return controllerStatus;
}

const char* HIDController::statusString() {
	static const char* const disconnected = "disconnected";
	static const char* const connecting = "connecting";
	static const char* const connected = "connected";
	static const char* const timeout = "timeout";
	static const char* const advertising = "advertising";
	static const char* const unknown = "unknown";

	return controllerStatus == DISCONNECTED ? disconnected:
	       controllerStatus == CONNECTING ? connecting:
	       controllerStatus == CONNECTED ? connected:
	       controllerStatus == TIMEOUT ? timeout:
	       controllerStatus == ADVERTISING ? advertising:
	       unknown;
}

void HIDController::init() {
	// https://github.com/jpbrucker/BLE_HID/blob/master/examples/examples_common.cpp
	DEBUG_PRINTF_BLE("ble.init\r\n");

	BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
	ble.init(bleInitComplete);
	
	// copied from https://github.com/lancaster-university/microbit-dal/commit/3c314794f07e5ac91331c9e9849475375708ec89
	// configure the stack to hold on to CPU during critical timing events.
 	// mbed-classic performs __disabe_irq calls in its timers, which can cause MIC failures 
 	// on secure BLE channels.
	ble_common_opt_radio_cpu_mutex_t opt;
	opt.enable = 1;
	sd_ble_opt_set(BLE_COMMON_OPT_RADIO_CPU_MUTEX, (const ble_opt_t *)&opt);
     
	while (!ble.hasInitialized()) { }
	DEBUG_PRINTF_BLE("ble.hasIntialized\r\n");
}


void HIDController::waitForEvent() {
	BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);

	WatchDog::reload();

	keyboardService->processSend();
	if (DEBUG_BLE_INTERRUPT) {
		ble.waitForEvent();
	} else {
		// disable internal HFCLK RC Clock surely. It consume 1mA constantly
		// TWI / SPI / UART must be disabled and boot without debug mode
		while (NRF_UART0->EVENTS_TXDRDY != 1);

		const uint32_t tx = NRF_UART0->PSELTXD;

		NRF_UART0->TASKS_STOPTX = 1;
		NRF_UART0->ENABLE = (UART_ENABLE_ENABLE_Disabled << UART_ENABLE_ENABLE_Pos);

		ble.waitForEvent();

		NRF_UART0->ENABLE = (UART_ENABLE_ENABLE_Enabled << UART_ENABLE_ENABLE_Pos);
		NRF_UART0->TASKS_STARTTX = 1;
		// dummy send to wakeup...
		NRF_UART0->PSELTXD = 0xFFFFFFFF;
		NRF_UART0->EVENTS_TXDRDY = 0;
		NRF_UART0->TXD = 0;
		while (NRF_UART0->EVENTS_TXDRDY != 1);
		NRF_UART0->PSELTXD = tx;
	}
}

void HIDController::appendReportData(const uint8_t key) {
	if (keyboardService) {
		keyboardService->appendReportData(key);
	}
}

void HIDController::deleteReportData(const uint8_t key) {
	if (keyboardService) {
		keyboardService->deleteReportData(key);
	}
}

void HIDController::queueCurrentReportData() {
	if (!connected()) return;
	if (keyboardService) {
		DEBUG_PRINTF_BLE("Q %d\r\n", keyboardService->bufferCount);
		keyboardService->queueCurrentReportData();
	}
}

void HIDController::updateBatteryLevel(const uint8_t percentage, const uint16_t voltage) {
	if (!batteryService) return;
	DEBUG_PRINTF_BLE("updateBatteryLevel\r\n");
	batteryService->updateBatteryLevel(percentage, voltage);
}

void HIDController::initializeConnection(const bool ignoreWhiteList = false) {
	ble_error_t error;

	DEBUG_PRINTF_BLE("initializeConnection(%d)\r\n", ignoreWhiteList);

	BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
	ble.gap().setAdvertisingInterval(20);
	ble.gap().setAdvertisingTimeout(30);
	if (ignoreWhiteList) {
		ble.gap().setAdvertisingPolicyMode(Gap::ADV_POLICY_IGNORE_WHITELIST);
	} else {
		// XXX : startAdvertising returns BLE_ERROR_PARAM_OUT_OF_RANGE
		// ble.gap().setAdvertisingPolicyMode(Gap::ADV_POLICY_FILTER_ALL_REQS);
		ble.gap().setAdvertisingPolicyMode(Gap::ADV_POLICY_IGNORE_WHITELIST);
	}

	// DEBUG_PRINTF_BLE("advertising\r\n");
	error = ble.gap().startAdvertising();
	if (error != BLE_ERROR_NONE) goto return_error;
	controllerStatus = ADVERTISING;
	return;

return_error:
	DEBUG_PRINTF_BLE("error with %d\r\n", error);
	return;
}