diff --git a/cmds/stagefright/codec.cpp b/cmds/stagefright/codec.cpp
index 5a43829..f3370a5 100644
--- a/cmds/stagefright/codec.cpp
+++ b/cmds/stagefright/codec.cpp
@@ -50,8 +50,6 @@
 
 struct CodecState {
     sp<MediaCodec> mCodec;
-    Vector<sp<ABuffer> > mCSD;
-    size_t mCSDIndex;
     Vector<sp<ABuffer> > mInBuffers;
     Vector<sp<ABuffer> > mOutBuffers;
     bool mSignalledInputEOS;
@@ -126,19 +124,8 @@
 
         CHECK_EQ(err, (status_t)OK);
 
-        size_t j = 0;
-        sp<ABuffer> buffer;
-        while (format->findBuffer(StringPrintf("csd-%d", j).c_str(), &buffer)) {
-            state->mCSD.push_back(buffer);
-
-            ++j;
-        }
-
-        state->mCSDIndex = 0;
         state->mSignalledInputEOS = false;
         state->mSawOutputEOS = false;
-
-        ALOGV("got %d pieces of codec specific data.", state->mCSD.size());
     }
 
     CHECK(!stateByTrack.isEmpty());
@@ -157,28 +144,6 @@
 
         ALOGV("got %d input and %d output buffers",
               state->mInBuffers.size(), state->mOutBuffers.size());
-
-        while (state->mCSDIndex < state->mCSD.size()) {
-            size_t index;
-            status_t err = codec->dequeueInputBuffer(&index, -1ll);
-            CHECK_EQ(err, (status_t)OK);
-
-            const sp<ABuffer> &srcBuffer =
-                state->mCSD.itemAt(state->mCSDIndex++);
-
-            const sp<ABuffer> &buffer = state->mInBuffers.itemAt(index);
-
-            memcpy(buffer->data(), srcBuffer->data(), srcBuffer->size());
-
-            err = codec->queueInputBuffer(
-                    index,
-                    0 /* offset */,
-                    srcBuffer->size(),
-                    0ll /* timeUs */,
-                    MediaCodec::BUFFER_FLAG_CODECCONFIG);
-
-            CHECK_EQ(err, (status_t)OK);
-        }
     }
 
     bool sawInputEOS = false;
diff --git a/include/media/stagefright/MediaCodec.h b/include/media/stagefright/MediaCodec.h
index d09049e..d96007b 100644
--- a/include/media/stagefright/MediaCodec.h
+++ b/include/media/stagefright/MediaCodec.h
@@ -186,6 +186,8 @@
 
     sp<ICrypto> mCrypto;
 
+    List<sp<ABuffer> > mCSD;
+
     MediaCodec(const sp<ALooper> &looper);
 
     static status_t PostAndAwaitResponse(
@@ -205,6 +207,9 @@
     bool handleDequeueOutputBuffer(uint32_t replyID, bool newRequest = false);
     void cancelPendingDequeueOperations();
 
+    void extractCSD(const sp<AMessage> &format);
+    status_t queueCSDInputBuffer(size_t bufferIndex);
+
     DISALLOW_EVIL_CONSTRUCTORS(MediaCodec);
 };
 
diff --git a/media/libstagefright/MediaCodec.cpp b/media/libstagefright/MediaCodec.cpp
index 2df0dd2..e032cfc 100644
--- a/media/libstagefright/MediaCodec.cpp
+++ b/media/libstagefright/MediaCodec.cpp
@@ -646,6 +646,28 @@
                         break;
                     }
 
+                    if (!mCSD.empty()) {
+                        ssize_t index = dequeuePortBuffer(kPortIndexInput);
+                        CHECK_GE(index, 0);
+
+                        // If codec specific data had been specified as
+                        // part of the format in the call to configure and
+                        // if there's more csd left, we submit it here
+                        // clients only get access to input buffers once
+                        // this data has been exhausted.
+
+                        status_t err = queueCSDInputBuffer(index);
+
+                        if (err != OK) {
+                            ALOGE("queueCSDInputBuffer failed w/ error %d",
+                                  err);
+
+                            mFlags |= kFlagStickyError;
+                            cancelPendingDequeueOperations();
+                        }
+                        break;
+                    }
+
                     if (mFlags & kFlagDequeueInputPending) {
                         CHECK(handleDequeueInputBuffer(mDequeueInputReplyID));
 
@@ -811,6 +833,8 @@
                 format->setInt32("encoder", true);
             }
 
+            extractCSD(format);
+
             mCodec->initiateConfigureComponent(format);
             break;
         }
@@ -1107,6 +1131,54 @@
     }
 }
 
+void MediaCodec::extractCSD(const sp<AMessage> &format) {
+    mCSD.clear();
+
+    size_t i = 0;
+    for (;;) {
+        sp<ABuffer> csd;
+        if (!format->findBuffer(StringPrintf("csd-%u", i).c_str(), &csd)) {
+            break;
+        }
+
+        mCSD.push_back(csd);
+        ++i;
+    }
+
+    ALOGV("Found %u pieces of codec specific data.", mCSD.size());
+}
+
+status_t MediaCodec::queueCSDInputBuffer(size_t bufferIndex) {
+    CHECK(!mCSD.empty());
+
+    BufferInfo *info =
+        &mPortBuffers[kPortIndexInput].editItemAt(bufferIndex);
+
+    sp<ABuffer> csd = *mCSD.begin();
+    mCSD.erase(mCSD.begin());
+
+    const sp<ABuffer> &codecInputData =
+        (mCrypto != NULL) ? info->mEncryptedData : info->mData;
+
+    if (csd->size() > codecInputData->capacity()) {
+        return -EINVAL;
+    }
+
+    memcpy(codecInputData->data(), csd->data(), csd->size());
+
+    AString errorDetailMsg;
+
+    sp<AMessage> msg = new AMessage(kWhatQueueInputBuffer, id());
+    msg->setSize("index", bufferIndex);
+    msg->setSize("offset", 0);
+    msg->setSize("size", csd->size());
+    msg->setInt64("timeUs", 0ll);
+    msg->setInt32("flags", BUFFER_FLAG_CODECCONFIG);
+    msg->setPointer("errorDetailMsg", &errorDetailMsg);
+
+    return onQueueInputBuffer(msg);
+}
+
 void MediaCodec::setState(State newState) {
     if (newState == INITIALIZED) {
         delete mSoftRenderer;