Camera2: Move preview callback processing to its own thread.

To reduce delays for HAL callbacks, manage preview callbacks in their
own thread.

Bug: 6243944
Change-Id: I7bef56949ac889ffce4e031bf40291a771a46f3e

5 files changed, 410 insertions, 223 deletions

diff --git a/services/camera/libcameraservice/Android.mk b/services/camera/libcameraservice/Android.mk
index 9898a70..1370c62 100644
--- a/services/camera/libcameraservice/Android.mk
+++ b/services/camera/libcameraservice/Android.mk
@@ -14,7 +14,8 @@ LOCAL_SRC_FILES:=               \
     camera2/CameraMetadata.cpp \
     camera2/Parameters.cpp \
     camera2/FrameProcessor.cpp \
-    camera2/CaptureProcessor.cpp
+    camera2/CaptureProcessor.cpp \
+    camera2/CallbackProcessor.cpp
 
 LOCAL_SHARED_LIBRARIES:= \
     libui \
diff --git a/services/camera/libcameraservice/Camera2Client.cpp b/services/camera/libcameraservice/Camera2Client.cpp
index d296445..acd290d 100644
--- a/services/camera/libcameraservice/Camera2Client.cpp
+++ b/services/camera/libcameraservice/Camera2Client.cpp
@@ -55,8 +55,6 @@ Camera2Client::Camera2Client(const sp<CameraService>& cameraService,
         mSharedCameraClient(cameraClient),
         mParameters(cameraId, cameraFacing),
         mPreviewStreamId(NO_STREAM),
-        mCallbackStreamId(NO_STREAM),
-        mCallbackHeapId(0),
         mRecordingStreamId(NO_STREAM),
         mRecordingHeapCount(kDefaultRecordingHeapCount)
 {
@@ -112,6 +110,11 @@ status_t Camera2Client::initialize(camera_module_t *module)
             String8::format("Camera2Client[%d]::CaptureProcessor", mCameraId);
     mCaptureProcessor->run(captureThreadName.string());
 
+    mCallbackProcessor = new CallbackProcessor(this);
+    String8 callbackThreadName =
+            String8::format("Camera2Client[%d]::CallbackProcessor", mCameraId);
+    mCallbackProcessor->run(callbackThreadName.string());
+
     if (gLogLevel >= 1) {
         ALOGD("%s: Default parameters converted from camera %d:", __FUNCTION__,
               mCameraId);
@@ -370,10 +373,7 @@ void Camera2Client::disconnect() {
         mRecordingStreamId = NO_STREAM;
     }
 
-    if (mCallbackStreamId != NO_STREAM) {
-        mDevice->deleteStream(mCallbackStreamId);
-        mCallbackStreamId = NO_STREAM;
-    }
+    mCallbackProcessor->deleteStream();
 
     mDevice.clear();
     SharedParameters::Lock l(mParameters);
@@ -616,7 +616,7 @@ status_t Camera2Client::startPreviewL(Parameters &params, bool restart) {
     bool callbacksEnabled = params.previewCallbackFlags &
         CAMERA_FRAME_CALLBACK_FLAG_ENABLE_MASK;
     if (callbacksEnabled) {
-        res = updateCallbackStream(params);
+        res = mCallbackProcessor->updateStream(params);
         if (res != OK) {
             ALOGE("%s: Camera %d: Unable to update callback stream: %s (%d)",
                     __FUNCTION__, mCameraId, strerror(-res), res);
@@ -635,7 +635,7 @@ status_t Camera2Client::startPreviewL(Parameters &params, bool restart) {
 
     if (callbacksEnabled) {
         uint8_t outputStreams[2] =
-                { mPreviewStreamId, mCallbackStreamId };
+                { mPreviewStreamId, mCallbackProcessor->getStreamId() };
         res = mPreviewRequest.update(
                 ANDROID_REQUEST_OUTPUT_STREAMS,
                 outputStreams, 2);
@@ -799,7 +799,7 @@ status_t Camera2Client::startRecordingL(Parameters &params, bool restart) {
     bool callbacksEnabled = params.previewCallbackFlags &
         CAMERA_FRAME_CALLBACK_FLAG_ENABLE_MASK;
     if (callbacksEnabled) {
-        res = updateCallbackStream(params);
+        res = mCallbackProcessor->updateStream(params);
         if (res != OK) {
             ALOGE("%s: Camera %d: Unable to update callback stream: %s (%d)",
                     __FUNCTION__, mCameraId, strerror(-res), res);
@@ -818,7 +818,8 @@ status_t Camera2Client::startRecordingL(Parameters &params, bool restart) {
 
     if (callbacksEnabled) {
         uint8_t outputStreams[3] =
-                { mPreviewStreamId, mRecordingStreamId, mCallbackStreamId };
+                { mPreviewStreamId, mRecordingStreamId,
+                  mCallbackProcessor->getStreamId() };
         res = mRecordingRequest.update(
                 ANDROID_REQUEST_OUTPUT_STREAMS,
                 outputStreams, 3);
@@ -1057,28 +1058,41 @@ status_t Camera2Client::takePicture(int msgType) {
             (recordingEnabled ? 0x1 : 0x0);
     switch ( streamSwitch ) {
         case 0: { // No recording, callbacks
-            uint8_t streamIds[2] = { mPreviewStreamId, captureStreamId };
+            uint8_t streamIds[2] = {
+                mPreviewStreamId,
+                captureStreamId
+            };
             res = mCaptureRequest.update(ANDROID_REQUEST_OUTPUT_STREAMS,
                     streamIds, 2);
             break;
         }
         case 1: { // Recording
-            uint8_t streamIds[3] = { mPreviewStreamId, mRecordingStreamId,
-                                     captureStreamId };
+            uint8_t streamIds[3] = {
+                mPreviewStreamId,
+                mRecordingStreamId,
+                captureStreamId
+            };
             res = mCaptureRequest.update(ANDROID_REQUEST_OUTPUT_STREAMS,
                     streamIds, 3);
             break;
         }
         case 2: { // Callbacks
-            uint8_t streamIds[3] = { mPreviewStreamId, mCallbackStreamId,
-                                     captureStreamId };
+            uint8_t streamIds[3] = {
+                mPreviewStreamId,
+                mCallbackProcessor->getStreamId(),
+                captureStreamId
+            };
             res = mCaptureRequest.update(ANDROID_REQUEST_OUTPUT_STREAMS,
                     streamIds, 3);
             break;
         }
         case 3: { // Both
-            uint8_t streamIds[4] = { mPreviewStreamId, mCallbackStreamId,
-                                     mRecordingStreamId, captureStreamId };
+            uint8_t streamIds[4] = {
+                mPreviewStreamId,
+                mCallbackProcessor->getStreamId(),
+                mRecordingStreamId,
+                captureStreamId
+            };
             res = mCaptureRequest.update(ANDROID_REQUEST_OUTPUT_STREAMS,
                     streamIds, 4);
             break;
@@ -1532,133 +1546,6 @@ void Camera2Client::SharedCameraClient::clear() {
     mCameraClient.clear();
 }
 
-void Camera2Client::onCallbackAvailable() {
-    ATRACE_CALL();
-    status_t res;
-    ALOGV("%s: Camera %d: Preview callback available", __FUNCTION__, mCameraId);
-
-    int callbackHeapId;
-    sp<Camera2Heap> callbackHeap;
-    size_t heapIdx;
-
-    CpuConsumer::LockedBuffer imgBuffer;
-    ALOGV("%s: Getting buffer", __FUNCTION__);
-    res = mCallbackConsumer->lockNextBuffer(&imgBuffer);
-    if (res != OK) {
-        ALOGE("%s: Camera %d: Error receiving next callback buffer: "
-                "%s (%d)", __FUNCTION__, mCameraId, strerror(-res), res);
-        return;
-    }
-
-    {
-        SharedParameters::Lock l(mParameters);
-
-        if ( l.mParameters.state != Parameters::PREVIEW
-                && l.mParameters.state != Parameters::RECORD
-                && l.mParameters.state != Parameters::VIDEO_SNAPSHOT) {
-            ALOGV("%s: Camera %d: No longer streaming",
-                    __FUNCTION__, mCameraId);
-            mCallbackConsumer->unlockBuffer(imgBuffer);
-            return;
-        }
-
-        if (! (l.mParameters.previewCallbackFlags &
-                CAMERA_FRAME_CALLBACK_FLAG_ENABLE_MASK) ) {
-            ALOGV("%s: No longer enabled, dropping", __FUNCTION__);
-            mCallbackConsumer->unlockBuffer(imgBuffer);
-            return;
-        }
-        if ((l.mParameters.previewCallbackFlags &
-                        CAMERA_FRAME_CALLBACK_FLAG_ONE_SHOT_MASK) &&
-                !l.mParameters.previewCallbackOneShot) {
-            ALOGV("%s: One shot mode, already sent, dropping", __FUNCTION__);
-            mCallbackConsumer->unlockBuffer(imgBuffer);
-            return;
-        }
-
-        if (imgBuffer.format != l.mParameters.previewFormat) {
-            ALOGE("%s: Camera %d: Unexpected format for callback: "
-                    "%x, expected %x", __FUNCTION__, mCameraId,
-                    imgBuffer.format, l.mParameters.previewFormat);
-            mCallbackConsumer->unlockBuffer(imgBuffer);
-            return;
-        }
-
-        size_t bufferSize = calculateBufferSize(imgBuffer.width, imgBuffer.height,
-                imgBuffer.format, imgBuffer.stride);
-        size_t currentBufferSize = (mCallbackHeap == 0) ?
-                0 : (mCallbackHeap->mHeap->getSize() / kCallbackHeapCount);
-        if (bufferSize != currentBufferSize) {
-            mCallbackHeap.clear();
-            mCallbackHeap = new Camera2Heap(bufferSize, kCallbackHeapCount,
-                    "Camera2Client::CallbackHeap");
-            if (mCallbackHeap->mHeap->getSize() == 0) {
-                ALOGE("%s: Camera %d: Unable to allocate memory for callbacks",
-                        __FUNCTION__, mCameraId);
-                mCallbackConsumer->unlockBuffer(imgBuffer);
-                return;
-            }
-
-            mCallbackHeapHead = 0;
-            mCallbackHeapFree = kCallbackHeapCount;
-            mCallbackHeapId++;
-        }
-
-        if (mCallbackHeapFree == 0) {
-            ALOGE("%s: Camera %d: No free callback buffers, dropping frame",
-                    __FUNCTION__, mCameraId);
-            mCallbackConsumer->unlockBuffer(imgBuffer);
-            return;
-        }
-        heapIdx = mCallbackHeapHead;
-        callbackHeap = mCallbackHeap;
-        callbackHeapId = mCallbackHeapId;
-
-        mCallbackHeapHead = (mCallbackHeapHead + 1) & kCallbackHeapCount;
-        mCallbackHeapFree--;
-
-        // TODO: Get rid of this memcpy by passing the gralloc queue all the way
-        // to app
-
-        ssize_t offset;
-        size_t size;
-        sp<IMemoryHeap> heap =
-            mCallbackHeap->mBuffers[heapIdx]->getMemory(&offset,
-                    &size);
-        uint8_t *data = (uint8_t*)heap->getBase() + offset;
-        memcpy(data, imgBuffer.data, bufferSize);
-
-        ALOGV("%s: Freeing buffer", __FUNCTION__);
-        mCallbackConsumer->unlockBuffer(imgBuffer);
-
-        // In one-shot mode, stop sending callbacks after the first one
-        if (l.mParameters.previewCallbackFlags &
-                CAMERA_FRAME_CALLBACK_FLAG_ONE_SHOT_MASK) {
-            ALOGV("%s: clearing oneshot", __FUNCTION__);
-            l.mParameters.previewCallbackOneShot = false;
-        }
-    }
-
-    // Call outside parameter lock to allow re-entrancy from notification
-    {
-        SharedCameraClient::Lock l(mSharedCameraClient);
-        if (l.mCameraClient != 0) {
-            ALOGV("%s: Camera %d: Invoking client data callback",
-                    __FUNCTION__, mCameraId);
-            l.mCameraClient->dataCallback(CAMERA_MSG_PREVIEW_FRAME,
-                    callbackHeap->mBuffers[heapIdx], NULL);
-        }
-    }
-
-    SharedParameters::Lock l(mParameters);
-    // Only increment free if we're still using the same heap
-    if (mCallbackHeapId == callbackHeapId) {
-        mCallbackHeapFree++;
-    }
-
-    ALOGV("%s: exit", __FUNCTION__);
-}
-
 void Camera2Client::onRecordingFrameAvailable() {
     ATRACE_CALL();
     status_t res;
@@ -1885,63 +1772,6 @@ status_t Camera2Client::updatePreviewRequest(const Parameters &params) {
     return OK;
 }
 
-status_t Camera2Client::updateCallbackStream(const Parameters &params) {
-    status_t res;
-
-    if (mCallbackConsumer == 0) {
-        // Create CPU buffer queue endpoint
-        mCallbackConsumer = new CpuConsumer(kCallbackHeapCount);
-        mCallbackWaiter = new CallbackWaiter(this);
-        mCallbackConsumer->setFrameAvailableListener(mCallbackWaiter);
-        mCallbackConsumer->setName(String8("Camera2Client::CallbackConsumer"));
-        mCallbackWindow = new SurfaceTextureClient(
-            mCallbackConsumer->getProducerInterface());
-    }
-
-    if (mCallbackStreamId != NO_STREAM) {
-        // Check if stream parameters have to change
-        uint32_t currentWidth, currentHeight, currentFormat;
-        res = mDevice->getStreamInfo(mCallbackStreamId,
-                &currentWidth, &currentHeight, &currentFormat);
-        if (res != OK) {
-            ALOGE("%s: Camera %d: Error querying callback output stream info: "
-                    "%s (%d)", __FUNCTION__, mCameraId, strerror(-res), res);
-            return res;
-        }
-        if (currentWidth != (uint32_t)params.previewWidth ||
-                currentHeight != (uint32_t)params.previewHeight ||
-                currentFormat != (uint32_t)params.previewFormat) {
-            // Since size should only change while preview is not running,
-            // assuming that all existing use of old callback stream is
-            // completed.
-            res = mDevice->deleteStream(mCallbackStreamId);
-            if (res != OK) {
-                ALOGE("%s: Camera %d: Unable to delete old output stream "
-                        "for callbacks: %s (%d)", __FUNCTION__, mCameraId,
-                        strerror(-res), res);
-                return res;
-            }
-            mCallbackStreamId = NO_STREAM;
-        }
-    }
-
-    if (mCallbackStreamId == NO_STREAM) {
-        ALOGV("Creating callback stream: %d %d format 0x%x",
-                params.previewWidth, params.previewHeight,
-                params.previewFormat);
-        res = mDevice->createStream(mCallbackWindow,
-                params.previewWidth, params.previewHeight,
-                params.previewFormat, 0, &mCallbackStreamId);
-        if (res != OK) {
-            ALOGE("%s: Camera %d: Can't create output stream for callbacks: "
-                    "%s (%d)", __FUNCTION__, mCameraId, strerror(-res), res);
-            return res;
-        }
-    }
-
-    return OK;
-}
-
 status_t Camera2Client::updateCaptureRequest(const Parameters &params) {
     ATRACE_CALL();
     status_t res;
diff --git a/services/camera/libcameraservice/Camera2Client.h b/services/camera/libcameraservice/Camera2Client.h
index 2695108..b2fd636 100644
--- a/services/camera/libcameraservice/Camera2Client.h
+++ b/services/camera/libcameraservice/Camera2Client.h
@@ -22,6 +22,7 @@
 #include "camera2/Parameters.h"
 #include "camera2/FrameProcessor.h"
 #include "camera2/CaptureProcessor.h"
+#include "camera2/CallbackProcessor.h"
 #include <binder/MemoryBase.h>
 #include <binder/MemoryHeapBase.h>
 #include <gui/CpuConsumer.h>
@@ -178,27 +179,7 @@ private:
 
     /** Preview callback related members */
 
-    int mCallbackStreamId;
-    static const size_t kCallbackHeapCount = 6;
-    sp<CpuConsumer>    mCallbackConsumer;
-    sp<ANativeWindow>  mCallbackWindow;
-    // Simple listener that forwards frame available notifications from
-    // a CPU consumer to the callback notification
-    class CallbackWaiter: public CpuConsumer::FrameAvailableListener {
-      public:
-        CallbackWaiter(Camera2Client *parent) : mParent(parent) {}
-        void onFrameAvailable() { mParent->onCallbackAvailable(); }
-      private:
-        Camera2Client *mParent;
-    };
-    sp<CallbackWaiter>  mCallbackWaiter;
-    sp<camera2::Camera2Heap>     mCallbackHeap;
-    int mCallbackHeapId;
-    size_t mCallbackHeapHead, mCallbackHeapFree;
-    // Handle callback image buffers
-    void onCallbackAvailable();
-
-    status_t updateCallbackStream(const Parameters &params);
+    sp<camera2::CallbackProcessor> mCallbackProcessor;
 
     /* Still image capture related members */
 
diff --git a/services/camera/libcameraservice/camera2/CallbackProcessor.cpp b/services/camera/libcameraservice/camera2/CallbackProcessor.cpp
new file mode 100644
index 0000000..854b890
--- /dev/null
+++ b/services/camera/libcameraservice/camera2/CallbackProcessor.cpp
@@ -0,0 +1,293 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_TAG "Camera2Client::CallbackProcessor"
+#define ATRACE_TAG ATRACE_TAG_CAMERA
+//#define LOG_NDEBUG 0
+
+#include <utils/Log.h>
+#include <utils/Trace.h>
+
+#include "CallbackProcessor.h"
+#include <gui/SurfaceTextureClient.h>
+#include "../Camera2Device.h"
+#include "../Camera2Client.h"
+
+
+namespace android {
+namespace camera2 {
+
+CallbackProcessor::CallbackProcessor(wp<Camera2Client> client):
+        Thread(false),
+        mClient(client),
+        mCallbackAvailable(false),
+        mCallbackStreamId(NO_STREAM) {
+}
+
+CallbackProcessor::~CallbackProcessor() {
+    ALOGV("%s: Exit", __FUNCTION__);
+}
+
+void CallbackProcessor::onFrameAvailable() {
+    Mutex::Autolock l(mInputMutex);
+    if (!mCallbackAvailable) {
+        mCallbackAvailable = true;
+        mCallbackAvailableSignal.signal();
+    }
+}
+
+status_t CallbackProcessor::updateStream(const Parameters &params) {
+    ATRACE_CALL();
+    status_t res;
+
+    Mutex::Autolock l(mInputMutex);
+
+    sp<Camera2Client> client = mClient.promote();
+    if (client == 0) return OK;
+    sp<Camera2Device> device = client->getCameraDevice();
+
+    if (mCallbackConsumer == 0) {
+        // Create CPU buffer queue endpoint
+        mCallbackConsumer = new CpuConsumer(kCallbackHeapCount);
+        mCallbackConsumer->setFrameAvailableListener(this);
+        mCallbackConsumer->setName(String8("Camera2Client::CallbackConsumer"));
+        mCallbackWindow = new SurfaceTextureClient(
+            mCallbackConsumer->getProducerInterface());
+    }
+
+    if (mCallbackStreamId != NO_STREAM) {
+        // Check if stream parameters have to change
+        uint32_t currentWidth, currentHeight, currentFormat;
+        res = device->getStreamInfo(mCallbackStreamId,
+                &currentWidth, &currentHeight, &currentFormat);
+        if (res != OK) {
+            ALOGE("%s: Camera %d: Error querying callback output stream info: "
+                    "%s (%d)", __FUNCTION__, client->getCameraId(),
+                    strerror(-res), res);
+            return res;
+        }
+        if (currentWidth != (uint32_t)params.previewWidth ||
+                currentHeight != (uint32_t)params.previewHeight ||
+                currentFormat != (uint32_t)params.previewFormat) {
+            // Since size should only change while preview is not running,
+            // assuming that all existing use of old callback stream is
+            // completed.
+            res = device->deleteStream(mCallbackStreamId);
+            if (res != OK) {
+                ALOGE("%s: Camera %d: Unable to delete old output stream "
+                        "for callbacks: %s (%d)", __FUNCTION__, client->getCameraId(),
+                        strerror(-res), res);
+                return res;
+            }
+            mCallbackStreamId = NO_STREAM;
+        }
+    }
+
+    if (mCallbackStreamId == NO_STREAM) {
+        ALOGV("Creating callback stream: %d %d format 0x%x",
+                params.previewWidth, params.previewHeight,
+                params.previewFormat);
+        res = device->createStream(mCallbackWindow,
+                params.previewWidth, params.previewHeight,
+                params.previewFormat, 0, &mCallbackStreamId);
+        if (res != OK) {
+            ALOGE("%s: Camera %d: Can't create output stream for callbacks: "
+                    "%s (%d)", __FUNCTION__, client->getCameraId(),
+                    strerror(-res), res);
+            return res;
+        }
+    }
+
+    return OK;
+}
+
+status_t CallbackProcessor::deleteStream() {
+    ATRACE_CALL();
+    status_t res;
+
+    Mutex::Autolock l(mInputMutex);
+
+    if (mCallbackStreamId != NO_STREAM) {
+        sp<Camera2Client> client = mClient.promote();
+        if (client == 0) return OK;
+        sp<Camera2Device> device = client->getCameraDevice();
+
+        device->deleteStream(mCallbackStreamId);
+        mCallbackStreamId = NO_STREAM;
+    }
+    return OK;
+}
+
+int CallbackProcessor::getStreamId() const {
+    Mutex::Autolock l(mInputMutex);
+    return mCallbackStreamId;
+}
+
+void CallbackProcessor::dump(int fd, const Vector<String16>& args) {
+}
+
+bool CallbackProcessor::threadLoop() {
+    status_t res;
+
+    {
+        Mutex::Autolock l(mInputMutex);
+        while (!mCallbackAvailable) {
+            res = mCallbackAvailableSignal.waitRelative(mInputMutex,
+                    kWaitDuration);
+            if (res == TIMED_OUT) return true;
+        }
+        mCallbackAvailable = false;
+    }
+
+    do {
+        sp<Camera2Client> client = mClient.promote();
+        if (client == 0) return false;
+        res = processNewCallback(client);
+    } while (res == OK);
+
+    return true;
+}
+
+status_t CallbackProcessor::processNewCallback(sp<Camera2Client> &client) {
+    ATRACE_CALL();
+    status_t res;
+
+    int callbackHeapId;
+    sp<Camera2Heap> callbackHeap;
+    size_t heapIdx;
+
+    CpuConsumer::LockedBuffer imgBuffer;
+    ALOGV("%s: Getting buffer", __FUNCTION__);
+    res = mCallbackConsumer->lockNextBuffer(&imgBuffer);
+    if (res != OK) {
+        if (res != BAD_VALUE) {
+            ALOGE("%s: Camera %d: Error receiving next callback buffer: "
+                    "%s (%d)", __FUNCTION__, client->getCameraId(), strerror(-res), res);
+        }
+        return res;
+    }
+    ALOGV("%s: Camera %d: Preview callback available", __FUNCTION__,
+            client->getCameraId());
+
+    {
+        SharedParameters::Lock l(client->getParameters());
+
+        if ( l.mParameters.state != Parameters::PREVIEW
+                && l.mParameters.state != Parameters::RECORD
+                && l.mParameters.state != Parameters::VIDEO_SNAPSHOT) {
+            ALOGV("%s: Camera %d: No longer streaming",
+                    __FUNCTION__, client->getCameraId());
+            mCallbackConsumer->unlockBuffer(imgBuffer);
+            return OK;
+        }
+
+        if (! (l.mParameters.previewCallbackFlags &
+                CAMERA_FRAME_CALLBACK_FLAG_ENABLE_MASK) ) {
+            ALOGV("%s: No longer enabled, dropping", __FUNCTION__);
+            mCallbackConsumer->unlockBuffer(imgBuffer);
+            return OK;
+        }
+        if ((l.mParameters.previewCallbackFlags &
+                        CAMERA_FRAME_CALLBACK_FLAG_ONE_SHOT_MASK) &&
+                !l.mParameters.previewCallbackOneShot) {
+            ALOGV("%s: One shot mode, already sent, dropping", __FUNCTION__);
+            mCallbackConsumer->unlockBuffer(imgBuffer);
+            return OK;
+        }
+
+        if (imgBuffer.format != l.mParameters.previewFormat) {
+            ALOGE("%s: Camera %d: Unexpected format for callback: "
+                    "%x, expected %x", __FUNCTION__, client->getCameraId(),
+                    imgBuffer.format, l.mParameters.previewFormat);
+            mCallbackConsumer->unlockBuffer(imgBuffer);
+            return INVALID_OPERATION;
+        }
+
+        // In one-shot mode, stop sending callbacks after the first one
+        if (l.mParameters.previewCallbackFlags &
+                CAMERA_FRAME_CALLBACK_FLAG_ONE_SHOT_MASK) {
+            ALOGV("%s: clearing oneshot", __FUNCTION__);
+            l.mParameters.previewCallbackOneShot = false;
+        }
+    }
+
+    size_t bufferSize = Camera2Client::calculateBufferSize(
+            imgBuffer.width, imgBuffer.height,
+            imgBuffer.format, imgBuffer.stride);
+    size_t currentBufferSize = (mCallbackHeap == 0) ?
+            0 : (mCallbackHeap->mHeap->getSize() / kCallbackHeapCount);
+    if (bufferSize != currentBufferSize) {
+        mCallbackHeap.clear();
+        mCallbackHeap = new Camera2Heap(bufferSize, kCallbackHeapCount,
+                "Camera2Client::CallbackHeap");
+        if (mCallbackHeap->mHeap->getSize() == 0) {
+            ALOGE("%s: Camera %d: Unable to allocate memory for callbacks",
+                    __FUNCTION__, client->getCameraId());
+            mCallbackConsumer->unlockBuffer(imgBuffer);
+            return INVALID_OPERATION;
+        }
+
+        mCallbackHeapHead = 0;
+        mCallbackHeapFree = kCallbackHeapCount;
+    }
+
+    if (mCallbackHeapFree == 0) {
+        ALOGE("%s: Camera %d: No free callback buffers, dropping frame",
+                __FUNCTION__, client->getCameraId());
+        mCallbackConsumer->unlockBuffer(imgBuffer);
+        return OK;
+    }
+
+    heapIdx = mCallbackHeapHead;
+
+    mCallbackHeapHead = (mCallbackHeapHead + 1) & kCallbackHeapCount;
+    mCallbackHeapFree--;
+
+    // TODO: Get rid of this memcpy by passing the gralloc queue all the way
+    // to app
+
+    ssize_t offset;
+    size_t size;
+    sp<IMemoryHeap> heap =
+            mCallbackHeap->mBuffers[heapIdx]->getMemory(&offset,
+                    &size);
+    uint8_t *data = (uint8_t*)heap->getBase() + offset;
+    memcpy(data, imgBuffer.data, bufferSize);
+
+    ALOGV("%s: Freeing buffer", __FUNCTION__);
+    mCallbackConsumer->unlockBuffer(imgBuffer);
+
+    // Call outside parameter lock to allow re-entrancy from notification
+    {
+        Camera2Client::SharedCameraClient::Lock l(client->mSharedCameraClient);
+        if (l.mCameraClient != 0) {
+            ALOGV("%s: Camera %d: Invoking client data callback",
+                    __FUNCTION__, client->getCameraId());
+            l.mCameraClient->dataCallback(CAMERA_MSG_PREVIEW_FRAME,
+                    mCallbackHeap->mBuffers[heapIdx], NULL);
+        }
+    }
+
+    // Only increment free if we're still using the same heap
+    mCallbackHeapFree++;
+
+    ALOGV("%s: exit", __FUNCTION__);
+
+    return OK;
+}
+
+}; // namespace camera2
+}; // namespace android
diff --git a/services/camera/libcameraservice/camera2/CallbackProcessor.h b/services/camera/libcameraservice/camera2/CallbackProcessor.h
new file mode 100644
index 0000000..36c51a3
--- /dev/null
+++ b/services/camera/libcameraservice/camera2/CallbackProcessor.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ANDROID_SERVERS_CAMERA_CAMERA2_CALLBACKPROCESSOR_H
+#define ANDROID_SERVERS_CAMERA_CAMERA2_CALLBACKPROCESSOR_H
+
+#include <utils/Thread.h>
+#include <utils/String16.h>
+#include <utils/Vector.h>
+#include <utils/Mutex.h>
+#include <utils/Condition.h>
+#include <gui/CpuConsumer.h>
+#include "Parameters.h"
+#include "CameraMetadata.h"
+#include "Camera2Heap.h"
+
+namespace android {
+
+class Camera2Client;
+
+namespace camera2 {
+
+/***
+ * Still image capture output image processing
+ */
+class CallbackProcessor:
+            public Thread, public CpuConsumer::FrameAvailableListener {
+  public:
+    CallbackProcessor(wp<Camera2Client> client);
+    ~CallbackProcessor();
+
+    void onFrameAvailable();
+
+    status_t updateStream(const Parameters &params);
+    status_t deleteStream();
+    int getStreamId() const;
+
+    void dump(int fd, const Vector<String16>& args);
+  private:
+    static const nsecs_t kWaitDuration = 10000000; // 10 ms
+    wp<Camera2Client> mClient;
+
+    mutable Mutex mInputMutex;
+    bool mCallbackAvailable;
+    Condition mCallbackAvailableSignal;
+
+    enum {
+        NO_STREAM = -1
+    };
+
+    int mCallbackStreamId;
+    static const size_t kCallbackHeapCount = 6;
+    sp<CpuConsumer>    mCallbackConsumer;
+    sp<ANativeWindow>  mCallbackWindow;
+    sp<Camera2Heap>    mCallbackHeap;
+    int mCallbackHeapId;
+    size_t mCallbackHeapHead, mCallbackHeapFree;
+
+    virtual bool threadLoop();
+
+    status_t processNewCallback(sp<Camera2Client> &client);
+
+};
+
+
+}; //namespace camera2
+}; //namespace android
+
+#endif