From f43fcdb2415c99a6c483e61410ad7e7bc927f87f Mon Sep 17 00:00:00 2001
From: ArenM <aren@peacevolution.org>
Date: Thu, 25 May 2023 20:17:40 -0400
Subject: [PATCH] Set bridge resolution when setting the mode (MR 31)

sun6i-csi-bridge will return EINVAL if the resolution it is configured
with is different than the resolution the camera is configured with. So
we have to set it's resolution when changing the camera resolution.
---
 src/camera.c        | 24 +++++++++++++++++++++++-
 src/camera.h        |  2 +-
 src/io_pipeline.c   | 30 +++++++++++++++++++++++++++++-
 tools/camera_test.c |  2 +-
 4 files changed, 54 insertions(+), 4 deletions(-)

diff --git a/src/camera.c b/src/camera.c
index 9a08f56..5519bbd 100644
--- a/src/camera.c
+++ b/src/camera.c
@@ -39,6 +39,7 @@ struct video_buffer {
 struct _MPCamera {
         int video_fd;
         int subdev_fd;
+        int bridge_fd;
 
         bool has_set_mode;
         MPMode current_mode;
@@ -53,7 +54,7 @@ struct _MPCamera {
 };
 
 MPCamera *
-mp_camera_new(int video_fd, int subdev_fd)
+mp_camera_new(int video_fd, int subdev_fd, int bridge_fd)
 {
         g_return_val_if_fail(video_fd != -1, NULL);
 
@@ -76,6 +77,7 @@ mp_camera_new(int video_fd, int subdev_fd)
         MPCamera *camera = malloc(sizeof(MPCamera));
         camera->video_fd = video_fd;
         camera->subdev_fd = subdev_fd;
+        camera->bridge_fd = bridge_fd;
         camera->has_set_mode = false;
         camera->num_buffers = 0;
         camera->use_mplane = use_mplane;
@@ -291,6 +293,26 @@ mp_camera_set_mode(MPCamera *camera, MPMode *mode)
                         return false;
                 }
 
+                // sun6i-csi-bridge will return EINVAL when trying to read
+                // frames if the resolution it's configured with doesn't match
+                // the resolution of its input.
+                if (camera->bridge_fd > 0) {
+                        struct v4l2_subdev_format bridge_fmt = fmt;
+
+                        if (xioctl(camera->bridge_fd,
+                                   VIDIOC_SUBDEV_S_FMT,
+                                   &bridge_fmt) == -1) {
+                                errno_printerr("VIDIOC_SUBDEV_S_FMT");
+                                return false;
+                        }
+
+                        if (fmt.format.width != bridge_fmt.format.width ||
+                            fmt.format.height != bridge_fmt.format.height) {
+                                g_printerr("Bridge format resolution mismatch\n");
+                                return false;
+                        }
+                }
+
                 // Some drivers like ov5640 don't allow you to set the frame format
                 // with too high a frame-rate, but that means the frame-rate won't be
                 // set after the format change. So we need to try again here if we
diff --git a/src/camera.h b/src/camera.h
index be3e071..ebf7af0 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -15,7 +15,7 @@ typedef struct {
 
 typedef struct _MPCamera MPCamera;
 
-MPCamera *mp_camera_new(int video_fd, int subdev_fd);
+MPCamera *mp_camera_new(int video_fd, int subdev_fd, int bridge_fd);
 void mp_camera_free(MPCamera *camera);
 
 void mp_camera_add_bg_task(MPCamera *camera, pid_t pid);
diff --git a/src/io_pipeline.c b/src/io_pipeline.c
index 1a7ab57..e9f80c2 100644
--- a/src/io_pipeline.c
+++ b/src/io_pipeline.c
@@ -127,6 +127,32 @@ mp_setup_media_link_pad_formats(struct device_info *dev_info,
         }
 }
 
+static int
+get_bridge_fd(const MPDevice *device)
+{
+        const struct media_v2_entity *bridge =
+                mp_device_find_entity_type(device, MEDIA_ENT_F_VID_IF_BRIDGE);
+        if (!bridge) {
+                g_printerr("Could not find device bridge entity\n");
+                return -1;
+        }
+
+        const struct media_v2_interface *bridge_interface =
+                mp_device_find_entity_interface(device, bridge->id);
+        char dev_name[260];
+        if (!mp_find_device_path(bridge_interface->devnode, dev_name, 260)) {
+                g_printerr("Could not find bridge path\n");
+                return -1;
+        }
+
+        int bridge_fd = open(dev_name, O_RDWR);
+        if (bridge_fd == -1) {
+                g_printerr("Could not open %s: %s\n", dev_name, strerror(errno));
+        }
+
+        return bridge_fd;
+}
+
 static void
 setup_camera(MPDeviceList **device_list, const struct mp_camera_config *config)
 {
@@ -235,7 +261,9 @@ setup_camera(MPDeviceList **device_list, const struct mp_camera_config *config)
                         exit(EXIT_FAILURE);
                 }
 
-                info->camera = mp_camera_new(dev_info->video_fd, info->fd);
+                int bridge_fd = get_bridge_fd(dev_info->device);
+                info->camera =
+                        mp_camera_new(dev_info->video_fd, info->fd, bridge_fd);
 
                 // Start with the capture format, this works around a bug with
                 // the ov5640 driver where it won't allow setting the preview
diff --git a/tools/camera_test.c b/tools/camera_test.c
index aa80121..ae311c3 100644
--- a/tools/camera_test.c
+++ b/tools/camera_test.c
@@ -115,7 +115,7 @@ main(int argc, char *argv[])
 
         printf("Opening the device took %fms\n", (open_end - find_end) * 1000);
 
-        MPCamera *camera = mp_camera_new(video_fd, subdev_fd);
+        MPCamera *camera = mp_camera_new(video_fd, subdev_fd, -1);
 
         MPControlList *controls = mp_camera_list_controls(camera);