RE: nRF52840 unexpectedly stops scanning

Sandrine — Tue, 10 Sep 2019 15:12:59 GMT

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!