Browse Source

v4l2_test: refactor the v4l2test to support both framebuffer and drm framework

1. keep the framebuffer features same with previous performance
2. drm support dmabuf between v4l2 and drm to implement the zero copy, the resolution need be found in the drm monitor modes
3. drm support mmap, this support any resolution
4. support sensor and uvc camera with YUYV
5. add debug log feature
6. usage see the help

Signed-off-by: andy.hu <andy.hu@starfivetech.com>
andy.hu 2 years ago
parent
commit
f0f444ed87

+ 303 - 0
package/starfive/v4l2_test/common.c

@@ -0,0 +1,303 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "common.h"
+#include <stddef.h>
+#include <jpeglib.h>
+#include <libdrm/drm_fourcc.h>
+#include "convert.h"
+#include "stf_log.h"
+
+/**
+    Print error message and terminate programm with EXIT_FAILURE return code.
+    \param s error message to print
+*/
+void errno_exit(const char* s)
+{
+    LOG(STF_LEVEL_ERR, "%s error %d, %s\n", s, errno, strerror(errno));
+    exit(EXIT_FAILURE);
+}
+
+void errno_print(const char *s)
+{
+    LOG(STF_LEVEL_ERR, "%s error %d, %s\n", s, errno, strerror(errno));
+}
+
+void dump_fourcc(uint32_t fourcc)
+{
+    LOG(STF_LEVEL_LOG, " %c%c%c%c \n",
+        fourcc,
+        fourcc >> 8,
+        fourcc >> 16,
+        fourcc >> 24);
+}
+
+// convert v4l2 format to fb format
+int v4l2fmt_to_fbfmt(uint32_t format)
+{
+    int pixformat = COLOR_RGB565;
+
+    switch (format) {
+    case V4L2_PIX_FMT_RGB565:
+        pixformat = COLOR_RGB565;
+        break;
+    case V4L2_PIX_FMT_RGB24:
+        pixformat = COLOR_RGB888_ARGB;
+        break;
+    case V4L2_PIX_FMT_YUV420:
+        pixformat = COLOR_YUV420P;
+        break;
+    case V4L2_PIX_FMT_YUYV:
+        pixformat = COLOR_YUV422_YUYV;
+        break;
+    case V4L2_PIX_FMT_NV21:
+        pixformat = COLOR_YUV420_NV21;
+        break;
+    case V4L2_PIX_FMT_NV12:
+        pixformat = COLOR_YUV420_NV12;
+        break;
+    case V4L2_PIX_FMT_YVYU:
+        pixformat = COLOR_YUV422_YVYU;
+        break;
+    default:
+        pixformat = COLOR_RGB565;
+        break;
+    }
+
+    return pixformat;
+}
+
+// convert v4l2 format to drm format
+uint32_t v4l2fmt_to_drmfmt(uint32_t v4l2_fmt)
+{
+    uint32_t drm_fmt;
+    // dump_fourcc(v4l2_fmt);
+
+    switch (v4l2_fmt) {
+    case V4L2_PIX_FMT_RGB565:
+        drm_fmt = DRM_FORMAT_RGB565;
+        break;
+    case V4L2_PIX_FMT_RGB24:
+        drm_fmt = DRM_FORMAT_ARGB8888;
+        break;
+    case V4L2_PIX_FMT_YUV420:
+        drm_fmt = DRM_FORMAT_YUV420;
+        break;
+    case V4L2_PIX_FMT_YUYV:
+        drm_fmt = DRM_FORMAT_YUYV;
+        break;
+    case V4L2_PIX_FMT_YVYU:
+        drm_fmt = DRM_FORMAT_YVYU;
+        break;
+    case V4L2_PIX_FMT_NV21:
+        drm_fmt = DRM_FORMAT_NV21;
+        break;
+    case V4L2_PIX_FMT_NV12:
+        drm_fmt = DRM_FORMAT_NV12;
+        break;
+    default:
+        drm_fmt = DRM_FORMAT_NV21;
+        LOG(STF_LEVEL_WARN, "drm not support the V4L2_format\n");
+        break;
+    }
+    return drm_fmt;
+}
+
+void test_performance()
+{
+    int count = 1920 * 1080 * 2;
+    float r, g, b;
+    float y = 10, u = 20, v = 30;
+    int int_r, int_g, int_b;
+    int int_y = 10, int_u = 20, int_v = 30;
+    struct timeval tv1, tv2, tv3;
+    long long elapse = 0;
+    int i;
+
+    gettimeofday(&tv1, NULL);
+    while (count--) {
+        b = 1.164 * (y - 16) + 2.018 * (u - 128);
+        g = 1.164 * (y - 16) - 0.813 * (v - 128) - 0.391 * (u - 128);
+        r = 1.164 * (y - 16) + 1.596 * (v - 128);
+        y ++;
+        u ++;
+        v ++;
+    }
+    gettimeofday(&tv2, NULL);
+    elapse = (tv2.tv_sec - tv1.tv_sec) * 1000 + (tv2.tv_usec - tv1.tv_usec) / 1000;
+    LOG(STF_LEVEL_INFO, "elapse: %lldms, out: r=%f, g=%f, b=%f\n",
+            elapse ,r, g, b);
+
+    count = 1920 * 1080 * 2;
+    gettimeofday(&tv1, NULL);
+    while (count--) {
+        int_b = 1164 * (y - 16) + 2018 * (u - 128);
+        int_g = 1164 * (y - 16) - 813 * (v - 128) - 391 * (u - 128);
+        int_r = 1164 * (y - 16) + 1596 * (v - 128);
+        int_y ++;
+        int_u ++;
+        int_v ++;
+    }
+    gettimeofday(&tv2, NULL);
+    elapse = (tv2.tv_sec - tv1.tv_sec) + (tv2.tv_usec - tv1.tv_usec);
+    LOG(STF_LEVEL_INFO, "elapse: %lldus, out: r=%d, g=%d, b=%d\n",
+            elapse , int_r, int_g, int_b);
+
+    count = 1920 * 1080 * 2;
+    unsigned char* arraybuf = NULL;
+    arraybuf = (unsigned char*)malloc(count);
+    if (!arraybuf) {
+        LOG(STF_LEVEL_ERR, "arraybuf malloc error\n");
+        return;
+    }
+
+    unsigned char* arraybuf2 = NULL;
+    arraybuf2 = (unsigned char*)malloc(count);
+    if (!arraybuf2) {
+        LOG(STF_LEVEL_ERR, "arraybuf2 malloc error\n");
+        return;
+    }
+
+    gettimeofday(&tv1, NULL);
+
+    for (i = 0; i < count; i++) {
+        arraybuf[i] = i + 1;
+    }
+
+    gettimeofday(&tv2, NULL);
+
+    memcpy(arraybuf2, arraybuf, count);
+
+    gettimeofday(&tv3, NULL);
+
+    elapse = (tv2.tv_sec - tv1.tv_sec) * 1000 + (tv2.tv_usec - tv1.tv_usec) / 1000;
+    LOG(STF_LEVEL_INFO, "for() elapse: %lldms\n", elapse);
+
+    elapse = (tv3.tv_sec - tv2.tv_sec) * 1000000 + (tv3.tv_usec - tv2.tv_usec);
+    LOG(STF_LEVEL_INFO, "memcpy() run elapse: %lldus\n", elapse);
+
+    convert_nv21_to_rgb((const uint8_t*)arraybuf, arraybuf2, 1920, 1080, 1);
+
+    free(arraybuf);
+    free(arraybuf2);
+}
+
+int write_file(char * filename, const uint8_t *image_buffer, int size)
+{
+    /*  More stuff */
+    FILE * outfile;               /*  target file */
+    if ((outfile = fopen(filename, "w+")) == NULL) {
+        LOG(STF_LEVEL_ERR, "can't open %s\n", filename);
+        return -1;
+    }
+    fwrite(image_buffer, size, 1, outfile);
+    fclose(outfile);
+    return 0 ;
+}
+
+int write_JPEG_file(char * filename, uint8_t *image_buffer,
+        int image_width, int image_height, int quality)
+{
+    struct jpeg_compress_struct cinfo;
+    struct jpeg_error_mgr jerr;
+
+    /*  More stuff */
+    FILE * outfile;               /*  target file */
+    JSAMPROW row_pointer[1];      /*  pointer to JSAMPLE row[s] */
+    int row_stride;               /*  physical row width in image buffer */
+
+    /*  Step 1: allocate and initialize JPEG compression object */
+    cinfo.err = jpeg_std_error(&jerr);
+    /*  Now we can initialize the JPEG compression object. */
+    jpeg_create_compress(&cinfo);
+
+    /*  Step 2: specify data destination (eg, a file) */
+    /*  Note: steps 2 and 3 can be done in either order. */
+    if ((outfile = fopen(filename, "w+")) == NULL) {
+        LOG(STF_LEVEL_ERR, "can't open %s\n", filename);
+        return -1;
+    }
+    jpeg_stdio_dest(&cinfo, outfile);
+
+    /*  Step 3: set parameters for compression */
+    cinfo.image_width = image_width;      /*  image width and height, in pixels */
+    cinfo.image_height = image_height;
+    cinfo.input_components = 3;           /*  # of color components per pixel */
+    cinfo.in_color_space = JCS_RGB;       /*  colorspace of input image */
+    jpeg_set_defaults(&cinfo);
+    jpeg_set_quality(&cinfo, quality, TRUE /*  limit to baseline-JPEG values */);
+
+    /*  Step 4: Start compressor */
+    jpeg_start_compress(&cinfo, TRUE);
+
+    /*  Step 5: while (scan lines remain to be written) */
+    row_stride = image_width * 3; /*  JSAMPLEs per row in image_buffer */
+    while (cinfo.next_scanline < cinfo.image_height) {
+        row_pointer[0] = & image_buffer[cinfo.next_scanline * row_stride];
+        (void) jpeg_write_scanlines(&cinfo, row_pointer, 1);
+    }
+
+    /*  Step 6: Finish compression */
+    jpeg_finish_compress(&cinfo);
+    /*  After finish_compress, we can close the output file. */
+    fclose(outfile);
+
+    /*  Step 7: release JPEG compression object */
+    /*  This is an important step since it will release a good deal of memory. */
+    jpeg_destroy_compress(&cinfo);
+
+    /*  And we're done! */
+    return 0;
+}
+
+void jpegWrite(uint8_t* img, char* jpegFilename,
+        uint32_t width, uint32_t height, int jpegQuality)
+{
+    struct jpeg_compress_struct cinfo;
+    struct jpeg_error_mgr jerr;
+
+    JSAMPROW row_pointer[1];
+    FILE *outfile = fopen( jpegFilename, "wb" );
+
+    // try to open file for saving
+    if (!outfile) {
+        errno_exit("jpeg");
+    }
+
+    // create jpeg data
+    cinfo.err = jpeg_std_error( &jerr );
+    jpeg_create_compress(&cinfo);
+    jpeg_stdio_dest(&cinfo, outfile);
+
+    // set image parameters
+    cinfo.image_width = width;
+    cinfo.image_height = height;
+    cinfo.input_components = 3;
+    cinfo.in_color_space = JCS_YCbCr;
+
+    // set jpeg compression parameters to default
+    jpeg_set_defaults(&cinfo);
+    // and then adjust quality setting
+    jpeg_set_quality(&cinfo, jpegQuality, TRUE);
+
+    // start compress
+    jpeg_start_compress(&cinfo, TRUE);
+
+    // feed data
+    while (cinfo.next_scanline < cinfo.image_height) {
+        row_pointer[0] = &img[cinfo.next_scanline * cinfo.image_width *  cinfo.input_components];
+        jpeg_write_scanlines(&cinfo, row_pointer, 1);
+    }
+
+    // finish compression
+    jpeg_finish_compress(&cinfo);
+
+    // destroy jpeg data
+    jpeg_destroy_compress(&cinfo);
+
+    // close output file
+    fclose(outfile);
+}
+
+

+ 89 - 0
package/starfive/v4l2_test/common.h

@@ -0,0 +1,89 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef __COMMON_H__
+#define __COMMON_H__
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <time.h>
+#include <asm/types.h>
+#include <linux/videodev2.h>
+#include <linux/fb.h>
+#include <string.h>
+#include <errno.h>
+#include <assert.h>
+#include <fcntl.h>
+#include <malloc.h>
+
+#ifndef TEST_VERSION
+#define TEST_VERSION "v0.0.1"
+#endif
+
+#define BUFCOUNT 4
+
+#define CLEAR(x)  memset (&(x), 0, sizeof (x))
+#define PCLEAR(x) memset ((x), 0, sizeof (*x))
+
+struct buffer {
+    void*   start;
+    size_t  length;
+    int     dmabuf_fd;
+    int     index;
+};
+
+enum COLOR_FORMAT{
+    COLOR_YUV422_UYVY = 0,  // 00={Y1,V0,Y0,U0}
+    COLOR_YUV422_VYUY = 1,  // 01={Y1,U0,Y0,V0}
+    COLOR_YUV422_YUYV = 2,  // 10={V0,Y1,U0,Y0}
+    COLOR_YUV422_YVYU = 3,  // 11={U0,Y1,V0,Y0}
+
+    COLOR_YUV420P,          // 4
+    COLOR_YUV420_NV21,      // 5
+    COLOR_YUV420_NV12,      // 6
+
+    COLOR_RGB888_ARGB,      // 7
+    COLOR_RGB888_ABGR,      // 8
+    COLOR_RGB888_RGBA,      // 9
+    COLOR_RGB888_BGRA,      // 10
+    COLOR_RGB565,           // 11
+};
+
+typedef enum IOMethod {
+    IO_METHOD_MMAP,
+    IO_METHOD_USERPTR,
+    IO_METHOD_DMABUF,
+    IO_METHOD_READ
+} IOMethod;
+
+typedef enum STF_DISP_TYPE {
+    STF_DISP_NONE = 0, // Not display
+    STF_DISP_FB,       // Use framebuffer framework display
+    STF_DISP_DRM       // Use drm framework display
+} STF_DISP_TYPE;
+
+extern void dump_fourcc(uint32_t fourcc);
+extern int v4l2fmt_to_fbfmt(uint32_t format);
+extern uint32_t v4l2fmt_to_drmfmt(uint32_t v4l2_fmt);
+extern void test_performance();
+extern void errno_exit(const char* s);
+extern void errno_print(const char *s);
+extern int write_file(char * filename, const unsigned char *image_buffer, int size);
+// extern void jpegWrite(unsigned char* img, char* jpegFilename);
+extern void jpegWrite(unsigned char* img, char* jpegFilename,
+        uint32_t width, uint32_t height, int jpegQuality);
+extern int write_JPEG_file(char * filename,unsigned char *image_buffer,
+        int image_width, int image_height, int quality);
+
+// inline int clip(int value, int min, int max) {
+//     return (value > max ? max : value < min ? min : value);
+// }
+
+
+#endif // __COMMON_H__

+ 206 - 197
package/starfive/v4l2_test/convert.c

@@ -2,41 +2,35 @@
 /*
  * Copyright (C) 2021 StarFive Technology Co., Ltd.
  */
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <assert.h>
-#include <getopt.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <errno.h>
-#include <malloc.h>
-#include <sys/stat.h>
-#include <sys/types.h>
-#include <sys/time.h>
-#include <time.h>
-#include <sys/mman.h>
-#include <sys/ioctl.h>
-#include <asm/types.h>
-#include <signal.h>
-#include <stdint.h>
-#include <inttypes.h>
-#include <linux/fb.h>
-
-extern struct fb_var_screeninfo g_vinfo;
-extern struct fb_fix_screeninfo g_finfo;
-extern unsigned int g_screensize;
-
-int yuyv_resize(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight)
+#include "common.h"
+
+static int g_screen_width = 0;
+static int g_screen_height = 0;
+static int g_screen_bpp = 0;
+static int g_screen_size = 0;
+static int g_screen_distype = 0; // STF_DISP_FB or STF_DISP_DRM
+
+void update_videocvt_param(int distype, int scr_width, int scr_height,
+        int scr_bpp, int scr_size)
+{
+    g_screen_distype = distype;
+    g_screen_width = scr_width;
+    g_screen_height = scr_height;
+    g_screen_bpp = scr_bpp;
+    g_screen_size = scr_size;
+}
+
+int yuyv_resize(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight)
 {
     int rows;
-    unsigned char *YUVindata, *YUVoutdata;    /* YUV and RGB pointer */
+    const uint8_t *YUVindata;
+    uint8_t *YUVoutdata;    /* YUV and RGB pointer */
     int YUVinpos;    /* Y U V offset */
     int width, height;
     int x_offset, y_offset;
-    unsigned char *tmp = malloc(g_screensize);
-    unsigned int start_timems;
-    unsigned int end_timems;
+    uint8_t *tmp = malloc(g_screen_size);
+    uint32_t start_timems;
+    uint32_t end_timems;
     struct timeval ts_start, ts_end;
 
     if (!tmp)
@@ -44,16 +38,16 @@ int yuyv_resize(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int i
 
     gettimeofday(&ts_start, NULL);
 
-    width = imgWidth > g_vinfo.xres ? g_vinfo.xres : imgWidth;
-    height = imgHeight > g_vinfo.yres ? g_vinfo.yres : imgHeight;
-    x_offset = (g_vinfo.xres - width) / 2;
-    y_offset = (g_vinfo.yres - height) / 2;
+    width = imgWidth > g_screen_width ? g_screen_width : imgWidth;
+    height = imgHeight > g_screen_height ? g_screen_height : imgHeight;
+    x_offset = (g_screen_width - width) / 2;
+    y_offset = (g_screen_height - height) / 2;
 
     YUVindata = inBuf;
     YUVoutdata = tmp;
 
-    if (imgWidth == g_vinfo.xres) {
-        YUVinpos = (y_offset * g_vinfo.xres + x_offset) * 2;
+    if (imgWidth == g_screen_width) {
+        YUVinpos = (y_offset * g_screen_width + x_offset) * 2;
         memcpy(&tmp[YUVinpos], inBuf, imgWidth * height * 2);
         memcpy(&outBuf[YUVinpos], &tmp[YUVinpos], imgWidth * height * 2);
         // memcpy(&outBuf[YUVinpos], inBuf, imgWidth * height * 2);
@@ -68,8 +62,8 @@ int yuyv_resize(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int i
     /* two bytes for one pixels */
     for(rows = 0; rows < height; rows++)
     {
-        // g_vinfo.xres, g_vinfo.yres g_vinfo.bits_per_pixel
-        YUVoutdata = tmp + ((rows + y_offset) * g_vinfo.xres + x_offset) * 2;
+        // g_screen_width, g_screen_height g_screen_bpp
+        YUVoutdata = tmp + ((rows + y_offset) * g_screen_width + x_offset) * 2;
         YUVinpos = rows * imgWidth * 2;
 
         memcpy(YUVoutdata, &YUVindata[YUVinpos], imgWidth * 2);
@@ -82,7 +76,7 @@ int yuyv_resize(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int i
 
     gettimeofday(&ts_start, NULL);
 
-    memcpy(outBuf, tmp, g_screensize);
+    memcpy(outBuf, tmp, g_screen_size);
 
     gettimeofday(&ts_end, NULL);
     start_timems = ts_start.tv_sec * 1000 + ts_start.tv_usec/1000;
@@ -94,17 +88,18 @@ int yuyv_resize(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int i
 
 }
 
-int convert_yuyv_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int is_yuyv)
+int convert_yuyv_to_nv12(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int is_yuyv)
 {
     int rows, cols;
-    unsigned char *nv12data, *YUVdata;
+    const uint8_t *YUVdata;
+    uint8_t *nv12data;
     int Ypos;
     int fb_Ypos, fb_Upos, fb_Vpos;
     int width, height;
     int x_offset, y_offset;
-    unsigned char *tmp = malloc(g_screensize);
-    unsigned int start_timems;
-    unsigned int end_timems;
+    uint8_t *tmp = malloc(g_screen_size);
+    uint32_t start_timems;
+    uint32_t end_timems;
     struct timeval ts_start, ts_end;
 
     if (!tmp)
@@ -112,10 +107,10 @@ int convert_yuyv_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWid
 
     gettimeofday(&ts_start, NULL);
 
-    width = imgWidth > g_vinfo.xres ? g_vinfo.xres : imgWidth;
-    height = imgHeight > g_vinfo.yres ? g_vinfo.yres : imgHeight;
-    x_offset = (g_vinfo.xres - width) / 2;
-    y_offset = (g_vinfo.yres - height) / 2;
+    width = imgWidth > g_screen_width ? g_screen_width : imgWidth;
+    height = imgHeight > g_screen_height ? g_screen_height : imgHeight;
+    x_offset = (g_screen_width - width) / 2;
+    y_offset = (g_screen_height - height) / 2;
 
     YUVdata = inBuf;
     nv12data = tmp;
@@ -123,10 +118,10 @@ int convert_yuyv_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWid
     /* two bytes for every pixels */
     for(rows = 0; rows < height; rows++)
     {
-        // g_vinfo.xres, g_vinfo.yres g_vinfo.bits_per_pixel
-        fb_Ypos = ((rows + y_offset) * g_vinfo.xres + x_offset);
-        fb_Upos = ((rows + y_offset) / 2 * g_vinfo.xres / 2 + x_offset / 2) * 2;
-        fb_Upos = g_vinfo.xres * g_vinfo.yres + fb_Upos;
+        // g_screen_width, g_screen_height g_screen_bpp
+        fb_Ypos = ((rows + y_offset) * g_screen_width + x_offset);
+        fb_Upos = ((rows + y_offset) / 2 * g_screen_width / 2 + x_offset / 2) * 2;
+        fb_Upos = g_screen_width * g_screen_height + fb_Upos;
         fb_Vpos = fb_Upos + 1;
 
         Ypos = rows * imgWidth * 2;
@@ -146,7 +141,7 @@ int convert_yuyv_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWid
 
     gettimeofday(&ts_start, NULL);
 
-    memcpy(outBuf, tmp, g_screensize);
+    memcpy(outBuf, tmp, g_screen_size);
 
     gettimeofday(&ts_end, NULL);
     start_timems = ts_start.tv_sec * 1000 + ts_start.tv_usec/1000;
@@ -157,43 +152,52 @@ int convert_yuyv_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWid
     return 0;
 }
 
-int convert_nv21_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int is_nv21)
+int convert_nv21_to_nv12(const uint8_t *inBuf, uint8_t *outBuf,
+        int imgWidth, int imgHeight, int uv_changed)
 {
     int rows, cols;
-    unsigned char *nv12data, *nv21data;
+    const uint8_t *nv21data;
+    uint8_t *nv12data;
     int Ypos, Upos, Vpos;
     int fb_Ypos, fb_Upos, fb_Vpos;
     int width, height;
     int x_offset, y_offset;
-    unsigned char *tmp = malloc(g_screensize);
-    unsigned int start_timems;
-    unsigned int end_timems;
+    uint8_t *tmp = malloc(g_screen_size);
+    uint32_t start_timems;
+    uint32_t end_timems;
     struct timeval ts_start, ts_end;
 
-    if (!tmp)
-        return -1;
-
+    assert(tmp);
     gettimeofday(&ts_start, NULL);
 
-    width = imgWidth > g_vinfo.xres ? g_vinfo.xres : imgWidth;
-    height = imgHeight > g_vinfo.yres ? g_vinfo.yres : imgHeight;
-    x_offset = (g_vinfo.xres - width) / 2;
-    y_offset = (g_vinfo.yres - height) / 2;
+    width = imgWidth > g_screen_width ? g_screen_width : imgWidth;
+    height = imgHeight > g_screen_height ? g_screen_height : imgHeight;
+    x_offset = (g_screen_width - width) / 2;
+    y_offset = (g_screen_height - height) / 2;
 
     nv21data = inBuf;
     nv12data = tmp;
 
-    if (imgWidth == g_vinfo.xres) {
-        fb_Ypos = y_offset * g_vinfo.xres + x_offset;
-        fb_Upos = (y_offset / 2 * g_vinfo.xres / 2 + x_offset / 2) * 2;
-        fb_Upos = g_vinfo.xres * g_vinfo.yres + fb_Upos;
+    // printf("[%s,%d]:imgWidth=%d, imgHeight=%d, g_screen_width=%d g_screen_height=%d g_screen_bpp=%d\n",__FUNCTION__,__LINE__,
+    //     imgWidth, imgHeight, g_screen_width, g_screen_height, g_screen_bpp);
+
+    // below for same image size and not uv changed
+    if (imgWidth == g_screen_width && !uv_changed) {
+        fb_Ypos = y_offset * g_screen_width + x_offset;
+        fb_Upos = (y_offset / 2 * g_screen_width / 2 + x_offset / 2) * 2;
+        fb_Upos = g_screen_width * g_screen_height + fb_Upos;
         Upos = imgWidth * imgHeight;
-        memcpy(&tmp[fb_Ypos], inBuf, imgWidth * height);
-        memcpy(&tmp[fb_Upos], &inBuf[Upos], imgWidth * height / 2);
-        memcpy(&outBuf[fb_Ypos], &tmp[fb_Ypos], imgWidth * height * 2);
-        memcpy(&outBuf[fb_Upos], &tmp[fb_Upos], imgWidth * height / 2);
-        // memcpy(&outBuf[fb_Ypos], inBuf, imgWidth * height);
-        // memcpy(&outBuf[fb_Upos], inBuf, imgWidth * height / 2);
+
+        if (STF_DISP_FB == g_screen_distype) {
+            // TODO: it is strange that with tmp buffer will more faster for framebuf
+            memcpy(&tmp[fb_Ypos], inBuf, imgWidth * height);
+            memcpy(&tmp[fb_Upos], &inBuf[Upos], imgWidth * height / 2);
+            memcpy(&outBuf[fb_Ypos], &tmp[fb_Ypos], imgWidth * height);
+            memcpy(&outBuf[fb_Upos], &tmp[fb_Upos], imgWidth * height / 2);
+        } else {
+            memcpy(&outBuf[fb_Ypos], inBuf, imgWidth * height);
+            memcpy(&outBuf[fb_Upos], &inBuf[Upos], imgWidth * height / 2);
+        }
         free(tmp);
         return 0;
     }
@@ -201,25 +205,26 @@ int convert_nv21_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWid
     /* two bytes for every pixels */
     for(rows = 0; rows < height; rows+=2)
     {
-        // g_vinfo.xres, g_vinfo.yres g_vinfo.bits_per_pixel
-        fb_Ypos = ((rows + y_offset) * g_vinfo.xres + x_offset);
-        fb_Upos = ((rows + y_offset) / 2 * g_vinfo.xres / 2 + x_offset / 2) * 2;
-        fb_Upos = g_vinfo.xres * g_vinfo.yres + fb_Upos;
+        // g_screen_width, g_screen_height g_screen_bpp
+        fb_Ypos = ((rows + y_offset) * g_screen_width + x_offset);
+        fb_Upos = ((rows + y_offset) / 2 * g_screen_width / 2 + x_offset / 2) * 2;
+        fb_Upos = g_screen_width * g_screen_height + fb_Upos;
         fb_Vpos = fb_Upos + 1;
 
         Ypos = rows * imgWidth;
         Upos = imgWidth * imgHeight + Ypos / 2;
         Vpos = Upos + 1;
         memcpy(&nv12data[fb_Ypos], &nv21data[Ypos], width);
-        memcpy(&nv12data[fb_Ypos+g_vinfo.xres], &nv21data[Ypos+imgWidth], width);
+        memcpy(&nv12data[fb_Ypos+g_screen_width], &nv21data[Ypos+imgWidth], width);
 
-        if (is_nv21) {
+        if (uv_changed) {
             for (cols = 0; cols < width; cols += 2) {
                 nv12data[fb_Upos+cols] = nv21data[Vpos+cols];
                 nv12data[fb_Vpos+cols] = nv21data[Upos+cols];
             }
-        } else
+        } else {
             memcpy(&nv12data[fb_Upos], &nv21data[Upos], width);
+        }
     }
 
     gettimeofday(&ts_end, NULL);
@@ -229,7 +234,7 @@ int convert_nv21_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWid
 
     gettimeofday(&ts_start, NULL);
 
-    memcpy(outBuf, tmp, g_screensize);
+    memcpy(outBuf, tmp, g_screen_size);
 
     gettimeofday(&ts_end, NULL);
     start_timems = ts_start.tv_sec * 1000 + ts_start.tv_usec/1000;
@@ -240,18 +245,19 @@ int convert_nv21_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWid
     return 0;
 }
 
-int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int is_nv21)
+int convert_nv21_to_rgb(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int is_nv21)
 {
     int rows ,cols;
     int y, u, v, r, g, b;
-    unsigned char *YUVdata, *RGBdata;
+    const uint8_t *YUVdata;
+    uint8_t *RGBdata;
     int Ypos, Upos, Vpos;
-    unsigned int i = 0;
+    uint32_t i = 0;
     int width, height;
     int x_offset, y_offset;
-    unsigned char *tmp = malloc(g_screensize);
-    unsigned int start_timems;
-    unsigned int end_timems;
+    uint8_t *tmp = malloc(g_screen_size);
+    uint32_t start_timems;
+    uint32_t end_timems;
     struct timeval ts_start, ts_end;
 
     if (!tmp)
@@ -259,10 +265,10 @@ int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
 
     gettimeofday(&ts_start, NULL);
 
-    width = imgWidth > g_vinfo.xres ? g_vinfo.xres : imgWidth;
-    height = imgHeight > g_vinfo.yres ? g_vinfo.yres : imgHeight;
-    x_offset = (g_vinfo.xres - width) / 2;
-    y_offset = (g_vinfo.yres - height) / 2;
+    width = imgWidth > g_screen_width ? g_screen_width : imgWidth;
+    height = imgHeight > g_screen_height ? g_screen_height : imgHeight;
+    x_offset = (g_screen_width - width) / 2;
+    y_offset = (g_screen_height - height) / 2;
 
     YUVdata = inBuf;
     RGBdata = tmp;
@@ -270,8 +276,8 @@ int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
     /* two bytes for every pixels */
     for(rows = 0; rows < height; rows++)
     {
-        // g_vinfo.xres, g_vinfo.yres g_vinfo.bits_per_pixel
-        RGBdata = tmp + ((rows + y_offset) * g_vinfo.xres + x_offset) * g_vinfo.bits_per_pixel / 8;
+        // g_screen_width, g_screen_height g_screen_bpp
+        RGBdata = tmp + ((rows + y_offset) * g_screen_width + x_offset) * g_screen_bpp / 8;
 
         Ypos = rows * imgWidth;
         Vpos = Upos = imgWidth * imgHeight + Ypos / 2;
@@ -296,10 +302,10 @@ int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
             b = b > 255 ? 255 : (b < 0 ? 0 : b);
 
             /* low -> high r g b */
-            if (g_vinfo.bits_per_pixel == 16) {   // RGB565
+            if (g_screen_bpp == 16) {   // RGB565
                 *(RGBdata ++) = (((g & 0x1c) << 3) | (b >> 3));   /* g low 5bit,b high 5bit */
                 *(RGBdata ++) = ((r & 0xf8) | (g >> 5));    /* r high 5bit,g high 3bit */
-            } else if (g_vinfo.bits_per_pixel == 24) {   // RGB888
+            } else if (g_screen_bpp == 24) {   // RGB888
                 *(RGBdata ++) = b;
                 *(RGBdata ++) = g;
                 *(RGBdata ++) = r;
@@ -331,10 +337,10 @@ int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
     gettimeofday(&ts_start, NULL);
 
 #if 1
-    memcpy(outBuf, tmp, g_screensize);
+    memcpy(outBuf, tmp, g_screen_size);
 #else
     int *p_outBuf, *p_tmp;
-    int size = g_screensize/4;
+    int size = g_screen_size/4;
     p_outBuf = outBuf;
     p_tmp = tmp;
 
@@ -391,13 +397,13 @@ static int Rgb2V(int r0, int g0, int b0)
 //Y1 -> yyyyyy
 //UV0 points destination rows of interleaved UV plane.
 //UV0 -> uvuvuv
-static void Rgb2NV12TwoRows(const unsigned char I0[],
-                            const unsigned char I1[],
+static void Rgb2NV12TwoRows(const uint8_t I0[],
+                            const uint8_t I1[],
                             int step,
                             const int image_width,
-                            unsigned char Y0[],
-                            unsigned char Y1[],
-                            unsigned char UV0[])
+                            uint8_t Y0[],
+                            uint8_t Y1[],
+                            uint8_t UV0[])
 {
     int x;  //Column index
 
@@ -405,48 +411,48 @@ static void Rgb2NV12TwoRows(const unsigned char I0[],
     for (x = 0; x < image_width; x += 2)
     {
         //Load R,G,B elements from first row (and convert to int).
-        unsigned char b00 = (I0[x*step + 0] & 0x1F) << 3;
-        unsigned char g00 = ((I0[x*step + 1] & 0x7) << 3 | I0[x*step + 0] >> 5) << 2;
-        unsigned char r00 = I0[x*step + 1] & (~0x7);
+        uint8_t b00 = (I0[x*step + 0] & 0x1F) << 3;
+        uint8_t g00 = ((I0[x*step + 1] & 0x7) << 3 | I0[x*step + 0] >> 5) << 2;
+        uint8_t r00 = I0[x*step + 1] & (~0x7);
 
         //Load next R,G,B elements from first row (and convert to int).
-        unsigned char b01 = (I0[x*step + step+0] & 0x1F) << 3;
-        unsigned char g01 = ((I0[x*step + step+1] & 0x7) << 3 | I0[x*step + step+0] >> 5) << 2;
-        unsigned char r01 = I0[x*step + step+1] & (~0x7);
+        uint8_t b01 = (I0[x*step + step+0] & 0x1F) << 3;
+        uint8_t g01 = ((I0[x*step + step+1] & 0x7) << 3 | I0[x*step + step+0] >> 5) << 2;
+        uint8_t r01 = I0[x*step + step+1] & (~0x7);
 
         //Load R,G,B elements from second row (and convert to int).
-        unsigned char b10 = (I1[x*step + 0] & 0x1F) << 3;
-        unsigned char g10 = ((I1[x*step + 1] & 0x7) << 3 | I1[x*step + 0] >> 5) << 2;
-        unsigned char r10 = I1[x*step + 1] & (~0x7);
+        uint8_t b10 = (I1[x*step + 0] & 0x1F) << 3;
+        uint8_t g10 = ((I1[x*step + 1] & 0x7) << 3 | I1[x*step + 0] >> 5) << 2;
+        uint8_t r10 = I1[x*step + 1] & (~0x7);
 
         //Load next R,G,B elements from second row (and convert to int).
-        unsigned char b11 = (I1[x*step + step+0] & 0x1F) << 3;
-        unsigned char g11 = ((I1[x*step + step+1] & 0x7) << 3 | I1[x*step + step+0] >> 5) << 2;
-        unsigned char r11 = I1[x*step + step+1] & (~0x7);
+        uint8_t b11 = (I1[x*step + step+0] & 0x1F) << 3;
+        uint8_t g11 = ((I1[x*step + step+1] & 0x7) << 3 | I1[x*step + step+0] >> 5) << 2;
+        uint8_t r11 = I1[x*step + step+1] & (~0x7);
 
         //Calculate 4 Y elements.
-        unsigned char y00 = Rgb2Y(r00, g00, b00);
-        unsigned char y01 = Rgb2Y(r01, g01, b01);
-        unsigned char y10 = Rgb2Y(r10, g10, b10);
-        unsigned char y11 = Rgb2Y(r11, g11, b11);
+        uint8_t y00 = Rgb2Y(r00, g00, b00);
+        uint8_t y01 = Rgb2Y(r01, g01, b01);
+        uint8_t y10 = Rgb2Y(r10, g10, b10);
+        uint8_t y11 = Rgb2Y(r11, g11, b11);
 
         //Calculate 4 U elements.
-        unsigned char u00 = Rgb2U(r00, g00, b00);
-        unsigned char u01 = Rgb2U(r01, g01, b01);
-        unsigned char u10 = Rgb2U(r10, g10, b10);
-        unsigned char u11 = Rgb2U(r11, g11, b11);
+        uint8_t u00 = Rgb2U(r00, g00, b00);
+        uint8_t u01 = Rgb2U(r01, g01, b01);
+        uint8_t u10 = Rgb2U(r10, g10, b10);
+        uint8_t u11 = Rgb2U(r11, g11, b11);
 
         //Calculate 4 V elements.
-        unsigned char v00 = Rgb2V(r00, g00, b00);
-        unsigned char v01 = Rgb2V(r01, g01, b01);
-        unsigned char v10 = Rgb2V(r10, g10, b10);
-        unsigned char v11 = Rgb2V(r11, g11, b11);
+        uint8_t v00 = Rgb2V(r00, g00, b00);
+        uint8_t v01 = Rgb2V(r01, g01, b01);
+        uint8_t v10 = Rgb2V(r10, g10, b10);
+        uint8_t v11 = Rgb2V(r11, g11, b11);
 
         //Calculate destination U element: average of 2x2 "original" U elements.
-        unsigned char u0 = (u00 + u01 + u10 + u11)/4;
+        uint8_t u0 = (u00 + u01 + u10 + u11)/4;
 
         //Calculate destination V element: average of 2x2 "original" V elements.
-        unsigned char v0 = (v00 + v01 + v10 + v11)/4;
+        uint8_t v0 = (v00 + v01 + v10 + v11)/4;
 
         //Store 4 Y elements (two in first row and two in second row).
         Y0[x + 0]    = y00;
@@ -485,34 +491,34 @@ static void Rgb2NV12TwoRows(const unsigned char I0[],
 //1. image_width must be a multiple of 2.
 //2. image_height must be a multiple of 2.
 //3. I and J must be two separate arrays (in place computation is not supported).
-void Rgb2NV12(const unsigned char I[], int step,
+void Rgb2NV12(const uint8_t I[], int step,
               const int image_width,
               const int image_height,
-              unsigned char J[])
+              uint8_t J[])
 {
     //In NV12 format, UV plane starts below Y plane.
-    // unsigned char *UV = &J[image_width*image_height];
-    unsigned char *UV = J;
+    // uint8_t *UV = &J[image_width*image_height];
+    uint8_t *UV = J;
 
     //I0 and I1 points two sequential source rows.
-    const unsigned char *I0;  //I0 -> rgbrgbrgbrgbrgbrgb...
-    const unsigned char *I1;  //I1 -> rgbrgbrgbrgbrgbrgb...
+    const uint8_t *I0;  //I0 -> rgbrgbrgbrgbrgbrgb...
+    const uint8_t *I1;  //I1 -> rgbrgbrgbrgbrgbrgb...
 
     //Y0 and Y1 points two sequential destination rows of Y plane.
-    unsigned char *Y0;    //Y0 -> yyyyyy
-    unsigned char *Y1;    //Y1 -> yyyyyy
+    uint8_t *Y0;    //Y0 -> yyyyyy
+    uint8_t *Y1;    //Y1 -> yyyyyy
 
     //UV0 points destination rows of interleaved UV plane.
-    unsigned char *UV0; //UV0 -> uvuvuv
+    uint8_t *UV0; //UV0 -> uvuvuv
 
     int y;  //Row index
     int width, height;
     int x_offset, y_offset;
 
-    width = image_width > g_vinfo.xres ? g_vinfo.xres : image_width;
-    height = image_height > g_vinfo.yres ? g_vinfo.yres : image_height;
-    x_offset = (g_vinfo.xres - width) / 2;
-    y_offset = (g_vinfo.yres - height) / 2;
+    width = image_width > g_screen_width ? g_screen_width : image_width;
+    height = image_height > g_screen_height ? g_screen_height : image_height;
+    x_offset = (g_screen_width - width) / 2;
+    y_offset = (g_screen_height - height) / 2;
 
     //In each iteration: process two rows of Y plane, and one row of interleaved UV plane.
     for (y = 0; y < height; y += 2)
@@ -520,10 +526,10 @@ void Rgb2NV12(const unsigned char I[], int step,
         I0 = &I[y*image_width*step];        //Input row width is image_width*3 bytes (each pixel is R,G,B).
         I1 = &I[(y+1)*image_width*step];
 
-        Y0 = &J[(y+y_offset)*g_vinfo.xres+x_offset];            //Output Y row width is image_width bytes (one Y element per pixel).
-        Y1 = &J[(y+1+y_offset)*g_vinfo.xres+x_offset];
+        Y0 = &J[(y+y_offset)*g_screen_width+x_offset];            //Output Y row width is image_width bytes (one Y element per pixel).
+        Y1 = &J[(y+1+y_offset)*g_screen_width+x_offset];
 
-        UV0 = &UV[g_vinfo.xres*g_vinfo.yres+((y+y_offset)/2*g_vinfo.xres/2+x_offset/2)*2];    //Output UV row - width is same as Y row width.
+        UV0 = &UV[g_screen_width*g_screen_height+((y+y_offset)/2*g_screen_width/2+x_offset/2)*2];    //Output UV row - width is same as Y row width.
 
         //Process two source rows into: Two Y destination row, and one destination interleaved U,V row.
         Rgb2NV12TwoRows(I0,
@@ -536,11 +542,11 @@ void Rgb2NV12(const unsigned char I[], int step,
     }
 }
 
-int convert_rgb565_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int is_nv21)
+int convert_rgb565_to_nv12(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int is_nv21)
 {
-    unsigned char *tmp = malloc(g_screensize);
-    unsigned int start_timems;
-    unsigned int end_timems;
+    uint8_t *tmp = malloc(g_screen_size);
+    uint32_t start_timems;
+    uint32_t end_timems;
     struct timespec ts_start, ts_end;
     clock_gettime(CLOCK_MONOTONIC, &ts_start);
 
@@ -553,7 +559,7 @@ int convert_rgb565_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgW
 
     clock_gettime(CLOCK_MONOTONIC, &ts_start);
 
-    memcpy(outBuf, tmp, g_screensize);
+    memcpy(outBuf, tmp, g_screen_size);
 
     clock_gettime(CLOCK_MONOTONIC, &ts_end);
     start_timems = ts_start.tv_sec * 1000 + ts_start.tv_nsec/1000000;
@@ -564,25 +570,26 @@ int convert_rgb565_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgW
     return 0;
 }
 
-int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int cvtMethod)
+int convert_yuyv_to_rgb(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int cvtMethod)
 {
     int rows ,cols;
     int y, u, v, r, g, b;
-    unsigned char *YUVdata, *RGBdata;
+    const uint8_t *YUVdata;
+    uint8_t *RGBdata;
     int Ypos, Upos, Vpos;
-    unsigned int i = 0;
+    uint32_t i = 0;
     int width, height;
     int x_offset, y_offset;
-    unsigned char *tmp = malloc(g_screensize);
-    unsigned int start_timems;
-    unsigned int end_timems;
+    uint8_t *tmp = malloc(g_screen_size);
+    uint32_t start_timems;
+    uint32_t end_timems;
     struct timespec ts_start, ts_end;
     clock_gettime(CLOCK_MONOTONIC, &ts_start);
 
-    width = imgWidth > g_vinfo.xres ? g_vinfo.xres : imgWidth;
-    height = imgHeight > g_vinfo.yres ? g_vinfo.yres : imgHeight;
-    x_offset = (g_vinfo.xres - width) / 2;
-    y_offset = (g_vinfo.yres - height) / 2;
+    width = imgWidth > g_screen_width ? g_screen_width : imgWidth;
+    height = imgHeight > g_screen_height ? g_screen_height : imgHeight;
+    x_offset = (g_screen_width - width) / 2;
+    y_offset = (g_screen_height - height) / 2;
 
     YUVdata = inBuf;
     RGBdata = tmp;
@@ -590,8 +597,8 @@ int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
     /* two bytes for every pixels */
     for(rows = 0; rows < height; rows++)
     {
-        // g_vinfo.xres, g_vinfo.yres g_vinfo.bits_per_pixel
-        RGBdata = tmp + ((rows + y_offset) * g_vinfo.xres + x_offset) * g_vinfo.bits_per_pixel / 8;
+        // g_screen_width, g_screen_height g_screen_bpp
+        RGBdata = tmp + ((rows + y_offset) * g_screen_width + x_offset) * g_screen_bpp / 8;
 
         Ypos = rows * imgWidth * 2;
         Upos = Ypos + 1;
@@ -613,10 +620,10 @@ int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
             b = b > 255 ? 255 : (b < 0 ? 0 : b);
 
             /* low -> high r g b */
-            if (g_vinfo.bits_per_pixel == 16) {   // RGB565
+            if (g_screen_bpp == 16) {   // RGB565
                 *(RGBdata ++) = (((g & 0x1c) << 3) | (b >> 3));    /* g low 5bits,b high 5bits */
                 *(RGBdata ++) = ((r & 0xf8) | (g >> 5));    /* r high 5bits, g high 3bits */
-            } else if (g_vinfo.bits_per_pixel == 24) {   // RGB888
+            } else if (g_screen_bpp == 24) {   // RGB888
                 *(RGBdata ++) = b;
                 *(RGBdata ++) = g;
                 *(RGBdata ++) = r;
@@ -646,7 +653,7 @@ int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
 
     clock_gettime(CLOCK_MONOTONIC, &ts_start);
 
-    memcpy(outBuf, tmp, g_screensize);
+    memcpy(outBuf, tmp, g_screen_size);
 
     clock_gettime(CLOCK_MONOTONIC, &ts_end);
     start_timems = ts_start.tv_sec * 1000 + ts_start.tv_nsec/1000000;
@@ -657,13 +664,13 @@ int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
     return 0;
 }
 
-int convert_yuv444_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int cvtMethod)
+int convert_yuv444_to_rgb(uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int cvtMethod)
 {
     int rows ,cols;
     int y, u, v, r, g, b;
-    unsigned char *YUVdata, *RGBdata;
+    uint8_t *YUVdata, *RGBdata;
     int Ypos;
-    unsigned char *tmp = malloc(g_screensize);
+    uint8_t *tmp = malloc(g_screen_size);
 
     YUVdata = inBuf;
     RGBdata = tmp;
@@ -687,10 +694,10 @@ int convert_yuv444_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
             b = b > 255 ? 255 : (b < 0 ? 0 : b);
 
             /* low -> high r g b */
-            if (g_vinfo.bits_per_pixel == 16) {   // RGB565
+            if (g_screen_bpp == 16) {   // RGB565
                 *(RGBdata ++) = (((g & 0x1c) << 3) | (b >> 3));    /* g low 5bits,b high 5bits */
                 *(RGBdata ++) = ((r & 0xf8) | (g >> 5));    /* r high 5bits,g high 3bits */
-            } else if (g_vinfo.bits_per_pixel == 24) {   // RGB888
+            } else if (g_screen_bpp == 24) {   // RGB888
                 *(RGBdata ++) = b;
                 *(RGBdata ++) = g;
                 *(RGBdata ++) = r;
@@ -704,30 +711,31 @@ int convert_yuv444_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
         }
     }
 
-    memcpy(outBuf, tmp, g_screensize);
+    memcpy(outBuf, tmp, g_screen_size);
     free(tmp);
     return 0;
 }
 
-int convert_rgb565_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int cvtMethod)
+int convert_rgb565_to_rgb(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int cvtMethod)
 {
     int rows ,cols;
-    unsigned char *RGB565data, *RGBdata;
+    const uint8_t *RGB565data;
+    uint8_t *RGBdata;
     int RGBpos;
     int width, height;
     int x_offset, y_offset;
-    unsigned char *tmp = malloc(g_screensize);
+    uint8_t *tmp = malloc(g_screen_size);
 
-    width = imgWidth > g_vinfo.xres ? g_vinfo.xres : imgWidth;
-    height = imgHeight > g_vinfo.yres ? g_vinfo.yres : imgHeight;
-    x_offset = (g_vinfo.xres - width) / 2;
-    y_offset = (g_vinfo.yres - height) / 2;
+    width = imgWidth > g_screen_width ? g_screen_width : imgWidth;
+    height = imgHeight > g_screen_height ? g_screen_height : imgHeight;
+    x_offset = (g_screen_width - width) / 2;
+    y_offset = (g_screen_height - height) / 2;
 
     RGB565data = inBuf;
     RGBdata = tmp;
 
-    if (imgWidth == g_vinfo.xres) {
-        RGBpos = (y_offset * g_vinfo.xres + x_offset) * 2;
+    if (imgWidth == g_screen_width) {
+        RGBpos = (y_offset * g_screen_width + x_offset) * 2;
         memcpy(&tmp[RGBpos], inBuf, imgWidth * height * 2);
         memcpy(&outBuf[RGBpos], &tmp[RGBpos], imgWidth * height * 2);
         // memcpy(&outBuf[RGBpos], inBuf, imgWidth * height * 2);
@@ -738,9 +746,9 @@ int convert_rgb565_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
     RGBpos = 0;
     for(rows = 0; rows < imgHeight; rows++)
     {
-        RGBdata = tmp + ((rows + y_offset) * g_vinfo.xres + x_offset) * g_vinfo.bits_per_pixel / 8;
+        RGBdata = tmp + ((rows + y_offset) * g_screen_width + x_offset) * g_screen_bpp / 8;
         RGBpos = rows * imgWidth * 2;
-        if (g_vinfo.bits_per_pixel == 16) {   // RGB565
+        if (g_screen_bpp == 16) {   // RGB565
             memcpy(RGBdata, &RGB565data[RGBpos], imgWidth * 2);
         } else {
             for(cols = 0; cols < imgWidth; cols++)
@@ -748,7 +756,7 @@ int convert_rgb565_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
                 *(RGBdata ++) = RGB565data[RGBpos] & 0x1F;
                 *(RGBdata ++) = (RGB565data[RGBpos + 1] & 0x7) << 3 | RGB565data[RGBpos] >> 5;
                 *(RGBdata ++) = RGB565data[RGBpos + 1] >> 3;
-                if (g_vinfo.bits_per_pixel == 32) {   // RGB888
+                if (g_screen_bpp == 32) {   // RGB888
                     *(RGBdata ++) = 0xFF;
                 }
                 RGBpos += 2;
@@ -756,25 +764,26 @@ int convert_rgb565_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
         }
     }
 
-    memcpy(outBuf, tmp, g_screensize);
+    memcpy(outBuf, tmp, g_screen_size);
     free(tmp);
     return 0;
 }
 
-int convert_rgb888_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int cvtMethod)
+int convert_rgb888_to_rgb(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int cvtMethod)
 {
     int rows ,cols;
-    unsigned char *RGB888data, *RGBdata;
+    const uint8_t *RGB888data;
+    uint8_t *RGBdata;
     int RGBpos;
     int width, height;
     int x_offset, y_offset;
-    unsigned char *tmp = malloc(g_screensize);
-    unsigned char r, g, b;
+    uint8_t *tmp = malloc(g_screen_size);
+    uint8_t r, g, b;
 
-    width = imgWidth > g_vinfo.xres ? g_vinfo.xres : imgWidth;
-    height = imgHeight > g_vinfo.yres ? g_vinfo.yres : imgHeight;
-    x_offset = (g_vinfo.xres - width) / 2;
-    y_offset = (g_vinfo.yres - height) / 2;
+    width = imgWidth > g_screen_width ? g_screen_width : imgWidth;
+    height = imgHeight > g_screen_height ? g_screen_height : imgHeight;
+    x_offset = (g_screen_width - width) / 2;
+    y_offset = (g_screen_height - height) / 2;
 
     RGB888data = inBuf;
     RGBdata = tmp;
@@ -782,14 +791,14 @@ int convert_rgb888_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
     RGBpos = 0;
     for(rows = 0; rows < imgHeight; rows++)
     {
-        RGBdata = tmp + ((rows + y_offset) * g_vinfo.xres + x_offset) * g_vinfo.bits_per_pixel / 8;
+        RGBdata = tmp + ((rows + y_offset) * g_screen_width + x_offset) * g_screen_bpp / 8;
         RGBpos = rows * imgWidth * 3;
-        if (g_vinfo.bits_per_pixel == 24) {   // RGB888
+        if (g_screen_bpp == 24) {   // RGB888
             memcpy(RGBdata, &RGB888data[RGBpos], imgWidth * 3);
         } else {
             for(cols = 0; cols < imgWidth; cols++)
             {
-                if (g_vinfo.bits_per_pixel == 16) {   // RGB565
+                if (g_screen_bpp == 16) {   // RGB565
                     b = RGB888data[RGBpos];
                     g = RGB888data[RGBpos + 1];
                     r = RGB888data[RGBpos + 2];
@@ -806,7 +815,7 @@ int convert_rgb888_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
         }
     }
 
-    memcpy(outBuf, tmp, g_screensize);
+    memcpy(outBuf, tmp, g_screen_size);
     free(tmp);
     return 0;
 }

+ 12 - 9
package/starfive/v4l2_test/convert.h

@@ -6,15 +6,18 @@
 #ifndef _CONVERT_H_
 #define _CONVERT_H_
 
-extern int yuyv_resize(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight);
+extern int yuyv_resize(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight);
 
-extern int convert_yuyv_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int is_yuyv);
-extern int convert_nv21_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int is_nv21);
-extern int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int is_nv21);
-extern int convert_rgb565_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int is_nv21);
-extern int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int cvtMethod);
-extern int convert_yuv444_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int cvtMethod);
-extern int convert_rgb565_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int cvtMethod);
-extern int convert_rgb888_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight, int cvtMethod);
+extern int convert_yuyv_to_nv12(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int is_yuyv);
+extern int convert_nv21_to_nv12(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int is_nv21);
+extern int convert_nv21_to_rgb(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int is_nv21);
+extern int convert_rgb565_to_nv12(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int is_nv21);
+extern int convert_yuyv_to_rgb(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int cvtMethod);
+extern int convert_yuv444_to_rgb(uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int cvtMethod);
+extern int convert_rgb565_to_rgb(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int cvtMethod);
+extern int convert_rgb888_to_rgb(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight, int cvtMethod);
+
+extern void update_videocvt_param(int distype, int scr_width, int scr_height,
+        int scr_bpp, int scr_size);
 
 #endif // _CONVERT_H_

+ 490 - 0
package/starfive/v4l2_test/stf_drm.c

@@ -0,0 +1,490 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include <sys/mman.h>
+#include <libdrm/drm_fourcc.h>
+#include "stf_drm.h"
+#include "stf_log.h"
+
+static int drm_open(const char *path, int need_dumb, int need_prime)
+{
+    int fd, flags;
+    uint64_t has_it;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    if ((fd = open(path, O_RDWR)) < 0) {
+        LOG(STF_LEVEL_ERR, "cannot open \"%s\"\n", path);
+        error("open");
+    }
+
+    /* set FD_CLOEXEC flag */
+    if ((flags = fcntl(fd, F_GETFD)) < 0
+        || fcntl(fd, F_SETFD, flags | FD_CLOEXEC) < 0)
+        fatal("fcntl FD_CLOEXEC failed");
+
+    if (need_dumb) {
+        if (drmGetCap(fd, DRM_CAP_DUMB_BUFFER, &has_it) < 0)
+            error("drmGetCap DRM_CAP_DUMB_BUFFER failed!");
+        if (has_it == 0)
+            fatal("can't give us dumb buffers");
+    }
+
+    if (need_prime) {
+        /* check prime */
+        if (drmGetCap(fd, DRM_CAP_PRIME, &has_it) < 0)
+            error("drmGetCap DRM_CAP_PRIME failed!");
+        if (!(has_it & DRM_PRIME_CAP_EXPORT))
+            fatal("can't export dmabuf");
+    }
+
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+    return fd;
+}
+
+static struct drm_dev_t *drm_find_dev(int fd, uint32_t width, uint32_t height)
+{
+    int i, m;
+    struct drm_dev_t *dev = NULL, *dev_head = NULL;
+    drmModeRes *res;
+    drmModeConnector *conn;
+    drmModeEncoder *enc;
+    drmModeModeInfo *mode = NULL, *preferred = NULL;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    if ((res = drmModeGetResources(fd)) == NULL) {
+        fatal("drmModeGetResources() failed");
+    }
+
+    LOG(STF_LEVEL_DEBUG, "count_connectors=%d\n", res->count_connectors);
+    /* find all available connectors */
+    for (i = 0; i < res->count_connectors; i++) {
+        conn = drmModeGetConnector(fd, res->connectors[i]);
+        if (conn != NULL && conn->connection == DRM_MODE_CONNECTED && conn->count_modes > 0) {
+            dev = (struct drm_dev_t *) malloc(sizeof(struct drm_dev_t));
+            memset(dev, 0, sizeof(struct drm_dev_t));
+
+            /* find preferred mode */
+            for (m = 0; m < conn->count_modes; m++) {
+                mode = &conn->modes[m];
+                if (mode->hdisplay == width && mode->vdisplay == height) {
+                    preferred = mode;
+                    LOG(STF_LEVEL_INFO, "find the matched mode, modes index=%d, %dx%d\n",
+                            m, width, height);
+                    break;
+                }
+                if (mode->type & DRM_MODE_TYPE_PREFERRED) {
+                    preferred = mode;
+                    LOG(STF_LEVEL_INFO, "find perferred mode, modes index=%d\n", m);
+                }
+                LOG(STF_LEVEL_DEBUG, "mode: %dx%d %s\n", mode->hdisplay, mode->vdisplay,
+                        mode->type & DRM_MODE_TYPE_PREFERRED ? "*" : "");
+            }
+
+            if (!preferred)
+                preferred = &conn->modes[0];
+
+            dev->conn_id = conn->connector_id;
+            // dev->next = NULL;
+            if (conn->count_encoders) {
+                if (conn->encoder_id) {
+                    dev->enc_id = conn->encoder_id;
+                } else {
+                    dev->enc_id = conn->encoders[0];
+                }
+            } else {
+                fatal("count_encoders is 0");
+            }
+
+            memcpy(&dev->mode, preferred, sizeof(drmModeModeInfo));
+            dev->width = preferred->hdisplay;
+            dev->height = preferred->vdisplay;
+
+            /* FIXME: use default encoder/crtc pair */
+            if ((enc = drmModeGetEncoder(fd, dev->enc_id)) == NULL)
+                fatal("drmModeGetEncoder() faild");
+            if (enc->crtc_id) {
+                dev->crtc_id = enc->crtc_id;
+            } else {
+                dev->crtc_id = res->crtcs[0];
+            }
+            drmModeFreeEncoder(enc);
+
+            dev->saved_crtc = NULL;
+
+            /* create dev list */
+            dev->next = dev_head;
+            dev_head = dev;
+        }
+        drmModeFreeConnector(conn);
+    }
+
+    drmModeFreeResources(res);
+
+    LOG(STF_LEVEL_INFO, "selected connector(s)\n");
+    for (dev = dev_head; dev != NULL; dev = dev->next) {
+        LOG(STF_LEVEL_INFO, "connector id:%d\n", dev->conn_id);
+        LOG(STF_LEVEL_INFO, "\tencoder id:%d crtc id:%d\n", dev->enc_id, dev->crtc_id);
+        LOG(STF_LEVEL_INFO, "\twidth:%d height:%d\n", dev->width, dev->height);
+    }
+
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+    return dev_head;
+}
+
+static void drm_setup_buffer(int fd, struct drm_dev_t *dev,
+        int width, int height,
+        struct drm_buffer_t *buffer, int map, int export)
+{
+    struct drm_mode_create_dumb create_req;
+    struct drm_mode_map_dumb map_req;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    buffer->dmabuf_fd = -1;
+
+    memset(&create_req, 0, sizeof(struct drm_mode_create_dumb));
+    create_req.width = width;
+    create_req.height = height;
+    create_req.bpp = 24;
+    if (drmIoctl(fd, DRM_IOCTL_MODE_CREATE_DUMB, &create_req) < 0) {
+        fatal("drmIoctl DRM_IOCTL_MODE_CREATE_DUMB failed");
+    }
+
+    buffer->pitch = create_req.pitch;
+    buffer->size = create_req.size;
+    /* GEM buffer handle */
+    buffer->bo_handle = create_req.handle;
+
+    if (export) {
+        int ret;
+        ret = drmPrimeHandleToFD(fd, buffer->bo_handle,
+                DRM_CLOEXEC | DRM_RDWR, &buffer->dmabuf_fd);
+        if (ret < 0) {
+            fatal("could not export the dump buffer");
+        }
+        LOG(STF_LEVEL_INFO, "drm fd=%d, bo_handle=%d, dmabuf_fd=%d\n",
+                fd, buffer->bo_handle, buffer->dmabuf_fd);
+    }
+
+    if (map) {
+        memset(&map_req, 0, sizeof(struct drm_mode_map_dumb));
+        map_req.handle = buffer->bo_handle;
+        if (drmIoctl(fd, DRM_IOCTL_MODE_MAP_DUMB, &map_req)) {
+            fatal("drmIoctl DRM_IOCTL_MODE_MAP_DUMB failed");
+        }
+        buffer->buf = (uint8_t *)mmap(0, buffer->size,
+                PROT_READ | PROT_WRITE, MAP_SHARED,
+                fd, map_req.offset);
+        if (buffer->buf == MAP_FAILED) {
+            error("mmap");
+        }
+    }
+
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+static uint32_t drm_get_bpp_from_drm(uint32_t drmfmt)
+{
+    uint32_t bpp;
+
+    switch (drmfmt) {
+    case DRM_FORMAT_YUV420:
+    case DRM_FORMAT_YVU420:
+    case DRM_FORMAT_YUV422:
+    case DRM_FORMAT_NV12:
+    case DRM_FORMAT_NV21:
+    case DRM_FORMAT_NV16:
+        bpp = 8;
+        break;
+    case DRM_FORMAT_P010:
+        bpp = 10;
+        break;
+    case DRM_FORMAT_UYVY:
+    case DRM_FORMAT_YUYV:
+    case DRM_FORMAT_YVYU:
+    case DRM_FORMAT_P016:
+        bpp = 16;
+        break;
+    case DRM_FORMAT_BGR888:
+    case DRM_FORMAT_RGB888:
+        bpp = 24;
+        break;
+    default:
+        bpp = 32;
+        break;
+    }
+
+    return bpp;
+}
+
+static uint32_t drm_get_height_from_drm (uint32_t drmfmt, uint32_t height)
+{
+    uint32_t ret;
+
+    switch (drmfmt) {
+    case DRM_FORMAT_YUV420:
+    case DRM_FORMAT_YVU420:
+    case DRM_FORMAT_YUV422:
+    case DRM_FORMAT_NV12:
+    case DRM_FORMAT_NV21:
+    case DRM_FORMAT_P010:
+    case DRM_FORMAT_P016:
+        ret = height * 3 / 2;
+        break;
+    case DRM_FORMAT_NV16:
+        ret = height * 2;
+        break;
+    default:
+        ret = height;
+        break;
+    }
+
+    return ret;
+}
+
+static int drm_setup_buffer2(int fd, struct drm_dev_t *dev,
+        int width, int height,
+        struct drm_buffer_t *buffer, int map, int export)
+{
+    struct drm_mode_create_dumb create_req;
+    struct drm_mode_map_dumb map_req;
+    int ret = 0;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    buffer->dmabuf_fd = -1;
+
+    memset(&create_req, 0, sizeof(struct drm_mode_create_dumb));
+    // For NV12 or NV21, bpp is 8, height is height * 3 / 2
+    create_req.width = width;
+    create_req.height = drm_get_height_from_drm(dev->drm_format, height);
+    create_req.bpp = drm_get_bpp_from_drm(dev->drm_format);
+    if (drmIoctl(fd, DRM_IOCTL_MODE_CREATE_DUMB, &create_req) < 0) {
+        fatal("drmIoctl DRM_IOCTL_MODE_CREATE_DUMB failed");
+    }
+    buffer->pitch = create_req.pitch;
+    buffer->size = create_req.size;
+    buffer->bo_handle = create_req.handle; /* GEM buffer handle */
+
+    if (export) {
+#if 0
+        memset(&map_req, 0, sizeof(struct drm_mode_map_dumb));
+        map_req.handle = buffer->bo_handle;
+        ret = drmIoctl(fd, DRM_IOCTL_MODE_MAP_DUMB, &map_req);
+        if (ret) {
+            fatal("drmIoctl DRM_IOCTL_MODE_MAP_DUMB failed");
+        }
+        buffer->buf = (uint8_t *) mmap(0, buffer->size,
+            PROT_READ | PROT_WRITE, MAP_SHARED,
+            fd, map_req.offset);
+        if (buffer->buf == MAP_FAILED) {
+            error("drm mmap");
+        }
+        LOG(STF_LEVEL_INFO, "buffer->buf= %p\n", buffer->buf);
+#endif
+
+        ret = drmPrimeHandleToFD(fd, buffer->bo_handle,
+                DRM_CLOEXEC | DRM_RDWR, &buffer->dmabuf_fd);
+        if (ret < 0) {
+            fatal("could not export the dump buffer");
+        }
+        LOG(STF_LEVEL_DEBUG, "drm fd=%d, bo_handle=%d, dmabuf_fd=%d\n",
+                fd, buffer->bo_handle, buffer->dmabuf_fd);
+    }
+
+    if (map) {
+        memset(&map_req, 0, sizeof(struct drm_mode_map_dumb));
+        map_req.handle = buffer->bo_handle;
+
+        if (drmIoctl(fd, DRM_IOCTL_MODE_MAP_DUMB, &map_req)) {
+            fatal("drmIoctl DRM_IOCTL_MODE_MAP_DUMB failed");
+        }
+        buffer->buf = (uint8_t *)mmap(0, buffer->size,
+                PROT_READ | PROT_WRITE, MAP_SHARED,
+                fd, map_req.offset);
+        if (buffer->buf == MAP_FAILED) {
+            error("mmap");
+        }
+    }
+
+    uint32_t handles[4] = {0}, pitches[4] = {0}, offsets[4] = {0};
+    handles[0] = buffer->bo_handle;
+    pitches[0] = buffer->pitch;
+    offsets[0] = 0;
+    handles[1] = buffer->bo_handle;
+    pitches[1] = buffer->pitch;
+    offsets[1] = buffer->pitch * height;
+    // planes[0] = virtual;
+    // planes[1] = virtual + offsets[1];
+
+    LOG(STF_LEVEL_INFO, "map=%d, export=%d. width=%d, height=%d, "
+            "pitch=%d, size=%d, bo_handle=%d, dmabuf_fd=%d\n",
+            map, export, width, height, buffer->pitch, buffer->size,
+            buffer->bo_handle, buffer->dmabuf_fd);
+
+    //DRM_FORMAT_NV12
+    ret = drmModeAddFB2(fd, dev->width, dev->height,
+            dev->drm_format, handles, pitches, offsets, &(buffer->fb_id), 0);
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+    return ret;
+    // return drmModeAddFB(fd, dev->width, dev->height,
+    //         DEPTH, BPP, dev->bufs[i].pitch,
+    //         dev->bufs[i].bo_handle, &dev->bufs[i].fb_id);
+}
+
+static void drm_setup_dummy(int fd, struct drm_dev_t *dev, int map, int export)
+{
+    int i;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    for (i = 0; i < BUFCOUNT; i++) {
+        drm_setup_buffer(fd, dev, dev->width, dev->height,
+                &dev->bufs[i], map, export);
+    }
+
+    /* Assume all buffers have the same pitch */
+    dev->pitch = dev->bufs[0].pitch;
+    LOG(STF_LEVEL_INFO, "DRM: buffer pitch = %d bytes\n", dev->pitch);
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+static void drm_setup_fb(int fd, struct drm_dev_t *dev, int map, int export)
+{
+    int i;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+#if 0
+    for (i = 0; i < BUFCOUNT; i++) {
+        int ret;
+
+        drm_setup_buffer(fd, dev, dev->width, dev->height,
+                    &dev->bufs[i], map, export);
+
+        ret = drmModeAddFB(fd, dev->width, dev->height,
+            DEPTH, BPP, dev->bufs[i].pitch,
+            dev->bufs[i].bo_handle, &dev->bufs[i].fb_id);
+        if (ret)
+            fatal("drmModeAddFB failed");
+LOG(STF_LEVEL_INFO, "width=%d,height=%d,pitch=%d,bo_handle=%d,fb_id=%d\n",
+    dev->width, dev->height,dev->bufs[i].pitch,dev->bufs[i].bo_handle, dev->bufs[i].fb_id);
+    }
+#else
+    for (i = 0; i < BUFCOUNT; i++) {
+        int ret = -1;
+        ret = drm_setup_buffer2(fd, dev, dev->width, dev->height,
+                &dev->bufs[i], map, export);
+        if (ret) {
+            fatal("drmModeAddFB failed");
+        }
+    }
+#endif
+
+    /* Assume all buffers have the same pitch */
+    dev->pitch = dev->bufs[0].pitch;
+    LOG(STF_LEVEL_DEBUG, "DRM: buffer pitch %d bytes\n", dev->pitch);
+
+    dev->saved_crtc = drmModeGetCrtc(fd, dev->crtc_id); /* must store crtc data */
+
+    /* First buffer to DRM */
+    if (drmModeSetCrtc(fd, dev->crtc_id, dev->bufs[0].fb_id, 0, 0,
+            &dev->conn_id, 1, &dev->mode)) {
+        fatal("drmModeSetCrtc() failed");
+    }
+
+    /* First flip */
+    drmModePageFlip(fd, dev->crtc_id,
+                        dev->bufs[0].fb_id, DRM_MODE_PAGE_FLIP_EVENT,
+                        dev);
+
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+static void drm_destroy(int fd, struct drm_dev_t *dev_head)
+{
+    struct drm_dev_t *devp, *devp_tmp;
+    int i;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    for (devp = dev_head; devp != NULL;) {
+        if (devp->saved_crtc) {
+            drmModeSetCrtc(fd, devp->saved_crtc->crtc_id, devp->saved_crtc->buffer_id,
+                devp->saved_crtc->x, devp->saved_crtc->y, &devp->conn_id, 1, &devp->saved_crtc->mode);
+            drmModeFreeCrtc(devp->saved_crtc);
+        }
+
+        for (i = 0; i < BUFCOUNT; i++) {
+            struct drm_mode_destroy_dumb dreq = { .handle = devp->bufs[i].bo_handle };
+
+            if (devp->bufs[i].buf) {
+                munmap(devp->bufs[i].buf, devp->bufs[i].size);
+            }
+            if (devp->bufs[i].dmabuf_fd >= 0) {
+                close(devp->bufs[i].dmabuf_fd);
+            }
+            drmIoctl(fd, DRM_IOCTL_MODE_DESTROY_DUMB, &dreq);
+            drmModeRmFB(fd, devp->bufs[i].fb_id);
+        }
+
+        devp_tmp = devp;
+        devp = devp->next;
+        free(devp_tmp);
+    }
+
+    drmClose(fd);
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void stf_drm_open(DRMParam_t *param, char *device_name, int iomthd)
+{
+    int drm_fd;
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+
+    if (IO_METHOD_MMAP == iomthd) {
+        drm_fd = drm_open(device_name, 1, 0);
+    } else if (IO_METHOD_DMABUF == iomthd) {
+        drm_fd = drm_open(device_name, 1, 1);
+    } else {
+        LOG(STF_LEVEL_ERR, "drm: not support io_method %d\n", iomthd);
+        exit (EXIT_FAILURE);
+    }
+
+    param->fd = drm_fd;
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+// try to use v4l2_fmt
+void stf_drm_init(DRMParam_t *param, uint32_t width, uint32_t height,
+        uint32_t v4l2_fmt, int iomthd, int *dmabufs, int nsize)
+{
+    struct drm_dev_t *dev_head = NULL;
+    int i = 0;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    dev_head = drm_find_dev(param->fd, width, height);
+    if (dev_head == NULL) {
+        LOG(STF_LEVEL_ERR, "available drm_dev not found\n");
+        exit (EXIT_FAILURE);
+    }
+
+    dev_head->drm_format = v4l2fmt_to_drmfmt(v4l2_fmt);
+
+    if (IO_METHOD_MMAP == iomthd) {
+        drm_setup_fb(param->fd, dev_head, 1, 0);
+    } else if (IO_METHOD_DMABUF == iomthd) {
+        drm_setup_fb(param->fd, dev_head, 0, 1);
+        assert(nsize == BUFCOUNT);
+        for (i = 0; i < nsize; i++) {
+            dmabufs[i] = dev_head->bufs[i].dmabuf_fd;
+        }
+    } else {
+        LOG(STF_LEVEL_ERR, "drm: not support io_method %d\n", iomthd);
+        exit (EXIT_FAILURE);
+    }
+
+    param->dev_head = dev_head;
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void stf_drm_close(DRMParam_t *param)
+{
+    drm_destroy(param->fd, param->dev_head);
+}

+ 60 - 0
package/starfive/v4l2_test/stf_drm.h

@@ -0,0 +1,60 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef __STF_DRM_H__
+#define __STF_DRM_H__
+
+#include <libdrm/drm.h>
+#include <xf86drm.h>
+#include <xf86drmMode.h>
+#include "common.h"
+#include "stf_log.h"
+
+struct drm_buffer_t {
+    uint32_t pitch, size;
+
+    uint32_t fb_id;
+    int dmabuf_fd;  // used for dmabuf
+    int bo_handle;
+    uint8_t *buf;
+};
+
+typedef struct drm_dev_t {
+    uint32_t conn_id, enc_id, crtc_id;
+    uint32_t width, height, pitch;
+    drmModeModeInfo mode;
+    drmModeCrtc *saved_crtc;
+
+    // int v4l2_fd;
+    // int drm_fd;
+    uint32_t drm_format;
+
+    struct drm_buffer_t bufs[BUFCOUNT];
+    struct drm_dev_t *next;
+} drm_dev_t;
+
+typedef struct DRMParam_t {
+    drm_dev_t* dev_head;
+    int fd;
+} DRMParam_t;
+
+
+inline static void fatal(char *str)
+{
+    LOG(STF_LEVEL_ERR, "%s\n", str);
+    exit(EXIT_FAILURE);
+}
+
+inline static void error(char *str)
+{
+    perror(str);
+    exit(EXIT_FAILURE);
+}
+
+extern void stf_drm_open(DRMParam_t *param, char *device_name, int iomthd);
+extern void stf_drm_init(DRMParam_t *param, uint32_t width, uint32_t height,
+        uint32_t v4l2_fmt, int iomthd, int *dmabufs, int nsize);
+extern void stf_drm_close(DRMParam_t *param);
+
+#endif // __STF_DRM_H__

+ 128 - 0
package/starfive/v4l2_test/stf_framebuffer.c

@@ -0,0 +1,128 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+#include "common.h"
+#include "stf_framebuffer.h"
+#include "stf_log.h"
+
+#define FBIOPAN_GET_PP_MODE        0x4609
+#define FBIOPAN_SET_PP_MODE        0x460a
+
+void stf_fb_open(FBParam_t *param, char *device_name, char *stfbc_name)
+{
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    //open framebuffer
+    param->fd = open(device_name, O_RDWR);
+    if (param->fd == -1) {
+        LOG(STF_LEVEL_ERR, "Error: cannot open framebuffer device.\n");
+        exit(EXIT_FAILURE);
+    }
+    //open stfbc device for pp setting
+    param->stfbc_fd = open(stfbc_name, O_RDWR);
+    if (param->stfbc_fd == -1) {
+        LOG(STF_LEVEL_ERR, "Error: cannot open stfbcdev device.\n");
+        exit(EXIT_FAILURE);
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void stf_fb_close(FBParam_t *param)
+{
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    if (param->fd > 0) {
+        close(param->fd);
+        param->fd = 0;
+    }
+
+    if (param->stfbc_fd > 0) {
+        close(param->stfbc_fd);
+        param->stfbc_fd = 0;
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void stf_fb_init(FBParam_t *param, uint32_t v4l2_fmt)
+{
+    int pixformat;
+    struct pp_mode pp_info[3];
+    struct fb_var_screeninfo vinfo;
+    struct fb_fix_screeninfo finfo;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    // step 1: try to set the fb format got from cmd options
+    pixformat = v4l2fmt_to_fbfmt(v4l2_fmt);
+    if (-1 == ioctl(param->stfbc_fd, FBIOPAN_GET_PP_MODE, &pp_info[0])) {
+        LOG(STF_LEVEL_ERR, "Error reading variable information.\n");
+        exit (EXIT_FAILURE);
+    }
+    LOG(STF_LEVEL_INFO, "Before set_pp_mode. get fb format:%d, will set fb format:%d\n",
+            pp_info[1].src.format, pixformat);
+
+    pp_info[1].src.format = pixformat;
+    if (-1 == ioctl(param->stfbc_fd, FBIOPAN_SET_PP_MODE, &pp_info[0])) {
+        LOG(STF_LEVEL_ERR, "Error reading variable information.\n");
+        exit (EXIT_FAILURE);
+    }
+
+    if (-1 == ioctl(param->stfbc_fd, FBIOPAN_GET_PP_MODE, &pp_info[0])) {
+        LOG(STF_LEVEL_ERR, "Error reading variable information.\n");
+        exit (EXIT_FAILURE);
+    }
+    LOG(STF_LEVEL_INFO, "After set_pp_mode. get fb format:%d\n", pp_info[1].src.format);
+    pixformat = pp_info[1].src.format;
+    param->pixformat = pixformat;
+
+    // step 2: Get fixed screen information
+    if (-1 == ioctl(param->fd, FBIOGET_FSCREENINFO, &finfo)) {
+        LOG(STF_LEVEL_ERR, "Error reading fixed information.\n");
+        exit (EXIT_FAILURE);
+    }
+    memcpy(&(param->finfo), &finfo, sizeof(finfo));
+
+    // step 3: Get variable screen information
+    if (-1 == ioctl(param->fd, FBIOGET_VSCREENINFO, &vinfo)) {
+        LOG(STF_LEVEL_ERR, "Error reading variable information.\n");
+        exit (EXIT_FAILURE);
+    }
+    memcpy(&(param->vinfo), &vinfo, sizeof(vinfo));
+    LOG(STF_LEVEL_INFO, "print screen information:\n");
+    LOG(STF_LEVEL_INFO, "  vinfo.xres = %d, vinfo.yres = %d, grayscale = %d\n", vinfo.xres, vinfo.yres, vinfo.grayscale);
+    LOG(STF_LEVEL_INFO, "  vinfo.xoffset = %d, vinfo.yoffset = %d\n", vinfo.xoffset, vinfo.yoffset);
+    LOG(STF_LEVEL_INFO, "  vinfo.bits_per_pixel = %d, finfo.line_length = %d\n", vinfo.bits_per_pixel, finfo.line_length);
+    // if (ioctl(param->fd, FBIOPUT_VSCREENINFO, &g_vinfo) < 0) {
+    //     LOG(STF_LEVEL_ERR, "FBIOPUT_VSCREENINFO.\n");
+    //     exit (EXIT_FAILURE);
+    // }
+
+    // step 4: mmap fb buf to user space and update the actually screen param
+    param->width = vinfo.xres;
+    param->height = vinfo.yres;
+    param->bpp = vinfo.bits_per_pixel;
+    param->screen_size = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;
+    // g_screensize = g_vinfo.xres * g_vinfo.yres * 32 / 8;
+    param->screen_buf = (uint8_t *)mmap(NULL, param->screen_size,
+            PROT_READ | PROT_WRITE, MAP_SHARED, param->fd, 0);
+    if (param->screen_buf == (void *)(-1)) {
+        LOG(STF_LEVEL_ERR, "Error: failed to map framebuffer device to memory.\n");
+        exit (EXIT_FAILURE) ;
+    }
+    memset(param->screen_buf, 0x00, param->screen_size);
+
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void stf_fb_uninit(FBParam_t *param)
+{
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    if (-1 == munmap((void *)param->screen_buf, param->screen_size)) {
+        LOG(STF_LEVEL_ERR, " Error: framebuffer device munmap() failed.\n");
+        exit (EXIT_FAILURE) ;
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+

+ 46 - 0
package/starfive/v4l2_test/stf_framebuffer.h

@@ -0,0 +1,46 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef __STF_FRAMEBUFFER_H__
+#define __STF_FRAMEBUFFER_H__
+#include <linux/fb.h>
+#include <stdbool.h>
+#include "common.h"
+
+struct pp_video_mode {
+    enum COLOR_FORMAT format;
+    unsigned int height;
+    unsigned int width;
+    unsigned int addr;
+};
+
+struct pp_mode {
+    char pp_id;
+    bool bus_out;        /*out to ddr*/
+    bool fifo_out;        /*out to lcdc*/
+    bool inited;
+    struct pp_video_mode src;
+    struct pp_video_mode dst;
+};
+
+typedef struct FBParam_t {
+    int         fd;
+    int         stfbc_fd;
+    struct fb_var_screeninfo vinfo;
+    struct fb_fix_screeninfo finfo;
+    uint8_t *screen_buf;
+    uint32_t screen_size;
+
+    int pixformat;      // default COLOR_YUV420_NV21
+    uint32_t width;     // default 1920
+    uint32_t height;    // default 1080
+    uint32_t bpp;
+} FBParam_t;
+
+extern void stf_fb_open(FBParam_t *param, char *device_name, char *stfbc_name);
+extern void stf_fb_close(FBParam_t *param);
+extern void stf_fb_init(FBParam_t *param, uint32_t v4l2_fmt);
+extern void stf_fb_uninit(FBParam_t *param);
+
+#endif // __STF_FRAMEBUFFER_H__

+ 104 - 0
package/starfive/v4l2_test/stf_log.c

@@ -0,0 +1,104 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+#include "stf_log.h"
+
+#define ANSI_COLOR_ERR      "\x1b[31m"       // RED
+#define ANSI_COLOR_TRACE    "\x1b[32m"       // GREEN
+#define ANSI_COLOR_WARN     "\x1b[33m"       // YELLOW
+#define ANSI_COLOR_BLUE     "\x1b[34m"       // BLUE
+#define ANSI_COLOR_INFO     ""
+// For future
+#define ANSI_COLOR_MAGENTA  "\x1b[35m"       // MAGENTA
+#define ANSI_COLOR_CYAN     "\x1b[36m"       // CYAN
+#define ANSI_COLOR_RESET    "\x1b[0m"        // RESET
+
+#define MAX_PRINT_LENGTH        512
+#define STF_LOG_FILE_PATH       "./STF_ErrorLog.txt"
+#define STF_LOG_ENV_VARIABLE    "V4L2_DEBUG"
+
+// static unsigned int log_decor = LOG_HAS_TIME | LOG_HAS_FILE | LOG_HAS_COLOR;
+static unsigned int log_decor = LOG_HAS_TIME | LOG_HAS_COLOR;
+static FILE *fpLog  = NULL;
+static int max_log_level = STF_LEVEL_ERR;
+
+int init_log()
+{
+    char *strDebug_level = NULL;
+    int level;
+
+    if ((log_decor & LOG_HAS_FILE) && !fpLog) {
+        fpLog = fopen(STF_LOG_FILE_PATH, "w");
+    }
+
+    strDebug_level = getenv(STF_LOG_ENV_VARIABLE);
+    if (strDebug_level) {
+        level = atoi(strDebug_level);
+        if (level >=0) {
+            max_log_level = level;
+        }
+    }
+
+    return 0;
+}
+
+void deinit_log()
+{
+    if (fpLog) {
+        fclose(fpLog);
+        fpLog = NULL;
+    }
+}
+
+void set_maxLogLevel(int level)
+{
+    max_log_level = level;
+}
+
+int get_maxLogLevel()
+{
+    return max_log_level;
+}
+
+void logmsg(int level, const char *format, ...)
+{
+    va_list ptr;
+    char    logBuf[MAX_PRINT_LENGTH] = {0};
+    char*   prefix = "";
+    char*   postfix= "";
+
+    if (level > max_log_level)
+        return;
+
+    if ((log_decor & LOG_HAS_COLOR)) {
+        postfix = ANSI_COLOR_RESET;
+        switch (level) {
+        case STF_LEVEL_ERR: prefix = ANSI_COLOR_ERR "[ERROR]";   break;
+        case STF_LEVEL_WARN:  prefix = ANSI_COLOR_WARN"[WARN ]";  break;
+        case STF_LEVEL_INFO:  prefix = ANSI_COLOR_INFO"[INFO ]";  break;
+        case STF_LEVEL_DEBUG:  prefix = ANSI_COLOR_INFO"[DEBUG]";  break;
+        case STF_LEVEL_LOG:  prefix = ANSI_COLOR_INFO"[LOG  ]";  break;
+        case STF_LEVEL_TRACE: prefix = ANSI_COLOR_TRACE"[TRACE]"; break;
+        default:    prefix = "";               break;
+        }
+    }
+
+    va_start(ptr, format);
+    vsnprintf(logBuf, MAX_PRINT_LENGTH, format, ptr);
+    va_end(ptr);
+
+    fputs(prefix,  stderr);
+    fputs(logBuf,  stderr);
+    fputs(postfix, stderr);
+
+    if ((log_decor & LOG_HAS_FILE) && fpLog) {
+        fwrite(logBuf, strlen(logBuf), 1,fpLog);
+        fflush(fpLog);
+    }
+}
+

+ 58 - 0
package/starfive/v4l2_test/stf_log.h

@@ -0,0 +1,58 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef __STF_LOG_H__
+#define __STF_LOG_H__
+
+// Log Debug
+typedef enum {
+  STF_LEVEL_NONE = 0,
+  STF_LEVEL_ERR,
+  STF_LEVEL_WARN,
+  STF_LEVEL_INFO,
+  STF_LEVEL_DEBUG,
+  STF_LEVEL_LOG,
+  STF_LEVEL_TRACE,
+  STF_LEVEL_ALL
+} StfDebugLevelType;
+
+enum {
+    LOG_HAS_DAY_NAME   =    1, /**< Include day name [default: no]         */
+    LOG_HAS_YEAR       =    2, /**< Include year digit [no]                */
+    LOG_HAS_MONTH      =    4, /**< Include month [no]                     */
+    LOG_HAS_DAY_OF_MON =    8, /**< Include day of month [no]              */
+    LOG_HAS_TIME       =   16, /**< Include time [yes]                     */
+    LOG_HAS_MICRO_SEC  =   32, /**< Include microseconds [yes]             */
+    LOG_HAS_FILE       =   64, /**< Include sender in the log [yes]        */
+    LOG_HAS_NEWLINE    =  128, /**< Terminate each call with newline [yes] */
+    LOG_HAS_CR         =  256, /**< Include carriage return [no]           */
+    LOG_HAS_SPACE      =  512, /**< Include two spaces before log [yes]    */
+    LOG_HAS_COLOR      = 1024, /**< Colorize logs [yes on win32]           */
+    LOG_HAS_LEVEL_TEXT = 2048 /**< Include level text string [no]          */
+};
+enum {
+    TERM_COLOR_R      = 2,   /**< Red            */
+    TERM_COLOR_G      = 4,   /**< Green          */
+    TERM_COLOR_B      = 1,   /**< Blue.          */
+    TERM_COLOR_BRIGHT = 8    /**< Bright mask.   */
+};
+
+int init_log(void);
+void deinit_log(void);
+void set_maxLogLevel(int level);
+int get_maxLogLevel(void);
+void logmsg(int level, const char *format, ...);
+
+#define ENABLE_DEBUG
+#ifdef ENABLE_DEBUG
+#define LOG(level, fmt, ...)  logmsg(level, "[%s,%d]: " fmt, __FUNCTION__, __LINE__, ##__VA_ARGS__);
+#define FuncIn()  logmsg(STF_LEVEL_DEBUG, "[%s,%d]:  FUNC IN\n",  __FUNCTION__, __LINE__);
+#define FuncOut() logmsg(STF_LEVEL_DEBUG, "[%s,%d]:  FUNC OUT\n",  __FUNCTION__, __LINE__);
+#else
+#define LOG(level, fmt, ...)
+#define FuncIn()
+#define FuncOut()
+#endif
+
+#endif // __STF_LOG_H__

+ 698 - 0
package/starfive/v4l2_test/stf_v4l2.c

@@ -0,0 +1,698 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include <sys/mman.h>
+#include <libv4l2.h>
+#include <sys/ioctl.h>
+#include "common.h"
+#include "stf_v4l2.h"
+#include "stf_log.h"
+
+#define FILENAME_MAX_LEN     30
+struct stfisp_fw_info {
+    char filename[FILENAME_MAX_LEN];
+};
+
+struct v4l2_subdev_frame_size_enum {
+    __u32 index;
+    __u32 pad;
+    __u32 code;
+    __u32 min_width;
+    __u32 max_width;
+    __u32 min_height;
+    __u32 max_height;
+    __u32 which;
+    __u32 reserved[8];
+};
+
+#define VIDIOC_STFISP_LOAD_FW \
+        _IOW('V', BASE_VIDIOC_PRIVATE + 1, struct stfisp_fw_info)
+
+#define VIDIOC_SUBDEV_ENUM_FRAME_SIZE \
+        _IOWR('V', 74, struct v4l2_subdev_frame_size_enum)
+#define MEDIA_BUS_FMT_SRGGB10_1X10  0x300f
+
+/**
+    Do ioctl and retry if error was EINTR ("A signal was caught during the ioctl() operation."). Parameters are the same as on ioctl.
+    \param fd file descriptor
+    \param request request
+    \param argp argument
+    \returns result from ioctl
+*/
+int xioctl(int fd, int request, void* argp)
+{
+    int r;
+
+    // TODO: the orign is v4l2_ioctl()
+    do r = ioctl(fd, request, argp);
+    while (-1 == r && EINTR == errno);
+
+    return r;
+}
+
+void sensor_image_size_info(V4l2Param_t *param)
+{
+    int fd = 0;
+    uint32_t i = 0;
+    struct v4l2_subdev_frame_size_enum frame_size;
+    LOG(STF_LEVEL_DEBUG, "go in sensor_image_size_info....\n");
+
+    fd = open(param->device_name, O_RDWR /* required */ | O_NONBLOCK, 0);
+    if (-1 == fd) {
+        LOG(STF_LEVEL_ERR, "Cannot open '%s': %d, %s\n", param->device_name, errno, strerror(errno));
+        close(fd);
+        exit(EXIT_FAILURE);
+    }
+
+    for (i = 0; i < 4; i++) {
+        frame_size.index = i;
+        frame_size.code = MEDIA_BUS_FMT_SRGGB10_1X10;
+        if (-1 == ioctl(fd, VIDIOC_SUBDEV_ENUM_FRAME_SIZE, &frame_size)) {
+            close(fd);
+            errno_exit("VIDIOC_SIZE_INFO");
+        }
+        LOG(STF_LEVEL_DEBUG, "image_size: width[%d] = %d, height[%d] = %d \n",
+                    i, frame_size.min_width, i, frame_size.min_height);
+    }
+    close(fd);
+}
+
+void loadfw_start(char *filename, V4l2Param_t *param)
+{
+    struct stfisp_fw_info fw_info = {0};
+    int fd = 0;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    fd = open(param->device_name, O_RDWR /* required */ | O_NONBLOCK, 0);
+    if (-1 == fd) {
+        LOG(STF_LEVEL_ERR, "Cannot open '%s': %d, %s\n", param->device_name, errno, strerror(errno));
+        exit(EXIT_FAILURE);
+    }
+
+    if (filename && (strlen(filename) < FILENAME_MAX_LEN)) {
+        memcpy(fw_info.filename, filename, strlen(filename) + 1);
+    }
+
+    LOG(STF_LEVEL_INFO, "VIDIOC_STFISP_LOAD_FW = 0x%lx, filename = %s, size = %lu, device=%s\n",
+            VIDIOC_STFISP_LOAD_FW, fw_info.filename, sizeof(struct stfisp_fw_info), param->device_name);
+    if (-1 == ioctl(fd, VIDIOC_STFISP_LOAD_FW, &fw_info)) {
+        if (EINVAL == errno) {
+            close(fd);
+            LOG(STF_LEVEL_ERR, "%s is no V4L2 device\n", param->device_name);
+            exit(EXIT_FAILURE);
+        } else {
+            close(fd);
+            errno_exit("VIDIOC_STFISP_LOAD_FW");
+        }
+    }
+
+    close(fd);
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+static void convert_v4l2_mem_type(int iomthd, enum v4l2_memory *mem_type)
+{
+    if (iomthd < IO_METHOD_MMAP || iomthd > IO_METHOD_READ) {
+        LOG(STF_LEVEL_ERR, "iomthd %d out of range\n", iomthd);
+        exit(EXIT_FAILURE);
+    }
+
+    switch (iomthd) {
+    case IO_METHOD_MMAP:
+        *mem_type = V4L2_MEMORY_MMAP;
+        break;
+    case IO_METHOD_USERPTR:
+        *mem_type = V4L2_MEMORY_USERPTR;
+        break;
+    case IO_METHOD_DMABUF:
+        *mem_type = V4L2_MEMORY_DMABUF;
+        break;
+    case IO_METHOD_READ:
+        *mem_type = 0;  // not use memory machanism
+        break;
+    default:
+        *mem_type = 0;  // not use memory machanism
+        break;
+    }
+}
+
+void stf_v4l2_open(V4l2Param_t *param, char *device_name)
+{
+    struct stat st;
+    //struct v4l2_capability cap;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    // stat file
+    if (-1 == stat(device_name, &st)) {
+        LOG(STF_LEVEL_ERR, "Cannot identify '%s': %d, %s\n", device_name, errno, strerror(errno));
+        exit(EXIT_FAILURE);
+    }
+
+    // check if is device
+    if (!S_ISCHR(st.st_mode)) {
+        LOG(STF_LEVEL_ERR, "%s is no device\n", device_name);
+        exit(EXIT_FAILURE);
+    }
+
+    // open device
+    param->fd = v4l2_open(device_name, O_RDWR /* required */ | O_NONBLOCK, 0);
+    // param->fd = v4l2_open(device_name, O_RDWR, 0);
+    if (-1 == param->fd) {
+        LOG(STF_LEVEL_ERR, "Cannot open '%s': %d, %s\n", device_name, errno, strerror(errno));
+        exit(EXIT_FAILURE);
+    }
+
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void stf_v4l2_close(V4l2Param_t *param)
+{
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    if (-1 == v4l2_close(param->fd)) {
+        errno_exit("close");
+    }
+    param->fd = -1;
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void stf_v4l2_init(V4l2Param_t *param)
+{
+    struct v4l2_capability cap;
+    struct v4l2_cropcap cropcap;
+    struct v4l2_crop crop;
+    struct v4l2_format fmt;
+    struct v4l2_streamparm frameint;
+    //struct v4l2_streamparm frameget;
+    unsigned int min;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    // query capability
+    if (-1 == xioctl(param->fd, VIDIOC_QUERYCAP, &cap)) {
+        if (EINVAL == errno) {
+            LOG(STF_LEVEL_ERR, "%s is no V4L2 device\n", param->device_name);
+            exit(EXIT_FAILURE);
+        } else {
+            errno_exit("VIDIOC_QUERYCAP");
+        }
+    }
+    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+        LOG(STF_LEVEL_ERR, "%s is no video capture device\n", param->device_name);
+        exit(EXIT_FAILURE);
+    }
+
+    switch (param->io_mthd) {
+    case IO_METHOD_READ:
+        if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
+            LOG(STF_LEVEL_ERR, "%s does not support read i/o\n", param->device_name);
+            exit(EXIT_FAILURE);
+        }
+        break;
+    case IO_METHOD_MMAP:
+    case IO_METHOD_USERPTR:
+    case IO_METHOD_DMABUF:
+        if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+            LOG(STF_LEVEL_ERR, "%s does not support streaming i/o\n", param->device_name);
+            exit(EXIT_FAILURE);
+        }
+        break;
+    default:
+        LOG(STF_LEVEL_ERR, "%s does not specify streaming i/o\n", param->device_name);
+        exit(EXIT_FAILURE);
+        break;
+    }
+
+    /* Select video input, video standard and tune here. */
+    CLEAR(cropcap);
+    cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    if (0 == xioctl(param->fd, VIDIOC_CROPCAP, &cropcap)) {
+        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        crop.c = cropcap.defrect; /* reset to default */
+        if (-1 == xioctl(param->fd, VIDIOC_S_CROP, &crop)) {
+            switch (errno) {
+            case EINVAL:
+                /* Cropping not supported. */
+                break;
+            default:
+                /* Errors ignored. */
+                break;
+            }
+        }
+    } else {
+        /* Errors ignored. */
+    }
+
+    /* If the user has set the fps to -1, don't try to set the frame interval */
+    if (param->fps != -1) {
+        CLEAR(frameint);
+        /* Attempt to set the frame interval. */
+        frameint.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        frameint.parm.capture.timeperframe.numerator = 1;
+        frameint.parm.capture.timeperframe.denominator = param->fps;
+        if (-1 == xioctl(param->fd, VIDIOC_S_PARM, &frameint)) {
+            LOG(STF_LEVEL_WARN, "Unable to set frame interval.\n");
+        }
+        LOG(STF_LEVEL_INFO, "set frame interval = %d.\n", frameint.parm.capture.timeperframe.denominator);
+    }
+
+    // set v4l2_format
+    CLEAR(fmt);
+    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    fmt.fmt.pix.width = param->width;
+    fmt.fmt.pix.height = param->height;
+    fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+    fmt.fmt.pix.pixelformat = param->format;
+    if (-1 == xioctl(param->fd, VIDIOC_S_FMT, &fmt)) {
+        errno_exit("VIDIOC_S_FMT");
+    }
+
+    if (param->crop_flag) {
+        struct v4l2_selection sel_crop = {
+            V4L2_BUF_TYPE_VIDEO_CAPTURE,
+            V4L2_SEL_TGT_CROP,
+            0,
+            {param->crop_info.left, param->crop_info.top,
+                param->crop_info.width, param->crop_info.height}  // TODO: opt here
+        };
+        struct v4l2_selection get_crop = {
+            V4L2_BUF_TYPE_VIDEO_CAPTURE,
+            V4L2_SEL_TGT_CROP,
+        };
+        LOG(STF_LEVEL_DEBUG, "sel_crop.left = %d, %d, %d, %d\n",
+            sel_crop.r.left, sel_crop.r.top, sel_crop.r.width, sel_crop.r.height);
+
+        if (-1 == xioctl(param->fd, VIDIOC_S_SELECTION, &sel_crop)) {
+            LOG(STF_LEVEL_ERR, "S_SELECTION Failed.\n");
+        }
+        LOG(STF_LEVEL_DEBUG, "sel_crop.left = %d, %d, %d, %d\n",
+            sel_crop.r.left, sel_crop.r.top, sel_crop.r.width, sel_crop.r.height);
+
+        if (-1 == xioctl(param->fd, VIDIOC_G_SELECTION, &get_crop)) {
+            LOG(STF_LEVEL_ERR, "G_SELECTION Failed.\n");
+        }
+        LOG(STF_LEVEL_DEBUG, "get_crop.left = %d, %d, %d, %d\n",
+            get_crop.r.left, get_crop.r.top, get_crop.r.width, get_crop.r.height);
+
+        if (memcmp(&sel_crop, &get_crop, sizeof(sel_crop))) {
+            LOG(STF_LEVEL_WARN, "set/get selection diff.\n");
+        }
+    }
+
+    // get v4l2_format
+    memset(&fmt, 0, sizeof(struct v4l2_format));
+    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    fmt.fmt.pix.field = V4L2_FIELD_ANY;
+    if (-1 == ioctl(param->fd, VIDIOC_G_FMT, &fmt)) {
+        errno_exit("VIDIOC_G_FMT");
+    }
+    LOG(STF_LEVEL_INFO, "VIDIOC_G_FMT: type=%d, Fourcc format=%c%c%c%c\n",
+            fmt.type, fmt.fmt.pix.pixelformat & 0xff,
+            (fmt.fmt.pix.pixelformat >> 8) &0xff,
+            (fmt.fmt.pix.pixelformat >> 16) &0xff,
+            (fmt.fmt.pix.pixelformat >> 24) &0xff);
+    LOG(STF_LEVEL_INFO, " \t width=%d, height=%d, field=%d, bytesperline=%d, sizeimage=%d\n",
+            fmt.fmt.pix.width, fmt.fmt.pix.height, fmt.fmt.pix.field,
+            fmt.fmt.pix.bytesperline, fmt.fmt.pix.sizeimage);
+
+    //if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_YUV420) {
+    //if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_RGB565) {
+    if (fmt.fmt.pix.pixelformat != param->format) {
+        LOG(STF_LEVEL_ERR, "v4l2 didn't accept format %d. Can't proceed.\n", param->format);
+        exit(EXIT_FAILURE);
+    }
+
+    /* Note VIDIOC_S_FMT may change width and height. */
+    if (param->width != fmt.fmt.pix.width) {
+        param->width = fmt.fmt.pix.width;
+        LOG(STF_LEVEL_WARN, "Correct image width set to %i by device %s.\n", param->width, param->device_name);
+    }
+
+    if (param->height != fmt.fmt.pix.height) {
+        param->height = fmt.fmt.pix.height;
+        LOG(STF_LEVEL_WARN, "Correct image height set to %i by device %s.\n", param->height, param->device_name);
+    }
+
+    // TODO: who and why add the below here? Change bytesperline and sizeimage
+    /* Buggy driver paranoia. */
+    min = fmt.fmt.pix.width * 2;
+    if (fmt.fmt.pix.bytesperline < min) {
+        fmt.fmt.pix.bytesperline = min;
+    }
+    min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
+    if (fmt.fmt.pix.sizeimage < min) {
+        fmt.fmt.pix.sizeimage = min;
+    }
+    // param->image_size = fmt.fmt.pix.sizeimage; // wrong here
+    LOG(STF_LEVEL_LOG, "fmt.fmt.pix.sizeimage=%d, fmt.fmt.pix.bytesperline=%d\n",
+        fmt.fmt.pix.sizeimage, fmt.fmt.pix.bytesperline);
+
+    // convert mem type
+    convert_v4l2_mem_type(param->io_mthd, &param->mem_type);
+
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void stf_v4l2_uninit(V4l2Param_t *param)
+{
+    uint32_t i;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    switch (param->io_mthd) {
+    case IO_METHOD_READ:
+        free(param->pBuffers[0].start);
+        break;
+
+    case IO_METHOD_MMAP:
+        for (i = 0; i < param->n_buffers; ++i) {
+            if (-1 == v4l2_munmap(param->pBuffers[i].start,
+                    param->pBuffers[i].length)) {
+                errno_exit("munmap");
+            }
+        }
+        break;
+
+    case IO_METHOD_USERPTR:
+        for (i = 0; i < param->n_buffers; ++i) {
+            free(param->pBuffers[i].start);
+        }
+        break;
+
+    case IO_METHOD_DMABUF:
+        break;
+
+    default:
+        break;
+    }
+    free(param->pBuffers);
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+static void stf_v4l2_readInit(V4l2Param_t *param)
+{
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    param->n_buffers = 1;
+    param->pBuffers = calloc(param->n_buffers, sizeof(*param->pBuffers));
+    if (!param->pBuffers) {
+        LOG(STF_LEVEL_ERR, "Out of memory\n");
+        exit(EXIT_FAILURE);
+    }
+
+    param->pBuffers[0].length = param->image_size;
+    param->pBuffers[0].start = malloc(param->image_size);
+    if (!param->pBuffers[0].start) {
+        LOG(STF_LEVEL_ERR, "Out of memory\n");
+        exit(EXIT_FAILURE);
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+static void stf_v4l2_mmapInit(V4l2Param_t *param)
+{
+    int i = 0;
+    struct v4l2_requestbuffers req;
+    CLEAR(req);
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    req.count = BUFCOUNT;
+    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    req.memory = V4L2_MEMORY_MMAP;
+    if (-1 == xioctl(param->fd, VIDIOC_REQBUFS, &req)) {
+        if (EINVAL == errno) {
+            LOG(STF_LEVEL_ERR, "%s does not support memory mapping\n", param->device_name);
+            exit(EXIT_FAILURE);
+        } else {
+            errno_exit("VIDIOC_REQBUFS");
+        }
+    }
+    if (req.count < 2) {
+        LOG(STF_LEVEL_ERR, "Insufficient buffer memory on %s\n", param->device_name);
+        exit(EXIT_FAILURE);
+    }
+
+    param->n_buffers = req.count;
+    param->pBuffers = calloc(param->n_buffers, sizeof(*param->pBuffers));
+    if (!param->pBuffers) {
+        LOG(STF_LEVEL_ERR, "Out of memory\n");
+        exit(EXIT_FAILURE);
+    }
+
+    for (i = 0; i < param->n_buffers; ++i) {
+        struct v4l2_buffer buf;
+        CLEAR(buf);
+        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory = V4L2_MEMORY_MMAP;
+        buf.index = i;
+        if (-1 == xioctl(param->fd, VIDIOC_QUERYBUF, &buf))
+            errno_exit("VIDIOC_QUERYBUF");
+
+        param->pBuffers[i].length = buf.length;
+        param->pBuffers[i].start = v4l2_mmap(NULL, /* start anywhere */
+                buf.length, PROT_READ | PROT_WRITE, /* required */
+                MAP_SHARED, /* recommended */
+                param->fd, buf.m.offset);
+        if (MAP_FAILED == param->pBuffers[i].start) {
+            errno_exit("mmap");
+        }
+        param->image_size = buf.length; // NOTE: updated value
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+static void stf_v4l2_dmabufInit(V4l2Param_t *param, int *dmabufs, int count)
+{
+    int i = 0;
+    struct v4l2_requestbuffers req;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    CLEAR(req);
+    req.count = BUFCOUNT;  // TODO: modify later
+    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    req.memory = V4L2_MEMORY_DMABUF;
+
+    if (-1 == xioctl(param->fd, VIDIOC_REQBUFS, &req)) {
+        if (EINVAL == errno) {
+            fprintf(stderr, "does not support dmabuf\n");
+            exit(EXIT_FAILURE);
+        } else {
+            errno_print("VIDIOC_REQBUFS");
+        }
+    }
+    if (req.count < 2) {
+        fprintf(stderr, "Insufficient buffer memory\n");
+        exit(EXIT_FAILURE);
+    }
+
+    param->n_buffers = req.count;
+    param->pBuffers = calloc(param->n_buffers, sizeof(*param->pBuffers));
+    if (!param->pBuffers) {
+        LOG(STF_LEVEL_ERR, "Out of memory\n");
+        exit(EXIT_FAILURE);
+    }
+
+    for (i = 0; i < req.count; ++i) {
+        struct v4l2_buffer buf;
+        CLEAR(buf);
+        buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory      = V4L2_MEMORY_DMABUF;
+        buf.index       = i;
+        if (-1 == xioctl(param->fd, VIDIOC_QUERYBUF, &buf)) {
+            errno_print("VIDIOC_QUERYBUF");
+        }
+        param->pBuffers[i].index = buf.index;
+        param->pBuffers[i].dmabuf_fd = dmabufs[i];
+#if 0
+        param->pBuffers[i].length = buf.length;
+        param->pBuffers[i].start = mmap(NULL /* start anywhere */,
+                buf.length,
+                PROT_READ | PROT_WRITE /* required */,
+                MAP_SHARED /* recommended */,
+                dmabufs[i], 0);
+
+        if (MAP_FAILED == param->pBuffers[i].start)
+            errno_print("mmap");
+#endif
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+static void stf_v4l2_userptrInit(V4l2Param_t *param)
+{
+    struct v4l2_requestbuffers req;
+    unsigned int page_size;
+    unsigned int buffer_size = param->image_size;
+    int i = 0;
+
+    page_size = getpagesize();
+    buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
+
+    CLEAR(req);
+    req.count = BUFCOUNT;
+    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    req.memory = V4L2_MEMORY_USERPTR;
+    if (-1 == xioctl(param->fd, VIDIOC_REQBUFS, &req)) {
+        if (EINVAL == errno) {
+            LOG(STF_LEVEL_ERR, "%s does not support user pointer i/o\n", param->device_name);
+            exit(EXIT_FAILURE);
+        } else {
+            errno_exit("VIDIOC_REQBUFS");
+        }
+    }
+
+    param->n_buffers = 4;
+    param->pBuffers = calloc(param->n_buffers, sizeof(*param->pBuffers));
+    if (!param->pBuffers) {
+        LOG(STF_LEVEL_ERR, "Out of memory\n");
+        exit(EXIT_FAILURE);
+    }
+
+    for (i = 0; i < param->n_buffers; ++i) {
+        param->pBuffers[i].length = buffer_size;
+        param->pBuffers[i].start = memalign(/* boundary */ page_size, buffer_size);
+        if (!param->pBuffers[i].start) {
+            LOG(STF_LEVEL_ERR, "Out of memory\n");
+            exit(EXIT_FAILURE);
+        }
+    }
+    param->image_size = buffer_size;
+}
+
+void sft_v4l2_prepare_capturing(V4l2Param_t *param, int *dmabufs, int count)
+{
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    switch (param->io_mthd) {
+    case IO_METHOD_READ:
+        stf_v4l2_readInit(param);
+        break;
+
+    case IO_METHOD_MMAP:
+        stf_v4l2_mmapInit(param);
+        break;
+
+    case IO_METHOD_USERPTR:
+        stf_v4l2_userptrInit(param);
+        break;
+
+    case IO_METHOD_DMABUF:
+        stf_v4l2_dmabufInit(param, dmabufs, count);
+        break;
+
+    default:
+        LOG(STF_LEVEL_ERR, "%s does not specify streaming i/o\n", param->device_name);
+        exit(EXIT_FAILURE);
+        break;
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void sft_v4l2_start_capturing(V4l2Param_t *param)
+{
+    unsigned int i;
+    enum v4l2_buf_type type;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    switch (param->io_mthd) {
+    case IO_METHOD_READ:
+        /* Nothing to do. */
+        break;
+
+    case IO_METHOD_MMAP:
+    case IO_METHOD_USERPTR:
+    case IO_METHOD_DMABUF:
+        for (i = 0; i < param->n_buffers; ++i) {
+            stf_v4l2_queue_buffer(param, i);
+        }
+        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        if (-1 == xioctl(param->fd, VIDIOC_STREAMON, &type)) {
+            errno_exit("VIDIOC_STREAMON");
+        }
+        break;
+
+    default:
+        break;
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+void stf_v4l2_stop_capturing(V4l2Param_t *param)
+{
+    enum v4l2_buf_type type;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    switch (param->io_mthd) {
+    case IO_METHOD_READ:
+        /* Nothing to do. */
+        break;
+    case IO_METHOD_MMAP:
+    case IO_METHOD_USERPTR:
+    case IO_METHOD_DMABUF:
+        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        if (-1 == xioctl(param->fd, VIDIOC_STREAMOFF, &type))
+            errno_exit("VIDIOC_STREAMOFF");
+        break;
+    default:
+        break;
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+// support V4L2_MEMORY_MMAP / V4L2_MEMORY_USERPTR / V4L2_MEMORY_DMABUF
+// NOTE: for V4L2_MEMORY_USERPTR, index is pBuffers index
+void stf_v4l2_queue_buffer(V4l2Param_t *param, int index)
+{
+    struct v4l2_buffer buf;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    assert(index < param->n_buffers);
+    CLEAR(buf);
+    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    buf.memory = param->mem_type;
+    buf.index = index; // NOTE: for V4L2_MEMORY_USERPTR not used here?
+
+    if (param->mem_type == V4L2_MEMORY_DMABUF) {
+        //buf.m.fd = param->dmabuf_fd;
+        buf.m.fd = param->pBuffers[index].dmabuf_fd;
+    } else if (param->mem_type == V4L2_MEMORY_USERPTR) {
+        buf.m.userptr = (unsigned long)param->pBuffers[index].start;
+        buf.length = param->pBuffers[index].length;
+    }
+    if (-1 == xioctl(param->fd, VIDIOC_QBUF, &buf)) {
+        errno_print("VIDIOC_QBUF");
+    }
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+// support V4L2_MEMORY_MMAP / V4L2_MEMORY_USERPTR / V4L2_MEMORY_DMABUF
+int stf_v4l2_dequeue_buffer(V4l2Param_t *param, struct v4l2_buffer *buf)
+{
+    int index = 0;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    PCLEAR(buf);
+    buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    buf->memory = param->mem_type;
+    if (-1 == xioctl(param->fd, VIDIOC_DQBUF, buf)) {
+        switch (errno) {
+        case EAGAIN:
+            return 0;
+        case EIO:
+            /* Could ignore EIO, see spec. */
+            /* fall through */
+        default:
+            errno_print("VIDIOC_DQBUF");
+        }
+    }
+
+    index = buf->index;
+    if (param->mem_type == V4L2_MEMORY_USERPTR) {
+        for (index = 0; index < param->n_buffers; ++index) {
+            if (buf->m.userptr == (unsigned long)param->pBuffers[index].start
+                    && buf->length == param->pBuffers[index].length) {
+                break;
+            }
+        }
+    }
+
+    assert(index < param->n_buffers);
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+    return 1;
+}

+ 51 - 0
package/starfive/v4l2_test/stf_v4l2.h

@@ -0,0 +1,51 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef __STF_V4L2_H__
+#define __STF_V4L2_H__
+
+#include "common.h"
+
+// reference to enum v4l2_memory
+
+typedef struct V4l2Param_t {
+    char        *device_name;// = "/dev/video0";
+    int         fd;
+    IOMethod    io_mthd;  // IO_METHOD_MMAP
+    enum v4l2_memory mem_type;
+
+    // int         dmabuf_fd; // for IO_METHOD_DMABUF
+
+    struct buffer *pBuffers;
+    uint32_t  n_buffers;
+
+    uint32_t image_size;
+    uint32_t format;  // = V4L2_PIX_FMT_RGB565
+
+    uint32_t width;     // = 1920;
+    uint32_t height;    // = 1080;
+    uint32_t fps; // = 30;
+
+    int crop_flag;
+    struct v4l2_rect   crop_info;
+
+} V4l2Param_t;
+
+extern int xioctl(int fd, int request, void* argp);
+
+extern void sensor_image_size_info(V4l2Param_t *param);
+extern void loadfw_start(char *filename, V4l2Param_t *param);
+
+extern void stf_v4l2_open(V4l2Param_t *param, char *device_name);
+extern void stf_v4l2_close(V4l2Param_t *param);
+extern void stf_v4l2_init(V4l2Param_t *param);
+extern void stf_v4l2_uninit(V4l2Param_t *param);
+extern void sft_v4l2_prepare_capturing(V4l2Param_t *param, int *dmabufs, int count);
+extern void sft_v4l2_start_capturing(V4l2Param_t *param);
+extern void stf_v4l2_stop_capturing(V4l2Param_t *param);
+
+extern void stf_v4l2_queue_buffer(V4l2Param_t *param, int index);
+extern int stf_v4l2_dequeue_buffer(V4l2Param_t *param, struct v4l2_buffer *buf);
+
+#endif // __STF_V4L2_H__

File diff suppressed because it is too large
+ 311 - 713
package/starfive/v4l2_test/v4l2_test.c


+ 13 - 3
package/starfive/v4l2_test/v4l2_test.mk

@@ -12,11 +12,21 @@ define V4L2_TEST_BUILD_CMDS
 	cp package/starfive/v4l2_test/yuv.h $(@D)/
 	cp package/starfive/v4l2_test/convert.c $(@D)/
 	cp package/starfive/v4l2_test/convert.h $(@D)/
+	cp package/starfive/v4l2_test/stf_v4l2.c $(@D)/
+	cp package/starfive/v4l2_test/stf_v4l2.h $(@D)/
+	cp package/starfive/v4l2_test/stf_framebuffer.c $(@D)/
+	cp package/starfive/v4l2_test/stf_framebuffer.h $(@D)/
+	cp package/starfive/v4l2_test/stf_drm.c $(@D)/
+	cp package/starfive/v4l2_test/stf_drm.h $(@D)/
 	cp package/starfive/v4l2_test/config.h $(@D)/
-	cp package/starfive/v4l2_test/string.c $(@D)/
+	cp package/starfive/v4l2_test/common.c $(@D)/
+	cp package/starfive/v4l2_test/common.h $(@D)/
+	cp package/starfive/v4l2_test/stf_log.c $(@D)/
+	cp package/starfive/v4l2_test/stf_log.h $(@D)/
 	cp package/starfive/v4l2_test/pipeline_setting.sh $(@D)/
-	(cd $(@D); $(TARGET_CC) -Wall -O2 v4l2_test.c yuv.c convert.c string.c -l v4l2 -l jpeg -o v4l2test)
-	#(cd $(@D); $(TARGET_CC) -Wall -O2 v4l2_test.c -o v4l2test)
+	(cd $(@D); $(TARGET_CC) -Wall -O2 v4l2_test.c common.c yuv.c convert.c \
+			stf_v4l2.c stf_framebuffer.c stf_drm.c stf_log.c -lv4l2 -ljpeg \
+			-I$(STAGING_DIR)/usr/include/drm -ldrm -o v4l2test)
 endef
 
 define V4L2_TEST_INSTALL_TARGET_CMDS

+ 11 - 5
package/starfive/v4l2_test/yuv.c

@@ -51,7 +51,9 @@ void YUV420toYUV444(int width, int height, unsigned char* src, unsigned char* ds
 	}
 }
 
-void YUV420NV21toYUV444(int width, int height, unsigned char* src, unsigned char* dst) {
+void YUV420NV21toYUV444(int width, int height, unsigned char* src, unsigned char* dst,
+        int is_nv21)
+{
 	int line, column;
 	unsigned char *py, *pu, *pv;
 	unsigned char *tmp = dst;
@@ -64,10 +66,14 @@ void YUV420NV21toYUV444(int width, int height, unsigned char* src, unsigned char
 
 	for (line = 0; line < height; ++line) {
 		for (column = 0; column < width; ++column) {
-			py = base_py+(line*width)+column;
-			pu = base_pu+((line/2*width/2)+column/2)*2+1;
-			pv = base_pv+((line/2*width/2)+column/2)*2;
-
+			py = base_py + (line * width) + column;
+            if (is_nv21) {
+                pu = base_pu + ((line / 2 * width / 2) + column / 2) * 2 + 1;
+                pv = base_pv + ((line / 2 * width / 2) + column / 2) * 2;
+            } else {
+                pu = base_pu + ((line / 2 * width / 2) + column / 2) * 2;
+                pv = base_pv + ((line / 2 * width / 2) + column / 2) * 2 + 1;
+            }
 			*tmp++ = *py;
 			*tmp++ = *pu;
 			*tmp++ = *pv;

+ 3 - 3
package/starfive/v4l2_test/yuv.h

@@ -4,7 +4,7 @@
 void YUV420toYUV444(int width, int height, unsigned char* src, unsigned char* dst);
 void YUV422toYUV444(int width, int height, unsigned char* src, unsigned char* dst);
 void RGB565toRGB888(int width, int height, unsigned char* src, unsigned char* dst);
-void RAW12toRAW16(int width, int height, unsigned char* src, unsigned char* dst);
-void YUV420NV21toYUV444(int width, int height, unsigned char* src, unsigned char* dst);
-
+void RAW12toRAW16(int width, int height, const unsigned char* src, unsigned char* dst);
+void YUV420NV21toYUV444(int width, int height, unsigned char* src, unsigned char* dst,
+        int is_nv21);
 #endif

Some files were not shown because too many files changed in this diff