commit afbc15a68af5ae9160df242d1cba68f5fa08f76d Author: Martijn Braam Date: Sat Jul 8 18:30:07 2023 +0200 Initial commit diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a719b02 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.25) +project(libmegapixels C) + +set(LIBRARY_VERSION_MAJOR 0) +set(LIBRARY_VERSION_STRING 0.1) + +set(CMAKE_C_STANDARD 23) +set(CMAKE_C_VISIBILITY_PRESET hidden) + +add_library(megapixels SHARED include/libmegapixels.h src/findconfig.c src/parse.c src/mode.c src/pipeline.c src/log.c src/util.c) +set_target_properties(megapixels PROPERTIES + VERSION ${LIBRARY_VERSION_STRING} + SOVERSION ${LIBRARY_VERSION_MAJOR} + PUBLIC_HEADER include/libmegapixels.h) +target_include_directories(megapixels PUBLIC include) +target_link_libraries(megapixels "config") + + +add_executable(findconfig util/findconfig.c) +target_include_directories(findconfig PUBLIC include) +target_link_libraries(findconfig PUBLIC megapixels) + +add_executable(getframe util/getframe.c) +target_include_directories(getframe PUBLIC include) +target_link_libraries(getframe PUBLIC megapixels) \ No newline at end of file diff --git a/config/pine64,pinephone.conf b/config/pine64,pinephone.conf new file mode 100644 index 0000000..986b23b --- /dev/null +++ b/config/pine64,pinephone.conf @@ -0,0 +1,62 @@ +Version = 1; +Make: "PINE64"; +Model: "PinePhone"; + +Rear: { + SensorDriver: "ov5640"; + BridgeDriver: "sun6i-csi"; + FlashPath: "/sys/class/leds/white:flash"; + IsoMin: 100; + IsoMax: 64000; + + Modes: ( + { + Width: 2592; + Height: 1944; + Rate: 15; + Format: "BGGR8"; + Rotate: 270; + FocalLength: 3.33; + FNumber: 3.0; + + Pipeline: ( + {Type: "Link", From: "ov5640", FromPad: 0, To: "sun6i-csi", ToPad: 0}, + {Type: "Mode", Entity: "ov5640", Width: 2592, Height: 1944, Format: "BGGR8"}, + ); + }, + { + Width: 1280; + Height: 720; + Rate: 30; + Format: "BGGR8"; + FocalLength: 3.33; + FNumber: 3.0; + + Pipeline: ( + {Type: "Link", From: "ov5640", FromPad: 0, To: "sun6i-csi", ToPad: 0}, + ); + + } + ); +}; + +Front: { + SensorDriver: "gc2145"; + BridgeDriver: "sun6i-csi"; + FlashDisplay: true; + + Modes: ( + { + Width: 1280; + Height: 960; + Rate: 60; + Format: "BGGR8"; + Rotate: 90; + Mirror: true; + + Pipeline: ( + {Type: "Link", From: "gc2145", FromPad: 0, To: "sun6i-csi", ToPad: 0}, + ); + } + ); +}; \ No newline at end of file diff --git a/config/uvc.conf b/config/uvc.conf new file mode 100644 index 0000000..38e6f48 --- /dev/null +++ b/config/uvc.conf @@ -0,0 +1,31 @@ +Version = 1; +Make: "UVC"; +Model: "UVC"; + +Rear: { + SensorDriver: "ov5640"; + BridgeDriver: "uvcvideo"; + FlashPath: "/sys/class/leds/white:flash"; + IsoMin: 100; + IsoMax: 64000; + + Modes: ( + { + Width: 2592; + Height: 1944; + Rate: 15; + Format: "BGGR8"; + Rotate: 270; + FocalLength: 3.33; + FNumber: 3.0; + }, + { + Width: 1280; + Height: 720; + Rate: 30; + Format: "BGGR8"; + FocalLength: 3.33; + FNumber: 3.0; + } + ); +}; \ No newline at end of file diff --git a/include/libmegapixels.h b/include/libmegapixels.h new file mode 100644 index 0000000..1eae2fe --- /dev/null +++ b/include/libmegapixels.h @@ -0,0 +1,86 @@ +#ifndef LIBMEGAPIXELS_HEADER +#define LIBMEGAPIXELS_HEADER + +#include + +#define EXPORT __attribute__((__visibility__("default"))) + + +EXPORT int +libmegapixels_find_config(char *configfile); + +#define LIBMEGAPIXELS_CMD_LINK 1 +#define LIBMEGAPIXELS_CMD_MODE 2 + +struct _lmp_cmd { + int type; + const char *entity_from; + const char *entity_to; + int pad_from; + int pad_to; + int width; + int height; + uint32_t mode; + + uint32_t entity_from_id; + int pad_from_id; + uint32_t entity_to_id; + int pad_to_id; +}; +typedef struct _lmp_cmd libmegapixels_cmd; + +struct _lmp_mode { + int width; + int height; + int rate; + int format; + int rotation; + double focal_length; + double f_number; + + uint32_t v4l_pixfmt; + uint32_t media_busfmt; + + int num_cmds; + libmegapixels_cmd *cmds; +}; +typedef struct _lmp_mode libmegapixels_mode; + +struct _lmp_camera { + char *name; + char *sensor_name; + char *bridge_name; + + char *media_path; + char *sensor_path; + char *video_path; + int media_fd; + int sensor_fd; + int video_fd; + + int num_modes; + libmegapixels_mode *modes; +}; +typedef struct _lmp_camera libmegapixels_camera; + +struct _lmp_device_config { + char *path; + char *make; + char *model; + int count; + libmegapixels_camera **cameras; +}; +typedef struct _lmp_device_config libmegapixels_devconfig; + + +EXPORT int +libmegapixels_load_file(libmegapixels_devconfig **config, const char *file); + + +EXPORT int +libmegapixels_open(libmegapixels_camera *camera); + +EXPORT unsigned int +libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode); + +#endif \ No newline at end of file diff --git a/src/findconfig.c b/src/findconfig.c new file mode 100644 index 0000000..d3b318d --- /dev/null +++ b/src/findconfig.c @@ -0,0 +1,59 @@ +#include +#include +#include + +#ifndef SYSCONFDIR +#define SYSCONFDIR "/etc" +#endif +#ifndef DATADIR +#define DATADIR "/usr/share" +#endif + +static int +find_device_by_model(char *conffile, char *model) +{ + // Check config/%model.conf in the current working directory + sprintf(conffile, "config/%s.conf", model); + if (access(conffile, F_OK) != -1) { + return 1; + } + + // Check user overridden /etc/megapixels/config/%model.conf + sprintf(conffile, "%s/megapixels/config/%s.conf", SYSCONFDIR, model); + if (access(conffile, F_OK) != -1) { + return 1; + } + + // Check packaged /usr/share/megapixels/config/%model.conf + sprintf(conffile, "%s/megapixels/config/%s.conf", DATADIR, model); + if (access(conffile, F_OK) != -1) { + return 1; + } + return 0; +} + +int +libmegapixels_find_config(char *configfile) +{ + char model[512]; + FILE *fp; + + if (access("/proc/device-tree/compatible", F_OK) == -1) { + return 0; + } + fp = fopen("/proc/device-tree/compatible", "r"); + char *modelptr = model; + while (1) { + int c = fgetc(fp); + if (c == EOF) { + return 0; + } + *(modelptr++) = (char) c; + if (c == 0) { + if (find_device_by_model(configfile, model)) { + return 1; + } + modelptr = model; + } + } +} \ No newline at end of file diff --git a/src/log.c b/src/log.c new file mode 100644 index 0000000..dc5847f --- /dev/null +++ b/src/log.c @@ -0,0 +1,13 @@ +#include +#include +#include "log.h" + +void +log_error(const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + fprintf(stderr, "[libmegapixels] "); + vfprintf(stderr, fmt, args); + va_end(args); +} \ No newline at end of file diff --git a/src/log.h b/src/log.h new file mode 100644 index 0000000..fe34e83 --- /dev/null +++ b/src/log.h @@ -0,0 +1,4 @@ +#pragma once + +void +log_error(const char *fmt, ...); \ No newline at end of file diff --git a/src/mode.c b/src/mode.c new file mode 100644 index 0000000..e582462 --- /dev/null +++ b/src/mode.c @@ -0,0 +1,91 @@ +#include +#include +#include +#include "mode.h" + + +struct libmegapixels_modename { + char *name; + uint32_t v4l_pixel_format; + uint32_t media_bus_format; +}; + +static struct libmegapixels_modename mode_lut[] = { + { + .name = "unsupported", + .v4l_pixel_format = 0, + .media_bus_format = 0, + }, + { + .name = "BGGR8", + .v4l_pixel_format = V4L2_PIX_FMT_SBGGR8, + .media_bus_format = MEDIA_BUS_FMT_SBGGR8_1X8, + }, + { + .name = "GBRG8", + .v4l_pixel_format = V4L2_PIX_FMT_SGBRG8, + .media_bus_format = MEDIA_BUS_FMT_SGBRG8_1X8, + }, + { + .name = "GRBG8", + .v4l_pixel_format = V4L2_PIX_FMT_SGRBG8, + .media_bus_format = MEDIA_BUS_FMT_SGRBG8_1X8, + }, + { + .name = "RGGB8", + .v4l_pixel_format = V4L2_PIX_FMT_SRGGB8, + .media_bus_format = MEDIA_BUS_FMT_SRGGB8_1X8, + }, + { + .name = "BGGR10P", + .v4l_pixel_format = V4L2_PIX_FMT_SBGGR10P, + .media_bus_format = MEDIA_BUS_FMT_SBGGR10_1X10, + }, + { + .name = "GBRG10P", + .v4l_pixel_format = V4L2_PIX_FMT_SGBRG10P, + .media_bus_format = MEDIA_BUS_FMT_SGBRG10_1X10, + }, + { + .name = "GRBG10P", + .v4l_pixel_format = V4L2_PIX_FMT_SGRBG10P, + .media_bus_format = MEDIA_BUS_FMT_SGRBG10_1X10, + }, + { + .name = "RGGB10P", + .v4l_pixel_format = V4L2_PIX_FMT_SRGGB10P, + .media_bus_format = MEDIA_BUS_FMT_SRGGB10_1X10, + }, + { + .name = "UYVY", + .v4l_pixel_format = V4L2_PIX_FMT_UYVY, + .media_bus_format = MEDIA_BUS_FMT_UYVY8_2X8, + }, + { + .name = "YUYV", + .v4l_pixel_format = V4L2_PIX_FMT_YUYV, + .media_bus_format = MEDIA_BUS_FMT_YUYV8_2X8, + }, +}; + +uint32_t +format_name_to_v4l_pixfmt(const char *name) +{ + for (int i = 0; i < sizeof(mode_lut); i++) { + if (strcasecmp(mode_lut[i].name, name) == 0) { + return mode_lut[i].v4l_pixel_format; + } + } + return 0; +} + +uint32_t +format_name_to_media_busfmt(const char *name) +{ + for (int i = 0; i < sizeof(mode_lut); i++) { + if (strcasecmp(mode_lut[i].name, name) == 0) { + return mode_lut[i].media_bus_format; + } + } + return 0; +} \ No newline at end of file diff --git a/src/mode.h b/src/mode.h new file mode 100644 index 0000000..25bec38 --- /dev/null +++ b/src/mode.h @@ -0,0 +1,7 @@ +#pragma once + +uint32_t +format_name_to_v4l_pixfmt(const char *name); + +uint32_t +format_name_to_media_busfmt(const char *name); diff --git a/src/parse.c b/src/parse.c new file mode 100644 index 0000000..601cdcc --- /dev/null +++ b/src/parse.c @@ -0,0 +1,334 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "libmegapixels.h" +#include "mode.h" +#include "util.h" + +char * +find_path_for_devnode(struct media_v2_intf_devnode devnode) +{ + char uevent_path[PATH_MAX]; + snprintf(uevent_path, PATH_MAX, "/sys/dev/char/%d:%d/uevent", devnode.major, devnode.minor); + + FILE *fp = fopen(uevent_path, "r"); + if (!fp) { + return NULL; + } + + char line[PATH_MAX]; + char path[PATH_MAX]; + while (fgets(line, PATH_MAX, fp)) { + if (strncmp(line, "DEVNAME=", 8) == 0) { + // Drop newline + unsigned long length = strlen(line); + if (line[length - 1] == '\n') + line[length - 1] = '\0'; + + snprintf(path, length, "/dev/%s", line + 8); + return strdup(path); + } + } +} + +int +find_media_node(libmegapixels_camera *camera, const char *media_name, const char *sensor_name) +{ + struct dirent *dir; + DIR *d = opendir("/dev"); + while ((dir = readdir(d)) != NULL) { + if (strncmp(dir->d_name, "media", 5) == 0) { + char path[PATH_MAX]; + snprintf(path, PATH_MAX, "/dev/%s", dir->d_name); + + int media_fd = open(path, O_RDWR); + if (media_fd == -1) { + fprintf(stderr, "Could not open %s\n", path); + continue; + } + + struct media_device_info mdi; + if (xioctl(media_fd, MEDIA_IOC_DEVICE_INFO, &mdi) == -1) { + fprintf(stderr, "Could not MDI\n"); + close(media_fd); + } + if (strcmp(mdi.driver, media_name) != 0 && strcmp(mdi.model, media_name) != 0) { + close(media_fd); + continue; + } + + // This media device matches on model or driver, scan the entities for the sensor + struct media_v2_topology topology = {0}; + if (xioctl(media_fd, MEDIA_IOC_G_TOPOLOGY, &topology) == -1 || + topology.num_entities == 0) { + close(media_fd); + continue; + } + + struct media_v2_entity *entities = calloc(topology.num_entities, sizeof(struct media_v2_entity)); + struct media_v2_interface *interfaces = calloc(topology.num_interfaces, sizeof(struct media_v2_interface)); + struct media_v2_pad *pads = calloc(topology.num_pads, sizeof(struct media_v2_pad)); + struct media_v2_link *links = calloc(topology.num_links, sizeof(struct media_v2_link)); + + topology.ptr_entities = (uint64_t) entities; + topology.ptr_interfaces = (uint64_t) interfaces; + topology.ptr_pads = (uint64_t) pads; + topology.ptr_links = (uint64_t) links; + + if (xioctl(media_fd, MEDIA_IOC_G_TOPOLOGY, &topology) == -1) { + close(media_fd); + continue; + } + + // Find the sensor + unsigned long len = strlen(sensor_name); + int found = 0; + for (int i = 0; i < topology.num_entities; i++) { + if (strncmp(entities[i].name, sensor_name, len) == 0) { + found++; + for (int j = 0; j < topology.num_links; j++) { + if (links[j].sink_id != entities[i].id) { + continue; + } + for (int k = 0; k < topology.num_interfaces; k++) { + if (interfaces[k].id != links[j].source_id) { + continue; + } + + camera->sensor_path = find_path_for_devnode(interfaces[k].devnode); + break; + } + } + break; + } + } + + // Find the bridge + for (int i = 0; i < topology.num_entities; i++) { + if (entities[i].function == MEDIA_ENT_F_IO_V4L) { + found++; + for (int j = 0; j < topology.num_links; j++) { + if (links[j].sink_id != entities[i].id) { + continue; + } + for (int k = 0; k < topology.num_interfaces; k++) { + if (interfaces[k].id != links[j].source_id) { + continue; + } + + camera->video_path = find_path_for_devnode(interfaces[k].devnode); + break; + } + } + // TODO: find /dev path for this entity + break; + } + } + + close(media_fd); + if (found == 2) { + camera->media_path = strdup(path); + return 1; + } + } + + } + closedir(d); + return -1; +} + +int +load_camera(libmegapixels_devconfig *config, config_t *cfg, const char *name) +{ + const char *sensor_driver, *bridge_driver; + config_setting_t *root = config_lookup(cfg, name); + if (!config_setting_lookup_string(root, "SensorDriver", &sensor_driver)) { + return -1; + } + if (!config_setting_lookup_string(root, "BridgeDriver", &bridge_driver)) { + return -1; + } + libmegapixels_camera *camera; + camera = malloc(sizeof(libmegapixels_camera)); + int res = find_media_node(camera, bridge_driver, sensor_driver); + if (res < 0) { + free(camera); + return -1; + } + + camera->name = strdup(name); + camera->sensor_name = strdup(sensor_driver); + camera->bridge_name = strdup(bridge_driver); + config->cameras = realloc(config->cameras, (config->count + 1) * sizeof(config->cameras)); + if (config->cameras == NULL) { + return -1; + } + config->cameras[config->count++] = camera; + + config_setting_t *modes = config_setting_lookup(root, "Modes"); + config_setting_t *mode; + + int num_modes = config_setting_length(modes); + camera->modes = malloc(num_modes * sizeof(libmegapixels_mode)); + camera->num_modes = num_modes; + int n = 0; + while (1) { + mode = config_setting_get_elem(modes, n); + if (mode == NULL) { + break; + } + if (!config_setting_lookup_int(mode, "Width", &(camera->modes[n].width))) { + return -1; + } + if (!config_setting_lookup_int(mode, "Height", &(camera->modes[n].height))) { + return -1; + } + if (!config_setting_lookup_int(mode, "Rate", &(camera->modes[n].rate))) { + return -1; + } + + const char *fmt; + config_setting_lookup_string(mode, "Format", &fmt); + camera->modes[n].v4l_pixfmt = format_name_to_v4l_pixfmt(fmt); + camera->modes[n].media_busfmt = format_name_to_media_busfmt(fmt); + + + if (!config_setting_lookup_int(mode, "Rotate", &(camera->modes[n].rotation))) { + camera->modes[n].rotation = 0; + } + + if (!config_setting_lookup_float(mode, "FocalLength", &(camera->modes[n].focal_length))) { + camera->modes[n].focal_length = 0.0f; + } + if (!config_setting_lookup_float(mode, "FNumber", &(camera->modes[n].f_number))) { + camera->modes[n].f_number = 0.0f; + } + + config_setting_t *cmds = config_setting_lookup(mode, "Pipeline"); + config_setting_t *cmd; + + if (cmds == NULL) { + fprintf(stderr, "Missing pipeline\n"); + n++; + continue; + } + + int num_cmds = config_setting_length(cmds); + camera->modes[n].cmds = malloc(num_cmds * sizeof(libmegapixels_cmd)); + camera->modes[n].num_cmds = num_cmds; + int m = 0; + while (1) { + cmd = config_setting_get_elem(cmds, m); + if (cmd == NULL) { + break; + } + libmegapixels_cmd *cur = &(camera->modes[n].cmds[m]); + const char *type; + if (!config_setting_lookup_string(cmd, "Type", &type)) { + break; + } + if (strcmp(type, "Link") == 0) { + camera->modes[n].cmds[m].type = LIBMEGAPIXELS_CMD_LINK; + if (!config_setting_lookup_string(cmd, "From", &cur->entity_from)) { + fprintf(stderr, "Missing From\n"); + break; + } + if (!config_setting_lookup_string(cmd, "To", &cur->entity_to)) { + fprintf(stderr, "Missing To\n"); + break; + } + if (!config_setting_lookup_int(cmd, "FromPad", &cur->pad_from)) { + cur->pad_from = 0; + } + if (!config_setting_lookup_int(cmd, "ToPad", &cur->pad_to)) { + cur->pad_to = 0; + } + } else if (strcmp(type, "Mode") == 0) { + camera->modes[n].cmds[m].type = LIBMEGAPIXELS_CMD_MODE; + if (!config_setting_lookup_string(cmd, "Entity", &cur->entity_from)) { + fprintf(stderr, "Missing entity\n"); + break; + } + if (!config_setting_lookup_int(cmd, "Width", &cur->width)) { + cur->width = camera->modes[n].width; + } + if (!config_setting_lookup_int(cmd, "Height", &cur->height)) { + cur->height = camera->modes[n].height; + } + } + m++; + } + n++; + } + + return 0; +} + +int +libmegapixels_load_file(libmegapixels_devconfig **config, const char *file) +{ + config_t cfg; + config_setting_t *setting, *member; + + *config = malloc(sizeof(libmegapixels_devconfig)); + (*config)->path = strdup(file); + (*config)->count = 0; + (*config)->cameras = malloc(sizeof((*config)->cameras)); + + config_init(&cfg); + if (!config_read_file(&cfg, file)) { + fprintf(stderr, "Could not read %s\n", file); + config_destroy(&cfg); + return 0; + } + + int version = 0; + if (config_lookup_int(&cfg, "Version", &version)) { + if (version != 1) { + fprintf(stderr, "Invalid version %d\n", version); + return 0; + } + } else { + fprintf(stderr, "Missing version\n"); + return 0; + } + + if (!config_lookup_string(&cfg, "Make", (const char **) &(*config)->make)) { + (*config)->make = strdup("Megapixels"); + } + if (!config_lookup_string(&cfg, "Model", (const char **) &(*config)->model)) { + (*config)->model = strdup("Camera"); + } + + setting = config_root_setting(&cfg); + int n = 0; + + while (1) { + member = config_setting_get_elem(setting, n++); + if (member == NULL) { + break; + } + + if (strcmp(member->name, "Version") == 0) { + continue; + } + if (strcmp(member->name, "Make") == 0) { + continue; + } + if (strcmp(member->name, "Model") == 0) { + continue; + } + load_camera(*config, &cfg, member->name); + } + + return 1; +} \ No newline at end of file diff --git a/src/pipeline.c b/src/pipeline.c new file mode 100644 index 0000000..a3d6927 --- /dev/null +++ b/src/pipeline.c @@ -0,0 +1,201 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include "libmegapixels.h" +#include "log.h" +#include "util.h" + +int +setup_link(libmegapixels_camera *camera, uint32_t source_entity_id, uint32_t sink_entity_id, int enabled) +{ + struct media_link_desc link = {}; + link.flags = (enabled > 0) ? MEDIA_LNK_FL_ENABLED : 0; + link.source.entity = source_entity_id; + link.source.index = 0; + link.sink.entity = sink_entity_id; + link.sink.index = 0; + + if (xioctl(camera->media_fd, MEDIA_IOC_SETUP_LINK, &link) == -1) { + log_error("Could not setup link: %s\n", strerror(errno)); + return -1; + } + return 0; +} + +int +load_entity_ids(libmegapixels_camera *camera) +{ + // This media device matches on model or driver, scan the entities for the sensor + struct media_v2_topology topology = {0}; + if (xioctl(camera->media_fd, MEDIA_IOC_G_TOPOLOGY, &topology) == -1 || + topology.num_entities == 0) { + close(camera->media_fd); + return -1; + } + + struct media_v2_entity *entities = calloc(topology.num_entities, sizeof(struct media_v2_entity)); + struct media_v2_interface *interfaces = calloc(topology.num_interfaces, sizeof(struct media_v2_interface)); + struct media_v2_pad *pads = calloc(topology.num_pads, sizeof(struct media_v2_pad)); + struct media_v2_link *links = calloc(topology.num_links, sizeof(struct media_v2_link)); + + topology.ptr_entities = (uint64_t) entities; + topology.ptr_interfaces = (uint64_t) interfaces; + topology.ptr_pads = (uint64_t) pads; + topology.ptr_links = (uint64_t) links; + + if (xioctl(camera->media_fd, MEDIA_IOC_G_TOPOLOGY, &topology) == -1) { + close(camera->media_fd); + return -1; + } + + for (int i = 0; i < camera->num_modes; i++) { + libmegapixels_mode *mode = &camera->modes[i]; + + for (int j = 0; j < mode->num_cmds; j++) { + libmegapixels_cmd *cmd = &mode->cmds[j]; + + if (cmd->type == LIBMEGAPIXELS_CMD_LINK) { + int found_from = 0; + int found_to = 0; + for (int k = 0; k < topology.num_entities; k++) { + if (strncmp(entities[k].name, cmd->entity_from, strlen(cmd->entity_from)) == 0) { + cmd->entity_from_id = entities[k].id; + found_from++; + } + if (strncmp(entities[k].name, cmd->entity_to, strlen(cmd->entity_to)) == 0) { + cmd->entity_to_id = entities[k].id; + found_to++; + } + } + if (found_from != 1) { + log_error("Could not find entity '%s'\n", cmd->entity_from); + return -1; + } + if (found_to != 1) { + log_error("Could not find entity '%s'\n", cmd->entity_to); + return -1; + } + } + } + } + + for (int i = 0; i < topology.num_links; i++) { + log_error("Link %d flags", i); + if (links[i].flags & MEDIA_LNK_FL_ENABLED) { + fprintf(stderr, " [enabled]"); + } + if (links[i].flags & MEDIA_LNK_FL_IMMUTABLE) { + fprintf(stderr, " [immutable]"); + } + if (links[i].flags & MEDIA_LNK_FL_DYNAMIC) { + fprintf(stderr, " [dynamic]"); + } + if (links[i].flags & MEDIA_LNK_FL_INTERFACE_LINK) { + fprintf(stderr, " [interface-link]"); + } + fprintf(stderr, "\n"); + if (links[i].flags & MEDIA_LNK_FL_ENABLED && !(links[i].flags & MEDIA_LNK_FL_IMMUTABLE)) { + uint32_t source_entity, sink_entity; + for (int j = 0; j < topology.num_pads; j++) { + if (pads[j].id == links[i].source_id) { + source_entity = pads[j].entity_id; + } else if (pads[j].id == links[i].sink_id) { + sink_entity = pads[j].entity_id; + } + } + + setup_link(camera, source_entity, sink_entity, 0); + } + } + return 0; +} + +int +libmegapixels_open(libmegapixels_camera *camera) +{ + if (camera->media_fd != 0) { + log_error("Camera already opened\n"); + return -1; + } + if (camera->sensor_fd != 0) { + log_error("Sensor already opened\n"); + return -1; + } + if (camera->video_fd != 0) { + log_error("Bridge already opened\n"); + return -1; + } + + camera->media_fd = open(camera->media_path, O_RDWR); + if (camera->media_fd < 0) { + log_error("Could not open %s: %s\n", camera->media_path, strerror(errno)); + return -1; + } + + camera->sensor_fd = open(camera->sensor_path, O_RDWR); + if (camera->sensor_fd < 0) { + log_error("Could not open %s: %s\n", camera->sensor_path, strerror(errno)); + return -1; + } + + camera->video_fd = open(camera->video_path, O_RDWR); + if (camera->video_fd < 0) { + log_error("Could not open %s: %s\n", camera->video_path, strerror(errno)); + return -1; + } + + int ret = load_entity_ids(camera); + if (ret < 0) { + return ret; + } + + return 0; +} + +unsigned int +libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode) +{ + for (int i = 0; i < mode->num_cmds; i++) { + libmegapixels_cmd *cmd = &mode->cmds[i]; + struct v4l2_subdev_format subdev_fmt = {}; + fprintf(stderr, "Do %d\n", cmd->type); + switch (cmd->type) { + case LIBMEGAPIXELS_CMD_LINK: + if (setup_link(camera, cmd->entity_from_id, cmd->entity_to_id, 1) != 0) { + log_error("Could not link %d -> %d [%s -> %s] \n", cmd->entity_from_id, cmd->entity_to_id, + cmd->entity_from, + cmd->entity_to); + } + break; + case LIBMEGAPIXELS_CMD_MODE: + subdev_fmt.pad = cmd->pad_to; + subdev_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; + subdev_fmt.format.width = cmd->width; + subdev_fmt.format.height = cmd->height; + subdev_fmt.format.code = mode->media_busfmt; + subdev_fmt.format.field = V4L2_FIELD_ANY; + if (xioctl(camera->sensor_fd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt) == -1) { + log_error("Could not set mode on sensor: %s\n", strerror(errno)); + } + break; + } + } + + struct v4l2_format format = {0}; + format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + format.fmt.pix.width = mode->width; + format.fmt.pix.height = mode->height; + format.fmt.pix.pixelformat = mode->v4l_pixfmt; + format.fmt.pix.field = V4L2_FIELD_ANY; + + if (xioctl(camera->video_fd, VIDIOC_S_FMT, &format) == -1) { + log_error("Could not set mode on bridge: %s\n", strerror(errno)); + return 0; + } + return format.fmt.pix.sizeimage; +} \ No newline at end of file diff --git a/src/util.c b/src/util.c new file mode 100644 index 0000000..0d537b6 --- /dev/null +++ b/src/util.c @@ -0,0 +1,13 @@ +#include +#include +#include "util.h" + +int +xioctl(int fd, int request, void *arg) +{ + int r; + do { + r = ioctl(fd, request, arg); + } while (r == -1 && errno == EINTR); + return r; +} \ No newline at end of file diff --git a/src/util.h b/src/util.h new file mode 100644 index 0000000..eddc4b5 --- /dev/null +++ b/src/util.h @@ -0,0 +1,4 @@ +#pragma once + +int +xioctl(int fd, int request, void *arg); \ No newline at end of file diff --git a/util/findconfig.c b/util/findconfig.c new file mode 100644 index 0000000..ee8f18a --- /dev/null +++ b/util/findconfig.c @@ -0,0 +1,40 @@ +#include +#include +#include + +int +main() +{ + char configpath[PATH_MAX]; + int ret = libmegapixels_find_config(configpath); + + + libmegapixels_devconfig *config = {0}; + + if (ret) { + printf("Using config: %s\n", configpath); + libmegapixels_load_file(&config, configpath); + } else { + printf("No config found\n"); + libmegapixels_load_file(&config, "config/uvc.conf"); + } + printf("Device: %s %s\n", config->make, config->model); + printf("Found %d cameras\n", config->count); + + for (int i = 0; i < config->count; i++) { + printf("\n----[ Camera %s (%d) ]----\n", config->cameras[i]->name, i); + printf("Media : %s (%s)\n", config->cameras[i]->bridge_name, config->cameras[i]->media_path); + printf("Sensor: %s (%s)\n", config->cameras[i]->sensor_name, config->cameras[i]->sensor_path); + printf("Bridge: %s\n", config->cameras[i]->video_path); + printf("Modes : "); + for (int j = 0; j < config->cameras[i]->num_modes; j++) { + if (j > 0) { + printf(" "); + } + libmegapixels_mode *mode = &config->cameras[i]->modes[j]; + + printf("%dx%d@%d\n", mode->width, mode->height, mode->rate); + } + } + return 0; +} \ No newline at end of file diff --git a/util/getframe.c b/util/getframe.c new file mode 100644 index 0000000..ac22dbb --- /dev/null +++ b/util/getframe.c @@ -0,0 +1,152 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct buffer { + void *start; + size_t length; +}; +struct buffer *buffers; + +int +xioctl(int fd, int request, void *arg) +{ + int r; + do { + r = ioctl(fd, request, arg); + } while (r == -1 && errno == EINTR); + return r; +} + +int +main() +{ + char configpath[PATH_MAX]; + int ret = libmegapixels_find_config(configpath); + libmegapixels_devconfig *config = {0}; + if (ret) { + printf("Using config: %s\n", configpath); + libmegapixels_load_file(&config, configpath); + } else { + fprintf(stderr, "No config found for this device\n"); + return 1; + } + + if (config->count == 0) { + fprintf(stderr, "No valid camera configuration\n"); + return 1; + } + + libmegapixels_camera *camera = config->cameras[0]; + if (libmegapixels_open(camera) != 0) { + fprintf(stderr, "Could not open default camera\n"); + return 1; + } + + libmegapixels_mode *mode = &camera->modes[0]; + unsigned int frame_size = libmegapixels_select_mode(camera, mode); + if (frame_size == 0) { + fprintf(stderr, "Could not select mode\n"); + return 1; + } + + // Do the reqular V4L2 stuff to get a frame + struct v4l2_capability cap; + if (ioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) != 0) { + fprintf(stderr, "VIDIOC_QUERYCAP failed\n"); + return 1; + } + if (!(cap.capabilities & V4L2_CAP_STREAMING)) { + fprintf(stderr, "Device does not support streaming i/o\n"); + return 1; + } + + struct v4l2_requestbuffers req = {0}; + req.count = 4; + req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + req.memory = V4L2_MEMORY_MMAP; + if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) { + fprintf(stderr, "VIDIOC_REQBUFS failed\n"); + return 1; + } + buffers = calloc(req.count, sizeof(*buffers)); + for (int i = 0; i < req.count; i++) { + struct v4l2_buffer buf = {0}; + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + buf.index = i; + + if (xioctl(camera->video_fd, VIDIOC_QUERYBUF, &buf) == -1) { + fprintf(stderr, "VIDIOC_QUERYBUF failed\n"); + return 1; + } + + buffers[i].length = buf.length; + buffers[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, camera->video_fd, buf.m.offset); + if (buffers[i].start == MAP_FAILED) { + fprintf(stderr, "mmap() failed\n"); + return 1; + } + } + + for (int i = 0; i < req.count; i++) { + struct v4l2_buffer qbuf = {0}; + qbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + qbuf.memory = V4L2_MEMORY_MMAP; + qbuf.index = i; + + if (xioctl(camera->video_fd, VIDIOC_QBUF, &qbuf) == -1) { + fprintf(stderr, "VIDIOC_QBUF failed\n"); + return 1; + } + } + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (xioctl(camera->video_fd, VIDIOC_STREAMON, &type) == -1) { + fprintf(stderr, "VIDIOC_STREAMON failed\n"); + return 1; + } + + + int count = 5; + while (count-- > 0) { + while (1) { + fd_set(fds); + FD_ZERO(&fds); + FD_SET(camera->video_fd, &fds); + + int sr = select(FD_SETSIZE, &fds, NULL, NULL, NULL); + if (sr == -1) { + if (errno == EINTR) { + count++; + continue; + } + fprintf(stderr, "select() failed: %s\n", strerror(errno)); + return 1; + } + + + struct v4l2_buffer buf = {0}; + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + if (xioctl(camera->video_fd, VIDIOC_DQBUF, &buf) == -1) { + fprintf(stderr, "VIDIOC_DQBUF failed\n"); + return 1; + } + fprintf(stderr, "GOT FRAME!\n"); + if (xioctl(camera->video_fd, VIDIOC_QBUF, &buf) == -1) { + fprintf(stderr, "VIDIOC_DQBUF failed\n"); + return 1; + } + break; + } + } + return 0; +} \ No newline at end of file