Initial commit

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)

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},
+            );
+        }
+    );
+};

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;
+        }
+    );
+};

include/libmegapixels.h

@@ -0,0 +1,86 @@
+#ifndef LIBMEGAPIXELS_HEADER
+#define LIBMEGAPIXELS_HEADER
+
+#include <stdint.h>
+
+#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

src/findconfig.c

@@ -0,0 +1,59 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <libmegapixels.h>
+
+#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;
+		}
+	}
+}

src/log.c

@@ -0,0 +1,13 @@
+#include <stdarg.h>
+#include <stdio.h>
+#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);
+}

src/log.h

@@ -0,0 +1,4 @@
+#pragma once
+
+void
+log_error(const char *fmt, ...);

src/mode.c

@@ -0,0 +1,91 @@
+#include <stdint.h>
+#include <linux/v4l2-subdev.h>
+#include <strings.h>
+#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;
+}

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);

src/parse.c

@@ -0,0 +1,334 @@
+#include <stdio.h>
+#include <libconfig.h>
+#include <malloc.h>
+#include <string.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <errno.h>
+#include <linux/media.h>
+#include <stdint.h>
+#include <limits.h>
+#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;
+}

src/pipeline.c

@@ -0,0 +1,201 @@
+#include <fcntl.h>
+#include <errno.h>
+#include <string.h>
+#include <linux/media.h>
+#include <linux/v4l2-subdev.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#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;
+}

src/util.c

@@ -0,0 +1,13 @@
+#include <sys/ioctl.h>
+#include <errno.h>
+#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;
+}

src/util.h

@@ -0,0 +1,4 @@
+#pragma once
+
+int
+xioctl(int fd, int request, void *arg);

util/findconfig.c

@@ -0,0 +1,40 @@
+#include <libmegapixels.h>
+#include <stdio.h>
+#include <limits.h>
+
+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;
+}

util/getframe.c

@@ -0,0 +1,152 @@
+#include <libmegapixels.h>
+#include <stdio.h>
+#include <limits.h>
+#include <string.h>
+#include <linux/videodev2.h>
+#include <sys/ioctl.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <errno.h>
+#include <sys/mman.h>
+
+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;
+}