1 files changed, 248 insertions, 57 deletions

diff --git a/media/libstagefright/wifi-display/source/TSPacketizer.cpp b/media/libstagefright/wifi-display/source/TSPacketizer.cpp
index a5679ad..edcc087 100644
--- a/media/libstagefright/wifi-display/source/TSPacketizer.cpp
+++ b/media/libstagefright/wifi-display/source/TSPacketizer.cpp
@@ -58,6 +58,7 @@ struct TSPacketizer::Track : public RefBase {
     sp<ABuffer> descriptorAt(size_t index) const;
 
     void finalize();
+    void extractCSDIfNecessary();
 
 protected:
     virtual ~Track();
@@ -77,6 +78,7 @@ private:
 
     bool mAudioLacksATDSHeaders;
     bool mFinalized;
+    bool mExtractedCSD;
 
     DISALLOW_EVIL_CONSTRUCTORS(Track);
 };
@@ -90,14 +92,21 @@ TSPacketizer::Track::Track(
       mStreamID(streamID),
       mContinuityCounter(0),
       mAudioLacksATDSHeaders(false),
-      mFinalized(false) {
+      mFinalized(false),
+      mExtractedCSD(false) {
     CHECK(format->findString("mime", &mMIME));
+}
+
+void TSPacketizer::Track::extractCSDIfNecessary() {
+    if (mExtractedCSD) {
+        return;
+    }
 
     if (!strcasecmp(mMIME.c_str(), MEDIA_MIMETYPE_VIDEO_AVC)
             || !strcasecmp(mMIME.c_str(), MEDIA_MIMETYPE_AUDIO_AAC)) {
         for (size_t i = 0;; ++i) {
             sp<ABuffer> csd;
-            if (!format->findBuffer(StringPrintf("csd-%d", i).c_str(), &csd)) {
+            if (!mFormat->findBuffer(StringPrintf("csd-%d", i).c_str(), &csd)) {
                 break;
             }
 
@@ -111,6 +120,8 @@ TSPacketizer::Track::Track(
             }
         }
     }
+
+    mExtractedCSD = true;
 }
 
 TSPacketizer::Track::~Track() {
@@ -250,12 +261,24 @@ void TSPacketizer::Track::finalize() {
             data[0] = 40;  // descriptor_tag
             data[1] = 4;  // descriptor_length
 
-            CHECK_EQ(mCSD.size(), 1u);
-            const sp<ABuffer> &sps = mCSD.itemAt(0);
-            CHECK(!memcmp("\x00\x00\x00\x01", sps->data(), 4));
-            CHECK_GE(sps->size(), 7u);
-            // profile_idc, constraint_set*, level_idc
-            memcpy(&data[2], sps->data() + 4, 3);
+            if (mCSD.size() > 0) {
+                CHECK_GE(mCSD.size(), 1u);
+                const sp<ABuffer> &sps = mCSD.itemAt(0);
+                CHECK(!memcmp("\x00\x00\x00\x01", sps->data(), 4));
+                CHECK_GE(sps->size(), 7u);
+                // profile_idc, constraint_set*, level_idc
+                memcpy(&data[2], sps->data() + 4, 3);
+            } else {
+                int32_t profileIdc, levelIdc, constraintSet;
+                CHECK(mFormat->findInt32("profile-idc", &profileIdc));
+                CHECK(mFormat->findInt32("level-idc", &levelIdc));
+                CHECK(mFormat->findInt32("constraint-set", &constraintSet));
+                CHECK_GE(profileIdc, 0u);
+                CHECK_GE(levelIdc, 0u);
+                data[2] = profileIdc;    // profile_idc
+                data[3] = constraintSet; // constraint_set*
+                data[4] = levelIdc;      // level_idc
+            }
 
             // AVC_still_present=0, AVC_24_hour_picture_flag=0, reserved
             data[5] = 0x3f;
@@ -319,10 +342,38 @@ void TSPacketizer::Track::finalize() {
 
 ////////////////////////////////////////////////////////////////////////////////
 
-TSPacketizer::TSPacketizer()
-    : mPATContinuityCounter(0),
+TSPacketizer::TSPacketizer(uint32_t flags)
+    : mFlags(flags),
+      mPATContinuityCounter(0),
       mPMTContinuityCounter(0) {
     initCrcTable();
+
+    if (flags & (EMIT_HDCP20_DESCRIPTOR | EMIT_HDCP21_DESCRIPTOR)) {
+        int32_t hdcpVersion;
+        if (flags & EMIT_HDCP20_DESCRIPTOR) {
+            CHECK(!(flags & EMIT_HDCP21_DESCRIPTOR));
+            hdcpVersion = 0x20;
+        } else {
+            CHECK(!(flags & EMIT_HDCP20_DESCRIPTOR));
+
+            // HDCP2.0 _and_ HDCP 2.1 specs say to set the version
+            // inside the HDCP descriptor to 0x20!!!
+            hdcpVersion = 0x20;
+        }
+
+        // HDCP descriptor
+        sp<ABuffer> descriptor = new ABuffer(7);
+        uint8_t *data = descriptor->data();
+        data[0] = 0x05;  // descriptor_tag
+        data[1] = 5;  // descriptor_length
+        data[2] = 'H';
+        data[3] = 'D';
+        data[4] = 'C';
+        data[5] = 'P';
+        data[6] = hdcpVersion;
+
+        mProgramInfoDescriptors.push_back(descriptor);
+    }
 }
 
 TSPacketizer::~TSPacketizer() {
@@ -388,6 +439,17 @@ ssize_t TSPacketizer::addTrack(const sp<AMessage> &format) {
     return mTracks.add(track);
 }
 
+status_t TSPacketizer::extractCSDIfNecessary(size_t trackIndex) {
+    if (trackIndex >= mTracks.size()) {
+        return -ERANGE;
+    }
+
+    const sp<Track> &track = mTracks.itemAt(trackIndex);
+    track->extractCSDIfNecessary();
+
+    return OK;
+}
+
 status_t TSPacketizer::packetize(
         size_t trackIndex,
         const sp<ABuffer> &_accessUnit,
@@ -452,16 +514,121 @@ status_t TSPacketizer::packetize(
     // reserved = b1
     // the first fragment of "buffer" follows
 
+    // Each transport packet (except for the last one contributing to the PES
+    // payload) must contain a multiple of 16 bytes of payload per HDCP spec.
+    bool alignPayload =
+        (mFlags & (EMIT_HDCP20_DESCRIPTOR | EMIT_HDCP21_DESCRIPTOR));
+
+    /*
+       a) The very first PES transport stream packet contains
+
+       4 bytes of TS header
+       ... padding
+       14 bytes of static PES header
+       PES_private_data_len + 1 bytes (only if PES_private_data_len > 0)
+       numStuffingBytes bytes
+
+       followed by the payload
+
+       b) Subsequent PES transport stream packets contain
+
+       4 bytes of TS header
+       ... padding
+
+       followed by the payload
+    */
+
     size_t PES_packet_length = accessUnit->size() + 8 + numStuffingBytes;
     if (PES_private_data_len > 0) {
         PES_packet_length += PES_private_data_len + 1;
     }
 
-    size_t numTSPackets;
-    if (PES_packet_length <= 178) {
-        numTSPackets = 1;
-    } else {
-        numTSPackets = 1 + ((PES_packet_length - 178) + 183) / 184;
+    size_t numTSPackets = 1;
+
+    {
+        // Make sure the PES header fits into a single TS packet:
+        size_t PES_header_size = 14 + numStuffingBytes;
+        if (PES_private_data_len > 0) {
+            PES_header_size += PES_private_data_len + 1;
+        }
+
+        CHECK_LE(PES_header_size, 188u - 4u);
+
+        size_t sizeAvailableForPayload = 188 - 4 - PES_header_size;
+        size_t numBytesOfPayload = accessUnit->size();
+
+        if (numBytesOfPayload > sizeAvailableForPayload) {
+            numBytesOfPayload = sizeAvailableForPayload;
+
+            if (alignPayload && numBytesOfPayload > 16) {
+                numBytesOfPayload -= (numBytesOfPayload % 16);
+            }
+        }
+
+        size_t numPaddingBytes = sizeAvailableForPayload - numBytesOfPayload;
+        ALOGV("packet 1 contains %zd padding bytes and %zd bytes of payload",
+              numPaddingBytes, numBytesOfPayload);
+
+        size_t numBytesOfPayloadRemaining = accessUnit->size() - numBytesOfPayload;
+
+#if 0
+        // The following hopefully illustrates the logic that led to the
+        // more efficient computation in the #else block...
+
+        while (numBytesOfPayloadRemaining > 0) {
+            size_t sizeAvailableForPayload = 188 - 4;
+
+            size_t numBytesOfPayload = numBytesOfPayloadRemaining;
+
+            if (numBytesOfPayload > sizeAvailableForPayload) {
+                numBytesOfPayload = sizeAvailableForPayload;
+
+                if (alignPayload && numBytesOfPayload > 16) {
+                    numBytesOfPayload -= (numBytesOfPayload % 16);
+                }
+            }
+
+            size_t numPaddingBytes = sizeAvailableForPayload - numBytesOfPayload;
+            ALOGI("packet %zd contains %zd padding bytes and %zd bytes of payload",
+                    numTSPackets + 1, numPaddingBytes, numBytesOfPayload);
+
+            numBytesOfPayloadRemaining -= numBytesOfPayload;
+            ++numTSPackets;
+        }
+#else
+        // This is how many bytes of payload each subsequent TS packet
+        // can contain at most.
+        sizeAvailableForPayload = 188 - 4;
+        size_t sizeAvailableForAlignedPayload = sizeAvailableForPayload;
+        if (alignPayload) {
+            // We're only going to use a subset of the available space
+            // since we need to make each fragment a multiple of 16 in size.
+            sizeAvailableForAlignedPayload -=
+                (sizeAvailableForAlignedPayload % 16);
+        }
+
+        size_t numFullTSPackets =
+            numBytesOfPayloadRemaining / sizeAvailableForAlignedPayload;
+
+        numTSPackets += numFullTSPackets;
+
+        numBytesOfPayloadRemaining -=
+            numFullTSPackets * sizeAvailableForAlignedPayload;
+
+        // numBytesOfPayloadRemaining < sizeAvailableForAlignedPayload
+        if (numFullTSPackets == 0 && numBytesOfPayloadRemaining > 0) {
+            // There wasn't enough payload left to form a full aligned payload,
+            // the last packet doesn't have to be aligned.
+            ++numTSPackets;
+        } else if (numFullTSPackets > 0
+                && numBytesOfPayloadRemaining
+                    + sizeAvailableForAlignedPayload > sizeAvailableForPayload) {
+            // The last packet emitted had a full aligned payload and together
+            // with the bytes remaining does exceed the unaligned payload
+            // size, so we need another packet.
+            ++numTSPackets;
+        }
+#endif
     }
 
     if (flags & EMIT_PAT_AND_PMT) {
@@ -564,8 +731,9 @@ status_t TSPacketizer::packetize(
         // reserved = b111
         // PCR_PID = kPCR_PID (13 bits)
         // reserved = b1111
-        // program_info_length = 0x000
-        //   one or more elementary stream descriptions follow:
+        // program_info_length = 0x???
+        //   program_info_descriptors follow
+        // one or more elementary stream descriptions follow:
         //   stream_type = 0x??
         //   reserved = b111
         //   elementary_PID = b? ???? ???? ???? (13 bits)
@@ -597,8 +765,21 @@ status_t TSPacketizer::packetize(
         *ptr++ = 0x00;
         *ptr++ = 0xe0 | (kPID_PCR >> 8);
         *ptr++ = kPID_PCR & 0xff;
-        *ptr++ = 0xf0;
-        *ptr++ = 0x00;
+
+        size_t program_info_length = 0;
+        for (size_t i = 0; i < mProgramInfoDescriptors.size(); ++i) {
+            program_info_length += mProgramInfoDescriptors.itemAt(i)->size();
+        }
+
+        CHECK_LT(program_info_length, 0x400);
+        *ptr++ = 0xf0 | (program_info_length >> 8);
+        *ptr++ = (program_info_length & 0xff);
+
+        for (size_t i = 0; i < mProgramInfoDescriptors.size(); ++i) {
+            const sp<ABuffer> &desc = mProgramInfoDescriptors.itemAt(i);
+            memcpy(ptr, desc->data(), desc->size());
+            ptr += desc->size();
+        }
 
         for (size_t i = 0; i < mTracks.size(); ++i) {
             const sp<Track> &track = mTracks.itemAt(i);
@@ -691,8 +872,6 @@ status_t TSPacketizer::packetize(
 
     uint64_t PTS = (timeUs * 9ll) / 100ll;
 
-    bool padding = (PES_packet_length < (188 - 10));
-
     if (PES_packet_length >= 65536) {
         // This really should only happen for video.
         CHECK(track->isVideo());
@@ -701,19 +880,37 @@ status_t TSPacketizer::packetize(
         PES_packet_length = 0;
     }
 
+    size_t sizeAvailableForPayload = 188 - 4 - 14 - numStuffingBytes;
+    if (PES_private_data_len > 0) {
+        sizeAvailableForPayload -= PES_private_data_len + 1;
+    }
+
+    size_t copy = accessUnit->size();
+
+    if (copy > sizeAvailableForPayload) {
+        copy = sizeAvailableForPayload;
+
+        if (alignPayload && copy > 16) {
+            copy -= (copy % 16);
+        }
+    }
+
+    size_t numPaddingBytes = sizeAvailableForPayload - copy;
+
     uint8_t *ptr = packetDataStart;
     *ptr++ = 0x47;
     *ptr++ = 0x40 | (track->PID() >> 8);
     *ptr++ = track->PID() & 0xff;
-    *ptr++ = (padding ? 0x30 : 0x10) | track->incrementContinuityCounter();
 
-    if (padding) {
-        size_t paddingSize = 188 - 10 - PES_packet_length;
-        *ptr++ = paddingSize - 1;
-        if (paddingSize >= 2) {
+    *ptr++ = (numPaddingBytes > 0 ? 0x30 : 0x10)
+                | track->incrementContinuityCounter();
+
+    if (numPaddingBytes > 0) {
+        *ptr++ = numPaddingBytes - 1;
+        if (numPaddingBytes >= 2) {
             *ptr++ = 0x00;
-            memset(ptr, 0xff, paddingSize - 2);
-            ptr += paddingSize - 2;
+            memset(ptr, 0xff, numPaddingBytes - 2);
+            ptr += numPaddingBytes - 2;
         }
     }
 
@@ -749,25 +946,14 @@ status_t TSPacketizer::packetize(
         *ptr++ = 0xff;
     }
 
-    // 18 bytes of TS/PES header leave 188 - 18 = 170 bytes for the payload
-
-    size_t sizeLeft = packetDataStart + 188 - ptr;
-    size_t copy = accessUnit->size();
-    if (copy > sizeLeft) {
-        copy = sizeLeft;
-    }
-
     memcpy(ptr, accessUnit->data(), copy);
     ptr += copy;
-    CHECK_EQ(sizeLeft, copy);
-    memset(ptr, 0xff, sizeLeft - copy);
 
+    CHECK_EQ(ptr, packetDataStart + 188);
     packetDataStart += 188;
 
     size_t offset = copy;
     while (offset < accessUnit->size()) {
-        bool padding = (accessUnit->size() - offset) < (188 - 4);
-
         // for subsequent fragments of "buffer":
         // 0x47
         // transport_error_indicator = b0
@@ -779,35 +965,40 @@ status_t TSPacketizer::packetize(
         // continuity_counter = b????
         // the fragment of "buffer" follows.
 
+        size_t sizeAvailableForPayload = 188 - 4;
+
+        size_t copy = accessUnit->size() - offset;
+
+        if (copy > sizeAvailableForPayload) {
+            copy = sizeAvailableForPayload;
+
+            if (alignPayload && copy > 16) {
+                copy -= (copy % 16);
+            }
+        }
+
+        size_t numPaddingBytes = sizeAvailableForPayload - copy;
+
         uint8_t *ptr = packetDataStart;
         *ptr++ = 0x47;
         *ptr++ = 0x00 | (track->PID() >> 8);
         *ptr++ = track->PID() & 0xff;
 
-        *ptr++ = (padding ? 0x30 : 0x10) | track->incrementContinuityCounter();
+        *ptr++ = (numPaddingBytes > 0 ? 0x30 : 0x10)
+                    | track->incrementContinuityCounter();
 
-        if (padding) {
-            size_t paddingSize = 188 - 4 - (accessUnit->size() - offset);
-            *ptr++ = paddingSize - 1;
-            if (paddingSize >= 2) {
+        if (numPaddingBytes > 0) {
+            *ptr++ = numPaddingBytes - 1;
+            if (numPaddingBytes >= 2) {
                 *ptr++ = 0x00;
-                memset(ptr, 0xff, paddingSize - 2);
-                ptr += paddingSize - 2;
+                memset(ptr, 0xff, numPaddingBytes - 2);
+                ptr += numPaddingBytes - 2;
             }
         }
 
-        // 4 bytes of TS header leave 188 - 4 = 184 bytes for the payload
-
-        size_t sizeLeft = packetDataStart + 188 - ptr;
-        size_t copy = accessUnit->size() - offset;
-        if (copy > sizeLeft) {
-            copy = sizeLeft;
-        }
-
         memcpy(ptr, accessUnit->data() + offset, copy);
         ptr += copy;
-        CHECK_EQ(sizeLeft, copy);
-        memset(ptr, 0xff, sizeLeft - copy);
+        CHECK_EQ(ptr, packetDataStart + 188);
 
         offset += copy;
         packetDataStart += 188;