File content as of revision 129:590bdc2dcf5b:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2013 ARM 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 "debug.h"
#include "common_config.h"
#include "ble/BLE.h"
#include "ble/services/UARTService.h"
#include "SecurityManager.h"
#include "BleManager.h"
#if MBED_CONF_APP_FILESYSTEM_SUPPORT
#include "LittleFileSystem.h"
#include "HeapBlockDevice.h"
#endif //MBED_CONF_APP_FILESYSTEM_SUPPORT
#define FILE_CODE       "btle"


//static const uint8_t DEVICE_NAME[] = "SM_device";
//static const uint16_t uuid16_list[] = {LEDService::LED_SERVICE_UUID};
extern UARTService *uart;

/** This example demonstrates all the basic setup required
 *  for pairing and setting up link security both as a central and peripheral
 *
 *  The example is implemented as two classes, one for the peripheral and one
 *  for central inheriting from a common base. They are run in sequence and
 *  require a peer device to connect to. During the peripheral device demonstration
 *  a peer device is required to connect. In the central device demonstration
 *  this peer device will be scanned for and connected to - therefore it should
 *  be advertising with the same address as when it connected.
 *
 *  During the test output is written on the serial connection to monitor its
 *  progress.
 */

//static const uint8_t DEVICE_NAME[] = "SM_device";

/* for demonstration purposes we will store the peer device address
 * of the device that connects to us in the first demonstration
 * so we can use its address to reconnect to it later */
//static BLEProtocol::AddressBytes_t peer_address;

/** Base class for both peripheral and central. The same class that provides
 *  the logic for the application also implements the SecurityManagerEventHandler
 *  which is the interface used by the Security Manager to communicate events
 *  back to the applications. You can provide overrides for a selection of events
 *  your application is interested in.
 */
SMDevice::SMDevice(BLE &ble, events::EventQueue &event_queue, 
                   BLEProtocol::AddressBytes_t &peer_address, 
                   MemoryPool<at_ble_msg_t, PQDSZ_BLE> *aT2BleDatamPool, 
                   Queue<at_ble_msg_t, PQDSZ_BLE> *aT2BleDataQueue, 
                   MemoryPool<ble_at_msg_t, PQDSZ_BLE> *ble2ATDatamPool, 
                   Queue<ble_at_msg_t, PQDSZ_BLE> *ble2ATDataQueue, 
                   ble_config_t *ble_config) :
        _ble(ble),
        _event_queue(event_queue),
        _peer_address(peer_address),
        _aT2BleDatamPool (aT2BleDatamPool),
        _aT2BleDataQueue (aT2BleDataQueue),
        _ble2ATDatamPool (ble2ATDatamPool),
        _ble2ATDataQueue (ble2ATDataQueue),
        ble_config(ble_config),
        _handle(0),
        _is_connecting(false), 
        _led1(LED1, 0)
{ 
    isConnected = false;
}

SMDevice::~SMDevice()
{
    if (_ble.hasInitialized()) {
        _ble.shutdown();
    }
}

/** Start BLE interface initialisation */
void SMDevice::run()
{
    dbg_printf(LOG, "\r\n [BTLE MAN]  Thread Id = %X\r\n", (uint32_t)ThisThread::get_id());

    ble_error_t error;

    /* to show we're running we'll blink every 10secs */
    //_event_queue.call_every(10000, this, &SMDevice::blink);

    /* to show we're advertising we'll print status every minute */
    //_event_queue.call_every(60000, this, &SMDevice::reportGapState);

    /* process queues every BLE_PROCESS_QUEUES_INTERVAL_MS */
    _event_queue.call_every(BLE_PROCESS_QUEUES_INTERVAL_MS, this, &SMDevice::processQueues);



    if (_ble.hasInitialized()) {
        dbg_printf(LOG, "Ble instance already initialised.\r\n");
        return;
    }

    /* this will inform us off all events so we can schedule their handling
     * using our event queue */
    _ble.onEventsToProcess(
        makeFunctionPointer(this, &SMDevice::schedule_ble_events)
    );

    /* handle timeouts, for example when connection attempts fail */
    _ble.gap().onTimeout(
        makeFunctionPointer(this, &SMDevice::on_timeout)
    );

    error = _ble.init(this, &SMDevice::on_init_complete);

    if (error) {
        dbg_printf(LOG, "Error returned by BLE::init.\r\n");
        return;
    }

    /* this will not return until shutdown */
    //_event_queue.dispatch_forever();
}


void SMDevice::shutDown()
{
    if (_ble.hasInitialized()) {
        _ble.shutdown();
        dbg_printf(LOG, "Shutting down BLE Instance...\r\n");
        _event_queue.break_dispatch();
    }
}


/* event handler functions */

/** Respond to a pairing request. This will be called by the stack
 * when a pairing request arrives and expects the application to
 * call acceptPairingRequest or cancelPairingRequest */
void SMDevice::pairingRequest(
    ble::connection_handle_t connectionHandle
) {
    dbg_printf(LOG, "Pairing requested - authorising\r\n");
    _ble.securityManager().acceptPairingRequest(connectionHandle);
}

/** Inform the application of a successful pairing. Terminate the demonstration. */
void SMDevice::pairingResult(
    ble::connection_handle_t connectionHandle,
    SecurityManager::SecurityCompletionStatus_t result
) {
    if (result == SecurityManager::SEC_STATUS_SUCCESS) {
        dbg_printf(LOG, "Pairing successful\r\n");
    } else {
        dbg_printf(LOG, "Pairing failed\r\n");
    }
}

/** Inform the application of change in encryption status. This will be
 * communicated through the serial port */
void SMDevice::linkEncryptionResult(
    ble::connection_handle_t connectionHandle,
    ble::link_encryption_t result
) {
    if (result == ble::link_encryption_t::ENCRYPTED) {
        dbg_printf(LOG, "Link ENCRYPTED\r\n");
    } else if (result == ble::link_encryption_t::ENCRYPTED_WITH_MITM) {
        dbg_printf(LOG, "Link ENCRYPTED_WITH_MITM\r\n");
    } else if (result == ble::link_encryption_t::NOT_ENCRYPTED) {
        dbg_printf(LOG, "Link NOT_ENCRYPTED\r\n");
    }

#ifdef DEMO_BLE_SECURITY
    /* disconnect in 2 s */
    _event_queue.call_in(
        2000, &_ble.gap(),
        &Gap::disconnect, _handle, Gap::REMOTE_USER_TERMINATED_CONNECTION
    );
#endif
}

/** Override to start chosen activity when initialisation completes */
//void SMDevice::start() = 0;

/** This is called when BLE interface is initialised and starts the demonstration */
void SMDevice::on_init_complete(BLE::InitializationCompleteCallbackContext *event)
{
    ble_error_t error;

    if (event->error) {
        dbg_printf(LOG, "Error during the initialisation\r\n");
        return;
    }

    /* This path will be used to store bonding information but will fallback
     * to storing in memory if file access fails (for example due to lack of a filesystem) */
    const char* db_path = "/fs/bt_sec_db";
    /* If the security manager is required this needs to be called before any
     * calls to the Security manager happen. */
    error = _ble.securityManager().init(
        true,
        false,
        SecurityManager::IO_CAPS_DISPLAY_ONLY, // SecurityManager::IO_CAPS_NONE
        ble_config->pairingKey,
        false,
        db_path
    );

    if (error) {
        dbg_printf(LOG, "Error during init %d\r\n", error);
        return;
    }

    error = _ble.securityManager().preserveBondingStateOnReset(true);

    if (error) {
        dbg_printf(LOG, "Error during preserveBondingStateOnReset %d\r\n", error);
    }

#if MBED_CONF_APP_FILESYSTEM_SUPPORT
    /* Enable privacy so we can find the keys */
    error = _ble.gap().enablePrivacy(true);

    if (error) {
        dbg_printf(LOG, "Error enabling privacy\r\n");
    }

    Gap::PeripheralPrivacyConfiguration_t configuration_p = {
        /* use_non_resolvable_random_address */ false,
        Gap::PeripheralPrivacyConfiguration_t::REJECT_NON_RESOLVED_ADDRESS
    };
    _ble.gap().setPeripheralPrivacyConfiguration(&configuration_p);

    Gap::CentralPrivacyConfiguration_t configuration_c = {
        /* use_non_resolvable_random_address */ false,
        Gap::CentralPrivacyConfiguration_t::RESOLVE_AND_FORWARD
    };
    _ble.gap().setCentralPrivacyConfiguration(&configuration_c);

    /* this demo switches between being master and slave */
    _ble.securityManager().setHintFutureRoleReversal(true);
#endif

    /* Tell the security manager to use methods in this class to inform us
     * of any events. Class needs to implement SecurityManagerEventHandler. */
    _ble.securityManager().setSecurityManagerEventHandler(this);

    /* print device address */
    Gap::AddressType_t addr_type;
    Gap::Address_t addr;
    _ble.gap().getAddress(&addr_type, addr);
    dbg_printf(LOG, "Device address: %02x:%02x:%02x:%02x:%02x:%02x\r\n",
           addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);

    /* when scanning we want to connect to a peer device so we need to
     * attach callbacks that are used by Gap to notify us of events */
    _ble.gap().onConnection(this, &SMDevice::on_connect);
    _ble.gap().onDisconnection(this, &SMDevice::on_disconnect);
    _ble.gattServer().onDataWritten(this, &SMDevice::onDataWrittenCallback);
    //_ble.securityManager().onPasskeyDisplay(this, &SMDevice::passkeyDisplayCallback);
    //_ble.securityManager().onSecuritySetupCompleted(this, &SMDevice::securitySetupCompletedCallback);

    /* start test in 500 ms */
    _event_queue.call_in(500, this, &SMDevice::start);
}

/** This is called by Gap to notify the application we connected */
//void SMDevice::on_connect(const Gap::ConnectionCallbackParams_t *connection_event);

/** This is called by Gap to notify the application we disconnected,
 *  in our case it ends the demonstration. */
void SMDevice::on_disconnect(const Gap::DisconnectionCallbackParams_t *event)
{
    dbg_printf(LOG, "Disconnected\r\n");
#ifndef DEMO_BLE_SECURITY
    dbg_printf(LOG, "Restarting advertising...\r\n");
    _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
#endif
    isConnected = false;
    _event_queue.call(this, &SMDevice::setNextCommand, BLE_CMD_DISCONNECT);
}

/** End demonstration unexpectedly. Called if timeout is reached during advertising,
 * scanning or connection initiation */
void SMDevice::on_timeout(const Gap::TimeoutSource_t source)
{
    dbg_printf(LOG, "Unexpected timeout - aborting\r\n");
    _event_queue.break_dispatch();
}

/** Schedule processing of events from the BLE in the event queue. */
void SMDevice::schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context)
{
    _event_queue.call(mbed::callback(&context->ble, &BLE::processEvents));
};

/** Echo received data back                                       */
void SMDevice::EchoBleUartReceived()
{
    uart->writeString(buffer);
    uart->writeString("\n");    //flushes uart output buffer and sends data
}


/** Send data aynchronously using BLE                                       */
void SMDevice::sendBLEUartData(const uint8_t * buf, int len)
{
    //gapState.connected
    if(isConnected){    
        uart->write(buf, len);
        //uart->writeString("\n");    //flushes uart output buffer and sends data
    }
    else
    {
        dbg_printf(LOG, "BLE not connected\r\n");
    }
}


/**
 * This callback allows the UARTService to receive updates.
 *
 * @param[in] params
 *     Information about the characterisitc being updated.
 */
void SMDevice::onDataWrittenCallback(const GattWriteCallbackParams *params) {
    if ((uart != NULL) && (params->handle == uart->getTXCharacteristicHandle())) {
        uint16_t bytesRead = params->len;

        dbg_printf(LOG, "received %u bytes\n\r ", bytesRead);

        if(bytesRead >= 255){
            dbg_printf(LOG, "Overflow command %u n\r ", bytesRead);
            bytesRead = 255;
        }

        unsigned index = 0;
        for (; index < bytesRead; index++) {
            buffer[index] = params->data[index];
        }

        buffer[index++] = 0;
        at_data_resp = new ble_at_msg_t;
        at_data_resp->dataLen = params->len;
        // first set buffer to zeroes
        //memset(at_data_resp->buffer, 0x00, sizeof(at_data_resp->buffer));
        memcpy(at_data_resp->buffer, params->data, params->len);
        at_data_resp->buffer[params->len] = NULL;
        at_data_resp->at_resp = AT_BLE_RESPONSE;
        dbg_printf(LOG, "Data : %s : len = %d",(char *)at_data_resp->buffer, at_data_resp->dataLen);
        dbg_printf(LOG, "\r\n");
        dbg_printf(LOG, "Data : %s : len = %d",buffer, params->len);
        dbg_printf(LOG, "\r\n");
        /* directly queue received data */
        _event_queue.call(this, &SMDevice::sendATresponseBytes, AT_BLE_RESPONSE);
        /* start echo in 50 ms */
        _event_queue.call_in(50, this, &SMDevice::EchoBleUartReceived);
        //_event_queue.call(EchoBleUartReceived);

    }
}



 
/** Blink LED to show we're running */
void SMDevice::blink(void)
{
    _led1 = !_led1;
}


void SMDevice::reportGapState()
{
     //Gap::GapState_t gapState = _ble.gap().getState();
     char connStr[20] = " Not Connected ";
     char advStr[20] = " Not Advertising ";
     //char devName[20] = "";
     //if(gapState.advertising){
     if(_ble.gap().isAdvertisingActive(ble::LEGACY_ADVERTISING_HANDLE)){
         strncpy(advStr, " Advertising ", 20);
     }
     if(isConnected){
         strncpy(connStr, " Connected ", 20);
     }
     dbg_printf(LOG, "\n Advertising Status = %s\n Connection Status = %s\n", advStr, connStr);
     
}


/** A peripheral device will advertise, accept the connection and request
 * a change in link security. */
SMDevicePeripheral::SMDevicePeripheral(BLE &ble, events::EventQueue &event_queue, BLEProtocol::AddressBytes_t &peer_address, 
                                       MemoryPool<at_ble_msg_t, PQDSZ_BLE> *aT2BleDatamPool, 
                                       Queue<at_ble_msg_t, PQDSZ_BLE> *aT2BleDataQueue, 
                                       MemoryPool<ble_at_msg_t, PQDSZ_BLE> *ble2ATDatamPool, 
                                       Queue<ble_at_msg_t, PQDSZ_BLE> *ble2ATDataQueue, 
                                       ble_config_t *ble_config)
        : SMDevice(ble, event_queue, peer_address, 
                   aT2BleDatamPool, aT2BleDataQueue, 
                   ble2ATDatamPool, ble2ATDataQueue, 
                   ble_config) { }

void SMDevicePeripheral::start()
{
    /* Set up and start advertising */

    ble_error_t error;
    GapAdvertisingData advertising_data;

    /* add advertising flags */
    advertising_data.addFlags(GapAdvertisingData::LE_GENERAL_DISCOVERABLE
                              | GapAdvertisingData::BREDR_NOT_SUPPORTED);

    /* add device name */
    advertising_data.addData(
        GapAdvertisingData::COMPLETE_LOCAL_NAME,
        (const uint8_t *)ble_config->deviceName,
        strlen(ble_config->deviceName)
        );
    /* Setup primary service */
    uart = new UARTService(_ble);


    /* add device name */
    error = advertising_data.addData(
        GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS ,
        (const uint8_t *)UARTServiceUUID_reversed,
        sizeof(sizeof(UARTServiceUUID_reversed))
        );
    /* setup advertising */
    //error = _ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    //ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    //error = _ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    /* set up the services that can be discovered */
    //error = _ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,(const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
    

    //error = _ble.gap().setAdvertisingPayload(advertising_data);

    if (error) {
        dbg_printf(LOG, "Error during Gap::setAdvertisingPayload\r\n");
        return;
    }

    /* advertise to everyone */
    _ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    /* how many milliseconds between advertisements, lower interval
     * increases the chances of being seen at the cost of more power */
    //_ble.gap().setAdvertisingInterval(20);
    //_ble.gap().setAdvertisingTimeout(0);
    _ble.gap().setAdvertisingInterval(ble_config->advInterval); /* setting in ble_config */
    _ble.gap().setAdvertisingTimeout(ble_config->advTimeout);   /* setting in ble_config */

    error = _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);

    if (error) {
        dbg_printf(LOG, "Error during Gap::startAdvertising.\r\n");
        return;
    }

    dbg_printf(LOG, "Please connect to device\r\n");

    /** This tells the stack to generate a pairingRequest event
     * which will require this application to respond before pairing
     * can proceed. Setting it to false will automatically accept
     * pairing. */
    _ble.securityManager().setPairingRequestAuthorisation(true);
}

/** This is called by Gap to notify the application we connected,
 *  in our case it immediately requests a change in link security */
void SMDevicePeripheral::on_connect(const Gap::ConnectionCallbackParams_t *connection_event)
{
    ble_error_t error;

    /* remember the device that connects to us now so we can connect to it
     * during the next demonstration */
    memcpy(_peer_address, connection_event->peerAddr, sizeof(_peer_address));

    dbg_printf(LOG, "Connected to: %02x:%02x:%02x:%02x:%02x:%02x\r\n",
            _peer_address[5], _peer_address[4], _peer_address[3],
            _peer_address[2], _peer_address[1], _peer_address[0]);

    /* store the handle for future Security Manager requests */
    _handle = connection_event->handle;

    /* Request a change in link security. This will be done
     * indirectly by asking the master of the connection to
     * change it. Depending on circumstances different actions
     * may be taken by the master which will trigger events
     * which the applications should deal with. */
    error = _ble.securityManager().setLinkSecurity(
        _handle,
        SecurityManager::SECURITY_MODE_ENCRYPTION_NO_MITM
    );

    if (error) {
        dbg_printf(LOG, "Error during SM::setLinkSecurity %d\r\n", error);
        return;
    }
    dbg_printf(LOG, "SM::setLinkSecurity setup\r\n");
    isConnected = true;
    _event_queue.call(this, &SMDevicePeripheral::setNextCommand, BLE_CMD_CONNECT);
}

void SMDevicePeripheral::stopAdvertising()
{
    if (_ble.hasInitialized()) {
        ble_error_t error;
        error = _ble.gap().stopAdvertising(ble::LEGACY_ADVERTISING_HANDLE);;
        if(error){
            dbg_printf(LOG, " Error stopping advertising...\r\n");
            return;
        }
        dbg_printf(LOG, "Stopping advertising...\r\n");
        //_event_queue.break_dispatch();
    }
}
void SMDevicePeripheral::startAdvertising()
{
    if (_ble.hasInitialized()) {
        ble_error_t error;
        error = _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
        if(error){
            dbg_printf(LOG, " Error Restarting advertising...\r\n");
            return;
        }
        dbg_printf(LOG, "Restarting advertising...\r\n");
        //_event_queue.break_dispatch();
    }
}


void SMDevicePeripheral::sendConnectionResponses()
{
    // send ACL Connected +UUBTACLC
    at_data_resp = new ble_at_msg_t;
    char * respStr = (char *)at_data_resp->buffer;
    sprintf(respStr, "\r\n+UUBTACLC:%d,%d,%02X%02X%02X%02X%02X%02X\r\n", 
            DEFAULT_BTLE_CHANNEL, GATT_TYPE,_peer_address[5] ,_peer_address[4],
            _peer_address[3],_peer_address[2],_peer_address[1],_peer_address[0]);
    at_data_resp->dataLen = strlen(respStr); // get bytes total
    sendATresponseBytes(AT_BLE_EVENT);
    
    // send +UUDPC
    at_data_resp = new ble_at_msg_t;
    respStr = (char *)at_data_resp->buffer;
    sprintf(respStr, "\r\n+UUDPC:%d,%d,%d,%02X%02X%02X%02X%02X%02X,%d\r\n", 
            BTLE_PEER_HANDLE, BLE_CONNECTION, BLE_UUID_PROFILE,_peer_address[5] ,
            _peer_address[4], _peer_address[3],_peer_address[2],_peer_address[1],
            _peer_address[0], BLE_FRAME_SIZE);
    at_data_resp->dataLen = strlen(respStr); // get bytes total
    sendATresponseBytes(AT_BLE_EVENT);
    
    // send connect event
    at_data_resp = new ble_at_msg_t;
    at_data_resp->dataLen = 10; // 10 bytes total
    int idx = 0;
    // connect type BLE = 0x01
    at_data_resp->buffer[idx++] = 0x01;
    // Serial Port Service BLE profile = 0x0E (14)
    at_data_resp->buffer[idx++] = 0x0E;
    // copy peer device address
    memcpy(&at_data_resp->buffer[idx], _peer_address, sizeof(_peer_address));
    idx+=sizeof(_peer_address);
    // frame size 0x0166
    at_data_resp->buffer[idx++] = 0x01;
    at_data_resp->buffer[idx++] = 0x66;
    sendATresponseBytes(BLE_CONNECT_EVENT);
}


void SMDevicePeripheral::sendDisconnectionResponses()
{
    // send ACL disconnected +UUBTACLD
    at_data_resp = new ble_at_msg_t;
    char * respStr = (char *)at_data_resp->buffer;
    sprintf(respStr, "\r\n+UUBTACLD:%d\r\n", DEFAULT_BTLE_CHANNEL);
    at_data_resp->dataLen = strlen(respStr); // get bytes total
    sendATresponseBytes(AT_BLE_EVENT);
    
    // send +UUDPD
    at_data_resp = new ble_at_msg_t;
    respStr = (char *)at_data_resp->buffer;
    sprintf(respStr, "\r\n+UUDPD:%d\r\n", BTLE_PEER_HANDLE);
    at_data_resp->dataLen = strlen(respStr); // get bytes total
    sendATresponseBytes(AT_BLE_EVENT);
    
    // send connect event
    at_data_resp = new ble_at_msg_t;
    sendATresponseBytes(BLE_DISCONNECT_EVENT);
}



void SMDevicePeripheral::processQueues()
{
    dequeueATdataResponse();
    switch(bleCmd)
    {
    case BLE_CMD_NONE:
        break;
    case BLE_CMD_CONFIG:
        _aT2BleDatamPool->free(data_msg);
        bleCmd = BLE_CMD_NONE;
        break;
    case BLE_CMD_CONNECT:
        sendConnectionResponses();
        bleCmd = BLE_CMD_NONE;
        break;
    case BLE_CMD_DISCONNECT:
    {
        sendDisconnectionResponses();
        bleCmd = BLE_CMD_NONE;
        break;
    }
    case BLE_CMD_MAC_ADDR:
    {
        // send BLE MAC_ADDR
        _aT2BleDatamPool->free(data_msg);
        at_data_resp = new ble_at_msg_t;
        char * respStr = (char *)at_data_resp->buffer;
        /* print device address */
        Gap::AddressType_t addr_type;
        Gap::Address_t addr;
        _ble.gap().getAddress(&addr_type, addr);
        //dbg_printf(LOG, "Device address: %02x:%02x:%02x:%02x:%02x:%02x\r\n",
        //       addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
        sprintf(respStr, "\r\n%s%d,%02X%02X%02X%02X%02X%02XOK\r\n", LOCAL_ADDRESS_RESP
                                                                , BLE_IF_ID
                                                                , addr[5]
                                                                , addr[4]
                                                                , addr[3]
                                                                , addr[2]
                                                                , addr[1]
                                                                , addr[0]
                                                                );
        at_data_resp->dataLen = strlen(respStr); // get bytes total
        sendATresponseBytes(AT_BLE_MAC_RESP);
        bleCmd = BLE_CMD_NONE;
        break;
    }
    case BLE_CMD_DEVICE_NAME:
    {
        // send BLE_CMD_DEVICE_NAME
        _aT2BleDatamPool->free(data_msg);
        at_data_resp = new ble_at_msg_t;
        char * respStr = (char *)at_data_resp->buffer;
        sprintf(respStr, "\r\n%s%sOK\r\n", BLE_DEVICE_NAME_RESP, ble_config->deviceName);
        at_data_resp->dataLen = strlen(respStr); // get bytes total
        sendATresponseBytes(AT_BLE_NAME_RESP);
        bleCmd = BLE_CMD_NONE;
        break;
    }
    case BLE_CMD_SEND_RX_DATA_2AT:
        sendATresponseBytes(AT_BLE_RESPONSE);
        bleCmd = BLE_CMD_NONE;
        break;
    case BLE_CMD_SEND_AT_DATA_2BLE:
        sendBLEUartData(data_msg->buffer, data_msg->dataLen);
        _aT2BleDatamPool->free(data_msg);
        bleCmd = BLE_CMD_NONE;
        break;
    default:
        _aT2BleDatamPool->free(data_msg);
        bleCmd = BLE_CMD_NONE;
        break;
    }
    
}


bool SMDevicePeripheral::queueBleDataResponse(ble_at_msg_t at_resp)
{
    ble_at_msg_t *atData = _ble2ATDatamPool->alloc();
    if(atData == NULL) return false; // queue full;
    atData->at_resp        = at_resp.at_resp;
    atData->dataLen        = at_resp.dataLen;
    memcpy(atData->buffer, at_resp.buffer, at_resp.dataLen);
    _ble2ATDataQueue->put(atData);
    dbg_printf(LOG, "[BLE-MAN] queued data size = %d : at_resp = %d\n", at_resp.dataLen, at_resp.at_resp);
    return true;
}


bool SMDevicePeripheral::dequeueATdataResponse(){
    if(bleCmd != BLE_CMD_NONE) return false; // busy
    osEvent evt = _aT2BleDataQueue->get(0);
    if(evt.status == osEventMessage){
        data_msg = (at_ble_msg_t*)evt.value.p;
        setNextCommand(data_msg->ble_cmd);
        //_wiFi2ATDatamPool->free(data_msg);
    }
    return true;
}

void  SMDevicePeripheral::sendATresponseBytes(at_cmd_resp_t at_cmd)
{
    //at_data_resp = new ble_at_msg_t;
    // package and send on BLE data queue    
    // set string length 
    //at_data_resp->dataLen = len;
    // copy data
    //memcpy(at_data_resp->buffer, buf, len);
    // copy response type
    at_data_resp->at_resp = at_cmd;
    bool queueResult = true;
    int wait_count = 0;
    queueResult = queueBleDataResponse(*at_data_resp);
    delete at_data_resp;
    at_data_resp = NULL;
    dbg_printf(LOG, "[BLE-MAN] sendATresponseBytes completed successfully\r\n");
}                                            

bool SMDevicePeripheral::setNextCommand(ble_cmd_t cmd)
{
    dbg_printf(LOG, "\n [BLE-MAN] About to set next BLE manager command to %d\n", cmd);
    if(bleCmd == BLE_CMD_NONE){
        bleCmd = cmd;
        return true; // success
    }
    dbg_printf(LOG, "\n [BLE-MAN] Busy : current state = %d \n", bleCmd);
    return false; // BleManager busy
}

/** A central device will scan, connect to a peer and request pairing. */

SMDeviceCentral::SMDeviceCentral(BLE &ble, events::EventQueue &event_queue, BLEProtocol::AddressBytes_t &peer_address, 
                                 MemoryPool<at_ble_msg_t, PQDSZ_BLE> *aT2BleDatamPool, 
                                 Queue<at_ble_msg_t, PQDSZ_BLE> *aT2BleDataQueue, 
                                 MemoryPool<ble_at_msg_t, PQDSZ_BLE> *ble2ATDatamPool, 
                                 Queue<ble_at_msg_t, PQDSZ_BLE> *ble2ATDataQueue, 
                                 ble_config_t *ble_config)
    : SMDevice(ble, event_queue, peer_address, 
               aT2BleDatamPool, aT2BleDataQueue, 
               ble2ATDatamPool, ble2ATDataQueue, 
               ble_config) { };

void SMDeviceCentral::start()
{
    /* start scanning and attach a callback that will handle advertisements
     * and scan requests responses */
    ble_error_t error = _ble.gap().startScan(this, &SMDeviceCentral::on_scan);

    dbg_printf(LOG, "Please advertise\r\n");

    dbg_printf(LOG, "Scanning for: %02x:%02x:%02x:%02x:%02x:%02x\r\n",
           _peer_address[5], _peer_address[4], _peer_address[3],
           _peer_address[2], _peer_address[1], _peer_address[0]);

    if (error) {
        dbg_printf(LOG, "Error during Gap::startScan %d\r\n", error);
        return;
    }
}

/** Look at scan payload to find a peer device and connect to it */
void SMDeviceCentral::on_scan(const Gap::AdvertisementCallbackParams_t *params)
{
    /* don't bother with analysing scan result if we're already connecting */
    if (_is_connecting) {
        return;
    }

    /* connect to the same device that connected to us */
    if (memcmp(params->peerAddr, _peer_address, sizeof(_peer_address)) == 0) {

        ble_error_t error = _ble.gap().connect(
            params->peerAddr, params->peerAddrType,
            NULL, NULL
        );

        if (error) {
            dbg_printf(LOG, "Error during Gap::connect %d\r\n", error);
            return;
        }

        dbg_printf(LOG, "Connecting... ");

        /* we may have already scan events waiting
         * to be processed so we need to remember
         * that we are already connecting and ignore them */
        _is_connecting = true;

        return;
    }
}

/** This is called by Gap to notify the application we connected,
 *  in our case it immediately request pairing */
void SMDeviceCentral::on_connect(const Gap::ConnectionCallbackParams_t *connection_event)
{
    ble_error_t error;

    /* store the handle for future Security Manager requests */
    _handle = connection_event->handle;

    /* in this example the local device is the master so we request pairing */
    error = _ble.securityManager().requestPairing(_handle);

    dbg_printf(LOG, "Connected\r\n");

    if (error) {
        dbg_printf(LOG, "Error during SM::requestPairing %d\r\n", error);
        return;
    }

    /* upon pairing success the application will disconnect */
}



#if MBED_CONF_APP_FILESYSTEM_SUPPORT
bool create_filesystem()
{
    static LittleFileSystem fs("fs");

    /* replace this with any physical block device your board supports (like an SD card) */
    static HeapBlockDevice bd(4096, 256);

    int err = bd.init();

    if (err) {
        return false;
    }

    err = bd.erase(0, bd.size());

    if (err) {
        return false;
    }

    err = fs.mount(&bd);

    if (err) {
        /* Reformat if we can't mount the filesystem */
        dbg_printf(LOG, "No filesystem found, formatting...\r\n");

        err = fs.reformat(&bd);

        if (err) {
            return false;
        }
    }

    return true;
}
#endif //MBED_CONF_APP_FILESYSTEM_SUPPORT
#ifdef BLE_SECURITY_MAIN
int main()
{
    BLE& ble = BLE::Instance();
    events::EventQueue queue;

#if MBED_CONF_APP_FILESYSTEM_SUPPORT
    /* if filesystem creation fails or there is no filesystem the security manager
     * will fallback to storing the security database in memory */
    if (!create_filesystem()) {
        dbg_printf(LOG, "Filesystem creation failed, will use memory storage\r\n");
    }
#endif

    while(1) {
        {
            dbg_printf(LOG, "\r\n PERIPHERAL \r\n\r\n");
            SMDevicePeripheral peripheral(ble, queue, peer_address);
            peripheral.run();
        }

        {
            dbg_printf(LOG, "\r\n CENTRAL \r\n\r\n");
            SMDeviceCentral central(ble, queue, peer_address);
            central.run();
        }
    }

    return 0;
}
#endif