GK SDK 源码库: XMIPCLinuxV100R005C00SPC030 (kernel/tools/open_source excluded)

sample/uvc_app/video/hal/uvc_formats.c

@@ -0,0 +1,502 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "uvc_formats.h"
+#include "config_svc.h"
+#include "uvc_log.h"
+
+#define FORMAT_NV12  "nv12"
+#define FORMAT_NV21  "nv21"
+#define FORMAT_YUYV  "yuyv"
+#define FORMAT_H264  "h264"
+#define FORMAT_H265  "h265"
+#define FORMAT_MJPEG "mjpeg"
+
+#define ARRAY_SIZE(array) (sizeof(array) / sizeof(array[0]))
+#define v4l2_fourcc(a, b, c, d)\
+    ((__u32)(a) | ((__u32)(b) << 8) | ((__u32)(c) << 16) | ((__u32)(d) << 24))
+
+#define V4L2_PIX_FMT_H265 v4l2_fourcc('H', '2', '6', '5')   /* H265 with start codes */
+
+
+struct node {
+    void* data;
+    struct node* row;
+    struct node* column;
+};
+
+static struct node g_formats_list_head = {
+    .data = NULL,
+    .row  = NULL,
+    .column = NULL,
+};
+
+static int g_malloc_count = 0;
+
+static char* fcc_to_string(int fcc)
+{
+    switch (fcc) {
+        case V4L2_PIX_FMT_YUYV:
+            return "yuyv";
+
+        case V4L2_PIX_FMT_NV21:
+            return "nv21";
+
+        case V4L2_PIX_FMT_NV12:
+            return "nv12";
+
+        case V4L2_PIX_FMT_MJPEG:
+            return "mjpeg";
+
+        case V4L2_PIX_FMT_H264:
+            return "h264";
+
+        case V4L2_PIX_FMT_H265:
+            return "h265";
+
+        default:
+            return "Unknow";
+    }
+}
+
+static int string_to_fcc(char* str)
+{
+    if (memcmp(str, FORMAT_YUYV, strlen(str)) == 0) {
+        return V4L2_PIX_FMT_YUYV;
+    }
+
+    if (memcmp(str, FORMAT_NV21, strlen(str)) == 0) {
+        return V4L2_PIX_FMT_NV21;
+    }
+
+    if (memcmp(str, FORMAT_NV12, strlen(str)) == 0) {
+        return V4L2_PIX_FMT_NV12;
+    }
+
+    if (memcmp(str, FORMAT_MJPEG, strlen(str)) == 0) {
+        return V4L2_PIX_FMT_MJPEG;
+    }
+
+    if (memcmp(str, FORMAT_H264, strlen(str)) == 0) {
+        return V4L2_PIX_FMT_H264;
+    }
+
+    if (memcmp(str, FORMAT_H265, strlen(str)) == 0) {
+        return V4L2_PIX_FMT_H265;
+    }
+
+    return 0;
+}
+
+static void show_formats_info(void)
+{
+    int i, j;
+    struct node* list_head = &g_formats_list_head;
+    struct node* fmt_n = NULL;
+    struct node* frm_n = NULL;
+    struct uvc_format_info* fmt_info = NULL;
+    struct uvc_frame_info*  frm_info = NULL;
+
+    printf("\n");
+    printf("Formats info:\n");
+    fmt_n = list_head->row;
+    i = 1;
+    while (fmt_n) {
+        fmt_info = (struct uvc_format_info*)fmt_n->data;
+        printf("\tFormat %d: %s\n", i, fcc_to_string(fmt_info->fcc));
+        frm_n = fmt_n->column;
+        j = 1;
+        while (frm_n) {
+            frm_info = (struct uvc_frame_info*)frm_n->data;
+            printf("\tFrame %d: \t%d x %d \t@ %d.\n",
+                    j, frm_info->width, frm_info->height, frm_info->intervals[0]);
+            frm_n = frm_n->column;
+            j++;
+        }
+
+        fmt_n = fmt_n->row;
+        i++;
+        printf("\n");
+    }
+}
+
+static struct node* new_node(void* data)
+{
+    struct node* new = malloc(sizeof(struct node));
+
+    if (new == NULL) {
+        uvc_loge("Out of memory\n");
+    } else {
+        g_malloc_count++;
+        new->data = data;
+        new->row  = NULL;
+        new->column = NULL;
+    }
+
+    return new;
+}
+
+static struct node* new_format(int fcc)
+{
+    struct node* new_fmt = NULL;
+    struct uvc_format_info* fmt = malloc(sizeof(struct uvc_format_info));
+
+    if (fmt == NULL) {
+        uvc_loge("Out of memory.\n");
+    } else {
+        g_malloc_count++;
+        fmt->fcc      = fcc;
+        fmt->frames   = NULL;
+        fmt->frm_nums = 0;
+        new_fmt = new_node(fmt);
+        if (new_fmt == NULL) {
+            free(fmt);
+            g_malloc_count--;
+            fmt = NULL;
+        }
+    }
+
+    return new_fmt;
+}
+
+static struct node* new_frame(int w, int h, int interval)
+{
+    struct node* new_frm = NULL;
+    struct uvc_frame_info* frm = malloc(sizeof(struct uvc_frame_info));
+
+    if (frm == NULL) {
+        uvc_loge("Out of memory.\n");
+    } else {
+        g_malloc_count++;
+        memset(frm, 0, sizeof(struct uvc_frame_info));
+        frm->width  = w;
+        frm->height = h;
+        frm->intervals[0] = interval;
+
+        new_frm = new_node(frm);
+        if (new_frm == NULL) {
+            free(frm);
+            g_malloc_count--;
+            frm = NULL;
+        }
+    }
+
+    return new_frm;
+}
+
+static void add_format_tail(struct node* list_head, struct node* new_fmt)
+{
+    struct node* cur = list_head;
+
+    while (cur->row) {
+        cur = cur->row;
+    }
+
+    cur->row = new_fmt;
+}
+
+static void add_frame_tail(struct node* list_head, int fcc, struct node* new_frm)
+{
+    struct node* frm = NULL;
+    struct node* fmt = list_head->row;
+    struct uvc_format_info* fmt_info = NULL;
+
+    while (fmt) {
+        fmt_info = (struct uvc_format_info*)fmt->data;
+        if (fmt_info->fcc == fcc) {
+            fmt_info->frm_nums++;
+            break;
+        } else {
+            fmt = fmt->row;
+        }
+    }
+
+    if (fmt == NULL) {
+        uvc_loge("Not found format: %d.\n", fcc);
+        return;
+    }
+
+    frm = fmt;
+    while (frm->column) {
+        frm = frm->column;
+    }
+
+    frm->column = new_frm;
+}
+
+static void add_format(struct node* list_head, int fcc)
+{
+    struct node* new_fmt = new_format(fcc);
+
+    add_format_tail(list_head, new_fmt);
+}
+
+static void add_frame(struct node* list_head, int fcc, struct uvc_frame_info frm)
+{
+    struct node* new_frm = new_frame(frm.width, frm.height, frm.intervals[0]);
+
+    add_frame_tail(list_head, fcc, new_frm);
+}
+
+static char* parser_sub(char* in, char* out, char k)
+{
+    char* tmp = in;
+
+    while (1) {
+        if (*tmp == '\0') {
+            tmp = NULL;
+            break;
+        } else if (*tmp == k) {
+            /* step k */
+            tmp++;
+            break;
+        } else {
+            *out++ = *tmp++;
+        }
+    }
+
+    *out = '\0';
+
+    return tmp;
+}
+
+static char* parser_format(char* str, char* format)
+{
+    char k = ',';
+    return parser_sub(str, format, k);
+}
+
+static char* parser_frame(char* formats, char* frame)
+{
+    char k = ',';
+    return parser_sub(formats, frame, k);
+}
+
+static int frame_interval(int fps)
+{
+    return ((1000 * 10000) / fps);
+}
+
+static void parser_whf(char* frame, struct uvc_frame_info* frm_info)
+{
+    int i;
+    char param[16];
+    char* tmp = frame;
+    char k = '-';
+
+    /* width */
+    bzero(param, sizeof(param));
+    tmp = parser_sub(tmp, param, k);
+    frm_info->width = atoi(param);
+
+    /* height */
+    bzero(param, sizeof(param));
+    tmp = parser_sub(tmp, param, k);
+    frm_info->height = atoi(param);
+
+    /* fps */
+    i = 0;
+    do {
+        bzero(param, sizeof(param));
+        tmp = parser_sub(tmp, param, k);
+
+        if (i == ARRAY_SIZE(frm_info->intervals)) {
+            break;
+        }
+
+        frm_info->intervals[i++] = frame_interval(atoi(param));
+    } while (tmp != NULL);
+}
+
+static int init_format(char* key)
+{
+    int rc, fcc;
+    char str[256];
+    char frame[256];
+    char format[16];
+    char stream[32] = "stream:";
+    char* tmp = NULL;
+    struct uvc_frame_info frm_info= {0};
+
+    strcat(stream, key);
+    bzero(str, sizeof(str));
+    rc = get_config_string(stream, str, sizeof(str));
+    if (rc < 0) {
+        uvc_logd("Not found %s\n", stream);
+        return -1;
+    }
+
+    tmp = (char*)str;
+
+    bzero(format, sizeof(format));
+    tmp = parser_format(tmp, format);
+    if (tmp == NULL) {
+        uvc_loge("parser_format fail\n");
+        return 0;
+    }
+
+    uvc_logd("Format %s: %s\n", format, tmp);
+
+    fcc = string_to_fcc(format);
+    add_format(&g_formats_list_head, fcc);
+    do {
+        bzero(frame, sizeof(frame));
+        tmp = parser_frame(tmp, frame);
+
+        parser_whf(frame, &frm_info);
+
+        add_frame(&g_formats_list_head, fcc, frm_info);
+    } while (tmp != NULL);
+
+    return 0;
+}
+
+static int frame_min(int* fmt_index, int* frm_index)
+{
+    *fmt_index = 1;
+    *frm_index = 1;
+
+    uvc_logd("Format min {format %d, frame %d}\n", *fmt_index, *frm_index);
+    return 0;
+}
+
+static int frame_max(int* fmt_index, int* frm_index)
+{
+    struct node* list_head = &g_formats_list_head;
+    struct node* tmp = list_head;
+    int index;
+
+    index = 0;
+    while (tmp->row) {
+        tmp = tmp->row;
+        index++;
+    }
+    *fmt_index = index;
+
+    index = 0;
+    while (tmp->column) {
+        tmp = tmp->column;
+        index++;
+    }
+    *frm_index = index;
+
+    uvc_logd("Format max {format %d, frame %d}\n", *fmt_index, *frm_index);
+    return 0;
+}
+
+int find_frame(int* fmt_index, int* frm_index, int* fcc, struct uvc_frame_info* frame)
+{
+    struct node* list_head = &g_formats_list_head;
+    struct node* tmp = list_head;
+    struct uvc_format_info* fmt = NULL;
+    struct uvc_frame_info* frm = NULL;
+    int index;
+
+    if (list_head->row == NULL) {
+        uvc_loge("UVC formats is not initialized.\n");
+        return -1;
+    }
+
+    if (*fmt_index == 0) {
+        frame_min(fmt_index, frm_index);
+    } else if (*fmt_index < 0) {
+        frame_max(fmt_index, frm_index);
+    }
+
+    if (frame == NULL || fcc == NULL) {
+        return 0;
+    }
+
+    /* Find format */
+    index = *fmt_index;
+    while (index-- && tmp) {
+        tmp = tmp->row;
+    }
+
+    if (tmp == NULL) {
+        uvc_logw("Not found {format %d , frame %d }, Use {format 1, frame 1}\n", *fmt_index, *frm_index);
+        show_formats_info();
+        fflush(stdout);
+
+        *fmt_index = 1;
+        *frm_index = 1;
+
+        return find_frame(fmt_index, frm_index, fcc, frame);
+    }
+
+    fmt = (struct uvc_format_info*)tmp->data;
+    *fcc = fmt->fcc;
+
+    /* Find frame */
+    index = *frm_index;
+    while (index-- && tmp) {
+        tmp = tmp->column;
+    }
+
+    if (tmp == NULL) {
+        uvc_logw("Not found {format %d, frame %d}, Use {format %d, frame 1}\n", *fmt_index, *frm_index, *fmt_index);
+        show_formats_info();
+        fflush(stdout);
+
+        *frm_index = 1;
+
+        return find_frame(fmt_index, frm_index, fcc, frame);
+    }
+
+    frm = (struct uvc_frame_info*)tmp->data;
+    memcpy(frame, frm, sizeof(struct uvc_frame_info));
+
+    uvc_logd("Find {format %d, frame %d} : %s, %dx%d @ %d\n", *fmt_index, *frm_index,
+            fcc_to_string(*fcc), frame->width, frame->height, frame->intervals[0]);
+
+    return 0;
+}
+
+
+
+void uvc_formats_init(void)
+{
+    int i, ret;
+    char key[16];
+
+    i = 0;
+    do {
+        bzero(key, sizeof(key));
+        i++;
+        snprintf(key, sizeof(key), "fmt%d", i);
+        ret = init_format(key);
+    } while (ret == 0);
+}
+
+void uvc_formats_deinit(void)
+{
+    struct node* list_head = &g_formats_list_head;
+    struct node* row = list_head->row;
+    struct node* tmp = NULL;
+    struct node* column = NULL;
+
+    while (row) {
+        list_head->row = row->row;
+
+        /* release resources */
+        column = row;
+        while (column) {
+            tmp = column->column;
+            free(column->data);
+            g_malloc_count--;
+            free(column);
+            g_malloc_count--;
+            column = tmp;
+        }
+
+        row = list_head->row;
+    }
+
+    if (g_malloc_count) {
+        uvc_logw("Resource leakage.\n");
+    } else {
+        uvc_logi("Release resources ok.\n");
+    }
+}
+