Camera_test: BQ: Add support for writing cropped

- Add support for writing cropped buffers in camera_test
  return from surface texture. Camera_test needs to
  calculate the crop rectangle from the transform
  matrix. Using the calculated values camera_test
  will write the buffer accordingly.

Change-Id: I8884f06a98ddf404343424a4bec4a82a22dc01fe
Signed-off-by: Tyler Luu <tluu@ti.com>
Signed-off-by: Vladimir Petrov <vppetrov@mm-sol.com>

4 files changed, 159 insertions, 9 deletions

diff --git a/test/CameraHal/camera_test.h b/test/CameraHal/camera_test.h
index 89e89ce..f2dd219 100644
--- a/test/CameraHal/camera_test.h
+++ b/test/CameraHal/camera_test.h
@@ -218,6 +218,7 @@ typedef struct buffer_info {
     int width;
     int height;
     int format;
+    Rect crop;
     sp<GraphicBuffer> buf;
 } buffer_info_t;
 
@@ -321,6 +322,7 @@ public:
                 uint8_t *mappedBuffer;
                 unsigned int count;
                 unsigned int slot;
+                Rect crop;
             };
         public:
             Defer(BufferSourceThread* bst) :
@@ -352,7 +354,8 @@ public:
                 if (!mExiting) {
                     DeferContainer defer = mDeferQueue.itemAt(0);
                     printf ("=== handling buffer %d\n", defer.count);
-                    mBST->handleBuffer(defer.graphicBuffer, defer.mappedBuffer, defer.count);
+                    mBST->handleBuffer(defer.graphicBuffer, defer.mappedBuffer,
+                                       defer.count, defer.crop);
                     defer.graphicBuffer->unlock();
                     mDeferQueue.removeAt(0);
                     mBST->onHandled(defer.graphicBuffer, defer.slot);
@@ -360,12 +363,14 @@ public:
                 }
                 return false;
             }
-            void add(sp<GraphicBuffer> &gbuf, unsigned int count, unsigned int slot = 0) {
+            void add(sp<GraphicBuffer> &gbuf, const Rect &crop,
+                     unsigned int count, unsigned int slot = 0) {
                 Mutex::Autolock lock(mFrameQueueMutex);
                 DeferContainer defer;
                 defer.graphicBuffer = gbuf;
                 defer.count = count;
                 defer.slot = slot;
+                defer.crop = crop;
                 gbuf->lock(GRALLOC_USAGE_SW_READ_RARELY, (void**) &defer.mappedBuffer);
                 mDeferQueue.add(defer);
                 mFrameQueueCondition.signal();
@@ -426,7 +431,8 @@ public:
         return !mReturnedBuffers.isEmpty();
     }
 
-    void handleBuffer(sp<GraphicBuffer> &, uint8_t *, unsigned int);
+    void handleBuffer(sp<GraphicBuffer> &, uint8_t *, unsigned int, const Rect &);
+    Rect getCrop(sp<GraphicBuffer> &buffer, const float *mtx);
     void showMetadata(sp<IMemory> data);
 protected:
     void restartCapture() {
diff --git a/test/CameraHal/camera_test_bufferqueue.h b/test/CameraHal/camera_test_bufferqueue.h
index f696f67..a7a2ef5 100644
--- a/test/CameraHal/camera_test_bufferqueue.h
+++ b/test/CameraHal/camera_test_bufferqueue.h
@@ -80,11 +80,12 @@ public:
                 // the first time we acquire it. We are expected to hold a reference to
                 // it there after...
                 mBufferSlots[slot].mGraphicBuffer = item.mGraphicBuffer;
+                mBufferSlots[slot].mCrop = item.mCrop;
             }
             showMetadata(item.mMetadata);
             printf("\n");
             graphic_buffer = mBufferSlots[item.mBuf].mGraphicBuffer;
-            mDeferThread->add(graphic_buffer, mCounter++, item.mBuf);
+            mDeferThread->add(graphic_buffer, item.mCrop, mCounter++, item.mBuf);
             restartCapture();
             return true;
         }
diff --git a/test/CameraHal/camera_test_surfacetexture.cpp b/test/CameraHal/camera_test_surfacetexture.cpp
index c3b1012..423f780 100644
--- a/test/CameraHal/camera_test_surfacetexture.cpp
+++ b/test/CameraHal/camera_test_surfacetexture.cpp
@@ -8,6 +8,7 @@
 #include <pthread.h>
 #include <string.h>
 #include <climits>
+#include <math.h>
 
 #include <gui/SurfaceTexture.h>
 #include <gui/SurfaceTextureClient.h>
@@ -84,6 +85,26 @@ static size_t calcBufSize(int format, int width, int height)
     return buf_size;
 }
 
+static unsigned int calcOffset(int format, unsigned int width, unsigned int top, unsigned int left)
+{
+    unsigned int bpp;
+
+    switch (format) {
+        case HAL_PIXEL_FORMAT_TI_NV12:
+            bpp = 1;
+            break;
+        case HAL_PIXEL_FORMAT_TI_Y16:
+            bpp = 2;
+            break;
+        // add more formats later
+        default:
+            bpp = 1;
+            break;
+    }
+
+    return top * width + left * bpp;
+}
+
 static int getHalPixFormat(const char *format)
 {
     int pixformat = HAL_PIXEL_FORMAT_TI_NV12;
@@ -119,6 +140,61 @@ static int getUsageFromANW(int format)
     return usage;
 }
 
+static status_t writeCroppedNV12(unsigned int offset, unsigned int stride,
+                                 unsigned int bufWidth, unsigned int bufHeight,
+                                 const Rect &crop,
+                                 int format, int fd, unsigned char *buffer)
+{
+    unsigned char *luma = NULL, *chroma = NULL, *src = NULL;
+    unsigned int uvoffset;
+    size_t size;
+    int write_size;
+
+    if (!buffer || !crop.isValid()) {
+        return BAD_VALUE;
+    }
+
+    size = calcBufSize(format, stride, bufHeight);
+    src = buffer;
+
+    // offset to beginning of uv plane
+    uvoffset =  stride * bufHeight;
+    // offset to beginning of valid region of uv plane
+    uvoffset += (offset - (offset % stride)) / 2 + (offset % stride);
+
+    // start of valid luma region
+    luma = src + offset;
+    // start of valid chroma region
+    chroma = src + uvoffset;
+
+    // write luma line x line
+    unsigned int height = crop.height();
+    unsigned int width = crop.width();
+    write_size = width;
+    for (unsigned int i = 0; i < height; i++) {
+        if (write_size != write(fd, luma, width)) {
+            printf("Bad Write error (%d)%s\n",
+                    errno, strerror(errno));
+            return UNKNOWN_ERROR;
+        }
+        luma += stride;
+    }
+
+    // write chroma line x line
+    height /= 2;
+    write_size = width;
+    for (unsigned int i = 0; i < height; i++) {
+        if (write_size != write(fd, chroma, width)) {
+            printf("Bad Write error (%d)%s\n",
+                    errno, strerror(errno));
+            return UNKNOWN_ERROR;
+        }
+        chroma += stride;
+    }
+
+    return NO_ERROR;
+}
+
 void GLSurface::initialize(int display) {
     mEglDisplay = eglGetDisplay(EGL_DEFAULT_DISPLAY);
     ASSERT(EGL_SUCCESS == eglGetError());
@@ -428,9 +504,11 @@ void SurfaceTextureGL::drawTexture() {
 }
 
 // buffer source stuff
-void BufferSourceThread::handleBuffer(sp<GraphicBuffer> &graphic_buffer, uint8_t *buffer, unsigned int count) {
+void BufferSourceThread::handleBuffer(sp<GraphicBuffer> &graphic_buffer, uint8_t *buffer,
+                                        unsigned int count, const Rect &crop) {
     int size;
     buffer_info_t info;
+    unsigned int offset = 0;
     int fd = -1;
     char fn[256];
 
@@ -457,6 +535,7 @@ void BufferSourceThread::handleBuffer(sp<GraphicBuffer> &graphic_buffer, uint8_t
     info.height = graphic_buffer->getHeight();
     info.format = graphic_buffer->getPixelFormat();
     info.buf = graphic_buffer;
+    info.crop = crop;
 
     {
         Mutex::Autolock lock(mReturnedBuffersMutex);
@@ -464,6 +543,10 @@ void BufferSourceThread::handleBuffer(sp<GraphicBuffer> &graphic_buffer, uint8_t
     }
     mReturnedBuffers.add(info);
 
+    // re-calculate size and offset
+    size = calcBufSize((int) graphic_buffer->getPixelFormat(), crop.width(), crop.height());
+    offset = calcOffset((int) graphic_buffer->getPixelFormat(), info.width, crop.top, crop.left);
+
     // Do not write buffer to file if we are streaming capture
     // It adds too much latency
     if (!mRestartCapture) {
@@ -471,11 +554,17 @@ void BufferSourceThread::handleBuffer(sp<GraphicBuffer> &graphic_buffer, uint8_t
         sprintf(fn, "/sdcard/img%03d.raw", count);
         fd = open(fn, O_CREAT | O_WRONLY | O_TRUNC, 0777);
         if (fd >= 0) {
-            if (size != write(fd, buffer, size)) {
+            if (offset && (info.format == HAL_PIXEL_FORMAT_TI_NV12)) {
+                writeCroppedNV12(offset, info.width, info.width, info.height,
+                                 crop, info.format, fd, buffer);
+            } else if (size != write(fd, buffer + offset, size)) {
                 printf("Bad Write int a %s error (%d)%s\n", fn, errno, strerror(errno));
             }
-            printf("%s: buffer=%08X, size=%d stored at %s\n",
-                        __FUNCTION__, (int)buffer, info.size, fn);
+            printf("%s: buffer=%08X, size=%d stored at %s\n"
+                   "\tRect: top[%d] left[%d] right[%d] bottom[%d] width[%d] height[%d]\n",
+                        __FUNCTION__, (int)buffer, size, fn,
+                        crop.top, crop.left, crop.right, crop.bottom,
+                        crop.width(), crop.height());
             close(fd);
         } else {
             printf("error opening or creating %s\n", fn);
@@ -483,6 +572,56 @@ void BufferSourceThread::handleBuffer(sp<GraphicBuffer> &graphic_buffer, uint8_t
     }
 }
 
+Rect BufferSourceThread::getCrop(sp<GraphicBuffer> &graphic_buffer, const float *mtx) {
+    Rect crop(graphic_buffer->getWidth(), graphic_buffer->getHeight());
+
+    // calculate crop rectangle from tranformation matrix
+    float sx, sy, tx, ty, h, w;
+    unsigned int rect_x, rect_y;
+    /*   sx, 0, 0, 0,
+         0, sy, 0, 0,
+         0, 0, 1, 0,
+         tx, ty, 0, 1 */
+
+    sx = mtx[0];
+    sy = mtx[5];
+    tx = mtx[12];
+    ty = mtx[13];
+    w = float(graphic_buffer->getWidth());
+    h = float(graphic_buffer->getHeight());
+
+    unsigned int bottom = (unsigned int)(h - (ty * h + 1));
+    unsigned int left = (unsigned int)(tx * w -1);
+    rect_y = (unsigned int)(fabsf(sy) * h);
+    rect_x = (unsigned int)(fabsf(sx) * w);
+
+    // handle v-flip
+    if (sy < 0.0f) {
+        bottom = h - bottom;
+    }
+
+    // handle h-flip
+    if (sx < 0.0f) {
+        left = w - left;
+    }
+
+    unsigned int top = bottom - rect_y;
+    unsigned int right = left + rect_x;
+
+    Rect updatedCrop(left, top, right, bottom);
+    if (updatedCrop.isValid()) {
+        crop = updatedCrop;
+    } else {
+        printf("Crop for buffer %d is not valid: "
+               "left=%u, top=%u, right=%u, bottom=%u. "
+               "Will use default.\n",
+               mCounter,
+               left, top, right, bottom);
+    }
+
+    return crop;
+}
+
 void BufferSourceInput::setInput(buffer_info_t bufinfo, const char *format, ShotParameters &params) {
     ANativeWindowBuffer* anb;
     GraphicBufferMapper &mapper = GraphicBufferMapper::get();
diff --git a/test/CameraHal/camera_test_surfacetexture.h b/test/CameraHal/camera_test_surfacetexture.h
index fd125df..395e82d 100644
--- a/test/CameraHal/camera_test_surfacetexture.h
+++ b/test/CameraHal/camera_test_surfacetexture.h
@@ -151,6 +151,7 @@ public:
 
         mFW->waitForFrame();
         if (!mDestroying) {
+            float mtx[16] = {0.0};
             mSurfaceTexture->updateTexImage();
             printf("=== Metadata for buffer %d ===\n", mCounter);
 #ifndef ANDROID_API_JB_OR_LATER
@@ -158,7 +159,10 @@ public:
 #endif
             printf("\n");
             graphic_buffer = mSurfaceTexture->getCurrentBuffer();
-            mDeferThread->add(graphic_buffer, mCounter++);
+            mSurfaceTexture->getTransformMatrix(mtx);
+            Rect crop = getCrop(graphic_buffer, mtx);
+
+            mDeferThread->add(graphic_buffer, crop, mCounter++);
             restartCapture();
             return true;
         }