1 files changed, 55 insertions, 11 deletions

diff --git a/src/components/ble/NimbleController.cpp b/src/components/ble/NimbleController.cpp
index d8510bd..10eb429 100644
--- a/src/components/ble/NimbleController.cpp
+++ b/src/components/ble/NimbleController.cpp
@@ -2,6 +2,7 @@
 #include <cstring>
 
 #include <hal/nrf_rtc.h>
+#include <nrf_log.h>
 #define min // workaround: nimble's min/max macros conflict with libstdc++
 #define max
 #include <host/ble_gap.h>
@@ -23,14 +24,14 @@
 using namespace Pinetime::Controllers;
 
 NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
-                                   Pinetime::Controllers::Ble& bleController,
+                                   Ble& bleController,
                                    DateTime& dateTimeController,
-                                   Pinetime::Controllers::NotificationManager& notificationManager,
-                                   Controllers::Battery& batteryController,
+                                   NotificationManager& notificationManager,
+                                   Battery& batteryController,
                                    Pinetime::Drivers::SpiNorFlash& spiNorFlash,
-                                   Controllers::HeartRateController& heartRateController,
-                                   Controllers::MotionController& motionController,
-                                   Controllers::FS& fs)
+                                   HeartRateController& heartRateController,
+                                   MotionController& motionController,
+                                   FS& fs)
   : systemTask {systemTask},
     bleController {bleController},
     dateTimeController {dateTimeController},
@@ -76,6 +77,7 @@ int GAPEventCallback(struct ble_gap_event* event, void* arg) {
 
 void NimbleController::Init() {
   while (!ble_hs_synced()) {
+    vTaskDelay(10);
   }
 
   nptr = this;
@@ -184,7 +186,9 @@ int NimbleController::OnGAPEvent(ble_gap_event* event) {
     case BLE_GAP_EVENT_ADV_COMPLETE:
       NRF_LOG_INFO("Advertising event : BLE_GAP_EVENT_ADV_COMPLETE");
       NRF_LOG_INFO("reason=%d; status=%0X", event->adv_complete.reason, event->connect.status);
-      StartAdvertising();
+      if (bleController.IsRadioEnabled() && !bleController.IsConnected()) {
+        StartAdvertising();
+      }
       break;
 
     case BLE_GAP_EVENT_CONNECT:
@@ -220,9 +224,11 @@ int NimbleController::OnGAPEvent(ble_gap_event* event) {
       currentTimeClient.Reset();
       alertNotificationClient.Reset();
       connectionHandle = BLE_HS_CONN_HANDLE_NONE;
-      bleController.Disconnect();
-      fastAdvCount = 0;
-      StartAdvertising();
+      if(bleController.IsConnected()) {
+        bleController.Disconnect();
+        fastAdvCount = 0;
+        StartAdvertising();
+      }
       break;
 
     case BLE_GAP_EVENT_CONN_UPDATE:
@@ -278,7 +284,28 @@ int NimbleController::OnGAPEvent(ble_gap_event* event) {
       if (event->passkey.params.action == BLE_SM_IOACT_DISP) {
         struct ble_sm_io pkey = {0};
         pkey.action = event->passkey.params.action;
-        pkey.passkey = ble_ll_rand() % 1000000;
+
+        /*
+         * Passkey is a 6 digits code (1'000'000 possibilities).
+         * It is important every possible value has an equal probability
+         * of getting generated. Simply applying a modulo creates a bias
+         * since 2^32 is not a multiple of 1'000'000.
+         * To prevent that, we can reject values greater than 999'999.
+         *
+         * Rejecting values would happen a lot since 2^32-1 is way greater
+         * than 1'000'000. An optimisation is to use a multiple of 1'000'000.
+         * The greatest multiple of 1'000'000 lesser than 2^32-1 is
+         * 4'294'000'000.
+         *
+         * Great explanation at:
+         * https://research.kudelskisecurity.com/2020/07/28/the-definitive-guide-to-modulo-bias-and-how-to-avoid-it/
+         */
+        uint32_t passkey_rand;
+        do {
+          passkey_rand = ble_ll_rand();
+        } while (passkey_rand > 4293999999);
+        pkey.passkey = passkey_rand % 1000000;
+
         bleController.SetPairingKey(pkey.passkey);
         systemTask.PushMessage(Pinetime::System::Messages::OnPairing);
         ble_sm_inject_io(event->passkey.conn_handle, &pkey);
@@ -376,6 +403,23 @@ void NimbleController::NotifyBatteryLevel(uint8_t level) {
   }
 }
 
+void NimbleController::EnableRadio() {
+  bleController.EnableRadio();
+  bleController.Disconnect();
+  fastAdvCount = 0;
+  StartAdvertising();
+}
+
+void NimbleController::DisableRadio() {
+  bleController.DisableRadio();
+  if (bleController.IsConnected()) {
+    ble_gap_terminate(connectionHandle, BLE_ERR_REM_USER_CONN_TERM);
+    bleController.Disconnect();
+  } else {
+    ble_gap_adv_stop();
+  }
+}
+
 void NimbleController::PersistBond(struct ble_gap_conn_desc& desc) {
   union ble_store_key key;
   union ble_store_value our_sec, peer_sec, peer_cccd_set[MYNEWT_VAL(BLE_STORE_MAX_CCCDS)] = {0};