File content as of revision 11:dbc310addbf6:

#include "GAPPeripheral.h"

// constructor
GAPPeripheral::GAPPeripheral(BLE& ble, 
                             IDevKit& devKit, 
                             const string& deviceName, 
                             GapAdvertisingParams::AdvertisingType_t advType,
                             events::EventQueue& eventQueue,
                             Logger& logger) 
: m_devKit(devKit),
  m_ble(ble),
  m_deviceName(deviceName),
  m_advType(advType),
  m_eventQueue(eventQueue),
  m_logger(logger) {
}

GAPPeripheral::~GAPPeripheral() {
  if (m_ble.hasInitialized()) {
    m_ble.shutdown();
  }
}

void GAPPeripheral::advertise(void) {
  if (m_ble.hasInitialized()) {
    m_logger.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
  m_ble.onEventsToProcess(makeFunctionPointer(this, &GAPPeripheral::scheduleBleEvents));

  // handle timeouts, for example when connection attempts fail
  m_ble.gap().onTimeout(makeFunctionPointer(this, &GAPPeripheral::onTimeOut));

  // set this instance as the event handler for all gap events
  m_ble.gap().setEventHandler(this);
        
  // initialize the ble component
  ble_error_t error = m_ble.init(this, &GAPPeripheral::onInitComplete);
  if (error) {
    m_logger.log("Error returned by BLE::init: %d\r\n", error);
    return;
  }
  
  // to show we're running we'll blink
  m_eventQueue.call_every(BLINK_INTERVAL, &m_devKit, &IDevKit::toggleLed1);

  // this will not return until shutdown
  m_eventQueue.dispatch_forever(); 
}
 
// private methods
void GAPPeripheral::scheduleBleEvents(BLE::OnEventsToProcessCallbackContext *context) {
  m_eventQueue.call(mbed::callback(&context->ble, &BLE::processEvents));
}

void GAPPeripheral::onTimeOut(const Gap::TimeoutSource_t source) {
  switch (source) {
    case Gap::TIMEOUT_SRC_ADVERTISING:
      m_logger.log("Stopped advertising early due to timeout parameter\r\n");
      break;
    case Gap::TIMEOUT_SRC_SCAN:
      m_logger.log("Stopped scanning early due to timeout parameter\r\n");
      break;
    case Gap::TIMEOUT_SRC_CONN:
      m_logger.log("Failed to connect\r\n");
      break;
    default:
      m_logger.log("Unexpected timeout\r\n");
      break;
  }
}

void GAPPeripheral::onInitComplete(BLE::InitializationCompleteCallbackContext *event) {
  if (event->error) {
    m_logger.log("Error during the initialisation\r\n");
    return;
  }

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

  // setup the callback to be called upon connection
  m_ble.gap().onConnection(this, &GAPPeripheral::onConnect);
  
  // all calls are serialised on the user thread through the event queue
  // so schedule start of advertising in the same way
  m_eventQueue.call(this, &GAPPeripheral::startAdvertising);
};

void GAPPeripheral::onConnect(const Gap::ConnectionCallbackParams_t *connection_event) {
  m_logger.log("Connection request received\r\n"); 
}    

void GAPPeripheral::startAdvertising(void) {
  // add advertising flags
  GapAdvertisingData advertisingData;
  advertisingData.addFlags(GapAdvertisingData::LE_GENERAL_DISCOVERABLE
                           | GapAdvertisingData::BREDR_NOT_SUPPORTED);

  // add device name
  advertisingData.addData(GapAdvertisingData::COMPLETE_LOCAL_NAME,
                          (const uint8_t*) m_deviceName.c_str(),
                          m_deviceName.length());

  ble_error_t error = m_ble.gap().setAdvertisingPayload(advertisingData);
  if (error) {
    m_logger.log("Error during Gap::setAdvertisingPayload\r\n");
    return;
  }

  // how many milliseconds between advertisements, lower interval
  // increases the chances of being seen at the cost of more power
  uint16_t interval = 100;

  // advertising will continue for this many seconds or until connected
  uint16_t timeout = 3600;
  
  m_ble.gap().setAdvertisingType(m_advType);
  m_ble.gap().setAdvertisingInterval(interval);
  m_ble.gap().setAdvertisingTimeout(timeout);

  error = m_ble.gap().startAdvertising();
  if (error) {
    m_logger.log("Error during Gap::startAdvertising.\r\n");
    return;
  }

  m_logger.log("Advertising started (type: 0x%x, interval: %dms, timeout: %ds)\r\n",
               m_advType, interval, timeout);
}

void GAPPeripheral::setAdvertisementServiceData(ServiceDataPayload* serviceDataPayloadArray,
                                                uint8_t nbrOfServices) {
  // first clear advertising payload
  m_ble.gap().clearAdvertisingPayload();

  // set the flags
  m_ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
  
  // set the complete local name
  m_ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (const uint8_t*) m_deviceName.c_str(), m_deviceName.length());
  
  // set the list of 16 service uuids
  uint16_t serviceUUIDArraySize = nbrOfServices * sizeof(uint16_t);
  uint8_t* serviceUUIDArray = new uint8_t[serviceUUIDArraySize];
  for (uint8_t i = 0; i < nbrOfServices; i++) {
    uint16_encode(serviceDataPayloadArray[i].serviceUUID, serviceUUIDArray + i * sizeof(uint16_t));
  }
  m_ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, serviceUUIDArray, serviceUUIDArraySize);
  
  // set the service data payload - each payload is prepended with the service UUID
  for (uint8_t i = 0; i < nbrOfServices; i++) {
    uint16_t prependedServiceDataLength = serviceDataPayloadArray[i].serviceDataLength + 2;
    uint8_t* prependedServiceData = new uint8_t[prependedServiceDataLength];
    uint16_encode(serviceDataPayloadArray[i].serviceUUID, prependedServiceData);
    
    memcpy(prependedServiceData + 2, serviceDataPayloadArray[i].serviceData, serviceDataPayloadArray[i].serviceDataLength);
    
    m_ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SERVICE_DATA, prependedServiceData, prependedServiceDataLength);
    
    delete prependedServiceData;
    prependedServiceData = NULL;
  }  
}

void GAPPeripheral::uint16_encode(const uint16_t value, uint8_t* p_encoded_data) {
  p_encoded_data[0] = (uint8_t) ((value & 0x00FF) >> 0);
  p_encoded_data[1] = (uint8_t) ((value & 0xFF00) >> 8);
}

void GAPPeripheral::int16_encode(const int16_t value, uint8_t* p_encoded_data) {
  uint16_t tmp = value;
  uint16_encode(tmp, p_encoded_data);
}

void GAPPeripheral::uint32_encode(const uint32_t value, uint8_t* p_encoded_data) {
  p_encoded_data[0] = (uint8_t) ((value & 0x000000FF) >> 0);
  p_encoded_data[1] = (uint8_t) ((value & 0x0000FF00) >> 8);
  p_encoded_data[2] = (uint8_t) ((value & 0x00FF0000) >> 16);
  p_encoded_data[3] = (uint8_t) ((value & 0xFF000000) >> 24);
}