pbio/drv/bluetooth_cc2640: Set disconnected state early.

The self-disconnect appeared to somehow cause the program not to be saved on
shutdown. This might be indicative of a completely different issue, but
settting the disconnected state early so pbsys won't still try to send status
messages across a nonexistent link appears to work around the issue.

lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c

@@ -128,6 +128,7 @@ static bool device_discovery_done;
 // used to synchronize advertising data handler
 static bool advertising_data_received;
 // handle to connected Bluetooth device
+static bool busy_disconnecting;
 static uint16_t conn_handle = NO_CONNECTION;
 static uint16_t conn_mtu;
 
@@ -385,7 +386,7 @@ void pbdrv_bluetooth_stop_advertising(void) {
 }
 
 bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) {
-    if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && conn_handle != NO_CONNECTION) {
+    if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && conn_handle != NO_CONNECTION && !busy_disconnecting) {
         return true;
     }
 
@@ -504,6 +505,19 @@ static PT_THREAD(peripheral_scan_and_connect_task(struct pt *pt, pbio_task_t *ta
         }
     }
 
+    // Optionally, disconnect from host (usually Pybricks Code).
+    if (conn_handle != NO_CONNECTION &&
+        (peri->options & PBDRV_BLUETOOTH_PERIPHERAL_OPTIONS_DISCONNECT_HOST)) {
+        DEBUG_PRINT_PT(pt, "Disconnect from Pybricks code (%d).\n", conn_handle);
+        // Guard used in pbdrv_bluetooth_is_connected so higher level processes
+        // won't try to send anything while we are disconnecting.
+        busy_disconnecting = true;
+        PT_WAIT_WHILE(pt, write_xfer_size);
+        GAP_TerminateLinkReq(conn_handle, 0x13);
+        PT_WAIT_UNTIL(pt, conn_handle == NO_CONNECTION);
+        busy_disconnecting = false;
+    }
+
 restart_scan:
 
     PROCESS_CONTEXT_BEGIN(&pbdrv_bluetooth_spi_process);
@@ -613,15 +627,6 @@ static PT_THREAD(peripheral_scan_and_connect_task(struct pt *pt, pbio_task_t *ta
     PT_WAIT_UNTIL(pt, hci_command_status);
     PT_WAIT_UNTIL(pt, device_discovery_done);
 
-    // Optionally, disconnect from host (usually Pybricks Code).
-    if (conn_handle != NO_CONNECTION &&
-        (peri->options & PBDRV_BLUETOOTH_PERIPHERAL_OPTIONS_DISCONNECT_HOST)) {
-        DEBUG_PRINT_PT(pt, "Disconnect from Pybricks code (%d).\n", conn_handle);
-        PT_WAIT_WHILE(pt, write_xfer_size);
-        GAP_TerminateLinkReq(conn_handle, 0x13);
-        PT_WAIT_UNTIL(pt, conn_handle == NO_CONNECTION);
-    }
-
     // Connect to the peripheral.
 
     assert(peri->con_handle == NO_CONNECTION);