diff --git a/extmod/modbluetooth.c b/extmod/modbluetooth.c index d8068df594..2cff386f12 100644 --- a/extmod/modbluetooth.c +++ b/extmod/modbluetooth.c @@ -43,6 +43,10 @@ #error modbluetooth requires MICROPY_ENABLE_SCHEDULER #endif +#if MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS && !MICROPY_PY_BLUETOOTH_USE_SYNC_EVENTS +#error l2cap channels require synchronous modbluetooth events +#endif + #define MP_BLUETOOTH_CONNECT_DEFAULT_SCAN_DURATION_MS 2000 #define MICROPY_PY_BLUETOOTH_MAX_EVENT_DATA_TUPLE_LEN 5 @@ -785,6 +789,58 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_2(bluetooth_ble_gattc_exchange_mtu_obj, bluetooth #endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE +#if MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS + +STATIC mp_obj_t bluetooth_ble_l2cap_listen(mp_obj_t self_in, mp_obj_t psm_in, mp_obj_t mtu_in) { + (void)self_in; + mp_int_t psm = mp_obj_get_int(psm_in); + mp_int_t mtu = mp_obj_get_int(mtu_in); + return bluetooth_handle_errno(mp_bluetooth_l2cap_listen(psm, mtu)); +} +STATIC MP_DEFINE_CONST_FUN_OBJ_3(bluetooth_ble_l2cap_listen_obj, bluetooth_ble_l2cap_listen); + +STATIC mp_obj_t bluetooth_ble_l2cap_connect(size_t n_args, const mp_obj_t *args) { + mp_int_t conn_handle = mp_obj_get_int(args[1]); + mp_int_t psm = mp_obj_get_int(args[2]); + mp_int_t mtu = mp_obj_get_int(args[3]); + return bluetooth_handle_errno(mp_bluetooth_l2cap_connect(conn_handle, psm, mtu)); +} +STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(bluetooth_ble_l2cap_connect_obj, 4, 4, bluetooth_ble_l2cap_connect); + +STATIC mp_obj_t bluetooth_ble_l2cap_disconnect(mp_obj_t self_in, mp_obj_t conn_handle_in, mp_obj_t cid_in) { + (void)self_in; + mp_int_t conn_handle = mp_obj_get_int(conn_handle_in); + mp_int_t cid = mp_obj_get_int(cid_in); + return bluetooth_handle_errno(mp_bluetooth_l2cap_disconnect(conn_handle, cid)); +} +STATIC MP_DEFINE_CONST_FUN_OBJ_3(bluetooth_ble_l2cap_disconnect_obj, bluetooth_ble_l2cap_disconnect); + +STATIC mp_obj_t bluetooth_ble_l2cap_send(size_t n_args, const mp_obj_t *args) { + mp_int_t conn_handle = mp_obj_get_int(args[1]); + mp_int_t cid = mp_obj_get_int(args[2]); + mp_buffer_info_t bufinfo; + mp_get_buffer_raise(args[3], &bufinfo, MP_BUFFER_READ); + bool stalled = false; + bluetooth_handle_errno(mp_bluetooth_l2cap_send(conn_handle, cid, bufinfo.buf, bufinfo.len, &stalled)); + // Return True if the channel is still ready to send. + return mp_obj_new_bool(!stalled); +} +STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(bluetooth_ble_l2cap_send_obj, 4, 4, bluetooth_ble_l2cap_send); + +STATIC mp_obj_t bluetooth_ble_l2cap_recvinto(size_t n_args, const mp_obj_t *args) { + mp_int_t conn_handle = mp_obj_get_int(args[1]); + mp_int_t cid = mp_obj_get_int(args[2]); + mp_buffer_info_t bufinfo = {0}; + if (args[3] != mp_const_none) { + mp_get_buffer_raise(args[3], &bufinfo, MP_BUFFER_WRITE); + } + bluetooth_handle_errno(mp_bluetooth_l2cap_recvinto(conn_handle, cid, bufinfo.buf, &bufinfo.len)); + return MP_OBJ_NEW_SMALL_INT(bufinfo.len); +} +STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(bluetooth_ble_l2cap_recvinto_obj, 4, 4, bluetooth_ble_l2cap_recvinto); + +#endif // MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS + // ---------------------------------------------------------------------------- // Bluetooth object: Definition // ---------------------------------------------------------------------------- @@ -817,6 +873,13 @@ STATIC const mp_rom_map_elem_t bluetooth_ble_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_gattc_write), MP_ROM_PTR(&bluetooth_ble_gattc_write_obj) }, { MP_ROM_QSTR(MP_QSTR_gattc_exchange_mtu), MP_ROM_PTR(&bluetooth_ble_gattc_exchange_mtu_obj) }, #endif + #if MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS + { MP_ROM_QSTR(MP_QSTR_l2cap_listen), MP_ROM_PTR(&bluetooth_ble_l2cap_listen_obj) }, + { MP_ROM_QSTR(MP_QSTR_l2cap_connect), MP_ROM_PTR(&bluetooth_ble_l2cap_connect_obj) }, + { MP_ROM_QSTR(MP_QSTR_l2cap_disconnect), MP_ROM_PTR(&bluetooth_ble_l2cap_disconnect_obj) }, + { MP_ROM_QSTR(MP_QSTR_l2cap_send), MP_ROM_PTR(&bluetooth_ble_l2cap_send_obj) }, + { MP_ROM_QSTR(MP_QSTR_l2cap_recvinto), MP_ROM_PTR(&bluetooth_ble_l2cap_recvinto_obj) }, + #endif }; STATIC MP_DEFINE_CONST_DICT(bluetooth_ble_locals_dict, bluetooth_ble_locals_dict_table); @@ -1051,6 +1114,37 @@ void mp_bluetooth_gatts_on_mtu_exchanged(uint16_t conn_handle, uint16_t value) { invoke_irq_handler(MP_BLUETOOTH_IRQ_MTU_EXCHANGED, args, 2, NULL_U8, 0, NULL_ADDR, NULL_I8, 0, NULL_UUID, NULL_DATA, 0); } +#if MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS +mp_int_t mp_bluetooth_gattc_on_l2cap_accept(uint16_t conn_handle, uint16_t cid, uint16_t psm, uint16_t our_mtu, uint16_t peer_mtu) { + uint16_t args[] = {conn_handle, cid, psm, our_mtu, peer_mtu}; + mp_obj_t result = invoke_irq_handler(MP_BLUETOOTH_IRQ_L2CAP_ACCEPT, args, 5, NULL_U8, 0, NULL_ADDR, NULL_I8, 0, NULL_UUID, NULL_DATA, 0); + // Return non-zero from IRQ handler to fail the accept. + mp_int_t ret = 0; + mp_obj_get_int_maybe(result, &ret); + return ret; +} + +void mp_bluetooth_gattc_on_l2cap_connect(uint16_t conn_handle, uint16_t cid, uint16_t psm, uint16_t our_mtu, uint16_t peer_mtu) { + uint16_t args[] = {conn_handle, cid, psm, our_mtu, peer_mtu}; + invoke_irq_handler(MP_BLUETOOTH_IRQ_L2CAP_CONNECT, args, 5, NULL_U8, 0, NULL_ADDR, NULL_I8, 0, NULL_UUID, NULL_DATA, 0); +} + +void mp_bluetooth_gattc_on_l2cap_disconnect(uint16_t conn_handle, uint16_t cid, uint16_t psm, uint16_t status) { + uint16_t args[] = {conn_handle, cid, psm, status}; + invoke_irq_handler(MP_BLUETOOTH_IRQ_L2CAP_DISCONNECT, args, 4, NULL_U8, 0, NULL_ADDR, NULL_I8, 0, NULL_UUID, NULL_DATA, 0); +} + +void mp_bluetooth_gattc_on_l2cap_send_ready(uint16_t conn_handle, uint16_t cid, uint8_t status) { + uint16_t args[] = {conn_handle, cid}; + invoke_irq_handler(MP_BLUETOOTH_IRQ_L2CAP_SEND_READY, args, 2, &status, 1, NULL_ADDR, NULL_I8, 0, NULL_UUID, NULL_DATA, 0); +} + +void mp_bluetooth_gattc_on_l2cap_recv(uint16_t conn_handle, uint16_t cid) { + uint16_t args[] = {conn_handle, cid}; + invoke_irq_handler(MP_BLUETOOTH_IRQ_L2CAP_RECV, args, 2, NULL_U8, 0, NULL_ADDR, NULL_I8, 0, NULL_UUID, NULL_DATA, 0); +} +#endif // MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS + #if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE void mp_bluetooth_gap_on_scan_complete(void) { invoke_irq_handler(MP_BLUETOOTH_IRQ_SCAN_DONE, NULL_U16, 0, NULL_U8, 0, NULL_ADDR, NULL_I8, 0, NULL_UUID, NULL_DATA, 0); @@ -1203,6 +1297,23 @@ void mp_bluetooth_gatts_on_indicate_complete(uint16_t conn_handle, uint16_t valu schedule_ringbuf(atomic_state); } +bool mp_bluetooth_gatts_on_read_request(uint16_t conn_handle, uint16_t value_handle) { + (void)conn_handle; + (void)value_handle; + // This must be handled synchronously and therefore cannot implemented with the ringbuffer. + return true; +} + +void mp_bluetooth_gatts_on_mtu_exchanged(uint16_t conn_handle, uint16_t value) { + MICROPY_PY_BLUETOOTH_ENTER + mp_obj_bluetooth_ble_t *o = MP_OBJ_TO_PTR(MP_STATE_VM(bluetooth)); + if (enqueue_irq(o, 2 + 2, MP_BLUETOOTH_IRQ_MTU_EXCHANGED)) { + ringbuf_put16(&o->ringbuf, conn_handle); + ringbuf_put16(&o->ringbuf, value); + } + schedule_ringbuf(atomic_state); +} + #if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE void mp_bluetooth_gap_on_scan_complete(void) { MICROPY_PY_BLUETOOTH_ENTER @@ -1322,26 +1433,8 @@ void mp_bluetooth_gattc_on_read_write_status(uint8_t event, uint16_t conn_handle } schedule_ringbuf(atomic_state); } - #endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE -bool mp_bluetooth_gatts_on_read_request(uint16_t conn_handle, uint16_t value_handle) { - (void)conn_handle; - (void)value_handle; - // This must be handled synchronously and therefore cannot implemented with the ringbuffer. - return true; -} - -void mp_bluetooth_gatts_on_mtu_exchanged(uint16_t conn_handle, uint16_t value) { - MICROPY_PY_BLUETOOTH_ENTER - mp_obj_bluetooth_ble_t *o = MP_OBJ_TO_PTR(MP_STATE_VM(bluetooth)); - if (enqueue_irq(o, 2 + 2, MP_BLUETOOTH_IRQ_MTU_EXCHANGED)) { - ringbuf_put16(&o->ringbuf, conn_handle); - ringbuf_put16(&o->ringbuf, value); - } - schedule_ringbuf(atomic_state); -} - #endif // MICROPY_PY_BLUETOOTH_USE_SYNC_EVENTS // ---------------------------------------------------------------------------- diff --git a/extmod/modbluetooth.h b/extmod/modbluetooth.h index 52e3446ff3..9caddb0f3f 100644 --- a/extmod/modbluetooth.h +++ b/extmod/modbluetooth.h @@ -49,6 +49,11 @@ #define MICROPY_PY_BLUETOOTH_USE_SYNC_EVENTS (0) #endif +// A port can optionally enable support for L2CAP "Connection Oriented Channels". +#ifndef MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS +#define MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS (0) +#endif + // This is used to protect the ringbuffer. // A port may no-op this if MICROPY_PY_BLUETOOTH_USE_SYNC_EVENTS is enabled. #ifndef MICROPY_PY_BLUETOOTH_ENTER @@ -104,6 +109,11 @@ #define MP_BLUETOOTH_IRQ_GATTC_INDICATE (19) #define MP_BLUETOOTH_IRQ_GATTS_INDICATE_DONE (20) #define MP_BLUETOOTH_IRQ_MTU_EXCHANGED (21) +#define MP_BLUETOOTH_IRQ_L2CAP_ACCEPT (22) +#define MP_BLUETOOTH_IRQ_L2CAP_CONNECT (23) +#define MP_BLUETOOTH_IRQ_L2CAP_DISCONNECT (24) +#define MP_BLUETOOTH_IRQ_L2CAP_RECV (25) +#define MP_BLUETOOTH_IRQ_L2CAP_SEND_READY (26) #define MP_BLUETOOTH_ADDRESS_MODE_PUBLIC (0) #define MP_BLUETOOTH_ADDRESS_MODE_RANDOM (1) @@ -136,6 +146,11 @@ _IRQ_GATTC_NOTIFY = const(18) _IRQ_GATTC_INDICATE = const(19) _IRQ_GATTS_INDICATE_DONE = const(20) _IRQ_MTU_EXCHANGED = const(21) +_IRQ_L2CAP_ACCEPT = const(22) +_IRQ_L2CAP_CONNECT = const(23) +_IRQ_L2CAP_DISCONNECT = const(24) +_IRQ_L2CAP_RECV = const(25) +_IRQ_L2CAP_SEND_READY = const(26) */ // bluetooth.UUID type. @@ -250,7 +265,15 @@ int mp_bluetooth_gattc_write(uint16_t conn_handle, uint16_t value_handle, const // Initiate MTU exchange for a specific connection using the preferred MTU. int mp_bluetooth_gattc_exchange_mtu(uint16_t conn_handle); -#endif +#endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE + +#if MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS +int mp_bluetooth_l2cap_listen(uint16_t psm, uint16_t mtu); +int mp_bluetooth_l2cap_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu); +int mp_bluetooth_l2cap_disconnect(uint16_t conn_handle, uint16_t cid); +int mp_bluetooth_l2cap_send(uint16_t conn_handle, uint16_t cid, const uint8_t *buf, size_t len, bool *stalled); +int mp_bluetooth_l2cap_recvinto(uint16_t conn_handle, uint16_t cid, uint8_t *buf, size_t *len); +#endif // MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS ///////////////////////////////////////////////////////////////////////////// // API implemented by modbluetooth (called by port-specific implementations): @@ -294,7 +317,15 @@ void mp_bluetooth_gattc_on_data_available(uint8_t event, uint16_t conn_handle, u // Notify modbluetooth that a read or write operation has completed. void mp_bluetooth_gattc_on_read_write_status(uint8_t event, uint16_t conn_handle, uint16_t value_handle, uint16_t status); -#endif +#endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE + +#if MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS +mp_int_t mp_bluetooth_gattc_on_l2cap_accept(uint16_t conn_handle, uint16_t cid, uint16_t psm, uint16_t our_mtu, uint16_t peer_mtu); +void mp_bluetooth_gattc_on_l2cap_connect(uint16_t conn_handle, uint16_t cid, uint16_t psm, uint16_t our_mtu, uint16_t peer_mtu); +void mp_bluetooth_gattc_on_l2cap_disconnect(uint16_t conn_handle, uint16_t cid, uint16_t psm, uint16_t status); +void mp_bluetooth_gattc_on_l2cap_send_ready(uint16_t conn_handle, uint16_t cid, uint8_t status); +void mp_bluetooth_gattc_on_l2cap_recv(uint16_t conn_handle, uint16_t cid); +#endif // MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS // For stacks that don't manage attribute value data (currently all of them), helpers // to store this in a map, keyed by value handle. diff --git a/extmod/nimble/modbluetooth_nimble.c b/extmod/nimble/modbluetooth_nimble.c index c961aee326..fa416ebc3f 100644 --- a/extmod/nimble/modbluetooth_nimble.c +++ b/extmod/nimble/modbluetooth_nimble.c @@ -42,6 +42,10 @@ #include "services/gap/ble_svc_gap.h" #include "services/gatt/ble_svc_gatt.h" +// We need the definition of "struct ble_l2cap_chan". +// See l2cap_channel_event() for details. +#include "nimble/host/src/ble_l2cap_priv.h" + #ifndef MICROPY_PY_BLUETOOTH_DEFAULT_GAP_NAME #define MICROPY_PY_BLUETOOTH_DEFAULT_GAP_NAME "MPY NIMBLE" #endif @@ -66,6 +70,7 @@ STATIC int8_t ble_hs_err_to_errno_table[] = { [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_EBADDATA] = MP_EINVAL, }; STATIC int ble_hs_err_to_errno(int err) { @@ -1110,4 +1115,331 @@ int mp_bluetooth_gattc_exchange_mtu(uint16_t conn_handle) { #endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE +#if MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS + +// Fortunately NimBLE uses mbuf chains correctly with L2CAP COC (rather than +// accessing the mbuf internals directly), so we can use a small block size to +// avoid excessive fragmentation and rely on them chaining together for larger +// payloads. +#define L2CAP_BUF_BLOCK_SIZE (128) + +// This gives us enough room to have one MTU-size transmit buffer and two +// MTU-sized receive buffers. Note that we use the local MTU to calculate +// the buffer size. This means that if the peer MTU is larger, then +// there might not be enough space in the pool to send a full peer-MTU +// sized payload and mp_bluetooth_l2cap_send will return ENOMEM. +#define L2CAP_BUF_SIZE_MTUS_PER_CHANNEL (3) + +typedef struct _mp_bluetooth_nimble_l2cap_channel_t { + struct ble_l2cap_chan *chan; + struct os_mbuf_pool sdu_mbuf_pool; + struct os_mempool sdu_mempool; + struct os_mbuf *rx_pending; + uint16_t mtu; + os_membuf_t sdu_mem[]; +} mp_bluetooth_nimble_l2cap_channel_t; + +STATIC void destroy_l2cap_channel() { + // Only free the l2cap channel if we're the one that initiated the connection. + // Listeners continue listening on the same channel. + if (!MP_STATE_PORT(bluetooth_nimble_root_pointers)->l2cap_listening) { + MP_STATE_PORT(bluetooth_nimble_root_pointers)->l2cap_chan = NULL; + } +} + +STATIC int l2cap_channel_event(struct ble_l2cap_event *event, void *arg) { + DEBUG_printf("l2cap_channel_event: type=%d\n", event->type); + mp_bluetooth_nimble_l2cap_channel_t *chan = (mp_bluetooth_nimble_l2cap_channel_t *)arg; + struct ble_l2cap_chan_info info; + + switch (event->type) { + case BLE_L2CAP_EVENT_COC_CONNECTED: { + DEBUG_printf("l2cap_channel_event: connect: conn_handle=%d status=%d\n", event->connect.conn_handle, event->connect.status); + chan->chan = event->connect.chan; + + ble_l2cap_get_chan_info(event->connect.chan, &info); + if (event->connect.status == 0) { + mp_bluetooth_gattc_on_l2cap_connect(event->connect.conn_handle, info.scid, info.psm, info.our_coc_mtu, info.peer_coc_mtu); + } else { + mp_bluetooth_gattc_on_l2cap_disconnect(event->connect.conn_handle, info.scid, info.psm, event->connect.status); + destroy_l2cap_channel(); + } + break; + } + case BLE_L2CAP_EVENT_COC_DISCONNECTED: { + DEBUG_printf("l2cap_channel_event: disconnect: conn_handle=%d\n", event->disconnect.conn_handle); + ble_l2cap_get_chan_info(event->disconnect.chan, &info); + mp_bluetooth_gattc_on_l2cap_disconnect(event->disconnect.conn_handle, info.scid, info.psm, 0); + destroy_l2cap_channel(); + break; + } + case BLE_L2CAP_EVENT_COC_ACCEPT: { + DEBUG_printf("l2cap_channel_event: accept: conn_handle=%d peer_sdu_size=%d\n", event->accept.conn_handle, event->accept.peer_sdu_size); + chan->chan = event->accept.chan; + ble_l2cap_get_chan_info(event->accept.chan, &info); + int ret = mp_bluetooth_gattc_on_l2cap_accept(event->accept.conn_handle, info.scid, info.psm, info.our_coc_mtu, info.peer_coc_mtu); + if (ret != 0) { + return ret; + } + struct os_mbuf *sdu_rx = os_mbuf_get_pkthdr(&chan->sdu_mbuf_pool, 0); + assert(sdu_rx); + return ble_l2cap_recv_ready(chan->chan, sdu_rx); + } + case BLE_L2CAP_EVENT_COC_DATA_RECEIVED: { + DEBUG_printf("l2cap_channel_event: receive: conn_handle=%d len=%d\n", event->receive.conn_handle, OS_MBUF_PKTLEN(event->receive.sdu_rx)); + + if (chan->rx_pending) { + // Ideally this doesn't happen, as the sender should not get + // any more credits to send more data until we call + // ble_l2cap_recv_ready. However there might be multiple + // in-flight packets if the sender was able to send more than + // one before stalling. + DEBUG_printf("l2cap_channel_event: receive: appending to rx pending\n"); + // Note: os_mbuf_concat will just join the two together, so + // sdu_rx is now "owned" by rx_pending. + os_mbuf_concat(chan->rx_pending, event->receive.sdu_rx); + } else { + // Normal case is when the first payload arrives since calling + // ble_l2cap_recv_ready. + DEBUG_printf("l2cap_event: receive: new payload\n"); + // Take ownership of sdu_rx. + chan->rx_pending = event->receive.sdu_rx; + } + + struct os_mbuf *sdu_rx = os_mbuf_get_pkthdr(&chan->sdu_mbuf_pool, 0); + assert(sdu_rx); + + // ble_l2cap_coc_rx_fn invokes this event handler when a complete payload arrives. + // However, it NULLs chan->chan->coc_rx.sdu before doing so, expecting that + // ble_l2cap_recv_ready will be called to give it a new mbuf. + // This means that if another payload arrives before we call ble_l2cap_recv_ready + // then ble_l2cap_coc_rx_fn will NULL-deref coc_rx.sdu. + + // Because we're not yet ready to grant new credits to the channel, we can't call + // ble_l2cap_recv_ready yet, so instead we just give it a new mbuf. This requires + // ble_l2cap_priv.h for the definition of chan->chan (i.e. struct ble_l2cap_chan). + chan->chan->coc_rx.sdu = sdu_rx; + + ble_l2cap_get_chan_info(event->receive.chan, &info); + mp_bluetooth_gattc_on_l2cap_recv(event->receive.conn_handle, info.scid); + break; + } + case BLE_L2CAP_EVENT_COC_TX_UNSTALLED: { + DEBUG_printf("l2cap_channel_event: tx_unstalled: conn_handle=%d status=%d\n", event->tx_unstalled.conn_handle, event->tx_unstalled.status); + ble_l2cap_get_chan_info(event->receive.chan, &info); + // Map status to {0,1} (i.e. "sent everything", or "partial send"). + mp_bluetooth_gattc_on_l2cap_send_ready(event->tx_unstalled.conn_handle, info.scid, event->tx_unstalled.status == 0 ? 0 : 1); + break; + } + case BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED: { + DEBUG_printf("l2cap_channel_event: reconfig_completed: conn_handle=%d\n", event->reconfigured.conn_handle); + break; + } + case BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED: { + DEBUG_printf("l2cap_channel_event: peer_reconfigured: conn_handle=%d\n", event->reconfigured.conn_handle); + break; + } + default: { + DEBUG_printf("l2cap_channel_event: unknown event\n"); + break; + } + } + + return 0; +} + +STATIC mp_bluetooth_nimble_l2cap_channel_t *get_l2cap_channel_for_conn_cid(uint16_t conn_handle, uint16_t cid) { + // TODO: Support more than one concurrent L2CAP channel. At the moment we + // just verify that the cid refers to the current channel. + mp_bluetooth_nimble_l2cap_channel_t *chan = MP_STATE_PORT(bluetooth_nimble_root_pointers)->l2cap_chan; + + if (!chan) { + return NULL; + } + + struct ble_l2cap_chan_info info; + ble_l2cap_get_chan_info(chan->chan, &info); + + if (info.scid != cid || ble_l2cap_get_conn_handle(chan->chan) != conn_handle) { + return NULL; + } + + return chan; +} + +STATIC int create_l2cap_channel(uint16_t mtu, mp_bluetooth_nimble_l2cap_channel_t **out) { + if (MP_STATE_PORT(bluetooth_nimble_root_pointers)->l2cap_chan) { + // Only one L2CAP channel allowed. + // Additionally, if we're listening, then no connections may be initiated. + DEBUG_printf("create_l2cap_channel: channel already in use\n"); + return MP_EALREADY; + } + + // We want the TX and RX buffers to share a pool that is some multiple of + // the MTU size. Figure out how many blocks per MTU (rounding up), then + // multiply that by the "MTUs per channel" (set to 3 above). + const size_t buf_blocks = MP_CEIL_DIVIDE(mtu, L2CAP_BUF_BLOCK_SIZE) * L2CAP_BUF_SIZE_MTUS_PER_CHANNEL; + + mp_bluetooth_nimble_l2cap_channel_t *chan = m_new_obj_var(mp_bluetooth_nimble_l2cap_channel_t, uint8_t, OS_MEMPOOL_SIZE(buf_blocks, L2CAP_BUF_BLOCK_SIZE) * sizeof(os_membuf_t)); + MP_STATE_PORT(bluetooth_nimble_root_pointers)->l2cap_chan = chan; + + // Will be set in BLE_L2CAP_EVENT_COC_CONNECTED or BLE_L2CAP_EVENT_COC_ACCEPT. + chan->chan = NULL; + + chan->mtu = mtu; + chan->rx_pending = NULL; + + int err = os_mempool_init(&chan->sdu_mempool, buf_blocks, L2CAP_BUF_BLOCK_SIZE, chan->sdu_mem, "l2cap_sdu_pool"); + if (err != 0) { + DEBUG_printf("mp_bluetooth_l2cap_connect: os_mempool_init failed %d\n", err); + return MP_ENOMEM; + } + + err = os_mbuf_pool_init(&chan->sdu_mbuf_pool, &chan->sdu_mempool, L2CAP_BUF_BLOCK_SIZE, buf_blocks); + if (err != 0) { + DEBUG_printf("mp_bluetooth_l2cap_connect: os_mbuf_pool_init failed %d\n", err); + return MP_ENOMEM; + } + + *out = chan; + return 0; +} + +int mp_bluetooth_l2cap_listen(uint16_t psm, uint16_t mtu) { + DEBUG_printf("mp_bluetooth_l2cap_listen: psm=%d, mtu=%d\n", psm, mtu); + + mp_bluetooth_nimble_l2cap_channel_t *chan; + int err = create_l2cap_channel(mtu, &chan); + if (err != 0) { + return err; + } + + MP_STATE_PORT(bluetooth_nimble_root_pointers)->l2cap_listening = true; + + return ble_hs_err_to_errno(ble_l2cap_create_server(psm, mtu, &l2cap_channel_event, chan)); +} + +int mp_bluetooth_l2cap_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu) { + DEBUG_printf("mp_bluetooth_l2cap_connect: conn_handle=%d, psm=%d, mtu=%d\n", conn_handle, psm, mtu); + + mp_bluetooth_nimble_l2cap_channel_t *chan; + int err = create_l2cap_channel(mtu, &chan); + if (err != 0) { + return err; + } + + struct os_mbuf *sdu_rx = os_mbuf_get_pkthdr(&chan->sdu_mbuf_pool, 0); + assert(sdu_rx); + return ble_hs_err_to_errno(ble_l2cap_connect(conn_handle, psm, mtu, sdu_rx, &l2cap_channel_event, chan)); +} + +int mp_bluetooth_l2cap_disconnect(uint16_t conn_handle, uint16_t cid) { + DEBUG_printf("mp_bluetooth_l2cap_disconnect: conn_handle=%d, cid=%d\n", conn_handle, cid); + mp_bluetooth_nimble_l2cap_channel_t *chan = get_l2cap_channel_for_conn_cid(conn_handle, cid); + if (!chan) { + return MP_EINVAL; + } + return ble_hs_err_to_errno(ble_l2cap_disconnect(chan->chan)); +} + +int mp_bluetooth_l2cap_send(uint16_t conn_handle, uint16_t cid, const uint8_t *buf, size_t len, bool *stalled) { + DEBUG_printf("mp_bluetooth_l2cap_send: conn_handle=%d, cid=%d, len=%d\n", conn_handle, cid, (int)len); + + mp_bluetooth_nimble_l2cap_channel_t *chan = get_l2cap_channel_for_conn_cid(conn_handle, cid); + if (!chan) { + return MP_EINVAL; + } + + struct ble_l2cap_chan_info info; + ble_l2cap_get_chan_info(chan->chan, &info); + if (len > info.peer_coc_mtu) { + // This is verified by ble_l2cap_send anyway, but this lets us + // avoid copying a too-large buffer into an mbuf. + return MP_EINVAL; + } + + if (len > (L2CAP_BUF_SIZE_MTUS_PER_CHANNEL - 1) * info.our_coc_mtu) { + // Always ensure there's at least one local MTU of space left in the buffer + // for the RX buffer. + return MP_EINVAL; + } + + // Grab an mbuf from the pool, and append the incoming buffer to it. + struct os_mbuf *sdu_tx = os_mbuf_get_pkthdr(&chan->sdu_mbuf_pool, 0); + if (sdu_tx == NULL) { + return MP_ENOMEM; + } + int err = os_mbuf_append(sdu_tx, buf, len); + if (err) { + os_mbuf_free_chain(sdu_tx); + return MP_ENOMEM; + } + + err = ble_l2cap_send(chan->chan, sdu_tx); + if (err == BLE_HS_ESTALLED) { + // Stalled means that this one will still send but any future ones + // will fail until we receive an unstalled event. + *stalled = true; + err = 0; + } else { + *stalled = false; + } + + // Other error codes such as BLE_HS_EBUSY (we're stalled) or BLE_HS_EBADDATA (bigger than MTU). + return ble_hs_err_to_errno(err); +} + +int mp_bluetooth_l2cap_recvinto(uint16_t conn_handle, uint16_t cid, uint8_t *buf, size_t *len) { + mp_bluetooth_nimble_l2cap_channel_t *chan = get_l2cap_channel_for_conn_cid(conn_handle, cid); + if (!chan) { + return MP_EINVAL; + } + + MICROPY_PY_BLUETOOTH_ENTER + if (chan->rx_pending) { + size_t avail = OS_MBUF_PKTLEN(chan->rx_pending); + + if (buf == NULL) { + // Can use this to implement a poll - just find out how much is available. + *len = avail; + } else { + // Have dest buffer and data available. + // Figure out how much we should copy. + *len = min(*len, avail); + + // Extract the required number of bytes. + os_mbuf_copydata(chan->rx_pending, 0, *len, buf); + + if (*len == avail) { + // That's all that's available -- free this mbuf and re-enable receiving. + os_mbuf_free_chain(chan->rx_pending); + chan->rx_pending = NULL; + + // We've already given the channel a new mbuf in l2cap_channel_event above, so + // re-use that mbuf in the call to ble_l2cap_recv_ready. This will just + // give the channel more credits. + struct os_mbuf *sdu_rx = chan->chan->coc_rx.sdu; + assert(sdu_rx); + if (sdu_rx) { + ble_l2cap_recv_ready(chan->chan, sdu_rx); + } + } else { + // Trim the used bytes from the start of the mbuf. + // Positive argument means "trim this many from head". + os_mbuf_adj(chan->rx_pending, *len); + // Clean up any empty mbufs at the head. + chan->rx_pending = os_mbuf_trim_front(chan->rx_pending); + } + } + } else { + // No pending data. + *len = 0; + } + + MICROPY_PY_BLUETOOTH_EXIT + return 0; +} + +#endif // MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS + #endif // MICROPY_PY_BLUETOOTH && MICROPY_BLUETOOTH_NIMBLE diff --git a/extmod/nimble/modbluetooth_nimble.h b/extmod/nimble/modbluetooth_nimble.h index 7e401781e7..9ed64368b2 100644 --- a/extmod/nimble/modbluetooth_nimble.h +++ b/extmod/nimble/modbluetooth_nimble.h @@ -38,6 +38,12 @@ typedef struct _mp_bluetooth_nimble_root_pointers_t { // Pending service definitions. size_t n_services; struct ble_gatt_svc_def *services[MP_BLUETOOTH_NIMBLE_MAX_SERVICES]; + + #if MICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS + // L2CAP channels. + struct _mp_bluetooth_nimble_l2cap_channel_t *l2cap_chan; + bool l2cap_listening; + #endif } mp_bluetooth_nimble_root_pointers_t; enum { diff --git a/extmod/nimble/nimble/nimble_npl_os.h b/extmod/nimble/nimble/nimble_npl_os.h index bfabe56e89..d0803f7e2e 100644 --- a/extmod/nimble/nimble/nimble_npl_os.h +++ b/extmod/nimble/nimble/nimble_npl_os.h @@ -30,12 +30,25 @@ // This is included by nimble/nimble_npl.h -- include that rather than this file directly. #include +#include // --- Configuration of NimBLE data structures -------------------------------- +// This is used at runtime to align allocations correctly. #define BLE_NPL_OS_ALIGNMENT (sizeof(uintptr_t)) #define BLE_NPL_TIME_FOREVER (0xffffffff) +// This is used at compile time to force struct member alignment. See +// os_mempool.h for where this is used (none of these three macros are defined +// by default). +#define OS_CFG_ALIGN_4 (4) +#define OS_CFG_ALIGN_8 (8) +#if (ULONG_MAX == 0xffffffffffffffff) +#define OS_CFG_ALIGNMENT (OS_CFG_ALIGN_8) +#else +#define OS_CFG_ALIGNMENT (OS_CFG_ALIGN_4) +#endif + typedef uint32_t ble_npl_time_t; typedef int32_t ble_npl_stime_t; diff --git a/extmod/nimble/syscfg/syscfg.h b/extmod/nimble/syscfg/syscfg.h index 8a6f2338be..bef6b3b921 100644 --- a/extmod/nimble/syscfg/syscfg.h +++ b/extmod/nimble/syscfg/syscfg.h @@ -99,9 +99,11 @@ int nimble_sprintf(char *str, const char *fmt, ...); #define MYNEWT_VAL_BLE_HS_PHONY_HCI_ACKS (0) #define MYNEWT_VAL_BLE_HS_REQUIRE_OS (1) #define MYNEWT_VAL_BLE_HS_STOP_ON_SHUTDOWN_TIMEOUT (2000) -#define MYNEWT_VAL_BLE_L2CAP_COC_MAX_NUM (0) +#define MYNEWT_VAL_BLE_L2CAP_COC_MAX_NUM (1) +#define MYNEWT_VAL_BLE_L2CAP_COC_MPS (MYNEWT_VAL_MSYS_1_BLOCK_SIZE - 8) +#define MYNEWT_VAL_BLE_L2CAP_ENHANCED_COC (0) #define MYNEWT_VAL_BLE_L2CAP_JOIN_RX_FRAGS (1) -#define MYNEWT_VAL_BLE_L2CAP_MAX_CHANS (3*MYNEWT_VAL_BLE_MAX_CONNECTIONS) +#define MYNEWT_VAL_BLE_L2CAP_MAX_CHANS (3 * MYNEWT_VAL_BLE_MAX_CONNECTIONS) #define MYNEWT_VAL_BLE_L2CAP_RX_FRAG_TIMEOUT (30000) #define MYNEWT_VAL_BLE_L2CAP_SIG_MAX_PROCS (1) #define MYNEWT_VAL_BLE_MONITOR_CONSOLE_BUFFER_SIZE (128) diff --git a/ports/stm32/Makefile b/ports/stm32/Makefile index 75090a077e..ced851ca38 100644 --- a/ports/stm32/Makefile +++ b/ports/stm32/Makefile @@ -499,6 +499,7 @@ endif endif ifeq ($(MICROPY_BLUETOOTH_NIMBLE),1) +CFLAGS_MOD += -DMICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS=1 include $(TOP)/extmod/nimble/nimble.mk SRC_C += mpnimbleport.c endif diff --git a/ports/unix/Makefile b/ports/unix/Makefile index f72c05f1ad..6a936a2425 100644 --- a/ports/unix/Makefile +++ b/ports/unix/Makefile @@ -182,6 +182,7 @@ else # NimBLE is enabled. GIT_SUBMODULES += lib/mynewt-nimble +CFLAGS_MOD += -DMICROPY_PY_BLUETOOTH_ENABLE_L2CAP_CHANNELS=1 include $(TOP)/extmod/nimble/nimble.mk endif diff --git a/py/misc.h b/py/misc.h index aac9072446..fe2b3b8afa 100644 --- a/py/misc.h +++ b/py/misc.h @@ -53,6 +53,9 @@ typedef unsigned int uint; // Static assertion macro #define MP_STATIC_ASSERT(cond) ((void)sizeof(char[1 - 2 * !(cond)])) +// Round-up integer division +#define MP_CEIL_DIVIDE(a, b) (((a) + (b) - 1) / (b)) + /** memory allocation ******************************************/ // TODO make a lazy m_renew that can increase by a smaller amount than requested (but by at least 1 more element)