Initial commit

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