diff --git a/epicardium/ble/ble_main.c b/epicardium/ble/ble_main.c
index 693cecf2b8a8a93e09248313cee8e43e2ba40f4a..678ca50163bd344d71d9fa874390fa7bc0ee4f15 100644
--- a/epicardium/ble/ble_main.c
+++ b/epicardium/ble/ble_main.c
@@ -44,6 +44,8 @@
 #include "modules/log.h"
 
 static bool active;
+static uint8_t advertising_mode = APP_MODE_NONE;
+static uint8_t advertising_mode_target = APP_MODE_NONE;
 static enum ble_event_type ble_event;
 
 /**************************************************************************************************
@@ -355,15 +357,9 @@ static void bleSetup(bleMsg_t *pMsg)
   AppAdvSetData(APP_ADV_DATA_CONNECTABLE, 0, NULL);
   AppAdvSetData(APP_SCAN_DATA_CONNECTABLE, 0, NULL);
 
-  /* We only want to be bondable when the appropriate dialog is open */
-  AppSetBondable(FALSE);
-  /* TODO: Sadly, not advertising leads to a higher current consumption... */
-  if(AppDbCheckBonded() == FALSE) {
-    AppAdvStop();
-  } else {
-    AppAdvStart(APP_MODE_CONNECTABLE);
-  }
   active = true;
+  /* TODO: Sadly, not advertising leads to a higher current consumption... */
+  epic_ble_set_bondable(false);
 }
 
 void epic_ble_set_bondable(bool bondable)
@@ -373,18 +369,47 @@ void epic_ble_set_bondable(bool bondable)
 	}
 
 	if(bondable) {
-		LOG_INFO("ble", "Making bondable and discoverable");
 		/* We need to stop advertising in between or the
-		 * adv set will not be changed... */
-		AppAdvStop();
+		 * adv set will not be changed.
+		 * Also need to wait for the stop operation to finish
+		 * before we can start again
+		 * Also need to set the variables first as we don't
+		 * have a lock on the stack.*/
 		AppSetBondable(TRUE);
-		AppAdvStart(APP_MODE_DISCOVERABLE);
+		if(advertising_mode != APP_MODE_DISCOVERABLE) {
+			LOG_INFO("ble", "Making bondable and discoverable");
+			if(advertising_mode != APP_MODE_NONE) {
+				advertising_mode_target = APP_MODE_DISCOVERABLE;
+				advertising_mode = APP_MODE_NONE;
+				AppAdvStop();
+			} else {
+				advertising_mode = APP_MODE_DISCOVERABLE;
+				advertising_mode_target = APP_MODE_DISCOVERABLE;
+				AppAdvStart(advertising_mode);
+			}
+		}
 	} else {
-		LOG_INFO("ble", "Making connectable");
-		AppAdvStop();
 		AppSetBondable(FALSE);
 		if(AppDbCheckBonded()) {
-			AppAdvStart(APP_MODE_CONNECTABLE);
+			if(advertising_mode != APP_MODE_CONNECTABLE) {
+				LOG_INFO("ble", "Bonded. Making connectable");
+				if(advertising_mode != APP_MODE_NONE) {
+					advertising_mode_target = APP_MODE_CONNECTABLE;
+					advertising_mode = APP_MODE_NONE;
+					AppAdvStop();
+				} else {
+					advertising_mode = APP_MODE_CONNECTABLE;
+					advertising_mode_target = APP_MODE_CONNECTABLE;
+					AppAdvStart(advertising_mode);
+				}
+			}
+		} else {
+			if(advertising_mode != APP_MODE_NONE) {
+				LOG_INFO("ble", "Not bonded. Stop advertising");
+				advertising_mode = APP_MODE_NONE;
+				advertising_mode_target = APP_MODE_NONE;
+				AppAdvStop();
+			}
 		}
 	}
 }
@@ -475,7 +500,11 @@ static void bleProcMsg(bleMsg_t *pMsg)
       break;
 
     case DM_ADV_STOP_IND:
-      LOG_INFO("ble", "Advertisement stopped");
+      LOG_INFO("ble", "Advertisement stopped %u %u", advertising_mode, advertising_mode_target);
+      if(advertising_mode != advertising_mode_target) {
+        advertising_mode = advertising_mode_target;
+        AppAdvStart(advertising_mode);
+      }
       break;
 
     case DM_CONN_OPEN_IND: