pbio/drv/bluetooth: Cache matching advertisements.

When re-using an existing connection, this lets us make sure all filters match as if making a new connection.

Author: laurensvalk

lib/pbio/drv/bluetooth/bluetooth.c

@@ -188,10 +188,14 @@ pbio_error_t pbdrv_bluetooth_peripheral_get_connected(pbdrv_bluetooth_peripheral
             continue;
         }
 
-        // Callbacks must be the same.
+        // Callbacks must be the same, and still match with the given user.
+        // This ensures that we fail as intended when the same classes are used
+        // but the user has configured different filters such as the name.
         if (peri->config.match_adv != config->match_adv ||
             peri->config.match_adv_rsp != config->match_adv_rsp ||
-            peri->config.notification_handler != config->notification_handler) {
+            peri->config.notification_handler != config->notification_handler ||
+            !peri->config.match_adv(user, peri->config.match_adv_data, peri->config.match_adv_data_len) ||
+            !peri->config.match_adv_rsp(user, peri->config.match_adv_rsp_data, peri->config.match_adv_rsp_data_len)) {
             continue;
         }
 

lib/pbio/drv/bluetooth/bluetooth_btstack.c

@@ -726,6 +726,10 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s
 
     DEBUG_PRINT("Advertisement matched, waiting for scan response\n");
 
+    // Copy data to allow virtual re-connect in a new user program.
+    peri->config.match_adv_data_len = gap_event_advertising_report_get_data_length(event_packet);
+    memcpy(peri->config.match_adv_data, gap_event_advertising_report_get_data(event_packet), peri->config.match_adv_data_len);
+
     // Wait for advertising response unless timed out or cancelled.
     PBIO_OS_AWAIT_UNTIL(state, scan_timed_out || peri->cancel ||
         (hci_event_is_type(event_packet, GAP_EVENT_ADVERTISING_REPORT) && ({
@@ -748,12 +752,7 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s
     // that event data for processing.
     const uint8_t *data = gap_event_advertising_report_get_data(event_packet);
     uint8_t data_len = gap_event_advertising_report_get_data_length(event_packet);
-    if (peri->config.match_adv_rsp(peri->user, data, data_len)) {
-        // Copy name for later use.
-        if (data[1] == BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME) {
-            memcpy(peri->name, &data[2], sizeof(peri->name));
-        }
-    } else {
+    if (!peri->config.match_adv_rsp(peri->user, data, data_len)) {
         // We got a valid scan response from the device that matched our
         // advertising filter, but it did not match the response filter (e.g.
         // requested name did not match), so scan again.
@@ -763,6 +762,14 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s
 
     DEBUG_PRINT("Scan response matched, initiate connection to %s.\n", bd_addr_to_str(peri->bdaddr));
 
+    // Copy name for later use.
+    if (data[1] == BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME) {
+        memcpy(peri->name, &data[2], sizeof(peri->name));
+    }
+    // Copy response data to allow virtual re-connect in a new user program.
+    peri->config.match_adv_rsp_data_len = data_len;
+    memcpy(peri->config.match_adv_rsp_data, data, data_len);
+
     // We can stop scanning now.
     gap_stop_scan();
 

lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c

@@ -381,6 +381,9 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s
         // save the Bluetooth address for later
         peri->bdaddr_type = subevt->bdaddr_type;
         memcpy(peri->bdaddr, subevt->bdaddr, sizeof(peri->bdaddr));
+        // Copy data to allow virtual re-connect in a new user program.
+        peri->config.match_adv_data_len = subevt->data_length;
+        memcpy(peri->config.match_adv_data, subevt->data_RSSI, subevt->data_length);
         break;
     }
 
@@ -411,6 +414,9 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s
 
         // All checks passed, so copy the device name for later use.
         memcpy(peri->name, &subevt->data_RSSI[2], sizeof(peri->name));
+        // Copy data to allow virtual re-connect in a new user program.
+        peri->config.match_adv_rsp_data_len = subevt->data_length;
+        memcpy(peri->config.match_adv_rsp_data, subevt->data_RSSI, subevt->data_length);
         break;
     }
 

lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c

@@ -458,6 +458,10 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s
         // Save the Bluetooth address for later comparison against response.
         peri->bdaddr_type = read_buf[10];
         memcpy(peri->bdaddr, &read_buf[11], sizeof(peri->bdaddr));
+
+        // Copy data to allow virtual re-connect in a new user program.
+        peri->config.match_adv_data_len = read_buf[18];
+        memcpy(peri->config.match_adv_data, &read_buf[19], read_buf[18]);
         break;
     }
 
@@ -498,6 +502,9 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s
 
         // All checks passed, so copy the device name for later use.
         memcpy(peri->name, &read_buf[21], sizeof(peri->name));
+        // Copy data to allow virtual re-connect in a new user program.
+        peri->config.match_adv_rsp_data_len = read_buf[18];
+        memcpy(peri->config.match_adv_rsp_data, &read_buf[19], read_buf[18]);
         break;
     }
 

lib/pbio/include/pbdrv/bluetooth.h

@@ -123,6 +123,14 @@ typedef struct {
     pbdrv_bluetooth_peripheral_options_t options;
     /** Timeout before aborting scan and connect. Use 0 for no timeout. */
     uint32_t timeout;
+    /** Last matching advertisement data for this peripheral. */
+    uint8_t match_adv_data[PBDRV_BLUETOOTH_MAX_ADV_SIZE];
+    /** Last matching advertisement data length for this peripheral. */
+    uint8_t match_adv_data_len;
+    /** Last matching advertisement response data for this peripheral. */
+    uint8_t match_adv_rsp_data[PBDRV_BLUETOOTH_MAX_ADV_SIZE];
+    /** Last matching advertisement response data length for this peripheral. */
+    uint8_t match_adv_rsp_data_len;
 } pbdrv_bluetooth_peripheral_connect_config_t;
 
 /** Platform-specific state needed to operate the peripheral. */