bluez5: report latency to rendering when in A2DP sink role

Now that BlueZ supports delay reporting in A2DP sink role, implement
that.

Report value that gives the total latency between packet reception and
audio rendering.

Also make Latency parameter in media-source to be not just a dummy
value.

spa/plugins/bluez5/bluez5-dbus.c

@@ -3987,12 +3987,48 @@ static int transport_release(void *data)
 	return do_transport_release(data);
 }
 
+static int transport_set_delay(void *data, int64_t delay_nsec)
+{
+	struct spa_bt_transport *transport = data;
+	struct spa_bt_monitor *monitor = transport->monitor;
+	DBusMessageIter it[2];
+	spa_autoptr(DBusMessage) m = NULL;
+	uint16_t value;
+	const char *property = "Delay", *interface = BLUEZ_MEDIA_TRANSPORT_INTERFACE;
+
+	if (!(transport->profile & SPA_BT_PROFILE_A2DP_DUPLEX))
+		return -ENOTSUP;
+
+	value = SPA_CLAMP(delay_nsec / (100 * SPA_NSEC_PER_USEC), 0, 10 * UINT16_MAX);
+
+	if (transport->delay_us == 100 * value)
+		return 0;
+	transport->delay_us = 100 * value;
+
+	m = dbus_message_new_method_call(BLUEZ_SERVICE, transport->path, DBUS_INTERFACE_PROPERTIES, "Set");
+	if (m == NULL)
+		return -ENOMEM;
+
+	dbus_message_iter_init_append(m, &it[0]);
+	dbus_message_iter_append_basic(&it[0], DBUS_TYPE_STRING, &interface);
+	dbus_message_iter_append_basic(&it[0], DBUS_TYPE_STRING, &property);
+	dbus_message_iter_open_container(&it[0], DBUS_TYPE_VARIANT, DBUS_TYPE_UINT16_AS_STRING, &it[1]);
+	dbus_message_iter_append_basic(&it[1], DBUS_TYPE_UINT16, &value);
+	dbus_message_iter_close_container(&it[0], &it[1]);
+
+	if (!dbus_connection_send(monitor->conn, m, NULL))
+		return -EIO;
+
+	spa_log_debug(monitor->log, "transport %p: set delay %d us", transport, 100 * value);
+	return 0;
+}
 
 static const struct spa_bt_transport_implementation transport_impl = {
 	SPA_VERSION_BT_TRANSPORT_IMPLEMENTATION,
 	.acquire = transport_acquire,
 	.release = transport_release,
 	.set_volume = transport_set_volume,
+	.set_delay = transport_set_delay,
 };
 
 static void media_codec_switch_reply(DBusPendingCall *pending, void *userdata);

spa/plugins/bluez5/decode-buffer.h

@@ -200,18 +200,30 @@ static void spa_bt_decode_buffer_recover(struct spa_bt_decode_buffer *this)
 	spa_bt_rate_control_init(&this->ctl, level);
 }
 
-static SPA_UNUSED
-void spa_bt_decode_buffer_set_target_latency(struct spa_bt_decode_buffer *this, int32_t samples)
+static inline void spa_bt_decode_buffer_set_target_latency(struct spa_bt_decode_buffer *this, int32_t samples)
 {
 	this->target = samples;
 }
 
-static SPA_UNUSED
-void spa_bt_decode_buffer_set_max_latency(struct spa_bt_decode_buffer *this, int32_t samples)
+static inline void spa_bt_decode_buffer_set_max_latency(struct spa_bt_decode_buffer *this, int32_t samples)
 {
 	this->max_target = samples;
 }
 
+static inline int32_t spa_bt_decode_buffer_get_target(struct spa_bt_decode_buffer *this)
+{
+	const int32_t packet_size = SPA_CLAMP(this->packet_size.max, 0, INT32_MAX/8);
+	const int32_t max_buf = (this->buffer_size - this->buffer_reserve) / this->frame_size;
+	int32_t target;
+
+	if (this->target)
+		target = this->target;
+	else
+		target = BUFFERING_TARGET(this->spike.max, packet_size, max_buf);
+
+	return SPA_MIN(target, this->max_target);
+}
+
 static void spa_bt_decode_buffer_process(struct spa_bt_decode_buffer *this, uint32_t samples, uint32_t duration)
 {
 	const uint32_t data_size = samples * this->frame_size;
@@ -245,7 +257,6 @@ static void spa_bt_decode_buffer_process(struct spa_bt_decode_buffer *this, uint
 
 	if (this->received) {
 		const uint32_t avg_period = (uint64_t)this->rate * BUFFERING_SHORT_MSEC / 1000;
-		const int32_t max_buf = (this->buffer_size - this->buffer_reserve) / this->frame_size;
 		int32_t level, target;
 
 		/* Track buffer level */
@@ -256,12 +267,7 @@ static void spa_bt_decode_buffer_process(struct spa_bt_decode_buffer *this, uint
 		spa_bt_ptp_update(&this->spike, (int32_t)(this->ctl.avg - level), this->prev_consumed);
 
 		/* Update target level */
-		if (this->target)
-			target = this->target;
-		else
-			target = BUFFERING_TARGET(this->spike.max, packet_size, max_buf);
-
-		target = SPA_MIN(target, this->max_target);
+		target = spa_bt_decode_buffer_get_target(this);
 
 		if (level > SPA_MAX(4 * target, 2*(int32_t)duration) &&
 				avail > data_size) {

spa/plugins/bluez5/defs.h

@@ -616,6 +616,7 @@ struct spa_bt_transport_implementation {
 	int (*acquire) (void *data, bool optional);
 	int (*release) (void *data);
 	int (*set_volume) (void *data, int id, float volume);
+	int (*set_delay) (void *data, int64_t delay_nsec);
 	int (*destroy) (void *data);
 };
 
@@ -721,6 +722,7 @@ int spa_bt_transport_ensure_sco_io(struct spa_bt_transport *t, struct spa_loop *
 
 #define spa_bt_transport_destroy(t)		spa_bt_transport_impl(t, destroy, 0)
 #define spa_bt_transport_set_volume(t,...)	spa_bt_transport_impl(t, set_volume, 0, __VA_ARGS__)
+#define spa_bt_transport_set_delay(t,...)	spa_bt_transport_impl(t, set_delay, 0, __VA_ARGS__)
 
 static inline enum spa_bt_transport_state spa_bt_transport_state_from_string(const char *value)
 {

spa/plugins/bluez5/media-source.c

@@ -68,7 +68,7 @@ struct port {
 	struct spa_port_info info;
 	struct spa_io_buffers *io;
 	struct spa_io_rate_match *rate_match;
-	struct spa_latency_info latency;
+	struct spa_latency_info latency[2];
 #define IDX_EnumFormat	0
 #define IDX_Meta	1
 #define IDX_IO		2
@@ -87,6 +87,18 @@ struct port {
 	struct spa_bt_decode_buffer buffer;
 };
 
+struct delay_info {
+	union {
+		struct {
+			int32_t buffer;
+			uint32_t duration;
+		};
+		uint64_t v;
+	};
+};
+
+SPA_STATIC_ASSERT(sizeof(struct delay_info) == sizeof(uint64_t));
+
 struct impl {
 	struct spa_handle handle;
 	struct spa_node node;
@@ -94,6 +106,7 @@ struct impl {
 	struct spa_log *log;
 	struct spa_loop *data_loop;
 	struct spa_system *data_system;
+	struct spa_loop_utils *loop_utils;
 
 	struct spa_hook_list hooks;
 	struct spa_callbacks callbacks;
@@ -150,6 +163,9 @@ struct impl {
 	uint64_t sample_count;
 
 	uint32_t errqueue_count;
+
+	struct delay_info delay;
+	struct spa_source *update_delay_event;
 };
 
 #define CHECK_PORT(this,d,p)    ((d) == SPA_DIRECTION_OUTPUT && (p) == 0)
@@ -648,6 +664,44 @@ static void media_iso_pull(struct spa_bt_iso_io *iso_io)
 	 * iso-io whether this source is running or not. */
 }
 
+static void emit_port_info(struct impl *this, struct port *port, bool full);
+
+static void update_transport_delay(struct impl *this)
+{
+	struct port *port = &this->port;
+	struct delay_info info;
+	float latency;
+	int64_t latency_nsec;
+
+	if (!this->transport || !port->have_format)
+		return;
+
+	info.v = __atomic_load_n(&this->delay.v, __ATOMIC_RELAXED);
+
+	/* Latency to sink */
+	latency = info.buffer + info.duration
+		+ port->latency[SPA_DIRECTION_INPUT].min_rate
+		+ port->latency[SPA_DIRECTION_INPUT].min_quantum * info.duration;
+
+	latency_nsec = port->latency[SPA_DIRECTION_INPUT].min_ns
+		+ (int64_t)(latency * SPA_NSEC_PER_SEC / port->current_format.info.raw.rate);
+
+	spa_bt_transport_set_delay(this->transport, latency_nsec);
+
+	/* Latency from source */
+	port->latency[SPA_DIRECTION_OUTPUT] = SPA_LATENCY_INFO(SPA_DIRECTION_OUTPUT,
+			.min_rate = info.buffer, .max_rate = info.buffer);
+	port->info.change_mask |= SPA_PORT_CHANGE_MASK_PARAMS;
+	port->params[IDX_Latency].user++;
+	emit_port_info(this, port, false);
+}
+
+static void update_delay_event(void *data, uint64_t count)
+{
+	/* in main loop */
+	update_transport_delay(data);
+}
+
 static int do_start_iso_io(struct spa_loop *loop, bool async, uint32_t seq,
 		const void *data, size_t size, void *user_data)
 {
@@ -714,6 +768,10 @@ static int transport_start(struct impl *this)
 				port->current_format.info.raw.rate * 80 / 1000);
 	}
 
+	this->delay.buffer = -1;
+	this->delay.duration = 0;
+	this->update_delay_event = spa_loop_utils_add_event(this->loop_utils, update_delay_event, this);
+
 	this->sample_count = 0;
 	this->errqueue_count = 0;
 
@@ -790,6 +848,11 @@ static int do_remove_source(struct spa_loop *loop,
 		spa_bt_iso_io_set_cb(this->transport->iso_io, NULL, NULL);
 	set_timeout(this, 0);
 
+	if (this->update_delay_event) {
+		spa_loop_utils_destroy_source(this->loop_utils, this->update_delay_event);
+		this->update_delay_event = NULL;
+	}
+
 	return 0;
 }
 
@@ -1104,8 +1167,8 @@ impl_node_port_enum_params(void *object, int seq,
 
 	case SPA_PARAM_Latency:
 		switch (result.index) {
-		case 0:
-			param = spa_latency_build(&b, id, &port->latency);
+		case 0: case 1:
+			param = spa_latency_build(&b, id, &port->latency[result.index]);
 			break;
 		default:
 			return 0;
@@ -1224,8 +1287,28 @@ impl_node_port_set_param(void *object,
 		res = port_set_format(this, port, flags, param);
 		break;
 	case SPA_PARAM_Latency:
+	{
+		enum spa_direction other = SPA_DIRECTION_REVERSE(direction);
+		struct spa_latency_info info;
+
+		if (param == NULL)
+			info = SPA_LATENCY_INFO(other);
+		else if ((res = spa_latency_parse(param, &info)) < 0)
+			return res;
+		if (info.direction != other)
+			return -EINVAL;
+		if (memcmp(&port->latency[info.direction], &info, sizeof(info)) == 0)
+			return 0;
+
+		port->latency[info.direction] = info;
+		this->port.info.change_mask |= SPA_PORT_CHANGE_MASK_PARAMS;
+		this->port.params[IDX_Latency].user++;
+
+		update_transport_delay(this);
+		emit_port_info(this, port, false);
 		res = 0;
 		break;
+	}
 	default:
 		res = -ENOENT;
 		break;
@@ -1459,6 +1542,17 @@ static void process_buffering(struct impl *this)
 		spa_log_trace(this->log, "queue %d frames:%d", buffer->id, (int)samples);
 		spa_list_append(&port->ready, &buffer->link);
 	}
+
+	if (this->update_delay_event) {
+		int32_t target = spa_bt_decode_buffer_get_target(&port->buffer);
+
+		if (target != this->delay.buffer || duration != this->delay.duration) {
+			struct delay_info info = { .buffer = target, .duration = duration };
+
+			__atomic_store_n(&this->delay.v, info.v, __ATOMIC_RELAXED);
+			spa_loop_utils_signal_event(this->loop_utils, this->update_delay_event);
+		}
+	}
 }
 
 static int produce_buffer(struct impl *this)
@@ -1679,6 +1773,7 @@ impl_init(const struct spa_handle_factory *factory,
 	this->log = spa_support_find(support, n_support, SPA_TYPE_INTERFACE_Log);
 	this->data_loop = spa_support_find(support, n_support, SPA_TYPE_INTERFACE_DataLoop);
 	this->data_system = spa_support_find(support, n_support, SPA_TYPE_INTERFACE_DataSystem);
+	this->loop_utils = spa_support_find(support, n_support, SPA_TYPE_INTERFACE_LoopUtils);
 
 	spa_log_topic_init(this->log, &log_topic);
 
@@ -1690,6 +1785,10 @@ impl_init(const struct spa_handle_factory *factory,
 		spa_log_error(this->log, "a data system is needed");
 		return -EINVAL;
 	}
+	if (this->loop_utils == NULL) {
+		spa_log_error(this->log, "loop utils are needed");
+		return -EINVAL;
+	}
 
 	this->node.iface = SPA_INTERFACE_INIT(
 			SPA_TYPE_INTERFACE_Node,
@@ -1731,9 +1830,8 @@ impl_init(const struct spa_handle_factory *factory,
 	port->info.params = port->params;
 	port->info.n_params = N_PORT_PARAMS;
 
-	port->latency = SPA_LATENCY_INFO(SPA_DIRECTION_OUTPUT);
-	port->latency.min_quantum = 1.0f;
-	port->latency.max_quantum = 1.0f;
+	port->latency[SPA_DIRECTION_INPUT] = SPA_LATENCY_INFO(SPA_DIRECTION_INPUT);
+	port->latency[SPA_DIRECTION_OUTPUT] = SPA_LATENCY_INFO(SPA_DIRECTION_OUTPUT);
 
 	/* Init the buffer lists */
 	spa_list_init(&port->ready);