Add l2cap API

15 files changed, 732 insertions, 28 deletions

diff --git a/Cargo.lock b/Cargo.lock
index 5a8303e..166b52f 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -29,6 +29,16 @@ dependencies = [
 ]
 
 [[package]]
+name = "alloc-cortex-m"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c46567ff10c713079e2b9ee67638073471a6b265e22c0bcd8ca96f63545cc90b"
+dependencies = [
+ "cortex-m",
+ "linked_list_allocator",
+]
+
+[[package]]
 name = "anyfmt"
 version = "0.1.0"
 source = "git+https://github.com/akiles/embassy#2e062f562773f4f4ff978e7976c2d4b08b968a6c"
@@ -431,6 +441,12 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
 
 [[package]]
+name = "linked_list_allocator"
+version = "0.8.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "84565678e403453d1a27a0886882b3b271701e65146d972d9d7d9a4c4a0ff498"
+
+[[package]]
 name = "lock_api"
 version = "0.3.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
@@ -514,6 +530,7 @@ dependencies = [
 name = "nrf-softdevice-examples"
 version = "0.1.0"
 dependencies = [
+ "alloc-cortex-m",
  "cortex-m",
  "cortex-m-rt",
  "cortex-m-rtic",
@@ -522,6 +539,7 @@ dependencies = [
  "embassy-nrf",
  "fixed",
  "futures",
+ "heapless",
  "nrf-softdevice",
  "nrf-softdevice-defmt-rtt",
  "nrf-softdevice-s140",
diff --git a/examples/Cargo.toml b/examples/Cargo.toml
index 3e65522..e670266 100644
--- a/examples/Cargo.toml
+++ b/examples/Cargo.toml
@@ -36,6 +36,8 @@ nrf-softdevice = { version = "0.1.0", path = "../nrf-softdevice", features = ["d
 nrf-softdevice-s140 = { version = "0.1.1", path = "../nrf-softdevice-s140" }
 futures = { version = "0.3.5", default-features = false }
 fixed = "1.2.0"
+heapless = "0.5.6"
+alloc-cortex-m = "0.4.0"
 
 
 [[bin]]
diff --git a/examples/src/bin/ble_bas_central.rs b/examples/src/bin/ble_bas_central.rs
index 0bef2f4..5216aea 100644
--- a/examples/src/bin/ble_bas_central.rs
+++ b/examples/src/bin/ble_bas_central.rs
@@ -1,6 +1,7 @@
 #![no_std]
 #![no_main]
 #![feature(type_alias_impl_trait)]
+#![feature(alloc_error_handler)]
 
 #[path = "../example_common.rs"]
 mod example_common;
diff --git a/examples/src/bin/ble_bas_peripheral.rs b/examples/src/bin/ble_bas_peripheral.rs
index 24c59a7..08e74fa 100644
--- a/examples/src/bin/ble_bas_peripheral.rs
+++ b/examples/src/bin/ble_bas_peripheral.rs
@@ -1,6 +1,7 @@
 #![no_std]
 #![no_main]
 #![feature(type_alias_impl_trait)]
+#![feature(alloc_error_handler)]
 
 #[path = "../example_common.rs"]
 mod example_common;
diff --git a/examples/src/bin/ble_l2cap_central.rs b/examples/src/bin/ble_l2cap_central.rs
new file mode 100644
index 0000000..c1675b5
--- /dev/null
+++ b/examples/src/bin/ble_l2cap_central.rs
@@ -0,0 +1,176 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+#![feature(alloc_error_handler)]
+extern crate alloc;
+
+#[path = "../example_common.rs"]
+mod example_common;
+use example_common::*;
+
+use core::mem;
+use core::ptr::NonNull;
+use core::slice;
+use cortex_m_rt::entry;
+use defmt::info;
+use defmt::{panic, *};
+use embassy::executor::{task, Executor};
+use embassy::util::Forever;
+use heapless::consts::*;
+
+use nrf_softdevice::ble::l2cap::Packet as _;
+use nrf_softdevice::ble::{central, l2cap, Address, Connection, Uuid};
+use nrf_softdevice::raw;
+use nrf_softdevice::Softdevice;
+
+static EXECUTOR: Forever<Executor> = Forever::new();
+
+const PSM: u16 = 0x2349;
+
+#[task]
+async fn softdevice_task(sd: &'static Softdevice) {
+    sd.run().await;
+}
+
+#[task]
+async fn ble_central_task(sd: &'static Softdevice) {
+    info!("Scanning for peer...");
+
+    let config = central::ScanConfig { whitelist: None };
+    let res = central::scan(sd, config, |params| unsafe {
+        let mut data = slice::from_raw_parts(params.data.p_data, params.data.len as usize);
+        while data.len() != 0 {
+            let len = data[0] as usize;
+            if data.len() < len + 1 {
+                break;
+            }
+            if len < 1 {
+                break;
+            }
+            let key = data[1];
+            let value = &data[2..len + 1];
+
+            if key == 0x06
+                && value
+                    == &[
+                        0xeb, 0x04, 0x8b, 0xfd, 0x5b, 0x03, 0x21, 0xb5, 0xeb, 0x11, 0x65, 0x2f,
+                        0x18, 0xce, 0x9c, 0x82,
+                    ]
+            {
+                return Some(Address::new_random_static(params.peer_addr.addr));
+            }
+            data = &data[len + 1..];
+        }
+        None
+    })
+    .await;
+    let address = unwrap!(res);
+    info!("Scan found address {:?}", address);
+
+    let addrs = &[address];
+
+    let config = central::Config::default();
+    let conn = unwrap!(central::connect(sd, addrs, config).await);
+    info!("connected");
+
+    let l = l2cap::L2cap::<Packet>::init(sd);
+    let ch = unwrap!(l.setup(&conn, PSM).await);
+    info!("l2cap connected");
+
+    for i in 0..10 {
+        let mut v = Vec::with_capacity(Packet::MTU);
+        v.extend(&[i; Packet::MTU]);
+        unwrap!(ch.tx(Packet(v)));
+        info!("l2cap tx done");
+    }
+    futures::future::pending::<()>().await;
+}
+
+use alloc::vec::Vec;
+
+struct Packet(Vec<u8>);
+impl l2cap::Packet for Packet {
+    const MTU: usize = 512;
+    fn allocate() -> Option<NonNull<u8>> {
+        let mut v = Vec::with_capacity(Self::MTU);
+        let ptr = v.as_mut_ptr();
+        mem::forget(v);
+        info!("allocate {:u32}", ptr as u32);
+        NonNull::new(ptr)
+    }
+
+    fn into_raw_parts(mut self) -> (NonNull<u8>, usize) {
+        let ptr = self.0.as_mut_ptr();
+        let len = self.0.len();
+        mem::forget(self);
+        info!("into_raw_parts {:u32}", ptr as u32);
+        (unwrap!(NonNull::new(ptr)), len)
+    }
+
+    unsafe fn from_raw_parts(ptr: NonNull<u8>, len: usize) -> Self {
+        info!("from_raw_parts {:u32}", ptr.as_ptr() as u32);
+        Self(Vec::from_raw_parts(ptr.as_ptr(), len, Self::MTU))
+    }
+}
+
+#[entry]
+fn main() -> ! {
+    info!("Hello World!");
+
+    let config = nrf_softdevice::Config {
+        clock: Some(raw::nrf_clock_lf_cfg_t {
+            source: raw::NRF_CLOCK_LF_SRC_XTAL as u8,
+            rc_ctiv: 0,
+            rc_temp_ctiv: 0,
+            accuracy: 7,
+        }),
+        conn_gap: Some(raw::ble_gap_conn_cfg_t {
+            conn_count: 20,
+            event_length: 180,
+        }),
+        conn_gatt: Some(raw::ble_gatt_conn_cfg_t { att_mtu: 114 }),
+        conn_gattc: Some(raw::ble_gattc_conn_cfg_t {
+            write_cmd_tx_queue_size: 0,
+        }),
+        conn_gatts: Some(raw::ble_gatts_conn_cfg_t {
+            hvn_tx_queue_size: 0,
+        }),
+        gatts_attr_tab_size: Some(raw::ble_gatts_cfg_attr_tab_size_t { attr_tab_size: 512 }),
+        gap_role_count: Some(raw::ble_gap_cfg_role_count_t {
+            adv_set_count: 1,
+            periph_role_count: 5,
+            central_role_count: 15,
+            central_sec_count: 0,
+            _bitfield_1: raw::ble_gap_cfg_role_count_t::new_bitfield_1(0),
+        }),
+        gap_device_name: Some(raw::ble_gap_cfg_device_name_t {
+            p_value: b"HelloRust" as *const u8 as _,
+            current_len: 9,
+            max_len: 9,
+            write_perm: unsafe { mem::zeroed() },
+            _bitfield_1: raw::ble_gap_cfg_device_name_t::new_bitfield_1(
+                raw::BLE_GATTS_VLOC_STACK as u8,
+            ),
+        }),
+        conn_l2cap: Some(raw::ble_l2cap_conn_cfg_t {
+            ch_count: 1,
+            rx_mps: 247,
+            tx_mps: 247,
+            rx_queue_size: 10,
+            tx_queue_size: 10,
+        }),
+        ..Default::default()
+    };
+
+    let (sdp, p) = take_peripherals();
+    let sd = Softdevice::enable(sdp, &config);
+
+    let executor = EXECUTOR.put(Executor::new(cortex_m::asm::sev));
+    unwrap!(executor.spawn(softdevice_task(sd)));
+    unwrap!(executor.spawn(ble_central_task(sd)));
+
+    loop {
+        executor.run();
+        cortex_m::asm::wfe();
+    }
+}
diff --git a/examples/src/bin/ble_l2cap_peripheral.rs b/examples/src/bin/ble_l2cap_peripheral.rs
new file mode 100644
index 0000000..312a794
--- /dev/null
+++ b/examples/src/bin/ble_l2cap_peripheral.rs
@@ -0,0 +1,163 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+#![feature(alloc_error_handler)]
+extern crate alloc;
+
+#[path = "../example_common.rs"]
+mod example_common;
+use example_common::*;
+
+use core::mem;
+use core::ptr::NonNull;
+use cortex_m_rt::entry;
+use defmt::{panic, *};
+use heapless::consts::*;
+
+use nrf_softdevice::ble;
+use nrf_softdevice::ble::{l2cap, peripheral, Connection};
+use nrf_softdevice::{raw, RawError, Softdevice};
+
+use embassy::executor::{task, Executor};
+use embassy::util::Forever;
+static EXECUTOR: Forever<Executor> = Forever::new();
+
+const PSM: u16 = 0x2349;
+
+#[task]
+async fn softdevice_task(sd: &'static Softdevice) {
+    sd.run().await;
+}
+
+#[task]
+async fn bluetooth_task(sd: &'static Softdevice, config: peripheral::Config) {
+    info!("My address: {:?}", ble::get_address(sd));
+
+    #[rustfmt::skip]
+    let adv_data = &[
+        0x02, 0x01, raw::BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE as u8,
+        0x11, 0x06, 0xeb, 0x04, 0x8b, 0xfd, 0x5b, 0x03, 0x21, 0xb5, 0xeb, 0x11, 0x65, 0x2f, 0x18, 0xce, 0x9c, 0x82,
+        0x02, 0x09, b'H',
+    ];
+    #[rustfmt::skip]
+    let scan_data = &[ ];
+
+    let l = l2cap::L2cap::<Packet>::init(sd);
+
+    loop {
+        let conn = unwrap!(
+            peripheral::advertise(
+                sd,
+                peripheral::ConnectableAdvertisement::ScannableUndirected {
+                    adv_data,
+                    scan_data,
+                },
+                config,
+            )
+            .await
+        );
+
+        info!("advertising done!");
+
+        let ch = unwrap!(l.listen(&conn, PSM).await);
+        info!("l2cap connected");
+
+        loop {
+            let pkt = unwrap!(ch.rx().await);
+            info!("rx: {:[u8]}", pkt.0);
+        }
+
+        futures::future::pending::<()>().await;
+    }
+}
+
+use alloc::vec::Vec;
+
+struct Packet(Vec<u8>);
+impl l2cap::Packet for Packet {
+    const MTU: usize = 512;
+    fn allocate() -> Option<NonNull<u8>> {
+        let mut v = Vec::with_capacity(Self::MTU);
+        let ptr = v.as_mut_ptr();
+        mem::forget(v);
+        info!("allocate {:u32}", ptr as u32);
+        NonNull::new(ptr)
+    }
+
+    fn into_raw_parts(mut self) -> (NonNull<u8>, usize) {
+        let ptr = self.0.as_mut_ptr();
+        let len = self.0.len();
+        mem::forget(self);
+        info!("into_raw_parts {:u32}", ptr as u32);
+        (unwrap!(NonNull::new(ptr)), len)
+    }
+
+    unsafe fn from_raw_parts(ptr: NonNull<u8>, len: usize) -> Self {
+        info!("from_raw_parts {:u32}", ptr.as_ptr() as u32);
+        Self(Vec::from_raw_parts(ptr.as_ptr(), len, Self::MTU))
+    }
+}
+
+#[entry]
+fn main() -> ! {
+    info!("Hello World!");
+
+    let config = nrf_softdevice::Config {
+        clock: Some(raw::nrf_clock_lf_cfg_t {
+            source: raw::NRF_CLOCK_LF_SRC_XTAL as u8,
+            rc_ctiv: 0,
+            rc_temp_ctiv: 0,
+            accuracy: 7,
+        }),
+        conn_gap: Some(raw::ble_gap_conn_cfg_t {
+            conn_count: 20,
+            event_length: 180,
+        }),
+        conn_gatt: Some(raw::ble_gatt_conn_cfg_t { att_mtu: 114 }),
+        conn_gattc: Some(raw::ble_gattc_conn_cfg_t {
+            write_cmd_tx_queue_size: 0,
+        }),
+        conn_gatts: Some(raw::ble_gatts_conn_cfg_t {
+            hvn_tx_queue_size: 0,
+        }),
+        gatts_attr_tab_size: Some(raw::ble_gatts_cfg_attr_tab_size_t {
+            attr_tab_size: 1024,
+        }),
+        gap_role_count: Some(raw::ble_gap_cfg_role_count_t {
+            adv_set_count: 1,
+            periph_role_count: 5,
+            central_role_count: 15,
+            central_sec_count: 0,
+            _bitfield_1: raw::ble_gap_cfg_role_count_t::new_bitfield_1(0),
+        }),
+        gap_device_name: Some(raw::ble_gap_cfg_device_name_t {
+            p_value: b"HelloRust" as *const u8 as _,
+            current_len: 9,
+            max_len: 9,
+            write_perm: unsafe { mem::zeroed() },
+            _bitfield_1: raw::ble_gap_cfg_device_name_t::new_bitfield_1(
+                raw::BLE_GATTS_VLOC_STACK as u8,
+            ),
+        }),
+        conn_l2cap: Some(raw::ble_l2cap_conn_cfg_t {
+            ch_count: 1,
+            rx_mps: 256,
+            tx_mps: 256,
+            rx_queue_size: 10,
+            tx_queue_size: 10,
+        }),
+        ..Default::default()
+    };
+
+    let (sdp, p) = take_peripherals();
+    let sd = Softdevice::enable(sdp, &config);
+
+    let executor = EXECUTOR.put(Executor::new(cortex_m::asm::sev));
+    unwrap!(executor.spawn(softdevice_task(sd)));
+    unwrap!(executor.spawn(bluetooth_task(sd, peripheral::Config::default())));
+
+    loop {
+        executor.run();
+        cortex_m::asm::wfe();
+    }
+}
diff --git a/examples/src/bin/ble_peripheral_onoff.rs b/examples/src/bin/ble_peripheral_onoff.rs
index 8476bca..625b895 100644
--- a/examples/src/bin/ble_peripheral_onoff.rs
+++ b/examples/src/bin/ble_peripheral_onoff.rs
@@ -1,6 +1,7 @@
 #![no_std]
 #![no_main]
 #![feature(type_alias_impl_trait)]
+#![feature(alloc_error_handler)]
 
 #[path = "../example_common.rs"]
 mod example_common;
diff --git a/examples/src/bin/ble_scan.rs b/examples/src/bin/ble_scan.rs
index 021c5e0..4e54978 100644
--- a/examples/src/bin/ble_scan.rs
+++ b/examples/src/bin/ble_scan.rs
@@ -1,6 +1,7 @@
 #![no_std]
 #![no_main]
 #![feature(type_alias_impl_trait)]
+#![feature(alloc_error_handler)]
 
 #[path = "../example_common.rs"]
 mod example_common;
diff --git a/examples/src/bin/flash.rs b/examples/src/bin/flash.rs
index c673d21..825a62b 100644
--- a/examples/src/bin/flash.rs
+++ b/examples/src/bin/flash.rs
@@ -1,6 +1,7 @@
 #![no_std]
 #![no_main]
 #![feature(type_alias_impl_trait)]
+#![feature(alloc_error_handler)]
 
 #[path = "../example_common.rs"]
 mod example_common;
diff --git a/examples/src/bin/interrupts.rs b/examples/src/bin/interrupts.rs
index 5ec8f2e..1c777b5 100644
--- a/examples/src/bin/interrupts.rs
+++ b/examples/src/bin/interrupts.rs
@@ -1,6 +1,7 @@
 #![no_std]
 #![no_main]
 #![feature(type_alias_impl_trait)]
+#![feature(alloc_error_handler)]
 
 #[path = "../example_common.rs"]
 mod example_common;
diff --git a/examples/src/bin/rtic.rs b/examples/src/bin/rtic.rs
index 08071e8..bc46c0e 100644
--- a/examples/src/bin/rtic.rs
+++ b/examples/src/bin/rtic.rs
@@ -12,6 +12,7 @@
 #![no_main]
 #![no_std]
 #![feature(type_alias_impl_trait)]
+#![feature(alloc_error_handler)]
 
 #[path = "../example_common.rs"]
 mod example_common;
diff --git a/examples/src/example_common.rs b/examples/src/example_common.rs
index 8f4c2f2..7c8cdb1 100644
--- a/examples/src/example_common.rs
+++ b/examples/src/example_common.rs
@@ -5,9 +5,24 @@ use nrf_softdevice::pac;
 use nrf_softdevice_defmt_rtt as _; // global logger
 use panic_probe as _;
 
+use alloc_cortex_m::CortexMHeap;
+use core::alloc::Layout;
 use core::sync::atomic::{AtomicUsize, Ordering};
 use defmt::{panic, *};
 
+// this is the allocator the application will use
+#[global_allocator]
+static ALLOCATOR: CortexMHeap = CortexMHeap::empty();
+
+// define what happens in an Out Of Memory (OOM) condition
+#[alloc_error_handler]
+fn alloc_error(_layout: Layout) -> ! {
+    panic!("Alloc error");
+    loop {}
+}
+
+const HEAP_SIZE: usize = 32 * 1024; // in bytes
+
 #[defmt::timestamp]
 fn timestamp() -> u64 {
     static COUNT: AtomicUsize = AtomicUsize::new(0);
@@ -19,6 +34,9 @@ fn timestamp() -> u64 {
 
 // Take peripherals, split by softdevice and application
 pub fn take_peripherals() -> (nrf_softdevice::Peripherals, Peripherals) {
+    // Initialize the allocator BEFORE you use it
+    unsafe { ALLOCATOR.init(cortex_m_rt::heap_start() as usize, HEAP_SIZE) }
+
     let p = unwrap!(pac::Peripherals::take());
 
     (
diff --git a/nrf-softdevice/src/ble/connection.rs b/nrf-softdevice/src/ble/connection.rs
index ddde97b..3c84b59 100644
--- a/nrf-softdevice/src/ble/connection.rs
+++ b/nrf-softdevice/src/ble/connection.rs
@@ -108,6 +108,8 @@ impl ConnectionState {
         gatt_client::portal(conn_handle).call(gatt_client::PortalMessage::Disconnected);
         #[cfg(feature = "ble-gatt-server")]
         gatt_server::portal(conn_handle).call(gatt_server::PortalMessage::Disconnected);
+        #[cfg(feature = "ble-l2cap")]
+        l2cap::portal(conn_handle).call(l2cap::PortalMessage::Disconnected);
 
         trace!("conn {:u8}: disconnected", index);
     }
diff --git a/nrf-softdevice/src/ble/events.rs b/nrf-softdevice/src/ble/events.rs
index 4cfd412..c0dd241 100644
--- a/nrf-softdevice/src/ble/events.rs
+++ b/nrf-softdevice/src/ble/events.rs
@@ -48,21 +48,21 @@ pub(crate) unsafe fn on_evt(ble_evt: *const raw::ble_evt_t) {
         raw::BLE_GAP_EVTS_BLE_GAP_EVT_ADV_SET_TERMINATED => peripheral::on_adv_set_terminated(ble_evt, get_union_field(ble_evt, &evt.evt.gap_evt)),
 
         #[cfg(feature="ble-l2cap")]
-        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_SETUP_REQUEST => l2cap::on_ch_setup_request(ble_evt, get_union_field(ble_evt, &evt.evt.l2cap_evt)),
+        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_SETUP_REQUEST => l2cap::on_ch_setup_request(ble_evt),
         #[cfg(feature="ble-l2cap")]
-        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_SETUP_REFUSED => l2cap::on_ch_setup_refused(ble_evt, get_union_field(ble_evt, &evt.evt.l2cap_evt)),
+        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_SETUP_REFUSED => l2cap::on_ch_setup_refused(ble_evt),
         #[cfg(feature="ble-l2cap")]
-        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_SETUP => l2cap::on_ch_setup(ble_evt, get_union_field(ble_evt, &evt.evt.l2cap_evt)),
+        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_SETUP => l2cap::on_ch_setup(ble_evt),
         #[cfg(feature="ble-l2cap")]
-        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_RELEASED => l2cap::on_ch_released(ble_evt, get_union_field(ble_evt, &evt.evt.l2cap_evt)),
+        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_RELEASED => l2cap::on_ch_released(ble_evt),
         #[cfg(feature="ble-l2cap")]
-        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_SDU_BUF_RELEASED => l2cap::on_ch_sdu_buf_released(ble_evt, get_union_field(ble_evt, &evt.evt.l2cap_evt)),
+        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_SDU_BUF_RELEASED => l2cap::on_ch_sdu_buf_released(ble_evt),
         #[cfg(feature="ble-l2cap")]
-        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_CREDIT => l2cap::on_ch_credit(ble_evt, get_union_field(ble_evt, &evt.evt.l2cap_evt)),
+        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_CREDIT => l2cap::on_ch_credit(ble_evt),
         #[cfg(feature="ble-l2cap")]
-        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_RX => l2cap::on_ch_rx(ble_evt, get_union_field(ble_evt, &evt.evt.l2cap_evt)),
+        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_RX => l2cap::on_ch_rx(ble_evt),
         #[cfg(feature="ble-l2cap")]
-        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_TX => l2cap::on_ch_tx(ble_evt, get_union_field(ble_evt, &evt.evt.l2cap_evt)),
+        raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_TX => l2cap::on_ch_tx(ble_evt),
 
         #[cfg(feature="ble-gatt-client")]
         raw::BLE_GATTC_EVTS_BLE_GATTC_EVT_PRIM_SRVC_DISC_RSP => gatt_client::on_prim_srvc_disc_rsp(ble_evt, get_union_field(ble_evt, &evt.evt.gattc_evt)),
@@ -165,7 +165,7 @@ pub(crate) unsafe fn on_conn_param_update(
 ) {
     let conn_params = gap_evt.params.conn_param_update.conn_params;
 
-    trace!(
+    debug!(
         "on_conn_param_update conn_handle={:u16} conn_sup_timeout={:u16} max_conn_interval={:u16} min_conn_interval={:u16} slave_latency={:u16}",
         gap_evt.conn_handle,
         conn_params.conn_sup_timeout,
@@ -327,7 +327,7 @@ pub(crate) unsafe fn on_data_length_update(
         state.data_length_effective = effective_params.max_tx_octets as u8;
     });
 
-    trace!(
+    debug!(
         "on_data_length_update conn_handle={:u16} max_rx_octets={:u16} max_rx_time_us={:u16} max_tx_octets={:u16} max_tx_time_us={:u16}",
         gap_evt.conn_handle,
         effective_params.max_rx_octets,
diff --git a/nrf-softdevice/src/ble/l2cap.rs b/nrf-softdevice/src/ble/l2cap.rs
index c658d5a..a1d9f78 100644
--- a/nrf-softdevice/src/ble/l2cap.rs
+++ b/nrf-softdevice/src/ble/l2cap.rs
@@ -1,31 +1,349 @@
-//! Link-Layer Control and Adaptation Protocol (unimplemented)
+//! Link-Layer Control and Adaptation Protocol
 
+use core::convert::TryInto;
+use core::marker::PhantomData;
+use core::ptr;
+use core::ptr::NonNull;
+use core::sync::atomic::{AtomicBool, Ordering};
+use core::u16;
+
+use crate::ble::*;
 use crate::raw;
+use crate::util::{assert, panic, unreachable, *};
+use crate::{RawError, Softdevice};
+
+fn evt_conn_handle(ble_evt: *const raw::ble_evt_t) -> u16 {
+    let evt = unsafe { get_union_field(ble_evt, &(*ble_evt).evt.l2cap_evt) };
+    evt.conn_handle
+}
+
+pub(crate) fn on_ch_setup_request(ble_evt: *const raw::ble_evt_t) {
+    trace!("on_ch_setup_request");
+    let conn_handle = evt_conn_handle(ble_evt);
+    portal(conn_handle).call(PortalMessage::SetupRequest(ble_evt));
+}
+
+pub(crate) fn on_ch_setup_refused(ble_evt: *const raw::ble_evt_t) {
+    trace!("on_ch_setup_refused");
+    let conn_handle = evt_conn_handle(ble_evt);
+    portal(conn_handle).call(PortalMessage::SetupRefused(ble_evt));
+}
+
+pub(crate) fn on_ch_setup(ble_evt: *const raw::ble_evt_t) {
+    trace!("on_ch_setup");
+    let conn_handle = evt_conn_handle(ble_evt);
+    portal(conn_handle).call(PortalMessage::SetupDone(ble_evt));
+}
+
+pub(crate) fn on_ch_released(ble_evt: *const raw::ble_evt_t) {
+    trace!("on_ch_released");
+    let conn_handle = evt_conn_handle(ble_evt);
+}
+
+pub(crate) fn on_ch_sdu_buf_released(ble_evt: *const raw::ble_evt_t) {
+    trace!("on_ch_sdu_buf_released");
+    unsafe {
+        let l2cap_evt = get_union_field(ble_evt, &(*ble_evt).evt.l2cap_evt);
+        let evt = &l2cap_evt.params.ch_sdu_buf_released;
+        let pkt = unwrap!(NonNull::new(evt.sdu_buf.p_data));
+        (unwrap!(PACKET_FREE))(pkt)
+    }
+}
+
+pub(crate) fn on_ch_credit(ble_evt: *const raw::ble_evt_t) {
+    trace!("on_ch_credit");
+    let conn_handle = evt_conn_handle(ble_evt);
+}
+
+pub(crate) fn on_ch_rx(ble_evt: *const raw::ble_evt_t) {
+    trace!("on_ch_rx");
+    let conn_handle = evt_conn_handle(ble_evt);
+    portal(conn_handle).call(PortalMessage::Received(ble_evt));
+}
+
+pub(crate) fn on_ch_tx(ble_evt: *const raw::ble_evt_t) {
+    trace!("on_ch_tx");
+    unsafe {
+        let l2cap_evt = get_union_field(ble_evt, &(*ble_evt).evt.l2cap_evt);
+        let evt = &l2cap_evt.params.tx;
+        let pkt = unwrap!(NonNull::new(evt.sdu_buf.p_data));
+        (unwrap!(PACKET_FREE))(pkt)
+    }
+}
+
+#[derive(defmt::Format)]
+pub enum TxError {
+    Disconnected,
+    Raw(RawError),
+}
+
+impl From<DisconnectedError> for TxError {
+    fn from(err: DisconnectedError) -> Self {
+        TxError::Disconnected
+    }
+}
+
+impl From<RawError> for TxError {
+    fn from(err: RawError) -> Self {
+        TxError::Raw(err)
+    }
+}
+#[derive(defmt::Format)]
+pub enum RxError {
+    Disconnected,
+    Raw(RawError),
+}
+
+impl From<DisconnectedError> for RxError {
+    fn from(err: DisconnectedError) -> Self {
+        RxError::Disconnected
+    }
+}
 
-pub(crate) fn on_ch_setup_request(
-    _ble_evt: *const raw::ble_evt_t,
-    _l2cap_evt: &raw::ble_l2cap_evt_t,
-) {
+impl From<RawError> for RxError {
+    fn from(err: RawError) -> Self {
+        RxError::Raw(err)
+    }
 }
 
-pub(crate) fn on_ch_setup_refused(
-    _ble_evt: *const raw::ble_evt_t,
-    _l2cap_evt: &raw::ble_l2cap_evt_t,
-) {
+#[derive(defmt::Format)]
+pub enum SetupError {
+    Disconnected,
+    Refused,
+    Raw(RawError),
 }
 
-pub(crate) fn on_ch_setup(_ble_evt: *const raw::ble_evt_t, _l2cap_evt: &raw::ble_l2cap_evt_t) {}
+impl From<DisconnectedError> for SetupError {
+    fn from(err: DisconnectedError) -> Self {
+        SetupError::Disconnected
+    }
+}
 
-pub(crate) fn on_ch_released(_ble_evt: *const raw::ble_evt_t, _l2cap_evt: &raw::ble_l2cap_evt_t) {}
+impl From<RawError> for SetupError {
+    fn from(err: RawError) -> Self {
+        SetupError::Raw(err)
+    }
+}
 
-pub(crate) fn on_ch_sdu_buf_released(
-    _ble_evt: *const raw::ble_evt_t,
-    _l2cap_evt: &raw::ble_l2cap_evt_t,
-) {
+static PORTALS: [Portal<PortalMessage>; CONNS_MAX] = [Portal::new(); CONNS_MAX];
+pub(crate) fn portal(conn_handle: u16) -> &'static Portal<PortalMessage> {
+    &PORTALS[conn_handle as usize]
 }
 
-pub(crate) fn on_ch_credit(_ble_evt: *const raw::ble_evt_t, _l2cap_evt: &raw::ble_l2cap_evt_t) {}
+pub(crate) enum PortalMessage {
+    SetupDone(*const raw::ble_evt_t),
+    SetupRefused(*const raw::ble_evt_t),
+    SetupRequest(*const raw::ble_evt_t),
+    Received(*const raw::ble_evt_t),
+    Disconnected,
+}
 
-pub(crate) fn on_ch_rx(_ble_evt: *const raw::ble_evt_t, _l2cap_evt: &raw::ble_l2cap_evt_t) {}
+pub trait Packet: Sized {
+    const MTU: usize;
+    fn allocate() -> Option<NonNull<u8>>;
+    fn into_raw_parts(self) -> (NonNull<u8>, usize);
+    unsafe fn from_raw_parts(ptr: NonNull<u8>, len: usize) -> Self;
+}
 
-pub(crate) fn on_ch_tx(_ble_evt: *const raw::ble_evt_t, _l2cap_evt: &raw::ble_l2cap_evt_t) {}
+pub struct L2cap<P: Packet> {
+    _private: PhantomData<*mut P>,
+}
+
+static IS_INIT: AtomicBool = AtomicBool::new(false);
+static mut PACKET_FREE: Option<unsafe fn(NonNull<u8>)> = None;
+
+impl<P: Packet> L2cap<P> {
+    pub fn init(sd: &Softdevice) -> Self {
+        if IS_INIT.compare_and_swap(false, true, Ordering::AcqRel) {
+            panic!("L2cap::init() called multiple times.")
+        }
+
+        unsafe {
+            PACKET_FREE = Some(|ptr| {
+                P::from_raw_parts(ptr, 0);
+                // create Packet from pointer, will be freed on drop
+            })
+        }
+
+        Self {
+            _private: PhantomData,
+        }
+    }
+
+    pub async fn setup(&self, conn: &Connection, psm: u16) -> Result<Channel<P>, SetupError> {
+        let sd = unsafe { Softdevice::steal() };
+
+        let conn_handle = conn.with_state(|state| state.check_connected())?;
+        let mut cid: u16 = raw::BLE_L2CAP_CID_INVALID as _;
+        let params = raw::ble_l2cap_ch_setup_params_t {
+            le_psm: psm,
+            status: 0, // only used when responding
+            rx_params: raw::ble_l2cap_ch_rx_params_t {
+                rx_mps: sd.l2cap_rx_mps,
+                rx_mtu: P::MTU as u16,
+                sdu_buf: raw::ble_data_t {
+                    len: 0,
+                    p_data: ptr::null_mut(),
+                },
+            },
+        };
+        let ret = unsafe { raw::sd_ble_l2cap_ch_setup(conn_handle, &mut cid, &params) };
+        if let Err(err) = RawError::convert(ret) {
+            warn!("sd_ble_l2cap_ch_setup err {:?}", err);
+            return Err(err.into());
+        }
+        info!("cid {:u16}", cid);
+
+        portal(conn_handle)
+            .wait_once(|msg| match msg {
+                PortalMessage::Disconnected => Err(SetupError::Disconnected),
+                PortalMessage::SetupDone(ble_evt) => unsafe {
+                    let l2cap_evt = get_union_field(ble_evt, &(*ble_evt).evt.l2cap_evt);
+                    let evt = &l2cap_evt.params.ch_setup;
+                    Ok(Channel {
+                        conn: conn.clone(),
+                        cid,
+                        _private: PhantomData,
+                    })
+                },
+                PortalMessage::SetupRefused(ble_evt) => unsafe {
+                    let l2cap_evt = get_union_field(ble_evt, &(*ble_evt).evt.l2cap_evt);
+                    let evt = &l2cap_evt.params.ch_setup_refused;
+                    Err(SetupError::Refused)
+                },
+                _ => unreachable!(),
+            })
+            .await
+    }
+
+    pub async fn listen(&self, conn: &Connection, psm: u16) -> Result<Channel<P>, SetupError> {
+        let sd = unsafe { Softdevice::steal() };
+        let conn_handle = conn.with_state(|state| state.check_connected())?;
+
+        portal(conn_handle)
+            .wait_many(|msg| match msg {
+                PortalMessage::Disconnected => Some(Err(SetupError::Disconnected)),
+                PortalMessage::SetupRequest(ble_evt) => unsafe {
+                    let l2cap_evt = get_union_field(ble_evt, &(*ble_evt).evt.l2cap_evt);
+                    let evt = &l2cap_evt.params.ch_setup_request;
+
+                    let mut cid: u16 = l2cap_evt.local_cid;
+                    if evt.le_psm == psm {
+                        let params = raw::ble_l2cap_ch_setup_params_t {
+                            le_psm: evt.le_psm,
+                            status: raw::BLE_L2CAP_CH_STATUS_CODE_SUCCESS as _,
+                            rx_params: raw::ble_l2cap_ch_rx_params_t {
+                                rx_mps: sd.l2cap_rx_mps,
+                                rx_mtu: P::MTU as u16,
+                                sdu_buf: raw::ble_data_t {
+                                    len: 0,
+                                    p_data: ptr::null_mut(),
+                                },
+                            },
+                        };
+
+                        let ret = raw::sd_ble_l2cap_ch_setup(conn_handle, &mut cid, &params);
+                        if let Err(err) = RawError::convert(ret) {
+                            warn!("sd_ble_l2cap_ch_setup err {:?}", err);
+                            return Some(Err(err.into()));
+                        }
+
+                        info!("cid {:u16}", cid);
+                        Some(Ok(Channel {
+                            _private: PhantomData,
+                            cid,
+                            conn: conn.clone(),
+                        }))
+                    } else {
+                        let params = raw::ble_l2cap_ch_setup_params_t {
+                            le_psm: evt.le_psm,
+                            status: raw::BLE_L2CAP_CH_STATUS_CODE_LE_PSM_NOT_SUPPORTED as _,
+                            rx_params: mem::zeroed(),
+                        };
+
+                        let ret = raw::sd_ble_l2cap_ch_setup(conn_handle, &mut cid, &params);
+                        if let Err(err) = RawError::convert(ret) {
+                            warn!("sd_ble_l2cap_ch_setup err {:?}", err);
+                        }
+
+                        None
+                    }
+                },
+                _ => unreachable!(),
+            })
+            .await
+    }
+}
+
+pub struct Channel<P: Packet> {
+    _private: PhantomData<*mut P>,
+    conn: Connection,
+    cid: u16,
+}
+
+impl<P: Packet> Clone for Channel<P> {
+    fn clone(&self) -> Self {
+        Self {
+            _private: PhantomData,
+            conn: self.conn.clone(),
+            cid: self.cid,
+        }
+    }
+}
+
+impl<P: Packet> Channel<P> {
+    pub fn connection(&self) -> &Connection {
+        &self.conn
+    }
+
+    pub fn tx(&self, sdu: P) -> Result<(), TxError> {
+        let conn_handle = self.conn.with_state(|s| s.check_connected())?;
+
+        let (ptr, len) = sdu.into_raw_parts();
+        assert!(len <= P::MTU);
+        let data = raw::ble_data_t {
+            p_data: ptr.as_ptr(),
+            len: len as u16,
+        };
+
+        let ret = unsafe { raw::sd_ble_l2cap_ch_tx(conn_handle, self.cid, &data) };
+        if let Err(err) = RawError::convert(ret) {
+            // todo handle error
+            // probably free the packet because the SD didn't take ownership of it
+            panic!("sd_ble_l2cap_ch_tx err {:?}", err);
+        }
+
+        Ok(())
+    }
+
+    pub async fn rx(&self) -> Result<P, RxError> {
+        let conn_handle = self.conn.with_state(|s| s.check_connected())?;
+
+        let ptr = unwrap!(P::allocate());
+        let data = raw::ble_data_t {
+            p_data: ptr.as_ptr(),
+            len: P::MTU as u16,
+        };
+
+        let ret = unsafe { raw::sd_ble_l2cap_ch_rx(conn_handle, self.cid, &data) };
+        if let Err(err) = RawError::convert(ret) {
+            panic!("sd_ble_l2cap_ch_rx err {:?}", err);
+        }
+
+        portal(conn_handle)
+            .wait_once(|msg| match msg {
+                PortalMessage::Disconnected => Err(RxError::Disconnected),
+                PortalMessage::Received(ble_evt) => unsafe {
+                    let l2cap_evt = get_union_field(ble_evt, &(*ble_evt).evt.l2cap_evt);
+                    let evt = &l2cap_evt.params.rx;
+
+                    let ptr = unwrap!(NonNull::new(evt.sdu_buf.p_data));
+                    let len = evt.sdu_len;
+                    let pkt = Packet::from_raw_parts(ptr, len as usize);
+                    Ok(pkt)
+                },
+                _ => unreachable!(),
+            })
+            .await
+    }
+}