blob: 81d2449fd88a1e651251ff2b61ca7c6cec3aac31 [file] [log] [blame]
/*
* Copyright (c) 2021 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <ctype.h>
#include <kernel.h>
#include <string.h>
#include <stdlib.h>
#include <zephyr/types.h>
#include <console/console.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/iso.h>
#include <sys/byteorder.h>
#include <logging/log.h>
LOG_MODULE_REGISTER(iso_connected, LOG_LEVEL_DBG);
#define DEVICE_NAME CONFIG_BT_DEVICE_NAME
#define DEVICE_NAME_LEN (sizeof(DEVICE_NAME))
enum benchmark_role {
ROLE_CENTRAL,
ROLE_PERIPHERAL,
ROLE_QUIT
};
#define DEFAULT_CIS_RTN 2
#define DEFAULT_CIS_INTERVAL_US 7500
#define DEFAULT_CIS_LATENCY_MS 10
#define DEFAULT_CIS_PHY BT_GAP_LE_PHY_2M
#define DEFAULT_CIS_SDU_SIZE CONFIG_BT_ISO_TX_MTU
#define DEFAULT_CIS_PACKING 0
#define DEFAULT_CIS_FRAMING 0
#define DEFAULT_CIS_COUNT CONFIG_BT_ISO_MAX_CHAN
#define DEFAULT_CIS_SEC_LEVEL BT_SECURITY_L1
#define BUFFERS_ENQUEUED 2 /* Number of buffers enqueue for each channel */
BUILD_ASSERT(BUFFERS_ENQUEUED * CONFIG_BT_ISO_MAX_CHAN <= CONFIG_BT_ISO_TX_BUF_COUNT,
"Not enough buffers to enqueue");
struct iso_recv_stats {
uint32_t iso_recv_count;
uint32_t iso_lost_count;
};
struct iso_chan_work {
struct bt_iso_chan chan;
struct k_work_delayable send_work;
} iso_chans[CONFIG_BT_ISO_MAX_CHAN];
static enum benchmark_role role;
static struct bt_conn *default_conn;
static struct bt_iso_chan *cis[CONFIG_BT_ISO_MAX_CHAN];
static bool advertiser_found;
static bt_addr_le_t adv_addr;
static uint32_t last_received_counter;
static struct iso_recv_stats stats_current_conn;
static struct iso_recv_stats stats_overall;
static int64_t iso_conn_start_time;
static size_t total_iso_conn_count;
static uint32_t iso_send_count;
static struct bt_iso_cig *cig;
NET_BUF_POOL_FIXED_DEFINE(tx_pool, 1, BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_TX_MTU),
8, NULL);
static uint8_t iso_data[CONFIG_BT_ISO_TX_MTU];
static K_SEM_DEFINE(sem_adv, 0, 1);
static K_SEM_DEFINE(sem_iso_accept, 0, 1);
static K_SEM_DEFINE(sem_iso_connected, 0, CONFIG_BT_ISO_MAX_CHAN);
static K_SEM_DEFINE(sem_iso_disconnected, 0, CONFIG_BT_ISO_MAX_CHAN);
static K_SEM_DEFINE(sem_connected, 0, 1);
static K_SEM_DEFINE(sem_disconnected, 0, 1);
static struct bt_iso_chan_io_qos iso_tx_qos = {
.sdu = DEFAULT_CIS_SDU_SIZE, /* bytes */
.rtn = DEFAULT_CIS_RTN,
.phy = DEFAULT_CIS_PHY,
};
static struct bt_iso_chan_io_qos iso_rx_qos = {
.sdu = DEFAULT_CIS_SDU_SIZE, /* bytes */
.rtn = DEFAULT_CIS_RTN,
.phy = DEFAULT_CIS_PHY,
};
static struct bt_iso_chan_qos iso_qos = {
.tx = &iso_tx_qos,
.rx = &iso_rx_qos,
};
static struct bt_iso_cig_param cig_create_param = {
.interval = DEFAULT_CIS_INTERVAL_US, /* in microseconds */
.latency = DEFAULT_CIS_LATENCY_MS, /* milliseconds */
.sca = BT_GAP_SCA_UNKNOWN,
.packing = DEFAULT_CIS_PACKING,
.framing = DEFAULT_CIS_FRAMING,
.cis_channels = cis,
.num_cis = DEFAULT_CIS_COUNT
};
static enum benchmark_role device_role_select(void)
{
char role_char;
const char central_char = 'c';
const char peripheral_char = 'p';
const char quit_char = 'q';
while (true) {
printk("Choose device role - type %c (central role) or %c (peripheral role), or %c to quit: ",
central_char, peripheral_char, quit_char);
role_char = tolower(console_getchar());
printk("%c\n", role_char);
if (role_char == central_char) {
printk("Central role\n");
return ROLE_CENTRAL;
} else if (role_char == peripheral_char) {
printk("Peripheral role\n");
return ROLE_PERIPHERAL;
} else if (role_char == quit_char) {
printk("Quitting\n");
return ROLE_QUIT;
} else if (role_char == '\n' || role_char == '\r') {
continue;
}
printk("Invalid role: %c\n", role_char);
}
}
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_send(struct bt_iso_chan *chan)
{
int ret;
struct net_buf *buf;
struct iso_chan_work *chan_work;
if (chan->qos->tx == NULL || chan->qos->tx->sdu == 0) {
return;
}
chan_work = CONTAINER_OF(chan, struct iso_chan_work, chan);
buf = net_buf_alloc(&tx_pool, K_FOREVER);
if (buf == NULL) {
LOG_ERR("Could not allocate buffer");
k_work_reschedule(&chan_work->send_work, K_USEC(cig_create_param.interval));
return;
}
net_buf_reserve(buf, BT_ISO_CHAN_SEND_RESERVE);
net_buf_add_mem(buf, iso_data, iso_tx_qos.sdu);
ret = bt_iso_chan_send(chan, buf);
if (ret < 0) {
LOG_ERR("Unable to send data: %d", ret);
net_buf_unref(buf);
k_work_reschedule(&chan_work->send_work, K_USEC(cig_create_param.interval));
return;
}
iso_send_count++;
if ((iso_send_count % 100) == 0) {
LOG_INF("Sending value %u", iso_send_count);
}
}
static void iso_timer_timeout(struct k_work *work)
{
struct bt_iso_chan *chan;
struct iso_chan_work *chan_work;
struct k_work_delayable *delayable = k_work_delayable_from_work(work);
chan_work = CONTAINER_OF(delayable, struct iso_chan_work, send_work);
chan = &chan_work->chan;
iso_send(chan);
}
static void iso_sent(struct bt_iso_chan *chan)
{
struct iso_chan_work *chan_work;
chan_work = CONTAINER_OF(chan, struct iso_chan_work, chan);
k_work_reschedule(&chan_work->send_work, K_MSEC(0));
}
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 CISes */
if (info->flags & BT_ISO_FLAGS_VALID) {
stats_current_conn.iso_recv_count++;
stats_overall.iso_recv_count++;
stats_latest_arr[stats_latest_arr_pos++] = true;
} else {
stats_current_conn.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 Conn", &stats_current_conn);
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);
/* If multiple CIS was created, this will be the value of the last
* created in the CIG
*/
iso_conn_start_time = k_uptime_get();
k_sem_give(&sem_iso_connected);
}
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
*
* This duration is calculated for each CIS disconnected from the time
* of the last created CIS.
*/
static int64_t average_duration;
uint64_t iso_conn_duration;
uint64_t total_duration;
if (iso_conn_start_time > 0) {
iso_conn_duration = k_uptime_get() - iso_conn_start_time;
} else {
iso_conn_duration = 0;
}
total_duration = iso_conn_duration + (total_iso_conn_count - 1) * average_duration;
average_duration = total_duration / total_iso_conn_count;
LOG_INF("ISO Channel %p disconnected with reason 0x%02x after "
"%llu milliseconds (average duration %llu)",
chan, reason, iso_conn_duration, average_duration);
k_sem_give(&sem_iso_disconnected);
}
static struct bt_iso_chan_ops iso_ops = {
.recv = iso_recv,
.connected = iso_connected,
.disconnected = iso_disconnected,
.sent = iso_sent,
};
static int iso_accept(const struct bt_iso_accept_info *info,
struct bt_iso_chan **chan)
{
LOG_INF("Incoming ISO request from %p", (void *)info->acl);
for (int i = 0; i < ARRAY_SIZE(iso_chans); i++) {
if (iso_chans[i].chan.state == BT_ISO_STATE_DISCONNECTED) {
LOG_INF("Returning instance %d", i);
*chan = &iso_chans[i].chan;
cig_create_param.num_cis++;
k_sem_give(&sem_iso_accept);
return 0;
}
}
LOG_ERR("Could not accept any more CIS");
*chan = NULL;
return -ENOMEM;
}
static struct bt_iso_server iso_server = {
.sec_level = DEFAULT_CIS_SEC_LEVEL,
.accept = iso_accept,
};
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:
__fallthrough;
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 int start_scan(void)
{
int err;
err = bt_le_scan_start(BT_LE_SCAN_ACTIVE, NULL);
if (err != 0) {
LOG_ERR("Scan start failed: %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: %d", err);
return err;
}
LOG_INF("Scan stopped");
return 0;
}
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 (advertiser_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 peripheral with address %s (RSSI %i)",
le_addr, info->rssi);
bt_addr_le_copy(&adv_addr, info->addr);
advertiser_found = true;
k_sem_give(&sem_adv);
}
static struct bt_le_scan_cb scan_callbacks = {
.recv = scan_recv,
};
static void connected(struct bt_conn *conn, uint8_t err)
{
char addr[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
if (err != 0 && role == ROLE_CENTRAL) {
LOG_INF("Failed to connect to %s: %u", addr, err);
bt_conn_unref(default_conn);
default_conn = NULL;
return;
} else if (role == ROLE_PERIPHERAL) {
default_conn = bt_conn_ref(conn);
}
LOG_INF("Connected: %s", addr);
k_sem_give(&sem_connected);
}
static void disconnected(struct bt_conn *conn, uint8_t reason)
{
char addr[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
LOG_INF("Disconnected: %s (reason 0x%02x)", addr, reason);
bt_conn_unref(default_conn);
default_conn = NULL;
k_sem_give(&sem_disconnected);
}
static struct bt_conn_cb conn_callbacks = {
.connected = connected,
.disconnected = disconnected,
};
static size_t get_chars(char *buffer, size_t max_size)
{
size_t pos = 0;
while (pos < max_size) {
char c = tolower(console_getchar());
if (c == '\n' || c == '\r') {
break;
}
printk("%c", c);
buffer[pos++] = c;
}
printk("\n");
buffer[pos] = '\0';
return pos;
}
static int parse_rtn_arg(struct bt_iso_chan_io_qos *qos)
{
char buffer[4];
size_t char_count;
uint64_t rtn;
printk("Set RTN (current %u, default %u)\n",
qos->rtn, DEFAULT_CIS_RTN);
char_count = get_chars(buffer, sizeof(buffer) - 1);
if (char_count == 0) {
return DEFAULT_CIS_RTN;
}
rtn = strtoul(buffer, NULL, 0);
if (rtn > BT_ISO_CONNECTED_RTN_MAX) {
printk("Invalid RTN %llu", rtn);
return -EINVAL;
}
return (int)rtn;
}
static int parse_interval_arg(void)
{
char buffer[9];
size_t char_count;
uint64_t interval;
printk("Set interval (us) (current %u, default %u)\n",
cig_create_param.interval, DEFAULT_CIS_INTERVAL_US);
char_count = get_chars(buffer, sizeof(buffer) - 1);
if (char_count == 0) {
return DEFAULT_CIS_INTERVAL_US;
}
interval = strtoul(buffer, NULL, 0);
/* TODO: Replace literal ints with a #define once it has been created */
if (interval < BT_ISO_INTERVAL_MIN || interval > BT_ISO_INTERVAL_MAX) {
printk("Invalid interval %llu", interval);
return -EINVAL;
}
return (int)interval;
}
static int parse_latency_arg(void)
{
char buffer[6];
size_t char_count;
uint64_t latency;
printk("Set latency (ms) (current %u, default %u)\n",
cig_create_param.latency, DEFAULT_CIS_LATENCY_MS);
char_count = get_chars(buffer, sizeof(buffer) - 1);
if (char_count == 0) {
return DEFAULT_CIS_LATENCY_MS;
}
latency = strtoul(buffer, NULL, 0);
if (latency < BT_ISO_LATENCY_MIN || latency > BT_ISO_LATENCY_MAX) {
printk("Invalid latency %llu", latency);
return -EINVAL;
}
return (int)latency;
}
static int parse_phy_arg(struct bt_iso_chan_io_qos *qos)
{
char buffer[3];
size_t char_count;
uint64_t phy;
printk("Set PHY (current %u, default %u) - %u = 1M, %u = 2M, %u = Coded\n",
qos->phy, DEFAULT_CIS_PHY, BT_GAP_LE_PHY_1M,
BT_GAP_LE_PHY_2M, BT_GAP_LE_PHY_CODED);
char_count = get_chars(buffer, sizeof(buffer) - 1);
if (char_count == 0) {
return DEFAULT_CIS_PHY;
}
phy = strtoul(buffer, NULL, 0);
if (phy != BT_GAP_LE_PHY_1M &&
phy != BT_GAP_LE_PHY_2M &&
phy != BT_GAP_LE_PHY_CODED) {
printk("Invalid PHY %llu", phy);
return -EINVAL;
}
return (int)phy;
}
static int parse_sdu_arg(struct bt_iso_chan_io_qos *qos)
{
char buffer[6];
size_t char_count;
uint64_t sdu;
printk("Set SDU (current %u, default %u)\n",
qos->sdu, DEFAULT_CIS_SDU_SIZE);
char_count = get_chars(buffer, sizeof(buffer) - 1);
if (char_count == 0) {
return DEFAULT_CIS_SDU_SIZE;
}
sdu = strtoul(buffer, NULL, 0);
if (sdu > MIN(BT_ISO_MAX_SDU, sizeof(iso_data)) ||
sdu < sizeof(uint32_t) /* room for the counter */) {
printk("Invalid SDU %llu", sdu);
return -EINVAL;
}
return (int)sdu;
}
static int parse_cis_count_arg(void)
{
char buffer[4];
size_t char_count;
uint64_t cis_count;
printk("Set CIS count (current %u, default %u)\n",
cig_create_param.num_cis, DEFAULT_CIS_COUNT);
char_count = get_chars(buffer, sizeof(buffer) - 1);
if (char_count == 0) {
return DEFAULT_CIS_COUNT;
}
cis_count = strtoul(buffer, NULL, 0);
if (cis_count > MAX(BT_ISO_MAX_GROUP_ISO_COUNT, CONFIG_BT_ISO_MAX_CHAN)) {
printk("Invalid CIS count %llu", cis_count);
return -EINVAL;
}
return (int)cis_count;
}
static int parse_cig_args(void)
{
int interval;
int latency;
int cis_count;
printk("Follow the prompts. Press enter to use default values.\n");
cis_count = parse_cis_count_arg();
if (cis_count < 0) {
return -EINVAL;
}
interval = parse_interval_arg();
if (interval < 0) {
return -EINVAL;
}
latency = parse_latency_arg();
if (latency < 0) {
return -EINVAL;
}
cig_create_param.interval = interval;
cig_create_param.latency = latency;
cig_create_param.num_cis = cis_count;
return 0;
}
static int parse_cis_args(struct bt_iso_chan_io_qos *qos)
{
int rtn;
int phy;
int sdu;
printk("Follow the prompts. Press enter to use default values.\n");
rtn = parse_rtn_arg(qos);
if (rtn < 0) {
return -EINVAL;
}
phy = parse_phy_arg(qos);
if (phy < 0) {
return -EINVAL;
}
sdu = parse_sdu_arg(qos);
if (sdu < 0) {
return -EINVAL;
}
qos->rtn = rtn;
qos->phy = phy;
qos->sdu = sdu;
return 0;
}
static int change_central_settings(void)
{
char c;
int err;
printk("Change CIG settings (y/N)? (Current settings: cis_count=%u, "
"interval=%u, latency=%u)\n",
cig_create_param.num_cis, cig_create_param.interval,
cig_create_param.latency);
c = tolower(console_getchar());
if (c == 'y') {
err = parse_cig_args();
if (err != 0) {
return err;
}
printk("New settings: cis_count=%u, inteval=%u, latency=%u\n",
cig_create_param.num_cis, cig_create_param.interval,
cig_create_param.latency);
}
printk("Change TX settings (y/N)? (Current settings: rtn=%u, "
"phy=%u, sdu=%u)\n",
iso_tx_qos.rtn, iso_tx_qos.phy, iso_tx_qos.sdu);
c = tolower(console_getchar());
if (c == 'y') {
printk("Disable TX (y/N)?\n");
c = tolower(console_getchar());
if (c == 'y') {
iso_qos.tx = NULL;
printk("TX disabled\n");
} else {
iso_qos.tx = &iso_tx_qos;
err = parse_cis_args(&iso_tx_qos);
if (err != 0) {
return err;
}
printk("New settings: rtn=%u, phy=%u, sdu=%u\n",
iso_tx_qos.rtn, iso_tx_qos.phy, iso_tx_qos.sdu);
}
}
printk("Change RX settings (y/N)? (Current settings: rtn=%u, "
"phy=%u, sdu=%u)\n",
iso_rx_qos.rtn, iso_rx_qos.phy, iso_rx_qos.sdu);
c = tolower(console_getchar());
if (c == 'y') {
printk("Disable RX (y/N)?\n");
c = tolower(console_getchar());
if (c == 'y') {
if (iso_qos.tx == NULL) {
LOG_ERR("Cannot disable both TX and RX\n");
return -EINVAL;
}
iso_qos.rx = NULL;
printk("RX disabled\n");
} else {
printk("Set RX settings to TX settings (Y/n)?\n");
c = tolower(console_getchar());
if (c == 'n') {
err = parse_cis_args(&iso_rx_qos);
if (err != 0) {
return err;
}
printk("New settings: rtn=%u, phy=%u, sdu=%u\n",
iso_rx_qos.rtn, iso_rx_qos.phy,
iso_rx_qos.sdu);
} else {
(void)memcpy(&iso_rx_qos, &iso_tx_qos,
sizeof(iso_rx_qos));
}
}
}
return 0;
}
static int central_create_connection(void)
{
int err;
advertiser_found = false;
err = start_scan();
if (err != 0) {
LOG_ERR("Could not start scan: %d", err);
return err;
}
LOG_INF("Waiting for advertiser");
err = k_sem_take(&sem_adv, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_adv: %d", err);
return err;
}
LOG_INF("Stopping scan");
err = stop_scan();
if (err != 0) {
LOG_ERR("Could not stop scan: %d", err);
return err;
}
LOG_INF("Connecting");
err = bt_conn_le_create(&adv_addr, BT_CONN_LE_CREATE_CONN,
BT_LE_CONN_PARAM_DEFAULT, &default_conn);
if (err != 0) {
LOG_ERR("Create connection failed: %d", err);
return err;
}
err = k_sem_take(&sem_connected, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_connected: %d", err);
return err;
}
return 0;
}
static int central_create_cig(void)
{
struct bt_iso_connect_param connect_param[CONFIG_BT_ISO_MAX_CHAN];
int err;
iso_conn_start_time = 0;
LOG_INF("Creating CIG");
err = bt_iso_cig_create(&cig_create_param, &cig);
if (err != 0) {
LOG_ERR("Failed to create CIG: %d", err);
return err;
}
LOG_INF("Connecting ISO channels");
for (int i = 0; i < cig_create_param.num_cis; i++) {
connect_param[i].acl = default_conn;
connect_param[i].iso_chan = &iso_chans[i].chan;
}
err = bt_iso_chan_connect(connect_param, cig_create_param.num_cis);
if (err != 0) {
LOG_ERR("Failed to connect iso: %d", err);
return err;
}
total_iso_conn_count++;
for (int i = 0; i < cig_create_param.num_cis; i++) {
err = k_sem_take(&sem_iso_connected, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_iso_connected: %d", err);
return err;
}
}
return 0;
}
static void reset_sems(void)
{
(void)k_sem_reset(&sem_adv);
(void)k_sem_reset(&sem_iso_accept);
(void)k_sem_reset(&sem_iso_connected);
(void)k_sem_reset(&sem_iso_disconnected);
(void)k_sem_reset(&sem_connected);
(void)k_sem_reset(&sem_disconnected);
}
static int cleanup(void)
{
int err;
for (size_t i = 0; i < cig_create_param.num_cis; i++) {
(void)k_work_cancel_delayable(&iso_chans[i].send_work);
}
err = k_sem_take(&sem_disconnected, K_NO_WAIT);
if (err != 0) {
for (int i = 0; i < cig_create_param.num_cis; i++) {
err = k_sem_take(&sem_iso_disconnected, K_NO_WAIT);
if (err == 0) {
err = bt_iso_chan_disconnect(&iso_chans[i].chan);
if (err != 0) {
LOG_ERR("Could not disconnect ISO[%d]: %d",
i, err);
break;
}
} /* else ISO already disconnected */
}
err = bt_conn_disconnect(default_conn,
BT_HCI_ERR_REMOTE_USER_TERM_CONN);
if (err != 0) {
LOG_ERR("Could not disconnect ACL: %d", err);
return err;
}
err = k_sem_take(&sem_disconnected, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_disconnected: %d", err);
return err;
}
} /* else ACL already disconnected */
if (cig) {
err = bt_iso_cig_terminate(cig);
if (err != 0) {
LOG_ERR("Could not terminate CIG: %d", err);
return err;
}
cig = NULL;
}
return err;
}
static int run_central(void)
{
int err;
char c;
iso_conn_start_time = 0;
last_received_counter = 0;
memset(&stats_current_conn, 0, sizeof(stats_current_conn));
reset_sems();
printk("Change ISO settings (y/N)?\n");
c = tolower(console_getchar());
if (c == 'y') {
err = change_central_settings();
if (err != 0) {
LOG_ERR("Failed to set parameters: %d", err);
return err;
}
}
err = central_create_connection();
if (err != 0) {
LOG_ERR("Failed to create connection: %d", err);
return err;
}
err = central_create_cig();
if (err != 0) {
LOG_ERR("Failed to create CIG or connect CISes: %d", err);
return err;
}
for (size_t i = 0; i < cig_create_param.num_cis; i++) {
struct k_work_delayable *work = &iso_chans[i].send_work;
k_work_init_delayable(work, iso_timer_timeout);
for (int j = 0; j < BUFFERS_ENQUEUED; j++) {
iso_send(&iso_chans[i].chan);
}
}
err = k_sem_take(&sem_disconnected, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_disconnected: %d", err);
return err;
}
LOG_INF("Disconnected - Cleaning up");
for (size_t i = 0; i < cig_create_param.num_cis; i++) {
(void)k_work_cancel_delayable(&iso_chans[i].send_work);
}
for (int i = 0; i < cig_create_param.num_cis; i++) {
err = k_sem_take(&sem_iso_disconnected, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_iso_disconnected: %d", err);
return err;
}
}
err = bt_iso_cig_terminate(cig);
if (err != 0) {
LOG_ERR("Could not terminate CIG: %d", err);
return err;
}
cig = NULL;
return 0;
}
static int run_peripheral(void)
{
int err;
static bool initialized;
/* Reset */
cig_create_param.num_cis = 0;
iso_conn_start_time = 0;
last_received_counter = 0;
memset(&stats_current_conn, 0, sizeof(stats_current_conn));
reset_sems();
if (!initialized) {
LOG_INF("Registering ISO server");
err = bt_iso_server_register(&iso_server);
if (err != 0) {
LOG_ERR("ISO server register failed: %d", err);
return err;
}
initialized = true;
}
LOG_INF("Starting advertising");
err = bt_le_adv_start(BT_LE_ADV_CONN_NAME, NULL, 0, NULL, 0);
if (err != 0) {
LOG_ERR("Advertising failed to start: %d", err);
return err;
}
LOG_INF("Waiting for ACL connection");
err = k_sem_take(&sem_connected, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_connected: %d", err);
return err;
}
err = bt_le_adv_stop();
if (err != 0) {
LOG_ERR("Advertising failed to stop: %d", err);
return err;
}
LOG_INF("Waiting for ISO connection");
err = k_sem_take(&sem_iso_accept, K_SECONDS(2));
if (err != 0) {
return err;
}
for (int i = 0; i < cig_create_param.num_cis; i++) {
err = k_sem_take(&sem_iso_connected, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_iso_connected: %d", err);
return err;
}
}
total_iso_conn_count++;
for (size_t i = 0; i < cig_create_param.num_cis; i++) {
struct k_work_delayable *work = &iso_chans[i].send_work;
k_work_init_delayable(work, iso_timer_timeout);
for (int j = 0; j < BUFFERS_ENQUEUED; j++) {
iso_send(&iso_chans[i].chan);
}
}
/* Wait for disconnect */
err = k_sem_take(&sem_disconnected, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_disconnected: %d", err);
return err;
}
for (int i = 0; i < cig_create_param.num_cis; i++) {
err = k_sem_take(&sem_iso_disconnected, K_FOREVER);
if (err != 0) {
LOG_ERR("failed to take sem_iso_disconnected: %d", err);
return err;
}
}
LOG_INF("Disconnected - Cleaning up");
for (size_t i = 0; i < cig_create_param.num_cis; i++) {
(void)k_work_cancel_delayable(&iso_chans[i].send_work);
}
return 0;
}
void main(void)
{
int err;
LOG_INF("Starting Bluetooth Throughput example");
err = bt_enable(NULL);
if (err != 0) {
LOG_ERR("Bluetooth init failed: %d", err);
return;
}
bt_conn_cb_register(&conn_callbacks);
bt_le_scan_cb_register(&scan_callbacks);
err = console_init();
if (err != 0) {
LOG_ERR("Console init failed: %d", err);
return;
}
LOG_INF("Bluetooth initialized");
for (int i = 0; i < ARRAY_SIZE(iso_chans); i++) {
iso_chans[i].chan.ops = &iso_ops;
iso_chans[i].chan.qos = &iso_qos;
cis[i] = &iso_chans[i].chan;
}
/* Init data */
for (int i = 0; i < iso_tx_qos.sdu; i++) {
if (i < sizeof(iso_send_count)) {
continue;
}
iso_data[i] = (uint8_t)i;
}
while (true) {
int cleanup_err;
role = device_role_select();
if (role == ROLE_CENTRAL) {
err = run_central();
} else if (role == ROLE_PERIPHERAL) {
err = run_peripheral();
} else {
if (role != ROLE_QUIT) {
LOG_INF("Invalid role %u", role);
continue;
} else {
err = 0;
break;
}
}
if (err != 0) {
cleanup_err = cleanup();
if (cleanup_err != 0) {
LOG_ERR("Could not clean up: %d", err);
}
}
LOG_INF("Test complete: %d", err);
}
LOG_INF("Exiting");
}