nRF52840 unexpectedly stops scanning

[Sandrine]

Hello,

I have a running system constituted of a central and 15 peripherals using nRF52840 devices. The central is set to scan the peripherals for 5 seconds then stop and restart the scanning 30 seconds after and so on. After 5 seconds, the scan callback should receive a "NRF_BLE_SCAN_EVT_SCAN_TIMEOUT" event from the softdevice which should trigger a scan completed event in my code. However, after I let the system run for multiple hours, randomly at some point (sometimes after 5 hours, 12 hours, 1 hour), the scan callback is not called at all at the end of the 5 seconds and I don't understand why. I have tried to stop an on-going scan when the system wants to start scanning again for peripherals with the previous scanning sequence not officially completed. Unfortunately the software is not able to get out of the previous uncompleted scanning and is not able to perform device discoveries anymore even if I stopped the previous scan. Can somebody help me with this problem? Thanks.

[Vidar Berg]

Hello, 

I'm afraid can't think of any obvious reasons for this behavior. Are you able to share your project or relevant pieces of your code so I can try to review it here? I can make the ticket private if needed. 

After 5 seconds, the scan callback should receive a "NRF_BLE_SCAN_EVT_SCAN_TIMEOUT" event from the softdevice which should trigger a scan completed event in my code.

 Will the timer handler start scanning every 30 seconds even if you don't get this event. Also, does your code check the return value from "scan start"? 

Unfortunately the software is not able to get out of the previous uncompleted scanning and is not able to perform device discoveries anymore even if I stopped the previous scan. Can somebody help me with this problem? Thanks.

Is it possible that the program may have become "stuck" in the app error handler or the hardfault handler? Maybe you can attach a debugger to the device when this happens and read out the program counter value to check if this may be the case. You can use the nrfjprog command '--readregs' command for reading the core registers.

[Sandrine]

Hello,

Thanks for your answer.

Indeed, here is part of the ble_task:

#define BLE_TASK_STACK_SIZE           512u
#define BLE_TASK_QUEUE_SIZE           200u
#define BLE_IDLE_TIMEOUT              120000 // 2 minutes idle timeout

int BleTask_Init(void)
{
  int err;

  // Initialzie and start BLE driver
  err = BleDriver_Init(BLE_DEVICE_NAME_TRANSCEIVER);

  DBGPRINTF("BleDriver_init: %d", err);

  TimerCreateOneShot(&idleTimer, BLE_IDLE_TIMEOUT, (TimerCallbackT)IdleTimerCallback);
  TimerCreatePeriodic( &bleSweepTestTimer, BLE_SWEEP_INTERVAL_MS, (TimerCallbackT)BleSweepTestCallback );

  BleDriver_SetCallback( BleCallback, NULL );

  QueueCreate(&bleTaskMessageQueue, /* address of the message queue */
              BLE_TASK_QUEUE_SIZE); /* number of messages allowed */

  TaskCreate( &bleTask,             /* address of task control block */
              "bleTask",            /* Task name */
              &BleTask_Run,         /* Task "Run" function */
              NULL,
              bleTask_Stk,          /* base of the stack */
              BLE_TASK_STACK_SIZE,  /* stack size in TaskT */
              BLE_TASK_PRIORITY);   /* priority */

  SemaphoreCreate( &sema );

  return 0;
}

QueueT* BleTask_GetMessageQueue(void)
{
  return &bleTaskMessageQueue;
}

static void BleTask_Run(void* pArg)
{
  UNUSED(pArg);

  while (1)
  {
    // Run task core
    BleTask_RunCore();
  }
}

static void StartAdvertising(void)
{
  // Check if we need to stop current advertising
  if( receivedAdvertisingInfo == true )
  {
    receivedAdvertisingInfo = false;
    if( advertisingStarted == true )
    {
      BlePeripheralDriver_StopAdvertising();
      int err = BlePeripheralDriver_StopAdvertising();
      if (err < 0)
      {
         DBGPRINTF("BLE Task - Stopping Advertizing in StartAdvertizing failed with err %d", err);
      }
      advertisingStarted = false;
    }
  }

  // Check if we can start/re-start advertising
  if( (advertisingStarted == false) && (isBlePeripheralEnabled == true) )
  {
    int err;
    // Start advertising
    err = BlePeripheralDriver_StartAdvertising((void*)&manufacturerData, sizeof(manufacturerData));

    if ( err == 0 )
    {
      // Mark advertising as started
      advertisingStarted = true;
    }
    else
    {
      DBGPRINTF("BLE Task - Start Advertizing failed with err %d", err);
    }
  }
}

void BleTask_RunCore(void)
{
  int msgSize = 0;
  InterTaskMsg* msg = 0;

  // Wait for message
  msg = (InterTaskMsg*)QueueWait(&bleTaskMessageQueue, &msgSize, true, 0);

  // Process inter task message
  if( msg != 0 )
  {
    HandleInterTaskMsg( msg );
    MsgFree( &msg->base );
  }

  // Start advertising if enabled.
  StartAdvertising();

}

static void HandleInterTaskMsg( InterTaskMsg* msg )
{
  switch( msg->base.id )
  {
    case INTER_TASK_BLE_START_SCAN_ID:
      HandleScan( msg );
    break;

    case INTER_TASK_BLE_CONNECT_REQUEST_ID:
      HandleConnect( msg );
    break;

    case INTER_TASK_BLE_DISCONNECT_REQUEST_ID:
      HandleDisconnect( msg );
    break;

    case INTER_TASK_BLE_TO_PERIPHERAL_DATA_ID:
      HandleSendDataToPeripheralRequest( msg );
    break;

    case INTER_TASK_BLE_TO_CENTRAL_DATA_ID:
      HandleSendDataToCentralRequest( msg );
    break;

    case INTER_TASK_BLE_START_ADVERTISING_ID:
      HandleStartAdvertisingRequest(msg);
    break;

    case INTER_TASK_BLE_STOP_ADVERTISING_ID:
      HandleStopAdvertisingRequest(msg);
    break;

    case INTER_TASK_BLE_TRANSCEIVER_CHARACTERISTICS_ID:
      HandleBleTransceiverCharacteristics(msg);
    break;

    case INTER_TASK_BLE_DISABLE_ID:
    {
      // NOTE: Once bLE is disabled, there is no way to re-enable it
      isBleEnabled = false;
    }
    break;


    case INTER_TASK_BLE_TEST_STOP_ID:
    {
      HandleBleTestStop();
    }
    break;


    default:
      DBGPRINTF("BLE Task - Unknown message ID");
    break;
  }
}

static void BleCallback(enum BleEventId eventId, uint8_t* peerAddress, void* eventData, void* arg)
{
  UNUSED( arg );

  switch( eventId )
  {
    case BLE_CONNECTION_EVENT:
    {
      ProcessConnectionEvent( peerAddress, eventData );
    }
    break;

    case BLE_SCAN_COMPLETED_EVENT:
    {
      ProcessScanCompleteEvent();
    }
    break;

    case BLE_DATA_LINK_READY_EVENT:
    {
      ProcessDataLinkReadyEvent( peerAddress );
    }
    break;

    case BLE_DATA_RECEIVED_EVENT:
    {
      ProcessDataReceivedEvent( peerAddress, eventData );
    }
    break;

    case BLE_DATA_SENT_EVENT:
    {
      ProcessDataSentEvent();
    }
    break;

    case BLE_DEVICE_DISCOVERY_EVENT:
    {
        if( eventData != NULL )
        {
          ProcessDiscoveryEvent( peerAddress, eventData );
        }
    }
    break;

    case BLE_TCVR_CHAR_WRITE_EVENT:
    {
      if (eventData != NULL)
      {
        ProcessTcvrCharWriteEvent((BleTcvrCharacteristics*)eventData);
      }
    }
  }
}

static void HandleScan( InterTaskMsg* msg )
{
  UNUSED( msg );
  DBGPRINTF( "BLE Scan Starting\r\n" );
  int err = BleCentralDriver_StartScan(BLE_DEVICE_NAME_BELTPACK);
  if(err != 0)
  {
    DBGPRINTF( "BLE Scan FAILED with error : %d\r\n", err );
  }
}

static void HandleConnect( InterTaskMsg* msg )
{
  int err = BleCentralDriver_Connect(&msg->payload.peerAddress[0]);
  if( err != 0 )
  {
    isCentralConnected = false;
    DBGPRINTF( "BLE Error connecting, error %d \r\n", err );
  }
  else
  {
    isCentralConnected = true;
    memcpy(&centralConnectionAddress, &msg->payload.peerAddress[0], sizeof(blePeerAddress_t));
  }
}

static void HandleDisconnect( InterTaskMsg* msg )
{
  int err = BleCentralDriver_Disconnect(&msg->payload.peerAddress[0]) ;
  if( err != 0 )
  {
    DBGPRINTF( "BLE Error disconnecting error %d\r\n", err );
  }
  if( memcmp( &msg->payload.peerAddress[0], &centralConnectionAddress[0], sizeof(blePeerAddress_t) ) == 0 )
  {
    // Now disconnected from the previous connection
    isCentralConnected = false;
  }
}

static void HandleSendDataToPeripheralRequest( InterTaskMsg* msg )
{
  if( msg->payload.bleData.dataLength <= CONTROL_BLE_MAX_PAYLOAD_LEN )
  {
    while(BleCentralDriver_SendData(&msg->payload.bleData.peerAddress[0], &msg->payload.bleData.data[0], msg->payload.bleData.dataLength) == -EBUSY)
    {
      txBusy = true;
      if( SemaphoreWait( &sema, true, 5000 ) == -ETIMEDOUT )
      {
        txBusy = false;
        DBGPRINTF("BLE Task - Timeout sending data\r\n");
        int err = BleCentralDriver_Disconnect(&msg->payload.peerAddress[0]);
        if (err != 0)
        {
          DBGPRINTF("BLE Task - FAILED TO DISCONNECT HandleSendDataToCentralRequest %d", err);
        }
        break;
      }
    }
  }
  else
  {
    DBGPRINTF( "BLE Task - Invalid payload length: %u\r\n", msg->payload.bleData.dataLength );
  }
}

static void HandleSendDataToCentralRequest( InterTaskMsg* msg )
{
  if( dataLinkReady == true )
  {
    if( msg->payload.bleData.dataLength <= CONTROL_BLE_MAX_PAYLOAD_LEN )
    {
      while(BlePeripheralDriver_SendData(&msg->payload.bleData.data[0], msg->payload.bleData.dataLength) == -EBUSY)
      {
        txBusy = true;
        if( SemaphoreWait( &sema, true, 5000 ) == -ETIMEDOUT )
        {
          isConnected = false;
          dataLinkReady = false;
          txBusy = false;
          DBGPRINTF("BLE Task - Timeout sending data\r\n");
          int err = BlePeripheralDriver_Disconnect();
          if (err < 0)
          {
            DBGPRINTF("BLE Task - FAILED TO DISCONNECT HandleSendDataToCentralRequest");
          }
          break;
        }
      }
    }
    else
    {
      DBGPRINTF( "BLE Task - Invalid payload length: %u\r\n", msg->payload.bleData.dataLength );
    }
  }
}

static void HandleStartAdvertisingRequest( InterTaskMsg* msgs )
{
  DBGPRINTF("BLE Processing BLE Start Advertising");
  isBlePeripheralEnabled = true;

  manufacturerData.systemId = msgs->payload.advertising.systemId;
  memcpy(manufacturerData.systemName, msgs->payload.advertising.systemName, CONTROL_MAX_SYSTEM_NAME);
  manufacturerData.standalone = msgs->payload.advertising.standalone;
  manufacturerData.openForRegistration = msgs->payload.advertising.openForRegistration;

  receivedAdvertisingInfo = true;

}

static void HandleStopAdvertisingRequest( InterTaskMsg* msgs )
{
  DBGPRINTF("BLE Processing BLE Stop Advertising");
  isBlePeripheralEnabled = false;

  if (advertisingStarted == true)
  {
    BlePeripheralDriver_StopAdvertising();
    advertisingStarted = false;
  }
}

static void HandleBleTransceiverCharacteristics( InterTaskMsg* msg )
{
  int result;
  BleTcvrCharacteristics tcvrChars;
  if (!tcvrServiceStarted)
  {
    result = BlePeripheralDriver_StartTcvrService();
    if (result != 0)
    {
      SCtrlInterfaceTask_Trace("BlePeripheralDriver_StartTcvrService failed %d", result);
    }
    else
    {
      tcvrServiceStarted = true;
    }
  }

  tcvrChars.presenceFlags = msg->payload.bleTcvrCharacteristics.presenceFlags;
  tcvrChars.channel1 = msg->payload.bleTcvrCharacteristics.channel1;
  tcvrChars.channel2 = msg->payload.bleTcvrCharacteristics.channel2;
  tcvrChars.ipAddress = msg->payload.bleTcvrCharacteristics.ipAddress;
  memcpy(tcvrChars.name, msg->payload.bleTcvrCharacteristics.name, sizeof(tcvrChars.name));
  result = BlePeripheralDriver_UpdateTcvrCharacteristics(&tcvrChars);
  if (result != 0)
  {
    SCtrlInterfaceTask_Trace("BlePeripheralDriver_UpdateTcvrCharacteristics failed %d", result);
  }
}


static void ProcessDiscoveryEvent( uint8_t* peerAddress, struct BleDeviceDiscoveryData* discoveryEventData )
{
  if( (peerAddress != NULL) && (discoveryEventData != NULL) )
  {
    if(!discoveryEventData->manufacturerData.beltpack.upgradeRefused)
    {
      InterTaskMsg* pInterTaskMsg = (InterTaskMsg*)MsgAlloc( sizeof(InterTaskMsg) );
      if( pInterTaskMsg != NULL )
      {
        pInterTaskMsg->base.id = INTER_TASK_BLE_DEVICE_DISCOVERY_REPORT_ID;
        pInterTaskMsg->payload.discoveryReport.rssi = discoveryEventData->rssi;
        memcpy( &pInterTaskMsg->payload.discoveryReport.peerAddress[0], peerAddress, CONTROL_BLE_MAX_PAYLOAD_LEN );
        pInterTaskMsg->payload.discoveryReport.serialNumber = discoveryEventData->manufacturerData.beltpack.serialNumber;
        pInterTaskMsg->payload.discoveryReport.systemId = discoveryEventData->manufacturerData.beltpack.systemId;
        pInterTaskMsg->payload.discoveryReport.versionMajor = discoveryEventData->manufacturerData.beltpack.versionMajor;
        pInterTaskMsg->payload.discoveryReport.versionMinor = discoveryEventData->manufacturerData.beltpack.versionMinor;
        pInterTaskMsg->payload.discoveryReport.versionRev = discoveryEventData->manufacturerData.beltpack.versionRev;

        DBGPRINTF( "Device Discovery: %d %04x%08x %08X %08X %d %d %d\r\n",
            discoveryEventData->rssi,
            *((uint16_t*)(&peerAddress[4])),
            *((uint32_t*)&peerAddress[0]),
            discoveryEventData->manufacturerData.beltpack.serialNumber,
            discoveryEventData->manufacturerData.beltpack.systemId,
            discoveryEventData->manufacturerData.beltpack.versionMajor,
            discoveryEventData->manufacturerData.beltpack.versionMinor,
            discoveryEventData->manufacturerData.beltpack.versionRev
        );

        if( !QueuePost(SCtrlInterfaceTask_GetMessageQueue(), pInterTaskMsg, sizeof(*pInterTaskMsg)) )
        {
          MsgFree( &pInterTaskMsg->base );
          DBGPRINTF( "BLE Task : Error, Unable to post on Slave task\r\n" );
        }
      }
    }
    else
    {
      DBGPRINTF( "Upgrade refuse on discovered device: %d %04x%08x %08X %08X %d %d %d\r\n",
          discoveryEventData->rssi,
          *((uint16_t*)(&peerAddress[4])),
          *((uint32_t*)&peerAddress[0]),
          discoveryEventData->manufacturerData.beltpack.serialNumber,
          discoveryEventData->manufacturerData.beltpack.systemId,
          discoveryEventData->manufacturerData.beltpack.versionMajor,
          discoveryEventData->manufacturerData.beltpack.versionMinor,
          discoveryEventData->manufacturerData.beltpack.versionRev
      );
    }
  }
}

static void ProcessScanCompleteEvent( void )
{
  InterTaskMsg* pInterTaskMsg = (InterTaskMsg*)MsgAlloc( sizeof(InterTaskMsg) );
  if( pInterTaskMsg != NULL )
  {
    // Fill the message
    pInterTaskMsg->base.id = INTER_TASK_BLE_SCAN_COMPLETED_EVENT_ID;

    // Post the message
    if( !QueuePost(SCtrlInterfaceTask_GetMessageQueue(), pInterTaskMsg, sizeof(*pInterTaskMsg)) )
    {
      MsgFree( &pInterTaskMsg->base );
      DBGPRINTF( "BLE Task : Error, Unable to post on Slave task\r\n" );
      return;
    }

    DBGPRINTF( "BLE Scan Completed\r\n" );
  }
}

static void ProcessConnectionEvent( uint8_t* peerAddress, struct BleConnectionEventData* connectionEventData )
{
  if ( connectionEventData != NULL )
  {
    InterTaskMsg* pInterTaskMsg = (InterTaskMsg*)MsgAlloc( sizeof(InterTaskMsg) );
    if( pInterTaskMsg != NULL )
    {
      if (peerAddress == NULL)
      {
        // We are a peripheral connected to a central device.
        if( connectionEventData->connected == true )
        {
          TimerStart(&idleTimer);
          isConnected = true;
          advertisingStarted = false;
          int err = BlePeripheralDriver_StopAdvertising();
          if (err < 0)
          {
              DBGPRINTF("BLE Task - Stopping Advertizing in HandleStopAdvertisingRequest failed with err %d", err);
          }
          else
          {
            DBGPRINTF("BLE Connected");
          }
        }
        else
        {
          TimerStop(&idleTimer);
          isConnected = false;
          dataLinkReady = false;
          DBGPRINTF("BLE Disconnected");

          // Re-start advertising
          StartAdvertising();
        }
        memset( &pInterTaskMsg->payload.peerAddress[0], 0, BLE_GAP_ADDR_LEN );
      }
      else
      {
        if( connectionEventData->connected == true )
        {
          DBGPRINTF( "BLE Connected to: %04x%08x\r\n", *((uint16_t*)(&peerAddress[4])),*((uint32_t*)&peerAddress[0]) );
        }
        else
        {
          DBGPRINTF( "BLE Disconnected from: %04x%08x\r\n", *((uint16_t*)(&peerAddress[4])),*((uint32_t*)&peerAddress[0]) );
        }
        memcpy( &pInterTaskMsg->payload.peerAddress[0], peerAddress, BLE_GAP_ADDR_LEN );
      }

       // Fill the message
      if( connectionEventData->connected == true )
      {
        pInterTaskMsg->base.id = INTER_TASK_BLE_DEVICE_CONNECTED_EVENT_ID;
        DBGPRINTF( "BLE Task : Sending  INTER_TASK_BLE_DEVICE_CONNECTED_EVENT_ID\r\n" );
      }
      else
      {
        pInterTaskMsg->base.id = INTER_TASK_BLE_DEVICE_DISCONNECTED_EVENT_ID;
        DBGPRINTF( "BLE Task : Sending  INTER_TASK_BLE_DEVICE_DISCONNECTED_EVENT_ID\r\n" );

        // If we get disconnected during a transfer we need to signal the semaphore
        if( txBusy == true )
        {
          txBusy = false;
          SemaphoreSignal( &sema );
        }
      }

      // Post the message
      if( !QueuePost(SCtrlInterfaceTask_GetMessageQueue(), pInterTaskMsg, sizeof(*pInterTaskMsg)) )
      {
        MsgFree( &pInterTaskMsg->base );
        DBGPRINTF( "BLE Task : Error, Unable to post on Slave task\r\n" );
      }
    }
  }
}

static void ProcessDataLinkReadyEvent( uint8_t* peerAddress )
{
  InterTaskMsg* pInterTaskMsg = (InterTaskMsg*)MsgAlloc( sizeof(InterTaskMsg) );
  if( pInterTaskMsg != NULL )
  {
    // Fill the message
    pInterTaskMsg->base.id = INTER_TASK_BLE_DEVICE_DATA_LINK_READY_EVENT_ID;
    if (peerAddress == NULL)
    {
      TimerStart(&idleTimer);
      dataLinkReady = true;
      memset( &pInterTaskMsg->payload.peerAddress[0], 0, BLE_GAP_ADDR_LEN );
      DBGPRINTF( "BLE Data link ready with central");
    }
    else
    {
      memcpy( &pInterTaskMsg->payload.peerAddress[0], peerAddress, BLE_GAP_ADDR_LEN );
      DBGPRINTF( "BLE Data link ready with: %04x%08x\r\n", *((uint16_t*)(&peerAddress[4])),*((uint32_t*)&peerAddress[0]) );
    }

    // Post the message
    if( !QueuePost(SCtrlInterfaceTask_GetMessageQueue(), pInterTaskMsg, sizeof(*pInterTaskMsg)) )
    {
      MsgFree( &pInterTaskMsg->base );
      DBGPRINTF( "BLE Task : Error, Unable to post on Slave task\r\n" );
      return;
    }
  }
}

static void ProcessDataReceivedEvent( uint8_t* peerAddress, struct BleDataReceivedEventData* receivedEventData )
{
  if( receivedEventData != NULL )
  {
    InterTaskMsg* pInterTaskMsg = (InterTaskMsg*)MsgAlloc( sizeof(InterTaskMsg) );
    if( pInterTaskMsg != NULL )
    {
      // Fill the message
      if (peerAddress)
      {
        pInterTaskMsg->base.id = INTER_TASK_BLE_FROM_PERIPHERAL_DATA_ID;
        memcpy( &pInterTaskMsg->payload.bleData.peerAddress[0], peerAddress, BLE_GAP_ADDR_LEN );
      }
      else
      {
        DBGPRINTF("Received DATA from central");
        pInterTaskMsg->base.id = INTER_TASK_BLE_FROM_CENTRAL_DATA_ID;
        memset( &pInterTaskMsg->payload.bleData.peerAddress[0], 0, BLE_GAP_ADDR_LEN );
        TimerStart(&idleTimer);
      }

      pInterTaskMsg->payload.bleData.dataLength = receivedEventData->length;
      memcpy( &pInterTaskMsg->payload.bleData.data[0], receivedEventData->data, receivedEventData->length );

      // Post the message
      if( !QueuePost(SCtrlInterfaceTask_GetMessageQueue(), pInterTaskMsg, sizeof(*pInterTaskMsg)) )
      {
        MsgFree( &pInterTaskMsg->base );
        DBGPRINTF( "BLE Task : Error, Unable to post on Slave task\r\n" );
        return;
      }
    }
  }
}

static void ProcessDataSentEvent( void )
{
  
  if( txBusy == true )
  {
    txBusy = false;
    DBGPRINTF( "BLE Task : Data send evt busy\r\n" );
    SemaphoreSignal( &sema );
  }
}

static void ProcessTcvrCharWriteEvent(BleTcvrCharacteristics* eventData)
{
  InterTaskMsg* pInterTaskMsg = (InterTaskMsg*)MsgAlloc( sizeof(InterTaskMsg) );
  if( pInterTaskMsg != NULL )
  {
    DBGPRINTF("ProcessTcvrCharWriteEvent");
    pInterTaskMsg->base.id = INTER_TASK_BLE_TRANSCEIVER_CHARACTERISTICS_ID;

    pInterTaskMsg->payload.bleTcvrCharacteristics.presenceFlags = eventData->presenceFlags;
    pInterTaskMsg->payload.bleTcvrCharacteristics.channel1 = eventData->channel1;
    pInterTaskMsg->payload.bleTcvrCharacteristics.channel2 = eventData->channel2;
    pInterTaskMsg->payload.bleTcvrCharacteristics.ipAddress = eventData->ipAddress;
    memcpy(pInterTaskMsg->payload.bleTcvrCharacteristics.name, eventData->name,
    sizeof(pInterTaskMsg->payload.bleTcvrCharacteristics.name));
    // Post the message
    if( !QueuePost(SCtrlInterfaceTask_GetMessageQueue(), pInterTaskMsg, sizeof(*pInterTaskMsg)) )
    {
      MsgFree( &pInterTaskMsg->base );
      DBGPRINTF( "BLE Task : Error, Unable to post on Slave task\r\n" );
      return;
    }
    TimerStart(&idleTimer);
  }
}


static void IdleTimerCallback(void)
{
  // Kill the connection if still connected
  if(isConnected)
  {
    isConnected = false;
    dataLinkReady = false;
    int err = BlePeripheralDriver_Disconnect();
   if (err < 0)
    {
      DBGPRINTF("BLE Task - FAILED TO DISCONNECT LINK");
    }
  }
}

Here is the part of the ble_driver

#define APP_ADV_INTERVAL                    300                                     /**< The advertising interval (in units of 0.625 ms. This value corresponds to 187.5 ms). */
#define APP_ADV_DURATION                    0                                       /**< The advertising duration in units of 10 milliseconds (0 = no timeout). */

#define DEFAULT_CONN_INTERVAL               MSEC_TO_UNITS(BLE_CONNECTION_INTERVAL, UNIT_1_25_MS)
#define MIN_CONN_INTERVAL                   (uint16_t)(MSEC_TO_UNITS(7.5, UNIT_1_25_MS))    /**< Minimum acceptable connection interval, in units of 1.25 ms. */
#define MAX_CONN_INTERVAL                   (uint16_t)(MSEC_TO_UNITS(500, UNIT_1_25_MS))    /**< Maximum acceptable connection interval, in units of 1.25 ms. */
#define CONN_SUP_TIMEOUT                    (uint16_t)(MSEC_TO_UNITS(4000,  UNIT_10_MS))    /**< Connection supervisory timeout (4 seconds). */
#define SLAVE_LATENCY                       0                                               /**< Slave latency. */


#define BLE_DVR_DEBUG_PRINT                 1

enum BleDriverConnectionStatus
{
  BLE_DISCONNECTED,
  BLE_CONNECTING,
  BLE_CONNECTED,
};

struct BleDriverCallbackData
{
  BleDriver_Callback cb;
  void* arg;
} callbackData;

struct BleDriverConnectionMap
{
  ble_gap_addr_t gapAddr;
  uint16_t connectionHandle;
  ble_gap_conn_params_t connectionParams;
  enum BleDriverConnectionStatus status;
  bool dataLinkReady;
};

NRF_BLE_GATT_DEF(gattInstance);
NRF_BLE_QWR_DEF(qwrInstance);
NRF_BLE_SCAN_DEF(scanInstance);
BLE_DB_DISCOVERY_DEF(dbDiscoveryInstance);
static nrf_ble_spes_t spesInstance;
NRF_SDH_BLE_OBSERVER(spesBleObserver, BLE_SPES_BLE_OBSERVER_PRIO, nrf_ble_spes_on_ble_evt, &spesInstance);
static nrf_ble_spes_c_t specInstance;
NRF_SDH_BLE_OBSERVER(specBleObserver, BLE_SPES_C_BLE_OBSERVER_PRIO, nrf_ble_spes_c_on_ble_evt, &specInstance);
static ble_tcvr_t tcvrInstance;
NRF_SDH_BLE_OBSERVER(tcvrBleObserver, TCVR_BLE_OBSERVER_PRIO, ble_tcvr_on_ble_evt, &tcvrInstance);

static ble_gap_scan_params_t scanParams =
{
    .active        = 0x01,
    .interval      = NRF_BLE_SCAN_SCAN_INTERVAL,
    .window        = NRF_BLE_SCAN_SCAN_WINDOW,
    .filter_policy = BLE_GAP_SCAN_FP_ACCEPT_ALL,
    .timeout       = NRF_BLE_SCAN_SCAN_DURATION,
    .scan_phys     = BLE_GAP_PHY_1MBPS,
};
static const ble_gap_conn_params_t defaultConnectionParams =
{
    .min_conn_interval = MIN_CONN_INTERVAL,   // Minimum connection interval.
    .max_conn_interval = MAX_CONN_INTERVAL,   // Maximum connection interval.
    .slave_latency     = SLAVE_LATENCY,       // Slave latency.
    .conn_sup_timeout  = CONN_SUP_TIMEOUT     // Supervisory timeout.
};
static bool scanInProgress;
static int eScanResult = -1;
static struct BleDriverConnectionMap connectionMap[NRF_SDH_BLE_CENTRAL_LINK_COUNT];

static uint8_t m_adv_handle = BLE_GAP_ADV_SET_HANDLE_NOT_SET;
static uint8_t m_enc_advdata[BLE_GAP_ADV_SET_DATA_SIZE_MAX];
static ble_gap_adv_data_t m_adv_data =
{
  .adv_data =
  {
    .p_data = m_enc_advdata,
    .len    = BLE_GAP_ADV_SET_DATA_SIZE_MAX
  },
  .scan_rsp_data =
  {
    .p_data = NULL,
    .len    = 0
  }
};
static uint16_t m_conn_handle = BLE_CONN_HANDLE_INVALID;
static const char* m_bleDeviceNameAdvFilter = NULL;

int BleDriver_Init(const char* bleDeviceName)
{
  int err;

  // Configure and initialize the BLE stack.
  err = BleStackInit();
  if( err != 0 )
  {
    DbgPrintf("BleDriver_init: BleStackInit %d", err);
    return err;
  }

  // Initialize modules.
  err = GapInit(bleDeviceName);
  if ( err != 0 )
  {
    DbgPrintf("BleDriver_init: GapInit %d", err);
    return err;
  }
  err = GattInit();
  if( err != 0 )
  {
    DbgPrintf("BleDriver_init: GattInit %d", err);
    return err;
  }
  err = DiscoveryInit();
  if( err != 0 )
  {
    DbgPrintf("BleDriver_init: DiscoveryInit %d", err);
    return err;
  }
  err = ServicesInit();
  if( err != 0 )
  {
    DbgPrintf("BleDriver_init: ServicesInit %d", err);
    return err;
  }
  err = PeerManagerInit();
  if( err != 0 )
  {
    DbgPrintf("BleDriver_init: PeerManagerInit %d", err);
    return err;
  }
  err = ScanInit();
  if( err != 0 )
  {
    DbgPrintf("BleDriver_init: ScanInit %d", err);
    return err;
  }

  // Create the connection semaphore
  SemaphoreCreate(&connectionSema);

  // Create a uC-OS task for the BLE stack.
  nrf_sdh_ucos_init(NULL, NULL);

  // Success
  return 0;
}

int BleCentralDriver_StartScan(const char* bleDeviceFilter)
{
  uint32_t err;

  // BLE device name filter cannot be null at the moment.
  if (bleDeviceFilter == NULL)
  {
    DbgPrintf("Device Filter is NULL");
    return -EFAULT;
  }

  // Make sure we are not already scanning
  if( scanInProgress == true )
  {
    
    BleCentralDriver_StopScan();
    DbgPrintf("scan is in progress, restarting process received evt %d", eScanResult);
  }

  m_bleDeviceNameAdvFilter = bleDeviceFilter;

  // Set power level for scanning
  err = sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_SCAN_INIT, BLE_CONN_HANDLE_INVALID, 8);
  if( err != 0 )
  {
    DbgPrintf("sd_ble_gap_tx_power_set failed with error %u", err);
    return -EFAULT;
  }

  // Start scanning
  err = nrf_ble_scan_params_set(&scanInstance, &scanParams);
  if( err != 0 )
  {
    DbgPrintf("nrf_ble_scan_params_set failed with error %u", err);
    return -EFAULT;
  }

  err = nrf_ble_scan_start(&scanInstance);
  if( err != 0 )
  {
     DbgPrintf("nrf_ble_scan_start failed with error %u", err);
    return -EFAULT;
  }

  // Set flag
  scanInProgress = true;

  // Success
  return 0;
}

int BleCentralDriver_StopScan(void)
{
  // Stop scanning
  nrf_ble_scan_stop();

  // Reset flag
  scanInProgress = false;

  m_bleDeviceNameAdvFilter = NULL;

  // Success
  return 0;
}

int BleCentralDriver_Connect(uint8_t* peerAddress)
{
  int err;
  int freeSlot = -1;

  // Check for valid address
  if( peerAddress == NULL)
  {
    DbgPrintf("BleCentralDriver_Connect peerAddress null");
    return -EFAULT;
  }

  // Check if we are already connected to that peer
  if( GetConnectedSlotFromAddress(peerAddress) != -1 )
  {
    DbgPrintf("BleCentralDriver_Connect already connected to that peer");
    return -EFAULT;
  }

  // Find a free slot
  freeSlot = GetDisconnectedSlot();
  if( freeSlot == -1 )
  {
    DbgPrintf("BleCentralDriver_Connect no free slot found %d", freeSlot);
    return -EFAULT;
  }

  // Prepare connection map
  memset(&connectionMap[freeSlot].gapAddr, 0, sizeof(ble_gap_addr_t));
  memcpy(&connectionMap[freeSlot].gapAddr.addr, peerAddress, BLE_GAP_ADDR_LEN);
  connectionMap[freeSlot].gapAddr.addr_type = BLE_GAP_ADDR_TYPE_RANDOM_STATIC;
  connectionMap[freeSlot].connectionParams = defaultConnectionParams;
  connectionMap[freeSlot].dataLinkReady = false;

  // Initialize connection param
  sd_ble_gap_ppcp_set(&connectionMap[freeSlot].connectionParams);
  connectionMap[freeSlot].connectionParams.min_conn_interval = DEFAULT_CONN_INTERVAL;
  connectionMap[freeSlot].connectionParams.max_conn_interval = DEFAULT_CONN_INTERVAL;

  // Initiate connection
  err = sd_ble_gap_connect(&connectionMap[freeSlot].gapAddr, &scanParams, &connectionMap[freeSlot].connectionParams, APP_BLE_CONN_CFG_TAG);
  if( err != 0 )
  {
    DbgPrintf("BleCentralDriver_Connect error initializing the connection %d", err);
    return err;
  }

  // Set current slot as connecting
  connectionMap[freeSlot].status = BLE_CONNECTING;

  // Wait for connection to end
  SemaphoreWait(&connectionSema, true, 0);

  // Success
  return 0;
}

int BleCentralDriver_Disconnect(uint8_t* peerAddress)
{
  int currentSlot = -1;

  // Find the proper slot
  currentSlot = GetConnectedSlotFromAddress(peerAddress);
  if( currentSlot == -1 )
  {
    DbgPrintf("BleCentralDriver_Connect error finding proper slot");
    return -EFAULT;
  }

  // Initiate peer disconnection
  uint32_t err_code =  sd_ble_gap_disconnect(connectionMap[currentSlot].connectionHandle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
  if (err_code != NRF_SUCCESS)
  {
      DbgPrintf("BleCentralDriver_Connect error while initiating disconnection", err_code);
      return -EFAULT;
  }

  // Success
  return 0;
}

int BleCentralDriver_SendData(uint8_t* peerAddress, uint8_t* data, uint16_t length)
{
  int err;
  int currentSlot = -1;

  // Find the proper slot
  currentSlot = GetConnectedSlotFromAddress(peerAddress);
  if( currentSlot == -1 )
  {
    return -EFAULT;
  }

  // Make sure the data link is ready
  if( connectionMap[currentSlot].dataLinkReady == false )
  {
    return -EFAULT;
  }

  // Send data
  if( nrf_ble_spes_send_notif(connectionMap[currentSlot].connectionHandle, &spesInstance, data, length) != NRF_SUCCESS )
  {
    return -EBUSY;
  }

  // Success
  return 0;
}

int BlePeripheralDriver_StartAdvertising(void* pManufacturerData, uint32_t len)
{
  // Make sure we have manufacturer data
  if( pManufacturerData == NULL )
  {
    return -EFAULT;
  }

  // Initialize advertising
  AdvertisingInit(pManufacturerData, len);

  // Set power level for advertising
  sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_ADV, m_adv_handle, 8);

  // Start advertising
  uint32_t result = sd_ble_gap_adv_start(m_adv_handle, APP_BLE_CONN_CFG_TAG);
  if (result != NRF_SUCCESS)
  {
    DbgPrintf("BlePeripheralDriver_StartAdvertising failed %u", result);
    return -EFAULT;
  }

  // Success
  return 0;
}

int BlePeripheralDriver_StopAdvertising(void)
{
  // Stop advertising
  uint32_t result = sd_ble_gap_adv_stop(m_adv_handle);
  if (result != NRF_SUCCESS)
  {
    return -EFAULT;
  }

  // Success
  return 0;
}

bool BlePeripheralDriver_IsConnected(void)
{
  return (m_conn_handle != BLE_CONN_HANDLE_INVALID);
}

int BlePeripheralDriver_Disconnect(void)
{
  sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);

  // Success
  return 0;
}

int BlePeripheralDriver_SendData(uint8_t* data, uint16_t length)
{
  int err;

  // Send data
  if( nrf_ble_spes_send_notif(m_conn_handle, &spesInstance, data, length) != NRF_SUCCESS )
  {
    return -EBUSY;
  }

  // Success
  return 0;
}

int BleDriver_SetCallback(BleDriver_Callback callback, void* arg)
{
  callbackData.cb = callback;
  callbackData.arg = arg;
  return 0;
}

static int GetConnectedSlotFromAddress(uint8_t* peerAddress)
{
  for(uint8_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++)
  {
    if( (connectionMap[i].status == BLE_CONNECTED) && (memcmp(connectionMap[i].gapAddr.addr, peerAddress, BLE_GAP_ADDR_LEN) == 0) )
    {
      return i;
    }
  }
  return -1;
}

static int GetConnectedSlotFromHandle(uint16_t connectionHandle)
{
  for(uint8_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++)
  {
    if( (connectionMap[i].status == BLE_CONNECTED) && (connectionMap[i].connectionHandle == connectionHandle) )
    {
      return i;
    }
  }
  return -1;
}

static int GetDisconnectedSlot(void)
{
  for(uint8_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++)
  {
    if( connectionMap[i].status == BLE_DISCONNECTED )
    {
      return i;
    }
  }
  return -1;
}

static int GetConnectingSlot(void)
{
  for(uint8_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++)
  {
    if( connectionMap[i].status == BLE_CONNECTING )
    {
      return i;
    }
  }
  return -1;
}

static int BleStackInit(void)
{
  int err;

  DbgPrintf("BleStackInit: entering");

  err = nrf_sdh_enable_request();
  if( err != 0 )
  {
    DbgPrintf("BleStackInit: nrf_sdh_enable_request %d", err);
    return -EFAULT;
  }

  // Configure the BLE stack using the default settings.
  // Fetch the start address of the application RAM.
  uint32_t ram_start = 0;
  err = nrf_sdh_ble_default_cfg_set(APP_BLE_CONN_CFG_TAG, &ram_start);
  if( err != 0 )
  {
    DbgPrintf("BleStackInit: nrf_sdh_ble_default_cfg_set %d", err);
    return -EFAULT;
  }

  // Enable BLE stack.
  err = nrf_sdh_ble_enable(&ram_start);
  if( err != 0 )
  {
    DbgPrintf("BleStackInit: nrf_sdh_ble_enable %d", err);
    return -EFAULT;
  }

  // Register a handler for BLE events.
  NRF_SDH_BLE_OBSERVER(bleObserver, APP_BLE_OBSERVER_PRIO, BleCallback, NULL);

  // Success
  return 0;
}

static int GapInit(const char* bleDeviceName)
{
  ble_gap_conn_params_t   gap_conn_params;
  ble_gap_conn_sec_mode_t sec_mode;
  int err;

  BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);

  err = sd_ble_gap_device_name_set(&sec_mode, (const uint8_t *)bleDeviceName, strlen(bleDeviceName));
  if (err != 0)
  {
    return -EFAULT;
  }
  memset(&gap_conn_params, 0, sizeof(gap_conn_params));

  gap_conn_params.min_conn_interval = DEFAULT_CONN_INTERVAL;
  gap_conn_params.max_conn_interval = DEFAULT_CONN_INTERVAL;
  gap_conn_params.slave_latency     = SLAVE_LATENCY;
  gap_conn_params.conn_sup_timeout  = CONN_SUP_TIMEOUT;

  err = sd_ble_gap_ppcp_set(&gap_conn_params);
  if (err != 0)
  {
    return -EFAULT;
  }

  // Success
  return 0;
}

static int GattInit(void)
{
  ble_opt_t opt = {0};
  int err;

  err = nrf_ble_gatt_init(&gattInstance, NULL);
  if( err != 0 )
  {
    return -EFAULT;
  }

  err = nrf_ble_gatt_att_mtu_central_set(&gattInstance, NRF_SDH_BLE_GATT_MAX_MTU_SIZE);
  if( err != 0 )
  {
    return -EFAULT;
  }

  opt.common_opt.conn_evt_ext.enable = 1;
  err = sd_ble_opt_set(BLE_COMMON_OPT_CONN_EVT_EXT, &opt);
  if( err != 0 )
  {
    return -EFAULT;
  }

  return 0;
}

static int DiscoveryInit(void)
{
  int err;
  err = ble_db_discovery_init(DiscoveryCallback);
  if( err != 0 )
  {
    return -EFAULT;
  }

  return 0;
}

static int ServicesInit(void)
{
  int err;
  nrf_ble_qwr_init_t qwr_init = {0};

  // Initialize Serial Port Emulation Service
  err = nrf_ble_spes_init(&spesInstance, SpesCallback);
  if( err != 0 )
  {
    return -EFAULT;
  }
  err = nrf_ble_spes_c_init(&specInstance, SpecCallback);
  if( err != 0 )
  {
    return -EFAULT;
  }

  // Initialize Queued Write Module.
  qwr_init.error_handler = NULL;
  err = nrf_ble_qwr_init(&qwrInstance, &qwr_init);
  if ( err != 0 )
  {
    return -EFAULT;
  }

  return 0;
}

static int ScanInit(void)
{
  int err;
  nrf_ble_scan_init_t initScan = {0};

  // Initialize scan
  initScan.connect_if_match = false;
  initScan.conn_cfg_tag = APP_BLE_CONN_CFG_TAG;
  initScan.p_scan_param = &scanParams;
  err = nrf_ble_scan_init(&scanInstance, &initScan, ScanCallback);
  if(err != 0 )
  {
    return -EFAULT;
  }
  return 0;
}

static void ScanCallback(scan_evt_t const * p_scan_evt)
{
  ret_code_t err_code;
  switch(p_scan_evt->scan_evt_id)
  {
    case NRF_BLE_SCAN_EVT_SCAN_TIMEOUT:
    {
      scanInProgress = false;
      if( callbackData.cb != NULL )
      {
        callbackData.cb(BLE_SCAN_COMPLETED_EVENT, NULL, NULL, callbackData.arg);
      }
    }
    break;

    case NRF_BLE_SCAN_EVT_CONNECTED:
    {
      scanInProgress = false; // Scan ended because of a connection
    }
    break;

#if BLE_DVR_DEBUG_PRINT
    default:
    {
      eScanResult = p_scan_evt->scan_evt_id;
      DbgPrintf("Unknown scan Event: %u\r\n", p_scan_evt->scan_evt_id);
    }
#endif
  }
}

static void AdvertisingInit(void* pManufacturerData, uint32_t len)
{
  ble_gap_adv_params_t adv_params = {0};
  ble_advdata_t adv_data = {0};
  ble_advdata_manuf_data_t advManufacturerData = {0};

  advManufacturerData.data.p_data        = pManufacturerData;
  advManufacturerData.data.size          = len;
  advManufacturerData.company_identifier = BLE_COMPANY_ID;

  adv_params.properties.type = BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED;
  adv_params.p_peer_addr     = NULL;
  adv_params.filter_policy   = BLE_GAP_ADV_FP_ANY;
  adv_params.interval        = APP_ADV_INTERVAL;
  adv_params.duration        = 0;
  adv_params.primary_phy     = BLE_GAP_PHY_1MBPS;
  adv_params.secondary_phy   = BLE_GAP_PHY_1MBPS;

  adv_data.name_type               = BLE_ADVDATA_FULL_NAME;
  adv_data.include_appearance      = false;
  adv_data.flags                   = BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;
  adv_data.uuids_complete.uuid_cnt = 0;
  adv_data.p_manuf_specific_data   = &advManufacturerData;

  ble_advdata_encode(&adv_data, m_adv_data.adv_data.p_data, &m_adv_data.adv_data.len);

  sd_ble_gap_adv_set_configure(&m_adv_handle, &m_adv_data, &adv_params);
}

static int PeerManagerInit(void)
{
  ble_gap_sec_params_t sec_param;

  pm_init();

  memset(&sec_param, 0, sizeof(ble_gap_sec_params_t));

  // Security parameters to be used for all security procedures.
  sec_param.bond           = SEC_PARAM_BOND;
  sec_param.mitm           = SEC_PARAM_MITM;
  sec_param.lesc           = SEC_PARAM_LESC;
  sec_param.keypress       = SEC_PARAM_KEYPRESS;
  sec_param.io_caps        = SEC_PARAM_IO_CAPABILITIES;
  sec_param.oob            = SEC_PARAM_OOB;
  sec_param.min_key_size   = SEC_PARAM_MIN_KEY_SIZE;
  sec_param.max_key_size   = SEC_PARAM_MAX_KEY_SIZE;
  sec_param.kdist_own.enc  = 1;
  sec_param.kdist_own.id   = 1;
  sec_param.kdist_peer.enc = 1;
  sec_param.kdist_peer.id  = 1;

  pm_sec_params_set(&sec_param);

  pm_register(PeerManagerCallback);
}

static void PeerManagerCallback(pm_evt_t const * p_evt)
{
  switch (p_evt->evt_id)
  {
    case PM_EVT_CONN_SEC_CONFIG_REQ:
    {
      // Reject pairing request from an already bonded peer.
      pm_conn_sec_config_t conn_sec_config = {.allow_repairing = false};
      pm_conn_sec_config_reply(p_evt->conn_handle, &conn_sec_config);
    } break;

    case PM_EVT_STORAGE_FULL:
    {
      // Run garbage collection on the flash.
      fds_gc();
    } break;

    case PM_EVT_PEERS_DELETE_SUCCEEDED:
    {
      pm_peers_delete();
    } break;

#if BLE_DVR_DEBUG_PRINT
    default:
    {
      DbgPrintf("Unknown PM Event: %d", p_evt->evt_id);
    }
#endif
    break;
  }
}

static void BleCallback(ble_evt_t const * p_ble_evt, void * p_context)
{
  int currentSlot = -1;
  uint8_t* addr = NULL;

  switch (p_ble_evt->header.evt_id)
  {
    case BLE_GAP_EVT_ADV_REPORT:
    {
      DbgPrintf("BleCallback BLE_GAP_EVT_ADV_REPORT with status %u", p_ble_evt->evt.gap_evt.params.adv_report.type.status);
      ProcessAdvReport(&p_ble_evt->evt.gap_evt.params.adv_report);
    }
    break;

    case BLE_GAP_EVT_CONNECTED:
    {
      // Set power level for connection
      sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_CONN, p_ble_evt->evt.gap_evt.conn_handle, 8);

      // Received a connection when in Central.
      if (p_ble_evt->evt.gap_evt.params.connected.role == BLE_GAP_ROLE_CENTRAL)
      {
        // Get proper slot
        currentSlot = GetConnectingSlot();

        if( currentSlot == -1 )
        {
          DbgPrintf("BleCallback connecting slot error");
          return;
        }

        addr = connectionMap[currentSlot].gapAddr.addr;

        // Assign the connection handle
        connectionMap[currentSlot].connectionHandle = p_ble_evt->evt.gap_evt.conn_handle;

        // Update connection map
        connectionMap[currentSlot].status = BLE_CONNECTED;

        // Release the semaphore
        SemaphoreSignal(&connectionSema);
      }
      else
      {
        m_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
        nrf_ble_qwr_conn_handle_assign(&qwrInstance, m_conn_handle);

        // Update phy
        ble_gap_phys_t const phys =
        {
          .rx_phys = BLE_GAP_PHY_2MBPS,
          .tx_phys = BLE_GAP_PHY_2MBPS,
        };
        sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys);
      }

      // Start GATT service discovery
      ble_db_discovery_start(&dbDiscoveryInstance, p_ble_evt->evt.gap_evt.conn_handle);

      // Call user connection callback
      if( callbackData.cb != NULL )
      {
        struct BleConnectionEventData connectionEventData;
        connectionEventData.connected = true;
        callbackData.cb(BLE_CONNECTION_EVENT, addr, &connectionEventData, callbackData.arg);
      }
    }
    break;

    case BLE_GAP_EVT_DISCONNECTED:
    {
      // If disconnected from central (aka, we are the peripheral)
      if ( m_conn_handle == p_ble_evt->evt.common_evt.conn_handle )
      {
        m_conn_handle = BLE_CONN_HANDLE_INVALID;
      }
      // If disconnected from peripheral (aka, we are the central)
      else
      {
        // Find the proper slot
        currentSlot = GetConnectedSlotFromHandle(p_ble_evt->evt.common_evt.conn_handle);
        if( currentSlot == -1 )
        {
           DbgPrintf("BleCallback disconnected slot was not found");
          return;
        }

        // Update connection map
        connectionMap[currentSlot].status = BLE_DISCONNECTED;
        connectionMap[currentSlot].dataLinkReady = false;

        addr = connectionMap[currentSlot].gapAddr.addr;
      }

      // Call user connection callback
      if( callbackData.cb != NULL )
      {
        struct BleConnectionEventData connectionEventData;
        connectionEventData.connected = false;
        callbackData.cb(BLE_CONNECTION_EVENT, addr, &connectionEventData, callbackData.arg);
      }
    }
    break;

    case BLE_GAP_EVT_TIMEOUT:
    {
      // Find the proper slot
      currentSlot = GetConnectingSlot();
      if( currentSlot == -1 )
      {
        DbgPrintf("BleCallback - SLOT not found");
        return;
      }

      // Update connection map
      connectionMap[currentSlot].status = BLE_DISCONNECTED;
      connectionMap[currentSlot].dataLinkReady = false;

      // Inform the BLE task
      if (p_ble_evt->evt.gap_evt.params.timeout.src == BLE_GAP_TIMEOUT_SRC_CONN) 
      {
        DbgPrintf("BleCallback - BLE_GAP_TIMEOUT_SRC_CONN");
        callbackData.cb(BLE_CONNECTION_TIMEOUT_EVENT, addr, NULL, callbackData.arg);

      }
      else if (p_ble_evt->evt.gap_evt.params.timeout.src == BLE_GAP_TIMEOUT_SRC_SCAN)
      {
        DbgPrintf("BleCallback - BLE_GAP_TIMEOUT_SRC_SCAN");
        // TODO: handle...
      }
      else if (p_ble_evt->evt.gap_evt.params.timeout.src == BLE_GAP_TIMEOUT_SRC_AUTH_PAYLOAD)
      {
        DbgPrintf("BleCallback - BLE_GAP_TIMEOUT_SRC_AUTH_PAYLOAD");
        // TODO: handle...
      }

      // Release the semaphore
      SemaphoreSignal(&connectionSema);
    }
    break;

    case BLE_GAP_EVT_PHY_UPDATE_REQUEST:
    {
      ble_gap_phys_t const phys =
      {
        .rx_phys = BLE_GAP_PHY_2MBPS,
        .tx_phys = BLE_GAP_PHY_2MBPS,
      };
      sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys);
    }
    break;

    case BLE_GATTC_EVT_TIMEOUT:
      // Disconnect on GATT Client timeout event.
      sd_ble_gap_disconnect(p_ble_evt->evt.gattc_evt.conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
      if( callbackData.cb != NULL )
      {
        struct BleConnectionEventData connectionEventData;
        connectionEventData.connected = false;
        callbackData.cb(BLE_CONNECTION_EVENT, NULL, &connectionEventData, callbackData.arg);
      }
    break;

    case BLE_GATTS_EVT_TIMEOUT:
      // Disconnect on GATT Server timeout event.
      sd_ble_gap_disconnect(p_ble_evt->evt.gatts_evt.conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
      if( callbackData.cb != NULL )
      {
        struct BleConnectionEventData connectionEventData;
        connectionEventData.connected = false;
        callbackData.cb(BLE_CONNECTION_EVENT, NULL, &connectionEventData, callbackData.arg);
      }
    break;
#if BLE_DVR_DEBUG_PRINT
    default:
    {
      DbgPrintf("Unknown BLE Event: 0x%x", p_ble_evt->header.evt_id);
    }
#endif
  }
}

static void ProcessAdvReport(ble_gap_evt_adv_report_t const * pAdvReport)
{
  struct BleDeviceDiscoveryData deviceDiscoveryData;
  uint8_t* pAdvData;
  uint16_t advDataLen;
  uint16_t fieldLen;
  uint16_t offset = 0;
  uint16_t companyId = 0;

  // Initialize advertisement report for parsing.
  pAdvData = (uint8_t *)pAdvReport->data.p_data;
  advDataLen = pAdvReport->data.len;

  // Only report EDGE devices
  if( ble_advdata_name_find(pAdvData, advDataLen, m_bleDeviceNameAdvFilter) == true  )
  {
    // Extract manufacturer data
    offset = 0;
    fieldLen = ble_advdata_search(pAdvData, advDataLen, &offset, BLE_GAP_AD_TYPE_MANUFACTURER_SPECIFIC_DATA);

    // Validate minimal length BleManufacturerData + company ID
    if( fieldLen >= BLE_COMPANY_ID_LEN )
    {
      // Validate company ID
      memcpy(&companyId, &pAdvData[offset], BLE_COMPANY_ID_LEN);
      if( companyId == BLE_COMPANY_ID )
      {
        // Prepare device discovery data
        deviceDiscoveryData.rssi = pAdvReport->rssi;

        fieldLen -= BLE_COMPANY_ID_LEN;
        if ( fieldLen > sizeof(deviceDiscoveryData.manufacturerData) )
        {
          fieldLen = sizeof(deviceDiscoveryData.manufacturerData);
        }

        memcpy(&deviceDiscoveryData.manufacturerData, &pAdvData[offset+BLE_COMPANY_ID_LEN], fieldLen );

        // Call the user callback
        if( callbackData.cb != NULL )
        {
          callbackData.cb(BLE_DEVICE_DISCOVERY_EVENT, (uint8_t*)pAdvReport->peer_addr.addr, &deviceDiscoveryData, callbackData.arg);
        }
      }
    }
  }
}

static void DiscoveryCallback(ble_db_discovery_evt_t * p_evt)
{
  nrf_ble_spes_c_on_db_disc_evt(&specInstance, p_evt);
}

static void SpesCallback(nrf_ble_spes_evt_t evt)
{
  int currentSlot = -1;
  int err;
  uint8_t* addr = NULL;

  // Find the proper slot
  currentSlot = GetConnectedSlotFromHandle(evt.conn_handle);
  if( currentSlot != -1 )
  {
    addr = connectionMap[currentSlot].gapAddr.addr;
  }

  switch (evt.evt_type)
  {
    case NRF_BLE_SPES_EVT_NOTIF_ENABLED:
    {
      if (currentSlot != -1)
      {
        connectionMap[currentSlot].dataLinkReady = true;
      }

      // Call the user callback
      if( callbackData.cb != NULL )
      {
        callbackData.cb(BLE_DATA_LINK_READY_EVENT, addr, NULL, callbackData.arg);
      }
    }
    break;

    case NRF_BLE_SPES_EVT_TX_COMPLETED:
    {
      // Call the user callback
      if( callbackData.cb != NULL )
      {
        callbackData.cb(BLE_DATA_SENT_EVENT, addr, NULL, callbackData.arg);
      }
    }
    break;

#if BLE_DVR_DEBUG_PRINT
    default:
    {
      DbgPrintf("Unknown SPES event: %d",evt.evt_type );
    }
    break;
#endif
  }
}

static void SpecCallback(nrf_ble_spes_c_t * p_spes_c, nrf_ble_spes_c_evt_t * p_evt)
{
  int currentSlot = -1;
  uint8_t* addr = NULL;

  switch (p_evt->evt_type)
  {
    case NRF_BLE_SPES_C_EVT_DISCOVERY_COMPLETE:
    {
      // Assign the SPES handle
      nrf_ble_spes_c_handles_assign(p_spes_c, p_evt->conn_handle, &p_evt->params.peer_db);

      // Enable notifications
      nrf_ble_spes_c_notif_enable(p_spes_c);
    }
    break;

    case NRF_BLE_SPES_C_EVT_NOTIFICATION:
    {

      // Find the proper slot


      // Find the proper slot
      currentSlot = GetConnectedSlotFromHandle(p_evt->conn_handle);
      if( currentSlot != -1 )
      {
        addr = connectionMap[currentSlot].gapAddr.addr;
      }

      // Call user callback
      if( callbackData.cb != NULL )
      {
        struct BleDataReceivedEventData dataReceivedEventData;

        dataReceivedEventData.length = p_evt->params.hvx.dataLen;
        dataReceivedEventData.data = p_evt->params.hvx.data;
        callbackData.cb(BLE_DATA_RECEIVED_EVENT, addr, &dataReceivedEventData, callbackData.arg);
      }
    }
    break;

#if BLE_DVR_DEBUG_PRINT
    default:
    {
      DbgPrintf("Unknown SCPEC Event: %d", p_evt->evt_type);
    }
    break;
#endif
  }
}

static void HandleTcvrWrite(ble_tcvr_evt_t const * pEvt)
{
  char data[TCVR_CHANNEL_CHAR_LEN] = {0};
  BleTcvrCharacteristics chars = {0};

  // Ensure NUL-termination.
  uint16_t len = pEvt->u.write.dataLen;
  if (len > sizeof(data) - 1)
  {
    len = sizeof(data) - 1;
  }
  memcpy(data, pEvt->u.write.data, len);

  switch (pEvt->u.write.charId)
  {
  case BLE_TCVR_CHAR_CHANNEL_1_UUID:
  {
    char* ptr;
    unsigned long tmp = strtoul(data, &ptr, 10);
    chars.presenceFlags = BLE_TCVR_CHANNEL1_SET_FLAG;
    if (tmp <= UINT8_MAX && *ptr == 0)
    {
      chars.channel1 = (uint8_t)tmp;
    }
    else
    {
      // Trigger an invalid update so we will rewrite a good value.
      chars.channel1 = 0;
    }
    break;
  }

  case BLE_TCVR_CHAR_CHANNEL_2_UUID:
  {
    char* ptr;
    unsigned long tmp = strtoul(data, &ptr, 10);
    chars.presenceFlags = BLE_TCVR_CHANNEL2_SET_FLAG;
    if (tmp <= UINT8_MAX && *ptr == 0)
    {
      chars.channel2 = (uint8_t)tmp;
    }
    else
    {
      // Trigger an invalid update so we will rewrite a good value.
      chars.channel2 = 0;
    }
    break;
  }

  case BLE_TCVR_CHAR_IP_ADDRESS_UUID:
  {
    // Writing the IP address using BLE isn't supported.
    // We should never get here.
    break;
  }

  case BLE_TCVR_CHAR_NAME_UUID:
  {
    // Writing the transceiver name using BLE isn't supported.
    // We should never get here.
    break;
  }
  }

  if (chars.presenceFlags != 0 && callbackData.cb != NULL)
  {
    callbackData.cb(BLE_TCVR_CHAR_WRITE_EVENT, NULL, &chars, callbackData.arg);
  }
}

static void TcvrCallback(ble_tcvr_t const* pCtx, ble_tcvr_evt_t const * pEvt)
{
  switch (pEvt->evt_type)
  {
    case BLE_TCVR_EVT_WRITE:
      HandleTcvrWrite(pEvt);
      break;

    default:
      break;
  }
}

int BlePeripheralDriver_StartTcvrService(void)
{
  return ble_tcvr_init(&tcvrInstance, TcvrCallback);
}

static int WriteTcvrValue(uint16_t charId, void* pValue, uint16_t len)
{
  ble_gatts_value_t gattsValue;
  ble_gatts_hvx_params_t notifParam;
  uint16_t handleIndex = charId - BLE_TCVR_CHAR_CHANNEL_1_UUID;
  int result;
  // Write new attribute value
  gattsValue.len = len;
  gattsValue.offset = 0;
  gattsValue.p_value = (uint8_t*)pValue;
  // Fill notification data
  memset(&notifParam, 0, sizeof(notifParam));
  notifParam.type   = BLE_GATT_HVX_NOTIFICATION;
  notifParam.handle = tcvrInstance.tcvr_char_handles[handleIndex].value_handle;
  notifParam.p_data = gattsValue.p_value;
  notifParam.p_len  = &gattsValue.len;
  result = sd_ble_gatts_hvx(m_conn_handle, &notifParam);
  // Ignore some errors if the length was actually written.
  // It only means that the value could be written but the peer couldn't
  // be notified for reasons out of our control.
  if (*notifParam.p_len == len && (result == NRF_ERROR_INVALID_STATE ||
    result == NRF_ERROR_BUSY ||
    result == NRF_ERROR_FORBIDDEN ||
    result == BLE_ERROR_GATTS_SYS_ATTR_MISSING ||
    result == NRF_ERROR_RESOURCES)
    )
  {
    return NRF_SUCCESS;
  }
  return result;
}

///
/// @brief Update the transceiver characteristics.
///
/// @param[in] pTcvr  The transceiver characteristics to update.
int BlePeripheralDriver_UpdateTcvrCharacteristics(BleTcvrCharacteristics* pTcvr)
{
  int result = 0;
  char channel[TCVR_CHANNEL_CHAR_LEN] = {0};
  char ipAddress[TCVR_IP_CHAR_LEN] = {0};
  char name[TCVR_NAME_CHAR_LEN] = {0};
  uint16_t len;
  if (pTcvr->presenceFlags & BLE_TCVR_CHANNEL1_SET_FLAG)
  {
    len = snprintf(channel, sizeof(channel), "%u", pTcvr->channel1);
    result = WriteTcvrValue(BLE_TCVR_CHAR_CHANNEL_1_UUID, channel, len);
    if (result != 0)
    {
      return result;
    }
  }

  if (pTcvr->presenceFlags & BLE_TCVR_CHANNEL2_SET_FLAG)
  {
    len = snprintf(channel, sizeof(channel), "%u", pTcvr->channel2);
    result = WriteTcvrValue(BLE_TCVR_CHAR_CHANNEL_2_UUID, channel, len);
    if (result != 0)
    {
      return result;
    }
  }

  if (pTcvr->presenceFlags & BLE_TCVR_IP_ADDRESS_SET_FLAG)
  {
    uint8_t* pIp = (uint8_t*)&pTcvr->ipAddress;
    len = snprintf(ipAddress, sizeof(ipAddress), "%u.%u.%u.%u", pIp[0], pIp[1], pIp[2], pIp[3]);
    result = WriteTcvrValue(BLE_TCVR_CHAR_IP_ADDRESS_UUID, ipAddress, len);
    if (result != 0)
    {
      return result;
    }
  }

  if (pTcvr->presenceFlags & BLE_TCVR_NAME_SET_FLAG)
  {
    len = snprintf(name, sizeof(name), "%s", pTcvr->name);
    result = WriteTcvrValue(BLE_TCVR_CHAR_NAME_UUID, name, len);
    if (result != 0)
    {
      return result;
    }
  }
  return result;
}

When the issue occurs, I have these logs:

83357822 Device Discovery: -92 c5ea83387d3c 000F4251 00000CAB 0 0 132

83357828 Device Discovery: -87 e1a462b70c8b 00000001 00000CE9 99 59 50

83358444 Device Discovery: -77 e3ae212d9835 000F4241 00000CAB 99 44 53

83358754 Device Discovery: -88 f4de99e3458b 000F4244 00000CE9 0 0 131

83358763 Device Discovery: -83 f912898349f4 000F424B 00002000 99 55 24

83359373 Device Discovery: -82 e1a462b70c8b 00000001 00000CE9 99 59 50

83359393 Device Discovery: -77 e3ae212d9835 000F4241 00000CAB 99 44 53

83359709 Device Discovery: -88 f4de99e3458b 000F4244 00000CE9 0 0 131

83359713 Device Discovery: -83 f912898349f4 000F424B 00002000 99 55 24

83360333 Device Discovery: -83 e1a462b70c8b 00000001 00000CE9 99 59 50

83360938 Device Discovery: -86 f0bae1e36357 000F4250 00000CAB 0 0 132

83361252 Device Discovery: -88 f4de99e3458b 000F4244 00000CE9 0 0 131

83361274 Device Discovery: -93 c5ea83387d3c 000F4251 00000CAB 0 0 132

83361888 Device Discovery: -87 f0bae1e36357 000F4250 00000CAB 0 0 132

83362219 BLE Scan Completed

83392377 BLE Scan Starting

83392737 Device Discovery: -78 f912898349f4 000F424B 00002000 99 55 24

83393021 Device Discovery: -88 f4de99e3458b 000F4244 00000CE9 0 0 131

83393052 Device Discovery: -76 e3ae212d9835 000F4241 00000CAB 99 44 53

83394260 Device Discovery: -83 f912898349f4 000F424B 00002000 99 55 24

83394571 Device Discovery: -84 f4de99e3458b 000F4244 00000CE9 0 0 131

83394586 Device Discovery: -72 e3ae212d9835 000F4241 00000CAB 99 44 53

83395231 Device Discovery: -83 f912898349f4 000F424B 00002000 99 55 24

83395526 Device Discovery: -89 c5ea83387d3c 000F4251 00000CAB 0 0 132

83395531 Device Discovery: -84 f4de99e3458b 000F4244 00000CE9 0 0 131

83395533 Device Discovery: -85 f0bae1e36357 000F4250 00000CAB 0 0 132

83395541 Device Discovery: -72 e3ae212d9835 000F4241 00000CAB 99 44 53

83396147 Device Discovery: -87 e1a462b70c8b 00000001 00000CE9 99 59 50

83396764 Device Discovery: -89 f912898349f4 000F424B 00002000 99 55 24

83397073 Device Discovery: -87 f4de99e3458b 000F4244 00000CE9 0 0 131

83397080 Device Discovery: -70 e3ae212d9835 000F4241 00000CAB 99 44 53

83397110 Device Discovery: -87 e1a462b70c8b 00000001 00000CE9 99 59 50

83397117 BLE Scan Completed

83427281 BLE Scan Starting

83427300 Device Discovery: -88 c5ea83387d3c 000F4251 00000CAB 0 0 132

83427613 Device Discovery: -88 f4de99e3458b 000F4244 00000CE9 0 0 131

83428257 Device Discovery: -88 c5ea83387d3c 000F4251 00000CAB 0 0 132

83428264 Device Discovery: -72 e3ae212d9835 000F4241 00000CAB 99 44 53

83428547 Device Discovery: -89 f912898349f4 000F424B 00002000 99 55 24

83429168 Device Discovery: -84 f0bae1e36357 000F4250 00000CAB 0 0 132

83429795 Device Discovery: -69 e3ae212d9835 000F4241 00000CAB 99 44 53

83429809 Device Discovery: -87 e1a462b70c8b 00000001 00000CE9 99 59 50

83430135 Device Discovery: -86 f0bae1e36357 000F4250 00000CAB 0 0 132

83430750 Device Discovery: -67 e3ae212d9835 000F4241 00000CAB 99 44 53

83430767 Device Discovery: -87 e1a462b70c8b 00000001 00000CE9 99 59 50

83431061 Device Discovery: -85 f4de99e3458b 000F4244 00000CE9 0 0 131

83431347 Device Discovery: -82 e1a462b70c8b 00000001 00000CE9 99 59 50

83432001 Device Discovery: -79 f912898349f4 000F424B 00002000 99 55 24

83432022 BLE Scan Completed

83462183 BLE Scan Starting

83462530 Device Discovery: -87 e1a462b70c8b 00000001 00000CE9 99 59 50

83462541 Device Discovery: -83 f912898349f4 000F424B 00002000 99 55 24

83462827 Device Discovery: -85 f0bae1e36357 000F4250 00000CAB 0 0 132

83463460 Device Discovery: -67 e3ae212d9835 000F4241 00000CAB 99 44 53

83463791 Device Discovery: -86 f0bae1e36357 000F4250 00000CAB 0 0 132

83464071 Device Discovery: -82 e1a462b70c8b 00000001 00000CE9 99 59 50

83464082 Device Discovery: -90 f912898349f4 000F424B 00002000 99 55 24

83464107 Device Discovery: -88 f4de99e3458b 000F4244 00000CE9 0 0 131

83465019 Device Discovery: -76 e3ae212d9835 000F4241 00000CAB 99 44 53

83465039 Device Discovery: -82 e1a462b70c8b 00000001 00000CE9 99 59 50

83465652 Device Discovery: -85 f4de99e3458b 000F4244 00000CE9 0 0 131

83465977 Device Discovery: -77 e3ae212d9835 000F4241 00000CAB 99 44 53

83512360 BLE Scan Starting

83512360 scan is in progress, restarting process received evt -1
83562536 BLE Scan Starting

83562536 scan is in progress, restarting process received evt -1
83612699 BLE Scan Starting

83612699 scan is in progress, restarting process received evt -1
83662866 BLE Scan Starting

83662866 scan is in progress, restarting process received evt -1
83713032 BLE Scan Starting

83713032 scan is in progress, restarting process received evt -1
83763200 BLE Scan Starting

83763200 scan is in progress, restarting process received evt -1
83813369 BLE Scan Starting

83813369 scan is in progress, restarting process received evt -1
83863529 BLE Scan Starting

83863529 scan is in progress, restarting process received evt -1
83913692 BLE Scan Starting

83913692 scan is in progress, restarting process received evt -1
83963858 BLE Scan Starting

 So you will be able to see that we scan for 5 seconds ( we should receive a SCAN_COMPLETED_EVENT) and re-launch scanning after 30 seconds. When the issue appears, we never completed the previous scan. Then I attempt to stop the "unfinished" scan to be able to restart scanning again but it fails because I am not able to perform any discovery... the softdevice seems to be "stucked". Let me know if you understand my issue with the additional information provided. Thanks!

[Vidar Berg]

Hello,

I can't see anything wrong with the code and It's strange that scan starts without any errors. Are you using s140 v6.1.1? If not, please try it and see if it makes any differences. It does not seem to have any bug fixes directly related to this but found this in the release notes for s140 6.1.0:

Fixed an issue where the source of the timeout event might be set to BLE_GAP_TIMEOUT_SRC_CONN instead of BLE_GAP_TIMEOUT_SRC_SCAN when the scanner times out (DRGN-10000).

[Sandrine]

Indeed it is strange that no error is reported after attempting to re-start the scan. We are using softdevice version s140 v6.1.0. I saw there are 2 new releases after that one: s140 v6.1.1 and s140 v7.0.1. Would you advise to upgrade to s140 v7.0.1?

[Vidar Berg]

Sorry for the delay. I've briefly discussed the issue with our Softdevice team and it does not appear to be a known issue at least. So I'm not sure if it makes sense to update the Softdevice since you are on v6.1.0 already. But remember to increase the application's start address by 0x1000 if you want to give s140 v.7.0.1 a try. I will continue to investigate this here.