libmegapixels/src/pipeline.c

#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 <assert.h>
#include "libmegapixels.h"
#include "log.h"
#include "util.h"
#include "mode.h"

int
setup_link(libmegapixels_camera *camera, uint32_t source_entity_id, uint32_t sink_entity_id,
	uint16_t source_index, uint16_t sink_index, 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 = source_index;
	link.sink.entity = sink_entity_id;
	link.sink.index = sink_index;

	if (xioctl(camera->media_fd, MEDIA_IOC_SETUP_LINK, &link) == -1) {
		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;
				}
			} else if (cmd->type == LIBMEGAPIXELS_CMD_MODE || cmd->type == LIBMEGAPIXELS_CMD_CROP ||
				cmd->type == LIBMEGAPIXELS_CMD_INTERVAL) {
				int found = 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++;
						break;
					}
				}
				if (found != 1) {
					log_error("Could not find entity '%s'\n", cmd->entity_from);
				}
			}
		}
	}

	for (int i = 0; i < topology.num_links; i++) {
		if (links[i].flags & MEDIA_LNK_FL_ENABLED && !(links[i].flags & MEDIA_LNK_FL_IMMUTABLE)) {
			uint32_t source_entity = 0, sink_entity = 0;
			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, 0, 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;
	}

	if (camera->media_path) {
		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;
		}
	}

	if (camera->sensor_path) {
		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;
	}

	// If this is an UVC camera the sensor _is_ the video device
	if (camera->sensor_fd == 0) {
		camera->sensor_fd = camera->video_fd;
	}

	if (camera->media_fd > 0) {
		int ret = load_entity_ids(camera);
		if (ret < 0) {
			return ret;
		}
	}

	return 0;
}

void
libmegapixels_close(libmegapixels_camera *camera)
{
	int uvc = 0;
	if (camera->sensor_fd != 0 && camera->sensor_fd == camera->video_fd) {
		uvc = 1;
	}

	if (camera->media_fd != 0) {
		close(camera->media_fd);
		camera->media_fd = 0;
	}
	if (camera->sensor_fd != 0) {
		close(camera->sensor_fd);
		camera->sensor_fd = 0;
		if (uvc) {
			camera->video_fd = 0;
		}
	}
	if (camera->video_fd != 0) {
		close(camera->video_fd);
		camera->video_fd = 0;
	}

	for (int i = 0; i < camera->num_handles; i++) {
		if (camera->handles[i]->fd != 0) {
			close(camera->handles[i]->fd);
			camera->handles[i]->fd = 0;
		}
	}
}

unsigned int
libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode, struct v4l2_format *format)
{
	assert(camera->video_fd != 0);
	assert(camera->sensor_fd != 0);
	char modename[32] = {0};
	mode_snprintf(modename, 31, mode);
	log_debug("Setting '%s' mode to [%s]\n", camera->name, modename);
	for (int i = 0; i < mode->num_cmds; i++) {
		libmegapixels_cmd *cmd = mode->cmds[i];
		struct v4l2_subdev_format subdev_fmt = {};
		struct v4l2_subdev_crop subdev_crop = {};
		struct v4l2_subdev_frame_interval subdev_ival = {};
		int found = 0;
		libmegapixels_subdev *sd;

		switch (cmd->type) {
			case LIBMEGAPIXELS_CMD_LINK:
				log_debug("  Link %s:%d -> %s:%d\n", cmd->entity_from, cmd->pad_from, cmd->entity_to, cmd->pad_to);
				if (setup_link(camera, cmd->entity_from_id, cmd->entity_to_id, cmd->pad_from, cmd->pad_to, 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:
				log_debug("  Mode %s:%d [%dx%d %s]\n", cmd->entity_from, cmd->pad_from, cmd->width, cmd->height,
					libmegapixels_format_name(cmd->format));

				found = 0;
				for (int h = 0; h < camera->num_handles; h++) {
					if (camera->handles[h]->entity_id == cmd->entity_from_id) {
						sd = camera->handles[h];
						found++;
					}
				}
				if (found != 1) {
					log_error("Could not find handle for entity\n");
					break;
				}

				if (sd->fd == 0) {
					sd->fd = open(sd->path, O_RDWR);
					if (sd->fd < 0) {
						log_error("Could not open %s\n", sd->path);
						break;
					}
				}

				subdev_fmt.pad = cmd->pad_from;
				subdev_fmt.which = V4L2_SUBDEV_FORMAT_TRY;
				subdev_fmt.format.width = cmd->width;
				subdev_fmt.format.height = cmd->height;
				subdev_fmt.format.code = libmegapixels_format_to_media_busfmt(cmd->format);
				subdev_fmt.format.field = V4L2_FIELD_ANY;

				if (xioctl(sd->fd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt) == -1) {
					log_error("Could not try mode on %s:%d: %s\n", cmd->entity_from, cmd->pad_from, strerror(errno));
				}

				if (subdev_fmt.format.width != cmd->width || subdev_fmt.format.height != cmd->height) {
					log_error("Driver rejected resolution %dx%d, changed to %dx%d\n", cmd->width, cmd->height,
						subdev_fmt.format.width, subdev_fmt.format.height);
					break;
				}
				if (subdev_fmt.format.code != libmegapixels_format_to_media_busfmt(cmd->format)) {
					log_error("Driver rejected pixfmt try: %d\n", subdev_fmt.format.code);
					break;
				}
				subdev_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
				if (xioctl(sd->fd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt) == -1) {
					log_error("Could not set mode on %s:%d: %s\n", cmd->entity_from, cmd->pad_from, strerror(errno));
				}

				// There seem to be drivers that don't reject resolutions in V4L2_SUBDEV_FORMAT_TRY mode but
				// do silently change the resolution when doing the V4L2_SUBDEV_FORMAT_ACTIVE bit. So check
				// again since V4L2 is the wild west.
				if (subdev_fmt.format.width != cmd->width || subdev_fmt.format.height != cmd->height) {
					log_error("Driver rejected resolution %dx%d, changed to %dx%d\n", cmd->width, cmd->height,
						subdev_fmt.format.width, subdev_fmt.format.height);
					break;
				}
				if (subdev_fmt.format.code != libmegapixels_format_to_media_busfmt(cmd->format)) {
					log_error("Driver rejected pixfmt: %d\n", subdev_fmt.format.code);
					break;
				}
				break;
			case LIBMEGAPIXELS_CMD_INTERVAL:
				subdev_ival.pad = cmd->pad_from;
				struct v4l2_fract ival;
				ival.numerator = 1;
				ival.denominator = cmd->rate;
				subdev_ival.interval = ival;
				log_debug("  Rate %s:%d [%d]\n", cmd->entity_from, cmd->pad_from, cmd->rate);

				found = 0;
				for (int h = 0; h < camera->num_handles; h++) {
					if (camera->handles[h]->entity_id == cmd->entity_from_id) {
						sd = camera->handles[h];
						found++;
					}
				}
				if (found != 1) {
					log_error("Could not find handle for entity\n");
					break;
				}

				if (sd->fd == 0) {
					sd->fd = open(sd->path, O_RDWR);
					if (sd->fd < 0) {
						log_error("Could not open %s\n", sd->path);
						break;
					}
				}

				if (xioctl(sd->fd, VIDIOC_SUBDEV_S_FRAME_INTERVAL, &subdev_ival) == -1) {
					log_error("Could not set rate on %s:%d: %s\n", cmd->entity_from, cmd->pad_from, strerror(errno));
				}
				break;
			case LIBMEGAPIXELS_CMD_CROP:
				subdev_crop.pad = cmd->pad_from;
				subdev_crop.which = V4L2_SUBDEV_FORMAT_ACTIVE;
				subdev_crop.rect.top = cmd->top;
				subdev_crop.rect.left = cmd->left;
				subdev_crop.rect.width = cmd->width;
				subdev_crop.rect.height = cmd->height;

				log_debug("  Crop %s:%d [%dx%d+%d+%d]\n", cmd->entity_from, cmd->pad_from, cmd->width, cmd->height,
					cmd->top, cmd->left);

				found = 0;
				for (int h = 0; h < camera->num_handles; h++) {
					if (camera->handles[h]->entity_id == cmd->entity_from_id) {
						sd = camera->handles[h];
						found++;
					}
				}
				if (found != 1) {
					log_error("Could not find handle for entity\n");
					break;
				}

				if (sd->fd == 0) {
					sd->fd = open(sd->path, O_RDWR);
					if (sd->fd < 0) {
						log_error("Could not open %s\n", sd->path);
						break;
					}
				}

				if (xioctl(sd->fd, VIDIOC_SUBDEV_S_CROP, &subdev_crop) == -1) {
					log_error("Could not set crop on entity: %s\n", strerror(errno));
				}
				break;
		}
	}

	struct v4l2_capability cap;
	if (xioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) == -1) {
		log_error("Could not query %s: %s\n", camera->video_path, strerror(errno));
		return 0;
	}
	if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
		format->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		format->fmt.pix_mp.width = mode->width;
		format->fmt.pix_mp.height = mode->height;
		format->fmt.pix_mp.pixelformat = mode->v4l_pixfmt;
		format->fmt.pix_mp.field = V4L2_FIELD_ANY;
	} else {
		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 %s: %s\n", camera->video_path, strerror(errno));
		return 0;
	}
	if (xioctl(camera->video_fd, VIDIOC_G_FMT, format) == -1) {
		log_error("Could not get mode of %s: %s\n", camera->video_path, strerror(errno));
		return 0;
	}
	if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
		if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
			log_error("Capture driver changed capture type\n");
			return 0;
		}
		if (format->fmt.pix_mp.pixelformat != mode->v4l_pixfmt) {
			log_error("Capture driver changed pixfmt to %c%c%c%c\n",
				format->fmt.pix_mp.pixelformat & 0xff,
				format->fmt.pix_mp.pixelformat >> 8 & 0xff,
				format->fmt.pix_mp.pixelformat >> 16 & 0xff,
				format->fmt.pix_mp.pixelformat >> 24 & 0xff);
			return 0;
		}
	} else {
		if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
			log_error("Capture driver changed capture type\n");
			return 0;
		}
		if (format->fmt.pix.pixelformat != mode->v4l_pixfmt) {
			log_error("Capture driver changed pixfmt to %c%c%c%c\n",
				format->fmt.pix.pixelformat & 0xff,
				format->fmt.pix.pixelformat >> 8 & 0xff,
				format->fmt.pix.pixelformat >> 16 & 0xff,
				format->fmt.pix.pixelformat >> 24 & 0xff);
			return 0;
		}
	}
	camera->current_mode = mode;

	return 1;
}