blob: 19a18712508bd88c51493c3d82fa5376ef77700a [file] [log] [blame]
/* main_l2cap_ecred.c - Application main entry point */
/*
* Copyright (c) 2022 Nordic Semiconductor
*
* SPDX-License-Identifier: Apache-2.0
*/
#include "common.h"
#define LOG_MODULE_NAME main_l2cap_ecred
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(LOG_MODULE_NAME, LOG_LEVEL_DBG);
extern enum bst_result_t bst_result;
static struct bt_conn *default_conn;
static const struct bt_data ad[] = {
BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
};
#define DATA_MTU CONFIG_BT_L2CAP_TX_MTU
#define DATA_MPS 65
#define DATA_BUF_SIZE BT_L2CAP_SDU_BUF_SIZE(DATA_MTU)
#define L2CAP_CHANNELS 2
#define SERVERS 1
#define SDU_SEND_COUNT 200
#define ECRED_CHAN_MAX 5
#define LONG_MSG (DATA_MTU - 500)
#define SHORT_MSG (DATA_MPS - 2)
#define LONG_MSG_CHAN_IDX 0
#define SHORT_MSG_CHAN_IDX 1
NET_BUF_POOL_FIXED_DEFINE(rx_data_pool, L2CAP_CHANNELS, BT_L2CAP_BUF_SIZE(DATA_BUF_SIZE), 8, NULL);
NET_BUF_POOL_FIXED_DEFINE(tx_data_pool_0, 1, BT_L2CAP_BUF_SIZE(DATA_MTU), 8, NULL);
NET_BUF_POOL_FIXED_DEFINE(tx_data_pool_1, 1, BT_L2CAP_BUF_SIZE(DATA_MTU), 8, NULL);
static struct bt_l2cap_server servers[SERVERS];
void send_sdu_chan_worker(struct k_work *item);
struct channel {
uint8_t chan_id; /* Internal number that identifies L2CAP channel. */
struct bt_l2cap_le_chan le;
bool in_use;
size_t sdus_received;
size_t bytes_to_send;
uint8_t iteration;
struct net_buf *buf;
struct k_work work;
struct k_work_q work_queue;
uint8_t payload[DATA_MTU];
};
static struct channel channels[L2CAP_CHANNELS];
CREATE_FLAG(is_connected);
CREATE_FLAG(unsequenced_data);
#define T_STACK_SIZE 512
#define T_PRIORITY 5
static K_THREAD_STACK_ARRAY_DEFINE(stack_area, L2CAP_CHANNELS, T_STACK_SIZE);
static K_SEM_DEFINE(chan_conn_sem, 0, L2CAP_CHANNELS);
static K_SEM_DEFINE(all_chan_conn_sem, 0, 1);
static K_SEM_DEFINE(all_chan_disconn_sem, 0, 1);
static K_SEM_DEFINE(sent_sem, 0, L2CAP_CHANNELS);
static void init_workqs(void)
{
for (int i = 0; i < L2CAP_CHANNELS; i++) {
k_work_queue_init(&channels[i].work_queue);
k_work_queue_start(&channels[i].work_queue, stack_area[i],
K_THREAD_STACK_SIZEOF(*stack_area), T_PRIORITY, NULL);
}
}
static struct net_buf *chan_alloc_buf_cb(struct bt_l2cap_chan *chan)
{
LOG_DBG("Allocated on chan %p", chan);
return net_buf_alloc(&rx_data_pool, K_FOREVER);
}
static int chan_recv_cb(struct bt_l2cap_chan *l2cap_chan, struct net_buf *buf)
{
struct channel *chan = CONTAINER_OF(l2cap_chan, struct channel, le);
const uint32_t received_iterration = net_buf_pull_le32(buf);
LOG_DBG("received_iterration %i sdus_received %i, chan_id: %d, data_length: %d",
received_iterration, chan->sdus_received, chan->chan_id, buf->len);
if (!TEST_FLAG(unsequenced_data) && received_iterration != chan->sdus_received) {
FAIL("Received out of sequence data.");
}
const int retval = memcmp(buf->data + sizeof(received_iterration),
chan->payload + sizeof(received_iterration),
buf->len - sizeof(received_iterration));
if (retval) {
FAIL("Payload received didn't match expected value memcmp returned %i", retval);
}
/*By the time we rx on long msg channel we should have already rx on short msg channel*/
if (chan->chan_id == 0) {
if (channels[SHORT_MSG_CHAN_IDX].sdus_received !=
(channels[LONG_MSG_CHAN_IDX].sdus_received + 1)) {
FAIL("Didn't receive on short msg channel first");
}
}
chan->sdus_received++;
return 0;
}
static void chan_sent_cb(struct bt_l2cap_chan *l2cap_chan)
{
struct channel *chan = CONTAINER_OF(l2cap_chan, struct channel, le);
chan->buf = 0;
k_sem_give(&sent_sem);
LOG_DBG("chan_id: %d", chan->chan_id);
}
static void chan_connected_cb(struct bt_l2cap_chan *l2cap_chan)
{
struct channel *chan = CONTAINER_OF(l2cap_chan, struct channel, le);
LOG_DBG("chan_id: %d", chan->chan_id);
LOG_DBG("tx.mtu %d, tx.mps: %d, rx.mtu: %d, rx.mps %d", sys_cpu_to_le16(chan->le.tx.mtu),
sys_cpu_to_le16(chan->le.tx.mps), sys_cpu_to_le16(chan->le.rx.mtu),
sys_cpu_to_le16(chan->le.rx.mps));
k_sem_give(&chan_conn_sem);
if (k_sem_count_get(&chan_conn_sem) == L2CAP_CHANNELS) {
k_sem_give(&all_chan_conn_sem);
k_sem_reset(&all_chan_disconn_sem);
}
}
static void chan_disconnected_cb(struct bt_l2cap_chan *l2cap_chan)
{
struct channel *chan = CONTAINER_OF(l2cap_chan, struct channel, le);
LOG_DBG("chan_id: %d", chan->chan_id);
chan->in_use = false;
k_sem_take(&chan_conn_sem, K_FOREVER);
if (k_sem_count_get(&chan_conn_sem) == 0) {
k_sem_give(&all_chan_disconn_sem);
k_sem_reset(&all_chan_conn_sem);
}
}
static void chan_status_cb(struct bt_l2cap_chan *l2cap_chan, atomic_t *status)
{
struct channel *chan = CONTAINER_OF(l2cap_chan, struct channel, le);
LOG_DBG("chan_id: %d, status: %ld", chan->chan_id, *status);
}
static void chan_released_cb(struct bt_l2cap_chan *l2cap_chan)
{
struct channel *chan = CONTAINER_OF(l2cap_chan, struct channel, le);
LOG_DBG("chan_id: %d", chan->chan_id);
}
static void chan_reconfigured_cb(struct bt_l2cap_chan *l2cap_chan)
{
struct channel *chan = CONTAINER_OF(l2cap_chan, struct channel, le);
LOG_DBG("chan_id: %d", chan->chan_id);
}
static const struct bt_l2cap_chan_ops l2cap_ops = {
.alloc_buf = chan_alloc_buf_cb,
.recv = chan_recv_cb,
.sent = chan_sent_cb,
.connected = chan_connected_cb,
.disconnected = chan_disconnected_cb,
.status = chan_status_cb,
.released = chan_released_cb,
.reconfigured = chan_reconfigured_cb,
};
static struct channel *get_free_channel(void)
{
for (int idx = 0; idx < L2CAP_CHANNELS; idx++) {
struct channel *chan = &channels[idx];
if (chan->in_use) {
continue;
}
chan->chan_id = idx;
channels[idx].in_use = true;
(void)memset(chan->payload, idx, sizeof(chan->payload));
k_work_init(&chan->work, send_sdu_chan_worker);
chan->le.chan.ops = &l2cap_ops;
chan->le.rx.mtu = DATA_MTU;
chan->le.rx.mps = DATA_MPS;
return chan;
}
return NULL;
}
static void connect_num_channels(uint8_t num_l2cap_channels)
{
struct bt_l2cap_chan *allocated_channels[ECRED_CHAN_MAX] = { NULL };
for (int i = 0; i < num_l2cap_channels; i++) {
struct channel *chan = get_free_channel();
if (!chan) {
FAIL("failed, chan not free");
return;
}
allocated_channels[i] = &chan->le.chan;
}
const int err = bt_l2cap_ecred_chan_connect(default_conn, allocated_channels,
servers[0].psm);
if (err) {
FAIL("can't connect ecred %d ", err);
}
}
static void disconnect_all_channels(void)
{
for (int i = 0; i < ARRAY_SIZE(channels); i++) {
if (channels[i].in_use) {
LOG_DBG("Disconnecting channel: %d)", channels[i].chan_id);
const int err = bt_l2cap_chan_disconnect(&channels[i].le.chan);
if (err) {
LOG_DBG("can't disconnect channel (err: %d)", err);
}
channels[i].in_use = false;
}
}
}
static int accept(struct bt_conn *conn, struct bt_l2cap_chan **l2cap_chan)
{
struct channel *chan;
chan = get_free_channel();
if (!chan) {
return -ENOMEM;
}
*l2cap_chan = &chan->le.chan;
return 0;
}
static struct bt_l2cap_server *get_free_server(void)
{
for (int i = 0; i < SERVERS; i++) {
if (servers[i].psm) {
continue;
}
return &servers[i];
}
return NULL;
}
static void register_l2cap_server(void)
{
struct bt_l2cap_server *server;
server = get_free_server();
if (!server) {
FAIL("Failed to get free server");
return;
}
server->accept = accept;
server->psm = 0;
if (bt_l2cap_server_register(server) < 0) {
FAIL("Failed to get free server");
return;
}
LOG_DBG("L2CAP server registered, PSM:0x%X", server->psm);
}
static void connected(struct bt_conn *conn, uint8_t conn_err)
{
char addr[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
if (conn_err) {
FAIL("Failed to connect to %s (%u)", addr, conn_err);
bt_conn_unref(default_conn);
default_conn = NULL;
return;
}
default_conn = bt_conn_ref(conn);
LOG_DBG("%s", addr);
SET_FLAG(is_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_DBG("%s (reason 0x%02x)", addr, reason);
if (default_conn != conn) {
FAIL("Conn mismatch disconnect %s %s)", default_conn, conn);
return;
}
bt_conn_unref(default_conn);
default_conn = NULL;
UNSET_FLAG(is_connected);
}
BT_CONN_CB_DEFINE(conn_callbacks) = {
.connected = connected,
.disconnected = disconnected,
};
static void send_sdu(int iteration, int chan_idx, int bytes)
{
struct bt_l2cap_chan *chan = &channels[chan_idx].le.chan;
struct net_buf *buf;
/* First 4 bytes in sent payload is iteration count */
sys_put_le32(iteration, channels[chan_idx].payload);
if (channels[chan_idx].buf != 0) {
FAIL("Buf should have been deallocated by now");
return;
}
if (chan_idx == 0) {
buf = net_buf_alloc(&tx_data_pool_0, K_NO_WAIT);
} else {
buf = net_buf_alloc(&tx_data_pool_1, K_NO_WAIT);
}
if (buf == NULL) {
FAIL("Failed to get buff on ch %i, iteration %i should never happen", chan_idx,
chan_idx);
}
channels[chan_idx].buf = buf;
net_buf_reserve(buf, BT_L2CAP_CHAN_SEND_RESERVE);
net_buf_add_mem(buf, channels[chan_idx].payload, bytes);
LOG_DBG("bt_l2cap_chan_sending ch: %i bytes: %i iteration: %i", chan_idx, bytes, iteration);
const int ret = bt_l2cap_chan_send(chan, buf);
LOG_DBG("bt_l2cap_chan_send returned: %i", ret);
if (ret < 0) {
FAIL("Error: send failed error: %i", ret);
channels[chan_idx].buf = 0;
net_buf_unref(buf);
}
}
void send_sdu_chan_worker(struct k_work *item)
{
const struct channel *ch = CONTAINER_OF(item, struct channel, work);
send_sdu(ch->iteration, ch->chan_id, ch->bytes_to_send);
}
static void send_sdu_concurrently(void)
{
for (int i = 0; i < SDU_SEND_COUNT; i++) {
for (int k = 0; k < L2CAP_CHANNELS; k++) {
channels[k].iteration = i;
/* Assign the right msg to the right channel */
channels[k].bytes_to_send = (k == LONG_MSG_CHAN_IDX) ? LONG_MSG : SHORT_MSG;
const int err = k_work_submit_to_queue(&channels[k].work_queue,
&channels[k].work);
if (err < 0) {
FAIL("Failed to submit work to the queue, error: %d", err);
}
}
/* Wait until messages on all of the channels has been sent */
for (int l = 0; l < L2CAP_CHANNELS; l++) {
k_sem_take(&sent_sem, K_FOREVER);
}
}
}
static int change_mtu_on_channels(int num_channels, int new_mtu)
{
struct bt_l2cap_chan *reconf_channels[ECRED_CHAN_MAX] = { NULL };
for (int i = 0; i < num_channels; i++) {
reconf_channels[i] = &(&channels[i])->le.chan;
}
return bt_l2cap_ecred_chan_reconfigure(reconf_channels, new_mtu);
}
static void test_peripheral_main(void)
{
device_sync_init(PERIPHERAL_ID);
LOG_DBG("*L2CAP ECRED Peripheral started*");
init_workqs();
int err;
err = bt_enable(NULL);
if (err) {
FAIL("Can't enable Bluetooth (err %d)", err);
return;
}
LOG_DBG("Peripheral Bluetooth initialized.");
LOG_DBG("Connectable advertising...");
err = bt_le_adv_start(BT_LE_ADV_CONN_NAME, ad, ARRAY_SIZE(ad), NULL, 0);
if (err) {
FAIL("Advertising failed to start (err %d)", err);
return;
}
LOG_DBG("Advertising started.");
LOG_DBG("Peripheral waiting for connection...");
WAIT_FOR_FLAG_SET(is_connected);
LOG_DBG("Peripheral Connected.");
register_l2cap_server();
connect_num_channels(L2CAP_CHANNELS);
k_sem_take(&all_chan_conn_sem, K_FOREVER);
/* Disconnect and reconnect channels *****************************************************/
LOG_DBG("############# Disconnect and reconnect channels");
disconnect_all_channels();
k_sem_take(&all_chan_disconn_sem, K_FOREVER);
connect_num_channels(L2CAP_CHANNELS);
k_sem_take(&all_chan_conn_sem, K_FOREVER);
LOG_DBG("Send sync after reconnection");
device_sync_send();
/* Send bytes on both channels and expect ch 1 to receive all of them before ch 0 *********/
LOG_DBG("############# Send bytes on both channels concurrently");
send_sdu_concurrently();
/* Change mtu size on all connected channels *********************************************/
LOG_DBG("############# Change MTU of the channels");
err = change_mtu_on_channels(L2CAP_CHANNELS, CONFIG_BT_L2CAP_TX_MTU + 10);
if (err) {
FAIL("MTU change failed (err %d)\n", err);
}
/* Read from both devices (Central and Peripheral) at the same time **********************/
LOG_DBG("############# Read from both devices (Central and Peripheral) at the same time");
LOG_DBG("Wait for sync before sending the msg");
device_sync_wait();
LOG_DBG("Received sync");
send_sdu(0, 1, 10);
k_sem_take(&sent_sem, K_FOREVER);
disconnect_all_channels();
WAIT_FOR_FLAG_UNSET(is_connected);
PASS("L2CAP ECRED Peripheral tests Passed");
bs_trace_silent_exit(0);
}
static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
struct net_buf_simple *ad)
{
struct bt_le_conn_param *param;
int err;
err = bt_le_scan_stop();
if (err) {
FAIL("Stop LE scan failed (err %d)", err);
return;
}
param = BT_LE_CONN_PARAM_DEFAULT;
err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, param, &default_conn);
if (err) {
FAIL("Create conn failed (err %d)", err);
return;
}
}
static void test_central_main(void)
{
struct bt_le_scan_param scan_param = {
.type = BT_LE_SCAN_TYPE_ACTIVE,
.options = BT_LE_SCAN_OPT_NONE,
.interval = BT_GAP_SCAN_FAST_INTERVAL,
.window = BT_GAP_SCAN_FAST_WINDOW,
};
device_sync_init(CENTRAL_ID);
LOG_DBG("*L2CAP ECRED Central started*");
int err;
err = bt_enable(NULL);
if (err) {
FAIL("Can't enable Bluetooth (err %d)\n", err);
return;
}
LOG_DBG("Central Bluetooth initialized.\n");
err = bt_le_scan_start(&scan_param, device_found);
if (err) {
FAIL("Scanning failed to start (err %d)\n", err);
return;
}
LOG_DBG("Scanning successfully started\n");
LOG_DBG("Central waiting for connection...\n");
WAIT_FOR_FLAG_SET(is_connected);
LOG_DBG("Central Connected.\n");
register_l2cap_server();
LOG_DBG("Wait for sync after reconnection");
device_sync_wait();
LOG_DBG("Received sync");
/* Read from both devices (Central and Peripheral) at the same time **********************/
LOG_DBG("############# Read from both devices (Central and Peripheral) at the same time");
LOG_DBG("Send sync for SDU send");
SET_FLAG(unsequenced_data);
device_sync_send();
send_sdu(0, 1, 10);
/* Wait until all of the channels are disconnected */
k_sem_take(&all_chan_disconn_sem, K_FOREVER);
LOG_DBG("Both l2cap channels disconnected, test over\n");
UNSET_FLAG(unsequenced_data);
LOG_DBG("received PDUs on long msg channel %i and short msg channel %i",
channels[LONG_MSG_CHAN_IDX].sdus_received,
channels[SHORT_MSG_CHAN_IDX].sdus_received);
if (channels[LONG_MSG_CHAN_IDX].sdus_received < SDU_SEND_COUNT ||
channels[SHORT_MSG_CHAN_IDX].sdus_received < SDU_SEND_COUNT) {
FAIL("received less than %i", SDU_SEND_COUNT);
}
/* Disconnect */
LOG_DBG("Central Disconnecting....");
err = bt_conn_disconnect(default_conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
bt_conn_unref(default_conn);
LOG_DBG("Central tried to disconnect");
if (err) {
FAIL("Disconnection failed (err %d)", err);
return;
}
LOG_DBG("Central Disconnected.");
PASS("L2CAP ECRED Central tests Passed\n");
}
static const struct bst_test_instance test_def[] = {
{
.test_id = "peripheral",
.test_descr = "Peripheral L2CAP ECRED",
.test_post_init_f = test_init,
.test_tick_f = test_tick,
.test_main_f = test_peripheral_main
},
{
.test_id = "central",
.test_descr = "Central L2CAP ECRED",
.test_post_init_f = test_init,
.test_tick_f = test_tick,
.test_main_f = test_central_main
},
BSTEST_END_MARKER
};
struct bst_test_list *test_main_l2cap_ecred_install(struct bst_test_list *tests)
{
return bst_add_tests(tests, test_def);
}