[pulseaudio-discuss] [PATCH v10 04/11] bluetooth: Parse remote timestamp from A2DP RTP packets when available

[Pali Rohár]

Some A2DP codecs (e.g. SBC or aptX-HD) use RTP packets. For sources use
timestamps from RTP packets to calculate read index and therefore remote
timestamp for synchronization.
---
 src/modules/bluetooth/a2dp-codec-api.h       |  4 ++--
 src/modules/bluetooth/a2dp-codec-sbc.c       |  3 ++-
 src/modules/bluetooth/module-bluez5-device.c | 15 ++++++++++++++-
 3 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/src/modules/bluetooth/a2dp-codec-api.h b/src/modules/bluetooth/a2dp-codec-api.h
index 517dc76f1..2bdc768df 100644
--- a/src/modules/bluetooth/a2dp-codec-api.h
+++ b/src/modules/bluetooth/a2dp-codec-api.h
@@ -87,8 +87,8 @@ typedef struct pa_a2dp_codec {
     size_t (*encode_buffer)(void *codec_info, uint32_t timestamp, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed);
     /* Decode input_buffer of input_size to output_buffer of output_size,
      * returns size of filled ouput_buffer and set processed to size of
-     * processed input_buffer */
-    size_t (*decode_buffer)(void *codec_info, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed);
+     * processed input_buffer and set timestamp */
+    size_t (*decode_buffer)(void *codec_info, uint32_t *timestamp, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed);
 } pa_a2dp_codec;
 
 #endif
diff --git a/src/modules/bluetooth/a2dp-codec-sbc.c b/src/modules/bluetooth/a2dp-codec-sbc.c
index 6ab0b46cd..142c76ea4 100644
--- a/src/modules/bluetooth/a2dp-codec-sbc.c
+++ b/src/modules/bluetooth/a2dp-codec-sbc.c
@@ -579,7 +579,7 @@ static size_t encode_buffer(void *codec_info, uint32_t timestamp, const uint8_t
     return d - output_buffer;
 }
 
-static size_t decode_buffer(void *codec_info, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) {
+static size_t decode_buffer(void *codec_info, uint32_t *timestamp, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) {
     struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
 
     struct rtp_header *header;
@@ -631,6 +631,7 @@ static size_t decode_buffer(void *codec_info, const uint8_t *input_buffer, size_
         to_write -= written;
     }
 
+    *timestamp = ntohl(header->timestamp);
     *processed = p - input_buffer;
     return d - output_buffer;
 }
diff --git a/src/modules/bluetooth/module-bluez5-device.c b/src/modules/bluetooth/module-bluez5-device.c
index 08de1154b..0c4a69448 100644
--- a/src/modules/bluetooth/module-bluez5-device.c
+++ b/src/modules/bluetooth/module-bluez5-device.c
@@ -536,6 +536,7 @@ static int a2dp_process_push(struct userdata *u) {
     memchunk.index = memchunk.length = 0;
 
     for (;;) {
+        uint32_t timestamp;
         pa_usec_t tstamp;
         uint8_t *ptr;
         ssize_t l;
@@ -568,7 +569,8 @@ static int a2dp_process_push(struct userdata *u) {
         ptr = pa_memblock_acquire(memchunk.memblock);
         memchunk.length = pa_memblock_get_length(memchunk.memblock);
 
-        memchunk.length = u->a2dp_codec->decode_buffer(u->decoder_info, u->decoder_buffer, l, ptr, memchunk.length, &processed);
+        timestamp = 0; /* Decoder does not have to fill RTP timestamp */
+        memchunk.length = u->a2dp_codec->decode_buffer(u->decoder_info, &timestamp, u->decoder_buffer, l, ptr, memchunk.length, &processed);
 
         if (processed != (size_t) l) {
             pa_log_error("Decoding error");
@@ -583,6 +585,17 @@ static int a2dp_process_push(struct userdata *u) {
             break;
         }
 
+        /* Some codecs may provide RTP timestamp, so use it to update read_index for calculation of remote tstamp */
+        if (timestamp) {
+            /* RTP timestamp is only 32bit and may overflow, try to detect it */
+            size_t frame_size = pa_frame_size(&u->decoder_sample_spec);
+            uint64_t new_read_index1 = (uint64_t) timestamp * frame_size;
+            uint64_t new_read_index2 = (uint64_t) (0x100000000ULL + timestamp) * frame_size;
+            uint64_t delta_read_index1 = (new_read_index1 > u->read_index) ? (new_read_index1 - u->read_index) : (u->read_index - new_read_index1);
+            uint64_t delta_read_index2 = (new_read_index2 > u->read_index) ? (new_read_index2 - u->read_index) : (u->read_index - new_read_index2);
+            u->read_index = (delta_read_index1 < delta_read_index2) ? delta_read_index1 : delta_read_index2;
+        }
+
         u->read_index += (uint64_t) memchunk.length;
         pa_smoother_put(u->read_smoother, tstamp, pa_bytes_to_usec(u->read_index, &u->decoder_sample_spec));
         pa_smoother_resume(u->read_smoother, tstamp, true);
-- 
2.11.0