[RFC v2 14/18] bluetooth: Build and push fragmented MPEG frames

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



---
 src/modules/bluetooth/module-bluetooth-device.c |  229 +++++++++++++++++++----
 1 files changed, 192 insertions(+), 37 deletions(-)

diff --git a/src/modules/bluetooth/module-bluetooth-device.c b/src/modules/bluetooth/module-bluetooth-device.c
index 0acce34..cb4714d 100644
--- a/src/modules/bluetooth/module-bluetooth-device.c
+++ b/src/modules/bluetooth/module-bluetooth-device.c
@@ -213,6 +213,9 @@ enum {
 
 #define USE_SCO_OVER_PCM(u) (u->profile == PROFILE_HSP && (u->hsp.sco_sink && u->hsp.sco_source))
 
+#define MPEG_MIN_FRAME_SIZE 4
+#define MPEG_MAX_FRAME_SIZE 1728
+
 static int init_bt(struct userdata *u);
 static int init_profile(struct userdata *u);
 
@@ -1785,7 +1788,7 @@ static pa_bool_t mp3_synclength(uint32_t hi, uint32_t *len, uint32_t *sample_len
                             }
 
                             /* sanity check */
-                            if (bits > 4 && bits <= 1728) { /* max frame length */
+                            if (bits > MPEG_MIN_FRAME_SIZE && bits <= MPEG_MAX_FRAME_SIZE) { /* max frame length */
                                 *len = bits;
                                 *sample_len = mpeg_frame_length[id][1];
                                 return TRUE;
@@ -1818,7 +1821,7 @@ static pa_bool_t do_sync_iec958(const uint8_t **p_src, uint32_t *input_bytes)
 
         /* we need 4 bytes to detect the Pa,Pb preambles */
         preambles = src[0]<<24 | src[1]<<16 | src[2]<<8 | src[3];
-#define IEC958_MAGIC_NUMBER  0x72F81F4E
+#define IEC958_MAGIC_NUMBER  0x72F81F4E /* little endian encoded */
         if (preambles == IEC958_MAGIC_NUMBER) { /* little endian assumed */
             sync_found = TRUE;
             break;
@@ -2083,6 +2086,46 @@ static int a2dp_passthrough_process_render(struct userdata *u) {
     return ret;
 }
 
+/* Size of an IEC958 padded MPEG frame. */
+#define MPEGP_IEC_FRAME_SIZE (1152*4)
+/* Size of the IEC958 header. */
+#define MPEGP_IEC_HEADER_SIZE 8
+/* Size of the MPEG header. */
+#define MPEGP_MPEG_HEADER_SIZE 4
+
+typedef struct {
+  uint8_t sync_byte;
+
+  uint8_t protect:1;
+  uint8_t layer:2;
+  uint8_t sync:3;
+  uint8_t version:2;
+
+  uint8_t priv:1;
+  uint8_t padding:1;
+  uint8_t sampling:2;
+  uint8_t bitrate:4;
+
+  uint8_t emphasys:2;
+  uint8_t original:1;
+  uint8_t copyright:1;
+  uint8_t extension:2;
+  uint8_t channelmode:2;
+} mpeg_header;
+
+static int mp3_length(uint32_t header)
+{
+    mpeg_header *hdr = (mpeg_header *)&header;
+    int mpeg_frame_size = mpeg_layer_bitrates[hdr->version & 0x1][hdr->bitrate] * 1000
+                                          * mpeg_frame_length[hdr->version & 0x1][hdr->layer]
+                                          / mpeg_sampling_frequencies[hdr->version & 0x1][hdr->sampling] / 8;
+
+    if (hdr->padding)
+        mpeg_frame_size ++;
+
+    return mpeg_frame_size;
+}
+
 static int a2dp_process_push(struct userdata *u) {
     int ret = 0;
     pa_memchunk memchunk;
@@ -2092,11 +2135,16 @@ static int a2dp_process_push(struct userdata *u) {
     pa_assert(u->source);
     pa_assert(u->read_smoother);
 
-    memchunk.memblock = pa_memblock_new(u->core->mempool, u->block_size);
-    memchunk.index = memchunk.length = 0;
+    if (u->write_index == 0) {
+        memchunk.memblock = pa_memblock_new(u->core->mempool, u->block_size);
+        memchunk.index = memchunk.length = 0;
+    } else {
+        memchunk = u->write_memchunk;
+    }
 
     for (;;) {
         pa_bool_t found_tstamp = FALSE;
+        pa_bool_t complete = FALSE;
         pa_usec_t tstamp;
         struct a2dp_info *a2dp;
         struct rtp_header *header;
@@ -2147,7 +2195,10 @@ static int a2dp_process_push(struct userdata *u) {
         to_decode = l - sizeof(*header) - payload_size;
 
         d = pa_memblock_acquire(memchunk.memblock);
-        to_write = memchunk.length = pa_memblock_get_length(memchunk.memblock);
+        if (memchunk.length == 0)
+            memchunk.length = pa_memblock_get_length(memchunk.memblock);
+
+        to_write = memchunk.length;
 
         while (PA_LIKELY(to_decode > 0)) {
             size_t written;
@@ -2177,42 +2228,140 @@ static int a2dp_process_push(struct userdata *u) {
                 pa_assert_fp((size_t) decoded <= to_decode);
                 pa_assert_fp((size_t) decoded == a2dp->frame_length);
                 pa_assert_fp((size_t) written == a2dp->codesize);
+                complete = TRUE;
                 break;
 
             case A2DP_MODE_MPEG: {
-                uint8_t *iec, *orig;
+                uint8_t *iec;
+                const uint8_t *orig;
+                size_t mp3_len, payload_len, swap_len, sample_length;
                 size_t i;
 
-                pa_assert(to_write == 1152 * 4);
-                pa_assert(to_decode < 1728);
-                pa_assert(to_decode < u->link_mtu);
-
-                /* Build IEC frame (16le formatting) */
-                iec = d;
-                iec[0] = 0x72;
-                iec[1] = 0xF8;
-                iec[2] = 0x1F;
-                iec[3] = 0x4E;
-                iec[4] = 0x05;
-                iec[5] = 0x05;
-                iec[6] = to_decode % 256 * 8;
-                iec[7] = to_decode / 256 % 256 * 8;
-                iec += 8;
-
                 orig = p;
-                for (i = 0; i < to_decode / 2; i++) {
-                    *(iec + 1) = *orig;
-                    *iec = *(orig + 1);
-                    iec += 2;
-                    orig += 2;
-                }
+                pa_log_debug("MPEG: first bytes received : %x %x %x %x", (int)orig[0], (int)orig[1], (int)orig[2], (int)orig[3]);
+                if (u->write_index == 0) {
+
+                    if (mp3_synclength(orig[0]<<24 | orig[1]<<16 | orig[2]<<8 | orig[3], &mp3_len, &sample_length)) {
+
+                        mp3_len = mp3_length(orig[0]<<24 | orig[1]<<16 | orig[2]<<8 | orig[3]);
+
+                        /* round to ceiling for payload length, required because of byte swaps */
+                        payload_len = mp3_len + 1;
+                        payload_len >>= 1;
+                        payload_len <<= 1;
+
+                        /* memblock contains enough room for full IEC frame */
+
+                        /* If the frame is not complete, then either
+                              store and wait for another frame
+                       or push the existing data into PA (is it possible or useful, the zero padding would not be added?)
+                          his should not add latency since nothing can be done using an incomplete frame
+                          note that the iec header is added only on a starting frame
+                          */
+                        /* Maximum MPEG frame as seen elsewhere in this code : MPEG_MAX_FRAME_SIZE bytes */
+                        /* Example bluetooth MTU 672 bytes */
+
+                        /* Build IEC header (16le formatting) */
+                        iec = d;
+                        *iec++ = 0x72;
+                        *iec++ = 0xf8;
+                        *iec++ = 0x1f;
+                        *iec++ = 0x4e;
+                        *iec++ = 0x05;
+                        *iec++ = 0x05;
+                        *iec++ = (mp3_len * 8) & 0xFF;
+                        *iec++ = ((mp3_len * 8) >> 8) & 0xFF;
+
+                        /* Take the incoming bytes and swap to 16LE */
+                        swap_len = PA_MIN(to_decode,payload_len);
+                        for (i = 0; i < swap_len / 2; i++) {
+                            *iec++ = *(orig + 1);
+                            *iec++ = *orig;
+                            orig += 2;
+                        }
+                        if (swap_len % 2) {
+                            *iec++ = 0;
+                            *iec++ = *orig++;
+                        }
 
-                memset(iec, 0, to_write - to_decode - 8);
+                        /* FIXME: Should use mp3_len or payload_len? */
+                        /* Payload_len works with packet sent from pulse, but may not work if packet is sent by someone else */
+                        if (payload_len < to_decode) {
+                            /* This +1 is a really dirty hack because A2DP process render has sent a packet of the wrong size */
+                            /* Not implemented, should push packet and continue buffer processing */
+                            /* Not sure this happen in real life though */
+                            pa_assert_not_reached();
+                        } else if(payload_len == to_decode) {
+                            pa_assert(payload_len == (size_t)iec - (size_t)d - 8);
+                            /* If an MP3 packet is processed, pad with zeroes and push */
+                            memset(iec, 0, u->block_size - payload_len - 8);
+                            decoded = to_decode;
+                            written = memchunk.length;
+                            complete = TRUE;
+                        } else /* payload_len > to_decode */ {
+                            /* If we have only a part of an MP3 packet, then keep track of current state for next packet */
+                            pa_log_debug("MPEG: fragmented MP3 frame, mp3_len %u, to_decode %u", mp3_len, to_decode);
+                            u->write_index = to_decode; /* Number of MPEG data written until now */
+                            pa_assert(u->write_index % 2 == 0);
+                            u->write_memchunk = memchunk;
+                            pa_memblock_ref(memchunk.memblock);
+                            decoded = to_decode;
+                            written = decoded;
+                        }
 
-                decoded = to_decode;
-                written = to_write;
-                pa_log_debug("MPEG: decoded: %lu; written: %lu", (unsigned long) decoded, (unsigned long) written);
+                        pa_log_debug("MPEG: decoded: %lu; written: %lu", (unsigned long) decoded, (unsigned long) written);
+                    } else {
+                        pa_log("MPEG: Invalid frame, dropped");
+                        pa_assert_not_reached();
+                    }
+                } else {
+                    const uint8_t *hdr = d;
+                    pa_log_debug("MPEG: first bytes stored : %x %x %x %x %x %x %x %x %x %x %x %x", (int)hdr[0], (int)hdr[1], (int)hdr[2], (int)hdr[3], (int)hdr[4], (int)hdr[5], (int)hdr[6], (int)hdr[7], (int)hdr[8], (int)hdr[9], (int)hdr[10], (int)hdr[11]);
+                    mp3_len = mp3_length(hdr[10]<<24 | hdr[11]<<16 | hdr[8]<<8 | hdr[9]);
+                    pa_log_debug("MPEG: Continuation frame  u->write_index %lu, mp3_len %u, to_decode %u", (long unsigned int)u->write_index, mp3_len, to_decode);
+
+                    /* round to ceiling for payload length, required because of byte swaps */
+                    payload_len = mp3_len + 1;
+                    payload_len >>= 1;
+                    payload_len <<= 1;
+
+                    pa_assert(u->write_index % 2 == 0);
+                    pa_assert(u->write_index < MPEG_MAX_FRAME_SIZE);
+                    pa_assert(mp3_len <= MPEG_MAX_FRAME_SIZE);
+
+                    iec = d;
+                    iec += u->write_index + 8;
+
+                    /* Take the incoming bytes and swap to 16LE */
+                    swap_len = PA_MIN(to_decode, payload_len - u->write_index);
+                    for (i = 0; i < swap_len / 2; i++) {
+                        *iec++ = *(orig + 1);
+                        *iec++ = *orig;
+                        orig += 2;
+                    }
+                    if (swap_len % 2) {
+                        *iec++ = 0;
+                        *iec++ = *orig++;
+                    }
 
+                    if (payload_len == u->write_index + swap_len) {
+                        pa_assert(payload_len == (size_t)iec - (size_t)d - 8);
+                        /* Pad with zeroes */
+                        pa_log_debug("MPEG: fragmented MP3 frame, mp3_len %u, swap_len %u", mp3_len, swap_len);
+                        memset(iec, 0, u->block_size - u->write_index - swap_len - 8);
+                        decoded = to_decode;
+                        written = memchunk.length;
+                        complete = TRUE;
+                    } else if (payload_len > u->write_index + swap_len) {
+                        pa_log_debug("MPEG: to be continued");
+                        u->write_index += swap_len;
+                        pa_assert(u->write_index % 2 == 0);
+                        decoded = to_decode;
+                        written = decoded;
+                    } else {
+                        pa_log("MPEG: Something is wrong, mp3_len %u < u->write_index %u +  swap_len %u", mp3_len, (unsigned int)u->write_index, swap_len);
+                    }
+                }
                 break;
             }
             default:
@@ -2229,17 +2378,23 @@ static int a2dp_process_push(struct userdata *u) {
         }
 
         memchunk.length -= to_write;
-
         pa_memblock_release(memchunk.memblock);
 
-        pa_source_post(u->source, &memchunk);
+        if (complete) {
+            pa_log_debug("MPEG: frame pushed");
+            pa_source_post(u->source, &memchunk);
+            pa_memblock_unref(memchunk.memblock);
+
+            u->write_index = u->write_memchunk.index = u->write_memchunk.length = 0;
+            u->write_memchunk.memblock = NULL;
+        } else {
+            pa_log_debug("MPEG: frame kept for later");
+        }
 
         ret = 1;
         break;
     }
 
-    pa_memblock_unref(memchunk.memblock);
-
     return ret;
 }
 
@@ -3139,7 +3294,7 @@ static int bt_transport_config_a2dp_mpeg(struct userdata *u) {
             pa_assert_not_reached();
     }
 
-    u->block_size = 1152*4;
+    u->block_size = 1152*4; /* Maximum size of an IEC frame */
     u->leftover_bytes = 0;
 
     pa_log_info("MPEG selected\n");
-- 
1.7.5.4



[Index of Archives]     [Linux Audio Users]     [AMD Graphics]     [Linux USB Devel]     [Linux Audio Users]     [Yosemite News]     [Linux Kernel]     [Linux SCSI]

  Powered by Linux