PD#124392: screen_source: fix screencatch frame size

Change-Id: I13bc7b3e6e100e381b7ea803d143f948bf67495f

3 files changed, 229 insertions, 28 deletions

diff --git a/aml_screen.cpp b/aml_screen.cpp
index 560ff65..22c98aa 100644
--- a/aml_screen.cpp
+++ b/aml_screen.cpp
@@ -151,6 +151,16 @@ int screen_source_set_crop(struct aml_screen_device* dev, int x, int y, int widt
     return android::BAD_VALUE;
 }
 
+int screen_source_get_amlvideo2_crop(struct aml_screen_device* dev, int *x, int *y, int *width, int *height)
+{
+    android::vdin_screen_source* source = (android::vdin_screen_source*)dev->priv;
+
+    if ((x != NULL) && (y != NULL) && (width != NULL) && (height != NULL))
+        return source->get_amlvideo2_crop(x, y, width, height);
+
+    return android::BAD_VALUE;
+}
+
 int screen_source_set_amlvideo2_crop(struct aml_screen_device* dev, int x, int y, int width, int height)
 {
     android::vdin_screen_source* source = (android::vdin_screen_source*)dev->priv;
@@ -210,6 +220,18 @@ int screen_source_get_source_type(struct aml_screen_device* dev)
     return source->get_source_type();
 }
 
+int screen_source_get_current_sourcesize(struct aml_screen_device* dev, int *w, int *h)
+{
+    android::vdin_screen_source* source = (android::vdin_screen_source*)dev->priv;
+    return source->get_current_sourcesize(w, h);
+}
+
+int screen_source_set_screen_mode(struct aml_screen_device* dev, int  mode)
+{
+    android::vdin_screen_source* source = (android::vdin_screen_source*)dev->priv;
+    return source->set_screen_mode(mode);
+}
+
 int screen_source_start_v4l2_device(struct aml_screen_device* dev)
 {
     android::vdin_screen_source* source = (android::vdin_screen_source*)dev->priv;
@@ -289,6 +311,7 @@ static int aml_screen_device_open(const struct hw_module_t* module, const char*
         dev->ops.set_format = screen_source_set_format;
         dev->ops.set_rotation = screen_source_set_rotation;
         dev->ops.set_crop = screen_source_set_crop;
+        dev->ops.get_amlvideo2_crop = screen_source_get_amlvideo2_crop;
         dev->ops.set_amlvideo2_crop = screen_source_set_amlvideo2_crop;
         dev->ops.aquire_buffer = screen_source_aquire_buffer;
         dev->ops.release_buffer = screen_source_release_buffer;
@@ -298,6 +321,8 @@ static int aml_screen_device_open(const struct hw_module_t* module, const char*
         dev->ops.set_frame_rate = screen_source_set_frame_rate;
         dev->ops.set_source_type = screen_source_set_source_type;
         dev->ops.get_source_type = screen_source_get_source_type;
+        dev->ops.get_current_sourcesize = screen_source_get_current_sourcesize;
+        dev->ops.set_screen_mode = screen_source_set_screen_mode;
         // dev->ops.inc_buffer_refcount = screen_source_inc_buffer_refcount;
         dev->ops.start_v4l2_device = screen_source_start_v4l2_device;
         dev->ops.stop_v4l2_device = screen_source_stop_v4l2_device;
diff --git a/v4l2_vdin.cpp b/v4l2_vdin.cpp
index 772154c..fd4fc10 100644
--- a/v4l2_vdin.cpp
+++ b/v4l2_vdin.cpp
@@ -53,21 +53,19 @@ namespace android {
         (type *) ((char *) __mptr - (char *)(&((type *)0)->member)); })
 #endif
 
-#define BOUNDRY 32
-
-#define ALIGN(x) (x + (BOUNDRY) - 1)& ~((BOUNDRY) - 1)
+#define ALIGN(b,w) (((b)+((w)-1))/(w)*(w))
 
 static size_t getBufSize(int format, int width, int height)
 {
     size_t buf_size = 0;
-	
-    switch(format){
+    switch (format) {
         case V4L2_PIX_FMT_YVU420:
         case V4L2_PIX_FMT_NV21:
             buf_size = width * height * 3 / 2;
             break;
         case V4L2_PIX_FMT_YUYV:
         case V4L2_PIX_FMT_RGB565:
+        case V4L2_PIX_FMT_RGB565X:
             buf_size = width * height * 2;
             break;
         case V4L2_PIX_FMT_RGB24:
@@ -78,11 +76,80 @@ static size_t getBufSize(int format, int width, int height)
             break;
         default:
             ALOGE("Invalid format");
-            buf_size = 0;
-    }		
+            buf_size = width * height * 3 / 2;
+    }
     return buf_size;
 }
 
+static void two_bytes_per_pixel_memcpy_align32(unsigned char *dst, unsigned char *src, int width, int height)
+{
+        int stride = (width + 31) & ( ~31);
+        int w, h;
+        for (h=0; h<height; h++)
+        {
+                memcpy( dst, src, width*2);
+                dst += width*2;
+                src += stride*2;
+        }
+}
+
+static void nv21_memcpy_align32(unsigned char *dst, unsigned char *src, int width, int height)
+{
+        int stride = (width + 31) & ( ~31);
+        int w, h;
+        for (h=0; h<height*3/2; h++)
+        {
+                memcpy( dst, src, width);
+                dst += width;
+                src += stride;
+        }
+}
+
+static void yv12_memcpy_align32(unsigned char *dst, unsigned char *src, int width, int height)
+{
+        int new_width = (width + 63) & ( ~63);
+        int stride;
+        int w, h;
+        for (h=0; h<height; h++)
+        {
+                memcpy( dst, src, width);
+                dst += width;
+                src += new_width;
+        }
+
+        stride = ALIGN(width/2, 16);
+        for (h=0; h<height; h++)
+        {
+                memcpy( dst, src, width/2);
+                dst += stride;
+                src += new_width/2;
+        }
+}
+
+static void rgb24_memcpy_align32(unsigned char *dst, unsigned char *src, int width, int height)
+{
+        int stride = (width + 31) & ( ~31);
+        int w, h;
+        for (h=0; h<height; h++)
+        {
+                memcpy( dst, src, width*3);
+                dst += width*3;
+                src += stride*3;
+        }
+}
+
+static void rgb32_memcpy_align32(unsigned char *dst, unsigned char *src, int width, int height)
+{
+        int stride = (width + 31) & ( ~31);
+        int w, h;
+        for (h=0; h<height; h++)
+        {
+                memcpy( dst, src, width*4);
+                dst += width*4;
+                src += stride*4;
+        }
+}
+
 static int  getNativeWindowFormat(int format)
 {
     int nativeFormat = HAL_PIXEL_FORMAT_YCbCr_422_I;
@@ -126,7 +193,7 @@ vdin_screen_source::vdin_screen_source()
 }
 
 int vdin_screen_source::init(){
-	ALOGV("%s %d", __FUNCTION__, __LINE__);
+    ALOGV("%s %d", __FUNCTION__, __LINE__);
     mCameraHandle = open("/dev/video11", O_RDWR| O_NONBLOCK);
     if (mCameraHandle < 0)
     {
@@ -140,7 +207,10 @@ int vdin_screen_source::init(){
         close(mCameraHandle);
         return NO_MEMORY;
     }
-	mBufferCount = 4;
+    mBufferCount = 4;
+    for (int i = 0; i < mBufferCount; i++) {
+        src_temp[i] = NULL;
+    }
     mPixelFormat = V4L2_PIX_FMT_NV21;
     mNativeWindowPixelFormat = HAL_PIXEL_FORMAT_YCrCb_420_SP;
     mFrameWidth = 1280;
@@ -205,6 +275,9 @@ int vdin_screen_source::start_v4l2_device()
         }
         mVideoInfo->refcount[i] = 0;
         mBufs.add(mVideoInfo->mem[i],i);
+        if (mFrameWidth %32  != 0) {
+            mTemp_Bufs.add(src_temp[i],i);
+        }
     }
     ALOGV("[%s %d] VIDIOC_QUERYBUF successful", __FUNCTION__, __LINE__);
 
@@ -242,6 +315,14 @@ int vdin_screen_source::stop_v4l2_device()
             ALOGE("Unmap failed");
         }
     }
+    mBufs.clear();
+    if (mFrameWidth %32  != 0) {
+        mTemp_Bufs.clear();
+        for (int j = 0; j <mBufferCount; j++ ) {
+            free(src_temp[j]);
+            src_temp[j] = NULL;
+        }
+    }
     return ret;
 }
 int vdin_screen_source::start()
@@ -252,12 +333,12 @@ int vdin_screen_source::start()
         ALOGI("already open");
         return NO_ERROR;
     }
-	
+
     ret = start_v4l2_device();
     if(ret != NO_ERROR){
         ALOGE("Start v4l2 device failed:%d",ret);
         return ret;
-    } 
+    }
     if(mFrameType & NATIVE_WINDOW_DATA){
         ret = init_native_window();
         if(ret != NO_ERROR){
@@ -268,7 +349,7 @@ int vdin_screen_source::start()
     if(mFrameType & NATIVE_WINDOW_DATA || mFrameType & CALL_BACK_DATA){
         ALOGD("Create Work Thread");
         mWorkThread = new WorkThread(this);
-    }	
+    }
     if(mSetStateCB != NULL)
         mSetStateCB(START);
     mState = START;
@@ -369,24 +450,33 @@ int vdin_screen_source::set_format(int width, int height, int color_format)
     if(mOpen == true)
         return NO_ERROR;
     int ret;
-    mVideoInfo->width = ALIGN(width);
+    mVideoInfo->width = width;
     mVideoInfo->height = height;
     mVideoInfo->framesizeIn = (mVideoInfo->width * mVideoInfo->height << 3);      //note color format
     mVideoInfo->formatIn = color_format;
 
     mVideoInfo->format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    mVideoInfo->format.fmt.pix.width = ALIGN(width);
+    mVideoInfo->format.fmt.pix.width = width;
     mVideoInfo->format.fmt.pix.height = height;
     mVideoInfo->format.fmt.pix.pixelformat = color_format;
     mPixelFormat = color_format;
     mNativeWindowPixelFormat = getNativeWindowFormat(color_format);
-    mFrameWidth = ALIGN(width);
+    mFrameWidth = width;
     mFrameHeight = height;
     mBufferSize = getBufSize(color_format, mFrameWidth, mFrameHeight);
+    if (mFrameWidth %32  != 0) {
+        for (int i = 0; i < mBufferCount; i++) {
+            src_temp[i] = (long*) malloc(mBufferSize);
+            if (!src_temp[i]) {
+                ALOGE("malloc src_temp buffer failed .\n");
+                return NO_MEMORY;
+            }
+        }
+    }
     ALOGD("mFrameWidth:%d,mFrameHeight:%d",mFrameWidth,mFrameHeight);
     ALOGD("mPixelFormat:%x,mNativeWindowPixelFormat:%x,mBufferSize:%d",mPixelFormat,mNativeWindowPixelFormat,mBufferSize);
     ret = ioctl(mCameraHandle, VIDIOC_S_FMT, &mVideoInfo->format);
-    if (ret < 0) {		
+    if (ret < 0) {
         ALOGE("[%s %d]VIDIOC_S_FMT %d", __FUNCTION__, __LINE__, ret);
         return ret;
     }
@@ -457,6 +547,25 @@ int vdin_screen_source::set_frame_rate(int frameRate)
     return ret ;
 }
 
+int vdin_screen_source::get_amlvideo2_crop(int *x, int *y, int *width, int *height)
+{
+    ALOGV("[%s %d]", __FUNCTION__, __LINE__);
+    int ret = 0;
+
+    struct v4l2_crop crop;
+    memset(&crop, 0, sizeof(struct v4l2_crop));
+    crop.type = V4L2_BUF_TYPE_VIDEO_OVERLAY;
+    ret = ioctl(mCameraHandle, VIDIOC_S_CROP, &crop);
+    if (ret) {
+        ALOGE("get amlvideo2 crop  fail: %s. ret=%d", strerror(errno),ret);
+    }
+    *x = crop.c.left;
+    *y = crop.c.top;
+    *width = crop.c.width;
+    *height = crop.c.height;
+     return ret ;
+}
+
 int vdin_screen_source::set_amlvideo2_crop(int x, int y, int width, int height)
 {
     ALOGV("[%s %d]", __FUNCTION__, __LINE__);
@@ -472,7 +581,7 @@ int vdin_screen_source::set_amlvideo2_crop(int x, int y, int width, int height)
     crop.c.height = height;
     ret = ioctl(mCameraHandle, VIDIOC_S_CROP, &crop);
     if (ret) {
-        ALOGE("Set frame rate fail: %s. ret=%d", strerror(errno),ret);
+        ALOGE("Set amlvideo2 crop  fail: %s. ret=%d", strerror(errno),ret);
     }
 
     return ret ;
@@ -516,9 +625,39 @@ int vdin_screen_source::get_source_type()
     return sourceType;
 }
 
+int vdin_screen_source::get_current_sourcesize(int *width,  int *height)
+{
+    int ret = NO_ERROR;
+    struct v4l2_format format;
+    memset(&format, 0,sizeof(struct v4l2_format));
+
+    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    ret = ioctl(mCameraHandle, VIDIOC_G_FMT, &format);
+    if (ret < 0) {
+        ALOGE("Open: VIDIOC_G_FMT Failed: %s", strerror(errno));
+        return ret;
+    }
+    *width = format.fmt.pix.width;
+    *height = format.fmt.pix.height;
+    ALOGD("VIDIOC_G_FMT, w * h: %5d x %5d", *width,  *height);
+    return ret;
+}
+
+int vdin_screen_source::set_screen_mode(int  mode)
+{
+    int ret = NO_ERROR;
+
+    ret = ioctl(mCameraHandle, VIDIOC_S_OUTPUT, &mode);
+    if (ret < 0) {
+        ALOGE("VIDIOC_S_OUTPUT Failed: %s", strerror(errno));
+        return ret;
+    }
+    return ret;
+}
+
 int vdin_screen_source::aquire_buffer(aml_screen_buffer_info_t *buff_info)
 {
-        ALOGE("%s %d", __FUNCTION__, __LINE__);
+    ALOGE("%s %d", __FUNCTION__, __LINE__);
     int ret = -1;
     mVideoInfo->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     mVideoInfo->buf.memory = V4L2_MEMORY_MMAP;
@@ -539,11 +678,40 @@ int vdin_screen_source::aquire_buffer(aml_screen_buffer_info_t *buff_info)
     if (!(mFrameType & NATIVE_WINDOW_DATA || mFrameType & CALL_BACK_DATA))
         mVideoInfo->refcount[mVideoInfo->buf.index] += 1;
 
+    if (mFrameWidth %32  != 0) {
+        switch (mVideoInfo->format.fmt.pix.pixelformat) {
+            case V4L2_PIX_FMT_YVU420:
+                yv12_memcpy_align32((unsigned char*)src_temp[mVideoInfo->buf.index], (unsigned char*)mVideoInfo->mem[mVideoInfo->buf.index], mFrameWidth, mFrameHeight);
+                break;
+            case V4L2_PIX_FMT_NV21:
+                nv21_memcpy_align32((unsigned char*)src_temp[mVideoInfo->buf.index], (unsigned char*)mVideoInfo->mem[mVideoInfo->buf.index], mFrameWidth, mFrameHeight);
+                break;
+            case V4L2_PIX_FMT_YUYV:
+            case V4L2_PIX_FMT_RGB565:
+            case V4L2_PIX_FMT_RGB565X:
+                two_bytes_per_pixel_memcpy_align32 ((unsigned char*)src_temp[mVideoInfo->buf.index], (unsigned char*)mVideoInfo->mem[mVideoInfo->buf.index], mFrameWidth, mFrameHeight);
+                break;
+            case V4L2_PIX_FMT_RGB24:
+                rgb24_memcpy_align32((unsigned char*)src_temp[mVideoInfo->buf.index], (unsigned char*)mVideoInfo->mem[mVideoInfo->buf.index], mFrameWidth, mFrameHeight);
+                break;
+            case V4L2_PIX_FMT_RGB32:
+                rgb32_memcpy_align32((unsigned char*)src_temp[mVideoInfo->buf.index], (unsigned char*)mVideoInfo->mem[mVideoInfo->buf.index], mFrameWidth, mFrameHeight);
+                break;
+            default:
+                ALOGE("Invalid format , %s %d " , __FUNCTION__, __LINE__);
+        }
+        buff_info->buffer_mem    = src_temp[mVideoInfo->buf.index];
+        buff_info->buffer_canvas = mVideoInfo->canvas[mVideoInfo->buf.index];
+        buff_info->tv_sec        = mVideoInfo->buf.timestamp.tv_sec;
+        buff_info->tv_usec       = mVideoInfo->buf.timestamp.tv_usec;
+    } else {
+        buff_info->buffer_mem    = mVideoInfo->mem[mVideoInfo->buf.index];
+        buff_info->buffer_canvas = mVideoInfo->canvas[mVideoInfo->buf.index];
+        buff_info->tv_sec        = mVideoInfo->buf.timestamp.tv_sec;
+        buff_info->tv_usec       = mVideoInfo->buf.timestamp.tv_usec;
+    }
+
     ALOGE("%s finish %d ", __FUNCTION__, __LINE__);
-    buff_info->buffer_mem    = mVideoInfo->mem[mVideoInfo->buf.index];
-    buff_info->buffer_canvas = mVideoInfo->canvas[mVideoInfo->buf.index];
-    buff_info->tv_sec        = mVideoInfo->buf.timestamp.tv_sec;
-    buff_info->tv_usec       = mVideoInfo->buf.timestamp.tv_usec;
     return ret;
 }
 
@@ -564,22 +732,25 @@ int vdin_screen_source::release_buffer(long* ptr)
     v4l2_buffer hbuf_query;
 
     Mutex::Autolock autoLock(mLock);
-
-    currentIndex = mBufs.valueFor(ptr);
-    if(mVideoInfo->refcount[currentIndex] > 0){
+    if (mFrameWidth % 32 != 0) {
+        currentIndex = mTemp_Bufs.valueFor(ptr);
+    } else {
+        currentIndex = mBufs.valueFor(ptr);
+    }
+    if (mVideoInfo->refcount[currentIndex] > 0) {
         mVideoInfo->refcount[currentIndex] -= 1;
-    }else{
+    } else {
         ALOGE("return buffer when refcount already zero");
         return 0;
     }
-    if(mVideoInfo->refcount[currentIndex] == 0){
+    if (mVideoInfo->refcount[currentIndex] == 0) {
         memset(&hbuf_query,0,sizeof(v4l2_buffer));
         hbuf_query.index = currentIndex;
         hbuf_query.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
         hbuf_query.memory = V4L2_MEMORY_MMAP;
         ALOGV("return buffer :%d",currentIndex);
         ret = ioctl(mCameraHandle, VIDIOC_QBUF, &hbuf_query);
-        if(ret != 0){
+        if (ret != 0) {
             ALOGE("Return Buffer :%d failed", currentIndex);
         }
     }
diff --git a/v4l2_vdin.h b/v4l2_vdin.h
index 9794078..ef816d0 100644
--- a/v4l2_vdin.h
+++ b/v4l2_vdin.h
@@ -87,6 +87,7 @@ class vdin_screen_source {
         int set_format(int width = 640, int height = 480, int color_format = V4L2_PIX_FMT_NV21);
         int set_rotation(int degree);
         int set_crop(int x, int y, int width, int height);
+        int get_amlvideo2_crop(int *x, int *y, int *width, int *height);
         int set_amlvideo2_crop(int x, int y, int width, int height);
         int aquire_buffer(aml_screen_buffer_info_t *buff_info);
         // int inc_buffer_refcount(int* ptr);
@@ -97,6 +98,8 @@ class vdin_screen_source {
         int set_frame_rate(int frameRate);
         int set_source_type(int sourceType);
         int get_source_type();
+        int  get_current_sourcesize(int * width,int * height);
+        int  set_screen_mode(int  mode);
         int start_v4l2_device();
         int stop_v4l2_device();
         int set_port_type(int sourceType);
@@ -121,6 +124,7 @@ class vdin_screen_source {
     private:
         int mCurrentIndex;
         KeyedVector<long *, long> mBufs;
+        KeyedVector<long *, long> mTemp_Bufs;
         int mBufferCount;
         int mFrameWidth;
         int mFrameHeight;
@@ -138,6 +142,7 @@ class vdin_screen_source {
         app_data_callback mDataCB;
         bool mOpen;
         void *mUser;
+        long * src_temp[4];
 };
 
 }