Add crop and mplane support

include/libmegapixels.h

@@ -2,6 +2,7 @@
 #define LIBMEGAPIXELS_HEADER
 
 #include <stdint.h>
+#include <linux/videodev2.h>
 
 #define EXPORT __attribute__((__visibility__("default")))
 
@@ -11,6 +12,7 @@ libmegapixels_find_config(char *configfile);
 
 #define LIBMEGAPIXELS_CMD_LINK 1
 #define LIBMEGAPIXELS_CMD_MODE 2
+#define LIBMEGAPIXELS_CMD_CROP 3
 
 #define LIBMEGAPIXELS_CFA_NONE 0
 #define LIBMEGAPIXELS_CFA_BGGR 1
@@ -26,7 +28,9 @@ struct _lmp_cmd {
 		int pad_to;
 		int width;
 		int height;
-		uint32_t mode;
+		int top;
+		int left;
+		int format;
 
 		uint32_t entity_from_id;
 		int pad_from_id;
@@ -109,11 +113,14 @@ EXPORT void
 libmegapixels_close(libmegapixels_camera *camera);
 
 EXPORT unsigned int
-libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode);
+libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode, struct v4l2_format *format);
 
 EXPORT char *
 libmegapixels_v4l_pixfmt_to_string(uint32_t pixfmt);
 
+EXPORT char *
+libmegapixels_format_name(int format);
+
 EXPORT const char *
 libmegapixels_format_cfa(int format);
 

src/mode.c

@@ -211,6 +211,12 @@ libmegapixels_v4l_pixfmt_to_string(uint32_t pixfmt)
 	return "unknown";
 }
 
+char *
+libmegapixels_format_name(int format)
+{
+	return mode_lut[format].name;
+}
+
 // mp_pixel_format_bits_per_pixel
 uint32_t
 libmegapixels_format_bits_per_pixel(int format)

src/parse.c

@@ -276,6 +276,10 @@ load_camera(libmegapixels_devconfig *config, config_t *cfg, const char *name)
 		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) {
@@ -309,12 +313,47 @@ load_camera(libmegapixels_devconfig *config, config_t *cfg, const char *name)
 					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, "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++;
 		}

src/pipeline.c

@@ -82,7 +82,7 @@ load_entity_ids(libmegapixels_camera *camera)
 					log_error("Could not find entity '%s'\n", cmd->entity_to);
 					return -1;
 				}
-			} else if (cmd->type == LIBMEGAPIXELS_CMD_MODE) {
+			} else if (cmd->type == LIBMEGAPIXELS_CMD_MODE || cmd->type == LIBMEGAPIXELS_CMD_CROP) {
 				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) {
@@ -201,7 +201,7 @@ libmegapixels_close(libmegapixels_camera *camera)
 }
 
 unsigned int
-libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode)
+libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode, struct v4l2_format *format)
 {
 	assert(camera->video_fd != 0);
 	assert(camera->sensor_fd != 0);
@@ -211,9 +211,13 @@ 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 = {};
+		struct v4l2_subdev_crop subdev_crop = {};
+		int found = 0;
+		libmegapixels_subdev *sd;
+
 		switch (cmd->type) {
 			case LIBMEGAPIXELS_CMD_LINK:
-				log_debug("  Link %s -> %s\n", cmd->entity_from, cmd->entity_to);
+				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,
@@ -221,17 +225,16 @@ libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode
 				}
 				break;
 			case LIBMEGAPIXELS_CMD_MODE:
-				subdev_fmt.pad = cmd->pad_to;
+				subdev_fmt.pad = cmd->pad_from;
 				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.code = libmegapixels_format_to_media_busfmt(cmd->format);
 				subdev_fmt.format.field = V4L2_FIELD_ANY;
+				log_debug("  Mode %s:%d [%dx%d %s]\n", cmd->entity_from, cmd->pad_from, cmd->width, cmd->height,
+					libmegapixels_format_name(cmd->format));
 
-				log_debug("  Mode \n", cmd->entity_from);
-
-				libmegapixels_subdev *sd;
-				int found = 0;
+				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];
@@ -252,25 +255,102 @@ libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode
 				}
 
 				if (xioctl(sd->fd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt) == -1) {
-					log_error("Could not set mode on sensor: %s\n", strerror(errno));
+					log_error("Could not set mode 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_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));
+	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 format.fmt.pix.sizeimage;
+	return 1;
 }

util/getframe.c

@@ -35,6 +35,7 @@ usage(char *name)
 	fprintf(stderr, "Arguments:\n");
 	fprintf(stderr, "  -n offset   Get the Nth frame from the output (defaults to 5)\n");
 	fprintf(stderr, "  -c camera   Use a specific camera number\n");
+	fprintf(stderr, "  -m modenum  Use another camera mode than the first\n");
 	fprintf(stderr, "  -o file     File to store the frame in\n");
 	fprintf(stderr, "  -h          Display this help text\n");
 }
@@ -48,8 +49,9 @@ main(int argc, char *argv[])
 	char *end;
 	char *outfile = NULL;
 	int count = 5;
+	int mode_idx = 0;
 
-	while ((c = getopt(argc, argv, "hc:n:o:")) != -1) {
+	while ((c = getopt(argc, argv, "hc:n:o:m:")) != -1) {
 		switch (c) {
 			case 'c':
 				res = strtol(optarg, &end, 10);
@@ -70,6 +72,14 @@ main(int argc, char *argv[])
 				}
 				count = (int) res;
 				break;
+			case 'm':
+				res = strtol(optarg, &end, 10);
+				if (end == optarg || end == NULL || *end != '\0') {
+					fprintf(stderr, "Invalid number for -m\n");
+					return 1;
+				}
+				mode_idx = (int) res;
+				break;
 			case 'h':
 				usage(argv[0]);
 				return 0;
@@ -118,9 +128,13 @@ main(int argc, char *argv[])
 		fprintf(stderr, "Could not open default camera\n");
 		return 1;
 	}
+	if (mode_idx > camera->num_modes) {
+		fprintf(stderr, "Invalid mode index: %d\n", mode_idx);
+	}
 
-	libmegapixels_mode *mode = camera->modes[0];
-	unsigned int frame_size = libmegapixels_select_mode(camera, mode);
+	libmegapixels_mode *mode = camera->modes[mode_idx];
+	struct v4l2_format format = {0};
+	unsigned int frame_size = libmegapixels_select_mode(camera, mode, &format);
 	if (frame_size == 0) {
 		fprintf(stderr, "Could not select mode\n");
 		return 1;
@@ -128,37 +142,61 @@ main(int argc, char *argv[])
 
 	// Do the reqular V4L2 stuff to get a frame
 	struct v4l2_capability cap;
+	int mplanes = 0;
+	enum v4l2_buf_type buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	if (ioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) != 0) {
-		fprintf(stderr, "VIDIOC_QUERYCAP failed\n");
+		fprintf(stderr, "VIDIOC_QUERYCAP failed: %s\n", strerror(errno));
 		return 1;
 	}
 	if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
 		fprintf(stderr, "Device does not support streaming i/o\n");
 		return 1;
 	}
+	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+		if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
+			mplanes = 1;
+			buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+		} else {
+			fprintf(stderr, "Device does not support V4L2_CAP_VIDEO_CAPTURE\n");
+			return 1;
+		}
+	}
 
 	struct v4l2_requestbuffers req = {0};
 	req.count = 4;
-	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	req.type = buftype;
 	req.memory = V4L2_MEMORY_MMAP;
 	if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) {
-		fprintf(stderr, "VIDIOC_REQBUFS failed\n");
+		fprintf(stderr, "VIDIOC_REQBUFS failed: %s\n", strerror(errno));
 		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.type = buftype;
 		buf.memory = V4L2_MEMORY_MMAP;
 		buf.index = i;
 
+		struct v4l2_plane planes[1];
+		if (mplanes) {
+			buf.m.planes = planes;
+			buf.length = 1;
+		}
+
 		if (xioctl(camera->video_fd, VIDIOC_QUERYBUF, &buf) == -1) {
-			fprintf(stderr, "VIDIOC_QUERYBUF failed\n");
+			fprintf(stderr, "VIDIOC_QUERYBUF failed: %s\n", strerror(errno));
 			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);
+		unsigned int offset;
+		if (mplanes) {
+			buffers[i].length = planes[0].length;
+			offset = planes[0].m.mem_offset;
+		} else {
+			buffers[i].length = buf.length;
+			offset = buf.m.offset;
+		}
+		buffers[i].start = mmap(NULL, buffers[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, camera->video_fd, offset);
 		if (buffers[i].start == MAP_FAILED) {
 			fprintf(stderr, "mmap() failed\n");
 			return 1;
@@ -167,18 +205,24 @@ main(int argc, char *argv[])
 
 	for (int i = 0; i < req.count; i++) {
 		struct v4l2_buffer qbuf = {0};
-		qbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		qbuf.type = buftype;
 		qbuf.memory = V4L2_MEMORY_MMAP;
 		qbuf.index = i;
 
+		if (mplanes) {
+			struct v4l2_plane qplanes[1];
+			qbuf.m.planes = qplanes;
+			qbuf.length = 1;
+		}
+
 		if (xioctl(camera->video_fd, VIDIOC_QBUF, &qbuf) == -1) {
-			fprintf(stderr, "VIDIOC_QBUF failed\n");
+			fprintf(stderr, "VIDIOC_QBUF failed: %s\n", strerror(errno));
 			return 1;
 		}
 	}
-	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	enum v4l2_buf_type type = buftype;
 	if (xioctl(camera->video_fd, VIDIOC_STREAMON, &type) == -1) {
-		fprintf(stderr, "VIDIOC_STREAMON failed\n");
+		fprintf(stderr, "VIDIOC_STREAMON failed: %s\n", strerror(errno));
 		return 1;
 	}
 
@@ -201,8 +245,13 @@ main(int argc, char *argv[])
 
 
 			struct v4l2_buffer buf = {0};
-			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+			buf.type = buftype;
 			buf.memory = V4L2_MEMORY_MMAP;
+			if (mplanes) {
+				struct v4l2_plane dqplanes[1];
+				buf.m.planes = dqplanes;
+				buf.length = 1;
+			}
 			if (xioctl(camera->video_fd, VIDIOC_DQBUF, &buf) == -1) {
 				fprintf(stderr, "VIDIOC_DQBUF failed\n");
 				return 1;