nrf5/bluetooth: Adding function to stop advertisment if onging

2 files changed, 15 insertions, 9 deletions

diff --git a/nrf5/bluetooth/ble_drv.c b/nrf5/bluetooth/ble_drv.c
index 45b07f18c..89daf06e1 100644
--- a/nrf5/bluetooth/ble_drv.c
+++ b/nrf5/bluetooth/ble_drv.c
@@ -553,21 +553,14 @@ bool ble_drv_advertise_data(ubluepy_advertise_data_t * p_adv_params) {
     } else {
         m_adv_params.type        = BLE_GAP_ADV_TYPE_ADV_NONCONN_IND;
     }
+
     m_adv_params.p_peer_addr = NULL;                                // undirected advertisement
     m_adv_params.fp          = BLE_GAP_ADV_FP_ANY;
     m_adv_params.interval    = MSEC_TO_UNITS(100, UNIT_0_625_MS);   // approx 8 ms
     m_adv_params.timeout     = 0;                                   // infinite advertisment
 
-#if (BLUETOOTH_SD == 132)
-    if (m_adv_in_progress == true) {
-        if (sd_ble_gap_adv_stop() != 0) {
-            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                      "Can not stop advertisment."));
-        }
-    }
-#endif
+    ble_drv_advertise_stop();
 
-    m_adv_in_progress = false;
     err_code = sd_ble_gap_adv_start(&m_adv_params);
     if (err_code != 0) {
         nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
@@ -579,6 +572,17 @@ bool ble_drv_advertise_data(ubluepy_advertise_data_t * p_adv_params) {
     return true;
 }
 
+void ble_drv_advertise_stop(void) {
+    if (m_adv_in_progress == true) {
+        uint32_t err_code;
+        if ((err_code = sd_ble_gap_adv_stop()) != 0) {
+            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                      "Can not stop advertisment. status: 0x" HEX2_FMT, (uint16_t)err_code));
+        }
+    }
+    m_adv_in_progress = false;
+}
+
 void ble_drv_attr_s_read(uint16_t conn_handle, uint16_t handle, uint16_t len, uint8_t * p_data) {
     ble_gatts_value_t gatts_value;
     memset(&gatts_value, 0, sizeof(gatts_value));
diff --git a/nrf5/bluetooth/ble_drv.h b/nrf5/bluetooth/ble_drv.h
index 40c38576b..4ebc24902 100644
--- a/nrf5/bluetooth/ble_drv.h
+++ b/nrf5/bluetooth/ble_drv.h
@@ -86,6 +86,8 @@ bool ble_drv_characteristic_add(ubluepy_characteristic_obj_t * p_char_obj);
 
 bool ble_drv_advertise_data(ubluepy_advertise_data_t * p_adv_params);
 
+void ble_drv_advertise_stop(void);
+
 void ble_drv_gap_event_handler_set(mp_obj_t obs, ble_drv_gap_evt_callback_t evt_handler);
 
 void ble_drv_gatts_event_handler_set(mp_obj_t obj, ble_drv_gatts_evt_callback_t evt_handler);