pbio/drv/bluetooth_btstack: Generalizations to prepare for POSIX variant.

bricks/_common/sources.mk

@@ -204,6 +204,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\
 	src/color/util.c \
 	src/control_settings.c \
 	src/control.c \
+	src/debug.c \
 	src/dcmotor.c \
 	src/differentiator.c \
 	src/drivebase.c \

bricks/virtualhub/mpconfigport.h

@@ -11,7 +11,7 @@
 
 // Pybricks modules
 #define PYBRICKS_PY_COMMON                      (1)
-#define PYBRICKS_PY_COMMON_BLE                  (0)
+#define PYBRICKS_PY_COMMON_BLE                  (1)
 #define PYBRICKS_PY_COMMON_CHARGER              (1)
 #define PYBRICKS_PY_COMMON_COLOR_LIGHT          (1)
 #define PYBRICKS_PY_COMMON_CONTROL              (1)
@@ -29,14 +29,22 @@
 #define PYBRICKS_PY_EV3DEVICES                  (0)
 #define PYBRICKS_PY_EXPERIMENTAL                (1)
 #define PYBRICKS_PY_HUBS                        (1)
-#define PYBRICKS_PY_IODEVICES                   (0)
+#define PYBRICKS_PY_IODEVICES                   (1)
+#define PYBRICKS_PY_IODEVICES_ANALOG_SENSOR     (0)
+#define PYBRICKS_PY_IODEVICES_DC_MOTOR          (0)
+#define PYBRICKS_PY_IODEVICES_I2C_DEVICE        (0)
+#define PYBRICKS_PY_IODEVICES_LUMP_DEVICE       (0)
+#define PYBRICKS_PY_IODEVICES_LWP3_DEVICE       (1)
+#define PYBRICKS_PY_IODEVICES_PUP_DEVICE        (0)
+#define PYBRICKS_PY_IODEVICES_UART_DEVICE       (0)
+#define PYBRICKS_PY_IODEVICES_XBOX_CONTROLLER   (1)
 #define PYBRICKS_PY_NXTDEVICES                  (0)
 #define PYBRICKS_PY_PARAMETERS                  (1)
 #define PYBRICKS_PY_PARAMETERS_BUTTON           (1)
 #define PYBRICKS_PY_PARAMETERS_ICON             (0)
 #define PYBRICKS_PY_PARAMETERS_IMAGE            (1)
 #define PYBRICKS_PY_PUPDEVICES                  (1)
-#define PYBRICKS_PY_PUPDEVICES_REMOTE           (0)
+#define PYBRICKS_PY_PUPDEVICES_REMOTE           (1)
 #define PYBRICKS_PY_DEVICES                     (1)
 #define PYBRICKS_PY_ROBOTICS                    (1)
 #define PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE    (0)

lib/pbio/drv/bluetooth/bluetooth.c

@@ -23,8 +23,8 @@
 #define DEBUG 0
 
 #if DEBUG
-#include <pbdrv/../../drv/uart/uart_debug_first_port.h>
-#define DEBUG_PRINT pbdrv_uart_debug_printf
+#include <pbio/debug.h>
+#define DEBUG_PRINT pbio_debug
 #else
 #define DEBUG_PRINT(...)
 #endif
@@ -158,6 +158,10 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect(pbdrv_bluetooth_periphe
 
     pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton;
 
+    if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) {
+        return PBIO_ERROR_INVALID_OP;
+    }
+
     // Can't connect if already connected or already busy.
     if (pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL) || peri->func) {
         return PBIO_ERROR_BUSY;
@@ -188,8 +192,12 @@ pbio_error_t pbdrv_bluetooth_peripheral_disconnect(void) {
         return PBIO_SUCCESS;
     }
 
+    // Clear notification handler to avoid receiving further data.
+    if (peri->config) {
+        peri->config->notification_handler = NULL;
+    }
+
     // Initialize operation for handling on the main thread.
-    peri->config->notification_handler = NULL;
     peri->func = pbdrv_bluetooth_peripheral_disconnect_func;
     peri->err = PBIO_ERROR_AGAIN;
     pbio_os_request_poll();
@@ -296,6 +304,10 @@ pbdrv_bluetooth_advertising_state_t pbdrv_bluetooth_advertising_state;
 
 pbio_error_t pbdrv_bluetooth_start_advertising(bool start) {
 
+    if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) {
+        return PBIO_ERROR_INVALID_OP;
+    }
+
     bool is_advertising = pbdrv_bluetooth_advertising_state == PBDRV_BLUETOOTH_ADVERTISING_STATE_ADVERTISING_PYBRICKS;
 
     // Already in requested state. This makes it safe to call stop advertising
@@ -326,6 +338,10 @@ uint8_t pbdrv_bluetooth_broadcast_data_size;
 
 pbio_error_t pbdrv_bluetooth_start_broadcasting(const uint8_t *data, size_t size) {
 
+    if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) {
+        return PBIO_ERROR_INVALID_OP;
+    }
+
     if (advertising_or_scan_func) {
         return PBIO_ERROR_BUSY;
     }
@@ -370,6 +386,10 @@ pbdrv_bluetooth_start_observing_callback_t pbdrv_bluetooth_observe_callback;
 
 pbio_error_t pbdrv_bluetooth_start_observing(pbdrv_bluetooth_start_observing_callback_t callback) {
 
+    if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) {
+        return PBIO_ERROR_INVALID_OP;
+    }
+
     if (advertising_or_scan_func) {
         return PBIO_ERROR_BUSY;
     }
@@ -589,14 +609,19 @@ pbio_error_t pbdrv_bluetooth_close_user_tasks(pbio_os_state_t *state, pbio_os_ti
 
 void pbdrv_bluetooth_deinit(void) {
 
+    // If Bluetooth is not even initialized, nothing to do.
+    if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) {
+        return;
+    }
+
     // Under normal operation ::pbdrv_bluetooth_close_user_tasks completes
     // normally and there should be no user activity at this point. If there
     // is, a task got stuck, so exit forcefully.
     pbio_os_state_t unused;
     if (pbdrv_bluetooth_await_advertise_or_scan_command(&unused, NULL) != PBIO_SUCCESS ||
         pbdrv_bluetooth_await_peripheral_command(&unused, NULL) != PBIO_SUCCESS) {
 
-        // Hard reset without waitng on completion of any process.
+        // Hard reset without waiting on completion of any process.
         pbdrv_bluetooth_controller_reset_hard();
         return;
     }

lib/pbio/drv/bluetooth/bluetooth_btstack.c

@@ -46,12 +46,29 @@
 #define DEBUG 0
 
 #if DEBUG
-#include <pbdrv/../../drv/uart/uart_debug_first_port.h>
-#define DEBUG_PRINT pbdrv_uart_debug_printf
+#include <pbio/debug.h>
+#define DEBUG_PRINT pbio_debug
 #else
 #define DEBUG_PRINT(...)
 #endif
 
+#if DEBUG == 2
+static void btstack_hci_dump_reset(void) {
+}
+static void btstack_hci_dump_log_packet(uint8_t packet_type, uint8_t in, uint8_t *packet, uint16_t len) {
+    pbio_debug("HCI %s packet type: %02x, len: %u\n", in ? "in" : "out", packet_type, len);
+}
+static void btstack_hci_dump_log_message(int log_level, const char *format, va_list argptr) {
+    pbio_debug_va(format, argptr);
+    pbio_debug("\n");
+}
+static const hci_dump_t pbdrv_bluetooth_btstack_hci_dump = {
+    .reset = btstack_hci_dump_reset,
+    .log_packet = btstack_hci_dump_log_packet,
+    .log_message = btstack_hci_dump_log_message,
+};
+#endif
+
 typedef enum {
     CON_STATE_NONE,
     CON_STATE_WAIT_ADV_IND,
@@ -107,28 +124,6 @@ static hci_con_handle_t uart_con_handle = HCI_CON_HANDLE_INVALID;
 static pup_handset_t handset;
 #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
 
-bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) {
-    #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
-    if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && le_con_handle != HCI_CON_HANDLE_INVALID) {
-        return true;
-    }
-
-    if (connection == PBDRV_BLUETOOTH_CONNECTION_PYBRICKS && pybricks_con_handle != HCI_CON_HANDLE_INVALID) {
-        return true;
-    }
-
-    if (connection == PBDRV_BLUETOOTH_CONNECTION_UART && uart_con_handle != HCI_CON_HANDLE_INVALID) {
-        return true;
-    }
-
-    if (connection == PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL && peripheral_singleton.con_handle != HCI_CON_HANDLE_INVALID) {
-        return true;
-    }
-    #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
-
-    return false;
-}
-
 #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
 
 /**
@@ -185,6 +180,39 @@ static void propagate_event(uint8_t *packet) {
     event_packet = NULL;
 }
 
+bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) {
+
+    // Nothing connected if HCI is not running.
+    if (bluetooth_thread_err != PBIO_ERROR_AGAIN) {
+        return false;
+    }
+
+    #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
+
+    if (connection == PBDRV_BLUETOOTH_CONNECTION_HCI) {
+        return true;
+    }
+
+    if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && le_con_handle != HCI_CON_HANDLE_INVALID) {
+        return true;
+    }
+
+    if (connection == PBDRV_BLUETOOTH_CONNECTION_PYBRICKS && pybricks_con_handle != HCI_CON_HANDLE_INVALID) {
+        return true;
+    }
+
+    if (connection == PBDRV_BLUETOOTH_CONNECTION_UART && uart_con_handle != HCI_CON_HANDLE_INVALID) {
+        return true;
+    }
+
+    if (connection == PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL && peripheral_singleton.con_handle != HCI_CON_HANDLE_INVALID) {
+        return true;
+    }
+    #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
+
+    return false;
+}
+
 #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
 static void nordic_spp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
     switch (packet_type) {
@@ -216,9 +244,38 @@ static void nordic_spp_packet_handler(uint8_t packet_type, uint16_t channel, uin
 }
 #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
 
+/**
+ * Parses the HCI Read Local Version Information command response.
+ *
+ * @param [out] info      The device info to populate.
+ * @param [in]  payload   The payload of the HCI event.
+ */
+static void parse_hci_local_version_information(pbdrv_bluetooth_btstack_local_version_info_t *info, const uint8_t *payload) {
+
+    info->hci_version = payload[1];
+    info->hci_revision = pbio_get_uint16_le(&payload[2]);
+    info->lmp_pal_version = payload[4];
+    info->manufacturer = pbio_get_uint16_le(&payload[5]);
+    info->lmp_pal_subversion = pbio_get_uint16_le(&payload[7]);
+
+    #if DEBUG
+    // Show version in ev3dev format.
+    uint16_t chip = (info->lmp_pal_subversion & 0x7C00) >> 10;
+    uint16_t min_ver = (info->lmp_pal_subversion & 0x007F);
+    uint16_t maj_ver = (info->lmp_pal_subversion & 0x0380) >> 7;
+    if (info->lmp_pal_subversion & 0x8000) {
+        maj_ver |= 0x0008;
+    }
+    DEBUG_PRINT("LMP %04x: TIInit_%d.%d.%d.bts\n", info->lmp_pal_subversion, chip, maj_ver, min_ver);
+    #endif
+}
+
 // currently, this function just handles the Powered Up handset control.
 static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
 
+    // Platform-specific platform handler has priority.
+    pbdrv_bluetooth_btstack_platform_packet_handler(packet_type, channel, packet, size);
+
     #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
     pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton;
     #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
@@ -228,19 +285,9 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
             const uint8_t *rp = hci_event_command_complete_get_return_parameters(packet);
             switch (hci_event_command_complete_get_command_opcode(packet)) {
                 case HCI_OPCODE_HCI_READ_LOCAL_VERSION_INFORMATION: {
-                    uint16_t lmp_pal_subversion = pbio_get_uint16_le(&rp[7]);
-                    pbdrv_bluetooth_btstack_set_chipset(lmp_pal_subversion);
-
-                    #if DEBUG
-                    // Show version in ev3dev format.
-                    uint16_t chip = (lmp_pal_subversion & 0x7C00) >> 10;
-                    uint16_t min_ver = (lmp_pal_subversion & 0x007F);
-                    uint16_t maj_ver = (lmp_pal_subversion & 0x0380) >> 7;
-                    if (lmp_pal_subversion & 0x8000) {
-                        maj_ver |= 0x0008;
-                    }
-                    DEBUG_PRINT("LMP %04x: TIInit_%d.%d.%d.bts\n", lmp_pal_subversion, chip, maj_ver, min_ver);
-                    #endif
+                    pbdrv_bluetooth_btstack_local_version_info_t info;
+                    parse_hci_local_version_information(&info, rp);
+                    pbdrv_bluetooth_btstack_set_chipset(&info);
                     break;
                 }
                 default:
@@ -957,6 +1004,10 @@ static pbio_error_t bluetooth_btstack_handle_power_control(pbio_os_state_t *stat
 
 pbio_error_t pbdrv_bluetooth_controller_reset(pbio_os_state_t *state, pbio_os_timer_t *timer) {
 
+    if (pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) {
+        return PBIO_ERROR_INVALID_OP;
+    }
+
     #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
     static pbio_os_state_t sub;
 
@@ -1000,6 +1051,17 @@ static void bluetooth_btstack_run_loop_execute(void) {
     // not used
 }
 
+/**
+ * The event loop runs on every system poll, but we only need to go through
+ * the btstack data sources when explicitly requested.
+ */
+static volatile bool pbdrv_bluetooth_btstack_poll_requested;
+
+static void pbdrv_bluetooth_btstack_run_loop_trigger(void) {
+    pbdrv_bluetooth_btstack_poll_requested = true;
+    pbio_os_request_poll();
+}
+
 static const btstack_run_loop_t bluetooth_btstack_run_loop = {
     .init = btstack_run_loop_base_init,
     .add_data_source = btstack_run_loop_base_add_data_source,
@@ -1012,15 +1074,9 @@ static const btstack_run_loop_t bluetooth_btstack_run_loop = {
     .execute = bluetooth_btstack_run_loop_execute,
     .dump_timer = btstack_run_loop_base_dump_timer,
     .get_time_ms = pbdrv_clock_get_ms,
+    .poll_data_sources_from_irq = pbdrv_bluetooth_btstack_run_loop_trigger,
 };
 
-static bool do_poll_handler;
-
-void pbdrv_bluetooth_btstack_run_loop_trigger(void) {
-    do_poll_handler = true;
-    pbio_os_request_poll();
-}
-
 static pbio_os_process_t pbdrv_bluetooth_hci_process;
 
 /**
@@ -1029,11 +1085,13 @@ static pbio_os_process_t pbdrv_bluetooth_hci_process;
  */
 static pbio_error_t pbdrv_bluetooth_hci_process_thread(pbio_os_state_t *state, void *context) {
 
-    if (do_poll_handler) {
-        do_poll_handler = false;
+    if (pbdrv_bluetooth_btstack_poll_requested) {
+        pbdrv_bluetooth_btstack_poll_requested = false;
         btstack_run_loop_base_poll_data_sources();
     }
 
+    pbdrv_bluetooth_btstack_platform_poll();
+
     static pbio_os_timer_t btstack_timer = {
         .duration = 1,
     };
@@ -1055,6 +1113,19 @@ const uint8_t cc256x_init_script[] = {};
 
 void pbdrv_bluetooth_init_hci(void) {
 
+    // Proceed to start Bluetooth process only if platform init passes.
+    pbio_error_t err = pbdrv_bluetooth_btstack_platform_init();
+    if (err != PBIO_SUCCESS) {
+        return;
+    }
+
+    #if DEBUG == 2
+    hci_dump_init(&pbdrv_bluetooth_btstack_hci_dump);
+    hci_dump_enable_log_level(HCI_DUMP_LOG_LEVEL_INFO, true);
+    hci_dump_enable_log_level(HCI_DUMP_LOG_LEVEL_ERROR, true);
+    hci_dump_enable_log_level(HCI_DUMP_LOG_LEVEL_DEBUG, true);
+    #endif
+
     static btstack_packet_callback_registration_t hci_event_callback_registration;
 
     #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
@@ -1065,8 +1136,9 @@ void pbdrv_bluetooth_init_hci(void) {
     btstack_memory_init();
     btstack_run_loop_init(&bluetooth_btstack_run_loop);
 
+    // Chipset is not set here. This is done dynamically when we get the
+    // local version information.
     hci_init(pdata->transport_instance(), pdata->transport_config());
-    hci_set_chipset(pdata->chipset_instance());
     hci_set_control(pdata->control_instance());
 
     // REVISIT: do we need to call btstack_chipset_cc256x_set_power() or btstack_chipset_cc256x_set_power_vector()?