Add more mode support to libmegapixels

CMakeLists.txt

@@ -7,7 +7,7 @@ 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)
+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 src/convert.c)
 set_target_properties(megapixels PROPERTIES
         VERSION ${LIBRARY_VERSION_STRING}
         SOVERSION ${LIBRARY_VERSION_MAJOR}

include/libmegapixels.h

@@ -12,6 +12,12 @@ libmegapixels_find_config(char *configfile);
 #define LIBMEGAPIXELS_CMD_LINK 1
 #define LIBMEGAPIXELS_CMD_MODE 2
 
+#define LIBMEGAPIXELS_CFA_NONE 0
+#define LIBMEGAPIXELS_CFA_BGGR 1
+#define LIBMEGAPIXELS_CFA_GBRG 2
+#define LIBMEGAPIXELS_CFA_GRBG 3
+#define LIBMEGAPIXELS_CFA_RGGB 4
+
 struct _lmp_cmd {
 		int type;
 		const char *entity_from;
@@ -35,6 +41,7 @@ struct _lmp_mode {
 		int rate;
 		int format;
 		int rotation;
+		int mirrored;
 		double focal_length;
 		double f_number;
 
@@ -54,6 +61,7 @@ struct _lmp_subdev {
 typedef struct _lmp_subdev libmegapixels_subdev;
 
 struct _lmp_camera {
+		int index;
 		char *name;
 		char *sensor_name;
 		char *bridge_name;
@@ -67,6 +75,7 @@ struct _lmp_camera {
 
 		int num_modes;
 		libmegapixels_mode **modes;
+		libmegapixels_mode *current_mode;
 
 		int num_handles;
 		libmegapixels_subdev **handles;
@@ -102,4 +111,31 @@ libmegapixels_close(libmegapixels_camera *camera);
 EXPORT unsigned int
 libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode);
 
+EXPORT char *
+libmegapixels_v4l_pixfmt_to_string(uint32_t pixfmt);
+
+EXPORT const char *
+libmegapixels_format_cfa_pattern(int format);
+
+EXPORT uint32_t
+libmegapixels_mode_raw_width_to_width(int index, uint32_t width);
+
+EXPORT uint32_t
+libmegapixels_mode_width_to_padding(int index, uint32_t width);
+
+EXPORT uint32_t
+libmegapixels_mode_width_to_bytes(int index, uint32_t width);
+
+EXPORT uint32_t
+libmegapixels_format_to_v4l_pixfmt(int index);
+
+EXPORT uint32_t
+libmegapixels_format_to_media_busfmt(int index);
+
+EXPORT uint32_t
+libmegapixels_format_bits_per_pixel(int format);
+
+EXPORT int
+libmegapixels_mode_equals(libmegapixels_mode *a, libmegapixels_mode *b);
+
 #endif

src/convert.c

@@ -0,0 +1,18 @@
+#include <linux/v4l2-subdev.h>
+#include "libmegapixels.h"
+
+int
+libmegapixels_convert_to_rgb(uint32_t v4l_pixfmt, uint8_t *in_data, long in_size, uint8_t *out_data, long out_size)
+{
+	switch (v4l_pixfmt) {
+		case V4L2_PIX_FMT_SBGGR8:
+		case V4L2_PIX_FMT_SGBRG8:
+		case V4L2_PIX_FMT_SGRBG8:
+		case V4L2_PIX_FMT_SRGGB8:
+
+			break;
+		default:
+			return 0;
+	}
+	return 1;
+}

src/mode.c

@@ -1,7 +1,9 @@
-#include <stdint.h>
+#include <stdlib.h>
 #include <linux/v4l2-subdev.h>
 #include <strings.h>
+#include <assert.h>
 #include "mode.h"
+#include "libmegapixels.h"
 
 
 // TODO: The 16 bit formats are imported from millipixels and seem broken
@@ -10,131 +12,312 @@ static struct libmegapixels_modename mode_lut[] = {
 		.name = "unsupported",
 		.v4l_pixel_format = 0,
 		.media_bus_format = 0,
+		.bpc = 0,
+		.bpp = 0,
+		.cfa = LIBMEGAPIXELS_CFA_NONE,
 	},
 	{
 		.name = "BGGR8",
 		.v4l_pixel_format = V4L2_PIX_FMT_SBGGR8,
 		.media_bus_format = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.bpc = 8,
+		.bpp = 8,
+		.cfa = LIBMEGAPIXELS_CFA_BGGR,
 	},
 	{
 		.name = "GBRG8",
 		.v4l_pixel_format = V4L2_PIX_FMT_SGBRG8,
 		.media_bus_format = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.bpc = 8,
+		.bpp = 8,
+		.cfa = LIBMEGAPIXELS_CFA_GBRG,
 	},
 	{
 		.name = "GRBG8",
 		.v4l_pixel_format = V4L2_PIX_FMT_SGRBG8,
 		.media_bus_format = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.bpc = 8,
+		.bpp = 8,
+		.cfa = LIBMEGAPIXELS_CFA_GRBG,
 	},
 	{
 		.name = "RGGB8",
 		.v4l_pixel_format = V4L2_PIX_FMT_SRGGB8,
 		.media_bus_format = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.bpc = 8,
+		.bpp = 8,
+		.cfa = LIBMEGAPIXELS_CFA_RGGB,
 	},
 	{
 		.name = "BGGR10P",
 		.v4l_pixel_format = V4L2_PIX_FMT_SBGGR10P,
 		.media_bus_format = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_BGGR,
 	},
 	{
 		.name = "GBRG10P",
 		.v4l_pixel_format = V4L2_PIX_FMT_SGBRG10P,
 		.media_bus_format = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_GBRG,
 	},
 	{
 		.name = "GRBG10P",
 		.v4l_pixel_format = V4L2_PIX_FMT_SGRBG10P,
 		.media_bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_GRBG,
 	},
 	{
 		.name = "RGGB10P",
 		.v4l_pixel_format = V4L2_PIX_FMT_SRGGB10P,
 		.media_bus_format = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_RGGB,
 	},
 	{
 		.name = "BGGR10",
 		.v4l_pixel_format = V4L2_PIX_FMT_SBGGR10,
 		.media_bus_format = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_BGGR,
 	},
 	{
 		.name = "GBRG10",
 		.v4l_pixel_format = V4L2_PIX_FMT_SGBRG10,
 		.media_bus_format = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_GBRG,
 	},
 	{
 		.name = "GRBG10",
 		.v4l_pixel_format = V4L2_PIX_FMT_SGRBG10,
 		.media_bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_GRBG,
 	},
 	{
 		.name = "RGGB10",
 		.v4l_pixel_format = V4L2_PIX_FMT_SRGGB10,
 		.media_bus_format = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_RGGB,
 	},
 	{
 		.name = "BGGR16",
 		.v4l_pixel_format = V4L2_PIX_FMT_SBGGR16,
 		.media_bus_format = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_BGGR,
 	},
 	{
 		.name = "GBRG16",
 		.v4l_pixel_format = V4L2_PIX_FMT_SGBRG16,
 		.media_bus_format = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_GBRG,
 	},
 	{
 		.name = "GRBG16",
 		.v4l_pixel_format = V4L2_PIX_FMT_SGRBG16,
 		.media_bus_format =MEDIA_BUS_FMT_SGRBG10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_GRBG,
 	},
 	{
 		.name = "RGGB16",
 		.v4l_pixel_format = V4L2_PIX_FMT_SRGGB16,
 		.media_bus_format = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.bpc = 10,
+		.bpp = 10,
+		.cfa = LIBMEGAPIXELS_CFA_RGGB,
 	},
 	{
 		.name = "UYVY",
 		.v4l_pixel_format = V4L2_PIX_FMT_UYVY,
 		.media_bus_format = MEDIA_BUS_FMT_UYVY8_2X8,
+		.bpc = 8,
+		.bpp = 16,
+		.cfa = LIBMEGAPIXELS_CFA_NONE,
 	},
 	{
 		.name = "YUYV",
 		.v4l_pixel_format = V4L2_PIX_FMT_YUYV,
 		.media_bus_format = MEDIA_BUS_FMT_YUYV8_2X8,
+		.bpc = 8,
+		.bpp = 16,
+		.cfa = LIBMEGAPIXELS_CFA_NONE,
 	},
 };
 
-uint32_t
-format_name_to_v4l_pixfmt(const char *name)
+int
+libmegapixels_format_name_to_index(const char *name)
 {
 	int count = sizeof(mode_lut) / sizeof(mode_lut[0]);
 	for (int i = 0; i < count; i++) {
 		if (strcasecmp(mode_lut[i].name, name) == 0) {
-			return mode_lut[i].v4l_pixel_format;
+			return i;
 		}
 	}
 	return 0;
 }
 
 uint32_t
-format_name_to_media_busfmt(const char *name)
+libmegapixels_format_to_v4l_pixfmt(int index)
+{
+	return mode_lut[index].v4l_pixel_format;
+}
+
+uint32_t
+libmegapixels_format_to_media_busfmt(int index)
+{
+	return mode_lut[index].media_bus_format;
+}
+
+int
+libmegapixels_v4l_pixfmt_to_index(uint32_t pixfmt)
 {
 	int count = sizeof(mode_lut) / sizeof(mode_lut[0]);
 	for (int i = 0; i < count; i++) {
-		if (strcasecmp(mode_lut[i].name, name) == 0) {
-			return mode_lut[i].media_bus_format;
+		if (mode_lut[i].v4l_pixel_format == pixfmt) {
+			return i;
 		}
 	}
 	return 0;
 }
 
-struct libmegapixels_modename *
-v4l_pixfmt_to_mode(uint32_t pixfmt)
+char *
+libmegapixels_v4l_pixfmt_to_string(uint32_t pixfmt)
 {
 	int count = sizeof(mode_lut) / sizeof(mode_lut[0]);
 	for (int i = 0; i < count; i++) {
 		if (mode_lut[i].v4l_pixel_format == pixfmt) {
-			return &mode_lut[i];
+			return mode_lut[i].name;
 		}
 	}
-	return 0;
+	return "unknown";
+}
+
+// mp_pixel_format_bits_per_pixel
+uint32_t
+libmegapixels_format_bits_per_pixel(int format)
+{
+	return mode_lut[format].bpp;
+}
+
+const char *
+libmegapixels_format_cfa_pattern(int format)
+{
+	switch (mode_lut[format].cfa) {
+		case LIBMEGAPIXELS_CFA_BGGR:
+			return "\002\001\001\000";
+		case LIBMEGAPIXELS_CFA_GBRG:
+			return "\001\002\000\001";
+		case LIBMEGAPIXELS_CFA_GRBG:
+			return "\001\000\002\001";
+		case LIBMEGAPIXELS_CFA_RGGB:
+			return "\000\001\001\002";
+		default:
+			return NULL;
+	}
+}
+
+// mp_pixel_format_width_to_bytes
+uint32_t
+libmegapixels_mode_width_to_bytes(int index, uint32_t width)
+{
+	uint32_t bits_per_pixel = mode_lut[index].bpp;
+	uint64_t bits_per_width = width * (uint64_t) bits_per_pixel;
+	uint64_t remainder = bits_per_width % 8;
+	if (remainder == 0)
+		return bits_per_width / 8;
+
+	return (bits_per_width + 8 - remainder) / 8;
+}
+
+uint32_t
+libmegapixels_mode_width_to_padding(int index, uint32_t width)
+{
+	uint64_t bytes_per_width = libmegapixels_mode_width_to_bytes(index, width);
+	uint64_t remainder = bytes_per_width % 8;
+	if (remainder == 0)
+		return remainder;
+
+	return 8 - remainder;
+}
+
+uint32_t
+libmegapixels_mode_raw_width_to_width(int index, uint32_t width)
+{
+	switch (mode_lut[index].v4l_pixel_format) {
+		case V4L2_PIX_FMT_SBGGR8:
+		case V4L2_PIX_FMT_SGBRG8:
+		case V4L2_PIX_FMT_SGRBG8:
+		case V4L2_PIX_FMT_SRGGB8:
+			return width / 2;
+		case V4L2_PIX_FMT_SBGGR10P:
+		case V4L2_PIX_FMT_SGBRG10P:
+		case V4L2_PIX_FMT_SGRBG10P:
+		case V4L2_PIX_FMT_SRGGB10P:
+			return width / 2 * 5;
+		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YUYV:
+			return width;
+		default:
+			return 0;
+	}
+}
+
+uint32_t
+libmegapixels_mode_raw_height_to_height(int index, uint32_t height)
+{
+	switch (mode_lut[index].v4l_pixel_format) {
+		case V4L2_PIX_FMT_SBGGR8:
+		case V4L2_PIX_FMT_SGBRG8:
+		case V4L2_PIX_FMT_SGRBG8:
+		case V4L2_PIX_FMT_SRGGB8:
+		case V4L2_PIX_FMT_SBGGR10P:
+		case V4L2_PIX_FMT_SGBRG10P:
+		case V4L2_PIX_FMT_SGRBG10P:
+		case V4L2_PIX_FMT_SRGGB10P:
+			return height / 2;
+		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YUYV:
+			return height;
+		default:
+			return 0;
+	}
+}
+
+int
+libmegapixels_mode_equals(libmegapixels_mode *a, libmegapixels_mode *b)
+{
+	if (a == NULL || b == NULL) {
+		return 0;
+	}
+	if (a->width != b->width) {
+		return 0;
+	}
+	if (a->height != b->height) {
+		return 0;
+	}
+	if (a->rate != b->rate) {
+		return 0;
+	}
+	if (a->format != b->format) {
+		return 0;
+	}
+	return 1;
 }

src/mode.h

@@ -1,17 +1,18 @@
+#include <stdint.h>
+
 #pragma once
 
 struct libmegapixels_modename {
 		char *name;
 		uint32_t v4l_pixel_format;
 		uint32_t media_bus_format;
+		int bpp;
+		int bpc;
+		int cfa;
 };
 
-uint32_t
-format_name_to_v4l_pixfmt(const char *name);
-
-uint32_t
-format_name_to_media_busfmt(const char *name);
-
+int
+libmegapixels_v4l_pixfmt_to_index(uint32_t pixfmt);
 
-struct libmegapixels_modename *
-v4l_pixfmt_to_mode(uint32_t pixfmt);
+int
+libmegapixels_format_name_to_index(const char *name);

src/parse.c

@@ -237,13 +237,13 @@ load_camera(libmegapixels_devconfig *config, config_t *cfg, const char *name)
 
 		const char *fmt;
 		config_setting_lookup_string(mode, "Format", &fmt);
-		mm->v4l_pixfmt = format_name_to_v4l_pixfmt(fmt);
-		if (mm->v4l_pixfmt == 0) {
+		int format = libmegapixels_format_name_to_index(fmt);
+		if (!format) {
 			log_error("Unknown format '%s'\n", fmt);
 			return -1;
 		}
-		mm->media_busfmt = format_name_to_media_busfmt(fmt);
-
+		mm->v4l_pixfmt = libmegapixels_format_to_v4l_pixfmt(format);
+		mm->media_busfmt = libmegapixels_format_to_media_busfmt(format);
 
 		if (!config_setting_lookup_int(mode, "Rotate", &mm->rotation)) {
 			mm->rotation = 0;
@@ -389,8 +389,9 @@ uvc_create_modes(libmegapixels_camera *camera, int fd)
 	fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	while (xioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
 		fmtdesc.index++;
-		struct libmegapixels_modename *mn = v4l_pixfmt_to_mode(fmtdesc.pixelformat);
-		if (mn == NULL) {
+		libmegapixels_v4l_pixfmt_to_index(fmtdesc.pixelformat);
+		int format = libmegapixels_v4l_pixfmt_to_index(fmtdesc.pixelformat);
+		if (!format) {
 			continue;
 		}
 
@@ -412,7 +413,7 @@ uvc_create_modes(libmegapixels_camera *camera, int fd)
 						mode->v4l_pixfmt = fmtdesc.pixelformat;
 						mode->width = (int) framesize.discrete.width;
 						mode->height = (int) framesize.discrete.height;
-						mode->media_busfmt = mn->media_bus_format;
+						mode->media_busfmt = libmegapixels_format_to_media_busfmt(format);
 						mode->rotation = 0;
 						mode->rate = (int) ((double) frameinterval.discrete.denominator /
 							(double) frameinterval.discrete.numerator);
@@ -486,6 +487,7 @@ libmegapixels_load_uvc(libmegapixels_devconfig *config)
 				return -1;
 			}
 			config->cameras[config->count++] = camera;
+			camera->index = config->count;
 
 		}
 	}

src/pipeline.c

@@ -6,6 +6,7 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <stdio.h>
+#include <assert.h>
 #include "libmegapixels.h"
 #include "log.h"
 #include "util.h"
@@ -201,6 +202,8 @@ libmegapixels_close(libmegapixels_camera *camera)
 unsigned int
 libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode)
 {
+	assert(camera->video_fd != 0);
+	assert(camera->sensor_fd != 0);
 	for (int i = 0; i < mode->num_cmds; i++) {
 		libmegapixels_cmd *cmd = mode->cmds[i];
 		struct v4l2_subdev_format subdev_fmt = {};
@@ -259,5 +262,8 @@ libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode
 		log_error("Could not set mode on bridge: %s\n", strerror(errno));
 		return 0;
 	}
+
+	camera->current_mode = mode;
+
 	return format.fmt.pix.sizeimage;
 }