main.cpp

/* mbed Microcontroller Library
 * Copyright (c) 2006-2018 u-blox Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <events/mbed_events.h>
#include <mbed.h>
#include "ble/BLE.h"
#include "ble/DiscoveredCharacteristic.h"
#include "ble/DiscoveredService.h"

/**************************************************************************
 * MANIFEST CONSTANTS
 *************************************************************************/

// The maximum number of BLE addresses we can handle
// Note that this should be big enough to hold as many
// device as we can see around us (so that we know
// which ones are not interesting as well as those
// that are).  And there are loads of these things about.
#define MAX_NUM_BLE_DEVICES 100

// Storage required for a BLE address
#define BLE_ADDRESS_SIZE 6

// Storage required for a BLE address as a string
#define BLE_ADDRESS_STRING_SIZE 19

/**************************************************************************
 * TYPES
 *************************************************************************/

// The states that a BLE device can be in
typedef enum {
    BLE_DEVICE_STATE_UNKNOWN,
    BLE_DEVICE_STATE_NOT_WANTED,
    BLE_DEVICE_STATE_IS_WANTED,
    BLE_DEVICE_STATE_DISCOVERED,
    MAX_NUM_BLE_DEVICE_STATES
} BleDeviceState;

// Structure defining a BLE device
typedef struct {
    char address[BLE_ADDRESS_SIZE];
    BleDeviceState deviceState;
    bool connected;
    Gap::Handle_t connectionHandle;
} BleDevice;

/**************************************************************************
 * LOCAL VARIABLES
 *************************************************************************/

// Activity LED
static DigitalOut activityLedBar(LED1, 1); // Red (DS9), active low on a NINA B1 board

// Connection LED
static DigitalOut connectedLedBar(LED2, 1); // Green (DS9), active low on a NINA B1 board

// The BLE event queue
static EventQueue bleEventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);

// The list of BLE devices
static BleDevice peer_devices[MAX_NUM_BLE_DEVICES];

// Mutex to protect the list
static Mutex bleListMutex;

// The number of devices in the list
static int peer_devices_count = 0;

// TODO are these correct?
static const Gap::ConnectionParams_t connectionParams = {
    6 /* minConnectionInterval */,
    6 /* maxConnectionInterval */,
    0 /* slaveLatency */,
    100 /* connectionSupervisionTimeout (10 ms units) */
};

// Gap scanning parameters to minimise connection time (from https://os.mbed.com/docs/v5.8/reference/gap.html)
static const GapScanningParams connectionScanParams(
    GapScanningParams::SCAN_INTERVAL_MAX /* interval */,
    GapScanningParams::SCAN_WINDOW_MAX /* window */,
    3 /* timeout */,
    false /* active scanning */
);


static const size_t max_connections_count = 1;
static bool local_device_connecting = false;
static uint8_t connections_count = 0;

bool is_connection_allowed() {
    if (local_device_connecting) {
        return false;
    }

    return (connections_count < max_connections_count);
}

static const char *gapAdvertisingDataType[] = {
    "VALUE_NOT_ALLOWED",
    "FLAGS",
    "INCOMPLETE_LIST_16BIT_SERVICE_IDS",
    "COMPLETE_LIST_16BIT_SERVICE_IDS",
    "INCOMPLETE_LIST_32BIT_SERVICE_IDS",
    "COMPLETE_LIST_32BIT_SERVICE_IDS",
    "INCOMPLETE_LIST_128BIT_SERVICE_ID",
    "COMPLETE_LIST_128BIT_SERVICE_IDS",
    "SHORTENED_LOCAL_NAME",
    "COMPLETE_LOCAL_NAME",
    "TX_POWER_LEVEL",
    "DEVICE_ID",
    "SLAVE_CONNECTION_INTERVAL_RANGE",
    "LIST_128BIT_SOLICITATION_IDS",
    "SERVICE_DATA",
    "APPEARANCE",
    "ADVERTISING_INTERVAL"
};

static const char hexTable[] = {
    '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
    'a', 'b', 'c', 'd', 'e', 'f'
};

/**************************************************************************
 * STATIC FUNCTIONS
 *************************************************************************/

// Convert a sequence of bytes into a hex string, returning the number
// of characters written. The hex string is NOT null terminated.
static int bytesToHexString(const char *pInBuf, int lenInBuf, char *pOutBuf, int lenOutBuf)
{
    int y = 0;

    for (int x = 0; (x < lenInBuf) && (y < lenOutBuf); x++) {
        pOutBuf[y] = hexTable[(pInBuf[x] >> 4) & 0x0f]; // upper nibble
        y++;
        if (y < lenOutBuf) {
            pOutBuf[y] = hexTable[pInBuf[x] & 0x0f]; // lower nibble
            y++;
        }
    }

    return y;
}

// LED aliveness and status callback
static void periodicCallback(void) {
    Gap::GapState_t gapState;

    activityLedBar = !activityLedBar;

    gapState = BLE::Instance().gap().getState();
    if (gapState.connected) {
        connectedLedBar = 0;
    } else {
        connectedLedBar = 1;
    }
}

// Print a BLE_ADDRESS_SIZE digit binary BLE address out nicely as a
// string; pBuf must point to storage for a string at least
// BLE_ADDRESS_STRING_LENGTH bytes long
static char *printBleAddress(const char *pAddress, char *pBuf)
{
    int x = 0;

    for (int i = (BLE_ADDRESS_SIZE - 1); i >= 0; --i) {
        sprintf(pBuf + x, "%02x:", *(pAddress + i));
        x += 3;
    }
    *(pBuf + x - 1) = 0; // Remove the final ':'

    return pBuf;
}

// Find a BLE device in the list by its address
// Note that this does NOT lock the BLE list
static BleDevice *findBleDeviceInList(const char *pAddress)
{
    for (int i = 0; i < peer_devices_count; ++i) {
        if (memcmp(pAddress, peer_devices[i].address, sizeof(peer_devices[i].address)) == 0) {
            return &(peer_devices[i]);
        }
    }

    return NULL;
}

// Find a BLE device in the list by its connection
// Note that this does NOT lock the BLE list
static BleDevice *findBleConnectionInList(Gap::Handle_t connectionHandle)
{
    for (int i = 0; i < peer_devices_count; ++i) {
        if ((peer_devices[i].connected) &&
            (peer_devices[i].connectionHandle == connectionHandle)) {
            return &(peer_devices[i]);
        }
    }

    return NULL;
}

// Add a BLE device to the list, returning a pointer
// to the new entry.  If the device is already in the list
// a pointer is returned to the (unmodified) existing entry.
// If the list is already full then NULL is returned.
// Note that this does NOT lock the BLE list.
static BleDevice *addBleDeviceToList(const char *pAddress)
{
    BleDevice *device = findBleDeviceInList(pAddress);
    if (device) {
        return device;
    }

    if (peer_devices_count < (int) (sizeof(peer_devices) / sizeof (peer_devices[0]))) {
        device = &(peer_devices[peer_devices_count]);
        memcpy(device->address, pAddress, sizeof (device->address));
        device->deviceState = BLE_DEVICE_STATE_UNKNOWN;
        device->connected = false;
        ++peer_devices_count;
        return device;
    }

    return NULL;
}

// Process an advertisement and connect to the device if
// it is one that we want
static void advertisementCallback(const Gap::AdvertisementCallbackParams_t *pParams)
{
    if (is_connection_allowed() == false) {
        return;
    }

    char buf[BLE_ADDRESS_STRING_SIZE + 32];
    BleDevice *pBleDevice;
    ble_error_t bleError;
    int recordLength;
    int x = 0;
    bool discoverable = false;

    pBleDevice = addBleDeviceToList((const char *) pParams->peerAddr);
    if (pBleDevice == NULL) {
        printf("BLE device list is full (%d device(s))!\n", peer_devices_count);
        return;
    }

    // Only bother checking on the device if it isn't marked as "not wanted"
    // and if we're not already [attempting to] connect[ed] to it
    if ((pBleDevice->deviceState == BLE_DEVICE_STATE_UNKNOWN) &&
        ((pBleDevice->connected == false))) {
        printf("BLE device %s is visible.\n", printBleAddress((char *) pParams->peerAddr, buf));
        // Check if the device is discoverable
        while ((x < pParams->advertisingDataLen) && !discoverable) {
            /* The advertising payload is a collection of key/value records where
             * byte 0: length of the record excluding this byte but including the "type" byte
             * byte 1: The key, it is the type of the data
             * byte [2..N] The value. N is equal to byte0 - 1 */
            recordLength = pParams->advertisingData[x];
            if ((recordLength > 0) && (pParams->advertisingData[x + 1] != 0)) {  // Type of 0 is not allowed
                const int type = pParams->advertisingData[x + 1];
                const char *pValue = (const char *) pParams->advertisingData + x + 2;
                printf("  Advertising payload type 0x%02x", type);
                if (type < (int) (sizeof(gapAdvertisingDataType) / sizeof(gapAdvertisingDataType[0]))) {
                    printf(" (%s)", gapAdvertisingDataType[type]);
                }
                printf(" (%d byte(s)): 0x%.*s.\n", recordLength - 1,
                       bytesToHexString(pValue, recordLength - 1, buf, sizeof(buf)), buf);
                if ((type == GapAdvertisingData::FLAGS) &&
                    (*pValue & (GapAdvertisingData::LE_GENERAL_DISCOVERABLE | GapAdvertisingData::LE_LIMITED_DISCOVERABLE))) {
                    discoverable = true;
                }
                x += recordLength + 1;
            } else {
                x++;
            }
        }

        if (discoverable) {
            printf("  ...and discoverable, attempting to connect to it");

            // scan params to use for the connection operation - set to default
            // except for the timeout set to 5 seconds.
            GapScanningParams scan_params;
            scan_params.setTimeout(5);

            BLE::Instance().gap().stopScan();
            bleError = BLE::Instance().gap().connect(
                pParams->peerAddr,
                pParams->addressType,
                NULL,
                &scan_params
            );
            //bleError = BLE::Instance().gap().connect(pParams->peerAddr, BLEProtocol::AddressType::RANDOM_STATIC, &connectionParams, &connectionScanParams);
            if (bleError == BLE_ERROR_NONE) {
                local_device_connecting = true;
                printf(", connect() successfully issued.\n");
                printf("stop scan\r\n");
            } else if (bleError == BLE_ERROR_INVALID_STATE) {
                printf(", but CAN'T as BLE is in an invalid state (may already be connecting?).\n");
                printf("start scan\r\n");
                BLE::Instance().gap().startScan(advertisementCallback);
            } else {
                printf(", but unable to issue connect (error %d).\n", bleError);
                printf("start scan\r\n");
                BLE::Instance().gap().startScan(advertisementCallback);
            }
        } else {
            printf("  ...but not discoverable.\n");
            pBleDevice->deviceState = BLE_DEVICE_STATE_NOT_WANTED;
        }
    }
}

// Act on the discovery of a service
static void serviceDiscoveryCallback(const DiscoveredService *pService) {
    if (pService->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        printf("Service 0x%x attrs[%u %u].\n", pService->getUUID().getShortUUID(),
               pService->getStartHandle(), pService->getEndHandle());
    } else {
        printf("Service 0x");
        const char *pLongUUIDBytes = (char *) pService->getUUID().getBaseUUID();
        for (unsigned int i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
            printf("%02x", *(pLongUUIDBytes + i));
        }
        printf(" attrs[%u %u].\n", pService->getStartHandle(), pService->getEndHandle());
    }
}

// Act on the discovery of a characteristic
static void characteristicDiscoveryCallback(const DiscoveredCharacteristic *pCharacteristic) {
    printf("  Characteristic 0x%x valueAttr[%u] props[0x%x].\n", pCharacteristic->getUUID().getShortUUID(),
           pCharacteristic->getValueHandle(), (uint8_t) pCharacteristic->getProperties().broadcast());
}

// Handle end of service discovery
static void discoveryTerminationCallback(Gap::Handle_t connectionHandle) {
    char addressString[BLE_ADDRESS_STRING_SIZE];
    BleDevice *pBleDevice = findBleConnectionInList(connectionHandle);

    printf("Terminated service discovery for handle %u", connectionHandle);
    if (pBleDevice != NULL) {
        printf(", BLE device %s", printBleAddress(pBleDevice->address, addressString));
    }
    printf(".\n");

    BLE::Instance().gap().disconnect(
        connectionHandle,
        Gap::REMOTE_USER_TERMINATED_CONNECTION
    );
}

// When a connection has been made, find out what services are available and their characteristics
static void connectionCallback(const Gap::ConnectionCallbackParams_t *pParams) {

    // update state
    local_device_connecting = false;
    ++connections_count;
    printf("start scan\r\n");
    BLE::Instance().gap().startScan(advertisementCallback);

    char addressString[BLE_ADDRESS_STRING_SIZE];
    BleDevice *pBleDevice = findBleDeviceInList((char *) pParams->peerAddr);

    printf("BLE device %s is connected.\n", printBleAddress((char *) pParams->peerAddr, addressString));
    if (pBleDevice != NULL) {
        pBleDevice->connectionHandle = pParams->handle;
        pBleDevice->connected = true;
        if (pParams->role == Gap::CENTRAL) {
            printf("  Attempting to discover its services and characteristics...\n");
            BLE &ble = BLE::Instance();
            ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
            ble.gattClient().launchServiceDiscovery(pParams->handle, serviceDiscoveryCallback,
                                                    characteristicDiscoveryCallback,
                                                    BLE_UUID_UNKNOWN, BLE_UUID_UNKNOWN);
        }
    } else {
        // disconnect
        BLE::Instance().gap().disconnect(
            pParams->handle,
            Gap::REMOTE_USER_TERMINATED_CONNECTION
        );
    }
}

// Handle BLE peer disconnection
static void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *pParams) {
    // update local state
    --connections_count;

    char addressString[BLE_ADDRESS_STRING_SIZE];
    BleDevice *pBleDevice = findBleConnectionInList(pParams->handle);

    printf("Disconnected");
    if (pBleDevice != NULL) {
        printf(" from device %s", printBleAddress(pBleDevice->address, addressString));
        pBleDevice->deviceState = BLE_DEVICE_STATE_DISCOVERED;
        pBleDevice->connected = false;
    }
    printf(".\n");

    /* Start scanning again */
    //BLE::Instance().gap().startScan(advertisementCallback);
}

// Handle BLE initialisation error
static void onBleInitError(BLE &ble, ble_error_t error)
{
   printf("!!! BLE Error %d !!!\n", error);
}

static void on_timeout(Gap::TimeoutSource_t timeout_source) {
    switch (timeout_source) {
        case Gap::TIMEOUT_SRC_ADVERTISING:
            printf("Received Gap::TIMEOUT_SRC_ADVERTISING\n");
            break;

        case Gap::TIMEOUT_SRC_SECURITY_REQUEST:
            printf("Received Gap::TIMEOUT_SRC_SECURITY_REQUEST\n");
            break;

        case Gap::TIMEOUT_SRC_SCAN:
			printf("Received Gap::TIMEOUT_SRC_SCAN\n");

            // update state
            local_device_connecting = false;

            // restart scan
            printf("start scan\r\n");
            BLE::Instance().gap().startScan(advertisementCallback);

            break;

        case Gap::TIMEOUT_SRC_CONN:
            printf("Received Gap::TIMEOUT_SRC_CONN\n");
            // update state
            local_device_connecting = false;

            // restart scan
            printf("start scan\r\n");
            BLE::Instance().gap().startScan(advertisementCallback);
            break;

        default:
            break;
    }
}


// Handle BLE being initialised, finish configuration here
static void bleInitComplete(BLE::InitializationCompleteCallbackContext *pParams)
{
    BLE& ble = pParams->ble;
    ble_error_t error = pParams->error;
    Gap::AddressType_t addr_type;
    Gap::Address_t address;
    BLE::Instance().gap().getAddress(&addr_type, address);
    char addressString[BLE_ADDRESS_STRING_SIZE];

    if (error != BLE_ERROR_NONE) {
        /* In case of error, forward the error handling to onBleInitError */
        onBleInitError(ble, error);
        return;
    }

    /* Ensure that it is the default instance of BLE */
    if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }

    printf("This device's BLE address is %s.\n", printBleAddress((char *) address, addressString));

    ble.gap().onDisconnection(disconnectionCallback);
    ble.gap().onConnection(connectionCallback);
    ble.gap().onTimeout(on_timeout);

    // scan interval: 400 ms and scan window: 400 ms.
    // Every 400 ms the device will scan for 400 ms
    // This means that the device will scan continuously.
    ble.gap().setScanParams(400, 400);
    printf("start scan\r\n");
    ble.gap().startScan(advertisementCallback);
}

// Throw a BLE event onto the BLE event queue
static void scheduleBleEventsProcessing(
    BLE::OnEventsToProcessCallbackContext* pContext
) {
    BLE &ble = BLE::Instance();
    bleEventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}

// Run BLE for a given time; use -1 for infinity, in which
// case this function will never return
static void runBle(int durationMs)
{
    BLE &ble = BLE::Instance();

    ble.onEventsToProcess(scheduleBleEventsProcessing);
    ble.init(bleInitComplete);
    bleEventQueue.dispatch(durationMs);
}

/**************************************************************************
 * PUBLIC FUNCTIONS
 *************************************************************************/

// Main
int main()
{
    printf("\n\nStarting up.\n");

    bleEventQueue.call_every(500, periodicCallback);
    runBle(-1);
}