libmegapixels/src/parse.c

#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 <linux/videodev2.h>
#include "libmegapixels.h"
#include "mode.h"
#include "util.h"
#include "log.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);
		}
	}
	return "";
}

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;
						}
					}
					break;
				}
			}
			camera->num_handles = 0;
			for (int i = 0; i < topology.num_links; i++) {
				if (!(links[i].flags & MEDIA_LNK_FL_INTERFACE_LINK)) {
					continue;
				}
				camera->num_handles++;
			}

			camera->handles = calloc(camera->num_handles, sizeof(libmegapixels_subdev));
			int h = 0;
			for (int i = 0; i < topology.num_links; i++) {
				if (!(links[i].flags & MEDIA_LNK_FL_INTERFACE_LINK)) {
					continue;
				}
				libmegapixels_subdev *sd;
				sd = malloc(sizeof(libmegapixels_subdev));
				camera->handles[h] = sd;
				camera->handles[h]->entity_id = links[i].sink_id;
				camera->handles[h]->fd = 0;

				for (int j = 0; j < topology.num_interfaces; j++) {
					if (links[i].source_id != interfaces[j].id) {
						continue;
					}
					camera->handles[h]->path = find_path_for_devnode(interfaces[j].devnode);
				}
				h++;
			}

			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);
	log_debug("Loading camera '%s'\n", name);
	if (!config_setting_lookup_string(root, "SensorDriver", &sensor_driver)) {
		log_debug("  Section is missing SensorDriver\n");
		return -1;
	}
	if (!config_setting_lookup_string(root, "BridgeDriver", &bridge_driver)) {
		log_debug("  Section is missing BridgeDriver\n");
		return -1;
	}
	libmegapixels_camera *camera;
	camera = malloc(sizeof(libmegapixels_camera));
	camera->sensor_fd = 0;
	camera->media_fd = 0;
	camera->video_fd = 0;

	int res = find_media_node(camera, bridge_driver, sensor_driver);
	if (res < 0) {
		log_debug("  Could not find media node with this sensor\n");
		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;
		}
		libmegapixels_mode *mm = malloc(sizeof(libmegapixels_mode));
		camera->modes[n] = mm;
		if (!config_setting_lookup_int(mode, "Width", &mm->width)) {
			log_error("Missing Width\n");
			return -1;
		}
		if (!config_setting_lookup_int(mode, "Height", &mm->height)) {
			log_error("Missing Height\n");
			return -1;
		}
		if (!config_setting_lookup_int(mode, "Rate", &mm->rate)) {
			log_error("Missing Rate\n");
			return -1;
		}

		const char *fmt;
		config_setting_lookup_string(mode, "Format", &fmt);
		mm->format = libmegapixels_format_name_to_index(fmt);
		if (!mm->format) {
			log_error("Unknown format '%s'\n", fmt);
			return -1;
		}
		mm->v4l_pixfmt = libmegapixels_format_to_v4l_pixfmt(mm->format);
		mm->media_busfmt = libmegapixels_format_to_media_busfmt(mm->format);

		const char *xfer;
		if (config_setting_lookup_string(mode, "Transfer", &xfer)) {
			if (strcmp(xfer, "srgb") == 0) {
				mm->xfer = LIBMEGAPIXELS_XFER_SRGB;
			} else {
				mm->xfer = LIBMEGAPIXELS_XFER_RAW;
			}
		} else {
			mm->xfer = LIBMEGAPIXELS_XFER_RAW;
		}

		if (!config_setting_lookup_int(mode, "Rotate", &mm->rotation)) {
			mm->rotation = 0;
		}

		if (!config_setting_lookup_float(mode, "FocalLength", &mm->focal_length)) {
			mm->focal_length = 0.0f;
		}
		if (!config_setting_lookup_float(mode, "FNumber", &mm->f_number)) {
			mm->f_number = 0.0f;
		}
		char modename[32] = {0};
		mode_snprintf(modename, 31, mm);
		log_debug("  Adding mode [%s]\n", modename);

		config_setting_t *cmds = config_setting_lookup(mode, "Pipeline");
		config_setting_t *cmd;

		if (cmds == NULL) {
			log_error("Mode has no pipeline\n");
			n++;
			continue;
		}

		int num_cmds = config_setting_length(cmds);
		mm->cmds = (libmegapixels_cmd **) calloc(num_cmds, sizeof(libmegapixels_cmd *));
		mm->num_cmds = num_cmds;
		int m = 0;
		int last_width = camera->modes[n]->width;
		int last_height = camera->modes[n]->height;
		int last_format = camera->modes[n]->format;

		while (1) {
			cmd = config_setting_get_elem(cmds, m);
			if (cmd == NULL) {
				break;
			}
			libmegapixels_cmd *cur = calloc(1, sizeof(libmegapixels_cmd));
			mm->cmds[m] = cur;
			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 = last_width;
				}
				last_width = cur->width;
				if (!config_setting_lookup_int(cmd, "Height", &cur->height)) {
					cur->height = last_height;
				}
				last_height = cur->height;
				if (!config_setting_lookup_int(cmd, "Pad", &cur->pad_from)) {
					cur->pad_from = 0;
				}
				const char *newfmt;
				if (config_setting_lookup_string(cmd, "Format", &newfmt)) {
					cur->format = libmegapixels_format_name_to_index(newfmt);
					last_format = cur->format;
				} else {
					cur->format = last_format;
				}
			} else if (strcmp(type, "Rate") == 0) {
				camera->modes[n]->cmds[m]->type = LIBMEGAPIXELS_CMD_INTERVAL;
				if (!config_setting_lookup_string(cmd, "Entity", &cur->entity_from)) {
					fprintf(stderr, "Missing entity\n");
					break;
				}
				if (!config_setting_lookup_int(cmd, "Rate", &cur->rate)) {
					cur->rate = mm->rate;
				}
			} else if (strcmp(type, "Crop") == 0) {
				camera->modes[n]->cmds[m]->type = LIBMEGAPIXELS_CMD_CROP;
				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;
				}
				last_width = cur->width;
				if (!config_setting_lookup_int(cmd, "Height", &cur->height)) {
					cur->height = camera->modes[n]->height;
				}
				last_height = cur->height;
				if (!config_setting_lookup_int(cmd, "Top", &cur->top)) {
					cur->top = 0;
				}
				if (!config_setting_lookup_int(cmd, "Left", &cur->left)) {
					cur->left = 0;
				}
				if (!config_setting_lookup_int(cmd, "Pad", &cur->pad_from)) {
					cur->pad_from = 0;
				}
			}
			m++;
		}
		n++;
	}

	return 0;
}

int
libmegapixels_load_file(libmegapixels_devconfig *config, const char *file)
{
	if (config->loaded_config) {
		log_error("Config already loaded\n");
		return 0;
	}
	log_debug("Load config file %s\n", file);
	config->loaded_config = 1;
	config_t cfg;
	config_setting_t *setting, *member;

	config->path = strdup(file);
	if (config->count == 0) {
		config->cameras = malloc(0);
	}

	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", &config->make)) {
		config->make = strdup("Megapixels");
	}
	if (!config_lookup_string(&cfg, "Model", &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;
}

void
uvc_create_modes(libmegapixels_camera *camera, int fd)
{
	struct v4l2_fmtdesc fmtdesc = {0};
	fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	while (xioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
		fmtdesc.index++;
		libmegapixels_v4l_pixfmt_to_index(fmtdesc.pixelformat);
		int format = libmegapixels_v4l_pixfmt_to_index(fmtdesc.pixelformat);
		if (!format) {
			continue;
		}

		struct v4l2_frmsizeenum framesize = {0};
		framesize.pixel_format = fmtdesc.pixelformat;

		while (xioctl(fd, VIDIOC_ENUM_FRAMESIZES, &framesize) == 0) {
			if (framesize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {

				struct v4l2_frmivalenum frameinterval = {0};
				frameinterval.pixel_format = framesize.pixel_format;
				frameinterval.width = framesize.discrete.width;
				frameinterval.height = framesize.discrete.height;

				while (xioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &frameinterval) == 0) {
					frameinterval.index++;
					if (frameinterval.type == V4L2_FRMIVAL_TYPE_DISCRETE) {
						libmegapixels_mode *mode = calloc(1, sizeof(libmegapixels_mode));
						mode->format = format;
						mode->v4l_pixfmt = fmtdesc.pixelformat;
						mode->width = (int) framesize.discrete.width;
						mode->height = (int) framesize.discrete.height;
						mode->media_busfmt = libmegapixels_format_to_media_busfmt(format);
						mode->rotation = 0;
						mode->rate = (int) ((double) frameinterval.discrete.denominator /
							(double) frameinterval.discrete.numerator);

						camera->modes = realloc(camera->modes, (camera->num_modes + 1) * sizeof(camera->modes));
						if (camera->modes == NULL) {
							return;
						}
						camera->modes[camera->num_modes++] = mode;
					}
				}
			}
			framesize.index++;
		}
	}
}

int
libmegapixels_load_uvc(libmegapixels_devconfig *config)
{
	if (config->loaded_uvc) {
		log_error("libmegapixels_load_uvc was already called\n");
		return 0;
	}
	log_debug("Loading UVC cameras\n");
	config->loaded_uvc = 1;

	struct dirent *dir;
	DIR *d = opendir("/dev");
	while ((dir = readdir(d)) != NULL) {
		if (strncmp(dir->d_name, "video", 5) == 0) {
			char path[PATH_MAX];
			snprintf(path, PATH_MAX, "/dev/%s", dir->d_name);
			int fd = open(path, O_RDWR);
			if (fd < 0) {
				continue;
			}
			struct v4l2_capability vid_cap = {0};
			if (xioctl(fd, VIDIOC_QUERYCAP, &vid_cap) == -1) {
				perror("QUERYCAP");
				continue;
			}
			if (strcmp((char *) vid_cap.driver, "uvcvideo") != 0) {
				continue;
			}

			if (!(vid_cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
				continue;
			}

			if (!(vid_cap.capabilities & V4L2_CAP_STREAMING)) {
				continue;
			}

			struct v4l2_fmtdesc fmtdesc = {0};
			fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
			if (xioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) != 0) {
				continue;
			}

			libmegapixels_camera *camera;
			camera = calloc(1, sizeof(libmegapixels_camera));
			camera->name = strdup((const char *) vid_cap.card);
			camera->video_path = strdup(path);

			uvc_create_modes(camera, fd);

			close(fd);

			config->cameras = realloc(config->cameras, (config->count + 1) * sizeof(config->cameras));
			if (config->cameras == NULL) {
				return -1;
			}
			config->cameras[config->count++] = camera;
			camera->index = config->count;

		}
	}

	closedir(d);
	return 1;
}

int
libmegapixels_init(libmegapixels_devconfig **config)
{
	*config = calloc(1, sizeof(libmegapixels_devconfig));
	init_log(0);
	return 1;
}