From 9137d56ea655ee12795e874b076e5b0c804a8cf1 Mon Sep 17 00:00:00 2001
From: Martijn Braam <martijn@brixit.nl>
Date: Thu, 20 Jul 2023 22:50:43 +0200
Subject: [PATCH] Implement rate command

---
 config/purism,librem5.conf |  2 ++
 include/libmegapixels.h    |  2 ++
 src/parse.c                |  9 +++++++++
 src/pipeline.c             | 33 +++++++++++++++++++++++++++++++++
 4 files changed, 46 insertions(+)

diff --git a/config/purism,librem5.conf b/config/purism,librem5.conf
index 7850b67..2d8bb43 100644
--- a/config/purism,librem5.conf
+++ b/config/purism,librem5.conf
@@ -17,6 +17,7 @@ Rear: {
             # All the links on this platform are immutable
             Pipeline: (
                 {Type: "Mode", Entity: "s5k3l6xx"},
+                {Type: "Rate", Entity: "s5k3l6xx"},
                 {Type: "Mode", Entity: "imx8mq-mipi-csi2"},
                 {Type: "Mode", Entity: "csi"},
             );
@@ -29,6 +30,7 @@ Rear: {
             Rotate: 270;
             Pipeline: (
                 {Type: "Mode", Entity: "s5k3l6xx"},
+                {Type: "Rate", Entity: "s5k3l6xx"},
                 {Type: "Mode", Entity: "imx8mq-mipi-csi2"},
                 {Type: "Mode", Entity: "csi"},
             );
diff --git a/include/libmegapixels.h b/include/libmegapixels.h
index 73ba3a1..63eb5ef 100644
--- a/include/libmegapixels.h
+++ b/include/libmegapixels.h
@@ -13,6 +13,7 @@ libmegapixels_find_config(char *configfile);
 #define LIBMEGAPIXELS_CMD_LINK 1
 #define LIBMEGAPIXELS_CMD_MODE 2
 #define LIBMEGAPIXELS_CMD_CROP 3
+#define LIBMEGAPIXELS_CMD_INTERVAL 4
 
 #define LIBMEGAPIXELS_CFA_NONE 0
 #define LIBMEGAPIXELS_CFA_BGGR 1
@@ -31,6 +32,7 @@ struct _lmp_cmd {
 		int top;
 		int left;
 		int format;
+		int rate;
 
 		uint32_t entity_from_id;
 		int pad_from_id;
diff --git a/src/parse.c b/src/parse.c
index 7ae1891..63ea680 100644
--- a/src/parse.c
+++ b/src/parse.c
@@ -331,6 +331,15 @@ load_camera(libmegapixels_devconfig *config, config_t *cfg, const char *name)
 				} 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)) {
diff --git a/src/pipeline.c b/src/pipeline.c
index 929855c..4604f5a 100644
--- a/src/pipeline.c
+++ b/src/pipeline.c
@@ -212,6 +212,7 @@ libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode
 		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;
 
@@ -258,6 +259,38 @@ libmegapixels_select_mode(libmegapixels_camera *camera, libmegapixels_mode *mode
 					log_error("Could not set mode on %s:%d: %s\n", cmd->entity_from, cmd->pad_from, strerror(errno));
 				}
 				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;