extmod/modbluetooth: Add API for L2CAP channels.

Also known as L2CAP "connection oriented channels". This provides a
socket-like data transfer mechanism for BLE.

Currently only implemented for NimBLE on STM32 / Unix.

Signed-off-by: Jim Mussared <jim.mussared@gmail.com>

9 files changed, 504 insertions, 22 deletions

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 <stdint.h>
+#include <limits.h>
 
 // --- 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)