[RFC PATCH BlueZ 0/4] iso-tester: add tests for set & receive ISO HW timestamps

[RFC PATCH BlueZ 0/4] iso-tester: add tests for set & receive ISO HW timestamps

[Pauli Virtanen]

Add tests that utilize sending BT_SCM_PKT_ISO_TS / BT_SCM_PKT_SEQNUM to
kernel, and receiving HW timestamps from HCI LE Read ISO TX Sync.

RFC mainly because BT_SCM_PKT_SEQNUM. In cases where userspace decides
to not send packets on all intervals, this might be reasonable to allow
userspace to set, but not fully though out.

Adds tests:

ISO Send - HW Timestamping
ISO Send - SWHW Timestamping
ISO Send - Set HW Timestamp

Pauli Virtanen (4):
  bthost: fix receiving timestamped ISO packets
  btdev: implement LE Read ISO TX Sync command
  iso-tester: add tests for ISO HW timestamps
  iso-tester: Add test for sending ISO packets with set timestamps

 emulator/btdev.c     | 162 +++++++++++++++++++++++++++++++++++++++----
 emulator/bthost.c    |  83 ++++++++++++++--------
 emulator/bthost.h    |   2 +-
 tools/iso-tester.c   | 103 ++++++++++++++++++++++++---
 tools/l2cap-tester.c |   2 +-
 tools/sco-tester.c   |   2 +-
 tools/tester.h       |  70 ++++++++++++++++---
 7 files changed, 359 insertions(+), 65 deletions(-)

-- 
2.53.0

[RFC PATCH BlueZ 1/4] bthost: fix receiving timestamped ISO packets

[Pauli Virtanen]

Parse timestamped ISO packets correctly. Pass sequence number and
timestamp to the hook callback.
---
 emulator/bthost.c  | 83 ++++++++++++++++++++++++++++++----------------
 emulator/bthost.h  |  2 +-
 tools/iso-tester.c |  3 +-
 3 files changed, 58 insertions(+), 30 deletions(-)

diff --git a/emulator/bthost.c b/emulator/bthost.c
index b913c8015..7056dd8d1 100644
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -47,6 +47,8 @@
 #define iso_flags_pb(f)		(f & 0x0003)
 #define iso_flags_ts(f)		((f >> 2) & 0x0001)
 #define iso_flags_pack(pb, ts)	(((pb) & 0x03) | (((ts) & 0x01) << 2))
+#define iso_data_len(h)		((h) & 0x3fff)
+#define iso_data_flags(h)	((h) >> 14)
 #define iso_data_len_pack(h, f)	((uint16_t) ((h) | ((f) << 14)))
 
 #define L2CAP_FEAT_FIXED_CHAN	0x00000080
@@ -173,6 +175,11 @@ struct btconn {
 	uint16_t recv_len;
 	uint16_t data_len;
 	void *recv_data;
+	struct {
+		uint16_t sn;
+		bool ts;
+		uint32_t tstamp;
+	} recv_iso;
 };
 
 enum l2cap_mode {
@@ -3356,60 +3363,67 @@ static void process_sco(struct bthost *bthost, const void *data, uint16_t len)
 }
 
 static void process_iso_data(struct bthost *bthost, struct btconn *conn,
-					const void *data, uint16_t len)
+					bool ts, uint16_t sn, uint32_t tstamp,
+					const struct iovec *iov)
 {
-	const struct bt_hci_iso_data_start *data_hdr = data;
-	uint16_t data_len;
 	struct iso_hook *hook;
 
-	data_len = le16_to_cpu(data_hdr->slen);
-	if (len != sizeof(*data_hdr) + data_len) {
-		bthost_debug(bthost, "ISO invalid length: %u != %zu",
-					len, sizeof(*data_hdr) + data_len);
-		return;
-	}
-
-	bthost_debug(bthost, "ISO data: %u bytes (%u)", data_len, data_hdr->sn);
+	bthost_debug(bthost, "ISO data: %zu bytes ts %u sn %u tstamp %u",
+						iov->iov_len, ts, sn, tstamp);
 
 	hook = conn->iso_hook;
 	if (!hook)
 		return;
 
-	hook->func(data_hdr->data, data_len, hook->user_data);
+	hook->func(iov->iov_base, iov->iov_len, ts, sn, tstamp,
+							hook->user_data);
 }
 
 static void append_iso_data(struct bthost *bthost, struct btconn *conn,
-				uint8_t flags, const void *data, uint16_t len)
+				uint8_t flags, const struct iovec *iov)
 {
-	append_recv_data(bthost, conn, "ISO", flags, data, len);
+	struct iovec data;
+
+	append_recv_data(bthost, conn, "ISO", flags, iov->iov_base,
+								iov->iov_len);
 
 	if (conn->recv_len < conn->data_len) {
-		if (flags == 0x03) {
+		if (iso_flags_pb(flags) == 0x03) {
 			bthost_debug(bthost, "Unexpected ISO end frame");
 			free_recv_data(conn);
 		}
 		return;
 	}
 
-	process_iso_data(bthost, conn, conn->recv_data, conn->recv_len);
+	data.iov_base = conn->recv_data;
+	data.iov_len = conn->recv_len;
+	process_iso_data(bthost, conn, conn->recv_iso.ts, conn->recv_iso.sn,
+						conn->recv_iso.tstamp, &data);
 
 	free_recv_data(conn);
 }
 
 static void process_iso(struct bthost *bthost, const void *data, uint16_t len)
 {
-	const struct bt_hci_iso_hdr *iso_hdr = data;
+	struct iovec in = { .iov_base = (void *)data, .iov_len = len };
+	const struct bt_hci_iso_hdr *iso_hdr;
 	const struct bt_hci_iso_data_start *data_hdr;
 	uint16_t handle, iso_len, data_len;
-	uint8_t flags;
+	uint8_t flags, pb, ts;
 	struct btconn *conn;
 
+	iso_hdr = util_iov_pull_mem(&in, sizeof(*iso_hdr));
+	if (!iso_hdr)
+		return;
+
 	iso_len = le16_to_cpu(iso_hdr->dlen);
 	if (len != sizeof(*iso_hdr) + iso_len)
 		return;
 
 	handle = acl_handle(iso_hdr->handle);
-	flags = iso_flags_pb(acl_flags(iso_hdr->handle));
+	flags = acl_flags(iso_hdr->handle);
+	pb = iso_flags_pb(flags);
+	ts = iso_flags_ts(flags);
 
 	conn = bthost_find_conn(bthost, handle);
 	if (!conn) {
@@ -3417,9 +3431,7 @@ static void process_iso(struct bthost *bthost, const void *data, uint16_t len)
 		return;
 	}
 
-	data_hdr = (void *) data + sizeof(*iso_hdr);
-
-	switch (flags) {
+	switch (pb) {
 	case 0x00:
 	case 0x02:
 		if (conn->recv_data) {
@@ -3427,13 +3439,28 @@ static void process_iso(struct bthost *bthost, const void *data, uint16_t len)
 			free_recv_data(conn);
 		}
 
-		data_len = le16_to_cpu(data_hdr->slen) + sizeof(*data_hdr);
+		conn->recv_iso.ts = ts;
+		conn->recv_iso.tstamp = 0;
+		if (ts) {
+			if (!util_iov_pull_le32(&in, &conn->recv_iso.tstamp)) {
+				bthost_debug(bthost, "Invalid ISO ts");
+				return;
+			}
+		}
 
-		bthost_debug(bthost, "iso_len %u data_len %u", iso_len,
-								data_len);
+		data_hdr = util_iov_pull_mem(&in, sizeof(*data_hdr));
+		if (!data_hdr) {
+			bthost_debug(bthost, "Invalid ISO frame header");
+			return;
+		}
 
-		if (iso_len == data_len) {
-			process_iso_data(bthost, conn, iso_hdr->data, iso_len);
+		conn->recv_iso.sn = le16_to_cpu(data_hdr->sn);
+		data_len = iso_data_len(le16_to_cpu(data_hdr->slen));
+
+		if (in.iov_len == data_len) {
+			process_iso_data(bthost, conn, conn->recv_iso.ts,
+						conn->recv_iso.sn,
+						conn->recv_iso.tstamp, &in);
 			break;
 		}
 
@@ -3441,7 +3468,7 @@ static void process_iso(struct bthost *bthost, const void *data, uint16_t len)
 		/* fall through */
 	case 0x01:
 	case 0x03:
-		append_iso_data(bthost, conn, flags, iso_hdr->data, iso_len);
+		append_iso_data(bthost, conn, flags, &in);
 		break;
 	default:
 		bthost_debug(bthost, "Invalid ISO frame flags 0x%2.2x", flags);
diff --git a/emulator/bthost.h b/emulator/bthost.h
index c88ea0329..1f8b32d64 100644
--- a/emulator/bthost.h
+++ b/emulator/bthost.h
@@ -86,7 +86,7 @@ void bthost_add_sco_hook(struct bthost *bthost, uint16_t handle,
 				bthost_destroy_func_t destroy);
 
 typedef void (*bthost_iso_hook_func_t)(const void *data, uint16_t len,
-							void *user_data);
+		bool ts, uint16_t sn, uint32_t timestamp, void *user_data);
 
 void bthost_add_iso_hook(struct bthost *bthost, uint16_t handle,
 				bthost_iso_hook_func_t func, void *user_data,
diff --git a/tools/iso-tester.c b/tools/iso-tester.c
index b851d2cd8..2ddb29a00 100644
--- a/tools/iso-tester.c
+++ b/tools/iso-tester.c
@@ -1669,7 +1669,8 @@ static void client_connectable_complete(uint16_t opcode, uint8_t status,
 	}
 }
 
-static void bthost_recv_data(const void *buf, uint16_t len, void *user_data)
+static void bthost_recv_data(const void *buf, uint16_t len, bool ts,
+			uint16_t sn, uint32_t timestamp, void *user_data)
 {
 	struct test_data *data = user_data;
 	const struct iso_client_data *isodata = data->test_data;
-- 
2.53.0

[RFC PATCH BlueZ 2/4] btdev: implement LE Read ISO TX Sync command

[Pauli Virtanen]

Parse incoming ISO packets and implement LE Read ISO TX Sync.

The timestamps and sequence numbers are just some values; the emulator
does not have real ISOAL or clock, and we don't attempt to add them
here.  The last transmitted numbers are tracked in struct iso_data
attached to each ISO connection.
---
 emulator/btdev.c | 162 ++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 147 insertions(+), 15 deletions(-)

diff --git a/emulator/btdev.c b/emulator/btdev.c
index d3a9c6735..c5b78a4e9 100644
--- a/emulator/btdev.c
+++ b/emulator/btdev.c
@@ -52,10 +52,14 @@
 #define has_bredr(btdev)	(!((btdev)->features[4] & 0x20))
 #define has_le(btdev)		(!!((btdev)->features[4] & 0x40))
 
+#define iso_flags_pb(f)		(f & 0x0003)
+#define iso_flags_ts(f)		((f >> 2) & 0x0001)
+
 #define ACL_HANDLE BIT(0)
 #define SCO_HANDLE BIT(8)
 #define CIS_HANDLE SCO_HANDLE
 #define BIS_HANDLE BIT(9)
+
 #define SYNC_HANDLE 1
 #define INV_HANDLE 0xffff
 
@@ -84,6 +88,14 @@ struct btdev_conn {
 	void *data;
 };
 
+struct iso_data {
+	uint16_t tx_seq;
+	uint32_t tx_timestamp;
+	bool tx_active;
+	uint32_t sdu_interval;
+	struct bt_hci_bis bis;
+};
+
 struct btdev_al {
 	uint8_t type;
 	bdaddr_t addr;
@@ -1165,7 +1177,7 @@ static struct btdev_conn *conn_new(struct btdev *dev, uint16_t handle,
 }
 
 static struct btdev_conn *conn_link(struct btdev *dev, struct btdev *remote,
-					uint16_t handle, uint8_t type)
+		uint16_t handle, uint8_t type, struct btdev_conn **remote_conn)
 {
 	struct btdev_conn *conn1, *conn2;
 
@@ -1187,6 +1199,9 @@ static struct btdev_conn *conn_link(struct btdev *dev, struct btdev *remote,
 	util_debug(dev->debug_callback, dev->debug_data,
 				"conn2 %p handle 0x%04x", conn2, conn2->handle);
 
+	if (remote_conn)
+		*remote_conn = conn2;
+
 	return conn1;
 }
 
@@ -1200,7 +1215,7 @@ static struct btdev_conn *conn_add(struct btdev *dev,
 	if (!remote)
 		return NULL;
 
-	return conn_link(dev, remote, handle, type);
+	return conn_link(dev, remote, handle, type, NULL);
 }
 
 static struct btdev_conn *conn_add_acl(struct btdev *dev,
@@ -1211,24 +1226,40 @@ static struct btdev_conn *conn_add_acl(struct btdev *dev,
 
 static struct btdev_conn *conn_add_sco(struct btdev_conn *acl)
 {
-	return conn_link(acl->dev, acl->link->dev, SCO_HANDLE, HCI_SCODATA_PKT);
+	return conn_link(acl->dev, acl->link->dev, SCO_HANDLE, HCI_SCODATA_PKT,
+									NULL);
 }
 
 static struct btdev_conn *conn_add_cis(struct btdev_conn *acl, uint16_t handle)
 {
-	return conn_link(acl->dev, acl->link->dev, handle, HCI_ISODATA_PKT);
+	struct btdev_conn *conn, *remote;
+
+	conn = conn_link(acl->dev, acl->link->dev, handle, HCI_ISODATA_PKT,
+								&remote);
+	if (!conn)
+		return conn;
+
+	conn->data = new0(struct iso_data, 1);
+	remote->data = new0(struct iso_data, 1);
+
+	return conn;
 }
 
 static struct btdev_conn *conn_add_bis(struct btdev *dev, uint16_t handle,
 						const struct bt_hci_bis *bis)
 {
 	struct btdev_conn *conn;
+	struct iso_data *iso_data;
 
 	conn = conn_new(dev, handle, HCI_ISODATA_PKT);
 	if (!conn)
 		return conn;
 
-	conn->data = util_memdup(bis, sizeof(*bis));
+	iso_data = new0(struct iso_data, 1);
+	iso_data->bis = *bis;
+	iso_data->sdu_interval = get_le24(bis->sdu_interval);
+
+	conn->data = iso_data;
 
 	return conn;
 }
@@ -1261,12 +1292,14 @@ static struct btdev_conn *conn_link_bis(struct btdev *dev, struct btdev *remote,
 {
 	struct btdev_conn *conn;
 	struct btdev_conn *bis;
+	struct iso_data *iso_data;
 
 	bis = find_bis_index(remote, index);
 	if (!bis)
 		return NULL;
 
-	conn = conn_add_bis(dev, BIS_HANDLE, bis->data);
+	iso_data = bis->data;
+	conn = conn_add_bis(dev, BIS_HANDLE, &iso_data->bis);
 	if (!conn)
 		return NULL;
 
@@ -5801,12 +5834,14 @@ static void send_biginfo(struct btdev *dev, const struct btdev *remote,
 	struct bt_hci_evt_le_big_info_adv_report ev;
 	const struct btdev_conn *conn;
 	struct bt_hci_bis *bis;
+	struct iso_data *iso_data;
 
 	conn = find_bis_index(remote, 0);
 	if (!conn)
 		return;
 
-	bis = conn->data;
+	iso_data = conn->data;
+	bis = &iso_data->bis;
 
 	memset(&ev, 0, sizeof(ev));
 	ev.sync_handle = cpu_to_le16(sync_handle);
@@ -6772,8 +6807,43 @@ static int cmd_read_size_v2(struct btdev *dev, const void *data,
 static int cmd_read_iso_tx_sync(struct btdev *dev, const void *data,
 							uint8_t len)
 {
-	/* TODO */
-	return -ENOTSUP;
+	const struct bt_hci_cmd_le_read_iso_tx_sync *cmd = data;
+	struct bt_hci_rsp_le_read_iso_tx_sync rsp;
+	struct btdev_conn *conn;
+	struct iso_data *iso_data;
+
+	memset(&rsp, 0, sizeof(rsp));
+
+	rsp.handle = cmd->handle;
+
+	conn = queue_find(dev->conns, match_handle,
+					UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (!conn || conn->type != HCI_ISODATA_PKT) {
+		rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
+		cmd_complete(dev, BT_HCI_CMD_LE_READ_ISO_TX_SYNC, &rsp,
+								sizeof(rsp));
+		return 0;
+	}
+
+	iso_data = conn->data;
+
+	if (!iso_data->tx_active) {
+		rsp.status = BT_HCI_ERR_COMMAND_DISALLOWED;
+		cmd_complete(dev, BT_HCI_CMD_LE_READ_ISO_TX_SYNC, &rsp,
+								sizeof(rsp));
+		return 0;
+	}
+
+	/* TODO: COMMAND_DISALLOWED if not configured for tx */
+
+	/* TODO: time offset for framed packets */
+	rsp.status = BT_HCI_ERR_SUCCESS;
+	rsp.seq = cpu_to_le16(iso_data->tx_seq);
+	rsp.timestamp = cpu_to_le32(iso_data->tx_timestamp);
+
+	cmd_complete(dev, BT_HCI_CMD_LE_READ_ISO_TX_SYNC, &rsp, sizeof(rsp));
+
+	return 0;
 }
 
 static int find_cig(struct btdev *dev, uint8_t cig_id)
@@ -6977,6 +7047,7 @@ static void le_cis_estabilished(struct btdev *dev, struct btdev_conn *conn,
 {
 	struct bt_hci_evt_le_cis_established evt;
 	int cig_idx, cis_idx = -1;
+	struct iso_data *iso_data = conn->data;
 
 	memset(&evt, 0, sizeof(evt));
 
@@ -7017,6 +7088,8 @@ static void le_cis_estabilished(struct btdev *dev, struct btdev_conn *conn,
 				le_cig->params.c_interval), evt.c_latency);
 		put_le24(le_cis_latecy(evt.p_ft, evt.interval,
 				le_cig->params.p_interval), evt.p_latency);
+
+		iso_data->sdu_interval = get_le24(le_cig->params.c_interval);
 	}
 
 	le_meta_event(dev, BT_HCI_EVT_LE_CIS_ESTABLISHED, &evt, sizeof(evt));
@@ -7038,6 +7111,7 @@ static int cmd_create_cis_complete(struct btdev *dev, const void *data,
 		struct btdev_conn *iso;
 		struct bt_hci_evt_le_cis_req evt;
 		struct le_cig *le_cig;
+		struct iso_data *iso_data;
 		int cig_idx, cis_idx;
 
 		cig_idx = parse_cis_handle(le16_to_cpu(cis->cis_handle),
@@ -7073,6 +7147,9 @@ static int cmd_create_cis_complete(struct btdev *dev, const void *data,
 		evt.cig_id = le_cig->params.cig_id;
 		evt.cis_id = le_cig->cis[cis_idx].cis_id;
 
+		iso_data = iso->data;
+		iso_data->sdu_interval = get_le24(le_cig->params.c_interval);
+
 		le_cig->activated = true;
 
 		le_meta_event(iso->link->dev, BT_HCI_EVT_LE_CIS_REQ, &evt,
@@ -7417,6 +7494,7 @@ static int cmd_big_create_sync_complete(struct btdev *dev, const void *data,
 	} pdu;
 	struct btdev *remote;
 	struct btdev_conn *conn = NULL;
+	struct iso_data *iso_data;
 	struct bt_hci_bis *bis;
 	int i;
 	uint16_t sync_handle = le16_to_cpu(cmd->sync_handle);
@@ -7468,7 +7546,8 @@ static int cmd_big_create_sync_complete(struct btdev *dev, const void *data,
 		return 0;
 	}
 
-	bis = conn->data;
+	iso_data = conn->data;
+	bis = &iso_data->bis;
 
 	if (bis->encryption != cmd->encryption) {
 		pdu.ev.status = BT_HCI_ERR_ENC_MODE_NOT_ACCEPTABLE;
@@ -8611,18 +8690,38 @@ static void send_sco(struct btdev *dev, const void *data, uint16_t len)
 
 static void send_iso(struct btdev *dev, const void *data, uint16_t len)
 {
-	struct bt_hci_acl_hdr *hdr;
 	struct iovec iov[2];
-	struct btdev_conn *conn;
 	uint8_t pkt_type = BT_H4_ISO_PKT;
 
 	/* Packet type */
 	iov[0].iov_base = &pkt_type;
 	iov[0].iov_len = sizeof(pkt_type);
 
-	iov[1].iov_base = hdr = (void *) (data);
+	iov[1].iov_base = (void *) data;
 	iov[1].iov_len = len;
 
+	send_packet(dev, iov, 2);
+}
+
+static void process_iso(struct btdev *dev, const void *data, uint16_t len)
+{
+	struct iovec in;
+	struct btdev_conn *conn;
+	struct bt_hci_iso_hdr *hdr;
+	struct iso_data *tx_iso_data;
+	uint8_t pb;
+	bool ts;
+
+	in.iov_base = (void *) data;
+	in.iov_len = len;
+
+	hdr = util_iov_pull_mem(&in, sizeof(*hdr));
+	if (!hdr)
+		return;
+
+	pb = iso_flags_pb(acl_flags(hdr->handle));
+	ts = iso_flags_ts(acl_flags(hdr->handle));
+
 	conn = queue_find(dev->conns, match_handle,
 					UINT_TO_PTR(acl_handle(hdr->handle)));
 	if (!conn)
@@ -8630,8 +8729,41 @@ static void send_iso(struct btdev *dev, const void *data, uint16_t len)
 
 	num_completed_packets(dev, conn);
 
+	tx_iso_data = conn->data;
+
+	if (!(pb & 0x1)) {
+		struct bt_hci_iso_data_start *h;
+		uint32_t timestamp = 0;
+
+		if (ts) {
+			void *t;
+
+			t = util_iov_pull_mem(&in, sizeof(uint32_t));
+			if (!t)
+				return;
+
+			timestamp = get_le32(t);
+		}
+
+		h = util_iov_pull_mem(&in, sizeof(*h));
+		if (!h)
+			return;
+
+		/* TODO: use proper numbers here */
+		if (ts)
+			tx_iso_data->tx_timestamp = timestamp;
+
+		tx_iso_data->tx_active = true;
+		tx_iso_data->tx_seq++;
+		tx_iso_data->tx_timestamp += tx_iso_data->sdu_interval;
+
+		util_debug(dev->debug_callback, dev->debug_data,
+				"ISO tx %u %u",
+				tx_iso_data->tx_seq, tx_iso_data->tx_timestamp);
+	}
+
 	if (conn->link)
-		send_packet(conn->link->dev, iov, 2);
+		send_iso(conn->link->dev, data, len);
 }
 
 void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
@@ -8660,7 +8792,7 @@ void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
 		send_sco(btdev, data + 1, len - 1);
 		break;
 	case BT_H4_ISO_PKT:
-		send_iso(btdev, data + 1, len - 1);
+		process_iso(btdev, data + 1, len - 1);
 		break;
 	default:
 		util_debug(btdev->debug_callback, btdev->debug_data,
-- 
2.53.0

[RFC PATCH BlueZ 3/4] iso-tester: add tests for ISO HW timestamps

[Pauli Virtanen]

Add tests that check LE Read ISO TX Sync handling in kernel

ISO Send - HW Timestamping
ISO Send - SWHW Timestamping
---
 tools/iso-tester.c   | 36 +++++++++++++++++++++++++-
 tools/l2cap-tester.c |  2 +-
 tools/sco-tester.c   |  2 +-
 tools/tester.h       | 60 +++++++++++++++++++++++++++++++++++++-------
 4 files changed, 88 insertions(+), 12 deletions(-)

diff --git a/tools/iso-tester.c b/tools/iso-tester.c
index 2ddb29a00..fb04ab642 100644
--- a/tools/iso-tester.c
+++ b/tools/iso-tester.c
@@ -1051,6 +1051,31 @@ static const struct iso_client_data connect_send_tx_cmsg_timestamping = {
 	.cmsg_timestamping = true,
 };
 
+static const struct iso_client_data connect_send_hw_timestamping = {
+	.qos = QOS_16_2_1,
+	.expect_err = 0,
+	.send = &send_16_2_1,
+	.so_timestamping = (SOF_TIMESTAMPING_RAW_HARDWARE |
+					SOF_TIMESTAMPING_OPT_ID |
+					SOF_TIMESTAMPING_TX_HARDWARE),
+	.repeat_send = 1,
+	.repeat_send_pre_ts = 2,
+};
+
+static const struct iso_client_data connect_send_swhw_timestamping = {
+	.qos = QOS_16_2_1,
+	.expect_err = 0,
+	.send = &send_16_2_1,
+	.so_timestamping = (SOF_TIMESTAMPING_RAW_HARDWARE |
+					SOF_TIMESTAMPING_SOFTWARE |
+					SOF_TIMESTAMPING_OPT_ID |
+					SOF_TIMESTAMPING_TX_HARDWARE |
+					SOF_TIMESTAMPING_TX_SOFTWARE |
+					SOF_TIMESTAMPING_OPT_TX_SWHW),
+	.repeat_send = 1,
+	.repeat_send_pre_ts = 2,
+};
+
 static const struct iso_client_data listen_16_2_1_recv = {
 	.qos = QOS_16_2_1,
 	.expect_err = 0,
@@ -2604,13 +2629,15 @@ static void iso_tx_timestamping(struct test_data *data, GIOChannel *io)
 	int sk;
 	int err;
 	unsigned int count;
+	int64_t interval = 10000000ULL;
 
 	if (!(isodata->so_timestamping & TS_TX_RECORD_MASK))
 		return;
 
 	tester_print("Enabling TX timestamping");
 
-	tx_tstamp_init(&data->tx_ts, isodata->so_timestamping, false);
+	tx_tstamp_init(&data->tx_ts, isodata->so_timestamping, false,
+			(isodata->repeat_send_pre_ts + 1)*interval, interval);
 
 	for (count = 0; count < isodata->repeat_send + 1; ++count)
 		data->step += tx_tstamp_expect(&data->tx_ts, 0);
@@ -4092,6 +4119,13 @@ int main(int argc, char *argv[])
 			&connect_send_tx_cmsg_timestamping, setup_powered,
 			test_connect);
 
+	test_iso("ISO Send - HW Timestamping", &connect_send_hw_timestamping,
+						setup_powered, test_connect);
+
+	test_iso("ISO Send - SWHW Timestamping",
+			&connect_send_swhw_timestamping, setup_powered,
+			test_connect);
+
 	test_iso("ISO Receive - Success", &listen_16_2_1_recv, setup_powered,
 							test_listen);
 
diff --git a/tools/l2cap-tester.c b/tools/l2cap-tester.c
index c3aa96f84..5fd1f058f 100644
--- a/tools/l2cap-tester.c
+++ b/tools/l2cap-tester.c
@@ -1810,7 +1810,7 @@ static void l2cap_tx_timestamping(struct test_data *data, GIOChannel *io)
 	tester_print("Enabling TX timestamping");
 
 	tx_tstamp_init(&data->tx_ts, l2data->so_timestamping,
-					l2data->sock_type == SOCK_STREAM);
+					l2data->sock_type == SOCK_STREAM, 0, 0);
 
 	for (count = 0; count < l2data->repeat_send + 1; ++count)
 		data->step += tx_tstamp_expect(&data->tx_ts, l2data->data_len);
diff --git a/tools/sco-tester.c b/tools/sco-tester.c
index 3d6b59092..a4483cfdc 100644
--- a/tools/sco-tester.c
+++ b/tools/sco-tester.c
@@ -882,7 +882,7 @@ static void sco_tx_timestamping(struct test_data *data, GIOChannel *io)
 
 	tester_print("Enabling TX timestamping");
 
-	tx_tstamp_init(&data->tx_ts, scodata->so_timestamping, false);
+	tx_tstamp_init(&data->tx_ts, scodata->so_timestamping, false, 0, 0);
 
 	for (count = 0; count < scodata->repeat_send + 1; ++count)
 		data->step += tx_tstamp_expect(&data->tx_ts, 0);
diff --git a/tools/tester.h b/tools/tester.h
index 9df600f90..cb28e9cea 100644
--- a/tools/tester.h
+++ b/tools/tester.h
@@ -38,50 +38,73 @@ struct tx_tstamp_data {
 	struct {
 		uint32_t id;
 		uint32_t type;
+		int64_t value;
+		bool sw;
 	} expect[16];
 	unsigned int pos;
 	unsigned int count;
 	unsigned int sent;
 	uint32_t so_timestamping;
 	bool stream;
+	int64_t hw_base;
+	int64_t hw_step;
 };
 
 static inline void tx_tstamp_init(struct tx_tstamp_data *data,
-				uint32_t so_timestamping, bool stream)
+			uint32_t so_timestamping, bool stream,
+			int64_t hw_base, int64_t hw_step)
 {
 	memset(data, 0, sizeof(*data));
 	memset(data->expect, 0xff, sizeof(data->expect));
 
 	data->so_timestamping = so_timestamping;
 	data->stream = stream;
+	data->hw_base = hw_base;
+	data->hw_step = hw_step;
 }
 
 static inline int tx_tstamp_expect(struct tx_tstamp_data *data, size_t len)
 {
+	uint32_t ts = data->so_timestamping;
 	unsigned int pos = data->count;
 	int steps;
 
 	if (data->stream && len)
 		data->sent += len - 1;
 
-	if (data->so_timestamping & SOF_TIMESTAMPING_TX_SCHED) {
+	if (ts & SOF_TIMESTAMPING_TX_SCHED) {
 		g_assert(pos < ARRAY_SIZE(data->expect));
 		data->expect[pos].type = SCM_TSTAMP_SCHED;
 		data->expect[pos].id = data->sent;
+		data->expect[pos].sw = true;
 		pos++;
 	}
 
-	if (data->so_timestamping & SOF_TIMESTAMPING_TX_SOFTWARE) {
+	if ((ts & SOF_TIMESTAMPING_TX_SOFTWARE) &&
+					(!(ts & SOF_TIMESTAMPING_TX_HARDWARE) ||
+					(ts & SOF_TIMESTAMPING_OPT_TX_SWHW))) {
 		g_assert(pos < ARRAY_SIZE(data->expect));
 		data->expect[pos].type = SCM_TSTAMP_SND;
 		data->expect[pos].id = data->sent;
+		data->expect[pos].sw = true;
 		pos++;
 	}
 
-	if (data->so_timestamping & SOF_TIMESTAMPING_TX_COMPLETION) {
+	if (ts & SOF_TIMESTAMPING_TX_COMPLETION) {
 		g_assert(pos < ARRAY_SIZE(data->expect));
 		data->expect[pos].type = SCM_TSTAMP_COMPLETION;
 		data->expect[pos].id = data->sent;
+		data->expect[pos].sw = true;
+		pos++;
+	}
+
+	if (ts & SOF_TIMESTAMPING_TX_HARDWARE) {
+		g_assert(pos < ARRAY_SIZE(data->expect));
+		data->expect[pos].type = SCM_TSTAMP_SND;
+		data->expect[pos].value = data->hw_base +
+			data->sent * data->hw_step;
+		data->expect[pos].id = data->sent;
+		data->expect[pos].sw = false;
 		pos++;
 	}
 
@@ -105,6 +128,7 @@ static inline int tx_tstamp_recv(struct tx_tstamp_data *data, int sk, int len)
 	struct sock_extended_err *serr = NULL;
 	struct timespec now;
 	unsigned int i;
+	bool sw = true;
 
 	iov.iov_base = buf;
 	iov.iov_len = sizeof(buf);
@@ -156,6 +180,9 @@ static inline int tx_tstamp_recv(struct tx_tstamp_data *data, int sk, int len)
 		return -EINVAL;
 	}
 
+	if (!TS_NSEC(tss->ts))
+		sw = false;
+
 	if (serr->ee_errno != ENOMSG ||
 				serr->ee_origin != SO_EE_ORIGIN_TIMESTAMPING) {
 		tester_warn("BT_SCM_ERROR wrong for timestamping");
@@ -164,8 +191,9 @@ static inline int tx_tstamp_recv(struct tx_tstamp_data *data, int sk, int len)
 
 	clock_gettime(CLOCK_REALTIME, &now);
 
-	if (TS_NSEC(&now) < TS_NSEC(tss->ts) ||
-			TS_NSEC(&now) > TS_NSEC(tss->ts) + SEC_NSEC(10)) {
+	/* SW timestamp */
+	if (sw && (TS_NSEC(&now) < TS_NSEC(tss->ts) ||
+			TS_NSEC(&now) > TS_NSEC(tss->ts) + SEC_NSEC(10))) {
 		tester_warn("nonsense in timestamp");
 		return -EINVAL;
 	}
@@ -180,6 +208,9 @@ static inline int tx_tstamp_recv(struct tx_tstamp_data *data, int sk, int len)
 		if (data->expect[i].type >= 0xffff)
 			continue;
 
+		if (sw != data->expect[i].sw)
+			continue;
+
 		if (serr->ee_info == data->expect[i].type) {
 			data->expect[i].type = 0xffff;
 			break;
@@ -190,14 +221,22 @@ static inline int tx_tstamp_recv(struct tx_tstamp_data *data, int sk, int len)
 		return -EINVAL;
 	}
 
+	/* Note: +1 is kernel-provided offset since can't report 0 tstamp */
+	if (!sw && TS_NSEC(&tss->ts[2]) != data->expect[i].value + 1) {
+		tester_print("Bad HW timestamp %u != %u",
+					(unsigned int)TS_NSEC(&tss->ts[2]),
+					(unsigned int)data->expect[i].value);
+		return -EINVAL;
+	}
+
 	if ((data->so_timestamping & SOF_TIMESTAMPING_OPT_ID) &&
-				serr->ee_data != data->expect[i].id) {
+					serr->ee_data != data->expect[i].id) {
 		tester_warn("Bad timestamp id %u", serr->ee_data);
 		return -EINVAL;
 	}
 
-	tester_print("Got valid TX timestamp %u (type %u, id %u)", i,
-						serr->ee_info, serr->ee_data);
+	tester_print("Got valid TX timestamp %u (type %u, %s, id %u)", i,
+				serr->ee_info, sw ? "SW" : "HW", serr->ee_data);
 
 	++data->pos;
 
@@ -318,6 +357,9 @@ static inline void test_ethtool_get_ts_info(unsigned int index, int proto,
 		SOF_TIMESTAMPING_TX_COMPLETION;
 	int sk;
 
+	if (proto == BTPROTO_ISO)
+		so_timestamping |= SOF_TIMESTAMPING_TX_HARDWARE;
+
 	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, proto);
 	if (sk < 0) {
 		if (sk == -EPROTONOSUPPORT)
-- 
2.53.0

[RFC PATCH BlueZ 4/4] iso-tester: Add test for sending ISO packets with set timestamps

[Pauli Virtanen]

Add test

ISO Send - Set HW Timestamp
---
 tools/iso-tester.c | 68 +++++++++++++++++++++++++++++++++++++++-------
 tools/tester.h     | 10 +++++++
 2 files changed, 68 insertions(+), 10 deletions(-)

diff --git a/tools/iso-tester.c b/tools/iso-tester.c
index fb04ab642..2d4408d56 100644
--- a/tools/iso-tester.c
+++ b/tools/iso-tester.c
@@ -522,6 +522,9 @@ struct iso_client_data {
 	 * Used for testing TX timestamping OPT_ID.
 	 */
 	unsigned int repeat_send;
+
+	/* Set HW timestamp on sent packets */
+	bool set_hw_tstamp;
 };
 
 typedef bool (*iso_defer_accept_t)(struct test_data *data, GIOChannel *io,
@@ -1076,6 +1079,16 @@ static const struct iso_client_data connect_send_swhw_timestamping = {
 	.repeat_send_pre_ts = 2,
 };
 
+static const struct iso_client_data connect_send_set_hw_timestamp = {
+	.qos = QOS_16_2_1,
+	.expect_err = 0,
+	.send = &send_16_2_1,
+	.so_timestamping = (SOF_TIMESTAMPING_RAW_HARDWARE |
+					SOF_TIMESTAMPING_OPT_ID |
+					SOF_TIMESTAMPING_TX_HARDWARE),
+	.set_hw_tstamp = true,
+};
+
 static const struct iso_client_data listen_16_2_1_recv = {
 	.qos = QOS_16_2_1,
 	.expect_err = 0,
@@ -1710,6 +1723,9 @@ static void bthost_recv_data(const void *buf, uint16_t len, bool ts,
 			tester_test_failed();
 	} else if (!data->step)
 		tester_test_passed();
+
+	if (isodata->set_hw_tstamp && (timestamp != 0x1234 || sn != 1234))
+		tester_test_failed();
 }
 
 static void bthost_iso_disconnected(void *user_data)
@@ -2629,15 +2645,24 @@ static void iso_tx_timestamping(struct test_data *data, GIOChannel *io)
 	int sk;
 	int err;
 	unsigned int count;
-	int64_t interval = 10000000ULL;
+	int64_t interval = 10000000ULL, hw_base;
 
 	if (!(isodata->so_timestamping & TS_TX_RECORD_MASK))
 		return;
 
 	tester_print("Enabling TX timestamping");
 
+	/* Note: these depend on emulator/btdev.c details */
+	if (!isodata->set_hw_tstamp) {
+		interval = 10000000ULL;
+		hw_base = (isodata->repeat_send_pre_ts + 1)*interval;
+	} else {
+		hw_base = 0x1234 * 1000 + 10000000ULL;
+		interval = 0;
+	}
+
 	tx_tstamp_init(&data->tx_ts, isodata->so_timestamping, false,
-			(isodata->repeat_send_pre_ts + 1)*interval, interval);
+			hw_base, interval);
 
 	for (count = 0; count < isodata->repeat_send + 1; ++count)
 		data->step += tx_tstamp_expect(&data->tx_ts, 0);
@@ -2661,12 +2686,12 @@ static void iso_tx_timestamping(struct test_data *data, GIOChannel *io)
 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))];
+	char control[3 * CMSG_SPACE(sizeof(uint32_t))];
 	struct msghdr msg = {
 		.msg_iov = (struct iovec *)isodata->send,
 		.msg_iovlen = 1,
 	};
-	struct cmsghdr *cmsg;
+	struct cmsghdr *cmsg = NULL;
 	ssize_t ret;
 	int sk;
 
@@ -2674,20 +2699,39 @@ static void iso_send_data(struct test_data *data, GIOChannel *io)
 
 	sk = g_io_channel_unix_get_fd(io);
 
-	if (isodata->cmsg_timestamping) {
-		memset(control, 0, sizeof(control));
-		msg.msg_control = control;
-		msg.msg_controllen = sizeof(control);
+	memset(control, 0, sizeof(control));
+	msg.msg_control = control;
+	msg.msg_controllen = 0;
+
+	if (isodata->cmsg_timestamping) {
+		msg.msg_controllen += CMSG_SPACE(sizeof(uint32_t));
+		cmsg = cmsg ? CMSG_NXTHDR(&msg, cmsg) : CMSG_FIRSTHDR(&msg);
 
-		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 &
 					TS_TX_RECORD_MASK);
 	}
 
+	if (isodata->set_hw_tstamp) {
+		msg.msg_controllen += CMSG_SPACE(sizeof(uint32_t));
+		cmsg = cmsg ? CMSG_NXTHDR(&msg, cmsg) : CMSG_FIRSTHDR(&msg);
+
+		cmsg->cmsg_level = SOL_BLUETOOTH;
+		cmsg->cmsg_type = BT_SCM_PKT_ISO_TS;
+		cmsg->cmsg_len = CMSG_LEN(sizeof(uint32_t));
+		*((uint32_t *)CMSG_DATA(cmsg)) = 0x1234;
+
+		msg.msg_controllen += CMSG_SPACE(sizeof(uint16_t));
+		cmsg = CMSG_NXTHDR(&msg, cmsg);
+
+		cmsg->cmsg_level = SOL_BLUETOOTH;
+		cmsg->cmsg_type = BT_SCM_PKT_SEQNUM;
+		cmsg->cmsg_len = CMSG_LEN(sizeof(uint16_t));
+		*((uint16_t *)CMSG_DATA(cmsg)) = 1234;
+	}
+
 	ret = sendmsg(sk, &msg, 0);
 	if (ret < 0 || isodata->send->iov_len != (size_t) ret) {
 		tester_warn("Failed to write %zu bytes: %s (%d)",
@@ -4126,6 +4170,10 @@ int main(int argc, char *argv[])
 			&connect_send_swhw_timestamping, setup_powered,
 			test_connect);
 
+	test_iso("ISO Send - Set HW Timestamp",
+			&connect_send_set_hw_timestamp, setup_powered,
+			test_connect);
+
 	test_iso("ISO Receive - Success", &listen_16_2_1_recv, setup_powered,
 							test_listen);
 
diff --git a/tools/tester.h b/tools/tester.h
index cb28e9cea..d1ade4603 100644
--- a/tools/tester.h
+++ b/tools/tester.h
@@ -34,6 +34,16 @@
 #define TS_TX_RECORD_MASK		(SOF_TIMESTAMPING_TX_RECORD_MASK | \
 						SOF_TIMESTAMPING_TX_COMPLETION)
 
+#ifndef BT_PKT_SEQNUM
+#define BT_PKT_SEQNUM		22
+#endif
+#ifndef BT_SCM_PKT_SEQNUM
+#define BT_SCM_PKT_SEQNUM	0x05
+#endif
+#ifndef BT_SCM_PKT_ISO_TS
+#define BT_SCM_PKT_ISO_TS	0x06
+#endif
+
 struct tx_tstamp_data {
 	struct {
 		uint32_t id;
-- 
2.53.0

RE: iso-tester: add tests for set & receive ISO HW timestamps

[bluez.test.bot]

[-- Attachment #1: Type: text/plain, Size: 1991 bytes --]

This is automated email and please do not reply to this email!

Dear submitter,

Thank you for submitting the patches to the linux bluetooth mailing list.
This is a CI test results with your patch series:
PW Link:https://patchwork.kernel.org/project/bluetooth/list/?series=1065141

---Test result---

Test Summary:
CheckPatch                    PENDING   0.42 seconds
GitLint                       PENDING   0.45 seconds
BuildEll                      PASS      20.88 seconds
BluezMake                     PASS      655.27 seconds
MakeCheck                     PASS      18.55 seconds
MakeDistcheck                 PASS      246.14 seconds
CheckValgrind                 PASS      295.94 seconds
CheckSmatch                   WARNING   360.92 seconds
bluezmakeextell               PASS      183.99 seconds
IncrementalBuild              PENDING   0.36 seconds
ScanBuild                     PASS      1034.66 seconds

Details
##############################
Test: CheckPatch - PENDING
Desc: Run checkpatch.pl script
Output:

##############################
Test: GitLint - PENDING
Desc: Run gitlint
Output:

##############################
Test: CheckSmatch - WARNING
Desc: Run smatch tool with source
Output:
emulator/bthost.c:710:28: warning: Variable length array is used.emulator/bthost.c:711:32: warning: Variable length array is used.emulator/bthost.c:952:28: warning: Variable length array is used.emulator/bthost.c:986:28: warning: Variable length array is used.emulator/bthost.c:987:32: warning: Variable length array is used.emulator/btdev.c:482:29: warning: Variable length array is used.tools/sco-tester.c: note: in included file:./lib/bluetooth/bluetooth.h:232:15: warning: array of flexible structures./lib/bluetooth/bluetooth.h:237:31: warning: array of flexible structures
##############################
Test: IncrementalBuild - PENDING
Desc: Incremental build with the patches in the series
Output:



https://github.com/bluez/bluez/pull/1956/checks

---
Regards,
Linux Bluetooth