2 files changed, 86 insertions, 1 deletions

diff --git a/extmod/btstack/modbluetooth_btstack.c b/extmod/btstack/modbluetooth_btstack.c
index 73f28ffad3..3e3852e5c2 100644
--- a/extmod/btstack/modbluetooth_btstack.c
+++ b/extmod/btstack/modbluetooth_btstack.c
@@ -24,20 +24,52 @@
  * THE SOFTWARE.
  */
 
+#include "py/runtime.h"
+#include "py/mperrno.h"
+#include "py/mphal.h"
+
 #if MICROPY_PY_BLUETOOTH && MICROPY_BLUETOOTH_BTSTACK
 
 #include "extmod/btstack/modbluetooth_btstack.h"
 #include "extmod/modbluetooth.h"
 
+#include "lib/btstack/src/btstack.h"
+
+volatile int mp_bluetooth_btstack_state = MP_BLUETOOTH_BTSTACK_STATE_OFF;
+
 int mp_bluetooth_init(void) {
+    btstack_memory_init();
+
+    MP_STATE_PORT(bluetooth_btstack_root_pointers) = m_new0(mp_bluetooth_btstack_root_pointers_t, 1);
+
+    mp_bluetooth_btstack_port_init();
+    mp_bluetooth_btstack_state = MP_BLUETOOTH_BTSTACK_STATE_STARTING;
+
+    l2cap_init();
+    le_device_db_init();
+    sm_init();
+
+    // Set blank ER/IR keys to suppress BTstack warning.
+    // TODO handle this correctly.
+    sm_key_t dummy_key;
+    memset(dummy_key, 0, sizeof(dummy_key));
+    sm_set_er(dummy_key);
+    sm_set_ir(dummy_key);
+
+    mp_bluetooth_btstack_port_start();
+    mp_bluetooth_btstack_state = MP_BLUETOOTH_BTSTACK_STATE_ACTIVE;
     return 0;
 }
 
 void mp_bluetooth_deinit(void) {
+    mp_bluetooth_btstack_port_deinit();
+    mp_bluetooth_btstack_state = MP_BLUETOOTH_BTSTACK_STATE_OFF;
+
+    MP_STATE_PORT(bluetooth_btstack_root_pointers) = NULL;
 }
 
 bool mp_bluetooth_is_enabled(void) {
-    return false;
+    return mp_bluetooth_btstack_state == MP_BLUETOOTH_BTSTACK_STATE_ACTIVE;
 }
 
 void mp_bluetooth_get_device_addr(uint8_t *addr) {
@@ -45,10 +77,44 @@ void mp_bluetooth_get_device_addr(uint8_t *addr) {
 }
 
 int mp_bluetooth_gap_advertise_start(bool connectable, int32_t interval_us, const uint8_t *adv_data, size_t adv_data_len, const uint8_t *sr_data, size_t sr_data_len) {
+    uint16_t adv_int_min = interval_us / 625;
+    uint16_t adv_int_max = interval_us / 625;
+    uint8_t adv_type = connectable ? 0 : 2;
+    bd_addr_t null_addr = {0};
+
+    uint8_t direct_address_type = 0;
+    uint8_t channel_map = 0x07; // Use all three broadcast channels.
+    uint8_t filter_policy = 0x00; // None.
+
+    gap_advertisements_set_params(adv_int_min, adv_int_max, adv_type, direct_address_type, null_addr, channel_map, filter_policy);
+
+    // Copy the adv_data and sr_data into a persistent buffer (which is findable via the btstack root pointers).
+    size_t total_bytes = adv_data_len + sr_data_len;
+    if (total_bytes > MP_STATE_PORT(bluetooth_btstack_root_pointers)->adv_data_alloc) {
+        // Resize if necessary.
+        MP_STATE_PORT(bluetooth_btstack_root_pointers)->adv_data = m_new(uint8_t, total_bytes);
+        MP_STATE_PORT(bluetooth_btstack_root_pointers)->adv_data_alloc = total_bytes;
+    }
+    uint8_t *data = MP_STATE_PORT(bluetooth_btstack_root_pointers)->adv_data;
+
+    if (adv_data) {
+        memcpy(data, (uint8_t *)adv_data, adv_data_len);
+        gap_advertisements_set_data(adv_data_len, data);
+        data += adv_data_len;
+    }
+    if (sr_data) {
+        memcpy(data, (uint8_t *)sr_data, sr_data_len);
+        gap_scan_response_set_data(sr_data_len, data);
+    }
+
+    gap_advertisements_enable(true);
     return 0;
 }
 
 void mp_bluetooth_gap_advertise_stop(void) {
+    gap_advertisements_enable(false);
+    MP_STATE_PORT(bluetooth_btstack_root_pointers)->adv_data_alloc = 0;
+    MP_STATE_PORT(bluetooth_btstack_root_pointers)->adv_data = NULL;
 }
 
 int mp_bluetooth_gatts_register_service_begin(bool append) {
diff --git a/extmod/btstack/modbluetooth_btstack.h b/extmod/btstack/modbluetooth_btstack.h
index f58d307480..9e8c6ad3fb 100644
--- a/extmod/btstack/modbluetooth_btstack.h
+++ b/extmod/btstack/modbluetooth_btstack.h
@@ -27,4 +27,23 @@
 #ifndef MICROPY_INCLUDED_EXTMOD_BTSTACK_MODBLUETOOTH_BTSTACK_H
 #define MICROPY_INCLUDED_EXTMOD_BTSTACK_MODBLUETOOTH_BTSTACK_H
 
+typedef struct _mp_bluetooth_btstack_root_pointers_t {
+    // This stores both the advertising data and the scan response data, concatenated together.
+    uint8_t *adv_data;
+    // Total length of both.
+    size_t adv_data_alloc;
+} mp_bluetooth_btstack_root_pointers_t;
+
+enum {
+    MP_BLUETOOTH_BTSTACK_STATE_OFF,
+    MP_BLUETOOTH_BTSTACK_STATE_STARTING,
+    MP_BLUETOOTH_BTSTACK_STATE_ACTIVE,
+};
+
+extern volatile int mp_bluetooth_btstack_state;
+
+void mp_bluetooth_btstack_port_init(void);
+void mp_bluetooth_btstack_port_deinit(void);
+void mp_bluetooth_btstack_port_start(void);
+
 #endif // MICROPY_INCLUDED_EXTMOD_BTSTACK_MODBLUETOOTH_BTSTACK_H