package: Move v4l2test to starfive package

1.Create starfive package, and move v4l2test into it. 2.Sync the latest v4l2test package from JH7100. Signed-off-by: mason.huo <mason.huo@starfivetech.com> (cherry-pick commit 87583e4004ba7ceefa317d939ea8004f38eb9570 from jh7110-mm-devel branch)
2022-04-15 18:37:37 +08:00
parent 313e975390
commit cf9c62a4f1
27 changed files with 3693 additions and 2307 deletions
@@ -2573,9 +2573,7 @@ menu "Text editors and viewers"
 endmenu


-
-menu "v4l2 test"
-       source "package/v4l2_test/Config.in"
-       source "package/v4lutils/Config.in"
+menu "starfive packages"
+	source "package/starfive/Config.in"
 endmenu
 endmenu
@@ -0,0 +1,2 @@
+# starfive packages
+source "package/starfive/v4l2_test/Config.in"
@@ -0,0 +1 @@
+include $(sort $(wildcard package/starfive/*/*.mk))
@@ -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);
+}
+
+
@@ -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__
@@ -1,56 +1,73 @@
-#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>
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "common.h"

-extern struct fb_var_screeninfo vinfo;
-extern struct fb_fix_screeninfo finfo;
-extern int screensize;
+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

-int yuyv_resize(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int imgHeight)
+void update_videocvt_param(int distype, int scr_width, int scr_height,
+        int scr_bpp, int scr_size)
 {
-    int rows ,cols;   /* 行列标志 */
-    unsigned char *YUVindata, *YUVoutdata;    /* YUV和RGB数据指针 */
-    int YUVinpos;    /* Y U V在数据缓存中的偏移 */
-    unsigned int i = 0;
+    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;
+    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(screensize);
-    unsigned int start_timems;
-    unsigned int end_timems;
+    uint32_t start_timems;
+    uint32_t end_timems;
    struct timeval ts_start, ts_end;
+    static uint8_t *tmp = NULL;

    if (!tmp)
-        return -1;
+    {
+        tmp = malloc(g_screen_size);
+        if (tmp)
+        {
+            // for YUYV buffer, set it to black
+            for ( rows = 0; rows < g_screen_size; rows++)
+            {
+                if (rows == 0 || rows % 2 == 0)
+                {
+                    //even is Y
+                    tmp[rows] = 0;
+                }
+                else
+                {
+                    //odd is U or V
+                    tmp[rows] = 128;
+                }
+            }
+        }
+    }
+    assert(tmp);

    gettimeofday(&ts_start, NULL);

-    width = imgWidth > vinfo.xres ? vinfo.xres : imgWidth;
-    height = imgHeight > vinfo.yres ? vinfo.yres : imgHeight;
-    x_offset = (vinfo.xres - width) / 2;
-    y_offset = (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 == vinfo.xres) {
-        YUVinpos = (y_offset * 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);
@@ -58,15 +75,15 @@ int yuyv_resize(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int i
        start_timems = ts_start.tv_sec * 1000 + ts_start.tv_usec/1000;
        end_timems = ts_end.tv_sec * 1000 + ts_end.tv_usec/1000;
        // printf("%s: copy use %dms, sizeof(int) = %d\n", __func__, end_timems - start_timems, sizeof(int));
-        free(tmp);
+        //free(tmp);
        return 0;
    }

-    /* 每个像素两个字节 */
+    /* two bytes for one pixels */
    for(rows = 0; rows < height; rows++)
    {
-        // vinfo.xres, vinfo.yres vinfo.bits_per_pixel
-        YUVoutdata = tmp + ((rows + y_offset) * 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);
@@ -79,53 +96,61 @@ int yuyv_resize(unsigned char *inBuf, unsigned char *outBuf, int imgWidth, int i

    gettimeofday(&ts_start, NULL);

-    memcpy(outBuf, tmp, screensize);
+    memcpy(outBuf, tmp, g_screen_size);

    gettimeofday(&ts_end, NULL);
    start_timems = ts_start.tv_sec * 1000 + ts_start.tv_usec/1000;
    end_timems = ts_end.tv_sec * 1000 + ts_end.tv_usec/1000;
    // printf("%s: copy use %dms, sizeof(int) = %d\n", __func__, end_timems - start_timems, sizeof(int));

-    free(tmp);
+    //free(tmp);
    return 0;

 }

-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;   /* 行列标志 */
-    int y, u, v, r, g, b;   /* yuv rgb 相关分量 */
-    unsigned char *nv12data, *YUVdata;    /* YUV和RGB数据指针 */
-    int Ypos, Upos, Vpos;    /* Y U V在数据缓存中的偏移 */
-    int fb_Ypos, fb_Upos, fb_Vpos;    /* Y U V在数据缓存中的偏移 */
-    unsigned int i = 0;
+    int rows, cols;
+    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(screensize);
-    unsigned int start_timems;
-    unsigned int end_timems;
+    uint32_t start_timems;
+    uint32_t end_timems;
    struct timeval ts_start, ts_end;
+    static uint8_t *tmp = NULL;

    if (!tmp)
-        return -1;
+    {
+        tmp = malloc(g_screen_size);
+        if (tmp)
+        {
+            // for NV12 buffer, set it to black
+            memset(tmp, 0, g_screen_size / 3 * 2);
+            memset(tmp + g_screen_size / 3 * 2, 128, g_screen_size / 3);
+        }
+    }
+    assert(tmp);

    gettimeofday(&ts_start, NULL);

-    width = imgWidth > vinfo.xres ? vinfo.xres : imgWidth;
-    height = imgHeight > vinfo.yres ? vinfo.yres : imgHeight;
-    x_offset = (vinfo.xres - width) / 2;
-    y_offset = (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;

-    /* 每个像素两个字节 */
+    /* two bytes for every pixels */
    for(rows = 0; rows < height; rows++)
    {
-        // vinfo.xres, vinfo.yres vinfo.bits_per_pixel
-        fb_Ypos = ((rows + y_offset) * vinfo.xres + x_offset);
-        fb_Upos = ((rows + y_offset) / 2 * vinfo.xres / 2 + x_offset / 2) * 2;
-        fb_Upos = vinfo.xres * 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;
@@ -145,82 +170,101 @@ int convert_yuyv_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWid

    gettimeofday(&ts_start, NULL);

-    memcpy(outBuf, tmp, screensize);
+    memcpy(outBuf, tmp, g_screen_size);

    gettimeofday(&ts_end, NULL);
    start_timems = ts_start.tv_sec * 1000 + ts_start.tv_usec/1000;
    end_timems = ts_end.tv_sec * 1000 + ts_end.tv_usec/1000;
    // printf("%s: copy use %dms, sizeof(int) = %d\n", __func__, end_timems - start_timems, sizeof(int));

-    free(tmp);
+    //free(tmp);
    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;   /* 行列标志 */
-    int y, u, v, r, g, b;   /* yuv rgb 相关分量 */
-    unsigned char *nv12data, *nv21data;    /* YUV和RGB数据指针 */
-    int Ypos, Upos, Vpos;    /* Y U V在数据缓存中的偏移 */
-    int fb_Ypos, fb_Upos, fb_Vpos;    /* Y U V在数据缓存中的偏移 */
-    unsigned int i = 0;
+    int rows, cols;
+    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(screensize);
-    unsigned int start_timems;
-    unsigned int end_timems;
+    static uint8_t *tmp = NULL;
+    uint32_t start_timems;
+    uint32_t end_timems;
    struct timeval ts_start, ts_end;

    if (!tmp)
-        return -1;
+    {
+        tmp = malloc(g_screen_size);
+        if (tmp)
+        {
+            // for NV12 buffer, set it to black
+            memset(tmp, 0, g_screen_size / 3 * 2);
+            memset(tmp + g_screen_size / 3 * 2, 128, g_screen_size / 3);
+        }
+    }
+    assert(tmp);

    gettimeofday(&ts_start, NULL);

-    width = imgWidth > vinfo.xres ? vinfo.xres : imgWidth;
-    height = imgHeight > vinfo.yres ? vinfo.yres : imgHeight;
-    x_offset = (vinfo.xres - width) / 2;
-    y_offset = (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 == vinfo.xres) {
-        fb_Ypos = y_offset * vinfo.xres + x_offset;
-        fb_Upos = (y_offset / 2 * vinfo.xres / 2 + x_offset / 2) * 2;
-        fb_Upos = vinfo.xres * 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);
-        free(tmp);
+
+        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;
    }

-    /* 每个像素两个字节 */
+    /* two bytes for every pixels */
    for(rows = 0; rows < height; rows+=2)
    {
-        // vinfo.xres, vinfo.yres vinfo.bits_per_pixel
-        fb_Ypos = ((rows + y_offset) * vinfo.xres + x_offset);
-        fb_Upos = ((rows + y_offset) / 2 * vinfo.xres / 2 + x_offset / 2) * 2;
-        fb_Upos = vinfo.xres * 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+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);
@@ -230,29 +274,30 @@ int convert_nv21_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgWid

    gettimeofday(&ts_start, NULL);

-    memcpy(outBuf, tmp, screensize);
+    memcpy(outBuf, tmp, g_screen_size);

    gettimeofday(&ts_end, NULL);
    start_timems = ts_start.tv_sec * 1000 + ts_start.tv_usec/1000;
    end_timems = ts_end.tv_sec * 1000 + ts_end.tv_usec/1000;
    // printf("%s: copy use %dms, sizeof(int) = %d\n", __func__, end_timems - start_timems, sizeof(int));

-    free(tmp);
+    //free(tmp);
    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;   /* yuv rgb 相关分量 */
-    unsigned char *YUVdata, *RGBdata;    /* YUV和RGB数据指针 */
-    int Ypos, Upos, Vpos;    /* Y U V在数据缓存中的偏移 */
-    unsigned int i = 0;
+    int rows ,cols;
+    int y, u, v, r, g, b;
+    const uint8_t *YUVdata;
+    uint8_t *RGBdata;
+    int Ypos, Upos, Vpos;
+    uint32_t i = 0;
    int width, height;
    int x_offset, y_offset;
-    unsigned char *tmp = malloc(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)
@@ -260,19 +305,19 @@ int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt

    gettimeofday(&ts_start, NULL);

-    width = imgWidth > vinfo.xres ? vinfo.xres : imgWidth;
-    height = imgHeight > vinfo.yres ? vinfo.yres : imgHeight;
-    x_offset = (vinfo.xres - width) / 2;
-    y_offset = (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;

-    /* 每个像素两个字节 */
+    /* two bytes for every pixels */
    for(rows = 0; rows < height; rows++)
    {
-        // vinfo.xres, vinfo.yres vinfo.bits_per_pixel
-        RGBdata = tmp + ((rows + y_offset) * vinfo.xres + x_offset) * 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;
@@ -282,9 +327,8 @@ int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
            Upos = Vpos + 1;
        i = 0;

-        for(cols = 0; cols < width; cols++)
+        for (cols = 0; cols < width; cols++)
        {
-            /* 矩阵推到，百度 */
            y = YUVdata[Ypos];
            u = YUVdata[Upos] - 128;
            v = YUVdata[Vpos] - 128;
@@ -297,11 +341,11 @@ int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
            g = g > 255 ? 255 : (g < 0 ? 0 : g);
            b = b > 255 ? 255 : (b < 0 ? 0 : b);

-            /* 从低到高r g b */
-            if (vinfo.bits_per_pixel == 16) {   // RGB565
-                *(RGBdata ++) = (((g & 0x1c) << 3) | (b >> 3));   /* g低5位，b高5位 */
-                *(RGBdata ++) = ((r & 0xf8) | (g >> 5));    /* r高5位，g高3位 */
-            } else if (vinfo.bits_per_pixel == 24) {   // RGB888
+            /* low -> high r g b */
+            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_screen_bpp == 24) {   // RGB888
                *(RGBdata ++) = b;
                *(RGBdata ++) = g;
                *(RGBdata ++) = r;
@@ -313,7 +357,7 @@ int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
            }
            Ypos++;
            i++;
-            /* 每4个Y更新一次UV */
+            /* every 4 time y to update 1 time uv */
            if(!(i & 0x03))
            {
                Vpos = Upos = imgWidth * imgHeight + Ypos/2;
@@ -333,10 +377,10 @@ int convert_nv21_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
    gettimeofday(&ts_start, NULL);

 #if 1
-    memcpy(outBuf, tmp, screensize);
+    memcpy(outBuf, tmp, g_screen_size);
 #else
    int *p_outBuf, *p_tmp;
-    int size = screensize/4;
+    int size = g_screen_size/4;
    p_outBuf = outBuf;
    p_tmp = tmp;

@@ -393,13 +437,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

@@ -407,48 +451,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;
@@ -487,34 +531,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 > vinfo.xres ? vinfo.xres : image_width;
-    height = image_height > vinfo.yres ? vinfo.yres : image_height;
-    x_offset = (vinfo.xres - width) / 2;
-    y_offset = (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)
@@ -522,10 +566,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)*vinfo.xres+x_offset];            //Output Y row width is image_width bytes (one Y element per pixel).
-        Y1 = &J[(y+1+y_offset)*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[vinfo.xres*vinfo.yres+((y+y_offset)/2*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,
@@ -538,20 +582,14 @@ 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)
 {
-    int rows ,cols;    /* 行列标志 */
-    int y, u, v, r, g, b;    /* yuv rgb 相关分量 */
-    unsigned char *YUVdata, *RGBdata;    /* YUV和RGB数据指针 */
-    int Ypos, Upos, Vpos;    /* Y U V在数据缓存中的偏移 */
-    unsigned int i = 0;
-    unsigned char *tmp = malloc(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);

-
    Rgb2NV12(inBuf, 2, imgWidth, imgHeight, tmp);

    clock_gettime(CLOCK_MONOTONIC, &ts_end);
@@ -561,7 +599,7 @@ int convert_rgb565_to_nv12(unsigned char *inBuf, unsigned char *outBuf, int imgW

    clock_gettime(CLOCK_MONOTONIC, &ts_start);

-    memcpy(outBuf, tmp, 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;
@@ -572,34 +610,35 @@ 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;    /* yuv rgb 相关分量 */
-    unsigned char *YUVdata, *RGBdata;    /* YUV和RGB数据指针 */
-    int Ypos, Upos, Vpos;    /* Y U V在数据缓存中的偏移 */
-    unsigned int i = 0;
+    int rows ,cols;
+    int y, u, v, r, g, b;
+    const uint8_t *YUVdata;
+    uint8_t *RGBdata;
+    int Ypos, Upos, Vpos;
+    uint32_t i = 0;
    int width, height;
    int x_offset, y_offset;
-    unsigned char *tmp = malloc(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 > vinfo.xres ? vinfo.xres : imgWidth;
-    height = imgHeight > vinfo.yres ? vinfo.yres : imgHeight;
-    x_offset = (vinfo.xres - width) / 2;
-    y_offset = (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;

-    /* 每个像素两个字节 */
+    /* two bytes for every pixels */
    for(rows = 0; rows < height; rows++)
    {
-        // vinfo.xres, vinfo.yres vinfo.bits_per_pixel
-        RGBdata = tmp + ((rows + y_offset) * vinfo.xres + x_offset) * 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;
@@ -608,7 +647,6 @@ int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt

        for(cols = 0; cols < width; cols++)
        {
-            /* 矩阵推到，百度 */
            y = YUVdata[Ypos];
            u = YUVdata[Upos] - 128;
            v = YUVdata[Vpos] - 128;
@@ -621,11 +659,11 @@ int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
            g = g > 255 ? 255 : (g < 0 ? 0 : g);
            b = b > 255 ? 255 : (b < 0 ? 0 : b);

-            /* 从低到高r g b */
-            if (vinfo.bits_per_pixel == 16) {   // RGB565
-                *(RGBdata ++) = (((g & 0x1c) << 3) | (b >> 3));    /* g低5位，b高5位 */
-                *(RGBdata ++) = ((r & 0xf8) | (g >> 5));    /* r高5位，g高3位 */
-            } else if (vinfo.bits_per_pixel == 24) {   // RGB888
+            /* low -> high r g b */
+            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_screen_bpp == 24) {   // RGB888
                *(RGBdata ++) = b;
                *(RGBdata ++) = g;
                *(RGBdata ++) = r;
@@ -635,11 +673,11 @@ int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt
                *(RGBdata ++) = r;
                *(RGBdata ++) = 0xFF;
            }
-            /* 两个字节数据中包含一个Y */
+            /* two bytes contain 1 y */
            Ypos += 2;
            //Ypos++;
            i++;
-            /* 每两个Y更新一次UV */
+            /* every 2 y to update 1 uv */
            if(!(i & 0x01))
            {
                Upos = Ypos + 1;
@@ -655,7 +693,7 @@ int convert_yuyv_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWidt

    clock_gettime(CLOCK_MONOTONIC, &ts_start);

-    memcpy(outBuf, tmp, 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;
@@ -666,15 +704,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;    /* yuv rgb 相关分量 */
-    unsigned char *YUVdata, *RGBdata;    /* YUV和RGB数据指针 */
-    int Ypos, Upos, Vpos;    /* Y U V在数据缓存中的偏移 */
-    unsigned int i = 0;
-    unsigned short bar = 0;
-    unsigned char *tmp = malloc(screensize);
+    int rows ,cols;
+    int y, u, v, r, g, b;
+    uint8_t *YUVdata, *RGBdata;
+    int Ypos;
+    uint8_t *tmp = malloc(g_screen_size);

    YUVdata = inBuf;
    RGBdata = tmp;
@@ -697,11 +733,11 @@ int convert_yuv444_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
            g = g > 255 ? 255 : (g < 0 ? 0 : g);
            b = b > 255 ? 255 : (b < 0 ? 0 : b);

-            /* 从低到高r g b */
-            if (vinfo.bits_per_pixel == 16) {   // RGB565
-                *(RGBdata ++) = (((g & 0x1c) << 3) | (b >> 3));    /* g低5位，b高5位 */
-                *(RGBdata ++) = ((r & 0xf8) | (g >> 5));    /* r高5位，g高3位 */
-            } else if (vinfo.bits_per_pixel == 24) {   // RGB888
+            /* low -> high r g b */
+            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_screen_bpp == 24) {   // RGB888
                *(RGBdata ++) = b;
                *(RGBdata ++) = g;
                *(RGBdata ++) = r;
@@ -715,30 +751,31 @@ int convert_yuv444_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
        }
    }

-    memcpy(outBuf, tmp, 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;    /* YUV和RGB数据指针 */
+    int rows ,cols;
+    const uint8_t *RGB565data;
+    uint8_t *RGBdata;
    int RGBpos;
    int width, height;
    int x_offset, y_offset;
-    unsigned char *tmp = malloc(screensize);
+    uint8_t *tmp = malloc(g_screen_size);

-    width = imgWidth > vinfo.xres ? vinfo.xres : imgWidth;
-    height = imgHeight > vinfo.yres ? vinfo.yres : imgHeight;
-    x_offset = (vinfo.xres - width) / 2;
-    y_offset = (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 == vinfo.xres) {
-        RGBpos = (y_offset * 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);
@@ -749,9 +786,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) * vinfo.xres + x_offset) * vinfo.bits_per_pixel / 8;
+        RGBdata = tmp + ((rows + y_offset) * g_screen_width + x_offset) * g_screen_bpp / 8;
        RGBpos = rows * imgWidth * 2;
-        if (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++)
@@ -759,7 +796,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 (vinfo.bits_per_pixel == 32) {   // RGB888
+                if (g_screen_bpp == 32) {   // RGB888
                    *(RGBdata ++) = 0xFF;
                }
                RGBpos += 2;
@@ -767,25 +804,26 @@ int convert_rgb565_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
        }
    }

-    memcpy(outBuf, tmp, 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;    /* YUV和RGB数据指针 */
+    int rows ,cols;
+    const uint8_t *RGB888data;
+    uint8_t *RGBdata;
    int RGBpos;
    int width, height;
    int x_offset, y_offset;
-    unsigned char *tmp = malloc(screensize);
-    unsigned char r, g, b;
+    uint8_t *tmp = malloc(g_screen_size);
+    uint8_t r, g, b;

-    width = imgWidth > vinfo.xres ? vinfo.xres : imgWidth;
-    height = imgHeight > vinfo.yres ? vinfo.yres : imgHeight;
-    x_offset = (vinfo.xres - width) / 2;
-    y_offset = (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;
@@ -793,19 +831,19 @@ 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) * vinfo.xres + x_offset) * vinfo.bits_per_pixel / 8;
+        RGBdata = tmp + ((rows + y_offset) * g_screen_width + x_offset) * g_screen_bpp / 8;
        RGBpos = rows * imgWidth * 3;
-        if (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 (vinfo.bits_per_pixel == 16) {   // RGB565
+                if (g_screen_bpp == 16) {   // RGB565
                    b = RGB888data[RGBpos];
                    g = RGB888data[RGBpos + 1];
                    r = RGB888data[RGBpos + 2];
-                    *(RGBdata ++) = (((g & 0x1c) << 3) | (b >> 3));    /* g低5位，b高5位 */
-                    *(RGBdata ++) = ((r & 0xf8) | (g >> 5));    /* r高5位，g高3位 */
+                    *(RGBdata ++) = (((g & 0x1c) << 3) | (b >> 3));    /* g low 5bits，b high 5bits */
+                    *(RGBdata ++) = ((r & 0xf8) | (g >> 5));    /* r high 5bits，g high 3bits */
                } else {   // RGB8888
                    *(RGBdata ++) = RGB888data[RGBpos];
                    *(RGBdata ++) = RGB888data[RGBpos + 1];
@@ -817,7 +855,7 @@ int convert_rgb888_to_rgb(unsigned char *inBuf, unsigned char *outBuf, int imgWi
        }
    }

-    memcpy(outBuf, tmp, screensize);
+    memcpy(outBuf, tmp, g_screen_size);
    free(tmp);
    return 0;
 }
@@ -0,0 +1,23 @@
+
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef _CONVERT_H_
+#define _CONVERT_H_
+
+extern int yuyv_resize(const uint8_t *inBuf, uint8_t *outBuf, int imgWidth, int imgHeight);
+
+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_
@@ -0,0 +1,372 @@
+#!/bin/bash
+# SPDX-License-Identifier: GPL-2.0
+#
+# Copyright (C) 2021 StarFive Technology Co., Ltd.
+#
+
+USAGE="Usage:
+	  media-ctl-pipeline.sh -d devname -i interface_type -s sensor_type -a action
+	    devname:  Media device name (default select starfive media device)
+	    interface_type One of the following:
+			   dvp
+			   csiphy0
+			   csiphy1
+
+	    sensor_type One of the following:
+			VIN
+			ISP0
+			ISP1
+			ISP0RAW
+			ISP1RAW
+
+	    action One of the following:
+		   start
+		   stop
+
+            example:
+		start pipeline:
+			media-ctl-pipeline.sh -d /dev/media0 -i dvp -s ISP0 -a start
+
+		stop pipeline:
+			media-ctl-pipeline.sh -d /dev/media0 -i dvp -s ISP0 -a start
+"
+
+devname="/dev/media0"
+index=0
+while [ $index -le 10 ]
+do
+	dev="/dev/media""$index"
+	drivername=$(media-ctl -d $dev -p | grep driver | grep stf-vin)
+	if [ -n "$drivername" ]; then
+		devname=$dev
+		echo "found starfive media device: $devname"
+		break
+	fi
+	let index++
+done
+
+while getopts "d:i:s:a:" arg
+do
+	case $arg in
+		d)
+			devname=$OPTARG
+			echo "select dev name: $devname"
+			;;
+		i)
+			interface_type=$OPTARG
+			echo "select interface_type: $interface_type"
+			;;
+		s)
+			sensor_type=$OPTARG
+			echo "select sensor_type: $sensor_type"
+			;;
+		a)
+			action=$OPTARG
+			echo "select action: $action"
+			;;
+		?)
+			echo "unknow argument"
+			exit 1
+			;;
+	esac
+done
+
+if [ -z "$devname" ]; then
+	echo "unknow media device"
+	echo "$USAGE"
+	exit 1
+fi
+
+if [ -z "$interface_type" ]; then
+	echo "unknow interface type"
+	echo "$USAGE"
+	exit 1
+fi
+
+if [ -z "$sensor_type" ]; then
+	echo "unknow sensor type"
+	echo "$USAGE"
+	exit 1
+fi
+
+if [ -z "$action" ]; then
+	echo "unknow action type"
+	echo "$USAGE"
+	exit 1
+fi
+
+echo "media-ctl-pipeline.sh -d $devname -i $interface_type -s $sensor_type -a $action"
+
+case $interface_type in
+	dvp)
+		case $action in
+			start)
+				# media-ctl -d "$devname" -vl "'sc2235 1-0030':0 -> 'stf_dvp0':0 [1]"
+				# media-ctl -d "$devname" -vl "'ov5640 1-003c':0 -> 'stf_dvp0':0 [1]"
+				case $sensor_type in
+					VIN)
+						echo "DVP vin enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_vin0_wr':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_wr':1 -> 'stf_vin0_wr_video0':0 [1]"
+						;;
+					ISP0)
+						echo "DVP ISP0 enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_isp0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
+						;;
+					ISP0RAW)
+						echo "DVP ISP0RAW enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_isp0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
+						;;
+					ISP1)
+						echo "DVP ISP1 enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_isp1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
+						;;
+					ISP1RAW)
+						echo "DVP ISP1RAW enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_isp1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
+						;;
+					*)
+						echo "$USAGE"
+						exit 1
+						;;
+				esac
+				;;
+			stop)
+				# media-ctl -d "$devname" -vl "'sc2235 1-0030':0 -> 'stf_dvp0':0 [0]"
+				# media-ctl -d "$devname" -vl "'ov5640 1-003c':0 -> 'stf_dvp0':0 [0]"
+				case $sensor_type in
+					VIN)
+						echo "DVP vin disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_vin0_wr':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_wr':1 -> 'stf_vin0_wr_video0':0 [0]"
+						;;
+					ISP0)
+						echo "DVP ISP0 disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_isp0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
+						;;
+					ISP0RAW)
+						echo "DVP ISP0RAW disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_isp0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
+						;;
+					ISP1)
+						echo "DVP ISP1 disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_isp1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
+						;;
+					ISP1RAW)
+						echo "DVP ISP1RAW disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_dvp0':1 -> 'stf_isp1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
+						;;
+					*)
+						echo "$USAGE"
+						exit 1
+						;;
+				esac
+				;;
+			*)
+				echo "$USAGE"
+				exit 1
+				;;
+		esac
+		;;
+	csiphy0)
+		case $action in
+			start)
+				# media-ctl -d "$devname" -vl "'ov4689 0-0036':0 -> 'stf_csiphy0':0 [1]"
+				case $sensor_type in
+					VIN)
+						echo "csiphy0 CSIRX0 vin enable pipeline:"
+						;;
+					ISP0)
+						echo "csiphy0 CSIRX0 ISP0 enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_csi0':1 -> 'stf_isp0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
+						;;
+					ISP0RAW)
+						echo "csiphy0 CSIRX0 ISP0RAW enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_csi0':1 -> 'stf_isp0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
+						;;
+					ISP1)
+						echo "csiphy0 CSIRX0 ISP1 enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_csi0':1 -> 'stf_isp1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
+						;;
+					ISP1RAW)
+						echo "csiphy0 CSIRX0 ISP1RAW enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_csi0':1 -> 'stf_isp1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
+						;;
+
+					*)
+						echo "$USAGE"
+						exit 1
+						;;
+				esac
+				;;
+			stop)
+				# media-ctl -d "$devname" -vl "'ov4689 0-0036':0 -> 'stf_csiphy0':0 [0]"
+				case $sensor_type in
+					VIN)
+						echo "csiphy0 CSIRX0 vin disable pipeline:"
+						;;
+					ISP0)
+						echo "csiphy0 CSIRX0 ISP0 disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_csi0':1 -> 'stf_isp0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
+						;;
+					ISP0RAW)
+						echo "csiphy0 CSIRX0 ISP0RAW disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_csi0':1 -> 'stf_isp0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
+						;;
+					ISP1)
+						echo "csiphy0 CSIRX0 ISP1 disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_csi0':1 -> 'stf_isp1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
+						;;
+					ISP1RAW)
+						echo "csiphy0 CSIRX0 ISP1RAW disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_csi0':1 -> 'stf_isp1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
+						;;
+
+					*)
+						echo "$USAGE"
+						exit 1
+						;;
+				esac
+				;;
+			*)
+				echo "$USAGE"
+				exit 1
+				;;
+		esac
+		;;
+	csiphy1)
+		case $action in
+			start)
+				# media-ctl -d "$devname" -vl "'ov4689 2-0036':0 -> 'stf_csiphy1':0 [1]"
+				case $sensor_type in
+					VIN)
+						echo "csiphy1 CSIRX0 vin enable pipeline:"
+						;;
+					ISP0)
+						echo "csiphy1 CSIRX1 ISP0 enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_csi1':1 -> 'stf_isp0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
+						;;
+					ISP0RAW)
+						echo "csiphy1 CSIRX1 ISP0RAW enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_csi1':1 -> 'stf_isp0':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
+						;;
+					ISP1)
+						echo "csiphy1 CSIRX1 ISP1 enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_csi1':1 -> 'stf_isp1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
+						;;
+					ISP1RAW)
+						echo "csiphy1 CSIRX1 ISP1RAW enable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_csi1':1 -> 'stf_isp1':0 [1]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [1]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
+						;;
+
+					*)
+						echo "$USAGE"
+						exit 1
+						;;
+				esac
+				;;
+			stop)
+				# media-ctl -d "$devname" -vl "'ov4689 0-0036':0 -> 'stf_csiphy0':0 [0]"
+				case $sensor_type in
+					VIN)
+						echo "csiphy1 CSIRX0 vin disable pipeline:"
+						;;
+					ISP0)
+						echo "csiphy1 CSIRX1 ISP0 disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_csi1':1 -> 'stf_isp0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
+						;;
+					ISP0RAW)
+						echo "csiphy1 CSIRX1 ISP0RAW disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_csi1':1 -> 'stf_isp0':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
+						;;
+					ISP1)
+						echo "csiphy1 CSIRX1 ISP1 disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_csi1':1 -> 'stf_isp1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
+						;;
+					ISP1RAW)
+						echo "csiphy1 CSIRX1 ISP1RAW disable pipeline:"
+						media-ctl -d "$devname" -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_csi1':1 -> 'stf_isp1':0 [0]"
+						media-ctl -d "$devname" -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [0]"
+						# media-ctl -d "$devname" -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
+						;;
+					*)
+						echo "$USAGE"
+						exit 1
+						;;
+				esac
+				;;
+			*)
+				echo "$USAGE"
+				exit 1
+				;;
+		esac
+		;;
+	*)
+		echo "$USAGE"
+		exit 1
+		;;
+esac
+
+exit 0;
@@ -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);
+}
@@ -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__
@@ -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");
+}
+
+
@@ -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__
@@ -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);
+    }
+}
+
@@ -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__
@@ -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;
+}
@@ -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__
@@ -0,0 +1,919 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+
+#include <getopt.h>
+#include <signal.h>
+#include <inttypes.h>
+#include <stdbool.h>
+#include <libv4l2.h>
+#include <poll.h>
+#include "common.h"
+#include "yuv.h"
+#include "convert.h"
+#include "stf_v4l2.h"
+#include "stf_framebuffer.h"
+#include "stf_drm.h"
+#include "stf_log.h"
+
+#define FB_DEVICE_NAME       "/dev/fb0"
+#define STFBC_DEVICE_NAME    "/dev/stfbcdev"
+#define DRM_DEVICE_NAME      "/dev/dri/card0"
+#define V4L2_DFT_DEVICE_NAME "/dev/video0"
+
+typedef struct enum_value_t {
+  int    value;
+  const char *name;
+} enum_value_t;
+
+static const enum_value_t g_disp_values[] = {
+    { STF_DISP_NONE, "NONE"},
+    { STF_DISP_FB,   "FB"},
+    { STF_DISP_DRM,  "DRM"}
+};
+
+static const enum_value_t g_iomthd_values[] = {
+    { IO_METHOD_MMAP,    "MMAP"},
+    { IO_METHOD_USERPTR, "USERPTR"},
+    { IO_METHOD_DMABUF,  "DMABUF"},
+    { IO_METHOD_READ,    "READ"}
+};
+
+typedef struct {
+    V4l2Param_t v4l2_param;
+    FBParam_t fb_param;
+    DRMParam_t drm_param;
+
+    enum STF_DISP_TYPE disp_type;
+    enum IOMethod    io_mthd;
+    int continuous;
+
+    uint8_t jpegQuality;
+    char* jpegFilename;
+    FILE *rec_fp;
+
+    int dmabufs[BUFCOUNT];  // for dmabuf use, mmap not use it
+} ConfigParam_t;
+ConfigParam_t *gp_cfg_param = NULL;
+
+static int g_drm_buf_next_idx = -1;
+static int g_drm_buf_curr_idx = 0;
+
+static void alloc_default_config(ConfigParam_t **pp_data)
+{
+    ConfigParam_t *cfg_param = NULL;
+    cfg_param = malloc(sizeof(*cfg_param));
+    if (!cfg_param) {
+        errno_exit("malloc");
+    }
+    memset(cfg_param, 0, sizeof(*cfg_param));
+
+    cfg_param->disp_type = STF_DISP_NONE;
+    cfg_param->continuous = 0;
+    cfg_param->jpegQuality = 70;
+    cfg_param->io_mthd = IO_METHOD_MMAP;
+
+    cfg_param->v4l2_param.device_name = V4L2_DFT_DEVICE_NAME;
+    cfg_param->v4l2_param.fd = -1;
+    cfg_param->v4l2_param.io_mthd = cfg_param->io_mthd;
+    cfg_param->v4l2_param.width = 1920;
+    cfg_param->v4l2_param.height = 1080;
+    cfg_param->v4l2_param.image_size = cfg_param->v4l2_param.width *
+                cfg_param->v4l2_param.height * 3 / 2;
+    cfg_param->v4l2_param.format = V4L2_PIX_FMT_NV12; // V4L2_PIX_FMT_RGB565
+    cfg_param->v4l2_param.fps = 30;
+
+    // the fb param will be updated after fb init
+    cfg_param->fb_param.fd = -1;
+    cfg_param->fb_param.pixformat = COLOR_YUV420_NV21; // COLOR_RGB565
+    cfg_param->fb_param.width = 1920;
+    cfg_param->fb_param.height = 1080;
+    cfg_param->fb_param.bpp = 16;
+    cfg_param->fb_param.screen_size = cfg_param->fb_param.width *
+                cfg_param->fb_param.height * cfg_param->fb_param.bpp / 8;
+
+    *pp_data = cfg_param;
+}
+
+static void check_cfg_params(ConfigParam_t *cfg_param)
+{
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    int disp_type = cfg_param->disp_type;
+    int io_mthd = cfg_param->io_mthd;
+    int ret = EXIT_FAILURE;
+
+    assert(disp_type >= STF_DISP_NONE && disp_type <= STF_DISP_DRM);
+    assert(io_mthd >= IO_METHOD_MMAP && io_mthd <= IO_METHOD_READ);
+
+    // when mmap, support display NONE, DRM, FB
+    if (IO_METHOD_MMAP == io_mthd) {
+        ret = EXIT_SUCCESS;
+    }
+
+    // when dmabuf, only support DRM, and not save file
+    if (IO_METHOD_DMABUF == io_mthd
+            && STF_DISP_DRM == disp_type
+            && !cfg_param->jpegFilename) {
+        ret = EXIT_SUCCESS;
+    }
+
+    if (EXIT_FAILURE == ret) {
+        LOG(STF_LEVEL_ERR, "Not support: io method is %s, display type is %s\n",
+                g_iomthd_values[io_mthd].name, g_disp_values[disp_type].name);
+        exit(EXIT_FAILURE);
+    }
+
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+/**
+SIGINT interput handler
+*/
+void StopContCapture(int sig_id) {
+    LOG(STF_LEVEL_INFO, "stoping continuous capture\n");
+    gp_cfg_param->continuous = 0;
+}
+
+void InstallSIGINTHandler() {
+    struct sigaction sa;
+    CLEAR(sa);
+
+    sa.sa_handler = StopContCapture;
+    if (sigaction(SIGINT, &sa, 0) != 0) {
+        LOG(STF_LEVEL_ERR, "could not install SIGINT handler, continuous capture disabled\n");
+        gp_cfg_param->continuous = 0;
+    }
+}
+
+/**
+    process image read, recommand NV21 or NV12
+    TODO: use ffmpeg or opencv to convert the image format
+*/
+static void imageProcess(const uint8_t* inbuf, uint8_t* outbuf,
+        struct timeval timestamp)
+{
+    //timestamp.tv_sec
+    //timestamp.tv_usec
+    int in_width = gp_cfg_param->v4l2_param.width;
+    int in_height = gp_cfg_param->v4l2_param.height;
+    int in_imagesize = gp_cfg_param->v4l2_param.image_size;
+    uint32_t in_format = gp_cfg_param->v4l2_param.format;
+    char* jpegFilename = gp_cfg_param->jpegFilename;
+    int disp_type = gp_cfg_param->disp_type;
+    int new_in_format = 0;
+    int out_format = 0;
+    int out_size = 0;
+    uint8_t* dst = malloc(in_width * in_height * 3);
+    int is_yuv420sp = 0;  // NOTE: NV21 or NV12, it is special for starfive framebuffer
+    static int s_frmcnt = 0;
+
+    if (STF_DISP_FB == disp_type) {
+        new_in_format = v4l2fmt_to_fbfmt(in_format);
+        out_format = gp_cfg_param->fb_param.pixformat;
+        out_size = gp_cfg_param->fb_param.screen_size;
+        is_yuv420sp = gp_cfg_param->fb_param.vinfo.grayscale;
+    } else if (STF_DISP_DRM == disp_type) {
+        new_in_format = v4l2fmt_to_drmfmt(in_format);
+        out_format = gp_cfg_param->drm_param.dev_head->drm_format;
+        out_size = gp_cfg_param->drm_param.dev_head->bufs[0].size;
+    }
+
+    LOG(STF_LEVEL_LOG, "in_width=%d, in_height=%d, in_imagesize=%d, out_size=%d, p=%p\n",
+            in_width, in_height, in_imagesize, out_size, inbuf);
+
+    // write jpeg
+    char filename[512];
+    switch (in_format) {
+        case V4L2_PIX_FMT_YUV420:
+            // if (jpegFilename) {
+            //     // sprintf(filename, "%d-yuv420-%s", s_frmcnt, jpegFilename);
+            //     // YUV420toYUV444(in_width, in_height, inbuf, dst);
+            //     // jpegWrite(dst, filename);
+            //     sprintf(filename, "raw-%d-yuv420-%s", s_frmcnt, jpegFilename);
+            //     write_file(filename, inbuf, in_imagesize);
+            //     s_frmcnt++;
+            // }
+            if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
+                fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
+            }
+            break;
+        case V4L2_PIX_FMT_YUYV:
+        case V4L2_PIX_FMT_YVYU:
+            // if (jpegFilename) {
+            //     // sprintf(filename, "%d-yuv422-%s", s_frmcnt, jpegFilename);
+            //     // YUV422toYUV444(in_width, in_height, inbuf, dst);
+            //     // jpegWrite(dst, filename);
+            //     sprintf(filename, "raw-%d-yuv422-%s", s_frmcnt, jpegFilename);
+            //     write_file(filename, inbuf, in_imagesize);
+            //     s_frmcnt++;
+            // }
+            if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
+                fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
+            }
+            if (out_format == new_in_format) {
+                yuyv_resize(inbuf, outbuf, in_width, in_height);
+            } else if ((STF_DISP_FB == disp_type) && is_yuv420sp) {
+                convert_yuyv_to_nv12(inbuf, outbuf, in_width, in_height, 1);
+            } else if ((STF_DISP_DRM == disp_type) && (out_format == V4L2_PIX_FMT_NV12)) {
+                convert_yuyv_to_nv12(inbuf, outbuf, in_width, in_height, 1);
+            } else {
+                convert_yuyv_to_rgb(inbuf, outbuf, in_width, in_height, 0);
+            }
+            break;
+        case V4L2_PIX_FMT_NV21:
+            // if (gp_cfg_param->jpegFilename) {
+            //     // sprintf(filename, "%d-nv21-%s", s_frmcnt, gp_cfg_param->jpegFilename);
+            //     // YUV420NV21toYUV444(in_width, in_height, inbuf, dst, 0);
+            //     // jpegWrite(dst, filename);
+            //     sprintf(filename, "raw-nv21-%s",  gp_cfg_param->jpegFilename);
+            //     write_file(filename, inbuf, in_imagesize);
+            //     s_frmcnt++;
+            // }
+            if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
+                fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
+            }
+            LOG(STF_LEVEL_LOG, "out_format=%d, new_in_format=%d, is_yuv420sp=%d\n", out_format,
+                    new_in_format, is_yuv420sp);
+            if (outbuf) {
+                if (out_format == new_in_format) {
+                    convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 0);
+                } else if ((STF_DISP_FB == disp_type) && is_yuv420sp) {
+                    convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 1);
+                } else {
+                    convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 1);
+                }
+            }
+            break;
+        case V4L2_PIX_FMT_NV12:
+            // if (jpegFilename) {
+            //     // sprintf(filename, "%d-nv12-%s", s_frmcnt, jpegFilename);
+            //     // YUV420NV12toYUV444(in_width, in_height, inbuf, dst);
+            //     // jpegWrite(dst, filename);
+            //     sprintf(filename, "raw-%d-nv12-%s", s_frmcnt, jpegFilename);
+            //     write_file(filename, inbuf, in_imagesize);
+            //     s_frmcnt++;
+            // }
+            if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
+                fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
+            }
+            LOG(STF_LEVEL_DEBUG, "out_format=%d, new_in_format=%d, is_yuv420sp=%d\n", out_format,
+                    new_in_format, is_yuv420sp);
+            if (out_format == new_in_format) {
+                convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 0);
+            } else if ((STF_DISP_FB == disp_type) && is_yuv420sp) {
+                convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 1);
+            } else {
+                convert_nv21_to_rgb(inbuf, outbuf, in_width, in_height, 0);
+            }
+            break;
+        case V4L2_PIX_FMT_RGB24:
+            // if (jpegFilename) {
+            //     // sprintf(filename, "%d-rgb-%s", s_frmcnt, jpegFilename);
+            //     // RGB565toRGB888(in_width, in_height, inbuf, dst);
+            //     // write_JPEG_file(filename, inbuf, in_width, in_height, jpegQuality);
+            //     sprintf(filename, "raw-%d-rgb-%s", s_frmcnt, jpegFilename);
+            //     write_file(filename, inbuf, in_imagesize);
+            //     s_frmcnt++;
+            // }
+            if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
+                fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
+            }
+            convert_rgb888_to_rgb(inbuf, outbuf, in_width, in_height, 0);
+            break;
+        case V4L2_PIX_FMT_RGB565:
+            // if (jpegFilename) {
+            //     // sprintf(filename, "%d-rgb565-%s", s_frmcnt, jpegFilename);
+            //     // RGB565toRGB888(in_width, in_height, inbuf, dst);
+            //     // write_JPEG_file(filename, dst, in_width, in_height, jpegQuality);
+            //     sprintf(filename, "raw-%d-rgb565-%s", s_frmcnt, jpegFilename);
+            //     write_file(filename, inbuf, in_imagesize);
+            //     s_frmcnt++;
+            // }
+            if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
+                fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
+            }
+            if (out_format == new_in_format)
+                convert_rgb565_to_rgb(inbuf, outbuf, in_width, in_height, 0);
+            else if ((STF_DISP_FB == disp_type) && is_yuv420sp)
+                convert_rgb565_to_nv12(inbuf, outbuf, in_width, in_height, 0);
+            else
+                convert_rgb565_to_rgb(inbuf, outbuf, in_width, in_height, 0);
+            break;
+        case V4L2_PIX_FMT_SRGGB12:
+            if (jpegFilename)
+                sprintf(filename, "raw-%d-RGGB12-%s", s_frmcnt, jpegFilename);
+            else
+                sprintf(filename, "raw-%d-RGGB12.raw", s_frmcnt);
+            write_file(filename, inbuf, in_imagesize);
+            RAW12toRAW16(in_width, in_height, inbuf, dst);
+            if (jpegFilename)
+                sprintf(filename, "raw-%d-RGGB16-%s", s_frmcnt, jpegFilename);
+            else
+                sprintf(filename, "raw-%d-RGGB16.raw", s_frmcnt);
+            write_file(filename, (const uint8_t *)dst, in_width * in_height * 2);
+            s_frmcnt++;
+            break;
+        case V4L2_PIX_FMT_SGRBG12:
+            if (jpegFilename)
+                sprintf(filename, "raw-%d-GRBG12-%s", s_frmcnt, jpegFilename);
+            else
+                sprintf(filename, "raw-%d-GRBG12.raw", s_frmcnt);
+            write_file(filename, inbuf, in_imagesize);
+            RAW12toRAW16(in_width, in_height, inbuf, dst);
+            if (jpegFilename)
+                sprintf(filename, "raw-%d-GRBG16-%s", s_frmcnt, jpegFilename);
+            else
+                sprintf(filename, "raw-%d-GRBG16.raw", s_frmcnt);
+            write_file(filename, (const uint8_t *)dst, in_width * in_height * 2);
+            s_frmcnt++;
+            break;
+        case V4L2_PIX_FMT_SGBRG12:
+            if (jpegFilename)
+                sprintf(filename, "raw-%d-GBRG12-%s", s_frmcnt, jpegFilename);
+            else
+                sprintf(filename, "raw-%d-GBRG12.raw", s_frmcnt);
+            write_file(filename, inbuf, in_imagesize);
+            RAW12toRAW16(in_width, in_height, inbuf, dst);
+            if (jpegFilename)
+                sprintf(filename, "raw-%d-GBRG16-%s", s_frmcnt, jpegFilename);
+            else
+                sprintf(filename, "raw-%d-GBRG16.raw", s_frmcnt);
+            write_file(filename, (const uint8_t *)dst, in_width * in_height * 2);
+            s_frmcnt++;
+            break;
+        case V4L2_PIX_FMT_SBGGR12:
+            if (jpegFilename)
+                sprintf(filename, "raw-%d-BGGR12-%s", s_frmcnt, jpegFilename);
+            else
+                sprintf(filename, "raw-%d-BGGR12.raw", s_frmcnt);
+            write_file(filename, inbuf, in_imagesize);
+            RAW12toRAW16(in_width, in_height, inbuf, dst);
+            if (jpegFilename)
+                sprintf(filename, "raw-%d-BGGR16-%s", s_frmcnt, jpegFilename);
+            else
+                sprintf(filename, "raw-%d-BGGR16.raw", s_frmcnt);
+            write_file(filename, (const uint8_t *)dst, in_width * in_height * 2);
+            s_frmcnt++;
+            break;
+        default:
+            LOG(STF_LEVEL_ERR, "unknow in_format\n");
+            break;
+    }
+
+    // free temporary image
+    free(dst);
+}
+
+void calc_frame_fps()
+{
+    static uint32_t frm_cnt = 0;
+    static struct timespec ts_old;
+    struct timespec ts;
+    uint32_t fps = 0;
+    uint32_t diff_ms = 0;
+
+    if (frm_cnt == 0) {
+        clock_gettime(CLOCK_MONOTONIC, &ts_old);
+    }
+    if (frm_cnt++ >= 50) {
+        clock_gettime(CLOCK_MONOTONIC, &ts);
+        diff_ms = (ts.tv_sec - ts_old.tv_sec) * 1000 + (ts.tv_nsec - ts_old.tv_nsec) / 1000000;
+        fps = 1000  * (frm_cnt - 1) / diff_ms;
+        frm_cnt = 0;
+        LOG(STF_LEVEL_INFO, "pipeline display fps=%d\n", fps);
+    }
+}
+
+/**
+    read single frame
+*/
+static int frameRead(void)
+{
+    struct v4l2_buffer buf;
+    V4l2Param_t *pv4l2_param = &gp_cfg_param->v4l2_param;
+    uint8_t *dst = NULL;
+
+    if (STF_DISP_FB == gp_cfg_param->disp_type) {
+        dst = gp_cfg_param->fb_param.screen_buf;
+    } else if (STF_DISP_DRM == gp_cfg_param->disp_type &&
+            IO_METHOD_DMABUF != gp_cfg_param->io_mthd) {
+        dst = gp_cfg_param->drm_param.dev_head->bufs[0].buf;
+    } else {
+        LOG(STF_LEVEL_LOG, "Not display\n");
+    }
+
+    switch (pv4l2_param->io_mthd) {
+    case IO_METHOD_READ:
+        if (-1 == v4l2_read(pv4l2_param->fd, pv4l2_param->pBuffers[0].start,
+                pv4l2_param->pBuffers[0].length)) {
+            switch (errno) {
+            case EAGAIN:
+                return 0;
+            case EIO:
+                // Could ignore EIO, see spec.
+                // fall through
+            default:
+                errno_exit("read");
+            }
+        }
+        struct timespec ts;
+        struct timeval timestamp;
+        clock_gettime(CLOCK_MONOTONIC,&ts);
+        timestamp.tv_sec = ts.tv_sec;
+        timestamp.tv_usec = ts.tv_nsec/1000;
+        imageProcess((uint8_t *)(pv4l2_param->pBuffers[0].start), dst, timestamp);
+        break;
+
+    case IO_METHOD_MMAP:
+        stf_v4l2_dequeue_buffer(pv4l2_param, &buf);
+        LOG(STF_LEVEL_LOG, "buf.index=%d, n_buffers=%d\n",
+                buf.index, gp_cfg_param->v4l2_param.n_buffers);
+        imageProcess((uint8_t *)(pv4l2_param->pBuffers[buf.index].start), dst, buf.timestamp);
+        stf_v4l2_queue_buffer(pv4l2_param, buf.index);
+        if (STF_DISP_DRM == gp_cfg_param->disp_type) {
+            drmModePageFlip(gp_cfg_param->drm_param.fd, gp_cfg_param->drm_param.dev_head->crtc_id,
+                    gp_cfg_param->drm_param.dev_head->bufs[0].fb_id,
+                    DRM_MODE_PAGE_FLIP_EVENT, gp_cfg_param->drm_param.dev_head);
+        }
+        LOG(STF_LEVEL_LOG, "buf.index: %d, buf.bytesused=%d\n", buf.index, buf.bytesused);
+        break;
+
+    case IO_METHOD_USERPTR:
+        stf_v4l2_dequeue_buffer(pv4l2_param, &buf);
+        imageProcess((uint8_t *)(buf.m.userptr), dst, buf.timestamp);
+        stf_v4l2_queue_buffer(pv4l2_param, buf.index);
+        break;
+    case IO_METHOD_DMABUF:
+    default:
+        break;
+    }
+    return 1;
+}
+
+/**
+ *  mainloop_select: read frames with select() and process them
+ */
+static void mainloop_select(void)
+{
+    int count, i;
+    uint32_t numberOfTimeouts;
+
+    numberOfTimeouts = 0;
+    count = 3;
+
+    while (count-- > 0) {
+        for (i = 0; i < 1; i++) {
+            fd_set fds;
+            struct timeval tv;
+            int r;
+
+            FD_ZERO(&fds);
+            FD_SET(gp_cfg_param->v4l2_param.fd, &fds);
+
+            /* Timeout. */
+            tv.tv_sec = 1;
+            tv.tv_usec = 0;
+            r = select(gp_cfg_param->v4l2_param.fd + 1, &fds, NULL, NULL, &tv);
+            if (-1 == r) {
+                if (EINTR == errno) {
+                    continue;
+                }
+                errno_exit("select");
+            } else if (0 == r) {
+                if (numberOfTimeouts <= 0) {
+                    // count++;
+                } else {
+                    LOG(STF_LEVEL_ERR, "select timeout\n");
+                    exit(EXIT_FAILURE);
+                }
+            }
+            if (gp_cfg_param->continuous == 1) {
+                count = 3;
+            }
+
+            if (frameRead())
+                break;
+
+            /* EAGAIN - continue select loop. */
+        }
+    }
+}
+
+static void page_flip_handler(int fd, unsigned int frame,
+            unsigned int sec, unsigned int usec,
+            void *data)
+{
+    struct drm_dev_t *dev = data;
+
+    /* If we have a next buffer, then let's return the current one,
+        * and grab the next one.
+        */
+    if (g_drm_buf_next_idx > 0) {
+        stf_v4l2_queue_buffer(&gp_cfg_param->v4l2_param, g_drm_buf_curr_idx);
+        g_drm_buf_curr_idx = g_drm_buf_next_idx;
+        g_drm_buf_next_idx = -1;
+    }
+    drmModePageFlip(fd, dev->crtc_id, dev->bufs[g_drm_buf_curr_idx].fb_id,
+                    DRM_MODE_PAGE_FLIP_EVENT, dev);
+}
+
+static void mainloop()
+{
+    struct v4l2_buffer buf;
+    int r;
+    int count = 3;
+    drmEventContext ev;
+    struct pollfd* fds = NULL;
+    uint32_t nfds = 0;
+
+    LOG(STF_LEVEL_TRACE, "Enter\n");
+    if (STF_DISP_FB == gp_cfg_param->disp_type ||
+            IO_METHOD_MMAP == gp_cfg_param->io_mthd) {
+        // fb or (drm + mmap)
+        nfds = 1;
+        fds = (struct pollfd*)malloc(sizeof(struct pollfd) * nfds);
+        memset(fds, 0, sizeof(struct pollfd) * nfds);
+        fds[0].fd = gp_cfg_param->v4l2_param.fd;
+        fds[0].events = POLLIN;
+    } else if (STF_DISP_DRM == gp_cfg_param->disp_type &&
+            IO_METHOD_DMABUF == gp_cfg_param->io_mthd) {
+        // (drm + dmabuf)
+        nfds = 2;
+        fds = (struct pollfd*)malloc(sizeof(struct pollfd) * nfds);
+        memset(fds, 0, sizeof(struct pollfd) * nfds);
+        fds[0].fd = gp_cfg_param->v4l2_param.fd;
+        fds[0].events = POLLIN;
+        fds[1].fd = gp_cfg_param->drm_param.fd;
+        fds[1].events = POLLIN;
+
+        memset(&ev, 0, sizeof ev);
+        ev.version = DRM_EVENT_CONTEXT_VERSION;
+        ev.vblank_handler = NULL;
+        ev.page_flip_handler = page_flip_handler;
+    } else {
+        LOG(STF_LEVEL_ERR, "Display type %d and io method type %d not support\n",
+                gp_cfg_param->disp_type, gp_cfg_param->io_mthd);
+        exit(EXIT_FAILURE);
+    }
+
+    if (!gp_cfg_param->rec_fp && gp_cfg_param->jpegFilename) {
+        gp_cfg_param->rec_fp = fopen(gp_cfg_param->jpegFilename, "w+");
+        if (!gp_cfg_param->rec_fp) {
+            LOG(STF_LEVEL_ERR, "can't open %s\n", gp_cfg_param->jpegFilename);
+            exit(EXIT_FAILURE);
+        }
+    }
+
+    while (count-- > 0) {
+        r = poll(fds, nfds, 3000);
+        if (-1 == r) {
+            if (EINTR == errno) {
+                continue;
+            }
+            LOG(STF_LEVEL_ERR, "error in poll %d", errno);
+            break;
+        }
+        if (0 == r) {
+            LOG(STF_LEVEL_ERR, "poll timeout, %d\n", errno);
+            break;
+        }
+
+        if (STF_DISP_FB == gp_cfg_param->disp_type ||
+                IO_METHOD_MMAP == gp_cfg_param->io_mthd) {
+            // fb or (drm + mmap)
+            if (fds[0].revents & POLLIN) {
+                frameRead();
+                calc_frame_fps();
+            }
+        } else if (STF_DISP_DRM == gp_cfg_param->disp_type &&
+                IO_METHOD_DMABUF == gp_cfg_param->io_mthd) {
+            // drm + dmabuf
+            if (fds[0].revents & POLLIN) {
+                int dequeued = stf_v4l2_dequeue_buffer(&gp_cfg_param->v4l2_param, &buf);
+                if (dequeued) {
+                    g_drm_buf_next_idx = buf.index;
+                    frameRead(); // TODO: add support for save file later
+                    calc_frame_fps();
+                }
+            }
+            if (fds[1].revents & POLLIN) {
+                if (g_drm_buf_next_idx > 0) {
+                    drmHandleEvent(gp_cfg_param->drm_param.fd, &ev);
+                }
+            }
+        }
+
+        if (gp_cfg_param->continuous == 1) {
+            count = 3;
+        }
+
+        usleep(1 * 1000);
+    }
+
+    if (fds) {
+        free(fds);
+        fds = NULL;
+    }
+
+
+    if (gp_cfg_param->rec_fp) {
+        fclose(gp_cfg_param->rec_fp);
+        gp_cfg_param->rec_fp = NULL;
+    }
+
+
+    LOG(STF_LEVEL_TRACE, "Exit\n");
+}
+
+static void usage(FILE* fp, int argc, char** argv)
+{
+    fprintf(fp,
+        "Usage: %s [options]\n\n"
+        "Options:\n"
+        "-d | --device name   Video device name [default /dev/video0]\n"
+        "-h | --help          Print this message\n"
+        "-o | --output        Save raw data to filename\n"
+        //"-q | --quality       Set JPEG quality (0-100)\n"
+        "-m | --method        Set V4L2 videobuf2 memory type, default 0\n"
+        "                0: IO_METHOD_MMAP\n"
+        "                1: IO_METHOD_USERPTR\n"
+        "                2: IO_METHOD_DMABUF\n"
+        "                3: IO_METHOD_READ\n"
+        "-W | --width         Set v4l2 image width, default 1920\n"
+        "-H | --height        Set v4l2 image height, default 1080\n"
+        "-X | --left          Set v4l2 image crop x start\n"
+        "-Y | --up            Set v4l2 image crop y start\n"
+        "-R | --right         Set v4l2 image crop x width\n"
+        "-D | --down          Set v4l2 image crop y height\n"
+        "-I | --interval      Set frame interval (fps) (-1 to skip)\n"
+        "-c | --continuous    Do continous capture, stop with SIGINT.\n"
+        "-v | --version       Print version\n"
+        "-f | --format        image format, default 5\n"
+        "                0: V4L2_PIX_FMT_RGB565\n"
+        "                1: V4L2_PIX_FMT_RGB24\n"
+        "                2: V4L2_PIX_FMT_YUV420\n"
+        "                3: V4L2_PIX_FMT_YUYV\n"
+        "                4: V4L2_PIX_FMT_NV21\n"
+        "                5: V4L2_PIX_FMT_NV12\n"
+        "                6: V4L2_PIX_FMT_YVYU\n"
+        "                7: V4L2_PIX_FMT_SRGGB12\n"
+        "                8: V4L2_PIX_FMT_SGRBG12\n"
+        "                9: V4L2_PIX_FMT_SGBRG12\n"
+        "                10: V4L2_PIX_FMT_SBGGR12\n"
+        "                default: V4L2_PIX_FMT_NV12\n"
+        "-t | --distype       set display type, default 0\n"
+        "                0: Not display\n"
+        "                1: Use Framebuffer Display\n"
+        "                2: Use DRM Display\n"
+        "-l | --loadfw        load stfisp fw image\n"
+        "-s | --g_imagesize     print image size\n"
+        "\n"
+        "Eg:\n"
+        "\t drm: v4l2test -d /dev/video2 -f 5 -c -W 1920 -H 1080 -m 2 -t 2\n"
+        "\t fb:  v4l2test -d /dev/video2 -f 5 -c -W 1920 -H 1080 -m 0 -t 1\n"
+        "\n"
+        "Open debug log level: \n"
+        "\t export V4L2_DEBUG=3\n"
+        "\t default level 1, level range 0 ~ 7\n"
+        "",
+        argv[0]);
+}
+
+static const char short_options [] = "d:ho:q:m:W:H:I:vcf:t:X:Y:R:D:l:s";
+
+static const struct option long_options [] = {
+    { "device",     required_argument,      NULL,           'd' },
+    { "help",       no_argument,            NULL,           'h' },
+    { "output",     required_argument,      NULL,           'o' },
+    { "quality",    required_argument,      NULL,           'q' },
+    { "method",     required_argument,      NULL,           'm' },
+    { "width",      required_argument,      NULL,           'W' },
+    { "height",     required_argument,      NULL,           'H' },
+    { "left",       required_argument,      NULL,           'X' },
+    { "up",         required_argument,      NULL,           'Y' },
+    { "right",      required_argument,      NULL,           'R' },
+    { "down",       required_argument,      NULL,           'D' },
+    { "interval",   required_argument,      NULL,           'I' },
+    { "version",    no_argument,            NULL,           'v' },
+    { "continuous", no_argument,            NULL,           'c' },
+    { "format",     required_argument,      NULL,           'f' },
+    { "distype",    required_argument,      NULL,           't' },
+    { "loadfw",     required_argument,      NULL,           'l' },
+    { "g_imagesize",no_argument,            NULL,           's' },
+    { 0, 0, 0, 0 }
+};
+
+void parse_options(int argc, char **argv, ConfigParam_t *cfg_param)
+{
+    int index, c = 0;
+    int value = 0;
+
+    while ((c = getopt_long(argc, argv, short_options, long_options, &index)) != -1) {
+        switch (c) {
+        case 0: /* getopt_long() flag */
+            break;
+
+        case 'd':
+            cfg_param->v4l2_param.device_name = strdup(optarg);
+            break;
+
+        case 'h':
+            usage(stdout, argc, argv);
+            exit(EXIT_SUCCESS);
+
+        case 'o':
+            // set jpeg filename
+            cfg_param->jpegFilename = strdup(optarg);
+            break;
+
+        case 'q':
+            // set jpeg quality
+            cfg_param->jpegQuality = atoi(optarg);
+            break;
+
+        case 'm':
+            value = atoi(optarg);
+            if (value < IO_METHOD_MMAP || value > IO_METHOD_READ) {
+                LOG(STF_LEVEL_ERR, "io method %d is out of range [%d, %d]\n", value,
+                        IO_METHOD_MMAP, IO_METHOD_READ);
+                exit(EXIT_FAILURE);
+            }
+            LOG(STF_LEVEL_INFO, "io method: %s\n", g_iomthd_values[value].name);
+            cfg_param->io_mthd = value;
+            cfg_param->v4l2_param.io_mthd = cfg_param->io_mthd;
+            break;
+
+        case 'W':
+            // set v4l2 width
+            cfg_param->v4l2_param.width = atoi(optarg);
+            break;
+
+        case 'H':
+            // set v4l2 height
+            cfg_param->v4l2_param.height = atoi(optarg);
+            break;
+
+        case 'X':
+            // set x start
+            cfg_param->v4l2_param.crop_info.left = atoi(optarg);
+            cfg_param->v4l2_param.crop_flag = 1;
+            break;
+
+        case 'Y':
+            // set y start
+            cfg_param->v4l2_param.crop_info.top = atoi(optarg);
+            cfg_param->v4l2_param.crop_flag = 1;
+            break;
+
+        case 'R':
+            // set x width
+            cfg_param->v4l2_param.crop_info.width = atoi(optarg);
+            cfg_param->v4l2_param.crop_flag = 1;
+            break;
+
+        case 'D':
+            // set y height
+            cfg_param->v4l2_param.crop_info.height = atoi(optarg);
+            cfg_param->v4l2_param.crop_flag = 1;
+            break;
+
+        case 'I':
+            // set fps
+            cfg_param->v4l2_param.fps = atoi(optarg);
+            break;
+
+        case 'c':
+            // set flag for continuous capture, interuptible by sigint
+            cfg_param->continuous = 1;
+            InstallSIGINTHandler();
+            break;
+
+        case 'v':
+            printf("Version: %s\n", TEST_VERSION);
+            exit(EXIT_SUCCESS);
+            break;
+
+        case 'f':
+            LOG(STF_LEVEL_INFO, "v4l2 format: %s\n", optarg);
+            value = atoi(optarg);
+            LOG(STF_LEVEL_INFO, "v4l2 format: %d\n", value);
+            switch (value) {
+            case  0:
+                value = V4L2_PIX_FMT_RGB565;
+                break;
+            case  1:
+                value = V4L2_PIX_FMT_RGB24;
+                break;
+            case  2:
+                value = V4L2_PIX_FMT_YUV420;
+                break;
+            case  3:
+                value = V4L2_PIX_FMT_YUYV;
+                break;
+            case  4:
+                value = V4L2_PIX_FMT_NV21;
+                break;
+            case  5:
+                value = V4L2_PIX_FMT_NV12;
+                break;
+            case  6:
+                value = V4L2_PIX_FMT_YVYU;
+                break;
+            case  7:
+                value = V4L2_PIX_FMT_SRGGB12;
+                break;
+            case  8:
+                value = V4L2_PIX_FMT_SGRBG12;
+                break;
+            case  9:
+                value = V4L2_PIX_FMT_SGBRG12;
+                break;
+            case  10:
+                value = V4L2_PIX_FMT_SBGGR12;
+                break;
+            default:
+                value = V4L2_PIX_FMT_RGB565;
+                break;
+            }
+            cfg_param->v4l2_param.format = value;
+            break;
+
+        case 't':
+            value = atoi(optarg);
+            if (value < STF_DISP_NONE || value > STF_DISP_DRM) {
+                LOG(STF_LEVEL_ERR, "Display Type %d is out of range [%d, %d]\n", value,
+                        STF_DISP_NONE, STF_DISP_DRM);
+                exit(EXIT_FAILURE);
+            }
+            LOG(STF_LEVEL_INFO, "Display Type: %s\n", g_disp_values[value].name);
+            cfg_param->disp_type = value;
+            break;
+
+        case 'l':
+            loadfw_start(optarg, &(cfg_param->v4l2_param));
+            exit(EXIT_SUCCESS);
+            break;
+        case 's':
+            sensor_image_size_info(&(cfg_param->v4l2_param));
+            exit(EXIT_SUCCESS);
+            break;
+        default:
+            usage(stderr, argc, argv);
+            exit(EXIT_FAILURE);
+        }
+    }
+}
+
+int main(int argc, char **argv)
+{
+    init_log();
+    alloc_default_config(&gp_cfg_param);
+    parse_options(argc, argv, gp_cfg_param);
+    check_cfg_params(gp_cfg_param);
+
+    // open and initialize v4l2 device
+    stf_v4l2_open(&gp_cfg_param->v4l2_param, gp_cfg_param->v4l2_param.device_name);
+    stf_v4l2_init(&gp_cfg_param->v4l2_param);
+
+    if (STF_DISP_FB == gp_cfg_param->disp_type) {
+        stf_fb_open(&gp_cfg_param->fb_param, FB_DEVICE_NAME, STFBC_DEVICE_NAME);
+        stf_fb_init(&gp_cfg_param->fb_param, gp_cfg_param->v4l2_param.format);
+        update_videocvt_param(gp_cfg_param->disp_type, gp_cfg_param->fb_param.width,
+                gp_cfg_param->fb_param.height, gp_cfg_param->fb_param.bpp,
+                gp_cfg_param->fb_param.screen_size);
+
+    } else if (STF_DISP_DRM == gp_cfg_param->disp_type) {
+        stf_drm_open(&gp_cfg_param->drm_param, DRM_DEVICE_NAME, gp_cfg_param->io_mthd);
+        stf_drm_init(&gp_cfg_param->drm_param, gp_cfg_param->v4l2_param.width,
+                gp_cfg_param->v4l2_param.height, gp_cfg_param->v4l2_param.format,
+                gp_cfg_param->io_mthd, gp_cfg_param->dmabufs,
+                sizeof(gp_cfg_param->dmabufs) / sizeof(gp_cfg_param->dmabufs[0]));
+        update_videocvt_param(gp_cfg_param->disp_type, gp_cfg_param->drm_param.dev_head->width,
+                gp_cfg_param->drm_param.dev_head->height, 32,
+                gp_cfg_param->drm_param.dev_head->bufs[0].size);
+
+    }
+
+    // prepare and start v4l2 capturing
+    sft_v4l2_prepare_capturing(&gp_cfg_param->v4l2_param, gp_cfg_param->dmabufs, BUFCOUNT);
+    sft_v4l2_start_capturing(&(gp_cfg_param->v4l2_param));
+
+    // process frames
+    mainloop();
+
+    stf_v4l2_stop_capturing(&gp_cfg_param->v4l2_param);
+    stf_v4l2_uninit(&gp_cfg_param->v4l2_param);
+    stf_v4l2_close(&gp_cfg_param->v4l2_param);
+
+    if (STF_DISP_FB == gp_cfg_param->disp_type) {
+        stf_fb_uninit(&gp_cfg_param->fb_param);
+        stf_fb_close(&gp_cfg_param->fb_param);
+    } else if (STF_DISP_DRM == gp_cfg_param->disp_type) {
+        stf_drm_close(&gp_cfg_param->drm_param);
+    }
+
+    deinit_log();
+    free(gp_cfg_param);
+    return 0;
+}
@@ -0,0 +1,39 @@
+################################################################################
+#
+# v4l2test
+#
+################################################################################
+
+V4L2_TEST_LICENSE = GPL-2.0+
+
+define V4L2_TEST_BUILD_CMDS
+	cp package/starfive/v4l2_test/v4l2_test.c $(@D)/
+	cp package/starfive/v4l2_test/yuv.c $(@D)/
+	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/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/media-ctl-pipeline.sh $(@D)/
+	(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
+	install -m 0755 -D $(@D)/v4l2test $(TARGET_DIR)/usr/bin/v4l2test
+	install -m 0755 -D $(@D)/media-ctl-pipeline.sh $(TARGET_DIR)/usr/bin/media-ctl-pipeline.sh
+endef
+
+V4L2_TEST_DEPENDENCIES = jpeg libv4l
+$(eval $(generic-package))
+
@@ -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;
@@ -83,8 +89,8 @@ void YUV422toYUV444(int width, int height, unsigned char* src, unsigned char* ds
 	// In this format each four bytes is two pixels. Each four bytes is two Y's, a Cb and a Cr.
 	// Each Y goes to one of the pixels, and the Cb and Cr belong to both pixels.
 	unsigned char *base_py = src;
-	unsigned char *base_pu = src+(height*width);
-	unsigned char *base_pv = src+(height*width)+(height*width)/2;
+	//unsigned char *base_pu = src+(height*width);
+	//unsigned char *base_pv = src+(height*width)+(height*width)/2;

 	for (line = 0; line < height; ++line) {
 		for (column = 0; column < width; ++column) {
@@ -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
@@ -1,16 +0,0 @@
-
-#ifndef _CONVERT_H_
-#define _CONVERT_H_
-
-extern int yuyv_resize(unsigned char *inBuf, unsigned char *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);
-
-#endif // _CONVERT_H_
@@ -1,283 +0,0 @@
-#!/bin/sh
-
-USAGE="Usage: media-ctl-pipeline interface_type sensor_type {start|stop}"
-
-echo "Pipeline $1 $2 $3"
-
-case $1 in
-	dvp)
-		case $3 in
-        		start)
-				# media-ctl -vl "'sc2235 1-0030':0 -> 'stf_dvp0':0 [1]"
-				# media-ctl -vl "'ov5640 1-003c':0 -> 'stf_dvp0':0 [1]"
-				case $2 in
-					VIN)
-						echo "DVP vin 使能pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_vin0_wr':0 [1]"
-						# media-ctl -vl "'stf_vin0_wr':1 -> 'stf_vin0_wr_video0':0 [1]"
-						;;
-					ISP0)
-						echo "DVP ISP0 使能pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_isp0':0 [1]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
-						;;
-					ISP0RAW)
-						echo "DVP ISP0RAW 使能pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_isp0':0 [1]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
-						;;
-					ISP1)
-						echo "DVP ISP1 使能pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_isp1':0 [1]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
-						;;
-					ISP1RAW)
-						echo "DVP ISP1RAW 使能pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_isp1':0 [1]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
-						;;
-					*)
-						echo $USAGE
-						exit 1
-						;;
-				esac
-				;;
-			stop)
-				# media-ctl -vl "'sc2235 1-0030':0 -> 'stf_dvp0':0 [0]"
-				# media-ctl -vl "'ov5640 1-003c':0 -> 'stf_dvp0':0 [0]"
-				case $2 in
-					VIN)
-						echo "DVP vin 关闭pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_vin0_wr':0 [0]"
-						# media-ctl -vl "'stf_vin0_wr':1 -> 'stf_vin0_wr_video0':0 [0]"
-						;;
-					ISP0)
-						echo "DVP ISP0 关闭pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_isp0':0 [0]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
-						;;
-					ISP0RAW)
-						echo "DVP ISP0RAW 关闭pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_isp0':0 [0]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
-						;;
-					ISP1)
-						echo "DVP ISP1 关闭pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_isp1':0 [0]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
-						;;
-					ISP1RAW)
-						echo "DVP ISP1RAW 关闭pipeline:"
-						media-ctl -vl "'stf_dvp0':1 -> 'stf_isp1':0 [0]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
-						;;
-					*)
-						echo $USAGE
-						exit 1
-						;;
-				esac
-				;;
-			*)
-				echo $USAGE
-				exit 1
-				;;
-		esac
-		;;
-	csiphy0)
-		case $3 in
-        		start)
-				# media-ctl -vl "'ov4689 0-0036':0 -> 'stf_csiphy0':0 [1]"
-				case $2 in
-					VIN)
-						echo "csiphy0 CSIRX0 vin 使能pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [1]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_vin0_wr':0 [1]"
-						;;
-					ISP0)
-						echo "csiphy0 CSIRX0 ISP0 使能pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [1]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_isp0':0 [1]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
-						;;
-					ISP0RAW)
-						echo "csiphy0 CSIRX0 ISP0RAW 使能pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [1]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_isp0':0 [1]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
-						;;
-					ISP1)
-						echo "csiphy0 CSIRX0 ISP1 使能pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [1]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_isp1':0 [1]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
-						;;
-					ISP1RAW)
-						echo "csiphy0 CSIRX0 ISP1RAW 使能pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [1]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_isp1':0 [1]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
-						;;
-
-					*)
-						echo $USAGE
-						exit 1
-						;;
-				esac
-				;;
-			stop)
-				# media-ctl -vl "'ov4689 0-0036':0 -> 'stf_csiphy0':0 [0]"
-				case $2 in
-					VIN)
-						echo "csiphy0 CSIRX0 vin 关闭pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [0]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_vin0_wr':0 [0]"
-						;;
-					ISP0)
-						echo "csiphy0 CSIRX0 ISP0 关闭pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [0]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_isp0':0 [0]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
-						;;
-					ISP0RAW)
-						echo "csiphy0 CSIRX0 ISP0RAW 关闭pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [0]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_isp0':0 [0]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
-						;;
-					ISP1)
-						echo "csiphy0 CSIRX0 ISP1 关闭pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [0]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_isp1':0 [0]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
-						;;
-					ISP1RAW)
-						echo "csiphy0 CSIRX0 ISP1RAW 关闭pipeline:"
-						media-ctl -vl "'stf_csiphy0':1 -> 'stf_csi0':0 [0]"
-						media-ctl -vl "'stf_csi0':1 -> 'stf_isp1':0 [0]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
-						;;
-
-					*)
-						echo $USAGE
-						exit 1
-						;;
-				esac
-				;;
-			*)
-				echo $USAGE
-				exit 1
-				;;
-		esac
-		;;
-	csiphy1)
-		case $3 in
-        		start)
-				# media-ctl -vl "'ov4689 2-0036':0 -> 'stf_csiphy1':0 [1]"
-				case $2 in
-					VIN)
-						echo "csiphy1 CSIRX0 vin 使能pipeline:"
-						;;
-					ISP0)
-						echo "csiphy1 CSIRX1 ISP0 使能pipeline:"
-						media-ctl -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [1]"
-						media-ctl -vl "'stf_csi1':1 -> 'stf_isp0':0 [1]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
-						;;
-					ISP0RAW)
-						echo "csiphy1 CSIRX1 ISP0RAW 使能pipeline:"
-						media-ctl -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [1]"
-						media-ctl -vl "'stf_csi1':1 -> 'stf_isp0':0 [1]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [1]"
-						;;
-					ISP1)
-						echo "csiphy1 CSIRX1 ISP1 使能pipeline:"
-						media-ctl -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [1]"
-						media-ctl -vl "'stf_csi1':1 -> 'stf_isp1':0 [1]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
-						;;
-					ISP1RAW)
-						echo "csiphy1 CSIRX1 ISP1RAW 使能pipeline:"
-						media-ctl -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [1]"
-						media-ctl -vl "'stf_csi1':1 -> 'stf_isp1':0 [1]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [1]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [1]"
-						;;
-
-					*)
-						echo $USAGE
-						exit 1
-						;;
-				esac
-				;;
-			stop)
-				# media-ctl -vl "'ov4689 0-0036':0 -> 'stf_csiphy0':0 [0]"
-				case $2 in
-					VIN)
-						echo "csiphy1 CSIRX0 vin 关闭pipeline:"
-						;;
-					ISP0)
-						echo "csiphy1 CSIRX1 ISP0 关闭pipeline:"
-						media-ctl -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [0]"
-						media-ctl -vl "'stf_csi1':1 -> 'stf_isp0':0 [0]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
-						;;
-					ISP0RAW)
-						echo "csiphy1 CSIRX1 ISP0RAW 关闭pipeline:"
-						media-ctl -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [0]"
-						media-ctl -vl "'stf_csi1':1 -> 'stf_isp0':0 [0]"
-						media-ctl -vl "'stf_isp0':1 -> 'stf_vin0_isp0_raw':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp0':1 -> 'stf_vin0_isp0_video1':0 [0]"
-						;;
-					ISP1)
-						echo "csiphy1 CSIRX1 ISP1 关闭pipeline:"
-						media-ctl -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [0]"
-						media-ctl -vl "'stf_csi1':1 -> 'stf_isp1':0 [0]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
-						;;
-					ISP1RAW)
-						echo "csiphy1 CSIRX1 ISP1RAW 关闭pipeline:"
-						media-ctl -vl "'stf_csiphy1':1 -> 'stf_csi1':0 [0]"
-						media-ctl -vl "'stf_csi1':1 -> 'stf_isp1':0 [0]"
-						media-ctl -vl "'stf_isp1':1 -> 'stf_vin0_isp1_raw':0 [0]"
-						# media-ctl -vl "'stf_vin0_isp1':1 -> 'stf_vin0_isp1_video2':0 [0]"
-						;;
-					*)
-						echo $USAGE
-						exit 1
-						;;
-				esac
-				;;
-			*)
-				echo $USAGE
-				exit 1
-				;;
-		esac
-		;;
-	*)
-		echo $USAGE
-		exit 1
-		;;
-esac
-
-exit 0;
@@ -1,29 +0,0 @@
-################################################################################
-#
-# v4l2test
-#
-################################################################################
-
-V4L2_TEST_LICENSE = GPL-2.0+
-
-define V4L2_TEST_BUILD_CMDS
-	cp package/v4l2_test/v4l2_test.c $(@D)/
-	cp package/v4l2_test/yuv.c $(@D)/
-	cp package/v4l2_test/yuv.h $(@D)/
-	cp package/v4l2_test/convert.c $(@D)/
-	cp package/v4l2_test/convert.h $(@D)/
-	cp package/v4l2_test/config.h $(@D)/
-	cp package/v4l2_test/string.c $(@D)/
-	cp package/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)
-endef
-
-define V4L2_TEST_INSTALL_TARGET_CMDS
-        install -m 0755 -D $(@D)/v4l2test $(TARGET_DIR)/usr/bin/v4l2test
-        install -m 0755 -D $(@D)/pipeline_setting.sh $(TARGET_DIR)/usr/bin/pipeline_setting.sh
-endef
-
-V4L2_TEST_DEPENDENCIES = jpeg libv4l
-$(eval $(generic-package))
-
				`@@ -0,0 +1 @@`
				`include $(sort $(wildcard package/starfive//.mk))`