From 2bb0cfbf8137e02cc32aae3b36f85ef7300e8936 Mon Sep 17 00:00:00 2001 From: Laurent Pinchart Date: Sat, 9 Jun 2018 14:29:41 +0300 Subject: Split UVC gadget into a library and test application Split the project into a UVC gadget library and a test application. To avoid rolling out a custom build system, switch to CMake. Signed-off-by: Laurent Pinchart --- lib/configfs.c | 846 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 846 insertions(+) create mode 100644 lib/configfs.c (limited to 'lib/configfs.c') diff --git a/lib/configfs.c b/lib/configfs.c new file mode 100644 index 0000000..1dfde95 --- /dev/null +++ b/lib/configfs.c @@ -0,0 +1,846 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * ConfigFS Gadget device handling + * + * Copyright (C) 2018 Kieran Bingham + * + * Contact: Kieran Bingham + */ + +/* To provide basename and asprintf from the GNU library. */ + #define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "configfs.h" +#include "tools.h" + +/* ----------------------------------------------------------------------------- + * Path handling and support + */ + +static char *path_join(const char *dirname, const char *name) +{ + char *path; + + asprintf(&path, "%s/%s", dirname, name); + + return path; +} + +static char *path_glob_first_match(const char *g) +{ + glob_t globbuf; + char *match = NULL; + + glob(g, 0, NULL, &globbuf); + + if (globbuf.gl_pathc) + match = strdup(globbuf.gl_pathv[0]); + + 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) +{ + /* 4,294,967,295 */ + char buf[11]; + 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]; + 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 != buf) + *p = '\0'; + + return strdup(buf); +} + +/* ----------------------------------------------------------------------------- + * UDC parsing + */ + +/* + * udc_find_video_device - Find the video device node for a UVC function + * @udc: The UDC name + * @function: The UVC function name + * + * This function finds the video device node corresponding to a UVC function as + * specified by a @function name and @udc name. + * + * The @function parameter specifies the name of the USB function, usually in + * the form "uvc.%u". If NULL the first function found will be used. + * + * The @udc parameter specifies the name of the UDC. If NULL any UDC that + * contains a function matching the @function name will be used. + * + * Return a pointer to a newly allocated string containing the video device node + * full path if the function is found. Otherwise return NULL. The returned + * pointer must be freed by the caller with a call to free(). + */ +static char *udc_find_video_device(const char *udc, const char *function) +{ + char *vpath; + char *video = NULL; + glob_t globbuf; + unsigned int i; + + asprintf(&vpath, "/sys/class/udc/%s/device/gadget/video4linux/video*", + udc ? udc : "*"); + if (!vpath) + return NULL; + + 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); + } + + globfree(&globbuf); + + return video; +} + +/* ------------------------------------------------------------------------ + * GUIDs and formats + */ + +#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} + +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, + }, +}; + +/* ----------------------------------------------------------------------------- + * Legacy g_webcam support + */ + +static const struct uvc_function_config g_webcam_config = { + .control = { + .intf = { + .bInterfaceNumber = 0, + }, + }, + .streaming = { + .intf = { + .bInterfaceNumber = 1, + }, + .ep = { + .bInterval = 1, + .bMaxBurst = 0, + .wMaxPacketSize = 1024, + }, + .num_formats = 2, + .formats = (struct uvc_function_config_format[]) { + { + .index = 1, + .guid = UVC_GUID_FORMAT_YUY2, + .fcc = V4L2_PIX_FMT_YUYV, + .num_frames = 2, + .frames = (struct uvc_function_config_frame[]) { + { + .index = 1, + .width = 640, + .height = 360, + .num_intervals = 3, + .intervals = (unsigned int[]) { + 666666, + 10000000, + 50000000, + }, + }, { + .index = 2, + .width = 1280, + .height = 720, + .num_intervals = 1, + .intervals = (unsigned int[]) { + 50000000, + }, + }, + }, + }, { + .index = 2, + .guid = UVC_GUID_FORMAT_MJPEG, + .fcc = V4L2_PIX_FMT_MJPEG, + .num_frames = 2, + .frames = (struct uvc_function_config_frame[]) { + { + .index = 1, + .width = 640, + .height = 360, + .num_intervals = 3, + .intervals = (unsigned int[]) { + 666666, + 10000000, + 50000000, + }, + }, { + .index = 2, + .width = 1280, + .height = 720, + .num_intervals = 1, + .intervals = (unsigned int[]) { + 50000000, + }, + }, + }, + }, + }, + }, +}; + +static int parse_legacy_g_webcam(const char *udc, + struct uvc_function_config *fc) +{ + void *memdup(const void *src, size_t size) + { + void *dst; + + dst = malloc(size); + if (!dst) + return NULL; + memcpy(dst, src, size); + return dst; + } + + unsigned int i, j; + size_t size; + + *fc = g_webcam_config; + + /* + * We need to duplicate all sub-structures as the + * configfs_free_uvc_function() function expects them to be dynamically + * allocated. + */ + size = sizeof *fc->streaming.formats * fc->streaming.num_formats; + fc->streaming.formats = memdup(fc->streaming.formats, size); + + for (i = 0; i < fc->streaming.num_formats; ++i) { + struct uvc_function_config_format *format = + &fc->streaming.formats[i]; + + size = sizeof *format->frames * format->num_frames; + format->frames = memdup(format->frames, size); + + for (j = 0; j < format->num_frames; ++j) { + struct uvc_function_config_frame *frame = + &format->frames[j]; + + size = sizeof *frame->intervals * frame->num_intervals; + frame->intervals = memdup(frame->intervals, size); + } + } + + fc->video = udc_find_video_device(udc, NULL); + + return fc->video ? 0 : -ENODEV; +} + +/* ----------------------------------------------------------------------------- + * ConfigFS support + */ + +/* + * configfs_find_uvc_function - Find the ConfigFS full path for a UVC function + * @function: The UVC function name + * + * Return a pointer to a newly allocated string containing the full ConfigFS + * path to the function if the function is found. Otherwise return NULL. The + * returned pointer must be freed by the caller with a call to free(). + */ +static char *configfs_find_uvc_function(const char *function) +{ + const char *target = function ? function : "*"; + const char *root; + char *func_path; + char *path; + + /* + * The function description can be provided as a path from the + * usb_gadget root "g1/functions/uvc.0", or if there is no ambiguity + * over the gadget name, a shortcut "uvc.0" can be provided. + */ + if (!strchr(target, '/')) + root = "/sys/kernel/config/usb_gadget/*/functions"; + else + root = "/sys/kernel/config/usb_gadget"; + + path = path_join(root, target); + + func_path = path_glob_first_match(path); + free(path); + + return func_path; +} + +/* + * 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->video); + + 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 configfs_parse_streaming_format(const char *path, + struct uvc_function_config_format *format) +{ + 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; + } + + 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; + } + + struct dirent **entries; + unsigned int i; + int n_entries; + int ret; + + ret = attribute_read_uint(path, "bFormatIndex", &format->index); + if (ret < 0) + return ret; + + ret = attribute_read(path, "guidFormat", format->guid, + sizeof(format->guid)); + if (ret < 0) + return ret; + if (ret != 16) + 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 configfs_parse_streaming_header(const char *path, + struct uvc_function_config_streaming *cfg) +{ + 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; + } + + 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; + } + + 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 configfs_parse_streaming(const char *path, + struct uvc_function_config_streaming *cfg) +{ + int link_filter(const struct dirent *ent) + { + /* Accept all links. */ + return ent->d_type == DT_LNK; + } + + 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; +} + +/* + * configfs_parse_uvc_function - Parse a UVC function configuration in ConfigFS + * @function: The function name + * + * This function locates and parse the configuration of a UVC function in + * ConfigFS as specified by the @function name argument. The function name can + * be fully qualified with a gadget name (e.g. "g%u/functions/uvc.%u"), or as a + * shortcut can be an unqualified function name (e.g. "uvc.%u"). When the + * function name is unqualified, the first function matching the name in any + * UDC will be returned. + * + * Return a pointer to a newly allocated UVC function configuration structure + * that contains configuration parameters for the function, if the function is + * found. Otherwise return NULL. The returned pointer must be freed by the + * caller with a call to free(). + */ +struct uvc_function_config *configfs_parse_uvc_function(const char *function) +{ + struct uvc_function_config *fc; + char *fpath; + 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(function); + if (!fpath) { + /* + * If the function can't be found attempt legacy parsing to + * support the g_webcam gadget. The function parameter contains + * a UDC name in that case. + */ + ret = parse_legacy_g_webcam(function, fc); + if (ret) { + configfs_free_uvc_function(fc); + fc = NULL; + } + + return fc; + } + + /* + * Parse the function configuration. Remove the gadget name qualifier + * from the function name, if any. + */ + if (function) + function = basename(function); + + fc->udc = attribute_read_str(fpath, "../../UDC"); + fc->video = udc_find_video_device(fc->udc, function); + if (!fc->video) { + ret = -ENODEV; + goto done; + } + + ret = configfs_parse_uvc(fpath, fc); + +done: + if (ret) { + configfs_free_uvc_function(fc); + fc = NULL; + } + + free(fpath); + + return fc; +} -- cgit v1.2.3