diff --git a/extmod/modbluetooth_nimble.c b/extmod/modbluetooth_nimble.c new file mode 100644 index 0000000000..3a6d1cba43 --- /dev/null +++ b/extmod/modbluetooth_nimble.c @@ -0,0 +1,854 @@ +/* + * This file is part of the MicroPython project, http://micropython.org/ + * + * The MIT License (MIT) + * + * Copyright (c) 2019 Damien P. George + * Copyright (c) 2019 Jim Mussared + * + * 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 "py/runtime.h" +#include "py/mperrno.h" +#include "py/mphal.h" +#include "systick.h" +#include "pendsv.h" + +#if MICROPY_PY_BLUETOOTH && MICROPY_BLUETOOTH_NIMBLE + +#ifndef MICROPY_PY_BLUETOOTH_DEFAULT_NAME +#define MICROPY_PY_BLUETOOTH_DEFAULT_NAME "PYBD" +#endif + +#include "extmod/modbluetooth.h" + +#include "host/ble_hs.h" +#include "host/util/util.h" +#include "nimble/ble.h" +#include "nimble/nimble_port.h" +#include "services/gap/ble_svc_gap.h" +#include "transport/uart/ble_hci_uart.h" + +#define DEBUG_MALLOC_printf(...) //printf(__VA_ARGS__) +#define DEBUG_EVENT_printf(...) //printf(__VA_ARGS__) + +STATIC int8_t ble_hs_err_to_errno_table[] = { + [BLE_HS_EAGAIN] = MP_EAGAIN, + [BLE_HS_EALREADY] = MP_EALREADY, + [BLE_HS_EINVAL] = MP_EINVAL, + [BLE_HS_EMSGSIZE] = MP_EIO, + [BLE_HS_ENOENT] = MP_ENOENT, + [BLE_HS_ENOMEM] = MP_ENOMEM, + [BLE_HS_ENOTCONN] = MP_ENOTCONN, + [BLE_HS_ENOTSUP] = MP_EOPNOTSUPP, + [BLE_HS_EAPP] = MP_EIO, + [BLE_HS_EBADDATA] = MP_EIO, + [BLE_HS_EOS] = MP_EIO, + [BLE_HS_ECONTROLLER] = MP_EIO, + [BLE_HS_ETIMEOUT] = MP_ETIMEDOUT, + [BLE_HS_EDONE] = MP_EIO, // TODO: Maybe should be MP_EISCONN (connect uses this for "already connected"). + [BLE_HS_EBUSY] = MP_EBUSY, + [BLE_HS_EREJECT] = MP_EIO, + [BLE_HS_EUNKNOWN] = MP_EIO, + [BLE_HS_EROLE] = MP_EIO, + [BLE_HS_ETIMEOUT_HCI] = MP_EIO, + [BLE_HS_ENOMEM_EVT] = MP_EIO, + [BLE_HS_ENOADDR] = MP_EIO, + [BLE_HS_ENOTSYNCED] = MP_EIO, + [BLE_HS_EAUTHEN] = MP_EIO, + [BLE_HS_EAUTHOR] = MP_EIO, + [BLE_HS_EENCRYPT] = MP_EIO, + [BLE_HS_EENCRYPT_KEY_SZ] = MP_EIO, + [BLE_HS_ESTORE_CAP] = MP_EIO, + [BLE_HS_ESTORE_FAIL] = MP_EIO, + [BLE_HS_EPREEMPTED] = MP_EIO, + [BLE_HS_EDISABLED] = MP_EIO, +}; + +STATIC int ble_hs_err_to_errno(int err) { + if (0 <= err && err < MP_ARRAY_SIZE(ble_hs_err_to_errno_table)) { + return ble_hs_err_to_errno_table[err]; + } else { + return MP_EIO; + } +} + +// Maintain a linked list of heap memory that we've passed to Nimble, +// discoverable via the bluetooth_nimble_memory root pointer. + +// MP_STATE_PORT(bluetooth_nimble_memory) is a pointer to [next, prev, data...]. + +// TODO: This is duplicated from mbedtls. Perhaps make this a generic feature? +void *m_malloc_bluetooth(size_t size) { + void **ptr = m_malloc0(size + 2 * sizeof(uintptr_t)); + if (MP_STATE_PORT(bluetooth_nimble_memory) != NULL) { + MP_STATE_PORT(bluetooth_nimble_memory)[0] = ptr; + } + ptr[0] = NULL; + ptr[1] = MP_STATE_PORT(bluetooth_nimble_memory); + MP_STATE_PORT(bluetooth_nimble_memory) = ptr; + return &ptr[2]; +} + +#define m_new_bluetooth(type, num) ((type*)m_malloc_bluetooth(sizeof(type) * (num))) + +void m_free_bluetooth(void *ptr_in) { + void **ptr = &((void**)ptr_in)[-2]; + if (ptr[1] != NULL) { + ((void**)ptr[1])[0] = ptr[0]; + } + if (ptr[0] != NULL) { + ((void**)ptr[0])[1] = ptr[1]; + } else { + MP_STATE_PORT(bluetooth_nimble_memory) = ptr[1]; + } + m_free(ptr); +} + +// Check if a nimble ptr is tracked. +// If it isn't, that means that it's from a previous soft-reset cycle. +STATIC bool is_valid_nimble_malloc(void *ptr) { + DEBUG_MALLOC_printf("NIMBLE is_valid_nimble_malloc(%p)\n", ptr); + void** search = MP_STATE_PORT(bluetooth_nimble_memory); + while (search) { + if (&search[2] == ptr) { + return true; + } + + search = (void**)search[1]; + } + return false; +} + +void *nimble_malloc(size_t size) { + DEBUG_MALLOC_printf("NIMBLE malloc(%u)\n", (uint)size); + void* ptr = m_malloc_bluetooth(size); + DEBUG_MALLOC_printf(" --> %p\n", ptr); + return ptr; +} + +// Only free if it's still a valid pointer. +void nimble_free(void *ptr) { + DEBUG_MALLOC_printf("NIMBLE free(%p)\n", ptr); + if (ptr && is_valid_nimble_malloc(ptr)) { + m_free_bluetooth(ptr); + } +} + +// Only realloc if it's still a valid pointer. Otherwise just malloc. +void *nimble_realloc(void *ptr, size_t size) { + // This is only used by ble_gatts.c to grow the queue of pending services to be registered. + DEBUG_MALLOC_printf("NIMBLE realloc(%p, %u)\n", ptr, (uint)size); + void *ptr2 = nimble_malloc(size); + if (ptr && is_valid_nimble_malloc(ptr)) { + // If it's a realloc and we still have the old data, then copy it. + // This will happen as we add services. + memcpy(ptr2, ptr, size); + m_free_bluetooth(ptr); + } + return ptr2; +} + +STATIC ble_uuid_t* create_nimble_uuid(const mp_obj_bluetooth_uuid_t *uuid) { + if (uuid->type == MP_BLUETOOTH_UUID_TYPE_16) { + ble_uuid16_t *result = m_new(ble_uuid16_t, 1); + result->u.type = BLE_UUID_TYPE_16; + result->value = uuid->uuid._16; + return (ble_uuid_t*)result; + } else if (uuid->type == MP_BLUETOOTH_UUID_TYPE_32) { + ble_uuid32_t *result = m_new(ble_uuid32_t, 1); + result->u.type = BLE_UUID_TYPE_32; + result->value = uuid->uuid._32; + return (ble_uuid_t*)result; + } else if (uuid->type == MP_BLUETOOTH_UUID_TYPE_128) { + ble_uuid128_t *result = m_new(ble_uuid128_t, 1); + result->u.type = BLE_UUID_TYPE_128; + memcpy(result->value, uuid->uuid._128, 16); + return (ble_uuid_t*)result; + } else { + return NULL; + } +} + +#if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE + +STATIC mp_obj_bluetooth_uuid_t create_mp_uuid(const ble_uuid_any_t *uuid) { + mp_obj_bluetooth_uuid_t result; + switch (uuid->u.type) { + case BLE_UUID_TYPE_16: + result.type = MP_BLUETOOTH_UUID_TYPE_16; + result.uuid._16 = uuid->u16.value; + break; + case BLE_UUID_TYPE_32: + result.type = MP_BLUETOOTH_UUID_TYPE_32; + result.uuid._32 = uuid->u32.value; + break; + case BLE_UUID_TYPE_128: + result.type = MP_BLUETOOTH_UUID_TYPE_128; + memcpy(result.uuid._128, uuid->u128.value, 16); + break; + default: + assert(false); + } + return result; +} + +// modbluetooth (and the layers above it) work in BE addresses, Nimble works in LE. +STATIC void reverse_addr_byte_order(uint8_t *addr_out, const uint8_t *addr_in) { + for (int i = 0; i < 6; ++i) { + addr_out[i] = addr_in[5-i]; + } +} + +STATIC ble_addr_t create_nimble_addr(uint8_t addr_type, const uint8_t *addr) { + ble_addr_t addr_nimble; + addr_nimble.type = addr_type; + // Incoming addr is from modbluetooth (BE), so copy and convert to LE for Nimble. + reverse_addr_byte_order(addr_nimble.val, addr); + return addr_nimble; +} + +#endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE + +STATIC mp_map_t *gatts_db = MP_OBJ_NULL; + +typedef struct { + uint8_t *data; + size_t data_alloc; + size_t data_len; +} gatts_db_entry_t; + +/******************************************************************************/ +// RUN LOOP + +enum { + BLE_STATE_OFF, + BLE_STATE_STARTING, + BLE_STATE_ACTIVE, +}; + +static volatile int ble_state = BLE_STATE_OFF; + +extern void nimble_uart_process(void); +extern void os_eventq_run_all(void); +extern void os_callout_process(void); + +// Hook for pendsv poller to run this periodically every 128ms +#define NIMBLE_TICK(tick) (((tick) & ~(SYSTICK_DISPATCH_NUM_SLOTS - 1) & 0x7f) == 0) + +void nimble_poll(void) { + if (ble_state == BLE_STATE_OFF) { + return; + } + + nimble_uart_process(); + os_callout_process(); + os_eventq_run_all(); +} + +void mod_bluetooth_nimble_poll_wrapper(uint32_t ticks_ms) { + if (NIMBLE_TICK(ticks_ms)) { + pendsv_schedule_dispatch(PENDSV_DISPATCH_NIMBLE, nimble_poll); + } +} + +/******************************************************************************/ +// BINDINGS + +STATIC void reset_cb(int reason) { + (void)reason; +} + +STATIC void sync_cb(void) { + ble_hs_util_ensure_addr(0); // prefer public address + ble_svc_gap_device_name_set(MICROPY_PY_BLUETOOTH_DEFAULT_NAME); + + ble_state = BLE_STATE_ACTIVE; +} + +STATIC void create_gatts_db_entry(uint16_t handle) { + mp_map_elem_t *elem = mp_map_lookup(gatts_db, MP_OBJ_NEW_SMALL_INT(handle), MP_MAP_LOOKUP_ADD_IF_NOT_FOUND); + gatts_db_entry_t *entry = m_new(gatts_db_entry_t, 1); + entry->data = m_new(uint8_t, MP_BLUETOOTH_MAX_ATTR_SIZE); + entry->data_alloc = MP_BLUETOOTH_MAX_ATTR_SIZE; + entry->data_len = 0; + elem->value = MP_OBJ_FROM_PTR(entry); +} + +STATIC void gatts_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg) { + switch (ctxt->op) { + case BLE_GATT_REGISTER_OP_SVC: + // Called when a service is successfully registered. + DEBUG_EVENT_printf("gatts_register_cb: svc uuid=%p handle=%d\n", &ctxt->svc.svc_def->uuid, ctxt->svc.handle); + break; + + case BLE_GATT_REGISTER_OP_CHR: + // Called when a characteristic is successfully registered. + DEBUG_EVENT_printf("gatts_register_cb: chr uuid=%p def_handle=%d val_handle=%d\n", &ctxt->chr.chr_def->uuid, ctxt->chr.def_handle, ctxt->chr.val_handle); + + // Note: We will get this event for the default GAP Service, meaning that we allocate storage for the + // "device name" and "appearance" characteristics, even though we never see the reads for them. + // TODO: Possibly check if the service UUID is 0x1801 and ignore? + + // Allocate the gatts_db storage for this characteristic. + // Although this function is a callback, it's called synchronously from ble_hs_sched_start/ble_gatts_start, so safe to allocate. + create_gatts_db_entry(ctxt->chr.val_handle); + break; + + case BLE_GATT_REGISTER_OP_DSC: + // Called when a descriptor is successfully registered. + // Note: This is event is not called for the CCCD. + DEBUG_EVENT_printf("gatts_register_cb: dsc uuid=%p handle=%d\n", &ctxt->dsc.dsc_def->uuid, ctxt->dsc.handle); + + // See above, safe to alloc. + create_gatts_db_entry(ctxt->dsc.handle); + + // Unlike characteristics, we have to manually provide a way to get the handle back to the register method. + *((uint16_t*)ctxt->dsc.dsc_def->arg) = ctxt->dsc.handle; + break; + + default: + DEBUG_EVENT_printf("gatts_register_cb: unknown op %d\n", ctxt->op); + break; + } +} + +STATIC int gap_event_cb(struct ble_gap_event *event, void *arg) { + DEBUG_EVENT_printf("gap_event_cb: type=%d\n", event->type); + struct ble_gap_conn_desc desc; + uint8_t addr[6] = {0}; + + switch (event->type) { + case BLE_GAP_EVENT_CONNECT: + if (event->connect.status == 0) { + // Connection established. + ble_gap_conn_find(event->connect.conn_handle, &desc); + reverse_addr_byte_order(addr, desc.peer_id_addr.val); + mp_bluetooth_gap_on_connected_disconnected(MP_BLUETOOTH_IRQ_CENTRAL_CONNECT, event->connect.conn_handle, desc.peer_id_addr.type, addr); + } else { + // Connection failed. + mp_bluetooth_gap_on_connected_disconnected(MP_BLUETOOTH_IRQ_CENTRAL_DISCONNECT, event->connect.conn_handle, 0xff, addr); + } + break; + + case BLE_GAP_EVENT_DISCONNECT: + // Disconnect. + reverse_addr_byte_order(addr, event->disconnect.conn.peer_id_addr.val); + mp_bluetooth_gap_on_connected_disconnected(MP_BLUETOOTH_IRQ_CENTRAL_DISCONNECT, event->disconnect.conn.conn_handle, event->disconnect.conn.peer_id_addr.type, addr); + break; + } + + return 0; +} + +int mp_bluetooth_init(void) { + // Clean up if necessary. + mp_bluetooth_deinit(); + + MP_STATE_PORT(bluetooth_nimble_memory) = NULL; + + ble_hs_cfg.reset_cb = reset_cb; + ble_hs_cfg.sync_cb = sync_cb; + ble_hs_cfg.gatts_register_cb = gatts_register_cb; + ble_hs_cfg.store_status_cb = ble_store_util_status_rr; + + gatts_db = m_new_bluetooth(mp_map_t, 1); + + ble_hci_uart_init(); + nimble_port_init(); + + // By default, just register the default gap service. + ble_svc_gap_init(); + + ble_state = BLE_STATE_STARTING; + + ble_hs_start(); + + // Wait for sync callback + while (ble_state != BLE_STATE_ACTIVE) { + MICROPY_EVENT_POLL_HOOK + } + + return 0; +} + +// Called when the host stop procedure has completed. +STATIC void ble_hs_shutdown_stop_cb(int status, void *arg) { + ble_state = BLE_STATE_OFF; +} + +STATIC struct ble_hs_stop_listener ble_hs_shutdown_stop_listener; + +void mp_bluetooth_deinit(void) { + if (ble_state == BLE_STATE_OFF) { + return; + } + + mp_bluetooth_gap_advertise_stop(); + #if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE + mp_bluetooth_gap_scan_stop(); + #endif + + ble_hs_stop(&ble_hs_shutdown_stop_listener, ble_hs_shutdown_stop_cb, + NULL); + + while (ble_state != BLE_STATE_OFF) { + MICROPY_EVENT_POLL_HOOK + } + + // TODO: This should be a port-specific hook. + #ifdef pyb_pin_BT_REG_ON + mp_hal_pin_low(pyb_pin_BT_REG_ON); + #endif +} + +bool mp_bluetooth_is_enabled(void) { + return ble_state == BLE_STATE_ACTIVE; +} + +void mp_bluetooth_get_device_addr(uint8_t *addr) { + mp_hal_get_mac(MP_HAL_MAC_BDADDR, addr); +} + +int mp_bluetooth_gap_advertise_start(bool connectable, uint16_t interval_ms, const uint8_t *adv_data, size_t adv_data_len, const uint8_t *sr_data, size_t sr_data_len) { + int ret; + + mp_bluetooth_gap_advertise_stop(); + + if ((adv_data != NULL) && (adv_data_len > 0)) { + ret = ble_gap_adv_set_data(adv_data, adv_data_len); + if (ret != 0) { + return ble_hs_err_to_errno(ret); + } + } + + if ((sr_data != NULL) && (sr_data_len > 0)) { + ret = ble_gap_adv_rsp_set_data(sr_data, sr_data_len); + if (ret != 0) { + return ble_hs_err_to_errno(ret); + } + } + + // Convert from 1ms to 0.625ms units. + interval_ms = interval_ms * 8 / 5; + if (interval_ms < 0x20 || interval_ms > 0x4000) { + return MP_EINVAL; + } + + struct ble_gap_adv_params adv_params = { + .conn_mode = connectable ? BLE_GAP_CONN_MODE_UND : BLE_GAP_CONN_MODE_NON, + .disc_mode = BLE_GAP_DISC_MODE_GEN, + .itvl_min = interval_ms, + .itvl_max = interval_ms, + .channel_map = 7, // all 3 channels + }; + + ret = ble_gap_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, BLE_HS_FOREVER, &adv_params, gap_event_cb, NULL); + if (ret != 0) { + return ble_hs_err_to_errno(ret); + } + + return 0; +} + +void mp_bluetooth_gap_advertise_stop(void) { + if (ble_gap_adv_active()) { + ble_gap_adv_stop(); + } +} + +static int characteristic_access_cb(uint16_t conn_handle, uint16_t value_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { + DEBUG_EVENT_printf("characteristic_access_cb: conn_handle=%u value_handle=%u op=%u\n", conn_handle, value_handle, ctxt->op); + mp_map_elem_t *elem; + gatts_db_entry_t *entry; + switch (ctxt->op) { + case BLE_GATT_ACCESS_OP_READ_CHR: + case BLE_GATT_ACCESS_OP_READ_DSC: + #if MICROPY_PY_BLUETOOTH_GATTS_ON_READ_CALLBACK + // Allow Python code to override (by using gatts_write), or deny (by returning false) the read. + if (!mp_bluetooth_gatts_on_read_request(conn_handle, value_handle)) { + return BLE_ATT_ERR_READ_NOT_PERMITTED; + } + #endif + + elem = mp_map_lookup(gatts_db, MP_OBJ_NEW_SMALL_INT(value_handle), MP_MAP_LOOKUP); + if (!elem) { + return BLE_ATT_ERR_ATTR_NOT_FOUND; + } + entry = MP_OBJ_TO_PTR(elem->value); + + os_mbuf_append(ctxt->om, entry->data, entry->data_len); + + return 0; + case BLE_GATT_ACCESS_OP_WRITE_CHR: + case BLE_GATT_ACCESS_OP_WRITE_DSC: + elem = mp_map_lookup(gatts_db, MP_OBJ_NEW_SMALL_INT(value_handle), MP_MAP_LOOKUP); + if (!elem) { + return BLE_ATT_ERR_ATTR_NOT_FOUND; + } + entry = MP_OBJ_TO_PTR(elem->value); + entry->data_len = MIN(MP_BLUETOOTH_MAX_ATTR_SIZE, OS_MBUF_PKTLEN(ctxt->om)); + os_mbuf_copydata(ctxt->om, 0, entry->data_len, entry->data); + + mp_bluetooth_gatts_on_write(conn_handle, value_handle); + + return 0; + } + return BLE_ATT_ERR_UNLIKELY; +} + +int mp_bluetooth_gatts_register_service_begin(bool reset) { + int ret = ble_gatts_reset(); + if (ret != 0) { + return ble_hs_err_to_errno(ret); + } + + // Reset the gatt characteristic value db. + mp_map_init(gatts_db, 0); + + // By default, just register the default gap service. + ble_svc_gap_init(); + + return 0; +} + +int mp_bluetooth_gatts_register_service_end() { + int ret = ble_gatts_start(); + if (ret != 0) { + return ble_hs_err_to_errno(ret); + } + + return 0; +} + +int mp_bluetooth_gatts_register_service(mp_obj_bluetooth_uuid_t *service_uuid, mp_obj_bluetooth_uuid_t **characteristic_uuids, uint8_t *characteristic_flags, mp_obj_bluetooth_uuid_t **descriptor_uuids, uint8_t *descriptor_flags, uint8_t *num_descriptors, uint16_t *handles, size_t num_characteristics) { + // TODO: These allocs need to last until mp_bluetooth_gatts_register_service_end. + // Using m_new_bluetooth means they get leaked, but m_new would mean that they wouldn't be findable by GC. + size_t handle_index = 0; + size_t descriptor_index = 0; + + struct ble_gatt_chr_def *characteristics = m_new_bluetooth(struct ble_gatt_chr_def, num_characteristics + 1); + for (size_t i = 0; i < num_characteristics; ++i) { + characteristics[i].uuid = create_nimble_uuid(characteristic_uuids[i]); + characteristics[i].access_cb = characteristic_access_cb; + characteristics[i].arg = NULL; + characteristics[i].flags = characteristic_flags[i]; + characteristics[i].min_key_size = 0; + characteristics[i].val_handle = &handles[handle_index]; + ++handle_index; + + if (num_descriptors[i] == 0) { + characteristics[i].descriptors = NULL; + } else { + struct ble_gatt_dsc_def *descriptors = m_new_bluetooth(struct ble_gatt_dsc_def, num_descriptors[i] + 1); + + for (size_t j = 0; j < num_descriptors[i]; ++j) { + descriptors[j].uuid = create_nimble_uuid(descriptor_uuids[descriptor_index]); + descriptors[j].access_cb = characteristic_access_cb; + descriptors[j].att_flags = descriptor_flags[i]; + descriptors[j].min_key_size = 0; + // Unlike characteristic, Nimble doesn't provide an automatic way to remember the handle, so use the arg. + descriptors[j].arg = &handles[handle_index]; + ++descriptor_index; + ++handle_index; + } + descriptors[num_descriptors[i]].uuid = NULL; // no more descriptors + + characteristics[i].descriptors = descriptors; + } + } + characteristics[num_characteristics].uuid = NULL; // no more characteristics + + struct ble_gatt_svc_def *service = m_new_bluetooth(struct ble_gatt_svc_def, 2); + service[0].type = BLE_GATT_SVC_TYPE_PRIMARY; + service[0].uuid = create_nimble_uuid(service_uuid); + service[0].includes = NULL; + service[0].characteristics = characteristics; + service[1].type = 0; // no more services + + // Note: advertising must be stopped for gatts registration to work + + int ret = ble_gatts_count_cfg(service); + if (ret != 0) { + return ble_hs_err_to_errno(ret); + } + + ret = ble_gatts_add_svcs(service); + if (ret != 0) { + return ble_hs_err_to_errno(ret); + } + + return 0; +} + +int mp_bluetooth_gap_disconnect(uint16_t conn_handle) { + return ble_hs_err_to_errno(ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM)); +} + +int mp_bluetooth_gatts_read(uint16_t value_handle, uint8_t **value, size_t *value_len) { + mp_map_elem_t *elem = mp_map_lookup(gatts_db, MP_OBJ_NEW_SMALL_INT(value_handle), MP_MAP_LOOKUP); + if (!elem) { + return MP_EINVAL; + } + gatts_db_entry_t *entry = MP_OBJ_TO_PTR(elem->value); + *value = entry->data; + *value_len = entry->data_len; + return 0; +} + +int mp_bluetooth_gatts_write(uint16_t value_handle, const uint8_t *value, size_t value_len) { + mp_map_elem_t *elem = mp_map_lookup(gatts_db, MP_OBJ_NEW_SMALL_INT(value_handle), MP_MAP_LOOKUP); + if (!elem) { + return MP_EINVAL; + } + gatts_db_entry_t *entry = MP_OBJ_TO_PTR(elem->value); + if (value_len > entry->data_alloc) { + entry->data = m_new(uint8_t, value_len); + entry->data_alloc = value_len; + } + + memcpy(entry->data, value, value_len); + entry->data_len = value_len; + return 0; +} + +// TODO: Could use ble_gatts_chr_updated to send to all subscribed centrals. + +int mp_bluetooth_gatts_notify(uint16_t conn_handle, uint16_t value_handle) { + // Confusingly, notify/notify_custom/indicate are "gattc" function (even though they're used by peripherals (i.e. gatt servers)). + // See https://www.mail-archive.com/dev@mynewt.apache.org/msg01293.html + return ble_hs_err_to_errno(ble_gattc_notify(conn_handle, value_handle)); +} + +int mp_bluetooth_gatts_notify_send(uint16_t conn_handle, uint16_t value_handle, const uint8_t *value, size_t *value_len) { + struct os_mbuf *om = ble_hs_mbuf_from_flat(value, *value_len); + if (om == NULL) { + return -1; + } + // TODO: check that notify_custom takes ownership of om, if not os_mbuf_free_chain(om). + return ble_hs_err_to_errno(ble_gattc_notify_custom(conn_handle, value_handle, om)); +} + +int mp_bluetooth_gatts_indicate(uint16_t conn_handle, uint16_t value_handle) { + return ble_hs_err_to_errno(ble_gattc_indicate(conn_handle, value_handle)); +} + +#if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE + +STATIC int gap_scan_cb(struct ble_gap_event *event, void *arg) { + DEBUG_EVENT_printf("gap_scan_cb: event=%d\n", event->type); + + if (event->type == BLE_GAP_EVENT_DISC_COMPLETE) { + mp_bluetooth_gap_on_scan_complete(); + return 0; + } + + if (event->type != BLE_GAP_EVENT_DISC) { + return 0; + } + + DEBUG_EVENT_printf(" --> type %d\n", event->disc.event_type); + + + if (event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_ADV_IND || event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND) { + bool connectable = event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_ADV_IND; + uint8_t addr[6]; + reverse_addr_byte_order(addr, event->disc.addr.val); + mp_bluetooth_gap_on_scan_result(event->disc.addr.type, addr, connectable, event->disc.rssi, event->disc.data, event->disc.length_data); + } else if (event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) { + // TODO + } else if (event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND) { + // TODO + } + + return 0; +} + +int mp_bluetooth_gap_scan_start(int32_t duration_ms) { + if (duration_ms == 0) { + duration_ms = BLE_HS_FOREVER; + } + STATIC const struct ble_gap_disc_params discover_params = { + .itvl = BLE_GAP_SCAN_SLOW_INTERVAL1, + .window = BLE_GAP_SCAN_SLOW_WINDOW1, + .filter_policy = BLE_HCI_CONN_FILT_NO_WL, + .limited = 0, + .passive = 0, + .filter_duplicates = 0, + }; + int err = ble_gap_disc(BLE_OWN_ADDR_PUBLIC, duration_ms, &discover_params, gap_scan_cb, NULL); + return ble_hs_err_to_errno(err); +} + +int mp_bluetooth_gap_scan_stop(void) { + int err = ble_gap_disc_cancel(); + if (err == 0) { + mp_bluetooth_gap_on_scan_complete(); + return 0; + } + return ble_hs_err_to_errno(err); +} + +// Central role: GAP events for a connected peripheral. +STATIC int peripheral_gap_event_cb(struct ble_gap_event *event, void *arg) { + DEBUG_EVENT_printf("peripheral_gap_event_cb: event=%d\n", event->type); + struct ble_gap_conn_desc desc; + uint8_t buf[MP_BLUETOOTH_MAX_ATTR_SIZE]; + size_t len; + uint8_t addr[6] = {0}; + + switch (event->type) { + case BLE_GAP_EVENT_CONNECT: + if (event->connect.status == 0) { + // Connection established. + ble_gap_conn_find(event->connect.conn_handle, &desc); + reverse_addr_byte_order(addr, desc.peer_id_addr.val); + mp_bluetooth_gap_on_connected_disconnected(MP_BLUETOOTH_IRQ_PERIPHERAL_CONNECT, event->connect.conn_handle, desc.peer_id_addr.type, addr); + } else { + // Connection failed. + mp_bluetooth_gap_on_connected_disconnected(MP_BLUETOOTH_IRQ_PERIPHERAL_DISCONNECT, event->connect.conn_handle, 0xff, addr); + } + break; + + case BLE_GAP_EVENT_DISCONNECT: + // Disconnect. + reverse_addr_byte_order(addr, event->disconnect.conn.peer_id_addr.val); + mp_bluetooth_gap_on_connected_disconnected(MP_BLUETOOTH_IRQ_PERIPHERAL_DISCONNECT, event->disconnect.conn.conn_handle, event->disconnect.conn.peer_id_addr.type, addr); + + break; + + case BLE_GAP_EVENT_NOTIFY_RX: + len = MIN(MP_BLUETOOTH_MAX_ATTR_SIZE, OS_MBUF_PKTLEN(event->notify_rx.om)); + os_mbuf_copydata(event->notify_rx.om, 0, len, buf); + if (event->notify_rx.indication == 0) { + mp_bluetooth_gattc_on_data_available(MP_BLUETOOTH_IRQ_GATTC_NOTIFY, event->notify_rx.conn_handle, event->notify_rx.attr_handle, buf, len); + } else { + mp_bluetooth_gattc_on_data_available(MP_BLUETOOTH_IRQ_GATTC_INDICATE, event->notify_rx.conn_handle, event->notify_rx.attr_handle, buf, len); + } + break; + + case BLE_GAP_EVENT_CONN_UPDATE: + // TODO + break; + + case BLE_GAP_EVENT_CONN_UPDATE_REQ: + // TODO + break; + + default: + break; + } + return 0; +} + +int mp_bluetooth_gap_peripheral_connect(uint8_t addr_type, const uint8_t *addr, int32_t duration_ms) { + if (ble_gap_disc_active()) { + mp_bluetooth_gap_scan_stop(); + } + + // TODO: This is the same as ble_gap_conn_params_dflt (i.e. passing NULL). + STATIC const struct ble_gap_conn_params params = { + .scan_itvl = 0x0010, + .scan_window = 0x0010, + .itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN, + .itvl_max = BLE_GAP_INITIAL_CONN_ITVL_MAX, + .latency = BLE_GAP_INITIAL_CONN_LATENCY, + .supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT, + .min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN, + .max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN, + }; + + ble_addr_t addr_nimble = create_nimble_addr(addr_type, addr); + int err = ble_gap_connect(BLE_OWN_ADDR_PUBLIC, &addr_nimble, duration_ms, ¶ms, &peripheral_gap_event_cb, NULL); + return ble_hs_err_to_errno(err); +} + +STATIC int peripheral_discover_service_cb(uint16_t conn_handle, const struct ble_gatt_error *error, const struct ble_gatt_svc *service, void *arg) { + DEBUG_EVENT_printf("peripheral_discover_service_cb: conn_handle=%d status=%d start_handle=%d\n", conn_handle, error->status, service->start_handle); + // TODO: Find out what error->status == 14 means (probably "end of services"). + if (error->status == 0) { + mp_obj_bluetooth_uuid_t service_uuid = create_mp_uuid(&service->uuid); + mp_bluetooth_gattc_on_primary_service_result(conn_handle, service->start_handle, service->end_handle, &service_uuid); + } + return 0; +} + +int mp_bluetooth_gattc_discover_primary_services(uint16_t conn_handle) { + int err = ble_gattc_disc_all_svcs(conn_handle, &peripheral_discover_service_cb, NULL); + return ble_hs_err_to_errno(err); +} + +STATIC int ble_gatt_characteristic_cb(uint16_t conn_handle, const struct ble_gatt_error *error, const struct ble_gatt_chr *characteristic, void *arg) { + if (error->status == 0) { + mp_obj_bluetooth_uuid_t characteristic_uuid = create_mp_uuid(&characteristic->uuid); + mp_bluetooth_gattc_on_characteristic_result(conn_handle, characteristic->def_handle, characteristic->val_handle, characteristic->properties, &characteristic_uuid); + } + return 0; +} + +int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle) { + int err = ble_gattc_disc_all_chrs(conn_handle, start_handle, end_handle, &ble_gatt_characteristic_cb, NULL); + return ble_hs_err_to_errno(err); +} + +STATIC int ble_gatt_descriptor_cb(uint16_t conn_handle, const struct ble_gatt_error *error, uint16_t characteristic_val_handle, const struct ble_gatt_dsc *descriptor, void *arg) { + if (error->status == 0) { + mp_obj_bluetooth_uuid_t descriptor_uuid = create_mp_uuid(&descriptor->uuid); + mp_bluetooth_gattc_on_descriptor_result(conn_handle, descriptor->handle, &descriptor_uuid); + } + return 0; +} + +int mp_bluetooth_gattc_discover_descriptors(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle) { + int err = ble_gattc_disc_all_dscs(conn_handle, start_handle, end_handle, &ble_gatt_descriptor_cb, NULL); + return ble_hs_err_to_errno(err); +} + +STATIC int ble_gatt_attr_read_cb(uint16_t conn_handle, const struct ble_gatt_error *error, struct ble_gatt_attr *attr, void *arg) { + // TODO: Maybe send NULL if error->status non-zero. + if (error->status == 0) { + uint8_t buf[MP_BLUETOOTH_MAX_ATTR_SIZE]; + size_t len = MIN(MP_BLUETOOTH_MAX_ATTR_SIZE, OS_MBUF_PKTLEN(attr->om)); + os_mbuf_copydata(attr->om, 0, len, buf); + mp_bluetooth_gattc_on_data_available(MP_BLUETOOTH_IRQ_GATTC_READ_RESULT, conn_handle, attr->handle, buf, len); + } + return 0; +} + +// Initiate read of a value from the remote peripheral. +int mp_bluetooth_gattc_read(uint16_t conn_handle, uint16_t value_handle) { + int err = ble_gattc_read(conn_handle, value_handle, &ble_gatt_attr_read_cb, NULL); + return ble_hs_err_to_errno(err); +} + +STATIC int ble_gatt_attr_write_cb(uint16_t conn_handle, const struct ble_gatt_error *error, struct ble_gatt_attr *attr, void *arg) { + mp_bluetooth_gattc_on_write_status(conn_handle, attr->handle, error->status); + return 0; +} + +// Write the value to the remote peripheral. +int mp_bluetooth_gattc_write(uint16_t conn_handle, uint16_t value_handle, const uint8_t *value, size_t *value_len) { + int err = ble_gattc_write_flat(conn_handle, value_handle, value, *value_len, &ble_gatt_attr_write_cb, NULL); + return ble_hs_err_to_errno(err); +} + +#endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE + +#endif // MICROPY_PY_BLUETOOTH && MICROPY_BLUETOOTH_NIMBLE