pipewire/src/modules/module-avb/stream.c

/* AVB support
 *
 * Copyright © 2022 Wim Taymans
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice (including the next
 * paragraph) shall be included in all copies or substantial portions of the
 * Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

#include <unistd.h>
#include <linux/if_ether.h>
#include <linux/if_packet.h>
#include <linux/net_tstamp.h>
#include <net/if.h>
#include <sys/ioctl.h>

#include <spa/debug/mem.h>
#include <spa/pod/builder.h>
#include <spa/param/audio/format-utils.h>

#include "iec61883.h"
#include "stream.h"
#include "utils.h"
#include "aecp-aem-descriptors.h"

static void on_stream_destroy(void *d)
{
	struct stream *stream = d;
	spa_hook_remove(&stream->stream_listener);
	stream->stream = NULL;
}

static void on_source_stream_process(void *data)
{
	struct stream *stream = data;
	struct pw_buffer *buf;
	struct spa_data *d;
	uint32_t index, n_bytes;
	int32_t avail, wanted;

	if ((buf = pw_stream_dequeue_buffer(stream->stream)) == NULL) {
		pw_log_debug("out of buffers: %m");
		return;
	}

	d = buf->buffer->datas;

	wanted = buf->requested ? buf->requested * stream->stride : d[0].maxsize;

	n_bytes = SPA_MIN(d[0].maxsize, (uint32_t)wanted);

	avail = spa_ringbuffer_get_read_index(&stream->ring, &index);

	if (avail < wanted) {
		pw_log_warn("capture underrun %d < %d", avail, wanted);
		memset(d[0].data, 0, n_bytes);
	} else {
		spa_ringbuffer_read_data(&stream->ring,
				stream->buffer_data,
				stream->buffer_size,
				index % stream->buffer_size,
				d[0].data, n_bytes);
		index += n_bytes;
		spa_ringbuffer_read_update(&stream->ring, index);
	}

	d[0].chunk->size = n_bytes;
	d[0].chunk->stride = stream->stride;
	d[0].chunk->offset = 0;
	buf->size = n_bytes / stream->stride;

	pw_stream_queue_buffer(stream->stream, buf);
}

static const struct pw_stream_events source_stream_events = {
	PW_VERSION_STREAM_EVENTS,
	.destroy = on_stream_destroy,
	.process = on_source_stream_process
};

static inline void
set_iovec(struct spa_ringbuffer *rbuf, void *buffer, uint32_t size,
		uint32_t offset, struct iovec *iov, uint32_t len)
{
	iov[0].iov_len = SPA_MIN(len, size - offset);
	iov[0].iov_base = SPA_PTROFF(buffer, offset, void);
	iov[1].iov_len = len - iov[0].iov_len;
	iov[1].iov_base = buffer;
}

static int flush_write(struct stream *stream, uint64_t current_time)
{
	int32_t avail;
	uint32_t index;
        uint64_t ptime, txtime;
	int pdu_count;
	ssize_t n;
	struct avb_packet_iec61883 *p = (struct avb_packet_iec61883*)stream->pdu;

	avail = spa_ringbuffer_get_read_index(&stream->ring, &index);

	pdu_count = (avail / stream->stride) / stream->frames_per_pdu;

	txtime = current_time + stream->t_uncertainty;
	ptime = txtime + stream->mtt;

	while (pdu_count--) {
		*(uint64_t*)CMSG_DATA(stream->cmsg) = txtime;

		set_iovec(&stream->ring,
			stream->buffer_data,
			stream->buffer_size,
			index % stream->buffer_size,
			&stream->iov[1], stream->payload_size);

		p->seq_num = stream->pdu_seq++;
		p->tv = 1;
		p->timestamp = ptime;

		n = sendmsg(stream->source->fd, &stream->msg, 0);
		if (n < 0 || n != (ssize_t)stream->pdu_size) {
			pw_log_error("sendmsg() failed %zd != %zd: %m",
					n, stream->pdu_size);
		}
		txtime += stream->pdu_period;
		ptime += stream->pdu_period;
		index += stream->payload_size;
	}
	spa_ringbuffer_read_update(&stream->ring, index);
	return 0;
}

static void on_sink_stream_process(void *data)
{
	struct stream *stream = data;
	struct pw_buffer *buf;
	struct spa_data *d;
	int32_t filled;
	uint32_t index, offs, avail, size;
	struct timespec now;

	if ((buf = pw_stream_dequeue_buffer(stream->stream)) == NULL) {
		pw_log_debug("out of buffers: %m");
		return;
	}

	d = buf->buffer->datas;

	offs = SPA_MIN(d[0].chunk->offset, d[0].maxsize);
	size = SPA_MIN(d[0].chunk->size, d[0].maxsize - offs);
	avail = size - offs;

	filled = spa_ringbuffer_get_write_index(&stream->ring, &index);

	if (filled >= (int32_t)stream->buffer_size) {
		pw_log_warn("playback overrun %d >= %zd", filled, stream->buffer_size);
	} else {
		spa_ringbuffer_write_data(&stream->ring,
				stream->buffer_data,
				stream->buffer_size,
				index % stream->buffer_size,
				SPA_PTROFF(d[0].data, offs, void), avail);
		index += avail;
		spa_ringbuffer_write_update(&stream->ring, index);
	}
	pw_stream_queue_buffer(stream->stream, buf);

	clock_gettime(CLOCK_TAI, &now);
	flush_write(stream, SPA_TIMESPEC_TO_NSEC(&now));
}

static void setup_pdu(struct stream *stream)
{
	struct avb_packet_iec61883 *p = (struct avb_packet_iec61883*)stream->pdu;
	ssize_t payload_size, hdr_size, pdu_size;

	hdr_size = sizeof(*p);
	payload_size = stream->stride * stream->frames_per_pdu;
	pdu_size = hdr_size + payload_size;

	spa_zero(stream->pdu);
	p->eth.type = htons(0x8100);
	p->vlan = htonl(0x600222f0);

	if (stream->direction == SPA_DIRECTION_OUTPUT) {
		p->subtype = AVB_SUBTYPE_61883_IIDC;
		p->sv = 1;
		p->stream_id = htobe64(stream->id);
		p->data_len = htons(payload_size+8);
		p->tag = 0x1;
		p->channel = 0x1f;
		p->tcode = 0xa;
		p->sid = 0x3f;
		p->dbs = 0x8;
		p->qi2 = 0x2;
		p->format_id = 0x10;
		p->fdf = 0x2;
		p->syt = htons(0xffff);
	}
	stream->hdr_size = hdr_size;
	stream->payload_size = payload_size;
	stream->pdu_size = pdu_size;
}

static int setup_msg(struct stream *stream)
{
	stream->iov[0].iov_base = stream->pdu;
	stream->iov[0].iov_len = stream->hdr_size;
	stream->iov[1].iov_base = SPA_PTROFF(stream->pdu, stream->hdr_size, void);
	stream->iov[1].iov_len = stream->payload_size;
	stream->iov[2].iov_base = SPA_PTROFF(stream->pdu, stream->hdr_size, void);
	stream->iov[2].iov_len = 0;
	stream->msg.msg_name = &stream->sock_addr;
	stream->msg.msg_namelen = sizeof(stream->sock_addr);
	stream->msg.msg_iov = stream->iov;
	stream->msg.msg_iovlen = 3;
	stream->msg.msg_control = stream->control;
	stream->msg.msg_controllen = sizeof(stream->control);
	stream->cmsg = CMSG_FIRSTHDR(&stream->msg);
	stream->cmsg->cmsg_level = SOL_SOCKET;
	stream->cmsg->cmsg_type = SCM_TXTIME;
	stream->cmsg->cmsg_len = CMSG_LEN(sizeof(__u64));
	return 0;
}

static const struct pw_stream_events sink_stream_events = {
	PW_VERSION_STREAM_EVENTS,
	.destroy = on_stream_destroy,
	.process = on_sink_stream_process
};

struct stream *server_create_stream(struct server *server,
		enum spa_direction direction, uint16_t index)
{
	struct stream *stream;
	const struct descriptor *desc;
	uint32_t n_params;
	const struct spa_pod *params[1];
	uint8_t buffer[1024];
	struct spa_pod_builder b;
	int res;

	desc = server_find_descriptor(server,
			direction == SPA_DIRECTION_INPUT ?
			AVB_AEM_DESC_STREAM_INPUT :
			AVB_AEM_DESC_STREAM_OUTPUT, index);
	if (desc == NULL)
		return NULL;

	stream = calloc(1, sizeof(*stream));
	if (stream == NULL)
		return NULL;

	stream->server = server;
	stream->direction = direction;
	stream->index = index;
	stream->desc = desc;
	spa_list_append(&server->streams, &stream->link);

	stream->id = (uint64_t)server->mac_addr[0] << 56 |
			(uint64_t)server->mac_addr[1] << 48 |
			(uint64_t)server->mac_addr[2] << 40 |
			(uint64_t)server->mac_addr[3] << 32 |
			(uint64_t)server->mac_addr[4] << 24 |
			(uint64_t)server->mac_addr[5] << 16 |
			htons(index);

	stream->listener_attr = avb_msrp_attribute_new(server->msrp,
			AVB_MSRP_ATTRIBUTE_TYPE_LISTENER);
	stream->talker_attr = avb_msrp_attribute_new(server->msrp,
			AVB_MSRP_ATTRIBUTE_TYPE_TALKER_ADVERTISE);
	stream->talker_attr->attr.talker.vlan_id = htons(2);
	stream->talker_attr->attr.talker.tspec_max_interval_frames =
		htons(AVB_MSRP_TSPEC_MAX_INTERVAL_FRAMES_DEFAULT);
	stream->talker_attr->attr.talker.priority = AVB_MSRP_PRIORITY_DEFAULT;
	stream->talker_attr->attr.talker.rank = AVB_MSRP_RANK_DEFAULT;
	stream->talker_attr->attr.talker.accumulated_latency = 0;

	stream->vlan_attr = avb_mvrp_attribute_new(server->mvrp,
			AVB_MVRP_ATTRIBUTE_TYPE_VID);
	stream->vlan_attr->attr.vid.vlan = htons(2);

	stream->buffer_data = calloc(1, BUFFER_SIZE);
	stream->buffer_size = BUFFER_SIZE;
	spa_ringbuffer_init(&stream->ring);

	if (direction == SPA_DIRECTION_INPUT) {
		stream->stream = pw_stream_new(server->impl->core, "source",
			pw_properties_new(
				PW_KEY_MEDIA_CLASS, "Audio/Source",
				PW_KEY_NODE_NAME, "avb.source",
				PW_KEY_NODE_DESCRIPTION, "AVB Source",
				PW_KEY_NODE_WANT_DRIVER, "true",
				NULL));
	} else {
		stream->stream = pw_stream_new(server->impl->core, "sink",
			pw_properties_new(
				PW_KEY_MEDIA_CLASS, "Audio/Sink",
				PW_KEY_NODE_NAME, "avb.sink",
				PW_KEY_NODE_DESCRIPTION, "AVB Sink",
				PW_KEY_NODE_WANT_DRIVER, "true",
				NULL));
	}
	if (stream->stream == NULL)
		goto error_free;

	pw_stream_add_listener(stream->stream,
			&stream->stream_listener,
			direction == SPA_DIRECTION_INPUT ?
				&source_stream_events :
				&sink_stream_events,
			stream);

	stream->info.info.raw.format = SPA_AUDIO_FORMAT_S24_32_BE;
	stream->info.info.raw.flags = SPA_AUDIO_FLAG_UNPOSITIONED;
	stream->info.info.raw.rate = 48000;
	stream->info.info.raw.channels = 8;
	stream->stride = 8 * 4;

	n_params = 0;
	spa_pod_builder_init(&b, buffer, sizeof(buffer));
	params[n_params++] = spa_format_audio_raw_build(&b,
			SPA_PARAM_EnumFormat, &stream->info.info.raw);

	if ((res = pw_stream_connect(stream->stream,
			pw_direction_reverse(direction),
			PW_ID_ANY,
			PW_STREAM_FLAG_MAP_BUFFERS |
			PW_STREAM_FLAG_INACTIVE |
			PW_STREAM_FLAG_RT_PROCESS,
			params, n_params)) < 0)
		goto error_free_stream;

	stream->frames_per_pdu = 6;
	stream->pdu_period = SPA_NSEC_PER_SEC * stream->frames_per_pdu /
                          stream->info.info.raw.rate;

	setup_pdu(stream);
	setup_msg(stream);

	return stream;

error_free_stream:
	pw_stream_destroy(stream->stream);
	errno = -res;
error_free:
	free(stream);
	return NULL;
}

void stream_destroy(struct stream *stream)
{
	avb_mrp_attribute_destroy(stream->listener_attr->mrp);
	spa_list_remove(&stream->link);
	free(stream);
}

static int setup_socket(struct stream *stream)
{
	struct server *server = stream->server;
	int fd, res;
	char buf[128];
	struct ifreq req;

	fd = socket(AF_PACKET, SOCK_RAW | SOCK_NONBLOCK, htons(ETH_P_ALL));
	if (fd < 0) {
		pw_log_error("socket() failed: %m");
		return -errno;
	}

	spa_zero(req);
	snprintf(req.ifr_name, sizeof(req.ifr_name), "%s", server->ifname);
	res = ioctl(fd, SIOCGIFINDEX, &req);
	if (res < 0) {
		pw_log_error("SIOCGIFINDEX %s failed: %m", server->ifname);
		res = -errno;
		goto error_close;
	}

	spa_zero(stream->sock_addr);
	stream->sock_addr.sll_family = AF_PACKET;
	stream->sock_addr.sll_protocol = htons(ETH_P_TSN);
	stream->sock_addr.sll_ifindex = req.ifr_ifindex;

	if (stream->direction == SPA_DIRECTION_OUTPUT) {
		struct sock_txtime txtime_cfg;

		res = setsockopt(fd, SOL_SOCKET, SO_PRIORITY, &stream->prio,
				sizeof(stream->prio));
		if (res < 0) {
			pw_log_error("setsockopt(SO_PRIORITY %d) failed: %m", stream->prio);
			res = -errno;
			goto error_close;
		}

		txtime_cfg.clockid = CLOCK_TAI;
		txtime_cfg.flags = 0;
		res = setsockopt(fd, SOL_SOCKET, SO_TXTIME, &txtime_cfg,
				sizeof(txtime_cfg));
		if (res < 0) {
			pw_log_error("setsockopt(SO_TXTIME) failed: %m");
			res = -errno;
			goto error_close;
		}
	} else {
		struct packet_mreq mreq;

		res = bind(fd, (struct sockaddr *) &stream->sock_addr, sizeof(stream->sock_addr));
		if (res < 0) {
			pw_log_error("bind() failed: %m");
			res = -errno;
			goto error_close;
		}

		spa_zero(mreq);
		mreq.mr_ifindex = req.ifr_ifindex;
		mreq.mr_type = PACKET_MR_MULTICAST;
		mreq.mr_alen = ETH_ALEN;
		memcpy(&mreq.mr_address, stream->addr, ETH_ALEN);
		res = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
				&mreq, sizeof(struct packet_mreq));

		pw_log_info("join %s", avb_utils_format_addr(buf, 128, stream->addr));

		if (res < 0) {
			pw_log_error("setsockopt(ADD_MEMBERSHIP) failed: %m");
			res = -errno;
			goto error_close;
		}
	}
	return fd;

error_close:
	close(fd);
	return res;
}

static void handle_iec61883_packet(struct stream *stream,
		struct avb_packet_iec61883 *p, int len)
{
	uint32_t index, n_bytes;
	int32_t filled;
	bool overrun = false;

	filled = spa_ringbuffer_get_write_index(&stream->ring, &index);
	overrun = filled > (int32_t) stream->buffer_size;
	if (overrun) {
		pw_log_warn("capture overrun %zd < %d", stream->buffer_size, filled);
	} else {
		n_bytes = ntohs(p->data_len) - 8;
		spa_ringbuffer_write_data(&stream->ring,
				stream->buffer_data,
				stream->buffer_size,
				index % stream->buffer_size,
				p->payload, n_bytes);
		index += n_bytes;
		spa_ringbuffer_write_update(&stream->ring, index);
	}

}

static void on_socket_data(void *data, int fd, uint32_t mask)
{
	struct stream *stream = data;

	if (mask & SPA_IO_IN) {
		int len;
		uint8_t buffer[2048];

		len = recv(fd, buffer, sizeof(buffer), 0);

		if (len < 0) {
			pw_log_warn("got recv error: %m");
		}
		else if (len < (int)sizeof(struct avb_packet_header)) {
			pw_log_warn("short packet received (%d < %d)", len,
					(int)sizeof(struct avb_packet_header));
		} else {
			struct avb_packet_iec61883 *h = (struct avb_packet_iec61883*)buffer;

			if (memcmp(h->eth.dest, stream->addr, 6) != 0 ||
			    h->subtype != AVB_SUBTYPE_61883_IIDC)
				return;

			handle_iec61883_packet(stream, h, len);
		}
	}
}

int stream_activate(struct stream *stream, uint64_t now)
{
	struct server *server = stream->server;
	struct avb_ethernet_header *h = (struct avb_ethernet_header *)stream->pdu;
	int fd, res;

	if (stream->source == NULL) {
		if ((fd = setup_socket(stream)) < 0)
			return fd;

		stream->source = pw_loop_add_io(server->impl->loop, fd,
				SPA_IO_IN, true, on_socket_data, stream);
		if (stream->source == NULL) {
			res = -errno;
			pw_log_error("stream %p: can't create source: %m", stream);
			close(fd);
			return res;
		}
	}

	avb_mrp_attribute_begin(stream->vlan_attr->mrp, now);
	avb_mrp_attribute_join(stream->vlan_attr->mrp, now, true);

	if (stream->direction == SPA_DIRECTION_INPUT) {
		stream->listener_attr->attr.listener.stream_id = htobe64(stream->peer_id);
		stream->listener_attr->param = AVB_MSRP_LISTENER_PARAM_READY;
		avb_mrp_attribute_begin(stream->listener_attr->mrp, now);
		avb_mrp_attribute_join(stream->listener_attr->mrp, now, true);

		stream->talker_attr->attr.talker.stream_id = htobe64(stream->peer_id);
		avb_mrp_attribute_begin(stream->talker_attr->mrp, now);
	} else {
		stream->listener_attr->attr.listener.stream_id = htobe64(stream->id);
		stream->listener_attr->param = AVB_MSRP_LISTENER_PARAM_IGNORE;
		avb_mrp_attribute_begin(stream->listener_attr->mrp, now);

		stream->talker_attr->attr.talker.stream_id = htobe64(stream->id);
		memcpy(stream->talker_attr->attr.talker.dest_addr, stream->addr, 6);

		stream->sock_addr.sll_halen = ETH_ALEN;
		memcpy(&stream->sock_addr.sll_addr, stream->addr, ETH_ALEN);
		memcpy(h->dest, stream->addr, 6);
		memcpy(h->src, server->mac_addr, 6);
		avb_mrp_attribute_begin(stream->talker_attr->mrp, now);
		avb_mrp_attribute_join(stream->talker_attr->mrp, now, true);
	}
	pw_stream_set_active(stream->stream, true);
	return 0;
}

int stream_deactivate(struct stream *stream, uint64_t now)
{
	pw_stream_set_active(stream->stream, false);

	if (stream->source != NULL) {
		pw_loop_destroy_source(stream->server->impl->loop, stream->source);
		stream->source = NULL;
	}

	avb_mrp_attribute_leave(stream->vlan_attr->mrp, now);

	if (stream->direction == SPA_DIRECTION_INPUT) {
		avb_mrp_attribute_leave(stream->listener_attr->mrp, now);
	} else {
		avb_mrp_attribute_leave(stream->talker_attr->mrp, now);
	}
	return 0;
}