circuitpython/devices/ble_hci/common-hal/_bleio/Adapter.c

/*
 * This file is part of the MicroPython project, http://micropython.org/
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2020 Dan Halbert for Adafruit Industries
 * Copyright (c) 2016 Glenn Ruben Bakke
 * Copyright (c) 2018 Artur Pacholec
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "hci.h"

#include "py/gc.h"
#include "py/mphal.h"
#include "py/objstr.h"
#include "py/runtime.h"
#include "supervisor/shared/safe_mode.h"
#include "supervisor/shared/tick.h"
#include "supervisor/usb.h"
#include "shared-bindings/_bleio/__init__.h"
#include "shared-bindings/_bleio/Adapter.h"
#include "shared-bindings/_bleio/Address.h"
#include "shared-bindings/nvm/ByteArray.h"
#include "shared-bindings/_bleio/Connection.h"
#include "shared-bindings/_bleio/ScanEntry.h"
#include "shared-bindings/time/__init__.h"

#define MSEC_TO_UNITS(TIME, RESOLUTION) (((TIME) * 1000) / (RESOLUTION))
#define SEC_TO_UNITS(TIME, RESOLUTION) (((TIME) * 1000000) / (RESOLUTION))
#define UNITS_TO_SEC(TIME, RESOLUTION) (((TIME) * (RESOLUTION)) / 1000000)
// 0.625 msecs (625 usecs)
#define ADV_INTERVAL_UNIT_FLOAT_SECS (0.000625)
// Microseconds is the base unit. The macros above know that.
#define UNIT_0_625_MS (625)
#define UNIT_1_25_MS  (1250)
#define UNIT_10_MS    (10000)

// TODO make this settable from Python.
#define DEFAULT_TX_POWER 0  // 0 dBm
#define MAX_ANONYMOUS_ADV_TIMEOUT_SECS (60*15)
#define MAX_LIMITED_DISCOVERABLE_ADV_TIMEOUT_SECS (180)

#define BLE_MIN_CONN_INTERVAL        MSEC_TO_UNITS(15, UNIT_0_625_MS)
#define BLE_MAX_CONN_INTERVAL        MSEC_TO_UNITS(15, UNIT_0_625_MS)
#define BLE_SLAVE_LATENCY            0
#define BLE_CONN_SUP_TIMEOUT         MSEC_TO_UNITS(4000, UNIT_10_MS)

bleio_connection_internal_t bleio_connections[BLEIO_TOTAL_CONNECTION_COUNT];

// STATIC bool adapter_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
//     bleio_adapter_obj_t *self = (bleio_adapter_obj_t*)self_in;

//     // For debugging.
//     // mp_printf(&mp_plat_print, "Adapter event: 0x%04x\n", ble_evt->header.evt_id);

//     switch (ble_evt->header.evt_id) {
//         case BLE_GAP_EVT_CONNECTED: {
//             // Find an empty connection. One must always be available because the SD has the same
//             // total connection limit.
//             bleio_connection_internal_t *connection;
//             for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
//                 connection = &bleio_connections[i];
//                 if (connection->conn_handle == BLE_CONN_HANDLE_INVALID) {
//                     break;
//                 }
//             }

//             // Central has connected.
//             ble_gap_evt_connected_t* connected = &ble_evt->evt.gap_evt.params.connected;

//             connection->conn_handle = ble_evt->evt.gap_evt.conn_handle;
//             connection->connection_obj = mp_const_none;
//             connection->pair_status = PAIR_NOT_PAIRED;
//             connection->mtu = 0;

//             ble_drv_add_event_handler_entry(&connection->handler_entry, connection_on_ble_evt, connection);
//             self->connection_objs = NULL;

//             // Save the current connection parameters.
//             memcpy(&connection->conn_params, &connected->conn_params, sizeof(ble_gap_conn_params_t));

//             #if CIRCUITPY_VERBOSE_BLE
//             ble_gap_conn_params_t *cp = &connected->conn_params;
//             mp_printf(&mp_plat_print, "conn params: min_ci %d max_ci %d s_l %d sup_timeout %d\n", cp->min_conn_interval, cp->max_conn_interval, cp->slave_latency, cp->conn_sup_timeout);
//             #endif

//             // See if connection interval set by Central is out of range.
//             // If so, negotiate our preferred range.
//             ble_gap_conn_params_t conn_params;
//             sd_ble_gap_ppcp_get(&conn_params);
//             if (conn_params.min_conn_interval < connected->conn_params.min_conn_interval ||
//                 conn_params.min_conn_interval > connected->conn_params.max_conn_interval) {
//                 sd_ble_gap_conn_param_update(ble_evt->evt.gap_evt.conn_handle, &conn_params);
//             }
//             self->current_advertising_data = NULL;
//             break;
//         }
//         case BLE_GAP_EVT_DISCONNECTED: {
//             // Find the connection that was disconnected.
//             bleio_connection_internal_t *connection;
//             for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
//                 connection = &bleio_connections[i];
//                 if (connection->conn_handle == ble_evt->evt.gap_evt.conn_handle) {
//                     break;
//                 }
//             }
//             ble_drv_remove_event_handler(connection_on_ble_evt, connection);
//             connection->conn_handle = BLE_CONN_HANDLE_INVALID;
//             connection->pair_status = PAIR_NOT_PAIRED;
//             if (connection->connection_obj != mp_const_none) {
//                 bleio_connection_obj_t* obj = connection->connection_obj;
//                 obj->connection = NULL;
//                 obj->disconnect_reason = ble_evt->evt.gap_evt.params.disconnected.reason;
//             }
//             self->connection_objs = NULL;

//             break;
//         }

//         case BLE_GAP_EVT_ADV_SET_TERMINATED:
//             self->current_advertising_data = NULL;
//             break;

//         default:
//             // For debugging.
//             // mp_printf(&mp_plat_print, "Unhandled adapter event: 0x%04x\n", ble_evt->header.evt_id);
//             return false;
//             break;
//     }
//     return true;
// }

char default_ble_name[] = { 'C', 'I', 'R', 'C', 'U', 'I', 'T', 'P', 'Y', 0, 0, 0, 0 , 0};

STATIC void bleio_adapter_reset_name(bleio_adapter_obj_t *self) {
    uint8_t len = sizeof(default_ble_name) - 1;

    bt_addr_t addr;
    check_hci_error(hci_read_bd_addr(&addr));

    default_ble_name[len - 4] = nibble_to_hex_lower[addr.val[1] >> 4 & 0xf];
    default_ble_name[len - 3] = nibble_to_hex_lower[addr.val[1] & 0xf];
    default_ble_name[len - 2] = nibble_to_hex_lower[addr.val[0] >> 4 & 0xf];
    default_ble_name[len - 1] = nibble_to_hex_lower[addr.val[0] & 0xf];
    default_ble_name[len] = '\0'; // for now we add null for compatibility with C ASCIIZ strings

    common_hal_bleio_adapter_set_name(self, (char*) default_ble_name);
}

// Get various values and limits set by the adapter.
STATIC void bleio_adapter_get_info(bleio_adapter_obj_t *self) {

    // Get supported features.
    if (hci_le_read_local_supported_features(self->features) != HCI_OK) {
        mp_raise_bleio_BluetoothError(translate("Could not read BLE features"));
    }

    // Get ACL buffer info.
    uint16_t le_max_len;
    uint8_t le_max_num;
    if (hci_le_read_buffer_size(&le_max_len, &le_max_num) == HCI_OK) {
        self->max_acl_buffer_len = le_max_len;
        self->max_acl_num_buffers = le_max_num;
    } else {
        // LE Read Buffer Size not available; use the general Read Buffer Size.
        uint16_t acl_max_len;
        uint8_t sco_max_len;
        uint16_t acl_max_num;
        uint16_t sco_max_num;
        if (hci_read_buffer_size(&acl_max_len, &sco_max_len, &acl_max_num, &sco_max_num) != HCI_OK) {
            mp_raise_bleio_BluetoothError(translate("Could not read BLE buffer info"));
        }
        self->max_acl_buffer_len = acl_max_len;
        self->max_acl_num_buffers = acl_max_num;
    }

    // Get max advertising length if extended advertising is supported.
    if (BT_FEAT_LE_EXT_ADV(self->features)) {
        uint16_t max_adv_data_len;
        if (hci_le_read_maximum_advertising_data_length(&max_adv_data_len) != HCI_OK) {
            mp_raise_bleio_BluetoothError(translate("Could not get max advertising length"));
        }
        self->max_adv_data_len = max_adv_data_len;
    } else {
        self->max_adv_data_len = 31;
    }
}

void common_hal_bleio_adapter_hci_uart_init(bleio_adapter_obj_t *self, busio_uart_obj_t *uart, digitalio_digitalinout_obj_t *rts, digitalio_digitalinout_obj_t *cts) {
    self->hci_uart = uart;
    self->rts_digitalinout = rts;
    self->cts_digitalinout = cts;

    // Advertising-related fields are initialized by common_hal_bleio_adapter_set_enabled().
    self->enabled = false;

    common_hal_bleio_adapter_set_enabled(self, true);
    bleio_adapter_get_info(self);
    bleio_adapter_reset_name(self);
}

void common_hal_bleio_adapter_set_enabled(bleio_adapter_obj_t *self, bool enabled) {
    const bool is_enabled = common_hal_bleio_adapter_get_enabled(self);

    // Don't enable or disable twice
    if (is_enabled == enabled) {
        return;
    }

    self->enabled = enabled;

    // Stop any current activity; reset to known state.
    check_hci_error(hci_reset());
    self->now_advertising = false;
    self->extended_advertising = false;
    self->circuitpython_advertising = false;
    self->advertising_timeout_msecs = 0;
}

bool common_hal_bleio_adapter_get_enabled(bleio_adapter_obj_t *self) {
    return self->enabled;
}

bleio_address_obj_t *common_hal_bleio_adapter_get_address(bleio_adapter_obj_t *self) {
    bt_addr_t addr;
    check_hci_error(hci_read_bd_addr(&addr));

    bleio_address_obj_t *address = m_new_obj(bleio_address_obj_t);
    address->base.type = &bleio_address_type;

    common_hal_bleio_address_construct(address, addr.val, BT_ADDR_LE_PUBLIC);
    return address;
}

mp_obj_str_t* common_hal_bleio_adapter_get_name(bleio_adapter_obj_t *self) {
    return self->name;
}

void common_hal_bleio_adapter_set_name(bleio_adapter_obj_t *self, const char* name) {
    self->name = mp_obj_new_str(name, strlen(name));
}


// STATIC bool scan_on_ble_evt(ble_evt_t *ble_evt, void *scan_results_in) {
//     bleio_scanresults_obj_t *scan_results = (bleio_scanresults_obj_t*)scan_results_in;

//     if (ble_evt->header.evt_id == BLE_GAP_EVT_TIMEOUT &&
//         ble_evt->evt.gap_evt.params.timeout.src == BLE_GAP_TIMEOUT_SRC_SCAN) {
//         shared_module_bleio_scanresults_set_done(scan_results, true);
//         ble_drv_remove_event_handler(scan_on_ble_evt, scan_results);
//         return true;
//     }

//     if (ble_evt->header.evt_id != BLE_GAP_EVT_ADV_REPORT) {
//         return false;
//     }
//     ble_gap_evt_adv_report_t *report = &ble_evt->evt.gap_evt.params.adv_report;

//     shared_module_bleio_scanresults_append(scan_results,
//                                            supervisor_ticks_ms64(),
//                                            report->type.connectable,
//                                            report->type.scan_response,
//                                            report->rssi,
//                                            report->peer_addr.addr,
//                                            report->peer_addr.addr_type,
//                                            report->data.p_data,
//                                            report->data.len);

//     const uint32_t err_code = sd_ble_gap_scan_start(NULL, scan_results->common_hal_data);
//     if (err_code != NRF_SUCCESS) {
//         // TODO: Pass the error into the scan results so it can throw an exception.
//         scan_results->done = true;
//     }
//     return true;
// }

mp_obj_t common_hal_bleio_adapter_start_scan(bleio_adapter_obj_t *self, uint8_t* prefixes, size_t prefix_length, bool extended, mp_int_t buffer_size, mp_float_t timeout, mp_float_t interval, mp_float_t window, mp_int_t minimum_rssi, bool active) {
    if (self->scan_results != NULL) {
        if (!shared_module_bleio_scanresults_get_done(self->scan_results)) {
            mp_raise_bleio_BluetoothError(translate("Scan already in progess. Stop with stop_scan."));
        }
        self->scan_results = NULL;
    }
    self->scan_results = shared_module_bleio_new_scanresults(buffer_size, prefixes, prefix_length, minimum_rssi);

    // size_t max_packet_size = extended ? BLE_GAP_SCAN_BUFFER_EXTENDED_MAX_SUPPORTED : BLE_GAP_SCAN_BUFFER_MAX;
    // uint8_t *raw_data = m_malloc(sizeof(ble_data_t) + max_packet_size, false);
    // ble_data_t * sd_data = (ble_data_t *) raw_data;
    // self->scan_results->common_hal_data = sd_data;
    // sd_data->len = max_packet_size;
    // sd_data->p_data = raw_data + sizeof(ble_data_t);

    // ble_drv_add_event_handler(scan_on_ble_evt, self->scan_results);

    // uint32_t nrf_timeout = SEC_TO_UNITS(timeout, UNIT_10_MS);
    // if (timeout <= 0.0001) {
    //     nrf_timeout = BLE_GAP_SCAN_TIMEOUT_UNLIMITED;
    // }

    // ble_gap_scan_params_t scan_params = {
    //     .extended = extended,
    //     .interval = SEC_TO_UNITS(interval, UNIT_0_625_MS),
    //     .timeout = nrf_timeout,
    //     .window = SEC_TO_UNITS(window, UNIT_0_625_MS),
    //     .scan_phys = BLE_GAP_PHY_1MBPS,
    //     .active = active
    // };
    // uint32_t err_code;
    // vm_used_ble = true;
    // err_code = sd_ble_gap_scan_start(&scan_params, sd_data);

    // if (err_code != NRF_SUCCESS) {
    //     self->scan_results = NULL;
    //     ble_drv_remove_event_handler(scan_on_ble_evt, self->scan_results);
    //     check_nrf_error(err_code);
    // }

    return MP_OBJ_FROM_PTR(self->scan_results);
}

void common_hal_bleio_adapter_stop_scan(bleio_adapter_obj_t *self) {
    check_hci_error(hci_le_set_scan_enable(BT_HCI_LE_SCAN_DISABLE, BT_HCI_LE_SCAN_FILTER_DUP_DISABLE));
    shared_module_bleio_scanresults_set_done(self->scan_results, true);
    self->scan_results = NULL;
}

// typedef struct {
//     uint16_t conn_handle;
//     volatile bool done;
// } connect_info_t;

// STATIC bool connect_on_ble_evt(ble_evt_t *ble_evt, void *info_in) {
//     connect_info_t *info = (connect_info_t*)info_in;

//     switch (ble_evt->header.evt_id) {
//         case BLE_GAP_EVT_CONNECTED:
//             info->conn_handle = ble_evt->evt.gap_evt.conn_handle;
//             info->done = true;

//             break;

//         case BLE_GAP_EVT_TIMEOUT:
//             // Handle will be invalid.
//             info->done = true;
//             break;
//         default:
//             // For debugging.
//             // mp_printf(&mp_plat_print, "Unhandled central event: 0x%04x\n", ble_evt->header.evt_id);
//             return false;
//             break;
//     }
//     return true;
// }

mp_obj_t common_hal_bleio_adapter_connect(bleio_adapter_obj_t *self, bleio_address_obj_t *address, mp_float_t timeout) {

    // ble_gap_addr_t addr;

    // addr.addr_type = address->type;
    // mp_buffer_info_t address_buf_info;
    // mp_get_buffer_raise(address->bytes, &address_buf_info, MP_BUFFER_READ);
    // memcpy(addr.addr, (uint8_t *) address_buf_info.buf, NUM_BLEIO_ADDRESS_BYTES);

    // ble_gap_scan_params_t scan_params = {
    //     .interval = MSEC_TO_UNITS(100, UNIT_0_625_MS),
    //     .window = MSEC_TO_UNITS(100, UNIT_0_625_MS),
    //     .scan_phys = BLE_GAP_PHY_1MBPS,
    //     // timeout of 0 means no timeout
    //     .timeout = SEC_TO_UNITS(timeout, UNIT_10_MS),
    // };

    // ble_gap_conn_params_t conn_params = {
    //     .conn_sup_timeout = MSEC_TO_UNITS(4000, UNIT_10_MS),
    //     .min_conn_interval = MSEC_TO_UNITS(15, UNIT_1_25_MS),
    //     .max_conn_interval = MSEC_TO_UNITS(300, UNIT_1_25_MS),
    //     .slave_latency = 0,          // number of conn events
    // };

    // connect_info_t event_info;
    // ble_drv_add_event_handler(connect_on_ble_evt, &event_info);
    // event_info.done = false;

    vm_used_ble = true;
    // uint32_t err_code = sd_ble_gap_connect(&addr, &scan_params, &conn_params, BLE_CONN_CFG_TAG_CUSTOM);

    // if (err_code != NRF_SUCCESS) {
    //     ble_drv_remove_event_handler(connect_on_ble_evt, &event_info);
    //     check_nrf_error(err_code);
    // }

    // while (!event_info.done) {
    //     RUN_BACKGROUND_TASKS;
    // }

    // ble_drv_remove_event_handler(connect_on_ble_evt, &event_info);

    // uint16_t conn_handle = event_info.conn_handle;
    // if (conn_handle == BLE_CONN_HANDLE_INVALID) {
    //     mp_raise_bleio_BluetoothError(translate("Failed to connect: timeout"));
    // }

    // // Negotiate for better PHY, larger MTU and data lengths since we are the central. These are
    // // nice-to-haves so ignore any errors.
    // ble_gap_phys_t const phys = {
    //     .rx_phys = BLE_GAP_PHY_AUTO,
    //     .tx_phys = BLE_GAP_PHY_AUTO,
    // };
    // sd_ble_gap_phy_update(conn_handle, &phys);
    // sd_ble_gattc_exchange_mtu_request(conn_handle, BLE_GATTS_VAR_ATTR_LEN_MAX);
    // sd_ble_gap_data_length_update(conn_handle, NULL, NULL);

    // Make the connection object and return it.
    // for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
    //     bleio_connection_internal_t *connection = &bleio_connections[i];
    //     if (connection->conn_handle == conn_handle) {
    //         return bleio_connection_new_from_internal(connection);
    //     }
    // }

    mp_raise_bleio_BluetoothError(translate("Failed to connect: internal error"));

    return mp_const_none;
}

// The nRF SD 6.1.0 can only do one concurrent advertisement so share the advertising handle.
//FIX uint8_t adv_handle = BLE_GAP_ADV_SET_HANDLE_NOT_SET;

STATIC void check_data_fit(size_t data_len, bool connectable) {
    //FIX  if (data_len > BLE_GAP_ADV_SET_DATA_SIZE_EXTENDED_MAX_SUPPORTED ||
    //     (connectable && data_len > BLE_GAP_ADV_SET_DATA_SIZE_EXTENDED_CONNECTABLE_MAX_SUPPORTED)) {
    //     mp_raise_ValueError(translate("Data too large for advertisement packet"));
    // }
}

// STATIC bool advertising_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
//     bleio_adapter_obj_t *self = (bleio_adapter_obj_t*)self_in;

//     switch (ble_evt->header.evt_id) {
//         case BLE_GAP_EVT_ADV_SET_TERMINATED:
//             common_hal_bleio_adapter_stop_advertising(self);
//             ble_drv_remove_event_handler(advertising_on_ble_evt, self_in);
//             break;

//         default:
//             // For debugging.
//             // mp_printf(&mp_plat_print, "Unhandled advertising event: 0x%04x\n", ble_evt->header.evt_id);
//             return false;
//             break;
//     }
//     return true;
// }

uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable, bool anonymous, uint32_t timeout, float interval, uint8_t *advertising_data, uint16_t advertising_data_len, uint8_t *scan_response_data, uint16_t scan_response_data_len) {
    if (self->now_advertising) {
        if (self->circuitpython_advertising) {
            common_hal_bleio_adapter_stop_advertising(self);
        } else {
            // User-requested advertising.
            // TODO allow multiple advertisements.
            // Already advertising. Can't advertise twice.
            return 1;
        }
    }

    // Peer address, which we don't use (no directed advertising).
    bt_addr_le_t empty_addr = { 0 };

    bool extended =
        advertising_data_len > self->max_adv_data_len || scan_response_data_len > self->max_adv_data_len;

    if (extended) {
        if (!BT_FEAT_LE_EXT_ADV(self->features)) {
            mp_raise_bleio_BluetoothError(translate("Data length needs extended advertising, but this adapter does not support it"));
        }

        uint16_t props = 0;
        if (connectable) {
            props |= BT_HCI_LE_ADV_PROP_CONN;
        }
        if (scan_response_data_len > 0) {
            props |= BT_HCI_LE_ADV_PROP_SCAN;
        }

        // Advertising interval.
        uint32_t interval_units = SEC_TO_UNITS(interval, UNIT_0_625_MS);

        check_hci_error(
            hci_le_set_extended_advertising_parameters(
                0,              // handle
                props,          // adv properties
                interval_units, // min interval
                interval_units, // max interval
                0b111,          // channel map: channels 37, 38, 39
                anonymous ? BT_ADDR_LE_RANDOM : BT_ADDR_LE_PUBLIC,
                &empty_addr,    // peer_addr,
                0x00,           // filter policy: no filter
                DEFAULT_TX_POWER,
                BT_HCI_LE_EXT_SCAN_PHY_1M, // Secondary PHY to use
                0x00,                      // AUX_ADV_IND shall be sent prior to next adv event
                BT_HCI_LE_EXT_SCAN_PHY_1M, // Secondary PHY to use
                0x00,                      // Advertising SID
                0x00                       // Scan req notify disable
                ));

        // We can use the duration mechanism provided, instead of our own.
        self->advertising_timeout_msecs = 0;

        uint8_t handle[1] = { 0 };
        uint16_t duration_10msec[1] = { timeout * 100 };
        uint8_t max_ext_adv_evts[1] =  { 0 };
        check_hci_error(
            hci_le_set_extended_advertising_enable(
                BT_HCI_LE_ADV_ENABLE,
                1,                // one advertising set.
                handle,
                duration_10msec,
                max_ext_adv_evts
                ));

        self->extended_advertising = true;
    } else {
        // Legacy advertising (not extended).

        uint8_t adv_type;
        if (connectable) {
            // Connectable, scannable, undirected.
            adv_type = BT_HCI_ADV_IND;
        } else if (scan_response_data_len > 0) {
            // Unconnectable, scannable, undirected.
            adv_type = BT_HCI_ADV_SCAN_IND;
        } else {
            // Unconnectable, unscannable, undirected.
            adv_type = BT_HCI_ADV_NONCONN_IND;
        }

        // Advertising interval.
        uint16_t interval_units = SEC_TO_UNITS(interval, UNIT_0_625_MS);

        check_hci_error(
            hci_le_set_advertising_parameters(
                interval_units, // min interval
                interval_units, // max interval
                adv_type,
                anonymous ? BT_ADDR_LE_RANDOM : BT_ADDR_LE_PUBLIC,
                &empty_addr,
                0b111,          // channel map: channels 37, 38, 39
                0x00            // filter policy: no filter
                ));

        // The HCI commands expect 31 octets, even though the actual data length may be shorter.
        uint8_t full_data[31] = { 0 };
        memcpy(full_data, advertising_data, MIN(sizeof(full_data), advertising_data_len));
        check_hci_error(hci_le_set_advertising_data(advertising_data_len, full_data));
        memset(full_data, 0, sizeof(full_data));
        if (scan_response_data_len > 0) {
            memcpy(full_data, advertising_data, MIN(sizeof(full_data), scan_response_data_len));
            check_hci_error(hci_le_set_scan_response_data(scan_response_data_len, full_data));
        }

        // No duration mechanism is provided for legacy advertising, so we need to do our own.
        self->advertising_timeout_msecs = timeout * 1000;
        self->advertising_start_ticks = supervisor_ticks_ms64();

        // Start advertising.
        check_hci_error(hci_le_set_advertising_enable(BT_HCI_LE_ADV_ENABLE));
        self->extended_advertising = false;
    } // end legacy advertising setup

    vm_used_ble = true;
    self->now_advertising = true;
    return 0;
}

void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable, bool anonymous, uint32_t timeout, mp_float_t interval, mp_buffer_info_t *advertising_data_bufinfo, mp_buffer_info_t *scan_response_data_bufinfo) {
    // interval value has already been validated.

    check_data_fit(advertising_data_bufinfo->len, connectable);
    check_data_fit(scan_response_data_bufinfo->len, connectable);

    if (advertising_data_bufinfo->len > 31 && scan_response_data_bufinfo->len > 0) {
        mp_raise_bleio_BluetoothError(translate("Extended advertisements with scan response not supported."));
    }

    // Anonymous mode requires a timeout so that we don't continue to broadcast
    // the same data while cycling the MAC address -- otherwise, what's the
    // point of randomizing the MAC address?
    if (timeout == 0 && anonymous) {
        timeout = MAX_ANONYMOUS_ADV_TIMEOUT_SECS;
    } else {
        if (timeout > MAX_LIMITED_DISCOVERABLE_ADV_TIMEOUT_SECS) {
            mp_raise_bleio_BluetoothError(translate("Timeout is too long: Maximum timeout length is %d seconds"),
                                          MAX_LIMITED_DISCOVERABLE_ADV_TIMEOUT_SECS);
        }
    }

    const uint32_t result =_common_hal_bleio_adapter_start_advertising(
        self, connectable, anonymous, timeout, interval,
        advertising_data_bufinfo->buf,
        advertising_data_bufinfo->len,
        scan_response_data_bufinfo->buf,
        scan_response_data_bufinfo->len);

    if (result) {
        mp_raise_bleio_BluetoothError(translate("Already advertising"));
    }
    self->circuitpython_advertising = false;
}

void common_hal_bleio_adapter_stop_advertising(bleio_adapter_obj_t *self) {
    self->now_advertising = false;
    self->extended_advertising = false;
    self->circuitpython_advertising = false;
    int result = hci_le_set_advertising_enable(BT_HCI_LE_ADV_DISABLE);
    // OK if we're already stopped.
    if (result != BT_HCI_ERR_CMD_DISALLOWED) {
        check_hci_error(result);
    }

    //TODO startup CircuitPython advertising again.
}

bool common_hal_bleio_adapter_get_advertising(bleio_adapter_obj_t *self) {
    return self->now_advertising;
}

bool common_hal_bleio_adapter_get_connected(bleio_adapter_obj_t *self) {
    for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
        bleio_connection_internal_t *connection = &bleio_connections[i];
        if (connection->conn_handle != BLE_CONN_HANDLE_INVALID) {
            return true;
        }
    }
    return false;
}

mp_obj_t common_hal_bleio_adapter_get_connections(bleio_adapter_obj_t *self) {
    if (self->connection_objs != NULL) {
        return self->connection_objs;
    }
    size_t total_connected = 0;
    mp_obj_t items[BLEIO_TOTAL_CONNECTION_COUNT];
    for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
        bleio_connection_internal_t *connection = &bleio_connections[i];
        if (connection->conn_handle != BLE_CONN_HANDLE_INVALID) {
            if (connection->connection_obj == mp_const_none) {
                connection->connection_obj = bleio_connection_new_from_internal(connection);
            }
            items[total_connected] = connection->connection_obj;
            total_connected++;
        }
    }
    self->connection_objs = mp_obj_new_tuple(total_connected, items);
    return self->connection_objs;
}

void common_hal_bleio_adapter_erase_bonding(bleio_adapter_obj_t *self) {
    //FIX bonding_erase_storage();
}

void bleio_adapter_gc_collect(bleio_adapter_obj_t* adapter) {
    gc_collect_root((void**)adapter, sizeof(bleio_adapter_obj_t) / sizeof(size_t));
    gc_collect_root((void**)bleio_connections, sizeof(bleio_connections) / sizeof(size_t));
}

void bleio_adapter_reset(bleio_adapter_obj_t* adapter) {
    common_hal_bleio_adapter_stop_scan(adapter);
    if (adapter->now_advertising) {
        common_hal_bleio_adapter_stop_advertising(adapter);
    }

    adapter->connection_objs = NULL;
    for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
        bleio_connection_internal_t *connection = &bleio_connections[i];
        // Disconnect all connections with Python state cleanly. Keep any supervisor-only connections.
        if (connection->connection_obj != mp_const_none &&
            connection->conn_handle != BLE_CONN_HANDLE_INVALID) {
            common_hal_bleio_connection_disconnect(connection);
        }
        connection->connection_obj = mp_const_none;
    }
}

void bleio_adapter_background(bleio_adapter_obj_t* adapter) {
    if (adapter->advertising_timeout_msecs > 0 &&
        supervisor_ticks_ms64() - adapter->advertising_start_ticks > adapter->advertising_timeout_msecs) {
        adapter->advertising_timeout_msecs = 0;
        common_hal_bleio_adapter_stop_advertising(adapter);
    }

    hci_poll_for_incoming_pkt();
}