PD #81697: add yuv420 format for mjpeg post process && add support for camera Xfinity

4 files changed, 335 insertions, 14 deletions

diff --git a/V4LCameraAdapter/V4LCameraAdapter.cpp b/V4LCameraAdapter/V4LCameraAdapter.cpp
index 4922878..4343e12 100755
--- a/V4LCameraAdapter/V4LCameraAdapter.cpp
+++ b/V4LCameraAdapter/V4LCameraAdapter.cpp
@@ -772,7 +772,7 @@ status_t V4LCameraAdapter::UseBuffersPreview(void* bufArr, int num)
         pixfmt = V4L2_PIX_FMT_YUYV;
         mPixelFormat = CameraFrame::PIXEL_FMT_YUYV;
     }
-
+    
     mSensorFormat = pixfmt;
 #ifdef AMLOGIC_USB_CAMERA_SUPPORT
     if((mUseMJPEG == true)&&(mSupportMJPEG == true)&&(width>=640)&&(height>=480))
@@ -1520,7 +1520,7 @@ int V4LCameraAdapter::previewThread()
                 fillThisBuffer((uint8_t*) mPreviewBufs.keyAt(mPreviewIdxs.valueFor(index)), CameraFrame::PREVIEW_FRAME_SYNC);
                 //CAMHAL_LOGEA("jpeg decode failed");
                 return -1;
-            }   
+            }            
             frame.mLength = width*height*3/2;
         }else{
             if(DEFAULT_PREVIEW_PIXEL_FORMAT == V4L2_PIX_FMT_YUYV){ // 422I
@@ -2573,6 +2573,7 @@ extern "C" void loadCaps(int camera_id, CameraProperties::Properties* params) {
     const char DEFAULT_PICTURE_FORMAT[] = "jpeg";
     const char DEFAULT_PICTURE_SIZE[] = "640x480";
     const char PREVIEW_FORMAT_420SP[] = "yuv420sp";
+    const char PREVIEW_FORMAT_420P[] = "yuv420p";
     const char PREVIEW_FORMAT_422I[] = "yuv422i-yuyv";
     const char DEFAULT_PREVIEW_SIZE[] = "640x480";
     const char DEFAULT_NUM_PREV_BUFS[] = "6";
@@ -2668,7 +2669,7 @@ extern "C" void loadCaps(int camera_id, CameraProperties::Properties* params) {
         params->set(CameraProperties::PREVIEW_FORMAT,PREVIEW_FORMAT_420SP);
     }else{ //default case
         //params->set(CameraProperties::SUPPORTED_PREVIEW_FORMATS,PREVIEW_FORMAT_420SP);
-        params->set(CameraProperties::PREVIEW_FORMAT,PREVIEW_FORMAT_420SP);
+        params->set(CameraProperties::PREVIEW_FORMAT,PREVIEW_FORMAT_420P);
     }
 
 #ifdef AMLOGIC_USB_CAMERA_SUPPORT
diff --git a/inc/mjpeg/colorspaces.h b/inc/mjpeg/colorspaces.h
index 8787c4f..3479568 100755
--- a/inc/mjpeg/colorspaces.h
+++ b/inc/mjpeg/colorspaces.h
@@ -254,6 +254,12 @@ yuv420pto422(int * out,unsigned char *pic,int width);
 void 
 yuv422pto422(int * out,unsigned char *pic,int width);
 
+void 
+yuv420pto420sp(int * out,addr *pic,int width);
+
+void 
+yuv420pto420p(int * out,addr *pic,int width);
+
 void
 yuv422pto420sp(int * out, addr *pic,int width);
 
diff --git a/mjpeg/colorspaces.c b/mjpeg/colorspaces.c
index 9fdce8f..723505c 100755
--- a/mjpeg/colorspaces.c
+++ b/mjpeg/colorspaces.c
@@ -1398,6 +1398,302 @@ void yuv422pto422(int * out,unsigned char *pic,int width)
 	}
 }
 
+void yuv420pto420sp(int * out, addr *pic, int width)
+{
+    int j, k;
+    unsigned char *pic0, *pic1, *uv;
+    int *outy, *outu, *outv;
+    int *outy1 ;
+    int *outy2 ;
+    int *outu1 ;
+    int *outv1 ;
+
+    pic0 = pic->y;
+    pic1 = pic->y + width;
+    uv = pic->v;
+    outy = out;
+    outu = out + 64 * 4;
+    outv = out + 64 * 5;    
+
+    for (j = 0; j < 8; j++) 
+    {
+        outy1 = outy;
+        outy2 = outy+8;
+        outv1 = outv;
+        outu1 = outu;
+        
+        {
+            asm volatile(
+                    "mov r0,#0                                          \n\t"
+                    "vdup.u32 d30, r0                                   \n\t"
+                    "mov r0,#255                                        \n\t"
+                    "vdup.u32 d31, r0                                   \n\t"
+                    
+                    /***	  line1				***/
+                    "mov r0, #256  @256=64*4\n\t"
+                    "vld4.32 {d26,d27,d28,d29}, [%[outy1]], r0          \n\t"
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[pic0]]!  \n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[pic0]]!  \n\t"
+                    
+					/***		mb 2            ***/
+					"vld4.32 {d26,d27,d28,d29}, [%[outy1]]		        \n\t"
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[pic0]]!  \n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[pic0]]!  \n\t"
+                    
+                    /***	    line2			***/
+                    "vld4.32 {d26,d27,d28,d29}, [%[outy2]],r0           \n\t"
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[pic1]]!  \n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[pic1]]!  \n\t"
+
+					/***	     mb2			***/
+                    "vld4.32 {d26,d27,d28,d29}, [%[outy2]]          	\n\t"
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[pic1]]!  \n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[pic1]]!  \n\t"
+                    
+					/***		 uv				***/
+                    "mov r0, #16 @16=4*4                                \n\t"
+                    "vld4.32 {d22,d24,d26,d28}, [%[outv1]], r0          \n\t"
+                    "vld4.32 {d23,d25,d27,d29}, [%[outu1]], r0          \n\t"
+
+                    "mov r0, #128                                       \n\t"
+                    "vdup.u32 d30, r0                                   \n\t"
+                    "vqadd.s32 d22, d22, d30                            \n\t"
+                    "vqadd.s32 d23, d23, d30                            \n\t"
+                    "vqadd.s32 d24, d24, d30                            \n\t"
+                    "vqadd.s32 d25, d25, d30                            \n\t"
+                    "vqadd.s32 d26, d26, d30                            \n\t"
+                    "vqadd.s32 d27, d27, d30                            \n\t"
+                    "vqadd.s32 d28, d28, d30                            \n\t"
+                    "vqadd.s32 d29, d29, d30                            \n\t"
+
+                    "mov r0, #0                                         \n\t"
+                    "vdup.u32 d30, r0                                   \n\t"
+
+                    "vmax.s32 d22, d22, d30                             \n\t"
+                    "vmin.s32 d22, d22, d31                             \n\t"
+                    "vmax.s32 d24, d24, d30                             \n\t"
+                    "vmin.s32 d24, d24, d31                             \n\t"
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"
+
+                    "vmax.s32 d23, d23, d30                             \n\t"
+                    "vmin.s32 d23, d23, d31                             \n\t"
+                    "vmax.s32 d25, d25, d30                             \n\t"
+                    "vmin.s32 d25, d25, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+
+                    "vst4.8  {d22[0],d23[0],d24[0],d25[0]}, [%[uv]]!  	\n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[uv]]!  	\n\t"
+                    "vst4.8  {d22[4],d23[4],d24[4],d25[4]}, [%[uv]]!  	\n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[uv]]!  	\n\t"
+                    //////////////////////////////
+
+                    "4:@end                                             \n\t"
+                    : [outy1] "+r" (outy1), [outy2] "+r" (outy2), 
+                [pic0]  "+r" (pic0),  [pic1] "+r" (pic1), 
+                [outu1] "+r" (outu1), [outv1] "+r" (outv1), 
+                [uv] "+r" (uv)
+                    : [width] "r" (width)
+                       : "cc", "memory", "r0","q11", "q12", "q13","q14","q15"
+                           );
+        }
+        if(j == 3)
+        	outy += 80;
+        else
+        	outy += 16;
+        outu +=8; outv +=8;
+        pic0 += 2 * (width - 8);
+        pic1 += 2 * (width - 8);
+        uv  += width - 16;
+    }
+}
+
+void yuv420pto420p(int * out, addr *pic, int width)
+{
+    int j, k;
+    unsigned char *pic0, *pic1, *u, *v;
+    int *outy, *outu, *outv;
+    int *outy1 ;
+    int *outy2 ;
+    int *outu1 ;
+    int *outv1 ;
+
+    pic0 = pic->y;
+    pic1 = pic->y + width;
+    v = pic->v;
+    u = pic->u;
+    outy = out;
+    outu = out + 64 * 4;
+    outv = out + 64 * 5;    
+
+    for (j = 0; j < 8; j++) 
+    {
+        outy1 = outy;
+        outy2 = outy+8;
+        outv1 = outv;
+        outu1 = outu;
+
+        {
+            asm volatile(
+                    "mov r0,#0                                          \n\t"
+                    "vdup.u32 d30, r0                                   \n\t"
+                    "mov r0,#255                                        \n\t"
+                    "vdup.u32 d31, r0                                   \n\t"
+                    
+                    /***	  line1				***/
+                    "mov r0, #256  @256=64*4\n\t"
+                    "vld4.32 {d26,d27,d28,d29}, [%[outy1]], r0          \n\t"
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[pic0]]!  \n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[pic0]]!  \n\t"
+                    
+					/***		mb 2            ***/
+					"vld4.32 {d26,d27,d28,d29}, [%[outy1]]		        \n\t"
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[pic0]]!  \n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[pic0]]!  \n\t"
+                    
+                    /***	    line2			***/
+                    "vld4.32 {d26,d27,d28,d29}, [%[outy2]],r0           \n\t"
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[pic1]]!  \n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[pic1]]!  \n\t"
+
+					/***	     mb2			***/
+                    "vld4.32 {d26,d27,d28,d29}, [%[outy2]]          	\n\t"
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[pic1]]!  \n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[pic1]]!  \n\t"
+                    
+					/***		 uv				***/
+                    "mov r0, #16 @16=4*4                                \n\t"
+                    "vld4.32 {d22,d23,d24,d25}, [%[outv1]], r0          \n\t"
+                    "vld4.32 {d26,d27,d28,d29}, [%[outu1]], r0          \n\t"
+
+                    "mov r0, #128                                       \n\t"
+                    "vdup.u32 d30, r0                                   \n\t"
+                    "vqadd.s32 d22, d22, d30                            \n\t"
+                    "vqadd.s32 d23, d23, d30                            \n\t"
+                    "vqadd.s32 d24, d24, d30                            \n\t"
+                    "vqadd.s32 d25, d25, d30                            \n\t"
+                    "vqadd.s32 d26, d26, d30                            \n\t"
+                    "vqadd.s32 d27, d27, d30                            \n\t"
+                    "vqadd.s32 d28, d28, d30                            \n\t"
+                    "vqadd.s32 d29, d29, d30                            \n\t"
+
+                    "mov r0, #0                                         \n\t"
+                    "vdup.u32 d30, r0                                   \n\t"
+
+                    "vmax.s32 d22, d22, d30                             \n\t"
+                    "vmin.s32 d22, d22, d31                             \n\t"
+                    "vmax.s32 d23, d23, d30                             \n\t"
+                    "vmin.s32 d23, d23, d31                             \n\t"
+                    "vmax.s32 d24, d24, d30                             \n\t"
+                    "vmin.s32 d24, d24, d31                             \n\t"
+                    "vmax.s32 d25, d25, d30                             \n\t"
+                    "vmin.s32 d25, d25, d31                             \n\t"
+
+                    "vmax.s32 d26, d26, d30                             \n\t"
+                    "vmin.s32 d26, d26, d31                             \n\t"
+                    "vmax.s32 d27, d27, d30                             \n\t"
+                    "vmin.s32 d27, d27, d31                             \n\t"
+                    "vmax.s32 d28, d28, d30                             \n\t"
+                    "vmin.s32 d28, d28, d31                             \n\t"                    
+                    "vmax.s32 d29, d29, d30                             \n\t"
+                    "vmin.s32 d29, d29, d31                             \n\t"
+
+                    "vst4.8  {d22[0],d23[0],d24[0],d25[0]}, [%[v]]!  	\n\t"
+                    "vst4.8  {d22[4],d23[4],d24[4],d25[4]}, [%[v]]!  	\n\t"
+                    "vst4.8  {d26[0],d27[0],d28[0],d29[0]}, [%[u]]!  	\n\t"
+                    "vst4.8  {d26[4],d27[4],d28[4],d29[4]}, [%[u]]!  	\n\t"
+                    //////////////////////////////
+
+                    "4:@end                                             \n\t"
+                    : [outy1] "+r" (outy1), [outy2] "+r" (outy2), 
+                [pic0]  "+r" (pic0),  [pic1] "+r" (pic1), 
+                [outu1] "+r" (outu1), [outv1] "+r" (outv1), 
+                [u] "+r" (u), [v] "+r" (v)
+                    : [width] "r" (width)
+                       : "cc", "memory", "r0","q11","q12","q13","q14","q15"
+                           );
+        }
+        if(j == 3)
+        	outy += 80;
+        else
+        	outy += 16;
+        outu += 8; outv += 8;
+        pic0 += 2 * (width - 8);
+        pic1 += 2 * (width - 8);
+        u += width / 2 - 8;
+        v += width / 2 - 8;
+    }
+}
+
 void yuv422pto420sp(int * out, addr *pic, int width)
 {
     int j, k;
@@ -1505,7 +1801,7 @@ void yuv422pto420sp(int * out, addr *pic, int width)
                 [outu1] "+r" (outu1), [outv1] "+r" (outv1), 
                 [uv] "+r" (uv)
                     : [width] "r" (width)
-                       : "cc", "memory", "r0","r1", "r2", "r4", "q0", "q1"
+                       : "cc", "memory", "r0","q11","q12","q13","q14","q15"
                            );
         }
         outy += 16;outu +=8; outv +=8;
@@ -1621,7 +1917,7 @@ void yuv422pto420p(int * out, addr *pic, int width)
                 [outu1] "+r" (outu1), [outv1] "+r" (outv1), 
                 [v] "+r" (v),[u] "+r" (u)
                     : [width] "r" (width)
-                       : "cc", "memory", "r0","r1", "r2", "r4", "q0", "q1"
+                       : "cc", "memory", "r0","q11", "q12", "q13","q14","q15"
                            );
         }
         outy += 16;outu +=8; outv +=8;
diff --git a/mjpeg/jpegdec.c b/mjpeg/jpegdec.c
index 1f2166b..8794df1 100755
--- a/mjpeg/jpegdec.c
+++ b/mjpeg/jpegdec.c
@@ -242,6 +242,9 @@ int jpeg_decode(BYTE **pic, BYTE *buf, int width, int height, unsigned int outfo
 	ftopict convert;
 	int err = 0;
 	int isInitHuffman = 0;
+	int mb_x,mb_y;
+	int mc_x,mc_y;
+	int down_sampling = 1;
 	decdata = (struct jpeg_decdata *)malloc(sizeof(struct jpeg_decdata));
 	
 	for(i=0;i<6;i++) 
@@ -397,7 +400,6 @@ int jpeg_decode(BYTE **pic, BYTE *buf, int width, int height, unsigned int outfo
 		goto error;
 	}
 
-	int stride = 0;
 	switch (dscans[0].hv) 
 	{
 		case 0x22: // 411
@@ -408,9 +410,18 @@ int jpeg_decode(BYTE **pic, BYTE *buf, int width, int height, unsigned int outfo
 			xpitch = 16 * bpp;
 			pitch = width * bpp; // YUYV out
 			ypitch = 16 * pitch;
-			//convert = yuv420pto422; //choose the right conversion function
-			err = ERR_NOT_SUPPORTED;
-			goto error;
+			mb_x = 16;
+			mb_y = 16;
+			mc_y = 8;
+			if(outformat == V4L2_PIX_FMT_NV21){
+				convert = yuv420pto420sp; //choose the right conversion function
+				mc_x = 16;
+				down_sampling = 1;
+			}else{
+				convert = yuv420pto420p;
+				mc_x = 8;
+				down_sampling = 2;
+			}			
 			break;
 		case 0x21: //422
 			mb=4;
@@ -420,12 +431,17 @@ int jpeg_decode(BYTE **pic, BYTE *buf, int width, int height, unsigned int outfo
 			xpitch = 16 * bpp;
 			pitch = width * bpp; // YUYV out
 			ypitch = 8 * pitch;
+			mb_x = 16;
+			mb_y = 8;
+			mc_y = 8;
 			if(outformat == V4L2_PIX_FMT_NV21){
 				convert = yuv422pto420sp; //choose the right conversion function
-				stride = 2;
+				mc_x = 16;
+				down_sampling = 2;
 			}else{
 				convert = yuv422pto420p;
-				stride = 4;
+				mc_x = 8;
+				down_sampling = 4;
 			}
 			break;
 		case 0x11: //444
@@ -439,11 +455,13 @@ int jpeg_decode(BYTE **pic, BYTE *buf, int width, int height, unsigned int outfo
 			{
 				mb = 1;
 				//convert = yuv400pto422; //choose the right conversion function
+                CAMHAL_LOGEA("Format YUV400 Not Supproted");
 			}
 			else 
 			{
 				mb=3;
 				//convert = yuv444pto422; //choose the right conversion function
+                CAMHAL_LOGEA("Format YUV444 Not Supproted");
 			}
 			err = ERR_NOT_SUPPORTED;
 			goto error;
@@ -522,9 +540,9 @@ int jpeg_decode(BYTE **pic, BYTE *buf, int width, int height, unsigned int outfo
 					break;
 			} // switch enc411
 
-			paddr.y = *pic+my*width*8+mx*16; 
-			paddr.v = *pic+width*height+my*width*8/stride+mx*32/stride;
-			paddr.u = *pic+width*height*5/4+my*width*8/stride+mx*32/stride;
+			paddr.y = *pic + my * width * mb_y + mx * mb_x;
+			paddr.v = *pic + width * height + my * width * mc_y / down_sampling + mx * mc_x;
+			paddr.u = *pic + width * height*5/4 + my * width * mc_y / down_sampling + mx * mc_x;
 			convert(decdata->out,&paddr,width); 
 		}
 	}