503 lines
11 KiB
C
503 lines
11 KiB
C
#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");
|
|
}
|
|
}
|
|
|