/* SPDX-License-Identifier: LGPL-2.1-or-later */
|
/*
|
* ConfigFS Gadget device handling
|
*
|
* Copyright (C) 2020 Rockchip Electronics Co., Ltd.
|
*
|
*/
|
|
/* To provide basename and asprintf from the GNU library. */
|
#define _GNU_SOURCE
|
|
#include <unistd.h>
|
#include <dirent.h>
|
#include <stdio.h>
|
#include <stdbool.h>
|
#include <string.h>
|
#include <stdlib.h>
|
#include <fcntl.h>
|
#include <pthread.h>
|
#include <errno.h>
|
#include <glob.h>
|
#include <linux/videodev2.h>
|
|
#include "uvc_configfs.h"
|
|
#define V4L2_PIX_FMT_H265 v4l2_fourcc('H', '2', '6', '5') /* H265 with start codes */
|
|
#define UVC_FUNCTION_NAME "uvc*"
|
|
#define UVC_GUID_FORMAT_MJPEG \
|
{ 'M', 'J', 'P', 'G', 0x00, 0x00, 0x10, 0x00, \
|
0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71}
|
#define UVC_GUID_FORMAT_YUY2 \
|
{ 'Y', 'U', 'Y', '2', 0x00, 0x00, 0x10, 0x00, \
|
0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71}
|
#define UVC_GUID_FORMAT_NV12 \
|
{ 'N', 'V', '1', '2', 0x00, 0x00, 0x10, 0x00, \
|
0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71}
|
#define UVC_GUID_FORMAT_H264 \
|
{ 'H', '2', '6', '4', 0x00, 0x00, 0x10, 0x00, \
|
0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71}
|
#define UVC_GUID_FORMAT_H265 \
|
{ 'H', '2', '6', '5', 0x00, 0x00, 0x10, 0x00, \
|
0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71}
|
|
#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
|
|
struct uvc_function_format_info {
|
uint8_t guid[16];
|
uint32_t fcc;
|
};
|
|
static struct uvc_function_format_info uvc_formats[] = {
|
{
|
.guid = UVC_GUID_FORMAT_YUY2,
|
.fcc = V4L2_PIX_FMT_YUYV,
|
},
|
{
|
.guid = UVC_GUID_FORMAT_MJPEG,
|
.fcc = V4L2_PIX_FMT_MJPEG,
|
},
|
{
|
.guid = UVC_GUID_FORMAT_H264,
|
.fcc = V4L2_PIX_FMT_H264,
|
},
|
{
|
.guid = UVC_GUID_FORMAT_H265,
|
.fcc = V4L2_PIX_FMT_H265,
|
},
|
{
|
.guid = UVC_GUID_FORMAT_NV12,
|
.fcc = V4L2_PIX_FMT_NV12,
|
}
|
};
|
|
/* -----------------------------------------------------------------------------
|
* Path handling and support
|
*/
|
static char *path_join(const char *dirname, const char *name)
|
{
|
char *path;
|
int ret;
|
|
ret = asprintf(&path, "%s/%s", dirname, name);
|
|
/*
|
* asprintf returns -1 on allocation or other errors, leaving 'path'
|
* undefined. We shouldn't even call free(path) here. We want to return
|
* NULL on error, so we must manually set it.
|
*/
|
if (ret < 0)
|
path = NULL;
|
|
return path;
|
}
|
|
static char *path_glob_index_match(const char *g, unsigned int index)
|
{
|
glob_t globbuf;
|
char *match = NULL;
|
|
glob(g, 0, NULL, &globbuf);
|
|
if (globbuf.gl_pathc > index)
|
match = strdup(globbuf.gl_pathv[index]);
|
|
globfree(&globbuf);
|
|
return match;
|
}
|
|
/*
|
* Find and return the full path of the first directory entry that satisfies
|
* the match function.
|
*/
|
static char *dir_first_match(const char *dir, int(*match)(const struct dirent *))
|
{
|
struct dirent **entries;
|
unsigned int i;
|
int n_entries;
|
char *path;
|
|
n_entries = scandir(dir, &entries, match, alphasort);
|
if (n_entries < 0)
|
return NULL;
|
|
if (n_entries == 0) {
|
free(entries);
|
return NULL;
|
}
|
|
path = path_join(dir, entries[0]->d_name);
|
|
for (i = 0; i < (unsigned int)n_entries; ++i)
|
free(entries[i]);
|
|
free(entries);
|
|
return path;
|
}
|
|
/* -----------------------------------------------------------------------------
|
* Attribute handling
|
*/
|
static int attribute_read(const char *path, const char *file, void *buf,
|
unsigned int len)
|
{
|
char *f;
|
int ret;
|
int fd;
|
|
f = path_join(path, file);
|
if (!f)
|
return -ENOMEM;
|
|
fd = open(f, O_RDONLY);
|
free(f);
|
if (fd == -1) {
|
printf("Failed to open attribute %s: %s\n", file,
|
strerror(errno));
|
return -ENOENT;
|
}
|
|
ret = read(fd, buf, len);
|
close(fd);
|
|
if (ret < 0) {
|
printf("Failed to read attribute %s: %s\n", file,
|
strerror(errno));
|
return -ENODATA;
|
}
|
|
return len;
|
}
|
|
static int attribute_read_uint(const char *path, const char *file,
|
unsigned int *val)
|
{
|
char buf[11] = {0};
|
char *endptr;
|
int ret;
|
|
ret = attribute_read(path, file, buf, sizeof(buf) - 1);
|
if (ret < 0)
|
return ret;
|
|
buf[ret] = '\0';
|
errno = 0;
|
|
/* base 0: Autodetect hex, octal, decimal. */
|
*val = strtoul(buf, &endptr, 0);
|
if (errno)
|
return -errno;
|
|
if (endptr == buf)
|
return -ENODATA;
|
|
return 0;
|
}
|
|
static char *attribute_read_str(const char *path, const char *file)
|
{
|
char buf[1024] = {0};
|
char *p;
|
int ret;
|
|
ret = attribute_read(path, file, buf, sizeof(buf) - 1);
|
if (ret < 0)
|
return NULL;
|
|
buf[ret] = '\0';
|
|
p = strrchr(buf, '\n');
|
if(p == NULL)
|
return NULL;
|
if (p != buf)
|
*p = '\0';
|
|
return strdup(buf);
|
}
|
|
static char *configfs_find_uvc_function(unsigned int index)
|
{
|
const char *target = UVC_FUNCTION_NAME;
|
const char *format;
|
char *func_path;
|
char *path;
|
int ret;
|
|
format = "%s/usb_gadget/*/functions/%s";
|
ret = asprintf(&path, format, "/sys/kernel/config", target);
|
if (!ret)
|
return NULL;
|
|
func_path = path_glob_index_match(path, index);
|
|
free(path);
|
|
return func_path;
|
}
|
|
static int udc_find_video_device(const char *udc, const char *function)
|
{
|
char *vpath;
|
char *video = NULL;
|
glob_t globbuf;
|
int video_id = -1;
|
unsigned int i;
|
int ret;
|
|
ret = asprintf(&vpath,
|
"/sys/class/udc/%s/device/gadget/video4linux/video*",
|
udc ? udc : "*");
|
if (!ret)
|
return -1;
|
|
glob(vpath, 0, NULL, &globbuf);
|
free(vpath);
|
|
for (i = 0; i < globbuf.gl_pathc; ++i) {
|
char *config;
|
bool match;
|
|
/* Match on the first if no search string. */
|
if (!function)
|
break;
|
|
config = attribute_read_str(globbuf.gl_pathv[i],
|
"function_name");
|
match = strcmp(function, config) == 0;
|
free(config);
|
if (match)
|
break;
|
}
|
|
if (i < globbuf.gl_pathc) {
|
const char *v = basename(globbuf.gl_pathv[i]);
|
video = path_join("/dev", v);
|
if (sscanf(video, "/dev/video%d", &video_id) != 1)
|
video_id = -1;
|
free(video);
|
}
|
|
globfree(&globbuf);
|
|
return video_id;
|
}
|
|
/*
|
* configfs_free_uvc_function - Free a uvc_function_config object
|
* @fc: The uvc_function_config to be freed
|
*
|
* Free the given @fc function previously allocated by a call to
|
* configfs_parse_uvc_function().
|
*/
|
void configfs_free_uvc_function(struct uvc_function_config *fc)
|
{
|
unsigned int i, j;
|
|
free(fc->udc);
|
free(fc->dev_name);
|
|
for (i = 0; i < fc->streaming.num_formats; ++i) {
|
struct uvc_function_config_format *format =
|
&fc->streaming.formats[i];
|
|
for (j = 0; j < format->num_frames; ++j) {
|
struct uvc_function_config_frame *frame =
|
&format->frames[j];
|
|
free(frame->intervals);
|
}
|
free(format->frames);
|
}
|
free(fc->streaming.formats);
|
free(fc);
|
}
|
|
#define configfs_parse_child(parent, child, cfg, parse) \
|
({ \
|
char *__path; \
|
int __ret; \
|
\
|
__path = path_join((parent), (child)); \
|
if (__path) { \
|
__ret = parse(__path, (cfg)); \
|
free(__path); \
|
} else { \
|
__ret = -ENOMEM; \
|
} \
|
\
|
__ret; \
|
})
|
|
static int configfs_parse_interface(const char *path,
|
struct uvc_function_config_interface *cfg)
|
{
|
int ret;
|
|
ret = attribute_read_uint(path, "bInterfaceNumber",
|
&cfg->bInterfaceNumber);
|
|
return ret;
|
}
|
|
static int configfs_parse_control(const char *path,
|
struct uvc_function_config_control *cfg)
|
{
|
int ret;
|
|
ret = configfs_parse_interface(path, &cfg->intf);
|
|
return ret;
|
}
|
|
static int configfs_parse_streaming_frame(const char *path,
|
struct uvc_function_config_frame *frame)
|
{
|
char *intervals;
|
char *p;
|
int ret = 0;
|
|
ret = ret ? : attribute_read_uint(path, "bFrameIndex", &frame->index);
|
ret = ret ? : attribute_read_uint(path, "wWidth", &frame->width);
|
ret = ret ? : attribute_read_uint(path, "wHeight", &frame->height);
|
if (ret)
|
return ret;
|
|
intervals = attribute_read_str(path, "dwFrameInterval");
|
if (!intervals)
|
return -EINVAL;
|
|
for (p = intervals; *p; ) {
|
unsigned int interval;
|
unsigned int *mem;
|
char *endp;
|
size_t size;
|
|
interval = strtoul(p, &endp, 10);
|
if (*endp != '\0' && *endp != '\n') {
|
ret = -EINVAL;
|
break;
|
}
|
|
p = *endp ? endp + 1 : endp;
|
|
size = sizeof *frame->intervals * (frame->num_intervals + 1);
|
mem = realloc(frame->intervals, size);
|
if (!mem) {
|
ret = -ENOMEM;
|
break;
|
}
|
frame->intervals = mem;
|
frame->intervals[frame->num_intervals++] = interval;
|
}
|
free(intervals);
|
|
return ret;
|
}
|
|
static int frame_filter(const struct dirent *ent)
|
{
|
/* Accept all directories but "." and "..". */
|
if (ent->d_type != DT_DIR)
|
return 0;
|
if (!strcmp(ent->d_name, "."))
|
return 0;
|
if (!strcmp(ent->d_name, ".."))
|
return 0;
|
return 1;
|
}
|
|
static int frame_compare(const void *a, const void *b)
|
{
|
const struct uvc_function_config_frame *fa = a;
|
const struct uvc_function_config_frame *fb = b;
|
|
if (fa->index < fb->index)
|
return -1;
|
else if (fa->index == fb->index)
|
return 0;
|
else
|
return 1;
|
}
|
|
static int configfs_parse_streaming_format(const char *path,
|
struct uvc_function_config_format *format)
|
{
|
struct dirent **entries;
|
char link_target[1024] = {0};
|
char *segment;
|
unsigned int i;
|
int n_entries;
|
int ret;
|
|
ret = attribute_read_uint(path, "bFormatIndex", &format->index);
|
if (ret < 0)
|
return ret;
|
|
ret = readlink(path, link_target, sizeof(link_target) - 1);
|
if (ret < 0)
|
return ret;
|
|
link_target[ret] = '\0';
|
|
/*
|
* Extract the second-to-last path component of the link target,
|
* which contains the format descriptor type name as exposed by
|
* the UVC function driver.
|
*/
|
segment = strrchr(link_target, '/');
|
if (!segment)
|
return -EINVAL;
|
*segment = '\0';
|
segment = strrchr(link_target, '/');
|
if (!segment)
|
return -EINVAL;
|
segment++;
|
if (!strcmp(segment, "framebased")) {
|
ret = attribute_read(path, "guidFormat", format->guid,
|
sizeof(format->guid));
|
} else if (!strcmp(segment, "mjpeg")) {
|
static const uint8_t guid[16] = UVC_GUID_FORMAT_MJPEG;
|
memcpy(format->guid, guid, 16);
|
} else if (!strcmp(segment, "uncompressed")) {
|
ret = attribute_read(path, "guidFormat", format->guid,
|
sizeof(format->guid));
|
if (ret < 0)
|
return ret;
|
} else {
|
return -EINVAL;
|
}
|
|
for (i = 0; i < ARRAY_SIZE(uvc_formats); ++i) {
|
if (!memcmp(uvc_formats[i].guid, format->guid, 16)) {
|
format->fcc = uvc_formats[i].fcc;
|
break;
|
}
|
}
|
|
/* Find all entries corresponding to a frame and parse them. */
|
n_entries = scandir(path, &entries, frame_filter, alphasort);
|
if (n_entries < 0)
|
return -errno;
|
|
if (n_entries == 0) {
|
free(entries);
|
return -EINVAL;
|
}
|
|
format->num_frames = n_entries;
|
format->frames = calloc(sizeof *format->frames, format->num_frames);
|
if (!format->frames)
|
return -ENOMEM;
|
|
for (i = 0; i < (unsigned int)n_entries; ++i) {
|
char *frame;
|
|
frame = path_join(path, entries[i]->d_name);
|
if (!frame) {
|
ret = -ENOMEM;
|
goto done;
|
}
|
|
ret = configfs_parse_streaming_frame(frame, &format->frames[i]);
|
free(frame);
|
if (ret < 0)
|
goto done;
|
}
|
|
/* Sort the frames by index. */
|
qsort(format->frames, format->num_frames, sizeof *format->frames,
|
frame_compare);
|
|
done:
|
for (i = 0; i < (unsigned int)n_entries; ++i)
|
free(entries[i]);
|
free(entries);
|
|
return ret;
|
}
|
|
static int format_filter(const struct dirent *ent)
|
{
|
char *path;
|
bool valid;
|
|
/*
|
* Accept all links that point to a directory containing a
|
* "bFormatIndex" file.
|
*/
|
if (ent->d_type != DT_LNK)
|
return 0;
|
|
path = path_join(ent->d_name, "bFormatIndex");
|
if (!path)
|
return 0;
|
|
valid = access(path, R_OK);
|
free(path);
|
return valid;
|
}
|
|
static int format_compare(const void *a, const void *b)
|
{
|
const struct uvc_function_config_format *fa = a;
|
const struct uvc_function_config_format *fb = b;
|
|
if (fa->index < fb->index)
|
return -1;
|
else if (fa->index == fb->index)
|
return 0;
|
else
|
return 1;
|
}
|
|
static int configfs_parse_streaming_header(const char *path,
|
struct uvc_function_config_streaming *cfg)
|
{
|
struct dirent **entries;
|
unsigned int i;
|
int n_entries;
|
int ret;
|
|
/* Find all entries corresponding to a format and parse them. */
|
n_entries = scandir(path, &entries, format_filter, alphasort);
|
if (n_entries < 0)
|
return -errno;
|
|
if (n_entries == 0) {
|
free(entries);
|
return -EINVAL;
|
}
|
|
cfg->num_formats = n_entries;
|
cfg->formats = calloc(sizeof *cfg->formats, cfg->num_formats);
|
if (!cfg->formats)
|
return -ENOMEM;
|
|
for (i = 0; i < (unsigned int)n_entries; ++i) {
|
char *format;
|
|
format = path_join(path, entries[i]->d_name);
|
if (!format) {
|
ret = -ENOMEM;
|
goto done;
|
}
|
|
ret = configfs_parse_streaming_format(format, &cfg->formats[i]);
|
free(format);
|
if (ret < 0)
|
goto done;
|
}
|
|
/* Sort the formats by index. */
|
qsort(cfg->formats, cfg->num_formats, sizeof *cfg->formats,
|
format_compare);
|
|
done:
|
for (i = 0; i < (unsigned int)n_entries; ++i)
|
free(entries[i]);
|
free(entries);
|
|
return ret;
|
}
|
|
static int link_filter(const struct dirent *ent)
|
{
|
/* Accept all links. */
|
return ent->d_type == DT_LNK;
|
}
|
|
static int configfs_parse_streaming(const char *path,
|
struct uvc_function_config_streaming *cfg)
|
{
|
char *header;
|
char *class;
|
int ret;
|
|
ret = configfs_parse_interface(path, &cfg->intf);
|
if (ret < 0)
|
return ret;
|
/*
|
* Handle the high-speed class descriptors only for now. Find the first
|
* link to the class descriptors.
|
*/
|
class = path_join(path, "class/hs");
|
if (!class)
|
return -ENOMEM;
|
|
header = dir_first_match(class, link_filter);
|
free(class);
|
if (!header)
|
return -EINVAL;
|
|
ret = configfs_parse_streaming_header(header, cfg);
|
free(header);
|
return ret;
|
}
|
|
static int configfs_parse_uvc(const char *fpath,
|
struct uvc_function_config *fc)
|
{
|
int ret = 0;
|
|
ret = ret ? : configfs_parse_child(fpath, "control", &fc->control,
|
configfs_parse_control);
|
ret = ret ? : configfs_parse_child(fpath, "streaming", &fc->streaming,
|
configfs_parse_streaming);
|
|
/*
|
* These parameters should be part of the streaming interface in
|
* ConfigFS, but for legacy reasons they are located directly in the
|
* function directory.
|
*/
|
ret = ret ? : attribute_read_uint(fpath, "streaming_interval",
|
&fc->streaming.ep.bInterval);
|
ret = ret ? : attribute_read_uint(fpath, "streaming_maxburst",
|
&fc->streaming.ep.bMaxBurst);
|
ret = ret ? : attribute_read_uint(fpath, "streaming_maxpacket",
|
&fc->streaming.ep.wMaxPacketSize);
|
|
return ret;
|
}
|
|
struct uvc_function_config *configfs_parse_uvc_function(unsigned int index)
|
{
|
struct uvc_function_config *fc;
|
char *fpath;
|
char *function;
|
int ret = 0;
|
|
fc = malloc(sizeof *fc);
|
if (fc == NULL)
|
return NULL;
|
|
memset(fc, 0, sizeof *fc);
|
|
/* Find the function in ConfigFS. */
|
fpath = configfs_find_uvc_function(index);
|
if (!fpath) {
|
free(fc);
|
return NULL;
|
}
|
|
function = basename(fpath);
|
|
fc->dev_name = attribute_read_str(fpath, "device_name");
|
if(fc->dev_name == NULL)
|
fc->dev_name = "UVC CAMERA";
|
fc->udc = attribute_read_str(fpath, "../../UDC");
|
fc->video = udc_find_video_device(fc->udc, function);
|
printf("fc->dev_name:%s, fc->udc:%s, fc->video:%d \n",fc->dev_name,fc->udc,fc->video);
|
if (fc->video < 0) {
|
ret = -ENODEV;
|
goto done;
|
}
|
fc->index = index;
|
|
ret = configfs_parse_uvc(fpath, fc);
|
|
done:
|
if (ret) {
|
configfs_free_uvc_function(fc);
|
fc = NULL;
|
}
|
|
free(fpath);
|
|
return fc;
|
}
|
