| /* 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); |
| } |