@@ -18,6 +18,9 @@
#include <poll.h>
#include <stdbool.h>
+#include <linux/errqueue.h>
+#include <linux/net_tstamp.h>
+
#include <glib.h>
#include "lib/bluetooth.h"
@@ -34,6 +37,8 @@
#include "src/shared/util.h"
#include "src/shared/queue.h"
+#include "tester-utils.h"
+
#define QOS_IO(_interval, _latency, _sdu, _phy, _rtn) \
{ \
.interval = _interval, \
@@ -462,11 +467,12 @@ struct test_data {
uint16_t handle;
uint16_t acl_handle;
struct queue *io_queue;
- unsigned int io_id[2];
+ unsigned int io_id[3];
uint8_t client_num;
int step;
bool reconnect;
bool suspending;
+ struct tx_tstamp_data tx_ts;
};
struct iso_client_data {
@@ -487,6 +493,10 @@ struct iso_client_data {
size_t base_len;
bool listen_bind;
bool pa_bind;
+ uint32_t so_timestamping;
+ bool msg_timestamping;
+ unsigned int send_extra;
+ unsigned int send_extra_pre_ts;
};
typedef bool (*iso_defer_accept_t)(struct test_data *data, GIOChannel *io);
@@ -677,15 +687,14 @@ static void io_free(void *data)
static void test_data_free(void *test_data)
{
struct test_data *data = test_data;
+ unsigned int i;
if (data->io_queue)
queue_destroy(data->io_queue, io_free);
- if (data->io_id[0] > 0)
- g_source_remove(data->io_id[0]);
-
- if (data->io_id[1] > 0)
- g_source_remove(data->io_id[1]);
+ for (i = 0; i < ARRAY_SIZE(data->io_id); ++i)
+ if (data->io_id[i] > 0)
+ g_source_remove(data->io_id[i]);
free(data);
}
@@ -987,6 +996,38 @@ static const struct iso_client_data connect_16_2_1_send = {
.send = &send_16_2_1,
};
+static const struct iso_client_data connect_send_tx_timestamping = {
+ .qos = QOS_16_2_1,
+ .expect_err = 0,
+ .send = &send_16_2_1,
+ .so_timestamping = (SOF_TIMESTAMPING_SOFTWARE |
+ SOF_TIMESTAMPING_OPT_ID |
+ SOF_TIMESTAMPING_TX_SOFTWARE),
+ .send_extra = 1,
+ .send_extra_pre_ts = 2,
+};
+
+static const struct iso_client_data connect_send_tx_sched_timestamping = {
+ .qos = QOS_16_2_1,
+ .expect_err = 0,
+ .send = &send_16_2_1,
+ .so_timestamping = (SOF_TIMESTAMPING_SOFTWARE |
+ SOF_TIMESTAMPING_TX_SOFTWARE |
+ SOF_TIMESTAMPING_OPT_TSONLY |
+ SOF_TIMESTAMPING_TX_SCHED),
+ .send_extra = 1,
+};
+
+static const struct iso_client_data connect_send_tx_msg_timestamping = {
+ .qos = QOS_16_2_1,
+ .expect_err = 0,
+ .send = &send_16_2_1,
+ .so_timestamping = (SOF_TIMESTAMPING_SOFTWARE |
+ SOF_TIMESTAMPING_TX_SOFTWARE),
+ .send_extra = 1,
+ .msg_timestamping = true,
+};
+
static const struct iso_client_data listen_16_2_1_recv = {
.qos = QOS_16_2_1,
.expect_err = 0,
@@ -1410,14 +1451,17 @@ static void bthost_recv_data(const void *buf, uint16_t len, void *user_data)
struct test_data *data = user_data;
const struct iso_client_data *isodata = data->test_data;
+ --data->step;
+
tester_print("Client received %u bytes of data", len);
if (isodata->send && (isodata->send->iov_len != len ||
memcmp(isodata->send->iov_base, buf, len))) {
if (!isodata->recv->iov_base)
tester_test_failed();
- } else
+ } else if (!data->step) {
tester_test_passed();
+ }
}
static void bthost_iso_disconnected(void *user_data)
@@ -2058,17 +2102,95 @@ static void iso_recv(struct test_data *data, GIOChannel *io)
data->io_id[0] = g_io_add_watch(io, G_IO_IN, iso_recv_data, data);
}
-static void iso_send(struct test_data *data, GIOChannel *io)
+static gboolean iso_recv_errqueue(GIOChannel *io, GIOCondition cond,
+ gpointer user_data)
{
+ struct test_data *data = user_data;
const struct iso_client_data *isodata = data->test_data;
- ssize_t ret;
+ int sk = g_io_channel_unix_get_fd(io);
+ int err;
+
+ data->step--;
+
+ err = tx_tstamp_recv(&data->tx_ts, sk, isodata->send->iov_len);
+ if (err > 0)
+ return TRUE;
+ else if (!err && !data->step)
+ tester_test_passed();
+ else
+ tester_test_failed();
+
+ data->io_id[2] = 0;
+ return FALSE;
+}
+
+static void iso_tx_timestamping(struct test_data *data, GIOChannel *io)
+{
+ const struct iso_client_data *isodata = data->test_data;
+ struct so_timestamping so = {
+ .flags = isodata->so_timestamping,
+ };
int sk;
+ int err;
+ unsigned int count;
+
+ if (!(isodata->so_timestamping & SOF_TIMESTAMPING_TX_RECORD_MASK))
+ return;
+
+ tester_print("Enabling TX timestamping");
+
+ tx_tstamp_init(&data->tx_ts, isodata->so_timestamping);
+
+ for (count = 0; count < isodata->send_extra + 1; ++count)
+ data->step += tx_tstamp_expect(&data->tx_ts);
sk = g_io_channel_unix_get_fd(io);
+ data->io_id[2] = g_io_add_watch(io, G_IO_ERR, iso_recv_errqueue, data);
+
+ if (isodata->msg_timestamping)
+ so.flags &= ~SOF_TIMESTAMPING_TX_RECORD_MASK;
+
+ err = setsockopt(sk, SOL_SOCKET, SO_TIMESTAMPING, &so, sizeof(so));
+ if (err < 0) {
+ tester_warn("setsockopt SO_TIMESTAMPING: %s (%d)",
+ strerror(errno), errno);
+ tester_test_failed();
+ return;
+ }
+}
+
+static void iso_send_data(struct test_data *data, GIOChannel *io)
+{
+ const struct iso_client_data *isodata = data->test_data;
+ char control[CMSG_SPACE(sizeof(uint32_t))];
+ struct msghdr msg = {
+ .msg_iov = (struct iovec *)isodata->send,
+ .msg_iovlen = 1,
+ };
+ struct cmsghdr *cmsg;
+ ssize_t ret;
+ int sk;
+
tester_print("Writing %zu bytes of data", isodata->send->iov_len);
- ret = writev(sk, isodata->send, 1);
+ sk = g_io_channel_unix_get_fd(io);
+
+ if (isodata->msg_timestamping) {
+ memset(control, 0, sizeof(control));
+ msg.msg_control = control;
+ msg.msg_controllen = sizeof(control);
+
+ cmsg = CMSG_FIRSTHDR(&msg);
+ cmsg->cmsg_level = SOL_SOCKET;
+ cmsg->cmsg_type = SO_TIMESTAMPING;
+ cmsg->cmsg_len = CMSG_LEN(sizeof(uint32_t));
+
+ *((uint32_t *)CMSG_DATA(cmsg)) = (isodata->so_timestamping &
+ SOF_TIMESTAMPING_TX_RECORD_MASK);
+ }
+
+ ret = sendmsg(sk, &msg, 0);
if (ret < 0 || isodata->send->iov_len != (size_t) ret) {
tester_warn("Failed to write %zu bytes: %s (%d)",
isodata->send->iov_len, strerror(errno), errno);
@@ -2076,6 +2198,22 @@ static void iso_send(struct test_data *data, GIOChannel *io)
return;
}
+ data->step++;
+}
+
+static void iso_send(struct test_data *data, GIOChannel *io)
+{
+ const struct iso_client_data *isodata = data->test_data;
+ unsigned int count;
+
+ for (count = 0; count < isodata->send_extra_pre_ts; ++count)
+ iso_send_data(data, io);
+
+ iso_tx_timestamping(data, io);
+
+ for (count = 0; count < isodata->send_extra + 1; ++count)
+ iso_send_data(data, io);
+
if (isodata->bcast) {
tester_test_passed();
return;
@@ -3197,6 +3335,17 @@ int main(int argc, char *argv[])
test_iso("ISO Send - Success", &connect_16_2_1_send, setup_powered,
test_connect);
+ test_iso("ISO Send - TX Timestamping", &connect_send_tx_timestamping,
+ setup_powered, test_connect);
+
+ test_iso("ISO Send - TX Sched Timestamping",
+ &connect_send_tx_sched_timestamping, setup_powered,
+ test_connect);
+
+ test_iso("ISO Send - TX Msg Timestamping",
+ &connect_send_tx_msg_timestamping, setup_powered,
+ test_connect);
+
test_iso("ISO Receive - Success", &listen_16_2_1_recv, setup_powered,
test_listen);
new file mode 100644
@@ -0,0 +1,163 @@
+// SPDX-License-Identifier: LGPL-2.1-or-later
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2022 Intel Corporation.
+ *
+ */
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <time.h>
+#include <sys/socket.h>
+#include <linux/errqueue.h>
+#include <linux/net_tstamp.h>
+
+#include <glib.h>
+
+#define SEC_NSEC(_t) ((_t) * 1000000000LL)
+#define TS_NSEC(_ts) (SEC_NSEC((_ts)->tv_sec) + (_ts)->tv_nsec)
+
+struct tx_tstamp_data {
+ struct {
+ uint32_t id;
+ uint32_t type;
+ } expect[16];
+ unsigned int pos;
+ unsigned int count;
+ unsigned int sent;
+ uint32_t so_timestamping;
+};
+
+static inline void tx_tstamp_init(struct tx_tstamp_data *data,
+ uint32_t so_timestamping)
+{
+ memset(data, 0, sizeof(*data));
+ memset(data->expect, 0xff, sizeof(data->expect));
+
+ data->so_timestamping = so_timestamping;
+}
+
+static inline int tx_tstamp_expect(struct tx_tstamp_data *data)
+{
+ unsigned int pos = data->count;
+ int steps;
+
+ if (data->so_timestamping & SOF_TIMESTAMPING_TX_SCHED) {
+ g_assert(pos < ARRAY_SIZE(data->expect));
+ data->expect[pos].type = SCM_TSTAMP_SCHED;
+ data->expect[pos].id = data->sent;
+ pos++;
+ }
+
+ if (data->so_timestamping & SOF_TIMESTAMPING_TX_SOFTWARE) {
+ g_assert(pos < ARRAY_SIZE(data->expect));
+ data->expect[pos].type = SCM_TSTAMP_SND;
+ data->expect[pos].id = data->sent;
+ pos++;
+ }
+
+ data->sent++;
+
+ steps = pos - data->count;
+ data->count = pos;
+ return steps;
+}
+
+static inline int tx_tstamp_recv(struct tx_tstamp_data *data, int sk, int len)
+{
+ unsigned char control[512];
+ ssize_t ret;
+ char buf[1024];
+ struct msghdr msg;
+ struct iovec iov;
+ struct cmsghdr *cmsg;
+ struct scm_timestamping *tss = NULL;
+ struct sock_extended_err *serr = NULL;
+ struct timespec now;
+
+ iov.iov_base = buf;
+ iov.iov_len = sizeof(buf);
+
+ memset(&msg, 0, sizeof(msg));
+ msg.msg_iov = &iov;
+ msg.msg_iovlen = 1;
+ msg.msg_control = control;
+ msg.msg_controllen = sizeof(control);
+
+ ret = recvmsg(sk, &msg, MSG_ERRQUEUE);
+ if (ret < 0) {
+ tester_warn("Failed to read from errqueue: %s (%d)",
+ strerror(errno), errno);
+ return -EINVAL;
+ }
+
+ if (data->so_timestamping & SOF_TIMESTAMPING_OPT_TSONLY) {
+ if (ret != 0) {
+ tester_warn("Packet copied back to errqueue");
+ return -EINVAL;
+ }
+ } else if (len > ret) {
+ tester_warn("Packet not copied back to errqueue: %zd", ret);
+ return -EINVAL;
+ }
+
+ for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
+ cmsg = CMSG_NXTHDR(&msg, cmsg)) {
+ if (cmsg->cmsg_level == SOL_SOCKET &&
+ cmsg->cmsg_type == SCM_TIMESTAMPING) {
+ tss = (void *)CMSG_DATA(cmsg);
+ } else if (cmsg->cmsg_level == SOL_BLUETOOTH &&
+ cmsg->cmsg_type == BT_SCM_ERROR) {
+ serr = (void *)CMSG_DATA(cmsg);
+ }
+ }
+
+ if (!tss) {
+ tester_warn("SCM_TIMESTAMPING not found");
+ return -EINVAL;
+ }
+
+ if (!serr) {
+ tester_warn("BT_SCM_ERROR not found");
+ return -EINVAL;
+ }
+
+ if (serr->ee_errno != ENOMSG ||
+ serr->ee_origin != SO_EE_ORIGIN_TIMESTAMPING) {
+ tester_warn("BT_SCM_ERROR wrong for timestamping");
+ return -EINVAL;
+ }
+
+ clock_gettime(CLOCK_REALTIME, &now);
+
+ if (TS_NSEC(&now) < TS_NSEC(tss->ts) ||
+ TS_NSEC(&now) > TS_NSEC(tss->ts) + SEC_NSEC(10)) {
+ tester_warn("nonsense in timestamp");
+ return -EINVAL;
+ }
+
+ if (data->pos >= data->count) {
+ tester_warn("Too many timestamps");
+ return -EINVAL;
+ }
+
+ if ((data->so_timestamping & SOF_TIMESTAMPING_OPT_ID) &&
+ serr->ee_data != data->expect[data->pos].id) {
+ tester_warn("Bad timestamp id %u", serr->ee_data);
+ return -EINVAL;
+ }
+
+ if (serr->ee_info != data->expect[data->pos].type) {
+ tester_warn("Bad timestamp type %u", serr->ee_info);
+ return -EINVAL;
+ }
+
+ tester_print("Got valid TX timestamp %u", data->pos);
+
+ ++data->pos;
+
+ return data->count - data->pos;
+}