samples/bluetooth/iso_broadcast_benchmark/src/receiver.c

/*
 * Copyright (c) 2021 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <ctype.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/conn.h>
#include <bluetooth/iso.h>
#include <sys/byteorder.h>
#include <console/console.h>

#include <logging/log.h>
LOG_MODULE_REGISTER(iso_broadcast_receiver, LOG_LEVEL_DBG);

#define DEVICE_NAME	CONFIG_BT_DEVICE_NAME
#define DEVICE_NAME_LEN (sizeof(DEVICE_NAME))

#define PA_RETRY_COUNT 6
#define ISO_RETRY_COUNT 10

struct iso_recv_stats {
	uint32_t     iso_recv_count;
	uint32_t     iso_lost_count;
};

static bool         broadcaster_found;
static bool         per_adv_lost;
static bool         big_sync_lost;
static bool         biginfo_received;
static bt_addr_le_t per_addr;
static uint8_t      per_sid;
static uint16_t     per_interval_ms;
static uint16_t     iso_interval_ms;
static uint8_t      bis_count;
static uint32_t     last_received_counter;
static int64_t      big_sync_start_time;
static size_t       big_sync_count;

static struct iso_recv_stats stats_current_sync;
static struct iso_recv_stats stats_overall;

static K_SEM_DEFINE(sem_per_adv, 0, 1);
static K_SEM_DEFINE(sem_per_sync, 0, 1);
static K_SEM_DEFINE(sem_per_sync_lost, 0, 1);
static K_SEM_DEFINE(sem_per_big_info, 0, 1);
static K_SEM_DEFINE(sem_big_sync, 0, 1);
static K_SEM_DEFINE(sem_big_sync_lost, 0, 1);

static const char *phy2str(uint8_t phy)
{
	switch (phy) {
	case 0: return "No packets";
	case BT_GAP_LE_PHY_1M: return "LE 1M";
	case BT_GAP_LE_PHY_2M: return "LE 2M";
	case BT_GAP_LE_PHY_CODED: return "LE Coded";
	default: return "Unknown";
	}
}

static bool data_cb(struct bt_data *data, void *user_data)
{
	char *name = user_data;
	uint8_t len;

	switch (data->type) {
	case BT_DATA_NAME_SHORTENED:
	case BT_DATA_NAME_COMPLETE:
		len = MIN(data->data_len, DEVICE_NAME_LEN - 1);
		memcpy(name, data->data, len);
		name[len] = '\0';
		return false;
	default:
		return true;
	}
}

static void scan_recv(const struct bt_le_scan_recv_info *info,
		      struct net_buf_simple *buf)
{
	char le_addr[BT_ADDR_LE_STR_LEN];
	char name[DEVICE_NAME_LEN];

	if (broadcaster_found) {
		return;
	}

	(void)memset(name, 0, sizeof(name));

	bt_data_parse(buf, data_cb, name);

	if (strncmp(DEVICE_NAME, name, strlen(DEVICE_NAME))) {
		return;
	}

	bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));

	LOG_INF("Found broadcaster with address %s (RSSI %i)",
		le_addr, info->rssi);

	broadcaster_found = true;

	per_sid = info->sid;
	per_interval_ms = BT_CONN_INTERVAL_TO_MS(info->interval);
	bt_addr_le_copy(&per_addr, info->addr);

	k_sem_give(&sem_per_adv);
}

static struct bt_le_scan_cb scan_callbacks = {
	.recv = scan_recv,
};

static void sync_cb(struct bt_le_per_adv_sync *sync,
		    struct bt_le_per_adv_sync_synced_info *info)
{
	LOG_INF("Periodic advertisement synced");

	k_sem_give(&sem_per_sync);
}

static void term_cb(struct bt_le_per_adv_sync *sync,
		    const struct bt_le_per_adv_sync_term_info *info)
{
	LOG_INF("Periodic advertisement sync terminated");

	per_adv_lost = true;
	k_sem_give(&sem_per_sync_lost);
}

static void biginfo_cb(struct bt_le_per_adv_sync *sync,
		       const struct bt_iso_biginfo *biginfo)
{
	if (biginfo_received) {
		return;
	}

	LOG_INF("BIGinfo received: num_bis %u, nse %u, interval %.2f ms, "
		"bn %u, pto %u, irc %u, max_pdu %u, sdu_interval %u us, "
		"max_sdu %u, phy %s, %s framing, %sencrypted",
		biginfo->num_bis, biginfo->sub_evt_count,
		BT_CONN_INTERVAL_TO_MS((float)biginfo->iso_interval),
		biginfo->burst_number, biginfo->offset, biginfo->rep_count,
		biginfo->max_pdu, biginfo->sdu_interval, biginfo->max_sdu,
		phy2str(biginfo->phy), biginfo->framing ? "with" : "without",
		biginfo->encryption ? "" : "not ");

	iso_interval_ms = BT_CONN_INTERVAL_TO_MS(biginfo->iso_interval);
	bis_count = MIN(biginfo->num_bis, CONFIG_BT_ISO_MAX_CHAN);
	biginfo_received = true;
	k_sem_give(&sem_per_big_info);
}

static struct bt_le_per_adv_sync_cb sync_callbacks = {
	.synced = sync_cb,
	.term = term_cb,
	.biginfo = biginfo_cb,
};

static void print_stats(char *name, struct iso_recv_stats *stats)
{
	uint32_t total_packets;

	total_packets = stats->iso_recv_count + stats->iso_lost_count;

	LOG_INF("%s: Received %u/%u (%.2f%%) - Total packets lost %u",
		name, stats->iso_recv_count, total_packets,
		(float)stats->iso_recv_count * 100 / total_packets,
		stats->iso_lost_count);

}

static void iso_recv(struct bt_iso_chan *chan,
		     const struct bt_iso_recv_info *info,
		     struct net_buf *buf)
{
	uint32_t total_packets;
	static bool stats_latest_arr[1000];
	static size_t stats_latest_arr_pos;

	/* NOTE: The packets received may be on different BISes */

	if (info->flags & BT_ISO_FLAGS_VALID) {
		stats_current_sync.iso_recv_count++;
		stats_overall.iso_recv_count++;
		stats_latest_arr[stats_latest_arr_pos++] = true;
	} else {
		stats_current_sync.iso_lost_count++;
		stats_overall.iso_lost_count++;
		stats_latest_arr[stats_latest_arr_pos++] = false;
	}

	if (stats_latest_arr_pos == sizeof(stats_latest_arr)) {
		stats_latest_arr_pos = 0;
	}

	total_packets = stats_overall.iso_recv_count + stats_overall.iso_lost_count;

	if ((total_packets % 100) == 0) {
		struct iso_recv_stats stats_latest = { 0 };

		for (int i = 0; i < ARRAY_SIZE(stats_latest_arr); i++) {
			/* If we have not yet received 1000 packets, break
			 * early
			 */
			if (i == total_packets) {
				break;
			}

			if (stats_latest_arr[i]) {
				stats_latest.iso_recv_count++;
			} else {
				stats_latest.iso_lost_count++;
			}
		}

		print_stats("Overall     ", &stats_overall);
		print_stats("Current Sync", &stats_current_sync);
		print_stats("Latest 1000 ", &stats_latest);
		LOG_INF(""); /* Empty line to separate the stats */
	}
}

static void iso_connected(struct bt_iso_chan *chan)
{
	LOG_INF("ISO Channel %p connected", chan);

	big_sync_start_time = k_uptime_get();

	k_sem_give(&sem_big_sync);
}

static void iso_disconnected(struct bt_iso_chan *chan, uint8_t reason)
{
	/* Calculate cumulative moving average - Be aware that this may
	 * cause overflow at sufficiently large counts or durations
	 */
	static int64_t average_duration;
	uint64_t big_sync_duration = k_uptime_get() - big_sync_start_time;
	uint64_t total_duration = big_sync_duration + (big_sync_count - 1) * average_duration;

	average_duration = total_duration / big_sync_count;

	LOG_INF("ISO Channel %p disconnected with reason 0x%02x after "
		"%llu milliseconds (average duration %llu)",
		chan, reason, big_sync_duration, average_duration);

	big_sync_lost = true;

	k_sem_give(&sem_big_sync_lost);
}

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_qos;

static struct bt_iso_chan_qos bis_iso_qos = {
	.rx = &iso_rx_qos,
};


static int start_scan(void)
{
	int err;

	err = bt_le_scan_start(BT_LE_SCAN_ACTIVE, NULL);
	if (err != 0) {
		LOG_ERR("Scan start failed (err %d)", err);
		return err;
	}

	LOG_INF("Scan started");

	return 0;
}

static int stop_scan(void)
{
	int err;

	err = bt_le_scan_stop();
	if (err != 0) {
		LOG_ERR("Scan stop failed (err %d)", err);
		return err;
	}
	LOG_INF("Scan stopped");

	return 0;
}

static int create_pa_sync(struct bt_le_per_adv_sync **sync)
{
	struct bt_le_per_adv_sync_param sync_create_param;
	int err;
	uint32_t sem_timeout;

	LOG_INF("Creating Periodic Advertising Sync");
	bt_addr_le_copy(&sync_create_param.addr, &per_addr);
	sync_create_param.options = 0;
	sync_create_param.sid = per_sid;
	sync_create_param.skip = 0;
	sync_create_param.timeout = (per_interval_ms * PA_RETRY_COUNT) / 10;
	sem_timeout = per_interval_ms * PA_RETRY_COUNT;
	err = bt_le_per_adv_sync_create(&sync_create_param, sync);
	if (err != 0) {
		LOG_ERR("Periodic advertisement sync create failed (err %d)",
		       err);
		return err;
	}

	LOG_INF("Waiting for periodic sync");
	err = k_sem_take(&sem_per_sync, K_MSEC(sem_timeout));
	if (err != 0) {
		LOG_INF("failed to take sem_per_sync (err %d)", err);

		LOG_INF("Deleting Periodic Advertising Sync");
		err = bt_le_per_adv_sync_delete(*sync);
		if (err != 0) {
			LOG_ERR("Failed to delete Periodic advertisement sync (err %d)",
			       err);

			return err;
		}
	}
	LOG_INF("Periodic sync established");

	return 0;
}

static int create_big_sync(struct bt_iso_big **big, struct bt_le_per_adv_sync *sync)
{
	int err;
	uint32_t sem_timeout = per_interval_ms * PA_RETRY_COUNT;
	uint32_t sync_timeout_ms;
	static struct bt_iso_chan bis_iso_chan[CONFIG_BT_ISO_MAX_CHAN];
	struct bt_iso_chan *bis[CONFIG_BT_ISO_MAX_CHAN];
	struct bt_iso_big_sync_param big_sync_param = {
		.bis_channels = bis,
		.num_bis = 0,
		.bis_bitfield = 0,
		.mse = 0,
		.sync_timeout = 0
	};

	for (int i = 0; i < ARRAY_SIZE(bis_iso_chan); i++) {
		bis_iso_chan[i].ops = &iso_ops;
		bis_iso_chan[i].qos = &bis_iso_qos;
		bis[i] = &bis_iso_chan[i];
	}

	LOG_INF("Waiting for BIG info");
	err = k_sem_take(&sem_per_big_info, K_MSEC(sem_timeout));
	if (err != 0) {
		LOG_ERR("failed to take sem_per_big_info (err %d)", err);
		return err;
	}

	sync_timeout_ms = iso_interval_ms * ISO_RETRY_COUNT;
	big_sync_param.sync_timeout = CLAMP(sync_timeout_ms / 10, 0x000A, 0x4000); /* 10 ms units */
	big_sync_param.num_bis = bis_count;
	/* BIS indexes start from 0x01, so add one to `i` */
	for (int i = 1; i <= big_sync_param.num_bis; i++) {
		big_sync_param.bis_bitfield |= BIT(i);
	}

	LOG_INF("Syncing to BIG");
	err = bt_iso_big_sync(sync, &big_sync_param, big);
	if (err != 0) {
		LOG_ERR("BIG sync failed (err %d)", err);
		return err;
	}

	LOG_INF("Waiting for BIG sync");
	err = k_sem_take(&sem_big_sync, K_MSEC(sem_timeout));
	if (err != 0) {
		LOG_ERR("failed to take sem_big_sync (err %d)", err);
		return err;
	}
	LOG_INF("BIG sync established");

	big_sync_count++;

	return 0;
}

static int cleanup(struct bt_le_per_adv_sync *sync, struct bt_iso_big *big)
{
	int pa_err = 0;
	int big_err = 0;

	if (!per_adv_lost && sync) {
		LOG_INF("Deleting Periodic advertisement Sync");
		pa_err = bt_le_per_adv_sync_delete(sync);
		if (pa_err != 0) {
			LOG_ERR("Failed to delete Periodic advertisement sync (err %d)",
				pa_err);
		}
	}

	if (!big_sync_lost && big) {
		LOG_INF("Terminating BIG Sync");
		big_err = bt_iso_big_terminate(big);
		if (big_err != 0) {
			LOG_ERR("BIG terminate failed (err %d)", big_err);
		}
	}

	if (pa_err != 0 || big_err != 0) {
		LOG_ERR("Cleanup failed (%d), recommend restart application to "
		       "avoid any potential leftovers",
		       pa_err != 0 ? pa_err : big_err);
	}

	return pa_err != 0 ? pa_err : big_err;
}

static void reset_sems(void)
{
	(void)k_sem_reset(&sem_per_adv);
	(void)k_sem_reset(&sem_per_sync);
	(void)k_sem_reset(&sem_per_sync_lost);
	(void)k_sem_reset(&sem_per_big_info);
	(void)k_sem_reset(&sem_big_sync);
	(void)k_sem_reset(&sem_big_sync_lost);
}

int test_run_receiver(void)
{
	struct bt_le_per_adv_sync *sync = NULL;
	struct bt_iso_big *big = NULL;
	int err;
	static bool callbacks_registered;

	broadcaster_found = false;
	per_adv_lost = false;
	big_sync_lost = false;
	biginfo_received = false;
	reset_sems();

	if (!callbacks_registered) {
		bt_le_per_adv_sync_cb_register(&sync_callbacks);
		bt_le_scan_cb_register(&scan_callbacks);
		callbacks_registered = true;
	}

	err = start_scan();
	if (err != 0) {
		return err;
	}

	LOG_INF("Waiting for periodic advertiser");
	err = k_sem_take(&sem_per_adv, K_FOREVER);
	if (err != 0) {
		LOG_ERR("failed to take sem_per_adv (err %d)", err);
		return err;
	}
	LOG_INF("Periodic advertiser found");

	err = stop_scan();
	if (err != 0) {
		return err;
	}

	err = create_pa_sync(&sync);
	if (err != 0) {
		return err;
	}

	last_received_counter = 0;
	memset(&stats_current_sync, 0, sizeof(stats_current_sync));
	big_sync_start_time = 0;

	err = create_big_sync(&big, sync);
	if (err != 0) {
		(void)cleanup(sync, big);
		return err;
	}

	err = k_sem_take(&sem_big_sync_lost, K_FOREVER);
	if (err != 0) {
		LOG_ERR("failed to take sem_big_sync_lost (err %d)", err);
		return err;
	}
	LOG_INF("BIG sync lost, returning");
	/* TODO: Add support to cancel the test early */

	return cleanup(sync, big);
}