modify the flow of usb camera

6 files changed, 371 insertions, 313 deletions

diff --git a/AmlogicCameraHardware.cpp b/AmlogicCameraHardware.cpp
index 1b151db..65941c9 100755
--- a/AmlogicCameraHardware.cpp
+++ b/AmlogicCameraHardware.cpp
@@ -18,6 +18,7 @@
 #define NDEBUG 0

 

 #define LOG_TAG "AmlogicCameraHardware"

+

 #include <utils/Log.h>

 

 #include "AmlogicCameraHardware.h"

@@ -37,12 +38,7 @@ int set_flash(bool mode);
 }

 #endif

 

-//for test

-static void convert_rgb16_to_yuv420sp(uint8_t *rgb, uint8_t *yuv, int width, int height);

-void convert_rgb24_to_rgb16(uint8_t *rgb888, uint8_t *rgb565, int width, int height);

-void yuyv422_to_rgb16(unsigned char *buf, unsigned char *rgb, int width, int height);

-void yuyv422_to_yuv420sp(unsigned char *bufsrc, unsigned char *bufdest, int width, int height);

-

+#include <util.h>

 namespace android

 {

 

@@ -157,6 +153,41 @@ static int SYS_reset_zoom(void)
     write_sys_int(VIDEO_ZOOM, 100);

     return 0;

 }

+#else

+static int SYS_enable_nextvideo()

+{

+    return 0;

+}

+

+static int SYS_close_video()

+{

+    return 0;

+}

+

+static int SYS_open_video()

+{

+    return 0;

+}

+

+static int SYS_disable_avsync()

+{

+    return 0;

+}

+

+static int SYS_disable_video_pause()

+{

+    return 0;

+}

+

+static int SYS_set_zoom(int zoom)

+{

+    return 0;

+}

+

+static int SYS_reset_zoom(void)

+{

+    return 0;

+}

 #endif

 

 

@@ -179,7 +210,7 @@ AmlogicCameraHardware::AmlogicCameraHardware(int camid)
                     mRecordEnable(0),

                     mState(0)

 {

-	LOGD("current camera is %d",camid);

+	LOGD("Current camera is %d",camid);

 #ifdef AMLOGIC_CAMERA_OVERLAY_SUPPORT

     SYS_disable_avsync();

     SYS_disable_video_pause();

@@ -190,12 +221,13 @@ AmlogicCameraHardware::AmlogicCameraHardware(int camid)
 	mCamera = HAL_GetCameraInterface(camid);

 	mCamera->Open();

 	initDefaultParameters();

+#ifdef AMLOGIC_USB_CAMERA_SUPPORT

 	mCamera->Close();

+#endif

 }

 

 AmlogicCameraHardware::~AmlogicCameraHardware()

 {

-    singleton.clear();

 	mCamera->Close();

 	delete mCamera;

 	mCamera = NULL;

@@ -386,18 +418,17 @@ int AmlogicCameraHardware::previewThread()
 		//get Record frames data

 		if(mMsgEnabled & CAMERA_MSG_VIDEO_FRAME)

 		{

-			//LOGD("return video frame");

 #ifndef AMLOGIC_CAMERA_OVERLAY_SUPPORT

 			sp<MemoryHeapBase> reocrdheap = mRecordHeap;

 			sp<MemoryBase> recordbuffer = mRecordBuffers[mCurrentPreviewFrame];

 #ifdef AMLOGIC_USB_CAMERA_SUPPORT

 			ssize_t recordoffset = offset*3/4;

 			uint8_t *recordframe = ((uint8_t *)reocrdheap->getBase()) + recordoffset;

-			yuyv422_to_yuv420sp((unsigned char *)frame,(unsigned char *)recordframe,width,height);

+			yuyv422_to_nv21((unsigned char *)frame,(unsigned char *)recordframe,width,height);

 #else

 			ssize_t recordoffset = offset;

 			uint8_t *recordframe = ((uint8_t *)reocrdheap->getBase()) + recordoffset;

-			convert_rgb16_to_yuv420sp(frame,recordframe,width,height);

+			convert_rgb16_to_nv21(frame,recordframe,width,height);

 #endif

 			mDataCbTimestamp(systemTime(),CAMERA_MSG_VIDEO_FRAME, recordbuffer, mCallbackCookie);

 #else

@@ -405,11 +436,10 @@ int AmlogicCameraHardware::previewThread()
 			//when use overlay, the preview format is the same as record

 			mDataCbTimestamp(systemTime(),CAMERA_MSG_VIDEO_FRAME, buffer, mCallbackCookie);

 #endif

-		}

+		}  

 

 		mCurrentPreviewFrame = (mCurrentPreviewFrame + 1) % kBufferCount;

 	}

-

 	return NO_ERROR;

 }

 

@@ -585,7 +615,8 @@ int AmlogicCameraHardware::pictureThread()
 }

 

 status_t AmlogicCameraHardware::takePicture()

-{

+{   

+	disableMsgType(CAMERA_MSG_PREVIEW_FRAME);

     stopPreview();

     if (createThread(beginPictureThread, this) == false)

         return -1;

@@ -671,22 +702,10 @@ void AmlogicCameraHardware::release()
 #endif

 }

 

-wp<CameraHardwareInterface> AmlogicCameraHardware::singleton;

-

 sp<CameraHardwareInterface> AmlogicCameraHardware::createInstance(int CamId)

 {

-    sp<CameraHardwareInterface> hardware = NULL;

-    if (singleton != 0)

-    {

-        hardware = singleton.promote();

-    }

-    if (hardware == NULL)

-    {

-        hardware = new AmlogicCameraHardware(CamId);

-        singleton = hardware;

-    }

-

-    return hardware;

+	LOGD("AmlogicCameraHardware ::createInstance\n");

+    return new AmlogicCameraHardware(CamId);

 }

 

 extern "C" sp<CameraHardwareInterface> HAL_openCameraHardware(int cameraId)

@@ -696,227 +715,3 @@ extern "C" sp<CameraHardwareInterface> HAL_openCameraHardware(int cameraId)
 }

 }; // namespace android

 

-void convert_rgb24_to_rgb16(uint8_t *src, uint8_t *dst, int width, int height)

-{

-	int src_len = width*height*3;

-    int i = 0;

-	int j = 0;

-

-    for (i = 0; i < src_len; i += 3)

-    {

-		dst[j] = (src[i]&0x1f) | (src[i+1]>>5);

-		dst[j+1] = ((src[i+1]>>2)<<5) | (src[i+2]>>3);

-        j += 2;

-    }

-}

-

-#define swap_cbcr

-static void convert_rgb16_to_yuv420sp(uint8_t *rgb, uint8_t *yuv, int width, int height)

-{

-	int iy =0, iuv = 0;

-    uint8_t* buf_y = yuv;

-    uint8_t* buf_uv = buf_y + width * height;

-    uint16_t* buf_rgb = (uint16_t *)rgb;

-	int h,w,val_rgb,val_r,val_g,val_b;

-	int y,u,v;

-    for (h = 0; h < height; h++) {

-        for (w = 0; w < width; w++) {

-            val_rgb = buf_rgb[h * width + w];

-            val_r = ((val_rgb & (0x1f << 11)) >> 11)<<3;

-            val_g = ((val_rgb & (0x3f << 5)) >> 5)<<2;

-            val_b = ((val_rgb & (0x1f << 0)) >> 0)<<3;

-            y = 0.30078 * val_r + 0.5859 * val_g + 0.11328 * val_b;

-            if (y > 255) {

-                y = 255;

-            } else if (y < 0) {

-                y = 0;

-            }

-            buf_y[iy++] = y;

-            if (0 == h % 2 && 0 == w % 2) {

-                u = -0.11328 * val_r - 0.33984 * val_g + 0.51179 * val_b + 128;

-                if (u > 255) {

-                    u = 255;

-                } else if (u < 0) {

-                    u = 0;

-                }

-                v = 0.51179 * val_r - 0.429688 * val_g - 0.08203 * val_b  + 128;

-                if (v > 255) {

-                    v = 255;

-                } else if (v < 0) {

-                    v = 0;

-                }

-#ifdef swap_cbcr

-                buf_uv[iuv++] = v;

-                buf_uv[iuv++] = u;

-#else

-                buf_uv[iuv++] = u;

-                buf_uv[iuv++] = v;

-#endif

-            }

-        }

-}

-}

-

-static inline void yuv_to_rgb16(unsigned char y,unsigned char u,unsigned char v,unsigned char *rgb)

-{

-    register int r,g,b;

-    int rgb16;

-

-    r = (1192 * (y - 16) + 1634 * (v - 128) ) >> 10;

-    g = (1192 * (y - 16) - 833 * (v - 128) - 400 * (u -128) ) >> 10;

-    b = (1192 * (y - 16) + 2066 * (u - 128) ) >> 10;

-

-    r = r > 255 ? 255 : r < 0 ? 0 : r;

-    g = g > 255 ? 255 : g < 0 ? 0 : g;

-    b = b > 255 ? 255 : b < 0 ? 0 : b;

-

-    rgb16 = (int)(((r >> 3)<<11) | ((g >> 2) << 5)| ((b >> 3) << 0));

-

-    *rgb = (unsigned char)(rgb16 & 0xFF);

-    rgb++;

-    *rgb = (unsigned char)((rgb16 & 0xFF00) >> 8);

-

-}

-

-void yuyv422_to_rgb16(unsigned char *buf, unsigned char *rgb, int width, int height)

-{

-    int x,y,z=0;

-    int blocks;

-

-    blocks = (width * height) * 2;

-

-    for (y = 0; y < blocks; y+=4) {

-        unsigned char Y1, Y2, U, V;

-

-        Y1 = buf[y + 0];

-        U = buf[y + 1];

-        Y2 = buf[y + 2];

-        V = buf[y + 3];

-

-        yuv_to_rgb16(Y1, U, V, &rgb[y]);

-        yuv_to_rgb16(Y2, U, V, &rgb[y + 2]);

-    }

-}

-

-void yuyv422_to_yuv420sp(unsigned char *bufsrc, unsigned char *bufdest, int width, int height)

-{

-    unsigned char *ptrsrcy1, *ptrsrcy2;

-    unsigned char *ptrsrcy3, *ptrsrcy4;

-    unsigned char *ptrsrccb1, *ptrsrccb2;

-    unsigned char *ptrsrccb3, *ptrsrccb4;

-    unsigned char *ptrsrccr1, *ptrsrccr2;

-    unsigned char *ptrsrccr3, *ptrsrccr4;

-    int srcystride, srcccstride;

-

-    ptrsrcy1  = bufsrc ;

-    ptrsrcy2  = bufsrc + (width<<1) ;

-    ptrsrcy3  = bufsrc + (width<<1)*2 ;

-    ptrsrcy4  = bufsrc + (width<<1)*3 ;

-

-    ptrsrccb1 = bufsrc + 1;

-    ptrsrccb2 = bufsrc + (width<<1) + 1;

-    ptrsrccb3 = bufsrc + (width<<1)*2 + 1;

-    ptrsrccb4 = bufsrc + (width<<1)*3 + 1;

-

-    ptrsrccr1 = bufsrc + 3;

-    ptrsrccr2 = bufsrc + (width<<1) + 3;

-    ptrsrccr3 = bufsrc + (width<<1)*2 + 3;

-    ptrsrccr4 = bufsrc + (width<<1)*3 + 3;

-

-    srcystride  = (width<<1)*3;

-    srcccstride = (width<<1)*3;

-

-    unsigned char *ptrdesty1, *ptrdesty2;

-    unsigned char *ptrdesty3, *ptrdesty4;

-    unsigned char *ptrdestcb1, *ptrdestcb2;

-    unsigned char *ptrdestcr1, *ptrdestcr2;

-    int destystride, destccstride;

-

-    ptrdesty1 = bufdest;

-    ptrdesty2 = bufdest + width;

-    ptrdesty3 = bufdest + width*2;

-    ptrdesty4 = bufdest + width*3;

-

-    ptrdestcb1 = bufdest + width*height;

-    ptrdestcb2 = bufdest + width*height + width;

-

-    ptrdestcr1 = bufdest + width*height + 1;

-    ptrdestcr2 = bufdest + width*height + width + 1;

-

-    destystride  = (width)*3;

-    destccstride = width;

-

-    int i, j;

-

-    for(j=0; j<(height/4); j++)

-    {

-        for(i=0;i<(width/2);i++)

-        {

-            (*ptrdesty1++) = (*ptrsrcy1);

-            (*ptrdesty2++) = (*ptrsrcy2);

-            (*ptrdesty3++) = (*ptrsrcy3);

-            (*ptrdesty4++) = (*ptrsrcy4);

-

-            ptrsrcy1 += 2;

-            ptrsrcy2 += 2;

-            ptrsrcy3 += 2;

-            ptrsrcy4 += 2;

-

-            (*ptrdesty1++) = (*ptrsrcy1);

-            (*ptrdesty2++) = (*ptrsrcy2);

-            (*ptrdesty3++) = (*ptrsrcy3);

-            (*ptrdesty4++) = (*ptrsrcy4);

-

-            ptrsrcy1 += 2;

-            ptrsrcy2 += 2;

-            ptrsrcy3 += 2;

-            ptrsrcy4 += 2;

-

-            (*ptrdestcb1) = (*ptrsrccb1);

-            (*ptrdestcb2) = (*ptrsrccb3);

-            ptrdestcb1 += 2;

-            ptrdestcb2 += 2;

-

-            ptrsrccb1 += 4;

-            ptrsrccb3 += 4;

-

-            (*ptrdestcr1) = (*ptrsrccr1);

-            (*ptrdestcr2) = (*ptrsrccr3);

-            ptrdestcr1 += 2;

-            ptrdestcr2 += 2;

-

-            ptrsrccr1 += 4;

-            ptrsrccr3 += 4;

-

-        }

-

-

-        /* Update src pointers */

-        ptrsrcy1  += srcystride;

-        ptrsrcy2  += srcystride;

-        ptrsrcy3  += srcystride;

-        ptrsrcy4  += srcystride;

-

-        ptrsrccb1 += srcccstride;

-        ptrsrccb3 += srcccstride;

-

-        ptrsrccr1 += srcccstride;

-        ptrsrccr3 += srcccstride;

-

-

-        /* Update dest pointers */

-        ptrdesty1 += destystride;

-        ptrdesty2 += destystride;

-        ptrdesty3 += destystride;

-        ptrdesty4 += destystride;

-

-        ptrdestcb1 += destccstride;

-        ptrdestcb2 += destccstride;

-

-        ptrdestcr1 += destccstride;

-        ptrdestcr2 += destccstride;

-

-    }

-}

-

-

diff --git a/AmlogicCameraHardware.h b/AmlogicCameraHardware.h
index ede6fd8..fa44e50 100755
--- a/AmlogicCameraHardware.h
+++ b/AmlogicCameraHardware.h
@@ -7,6 +7,7 @@
 #include <binder/MemoryBase.h>

 #include <binder/MemoryHeapBase.h>

 

+

 #ifndef AMLOGIC_USB_CAMERA_SUPPORT

 #define AMLOGIC_CAMERA_OVERLAY_SUPPORT 1

 #endif

@@ -86,8 +87,6 @@ private:
                         AmlogicCameraHardware(int camid = 0);

     virtual             ~AmlogicCameraHardware();

 

-    static wp<CameraHardwareInterface> singleton;

-

     static const int kBufferCount = 6;

 

 #ifndef AMLOGIC_CAMERA_OVERLAY_SUPPORT

diff --git a/Android.mk b/Android.mk
index c8623a4..6d5640e 100755
--- a/Android.mk
+++ b/Android.mk
@@ -23,6 +23,10 @@ endif
 
 ifeq ($(BOARD_USE_USB_CAMERA),true)
 	LOCAL_CFLAGS += -DAMLOGIC_USB_CAMERA_SUPPORT
+	LOCAL_SRC_FILES += util.cpp
+else
+ifeq ($(BOARD_HAVE_MULTI_CAMERAS),true)
+	LOCAL_CFLAGS += -DAMLOGIC_MULTI_CAMERA_SUPPORT
 endif
 
 ifeq ($(BOARD_HAVE_FLASHLIGHT),true)
diff --git a/V4L2/V4L2Camera.cpp b/V4L2/V4L2Camera.cpp
index 06af3cf..00f103e 100755
--- a/V4L2/V4L2Camera.cpp
+++ b/V4L2/V4L2Camera.cpp
@@ -21,12 +21,12 @@
 #include <unistd.h>
 
 extern "C" unsigned char* getExifBuf(char** attrlist, int attrCount, int* exifLen,int thumbnailLen,char* thumbnaildata);
-extern void yuyv422_to_rgb16(unsigned char *buf, unsigned char *rgb, int width, int height);
+#include "../util.h"
 namespace android {
 
 #define V4L2_PREVIEW_BUFF_NUM (6)
 #define V4L2_TAKEPIC_BUFF_NUM (1)
-extern void yuyv422_to_rgb24(unsigned char *buf, unsigned char *rgb, int width, int height);
+
 
 V4L2Camera::V4L2Camera(char* devname,int camid)
 {
@@ -52,6 +52,7 @@ V4L2Camera::~V4L2Camera()
 #define SYSFILE_CAMERA_SET_MIRROR "/sys/class/vm/mirror"
 static int writefile(char* path,char* content)
 {
+#ifndef AMLOGIC_USB_CAMERA_SUPPORT
     FILE* fp = fopen(path, "w+");
 
     LOGD("Write file %s(%p) content %s", path, fp, content);
@@ -67,7 +68,7 @@ static int writefile(char* path,char* content)
     }
     else
         LOGD("open file fail\n");
-
+#endif
     return 1;
 }
 
@@ -89,7 +90,9 @@ status_t	V4L2Camera::Open()
     if(m_pSetting->m_iDevFd == -1){
         m_pSetting->m_iDevFd = open(m_pSetting->m_pDevName, O_RDWR);
         if (m_pSetting->m_iDevFd != -1){
-            //LOGD("open %s success %d \n", m_pDevName,m_iDevFd);
+            LOGD("open %s success %d \n", m_pSetting->m_pDevName,m_pSetting->m_iDevFd);
+
+#			ifndef AMLOGIC_USB_CAMERA_SUPPORT
             const char* mirror = m_pSetting->GetInfo(CAMERA_MIRROR_MODE);
             if((mirror)&&((strcasecmp(mirror, "1")==0)||(strcasecmp(mirror, "enable")==0)||(strcasecmp(mirror, "true")==0))){
                 LOGD("*****Enable camera %d L-R mirror",m_pSetting->m_iCamId);
@@ -98,6 +101,8 @@ status_t	V4L2Camera::Open()
                 LOGD("*****Enable camera %d normal mode",m_pSetting->m_iCamId);
                 writefile(SYSFILE_CAMERA_SET_MIRROR,"0");
             }
+#			endif
+
             return NO_ERROR;
         }else{
             LOGD("open %s fail\n",m_pSetting->m_pDevName);
@@ -148,13 +153,13 @@ status_t	V4L2Camera::StartPreview()
 {
 	int w,h,pixelFormat;
 	m_bFirstFrame = true;
-	if(NO_ERROR != Open())
-		return UNKNOWN_ERROR;
 	if(m_pSetting != NULL )
 		m_pSetting->m_hParameter.getPreviewSize(&w,&h);
 	else
 		LOGE("%s,%d m_pSetting=NULL",__FILE__, __LINE__);
 #ifdef AMLOGIC_USB_CAMERA_SUPPORT
+	if(NO_ERROR != Open())
+		return UNKNOWN_ERROR;
 	pixelFormat = V4L2_PIX_FMT_YUYV;
 #else
 	pixelFormat = V4L2_PIX_FMT_NV21;
@@ -182,15 +187,18 @@ status_t	V4L2Camera::StopPreview()
 status_t	V4L2Camera::TakePicture()
 {
 	int w,h,pixelFormat;
+	m_pSetting->m_hParameter.getPictureSize(&w,&h);
 #ifdef AMLOGIC_USB_CAMERA_SUPPORT
+		if(NO_ERROR != Open())
+			return UNKNOWN_ERROR;
 	pixelFormat = V4L2_PIX_FMT_YUYV;
 #else
 	pixelFormat = V4L2_PIX_FMT_RGB24;
 #endif
-	if(NO_ERROR != Open())
-		return UNKNOWN_ERROR;
-	m_pSetting->m_hParameter.getPictureSize(&w,&h);
-	V4L2_BufferInit(w,h,V4L2_TAKEPIC_BUFF_NUM,pixelFormat);
+	if(NO_ERROR != V4L2_BufferInit(w,h,V4L2_TAKEPIC_BUFF_NUM,pixelFormat)) {
+		LOGD(" open devices error in w:%d,h:%d,fmt:%x\n",w,h,pixelFormat);
+		return  UNKNOWN_ERROR;
+	}
 	V4L2_BufferEnQue(0);
 	V4L2_StreamOn();
 	m_iPicIdx = V4L2_BufferDeQue();
@@ -200,13 +208,13 @@ status_t	V4L2Camera::TakePicture()
 
 status_t	V4L2Camera::TakePictureEnd()
 {
+	int ret;
 	m_iPicIdx = -1;
+	ret=V4L2_BufferUnInit();
 #ifdef AMLOGIC_USB_CAMERA_SUPPORT
-	V4L2_BufferUnInit();
-	return Close();
-#else
-	return V4L2_BufferUnInit();
+    ret=Close();
 #endif
+	return ret;
 }
 
 status_t	V4L2Camera::GetPreviewFrame(uint8_t* framebuf)
@@ -241,7 +249,7 @@ status_t	V4L2Camera::GetRawFrame(uint8_t* framebuf)
 #ifdef AMLOGIC_USB_CAMERA_SUPPORT
 		int w,h;
 		m_pSetting->m_hParameter.getPictureSize(&w,&h);
-		yuyv422_to_rgb16((unsigned char*)pV4L2Frames[m_iPicIdx],framebuf,w,h);
+		yuyv422_to_rgb16((unsigned char*)pV4L2Frames[m_iPicIdx],framebuf,pV4L2FrameSize[m_iPicIdx]);
 #else
 		memcpy(framebuf,pV4L2Frames[m_iPicIdx],pV4L2FrameSize[m_iPicIdx]);
 #endif
@@ -615,7 +623,7 @@ status_t V4L2Camera::V4L2_BufferInit(int Buf_W,int Buf_H,int Buf_Num,int colorfm
 	hformat.fmt.pix.pixelformat = colorfmt;
 	if (ioctl(m_pSetting->m_iDevFd, VIDIOC_S_FMT, &hformat) == -1)
 	{
-		LOGE("V4L2_BufferInit VIDIOC_S_FMT fail m_pSetting->m_iDevFd=%d width=%d height=%d pixelformat=(%c%c%c%c)",m_pSetting->m_iDevFd,hformat.fmt.pix.width,hformat.fmt.pix.height,colorfmt&0xff, (colorfmt>>8)&0xff, (colorfmt>>16)&0xff, (colorfmt>>24)&0xff);
+		LOGE("V4L2_BufferInit VIDIOC_S_FMT fail m_pSetting->m_iDevFd=%d width=%d height=%d pixelformat=(%c%c%c%c),errno:%d",m_pSetting->m_iDevFd,hformat.fmt.pix.width,hformat.fmt.pix.height,colorfmt&0xff, (colorfmt>>8)&0xff, (colorfmt>>16)&0xff, (colorfmt>>24)&0xff,errno);
 		return UNKNOWN_ERROR;
 	}
 
@@ -759,47 +767,4 @@ extern CameraInterface* HAL_GetCameraInterface(int Id)
 		return new V4L2Camera("/dev/video1",1);
 }
 
-static inline void yuv_to_rgb24(unsigned char y,unsigned char u,unsigned char v,unsigned char *rgb)
-{
-    register int r,g,b;
-    int rgb24;
-
-    r = (1192 * (y - 16) + 1634 * (v - 128) ) >> 10;
-    g = (1192 * (y - 16) - 833 * (v - 128) - 400 * (u -128) ) >> 10;
-    b = (1192 * (y - 16) + 2066 * (u - 128) ) >> 10;
-
-    r = r > 255 ? 255 : r < 0 ? 0 : r;
-    g = g > 255 ? 255 : g < 0 ? 0 : g;
-    b = b > 255 ? 255 : b < 0 ? 0 : b;
-
-    rgb24 = (int)((r <<16) | (g  << 8)| b);
-
-    *rgb = (unsigned char)r;
-    rgb++;
-    *rgb = (unsigned char)g;
-    rgb++;
-    *rgb = (unsigned char)b;
-}
-
-void yuyv422_to_rgb24(unsigned char *buf, unsigned char *rgb, int width, int height)
-{
-    int x,y,z=0;
-    int blocks;
-
-    blocks = (width * height) * 2;
-
-    for (y = 0,z=0; y < blocks; y+=4,z+=6) {
-        unsigned char Y1, Y2, U, V;
-
-        Y1 = buf[y + 0];
-        U = buf[y + 1];
-        Y2 = buf[y + 2];
-        V = buf[y + 3];
-
-        yuv_to_rgb24(Y1, U, V, &rgb[z]);
-        yuv_to_rgb24(Y2, U, V, &rgb[z + 3]);
-
-    }
-}
-
 };
diff --git a/util.cpp b/util.cpp
new file mode 100755
index 0000000..a1e7eb2
--- a/dev/null
+++ b/util.cpp
@@ -0,0 +1,285 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#define swap_cbcr
+static void convert_rgb16_to_nv21(uint8_t *rgb, uint8_t *yuv, int width, int height)
+{
+	int iy =0, iuv = 0;
+    uint8_t* buf_y = yuv;
+    uint8_t* buf_uv = buf_y + width * height;
+    uint16_t* buf_rgb = (uint16_t *)rgb;
+	int h,w,val_rgb,val_r,val_g,val_b;
+	int y,u,v;
+    for (h = 0; h < height; h++) {
+        for (w = 0; w < width; w++) {
+            val_rgb = buf_rgb[h * width + w];
+            val_r = ((val_rgb & (0x1f << 11)) >> 11)<<3;
+            val_g = ((val_rgb & (0x3f << 5)) >> 5)<<2;
+            val_b = ((val_rgb & (0x1f << 0)) >> 0)<<3;
+            y = 0.30078 * val_r + 0.5859 * val_g + 0.11328 * val_b;
+            if (y > 255) {
+                y = 255;
+            } else if (y < 0) {
+                y = 0;
+            }
+            buf_y[iy++] = y;
+            if (0 == h % 2 && 0 == w % 2) {
+                u = -0.11328 * val_r - 0.33984 * val_g + 0.51179 * val_b + 128;
+                if (u > 255) {
+                    u = 255;
+                } else if (u < 0) {
+                    u = 0;
+                }
+                v = 0.51179 * val_r - 0.429688 * val_g - 0.08203 * val_b  + 128;
+                if (v > 255) {
+                    v = 255;
+                } else if (v < 0) {
+                    v = 0;
+                }
+#ifdef swap_cbcr
+                buf_uv[iuv++] = v;
+                buf_uv[iuv++] = u;
+#else
+                buf_uv[iuv++] = u;
+                buf_uv[iuv++] = v;
+#endif
+            }
+        }
+}
+}
+
+static inline void yuv_to_rgb16(unsigned char y,unsigned char u,unsigned char v,unsigned char *rgb)
+{
+    register int r,g,b;
+    int rgb16;
+
+    r = (1192 * (y - 16) + 1634 * (v - 128) ) >> 10;
+    g = (1192 * (y - 16) - 833 * (v - 128) - 400 * (u -128) ) >> 10;
+    b = (1192 * (y - 16) + 2066 * (u - 128) ) >> 10;
+
+    r = r > 255 ? 255 : r < 0 ? 0 : r;
+    g = g > 255 ? 255 : g < 0 ? 0 : g;
+    b = b > 255 ? 255 : b < 0 ? 0 : b;
+
+    rgb16 = (int)(((r >> 3)<<11) | ((g >> 2) << 5)| ((b >> 3) << 0));
+
+    *rgb = (unsigned char)(rgb16 & 0xFF);
+    rgb++;
+    *rgb = (unsigned char)((rgb16 & 0xFF00) >> 8);
+
+}
+
+void yuyv422_to_rgb16(unsigned char *from, unsigned char *to, int size)
+{
+    int x,y,z=0;
+
+    for (y = 0; y < size; y+=4) {
+        unsigned char Y1, Y2, U, V;
+
+        Y1 = from[y + 0];
+        U = from[y + 1];
+        Y2 = from[y + 2];
+        V = from[y + 3];
+
+        yuv_to_rgb16(Y1, U, V, &to[y]);
+        yuv_to_rgb16(Y2, U, V, &to[y + 2]);
+    }
+}
+
+void yuyv422_to_rgb16(unsigned char *from, unsigned char *to, int width, int height)
+{
+    yuyv422_to_rgb16(from,to,(width * height) * 2);
+}
+
+void yuyv422_to_nv21(unsigned char *bufsrc, unsigned char *bufdest, int width, int height)
+{
+    unsigned char *ptrsrcy1, *ptrsrcy2;
+    unsigned char *ptrsrcy3, *ptrsrcy4;
+    unsigned char *ptrsrccb1, *ptrsrccb2;
+    unsigned char *ptrsrccb3, *ptrsrccb4;
+    unsigned char *ptrsrccr1, *ptrsrccr2;
+    unsigned char *ptrsrccr3, *ptrsrccr4;
+    int srcystride, srcccstride;
+
+    ptrsrcy1  = bufsrc ;
+    ptrsrcy2  = bufsrc + (width<<1) ;
+    ptrsrcy3  = bufsrc + (width<<1)*2 ;
+    ptrsrcy4  = bufsrc + (width<<1)*3 ;
+
+    ptrsrccb1 = bufsrc + 1;
+    ptrsrccb2 = bufsrc + (width<<1) + 1;
+    ptrsrccb3 = bufsrc + (width<<1)*2 + 1;
+    ptrsrccb4 = bufsrc + (width<<1)*3 + 1;
+
+    ptrsrccr1 = bufsrc + 3;
+    ptrsrccr2 = bufsrc + (width<<1) + 3;
+    ptrsrccr3 = bufsrc + (width<<1)*2 + 3;
+    ptrsrccr4 = bufsrc + (width<<1)*3 + 3;
+
+    srcystride  = (width<<1)*3;
+    srcccstride = (width<<1)*3;
+
+    unsigned char *ptrdesty1, *ptrdesty2;
+    unsigned char *ptrdesty3, *ptrdesty4;
+    unsigned char *ptrdestcb1, *ptrdestcb2;
+    unsigned char *ptrdestcr1, *ptrdestcr2;
+    int destystride, destccstride;
+
+    ptrdesty1 = bufdest;
+    ptrdesty2 = bufdest + width;
+    ptrdesty3 = bufdest + width*2;
+    ptrdesty4 = bufdest + width*3;
+
+    ptrdestcb1 = bufdest + width*height;
+    ptrdestcb2 = bufdest + width*height + width;
+
+    ptrdestcr1 = bufdest + width*height + 1;
+    ptrdestcr2 = bufdest + width*height + width + 1;
+
+    destystride  = (width)*3;
+    destccstride = width;
+
+    int i, j;
+
+    for(j=0; j<(height/4); j++)
+    {
+        for(i=0;i<(width/2);i++)
+        {
+            (*ptrdesty1++) = (*ptrsrcy1);
+            (*ptrdesty2++) = (*ptrsrcy2);
+            (*ptrdesty3++) = (*ptrsrcy3);
+            (*ptrdesty4++) = (*ptrsrcy4);
+
+            ptrsrcy1 += 2;
+            ptrsrcy2 += 2;
+            ptrsrcy3 += 2;
+            ptrsrcy4 += 2;
+
+            (*ptrdesty1++) = (*ptrsrcy1);
+            (*ptrdesty2++) = (*ptrsrcy2);
+            (*ptrdesty3++) = (*ptrsrcy3);
+            (*ptrdesty4++) = (*ptrsrcy4);
+
+            ptrsrcy1 += 2;
+            ptrsrcy2 += 2;
+            ptrsrcy3 += 2;
+            ptrsrcy4 += 2;
+
+			#if 0
+            (*ptrdestcb1) = (*ptrsrccb1);
+            (*ptrdestcb2) = (*ptrsrccb3);
+            #else 
+            (*ptrdestcb1) = (*ptrsrccr1);
+            (*ptrdestcb2) = (*ptrsrccr3);
+            #endif
+
+			#if 0
+            (*ptrdestcr1) = (*ptrsrccr1);
+            (*ptrdestcr2) = (*ptrsrccr3);
+            #else 
+            (*ptrdestcr1) = (*ptrsrccb1);
+            (*ptrdestcr2) = (*ptrsrccb3);
+            #endif
+            
+            ptrdestcb1 += 2;
+            ptrdestcb2 += 2;
+            ptrdestcr1 += 2;
+            ptrdestcr2 += 2;
+
+            ptrsrccb1 += 4;
+            ptrsrccb3 += 4;
+            ptrsrccr1 += 4;
+            ptrsrccr3 += 4;
+
+        }
+
+
+        /* Update src pointers */
+        ptrsrcy1  += srcystride;
+        ptrsrcy2  += srcystride;
+        ptrsrcy3  += srcystride;
+        ptrsrcy4  += srcystride;
+
+        ptrsrccb1 += srcccstride;
+        ptrsrccb3 += srcccstride;
+
+        ptrsrccr1 += srcccstride;
+        ptrsrccr3 += srcccstride;
+
+
+        /* Update dest pointers */
+        ptrdesty1 += destystride;
+        ptrdesty2 += destystride;
+        ptrdesty3 += destystride;
+        ptrdesty4 += destystride;
+
+        ptrdestcb1 += destccstride;
+        ptrdestcb2 += destccstride;
+
+        ptrdestcr1 += destccstride;
+        ptrdestcr2 += destccstride;
+
+    }
+}
+
+static inline void yuv_to_rgb24(unsigned char y,unsigned char u,unsigned char v,unsigned char *rgb)
+{
+    register int r,g,b;
+    int rgb24;
+
+    r = (1192 * (y - 16) + 1634 * (v - 128) ) >> 10;
+    g = (1192 * (y - 16) - 833 * (v - 128) - 400 * (u -128) ) >> 10;
+    b = (1192 * (y - 16) + 2066 * (u - 128) ) >> 10;
+
+    r = r > 255 ? 255 : r < 0 ? 0 : r;
+    g = g > 255 ? 255 : g < 0 ? 0 : g;
+    b = b > 255 ? 255 : b < 0 ? 0 : b;
+
+    rgb24 = (int)((r <<16) | (g  << 8)| b);
+
+    *rgb = (unsigned char)r;
+    rgb++;
+    *rgb = (unsigned char)g;
+    rgb++;
+    *rgb = (unsigned char)b;
+}
+
+void yuyv422_to_rgb24(unsigned char *buf, unsigned char *rgb, int width, int height)
+{
+    int x,y,z=0;
+    int blocks;
+
+    blocks = (width * height) * 2;
+
+    for (y = 0,z=0; y < blocks; y+=4,z+=6) {
+        unsigned char Y1, Y2, U, V;
+
+        Y1 = buf[y + 0];
+        U = buf[y + 1];
+        Y2 = buf[y + 2];
+        V = buf[y + 3];
+
+        yuv_to_rgb24(Y1, U, V, &rgb[z]);
+        yuv_to_rgb24(Y2, U, V, &rgb[z + 3]);
+
+    }
+}
+
+void convert_rgb24_to_rgb16(uint8_t *src, uint8_t *dst, int width, int height)
+{
+	int src_len = width*height*3;
+    int i = 0;
+	int j = 0;
+    
+    for (i = 0; i < src_len; i += 3)
+    {
+		dst[j] = (src[i]&0x1f) | (src[i+1]>>5);
+		dst[j+1] = ((src[i+1]>>2)<<5) | (src[i+2]>>3);
+        j += 2;
+    }
+}
+
diff --git a/util.h b/util.h
new file mode 100755
index 0000000..b60aeee
--- a/dev/null
+++ b/util.h
@@ -0,0 +1,10 @@
+#ifndef AML_CAMERA_HARDWARE_INCLUDE_
+#define AML_CAMERA_HARDWARE_INCLUDE_
+
+void convert_rgb16_to_nv21(uint8_t *rgb, uint8_t *yuv, int width, int height);
+void convert_rgb24_to_rgb16(uint8_t *rgb888, uint8_t *rgb565, int width, int height);
+void yuyv422_to_rgb16(unsigned char *from, unsigned char *to, int width,int height);
+void yuyv422_to_rgb16(unsigned char *from, unsigned char *to, int size);
+void yuyv422_to_rgb24(unsigned char *buf, unsigned char *rgb, int width, int height);
+void yuyv422_to_nv21(unsigned char *bufsrc, unsigned char *bufdest, int width, int height);
+#endif /* AML_CAMERA_HARDWARE_INCLUDE_*/