From 6560cc42e64811b64d22bb97e87810e7b1ab8f45 Mon Sep 17 00:00:00 2001
From: Thomas Gagneret <thomas.gagneret@hexploy.com>
Date: Wed, 14 Aug 2024 11:18:12 +0200
Subject: [PATCH] =?UTF-8?q?Peripheral=20TX=C2=A0ISO=20/=C2=A0Central=20RX?=
 =?UTF-8?q?=20ISO?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 samples/bluetooth/central_iso/prj.conf      |  21 +++
 samples/bluetooth/central_iso/src/main.c    | 149 +++++++++++---------
 samples/bluetooth/peripheral_iso/prj.conf   |  13 ++
 samples/bluetooth/peripheral_iso/src/main.c | 145 ++++++++++++-------
 4 files changed, 212 insertions(+), 116 deletions(-)

diff --git a/samples/bluetooth/central_iso/prj.conf b/samples/bluetooth/central_iso/prj.conf
index 81264161a5c..07486c6e4ae 100644
--- a/samples/bluetooth/central_iso/prj.conf
+++ b/samples/bluetooth/central_iso/prj.conf
@@ -1,3 +1,24 @@
 CONFIG_BT=y
 CONFIG_LOG=y
 CONFIG_BT_ISO_CENTRAL=y
+
+CONFIG_BT_DEVICE_NAME="ISO Central"
+CONFIG_BT_PERIPHERAL=y
+CONFIG_BT_MAX_PAIRED=5
+CONFIG_BT_MAX_CONN=5
+
+CONFIG_BT_SETTINGS=y
+CONFIG_FLASH=y
+CONFIG_FLASH_PAGE_LAYOUT=y
+CONFIG_FLASH_MAP=y
+CONFIG_NVS=y
+CONFIG_SETTINGS=y
+
+CONFIG_LOG=y
+CONFIG_LOG_BACKEND_UART=n
+CONFIG_LOG_BACKEND_RTT=y
+CONFIG_CONSOLE=y
+CONFIG_RTT_CONSOLE=y
+CONFIG_USE_SEGGER_RTT=y
+CONFIG_STDOUT_CONSOLE=y
+CONFIG_CBPRINTF_FP_SUPPORT=y
\ No newline at end of file
diff --git a/samples/bluetooth/central_iso/src/main.c b/samples/bluetooth/central_iso/src/main.c
index 1521e387862..7ab3879b0a8 100644
--- a/samples/bluetooth/central_iso/src/main.c
+++ b/samples/bluetooth/central_iso/src/main.c
@@ -19,65 +19,42 @@
 #include <zephyr/settings/settings.h>
 #include <zephyr/sys/byteorder.h>
 
-static void start_scan(void);
+/* USER CONFIGURATION */
+#define CUSTOM_BLE_ISO_TRANS_RTN (10)
+#define CUSTOM_BLE_ADVERTISE 1 /* 0 or 1 */
+#define CUSTOM_BLE_ISO_LATENCY_MS (50U)
+#define CUSTOM_BLE_ISO_SDU (CONFIG_BT_ISO_TX_MTU)
+#define CUSTOM_BLE_ISO_TRANS_INTERVAL_MS (10U) // Should not be edited
+/* ACL */
+#define BT_LE_CONN_PARAM_CUSTOM BT_LE_CONN_PARAM(70/1.25, \
+						  70/1.25, \
+						  0, 400)
+/* Advertising */
+#define BT_LE_ADV_PARAM_CONN                                                                        \
+	BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_ONE_TIME,                        \
+			BT_GAP_ADV_SLOW_INT_MIN, BT_GAP_ADV_SLOW_INT_MIN, NULL)
 
 static struct bt_conn *default_conn;
-static struct k_work_delayable iso_send_work;
 static struct bt_iso_chan iso_chan;
-static uint16_t seq_num;
-static uint16_t latency_ms = 10U; /* 10ms */
-static uint32_t interval_us = 10U * USEC_PER_MSEC; /* 10 ms */
-NET_BUF_POOL_FIXED_DEFINE(tx_pool, 1, BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_TX_MTU),
+static uint16_t latency_ms = CUSTOM_BLE_ISO_LATENCY_MS;
+static uint32_t interval_us = CUSTOM_BLE_ISO_TRANS_INTERVAL_MS * USEC_PER_MSEC; /* 10 ms */
+NET_BUF_POOL_FIXED_DEFINE(tx_pool, 1, BT_ISO_SDU_BUF_SIZE(CUSTOM_BLE_ISO_SDU),
 			  CONFIG_BT_CONN_TX_USER_DATA_SIZE, NULL);
 
-/**
- * @brief Send ISO data on timeout
- *
- * This will send an increasing amount of ISO data, starting from 1 octet.
- *
- * First iteration : 0x00
- * Second iteration: 0x00 0x01
- * Third iteration : 0x00 0x01 0x02
- *
- * And so on, until it wraps around the configured ISO TX MTU (CONFIG_BT_ISO_TX_MTU)
- *
- * @param work Pointer to the work structure
- */
-static void iso_timer_timeout(struct k_work *work)
-{
-	int ret;
-	static uint8_t buf_data[CONFIG_BT_ISO_TX_MTU];
-	static bool data_initialized;
-	struct net_buf *buf;
-	static size_t len_to_send = 1;
-
-	if (!data_initialized) {
-		for (int i = 0; i < ARRAY_SIZE(buf_data); i++) {
-			buf_data[i] = (uint8_t)i;
-		}
-
-		data_initialized = true;
-	}
-
-	buf = net_buf_alloc(&tx_pool, K_FOREVER);
-	net_buf_reserve(buf, BT_ISO_CHAN_SEND_RESERVE);
+static uint32_t start_seq_num = 0;
+static int32_t missed = 0;
+static uint32_t start_timestamp = 0;
+static bool first_connect = false;
 
-	net_buf_add_mem(buf, buf_data, len_to_send);
+static uint8_t mfg_data[] = { 0x59, 0x00, 0x00 };
 
-	ret = bt_iso_chan_send(&iso_chan, buf, seq_num++);
-
-	if (ret < 0) {
-		printk("Failed to send ISO data (%d)\n", ret);
-		net_buf_unref(buf);
-	}
-
-	k_work_schedule(&iso_send_work, K_USEC(interval_us));
+static const struct bt_data ad_peer[] = {
+    BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
+    BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1),
+    BT_DATA(BT_DATA_MANUFACTURER_DATA, &mfg_data, sizeof(mfg_data)),
+};
 
-	len_to_send++;
-	if (len_to_send > ARRAY_SIZE(buf_data)) {
-		len_to_send = 1;
-	}
-}
+static void start_scan(void);
 
 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
 			 struct net_buf_simple *ad)
@@ -107,10 +84,10 @@ static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
 		return;
 	}
 
-	err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_DEFAULT,
+	err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_CUSTOM,
 				&default_conn);
 	if (err) {
-		printk("Create conn to %s failed (%u)\n", addr_str, err);
+		printk("Create conn to %s failed (%d)\n", addr_str, err);
 		start_scan();
 	}
 }
@@ -133,33 +110,65 @@ static void iso_connected(struct bt_iso_chan *chan)
 {
 	printk("ISO Channel %p connected\n", chan);
 
-	seq_num = 0U;
-
-	/* Start send timer */
-	k_work_schedule(&iso_send_work, K_MSEC(0));
+	start_seq_num = 0U;
+	missed = 0U;
+	start_timestamp = k_uptime_get();
+	first_connect = true;
 }
 
 static void iso_disconnected(struct bt_iso_chan *chan, uint8_t reason)
 {
 	printk("ISO Channel %p disconnected (reason 0x%02x)\n", chan, reason);
-	k_work_cancel_delayable(&iso_send_work);
+}
+
+static void iso_recv(struct bt_iso_chan *chan, const struct bt_iso_recv_info *info,
+		struct net_buf *buf)
+{
+	static uint32_t seq_num = 0;
+
+	if (!(info->flags & BT_ISO_FLAGS_VALID)) {
+		printk("Invalid frame\n");
+		return;
+	}
+	uint32_t* custom_seq_number = (uint32_t*) buf->data;
+
+	if (first_connect && buf->len) {
+		seq_num = *custom_seq_number;
+		first_connect = false;
+		start_seq_num = seq_num;
+	}
+
+	if(!buf->len) {
+		printk("Empty packet received\n");
+	} else if (seq_num != *custom_seq_number){
+		printk("received %d instead of %d\n", *custom_seq_number, seq_num);
+		missed += (*custom_seq_number - seq_num);
+		seq_num = (*custom_seq_number)+1;
+	} else {
+		++seq_num;
+	}
+
+	if (!(seq_num%100)) {
+		printk("Missed %d packets (total %d) in %lld\n", missed, seq_num - start_seq_num, k_uptime_get() - start_timestamp);
+	}
 }
 
 static struct bt_iso_chan_ops iso_ops = {
+	.recv		= iso_recv,
 	.connected	= iso_connected,
 	.disconnected	= iso_disconnected,
 };
 
-static struct bt_iso_chan_io_qos iso_tx = {
-	.sdu = CONFIG_BT_ISO_TX_MTU,
+static struct bt_iso_chan_io_qos iso_rx = {
+	.sdu = CUSTOM_BLE_ISO_SDU,
 	.phy = BT_GAP_LE_PHY_2M,
-	.rtn = 1,
+	.rtn = CUSTOM_BLE_ISO_TRANS_RTN,
 	.path = NULL,
 };
 
 static struct bt_iso_chan_qos iso_qos = {
-	.tx = &iso_tx,
-	.rx = NULL,
+	.tx = NULL,
+	.rx = &iso_rx,
 };
 
 static void connected(struct bt_conn *conn, uint8_t err)
@@ -237,13 +246,12 @@ int main(void)
 		settings_load();
 	}
 
-
 	printk("Bluetooth initialized\n");
 
 	iso_chan.ops = &iso_ops;
 	iso_chan.qos = &iso_qos;
 #if defined(CONFIG_BT_SMP)
-	iso_chan.required_sec_level = BT_SECURITY_L2,
+	iso_chan.required_sec_level = BT_SECURITY_L1,
 #endif /* CONFIG_BT_SMP */
 
 	channels[0] = &iso_chan;
@@ -264,8 +272,17 @@ int main(void)
 		return 0;
 	}
 
+#if (CUSTOM_BLE_ADVERTISE == 1)
+	err = bt_le_adv_start(BT_LE_ADV_PARAM_CONN, ad_peer, ARRAY_SIZE(ad_peer), NULL, 0);
+	if (err != 0) {
+		printk("Failed to start advertising(%d)\n", err);
+		return 0;
+	} else {
+		printk("Advertising started\n");
+	}
+#endif
+
 	start_scan();
 
-	k_work_init_delayable(&iso_send_work, iso_timer_timeout);
 	return 0;
 }
diff --git a/samples/bluetooth/peripheral_iso/prj.conf b/samples/bluetooth/peripheral_iso/prj.conf
index a605030ca01..96f56f685e4 100644
--- a/samples/bluetooth/peripheral_iso/prj.conf
+++ b/samples/bluetooth/peripheral_iso/prj.conf
@@ -10,3 +10,16 @@ CONFIG_FLASH=y
 CONFIG_FLASH_MAP=y
 CONFIG_NVS=y
 CONFIG_SETTINGS=y
+
+CONFIG_LOG=y
+CONFIG_LOG_BACKEND_UART=n
+CONFIG_LOG_BACKEND_RTT=y
+CONFIG_CONSOLE=y
+CONFIG_RTT_CONSOLE=y
+CONFIG_USE_SEGGER_RTT=y
+CONFIG_STDOUT_CONSOLE=y
+CONFIG_CBPRINTF_FP_SUPPORT=y
+
+CONFIG_BT_SMP_APP_PAIRING_ACCEPT=y
+CONFIG_BT_KEYS_OVERWRITE_OLDEST=y
+CONFIG_BT_BONDING_REQUIRED=y
\ No newline at end of file
diff --git a/samples/bluetooth/peripheral_iso/src/main.c b/samples/bluetooth/peripheral_iso/src/main.c
index e93b11bcf7f..cfe04b7dfb0 100644
--- a/samples/bluetooth/peripheral_iso/src/main.c
+++ b/samples/bluetooth/peripheral_iso/src/main.c
@@ -18,6 +18,19 @@
 #include <zephyr/bluetooth/iso.h>
 #include <zephyr/settings/settings.h>
 
+/* USER CONFIGURATION */
+#define CUSTOM_BLE_ISO_TRANS_RTN (10)
+#define CUSTOM_BLE_ISO_SDU (CONFIG_BT_ISO_TX_MTU)
+
+#define CUSTOM_BLE_ISO_TRANS_INTERVAL_MS (10U) // Should not be edited
+
+static uint16_t send_seq_num = 0;
+static struct k_work_delayable iso_send_work;
+static uint32_t interval_us = CUSTOM_BLE_ISO_TRANS_INTERVAL_MS * USEC_PER_MSEC;
+
+NET_BUF_POOL_FIXED_DEFINE(tx_pool, 1, BT_ISO_SDU_BUF_SIZE(CUSTOM_BLE_ISO_SDU),
+			  CONFIG_BT_CONN_TX_USER_DATA_SIZE, NULL);
+
 static const struct bt_data ad[] = {
 	BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
 };
@@ -26,6 +39,10 @@ static const struct bt_data sd[] = {
 	BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1),
 };
 
+void start_send(void) {
+	k_work_schedule(&iso_send_work, K_USEC(10));
+}
+
 static void connected(struct bt_conn *conn, uint8_t err)
 {
 	char addr[BT_ADDR_LE_STR_LEN];
@@ -49,60 +66,50 @@ static void disconnected(struct bt_conn *conn, uint8_t reason)
 	printk("Disconnected from %s (reason 0x%02x)\n", addr, reason);
 }
 
-BT_CONN_CB_DEFINE(conn_callbacks) = {
-	.connected = connected,
-	.disconnected = disconnected,
-};
-
-/** Print data as d_0 d_1 d_2 ... d_(n-2) d_(n-1) d_(n) to show the 3 first and 3 last octets
- *
- * Examples:
- * 01
- * 0102
- * 010203
- * 01020304
- * 0102030405
- * 010203040506
- * 010203...050607
- * 010203...060708
- * etc.
- */
-static void iso_print_data(uint8_t *data, size_t data_len)
+static enum bt_security_err
+pairing_accept(struct bt_conn *conn, //NOSONAR - Zephyr prototype definition
+	       const struct bt_conn_pairing_feat *const feat)
 {
-	/* Maximum number of octets from each end of the data */
-	const uint8_t max_octets = 3;
-	char data_str[35];
-	size_t str_len;
-
-	str_len = bin2hex(data, MIN(max_octets, data_len), data_str, sizeof(data_str));
-	if (data_len > max_octets) {
-		if (data_len > (max_octets * 2)) {
-			static const char dots[] = "...";
-
-			strcat(&data_str[str_len], dots);
-			str_len += strlen(dots);
-		}
+	ARG_UNUSED(conn);
+	ARG_UNUSED(feat);
 
-		str_len += bin2hex(data + (data_len - MIN(max_octets, data_len - max_octets)),
-				   MIN(max_octets, data_len - max_octets),
-				   data_str + str_len,
-				   sizeof(data_str) - str_len);
-	}
-
-	printk("\t %s\n", data_str);
+	return BT_SECURITY_ERR_SUCCESS;
 }
 
-static void iso_recv(struct bt_iso_chan *chan, const struct bt_iso_recv_info *info,
-		struct net_buf *buf)
+static void security_changed_cb(struct bt_conn *conn, //NOSONAR - Zephyr prototype definition
+				bt_security_t level, enum bt_security_err err)
 {
-	if (info->flags & BT_ISO_FLAGS_VALID) {
-		printk("Incoming data channel %p len %u\n", chan, buf->len);
-		iso_print_data(buf->data, buf->len);
+	ARG_UNUSED(conn);
+
+	if (err) {
+		printk("Security failed: level %u err %d. Disconnecting\n", level, err);
+		bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL);
+	} else {
+		printk("Security changed: level %u\n", level);
+
+		if (level >= BT_SECURITY_L2) {
+			printk("Paired\n");
+		}
 	}
 }
 
+BT_CONN_CB_DEFINE(conn_callbacks) = {
+	.connected = connected,
+	.disconnected = disconnected,
+	.security_changed = security_changed_cb,
+};
+
+static struct bt_conn_auth_cb auth_cb = {
+	.pairing_accept = pairing_accept,
+};
+
 static void iso_connected(struct bt_iso_chan *chan)
 {
+	send_seq_num = 0U;
+
+	/* Start send timer */
+	k_work_schedule(&iso_send_work, K_MSEC(0));
+
 	printk("ISO Channel %p connected\n", chan);
 }
 
@@ -112,19 +119,20 @@ static void iso_disconnected(struct bt_iso_chan *chan, uint8_t reason)
 }
 
 static struct bt_iso_chan_ops iso_ops = {
-	.recv		= iso_recv,
 	.connected	= iso_connected,
 	.disconnected	= iso_disconnected,
 };
 
-static struct bt_iso_chan_io_qos iso_rx = {
-	.sdu = CONFIG_BT_ISO_TX_MTU,
+static struct bt_iso_chan_io_qos iso_tx = {
+	.sdu = CUSTOM_BLE_ISO_SDU,
+	.phy = BT_GAP_LE_PHY_2M,
+	.rtn = CUSTOM_BLE_ISO_TRANS_RTN,
 	.path = NULL,
 };
 
 static struct bt_iso_chan_qos iso_qos = {
-	.rx = &iso_rx,
-	.tx = NULL,
+	.rx = NULL,
+	.tx = &iso_tx,
 };
 
 static struct bt_iso_chan iso_chan = {
@@ -150,10 +158,42 @@ static int iso_accept(const struct bt_iso_accept_info *info,
 static struct bt_iso_server iso_server = {
 #if defined(CONFIG_BT_SMP)
 	.sec_level = BT_SECURITY_L1,
-#endif /* CONFIG_BT_SMP */
+#endif
 	.accept = iso_accept,
 };
 
+static void iso_timer_timeout(struct k_work *work)
+{
+	int ret;
+	static uint8_t buf_data[CUSTOM_BLE_ISO_SDU];
+	uint32_t* custom_seq_number = (uint32_t*) buf_data;
+	static size_t len_to_send = sizeof(*custom_seq_number);
+	struct net_buf *buf;
+
+#if CUSTOM_BLE_ISO_SDU < 4
+#error CUSTOM_BLE_ISO_SDU must be > 4 (sizeof(uint32_t))
+#endif
+
+	send_seq_num++;
+	(*custom_seq_number) = send_seq_num;
+
+	buf = net_buf_alloc(&tx_pool, K_FOREVER);
+	net_buf_reserve(buf, BT_ISO_CHAN_SEND_RESERVE);
+
+	net_buf_add_mem(buf, buf_data, len_to_send);
+
+	ret = bt_iso_chan_send(&iso_chan, buf, send_seq_num);
+
+	if (ret < 0) {
+		printk("Failed to send ISO data (%d)\n", ret);
+		net_buf_unref(buf);
+	} else if (!(send_seq_num%10)){
+		printk("Send %d\n", send_seq_num);
+	}
+
+	k_work_schedule(&iso_send_work, K_USEC(interval_us));
+}
+
 int main(void)
 {
 	int err;
@@ -168,6 +208,8 @@ int main(void)
 		settings_load();
 	}
 
+	bt_conn_auth_cb_register(&auth_cb);
+
 	printk("Bluetooth initialized\n");
 
 	err = bt_iso_server_register(&iso_server);
@@ -183,5 +225,8 @@ int main(void)
 	}
 
 	printk("Advertising successfully started\n");
+
+	k_work_init_delayable(&iso_send_work, iso_timer_timeout);
+
 	return 0;
 }
-- 
2.45.2

